-
局部路径规划与跟踪是智能汽车研究领域的一项核心技术，它是指汽车根据多种传感器探测出的行驶环境信息，规划出一条从起始点到目标点的满足汽车运动特定性能要求和评价标准的无碰撞路线，实现路径的最优化，路径跟踪控制器根据汽车的运动学和动力学约束，输出相应的控制参数，使智能汽车对规划路径进行跟踪.
目前常见的局部路径规划方法有人工势场法[1]、模糊控制法[2]、遗传算法[3]、神经网络法[4]等.人工势场法(APF)最早是由Khabit提出的[5]，其基本思想是通过传感器感知出环境中机器人、障碍物、目标点的位置，障碍物对机器人产生斥力势场，目标点产生引力势场，机器人在引力势场和斥力势场两者产生的复合势场中，搜索总势场下降的路线作为机器人避撞的最优路径.该方法具有结构简单，实时性强，规划路径高效平滑等优点，被广泛应用于无人机等小型机器人避撞系统的路径规划[6-9].传统的人工势场法用于机器人避撞路径规划时，较少考虑机器人的尺寸范围以及边界环境的影响，而智能车辆在行驶时，则需要考虑道路环境的边界约束，同时要满足实际的运动学和动力学约束.因此，需要改进传统的人工势能场法才能满足智能车辆自动驾驶的需要.另外，对期望路径进行快速有效的跟踪，同样是实现智能车辆避撞功能的关键.路径跟踪控制方法应用较多的有PID控制[10]、前馈-反馈控制[11]、模型预测控制[12]等.当汽车低速行驶时，车辆运动学约束影响较大，而随着速度增加，动力学特性对运动规划与控制的影响就越明显，因此，路径跟踪的控制量应满足车辆的非线性动力学约束和执行机构极限约束，而文中选择的模型预测控制具有多重约束集成控制的天然优势.
因此，本文借鉴人工势场法的基本理论思想，构造基于道路环境以及障碍物的改进人工势场模型，规划符合汽车实际行驶过程中多约束条件的避撞路径.同时，基于模型预测跟踪理论对规划路径进行实时跟踪，并通过CarSim/Simulink联合仿真验证路径规划及路径跟踪效果.
HTML
-
车辆正常行驶和转向避撞完成后都是以主车道中线为主要行驶路线，因此本文以道路和车辆起点所在yd内的区域完成转向，在此区域车辆沿主车道向前时引力势能场下降，向主车道两侧运动时引力能势场位置建立二维坐标系，车辆前方主车道中线上各点为引力点，构建引力势能场函数.车辆在前方纵向距离上升且变化速度越来越快，所构建的车道引力势能场函数表达式为
式中，α为车道引力势能场增益系数，x0为车辆起点横向位置坐标，yd为转向区域最大纵向距离.根据公式(1)得到不同位置引力势能场的三维分布图如图 1所示.
-
由于车辆平面外形可以简化为长方形，结合对路径平滑度的考虑和最优路线的理论要求，采用椭圆化斥力场边界的概念.假设障碍物所在位置(xo，yo)，车辆的长宽分别为2L和2W，那么将斥力势场边界假设为一个符合车辆外形的椭圆方程
将椭圆化的二维斥力场边界扩展到三维空间，其斥力势能场的三维分布图可通过二维正态分布函数表示，其表达式为：
为了简化公式(3)取ρ=0，令β=(2πσ1σ2)-1，斥力势场模型表达式为
式中，β表示斥力势能场增益系数，ρ表示横向和纵向间的相关系数，σ1和σ2为控制避撞路径形状的参数，考虑到指数函数必定大于零的性质，在斥力势场表达式中加入一个极小的正数Up加以平衡.下面本文结合车辆避撞过程的实际需要设计参数σ1和σ2.
在对驾驶员转向避撞行为的大量研究中发现，避撞路径大多可近似为一条五次多项式曲线[13]，五次多项式曲线所形成的避撞路线，能使避撞过程中车辆的位移、速度、加速度曲线光滑且连续，符合车辆实际行驶过程中的要求.本文采用五次多项式的参考路径设计斥力势场模型，车辆转向避撞区域起始位置为(x1，y1)，完成避撞返回到主车道位置为(x1，y2)，最顶点位置为(x2，yo)，则此时车辆转向避撞过程中纵向行驶距离为d=|y2-yo|=|yo-y1|，横向偏移距离为b=|x2-x1|，为使车辆在(x1，y2)处返回主车道，此时斥力势能场应为零，由此可得参数σ2的表达式为
根据避撞路线上各点斥力势能场相等这一条件，得到参数σ1的表达式为
根据公式(4)得到不同位置斥力势能场的三维分布以及势场等高线如图 2和图 3所示.
-
车辆在道路行驶过程中，如果没有其他障碍物只考虑道路因素的影响时，道路左右边界位置危险系数最大，左右车道中线位置危险系数最小，道路中线危险系数介于边界和车道中线两者之间.根据道路不同位置的危险程度，此处采用变化趋势快慢程度不同的函数类型构建道路势场模型.当车辆位于左右车道两条中线之间位置时，车辆在此区域相对安全，采用幅值较小且变化平缓的三角函数构建势场模型；当车辆位于其它位置时，车辆在此区域危险系数剧增，采用幅值较大且呈变化迅速的指数函数构建势场模型.综合考虑以上因素设计的道路边界势场函数表达式为
式中，γ1，γ2为道路边界势能场增益系数，L为道路横向宽度，xl和xr分别为左右车道中线横向位置.根据公式(7)得到不同位置道路边界势能场的三维分布如图 4所示.
因此，车辆在不同位置的人工势能场为车道引力势能场和障碍物斥力势能场及道路边界势能场的总和，其函数表达式为
1.1. 车道引力势能场模型
1.2. 障碍物斥力势能场模型
1.3. 道路边界势能场模型
-
本文采用的是如图 7所示的三自由度的平面运动车辆模型[14]，即只考虑纵向、横向和横摆运动.坐标系oxyz为车辆坐标系，坐标系OXY为惯性坐标系，均满足右手法则.根据参考文献[14]中的推导过程，得到车辆非线性动力学模型微分方程表达式为
其中
$\xi = \left[ {\dot y, \dot x, \varphi , \dot \varphi , Y, X} \right]$ 为状态量，${\dot y, \dot x}$ 表示车辆质心处的横向、纵向速度，${\varphi , \dot \varphi }$ 表示横摆角及横摆角速度，Y，X表示惯性坐标系中车辆位置，μ=[δf]前轮转角为控制量，(仅考虑前轮转向，δr=0).非线性动力学模型由于计算量大，难以满足实时性要求，因此下面采用文献[15]中的线性化方法将公式(9)中的非线性系统进行线性化，得到连续的线性时变系统状态方程为式中，A(t)为微分方程表达式f相对于状态量ξ的雅克比矩阵，表达式为
B(t)为微分方程表达式f相对于控制量μ的雅克比矩阵，表达式为
对公式(10)进行离散化处理，选取η=[φ，Y]T为输出量，即横摆角和惯性坐标系下纵坐标，得到离散的线性时变系统状态方程为
式中，Ak，t=Im+TA(t)，Bk，t=TB(t)，
${C_{k, t}} = \left[ {\begin{array}{*{20}{c}} 0&0&1&0&0&0\\ 0&0&0&0&1&0 \end{array}} \right]$ ，m为状态变量维度，T为采样时间. -
为了满足车辆对期望路径快速而平稳的跟踪，目标函数的设计需要尽量减小系统输出变量与目标值的偏差并且能对控制量前轮转角进行优化.汽车在实际工作情况下必须避免控制量突变的情况，保证控制量的连续性，因此本文采用软约束的方法[16]，用控制增量替代控制量作为状态量，进行目标函数的设计并使其最小化，同时加入松弛因子，当系统没有最优解的时候，用次优解代替最优解，防止出现没有可行解情况.设计的目标函数如下：
式中，Np和Nc分别为预测时域和控制时域，ΔU(t)为控制时域内的控制增量，ε为松弛因子，Q，R，κ分别为输出量、控制增量、松弛因子的权重矩阵. ηref=[φref，Yref]为参考输出变量，Yref由本文第2节中改进的人工势场模型规划的避撞路径可得.当汽车质心侧偏角很小时，汽车的横摆角速度可近似为
结合第2节的路径曲线，假设汽车行驶路径和参考轨迹一致，则汽车的参考横摆角速度为
式中，ν为汽车质心点速度，ρ为参考轨迹曲率，对参考横摆角速度
${{\dot \varphi }_{{\rm{ref}}}}$ 求积分可求参考横摆角φref.为求解公式(14)中的目标函数，需将控制输入由控制量转变为控制增量，在此假设
得到新的状态空间表达式为
式中，
${\widetilde A_{k, t}} = \left[ {\begin{array}{*{20}{c}} {{A_{k, t}}}&{{B_{k, t}}}\\ {{0_{m \times n}}}&{{I_m}} \end{array}} \right], $ ${\widetilde B_{k, t}} = \left[ {\begin{array}{*{20}{c}} {{B_{k, t}}}\\ {{I_m}} \end{array}} \right], $ ${\widetilde C_{k,t}} = \left[ {\begin{array}{*{20}{c}} 0&0&1&0&0&0&0\\ 0&0&0&0&1&0&0 \end{array}} \right],$ n为控制量维度.汽车在避撞过程中受到车辆转向性能和道路条件的约束，在使用上述目标函数的模型预测控制系统中，需要满足控制量、控制增量和输出变量的约束如下：
控制量约束：
控制增量约束：
输出变量约束：
式中，μmin、μmax分别为控制量的最小值和最大值，Δμmin、Δμmax分别为控制增量的最小值和最大值，ymin、ymax分别为输出变量的最小值和最大值.
除上述基本约束条件外，在汽车速度较快情况下为保证汽车的稳定性和舒适性，需要加入对汽车动力学的约束，主要包括质心侧偏角约束和横向加速度约束.根据博世公司的车辆稳定性研究结果[17]，将质心侧偏角约束和横向加速度约束设置如下：
质心侧偏角约束：
横向加速度约束：
式中，aymin和aymax分别为横向加速度的最小值和最大值，横向加速度约束此处主要是保证舒适性，在此将其设置为软约束，ε为松弛因子，εmax=10.
上述目标函数和约束条件联立为一个优化问题，经过相应的矩阵运算将其转换为计算机容易求解的标准二次规划问题，若已知t时刻的状态量ξ(t)和前一时刻的控制量u(t-1)，即可运用有效集法求得控制时域内的控制增量序列：
将序列第一个元素作用于系统，得到下一个控制周期的控制量
进入下一个控制周期时，重复上述步骤，完成对车辆期望轨迹的跟踪.
2.1. 车辆动力学模型
2.2. 目标函数及约束条件
-
仿真实验通过采用CarSim软件建立车辆动力学模型，Simulink中的S-Function模块编写控制程序的方法，建立CarSim/Simulink联合仿真平台如图 8所示.
CarSim建立车辆动力学模型选用车型为C-Class，Hatchback2012，仿真过程所需的主要车辆参数如表 1所示，改进的人势能场主要参数如表 2所示.
Simulink中编写的MPC路径跟踪控制程序所需的主要参数如表 3所示.
假设本车在双车道道路上行驶，速度为72 km/h，道路宽度为8 m，车辆初始位置为(6，0)，前方障碍物位置为(6，60)，根据前文采取的转向避撞策略进行汽车局部路径规划，当转向避撞区域距离参数取yd=100，b=4，d=40时，路径规划及路径跟踪仿真效果如图 9和图 10所示.
由图 9和图 10可见规划路径能成功避开障碍物，且跟踪路径和规划的期望路径吻合度很高，横向之间距离误差在0.3 m以内，横向位置最右点离道路边界距离接近2 m，远大于实际中汽车车身宽度的一半，说明设计的模型预测控制器能安全而平稳地跟踪参考轨迹从而避开障碍物.
图 11和图 12分别为控制量前轮转角和参考横摆角曲线.从图 11中可以看出控制量前轮转角范围控制在±3°之间，满足约束条件，整体变化平缓，没有发生突变和振荡情况，符合车辆实际转向避撞工况下前轮转角的变化趋势；图 12反应汽车避障过程中实际横摆角变化平滑且连续，与参考横摆角存在较小偏差且整体变化趋势相同，说明汽车在路径跟踪过程具有良好的稳定性.
为了进一步验证汽车避撞过程中的稳定性和舒适性，图 13和图 14分别为质心侧偏角曲线和横向加速度曲线，图 13中质心侧偏角在±0.4°以内，满足良好路面下-12°≤β≤12°的约束条件，同时验证了前文中参考横摆角速度近似计算公式前提条件的正确性. 图 14中横向加速度曲线变化平滑，没有明显抖动，最大横向加速度小于0.6 g(g为重力加速度)，一般汽车最大横向加速度能达到1.5 g，说明汽车在转向过程有良好的转向稳定性和舒适性.
-
1) 本文将用于机器人路径规划的传统人工势场法进行改进，通过综合考虑车道引力势能场、障碍物斥力势能场和道路边界势能场三者对汽车的影响，建立适用于汽车避撞过程局部路径规划的改进人工势场模型，仿真结果表明，改进的人工势能场模型能规划出符合汽车实际转向过程的避撞路线.
2) 以CarSim中建立的高精度动力学模型为基础设计模型预测控制器，并对其添加控制量约束和高速行驶条件下动力学约束.模型预测路径跟踪控制器在快速平稳地跟踪期望路径的前提下，能同时满足执行机构的约束和动力学约束，保证汽车在避撞过程中具有良好的稳定性和舒适性.
3) 本文局部路径规划针对的是结构化道路和汽车纵向速度不变的理想条件下，下一步可以进行更为复杂道路环境和汽车运动状态下的转向避撞路径规划的研究，并在实车上进行验证.
DownLoad: