一种智能车辆路径跟踪前轮转角补偿控制方法与流程

本发明涉及智能车辆运动控制领域；尤其是涉及一种基于改进purepursuit算法和运用粒子群算法优化前轮转角补偿控制器系数的路径跟踪控制方法。

背景技术：

运动作为智能车辆领域的核心技术之一，主要研究内容是如何在考虑相关约束的前提下，控制车辆沿预定路径行驶，并且满足车辆安全性、稳定性和舒适性的要求。purepursuit算法以目标点处车辆的横向偏差作为控制器输入，控制系统设计简单，大偏差和道路曲率不连续情况下同样适用，具有很好的鲁棒性。

purepursuit控制算法的前视距离受其他参数影响较大，而前视距离的选取是系统跟踪性能好坏的决定性因素。目前的purepursuit控制器设计中，通常将前视距离表示为纵向车速的一次或二次函数。专利cn106004996a中设定前视距离为定值，通过距离车辆最近点和前视距离处目标点之间的连线角度对前轮转角进行修正，虽然一定程度上改善了系统对目标路径的跟踪性能，但是随着车速的提升，跟踪误差会加速上升；专利cn104960520a将前视距离表示为纵向车速的二次函数关系，虽然在一定程度上比定前视距离系统跟踪路径的性能有所提升，但该方法未考虑到道路曲率对前视距离的影响，其跟踪精度随道路曲率增大而下降。另外，上述两种方案中，均未考虑路径跟踪过程中车辆的侧倾稳定性。

技术实现要素：

针对现有技术存在的问题，本发明需要提出基于改进purepursuit算法的一种智能车辆路径跟踪前轮转角补偿控制方法，即要提升purepursuit算法对目标路径的跟踪精度，还要考虑路径跟踪过程中车辆的侧倾稳定性。

为了实现以上目标，本发明的技术方案是：基于改进purepursuit算法的一种智能车辆路径跟踪前轮转角补偿控制方法，包括以下步骤：

步骤1：在carsim中建立车辆模型作为被控车辆参考模型；并设定模型输出量依次为：纵向速度vx、纵向位置x、横向位置y、车身侧倾角输入量依次为：前轮转角δf、后轮转角δr、前轮车轮转矩tf、后轮车轮转矩tr。

步骤2：基于车辆的实时纵向车速vx与纵向基准车速vd的车速偏差△v1、车身侧倾角设计纵向车速模糊控制器；其中纵向基准车速vd为被控车辆的参考基准车速，为已知设定值；纵向车速模糊控制器输出的纵向参考纵向车速vp分为两路：1)直接输入到改进purepursuit路径跟踪控制器；2)与车辆实时纵向车速vx做差得到△v2，△v2输入到车轮转矩pid控制器。

步骤3：基于车辆实时纵向车速vx和纵向车速模糊控制器输出的参考纵向车速vp，设计车轮转矩pid控制器，计算被控车辆跟踪参考纵向车速vp所需的车轮转矩tw，并将其作为被控车辆模型的前轮转矩输入。

步骤4：设计改进purepursuit路径跟踪控制器：改进purepursuit路径跟踪控制器根据车辆跟踪路径的偏差信息、目标路径信息和纵向车速模糊控制器输出的纵向参考车速vp，计算得到前视距离ld和车辆跟踪目标路径所需的前轮转角δ1；

步骤5：设计前轮转角补偿控制器：前轮补偿控制器根据车辆跟踪路径的横向偏差ey和方向偏差通过前轮补偿控制策略在线计算得到前轮补偿转角δ2，补偿转角δ2与δ1之和δf作为被控车辆前轮转角输入；

步骤6：运用粒子群算法对前轮转角补偿控制器的参数进行优化。

进一步，所述步骤1的被控车辆参考模型为前轮转向前轮驱动，后轮转向角度输入δr和后轮驱动输入tr均为零；

进一步，步骤2所述的车身侧倾角定义为向左为正，向右为负，所述的实时纵向车速vx与纵向基准车速vd车速偏差△v1定义为大于0为正，小于0为负；定义车身侧倾角和车速偏差△v1为纵向车速模糊控制器输入，纵向参考车速为纵向车速模糊控制器输出。

进一步，步骤2所述的纵向车速模糊控制器设计如下：

s1.车身侧倾角和车速偏差△v1作为输入量进行尺度变换到要求的论域范围[-6，6]内，并将其模糊分割为不同的模糊语言变量{nb，nm，ns，zn，zp，ps，pm，pb}；输出量的论域范围为[vd-1,vd+1]，其模糊语言变量为{nb，nm，ns，z，ps，pm，pb}。其中nb、nm、ns、zn、z、zp、ps、pm、pb分别代表负大、负中、负小、零负、零、零正、正小、正中、正大。

s2.采用专家经验法定义模糊规则库，模糊规则由64条如下if—then模糊语句构成：

ri：ifisaiand—△v1isbithenvpisci；

其中ai、bi、ci分别为第i条规则中输入变量输入变量△v1和输出变量vp对应的语言值，i＝1,2,……，64。

s3.选择三角形隶属函数和高斯隶属函数的组合作为输入变量的隶属度函数，高斯隶属函数和一般钟型隶属函数的组合作为输出变量的隶属度函数。另外，模糊逻辑推理采用mamdani法，运用加权平均法作为解模糊判决。

进一步，步骤3所述的车轮转矩pid控制器设计如下：

式中：kp2、ki2、kd2分别为比例、积分、微分系数；vp为参考纵向车速；vx为实时纵向车速。

进一步，步骤4所述的改进purepursuit路径跟踪控制器为：

式中：l为车辆轴距；ey为车辆与目标点处的横向偏差；ld为前视距离；

定义前视距离：ld＝0.5vp+△l1；其中vp为参考纵向车速；△l1为因道路曲率变化引起的前视距离补偿量，定义如下：

式中：kp1、ki1分别为比例、积分系数，ρ(t)为当前时刻目标点处的道路曲率。

进一步，步骤5所述的前轮转角补偿控制器设计如下：

式中：ey(t)为车辆与目标点处的横向偏差，定义目标点在车辆航向角方向的左边为正，右边为负；为车辆与目标点处的方向偏差，左边为正，右边为负；r1、r2为比例系数；q1、q2为积分系数。

进一步，步骤6所述的运用粒子群算法对前轮转角补偿控制器进行优化的参数为比例系数r1、r2和积分系数q1、q2；优化过程如下：

s1.种群初始化：随机产生m组4维粒子群，并逐个依次赋值给前轮转角补偿控制器的待优化参数r1、r2、q1、q2，针对每组4维粒子运行一次系统模型。

s2.选用平方误差积分准则：作为系统的误差性能指标，每运行一次系统模型，判断是否满足下列终止条件：

1)是否达到最大迭代次数n；2)系统的输出的平方误差j值是否小于j0。

其中n和j0根据具体路径和系统设定。

s3.若不满足终止条件，按照下式对该粒子进行更新操作，产生新的粒子，将产生的新粒子继续赋值给待优化参数r1、r2、q1、q2，直至满足终止条件将最后一次更新的粒子数值赋值给r1、r2、q1、q2，参数优化结束。

式中：ω为惯性权重；c1和c2为正的学习因子；r1和r2为0到1之间均匀分布的随机数；为第k次更新第i个粒子所在位置；为第k次更新第i个粒子更新的速度；为第k次更新个体极值；第k次更新全局极值。

和现有技术相比，本发明的有益效果为：

1)前视距离的计算同时考虑了车速和道路曲率因素，与传统purepursuit算法相比，计算得到的前轮转角更加精确，有效降低了路径跟踪过程中的行驶偏差。2)基于车身侧倾角和纵向车速偏差对车辆行驶的纵向车速进行微调，保证了在路径跟踪过程中提升跟踪精度的同时，兼顾了车辆的侧倾稳定性。

3)运用前轮转角补偿控制进一步提升了车辆对目标路径的跟踪精度，并运用粒子群算法对前馈补偿控制参数进行优化，避免了路径跟踪系统因系统参数调节不准确导致的跟踪性能下降的情况。

附图说明

图1是系统控制流程示意图。

图2是purepursuit算法前轮转向几何关系图。

图3是粒子群算法粒子更新流程图。

图4是粒子群算法优化前馈系统参数示意图。

图5是输入变量的隶属度函数示意图。

图6是输出变量的隶属度函数示意图。

具体实施方式

以下结合附图和技术方案对本发明做进一步说明：

如图1所示，本发明提及的路径跟踪前轮转角补偿控制系统包括被控车辆模型、改进purepursuit路径跟踪控制器、前轮转角补偿控制器、纵向车速模糊控制器和车轮转矩pid控制器5个部分。

控制系统的具体工作流程为：首先，给定目标路径和纵向基准车速vd。纵向车速模糊控制器根据被控车辆当前实际侧倾角实时纵向车速vx与纵向基准车速vd的车速偏差δv，对车辆的纵向车速进行微调，输出参考纵向车速vp。参考纵向车速vp与车辆实时纵向车速vx的偏差输入到车轮转矩pid控制器，控制车轮转矩大小，实现通过对车辆纵向速度的控制改善车辆侧倾稳定性的目的。改进purepursuit路径跟踪控制器根据目标路径信息、车辆当前的横向偏差ey和参考纵向车速vp计算得到前视距离ld和车辆跟踪目标路径所需的前轮转角δ1。前轮转角补偿控制器根据车辆当前的横向偏差ey和方向偏差得到前轮补偿转角δ2，实现对车辆前轮转角的补偿控制，提升对目标路径的跟踪精度。

本发明的具体实施步骤如下：

步骤1：在carsim中建立车辆模型作为被控车辆参考模型；并设定模型输出量依次为：纵向速度vx、纵向位置x、横向位置y、车身侧倾角输入量依次为：前轮转角δf、后轮转角δr、前轮车轮转矩tf、后轮车轮转矩tr。设定车辆为前轮转向、前轮驱动，因此后轮转角δr和后轮车轮转矩tr值为零。

步骤2：基于车辆的实时纵向车速vx与纵向基准车速vd的车速偏差△v1、车身侧倾角设计纵向车速模糊控制器；其中纵向基准车速vd为被控车辆的参考基准车速，为已知设定值；纵向车速模糊控制器可以根据纵向基准车速vd对被控车辆的行驶参考车速进行微调，当车身侧倾角较大时，输出较小的纵向参考车速vp，当车身侧倾角较小时，输出较大的纵向参考车速vp，达到跟踪路径的同时，一定程度上提升车辆的侧倾稳定性的目的。

步骤3：车身侧倾角定义为向左为正，向右为负，所述的实时纵向车速vx与纵向基准车速vd的车速偏差△v1定义为大于0为正，小于0为负；定义车身侧倾角和车速偏差△v1为纵向车速模糊控制器输入，纵向参考车速vp为纵向车速模糊控制器输出。其中纵向车速模糊控制器设计如下：

s1.车身侧倾角和车速偏差△v1作为输入量进行尺度变换到要求的论域范围[-6，6]内，并将其模糊分割为不同的模糊语言变量{nb，nm，ns，zn，zp，ps，pm，pb}；输出量的论域范围为[vd-1,vd+1]，其模糊语言变量为{nb，nm，ns，z，ps，pm，pb}。其中nb、nm、ns、zn、z、zp、ps、pm、pb分别代表负大、负中、负小、零负、零、零正、正小、正中、正大。

s2.采用专家经验法定义模糊规则库，模糊规则表如表1示。模糊规则由64条如下if—then模糊语句构成：

ri：ifisaiand—△v1isbithenvpisci；

其中ai、bi、ci分别为第i条规则中输入变量输入变量△v1和输出变量vp对应的语言值，i＝1,2,……，64。

表1模糊规则表

s3.为了灵敏的检测到车速偏差△v1和车身侧倾角的变化信息，选择三角形隶属函数和高斯隶属函数的组合作为输入变量的隶属度函数，其隶属度函数曲线如图5所示。同时前视距离的ld的变化过于灵敏则不利于系统的稳定性，因此，选择选择三角形隶属函数和高斯隶属函数的组合作为输入变量的隶属度函数，其隶属度函数曲线如图6所示。另外，模糊逻辑推理采用mamdani法，运用加权平均法作为解模糊判决。

步骤4：纵向车速模糊控制器输出的纵向参考车速vp分为两路：1)直接输入到改进purepursuit路径跟踪控制器；2)与车辆实时纵向车速vx做差得到△v2，△v2输入到车轮转矩pid控制器；其中车轮转矩pid控制器设计如下：

式中kp2、ki2、kd2分别为比例、积分、微分系数；vp为参考纵向车速；vx为实时纵向车速。当车辆实时纵向车速vx小于纵向参考车速vp时，增加前轮车轮转矩tf的输出达到控制车辆实时纵向车速vx上升的目的；当车辆实时纵向车速vx小于纵向参考车速vp时，降低前轮车轮转矩tf的输出达到控制车辆实时纵向车速vx上升的目的。

步骤5：设计改进purepursuit路径跟踪控制器；根据如图2所示的purepursuit算法前轮转向几何关系和ackermann转向几何原理设计改进purepursuit路径跟踪控制器为：

式中：l为车辆轴距；ey为车辆与目标点处的横向偏差；ld为前视距离；

因为前视距离ld为purepursuit路径跟踪控制器设计的唯一可变动参数，而速度和道路曲率对前视距离ld的选取都有较大影响，将前视距离设定为固定值或者速度的一次、二次函数显然是不能满足控制需求的，因此定义改进purepursuit路径跟踪控制器的前视距离为：ld＝0.5vp+△l1；其中vp为纵向车速模糊控制器输出的参考纵向车速；△l1为因道路曲率变化引起的前视距离补偿量，定义如下：

式中：kp1、ki1分别为比例、积分系数，ρ(t)为当前时刻目标点处的道路曲率。

步骤6：设计前轮转角补偿控制器；因为路径跟踪的目标不仅仅是路径的位置坐标，还有目标路径在目标点处的航向角，因前轮转角补偿控制器基于车辆跟踪路径的横向偏差ey和方向偏差进行设计：

式中：ey(t)为车辆与目标点处的横向偏差，定义目标点在车辆航向角方向的左边为正，右边为负；为车辆与目标点处的方向偏差，左边为正，右边为负；r1、r2为比例系数；q1、q2为积分系数。

当横向偏差ey(t)和方向偏差较大时，增加前轮转角补偿量使车辆加快跟踪上目标路径的速度；当横向偏差ey(t)和方向偏差较小时，减小前轮转角补偿量达到矫正过度转向量的目的。补偿转角δ2与改进purepursuit路径跟踪控制器输出的前轮转角δ1之和δf作为被控车辆前轮转角输入，对车辆进行转向控制。

步骤7：前轮转角补偿控制器参数优化；前轮转角补偿控制器的控制器参数r1、r2、q1、q2是前轮转角补偿控制器性能优劣的决定性因素，因此前轮转角补偿控制器参数的选取对整个路径跟踪控制系统至关重要。如图3所示为粒子群算法的粒子更新流程图，粒子群算法具有收敛速度快、需要调整的参数小，结构简单易于工程实现等优点，在工程上得到广泛的应用。因此运用粒子群算法优化比例系数r1、r2和积分系数q1、q2。如图4所示为运用粒子群算法优化比例系数r1、r2和积分系数q1、q2的流程图，优化过程如下：

s1.种群初始化：随机产生m组4维粒子群，并逐个依次赋值给前轮转角补偿控制器的待优化参数r1、r2、q1、q2，针对每组4维粒子运行一次系统模型。

s2.选用平方误差积分准则：作为系统的误差性能指标，每运行一次系统模型，判断是否满足下列终止条件：

2)是否达到最大迭代次数n；2)系统的输出的平方误差j值是否小于j0。

其中n和j0根据具体路径和系统设定。

s3.若不满足终止条件，按照下式对该粒子进行更新操作，产生新的粒子，将产生的新粒子继续赋值给待优化参数r1、r2、q1、q2，直至满足终止条件将最后一次更新的粒子数值赋值给r1、r2、q1、q2，参数优化结束。

式中：ω为惯性权重；c1和c2为正的学习因子；r1和r2为0到1之间均匀分布的随机数；为第k次更新第i个粒子所在位置；为第k次更新第i个粒子更新的速度；为第k次更新个体极值；第k次更新全局极值。