基于滚动优化的汽车换道动态轨迹规划和轨迹跟踪控制

岳丽姣，时 利，孟建平

（1.安徽江淮汽车集团股份有限公司 智能网联汽车研究院，安徽 合肥 230601；2.合肥工业大学 汽车与交通工程学院，安徽 合肥 230009）

引言

自动换道控制作为自动驾驶技术的核心功能模块之一，已成为智能汽车领域的重点[1]。实现自动换道控制的基础是进行换道轨迹规划与轨迹跟踪控制[2]。

为实现换道控制，一些学者使用AI*算法[3]、RRT[4]等基于搜索的方法，虽然这类方法可以在复杂的道路环境下规划出无碰撞轨迹，但是没有考虑汽车的非完整约束，可能导致汽车无法跟踪规划出的轨迹。为获得局部最优轨迹，Huang等[5]将人工势场（APF）与MPC 相结合通过模型预测控制的优化算法来优化选择人工势场定义下的规划轨迹。虽然该方法能实现换道避碍，但是由于道路势场在两条车道中心处最低，换道时参考量相当于从当前车道中心突变为目标车道中心，从而导致侧向加速度较大，舒适性较差。基于多项式的换道轨迹模型具有形式简单、曲率连续、可以灵活调整轨迹等优点[6]，能有效地解决上述问题。如LIE 等[7]学者在无障碍物环境下利用多项式规划一条车道变换轨迹，基于积分反步法设计轨迹跟踪控制器，实现安全换道。Yang[8]等根据换道起点和终点的边界条件结合考虑避障设置的约束，求取多项式参数，获得换道轨迹。大多基于多项式换道轨迹的研究中，轨迹模型虽然考虑了与周围车辆避撞的问题，但只是在换道初始时刻进行了参数计算，不能避免整个换道过程中因周围车辆状态发生变化而造成的碰撞危险[9]。为此，Chebly Alia 等[10]在基于多项式轨迹的基础上，引入基于回旋曲线的轨迹簇，并从中筛选出一条无碰撞轨迹，但是局部轨迹是否合适取决于轨迹簇的待选轨迹条数，轨迹条数越多，选出的轨迹越接近最优轨迹，但待选轨迹条数的增加会导致计算量增大，换道系统的实时性变差。

为使换道车辆能有效的应对前方车辆的状态变化，本文提出了一种基于滚动优化的换道动态轨迹规划和轨迹跟踪控制方法。所设计方法的结构如图1 所示，其主要由换道距离模型、换道参考轨迹模块、动态轨迹生产模块以及模型预测轨迹跟踪控制器组成。换道距离模型用于判断车辆是否需要换道。换道参考轨迹模块基于车-路信息，利用五次多项式的轨迹方程和由换道轨迹评价指标确定的换道时间共同构建出换道参考轨迹。动态轨迹生产模块以换道动态轨迹规划函数得到的动态轨迹和参考轨迹的偏离最小化以及相邻周期内所规划轨迹的偏差最小化为目标，设计滚动优化函数，结合包络椭圆约束、道路边界约束和稳定性约束，规划出动态换道轨迹。设计的模型预测轨迹跟踪控制器以规划的动态轨迹为目标，计算理想的前轮转角。最后由电机控制器控制车辆转向。

图1 换道控制方法的结构

1 车辆模型

本文采用包含沿y 轴方向侧向运动和绕z 轴横摆运动的二自由度的车辆模型。车身二自由度模型如图2 所示，其动力学模型可表示为：

图2 线性二自由度汽车模型

式中，β为车辆质心侧偏角；ω为横摆角速度；m为整车质量；Cf、Cr分别为前、后轮侧偏角刚度之和；lf、lr分别为质心到前后轴之间的距离；Iz为整车绕z 轴的转动惯量；δf为前轮转角。

在地面坐标系下，汽车运动学微分方程如下：

式中：x、y分别为车辆纵向和侧向坐标；φ为汽车航向角。在车辆稳定状态下，由二自由度汽车稳态转向模型得到车辆稳态横摆角速度:

2 换道动态轨迹规划设计

2.1 换道距离模型

当同车道前方存在低速障碍车辆时，为保证行车安全并提高通行效率，如果目标车道有充足的换道空间，被控车辆与前方障碍车辆之间的距离达到换道请求距离时，被控车辆产生换道需求，并进行换道参考轨迹规划。换道请求距离为：

其中，εv为道路法规规定的最小行车间距。如图3 所示，为了保证换道过程中的安全性，换道结束时在纵向上与原车道前方障碍车辆仍留有一定的安全裕度值D3，则：

图3 换道距离

式中，v为自车车速，vobs为前方障碍车辆车速，T1为换道时间。

2.2 换道参考轨迹规划

以换道起始时汽车质心为坐标原点，沿车身纵轴方向为x轴，沿横轴方向为y轴。要求规划出的轨迹能保证车辆侧向速度和侧向加速度连续，由换道起止点的6 个边界条件，可以确定多项式轨迹的6 个系数，所以本文采用5 次多项式来描述侧向位移轨迹：

换道的起始和终止时刻汽车侧向位移、侧向速度、侧向加速度分别为：

其中，H为车道宽度。结合公式（7）中边界条件，可求得换道参考轨迹的系数：

将所求系数代入到公式（9）中并对其进行二阶求导，可得到换道过程中的侧向加速度，即：

则侧向加速度最大值和最小值对应的时刻为：

为保证汽车在跟踪换道参考轨迹过程中的稳定性，对侧向加速度进行约束，侧向加速度满足约束条件：

联合公式（13）-（15），可求得换道时间T1应满足：

本文中取ay,max=－ay,min，=－ay,min=0.4g，即可求得最小换道时间T1min=2.35s。将T1min代入公式（8），求得满足侧向加速度约束的最小换道安全距离Smin。

换道时间越短，最大侧向加速度越大，换道时的乘坐舒适性变差；但换道时间越短，换道效率越高。为了综合行车效率和换道乘坐舒适性，构建换道轨迹评价指标：

换道轨迹评价指标随换道时间的变化曲线如图4 所示，本文选择换道时间为T1=3.6s。

图4 换道轨迹评价指标

2.3 换道参考轨迹规划

2.3.1 换道动态轨迹规划函数

为了保证换道过程中侧向加速度的连续性，较好的拟合出换道轨迹，结合当前采样时刻三个状态，要求动态轨迹多项式的阶数n≥3。阶数越高，拟合精度越好，但随阶数的增加，待优化参数数量增加，实时性变差。综合考虑拟合精度和实时性要求，本文采用四次多项式来表示动态换道轨迹：

当前采样时刻t0，结合汽车运动方程（5），则当前被控车辆横向行驶状态为：

即可得到：

其中：

则动态换道轨迹为：

式中，f（t）=[1t t2]，g（t）=[t3t4]。即可将u作为优化变量，实现对轨迹的优化。

2.3.2 滚动优化目标函数设计

为使动态规划的轨迹更接近换道参考轨迹，以保证汽车在换道过程中的舒适性和效率，对规划时域T内动态轨迹与参考轨迹差值的平方进行积分，从而获得与参考轨迹偏差的代价函数：

动态换道轨迹与参考轨迹偏差越大，其代价值越高。但仅以上述代价函数作为优化目标函数，则可能出现相邻两次规划周期内所规划的轨迹出现较大偏差，从而造成轨迹跟踪时方向盘出现较大抖动，为避免上述问题，对本次规划周期内规划的轨迹与上个规划周期内规划的轨迹偏差的平方进行积分，从而减小相邻规划周期内轨迹之间的偏差：

其中：ypref（t）为上个规划周期内规划出的轨迹。最终得到优化目标函数：

并可以离线求出滚动优化函数：

其中，ξ为与u无关的参数。

2.3.3 障碍车随动危险区域包络椭圆

实际情况下汽车行驶状态是连续变化的，短时间内不存在突变。基于此预测未来一段时间T内障碍车位置：

被控汽车在时间T内的纵向坐标为：

式中，x0为当前时刻被控车辆纵向坐标。以障碍车位置为椭圆圆心，以最小换道安全距离Smin为长半轴，对障碍车构建跟随其运动的危险区域包络椭圆，其边界为：

式中，Sy为横向安全距离。

2.3.4 约束条件

为防止与前方障碍车发生碰撞，将动态规划出的轨迹约束在前车随动危险包络椭圆范围之外，即当｜x（t）－xobs（t）｜＜Smin时有：

假设被控车辆可以理想的跟踪动态规划的轨迹，则换道过程中的侧向加速度为：

为规避跟随轨迹过程中因侧向加速度过大而造成的失稳可能，对汽车理想跟随轨迹过程中的侧向加速度进行约束，约束范围为：

为避免汽车驶出道路，将动态规划轨迹约束在道路安全区域范围内，防止被控车辆驶出车道边界或进入对向车道。

式中w为被控车辆宽度。将公式（18）分别代入以上约束公式中，即可求得对u的不等式约束，通过求解带约束二次规划问题，即可求得优化变量u，并将其代入公式（21），得到优化后的未来一段时间T内最优轨迹。

3 基于模型预测的轨迹跟踪控制器

为准确的跟踪规划出的动态轨迹，设计基于模型预测的轨迹跟踪控制器来获得理想的前轮转角。结合公式（4）、（5），以X=[y φ ω β]T为系统状态量，以前轮转角δf为控制量uc，可得离散化后的线性时变系统状态方程：

为保证控制量的连续性，防止控制量突变，需要对控制量增量进行约束，为方便处理，采用控制增量代替原控制量作为系统控制输入。并对系统状态量进行如下转换：

转换后新的系统控制输入为：

得到新的离散线性化状态方程：

其中，η（k｜t）=Y（k｜t）。设置控制器预测时域Np=8，控制时域Nc=5。

系统控制增量是未知的，通过设定适当的优化目标函数，对其进行优化求解，才能得到控制时域内的控制序列，并将控制序列中第一个元素作为实际控制增量作用于系统，实现车辆轨迹跟踪控制。根据参考文献[5]，构建以下形式的优化目标函数：

其中，Q 和R分别为权重矩阵。第一项反应轨迹跟踪能力；第二项反应对控制量平稳变化的要求；第三项为在目标函数中加入的松弛因子，防止无可行解的情况。考虑对控制量和控制增量的约束，控制量即前轮转角应被约束在机械结构所允许的最大范围之内，则有：

根据参考文献[10]并结合本文离散化周期，取△ucmax=－△ucmin=1.7deg。

4 仿真与试验

为准确地评估所设计的换道控制方法的有效性，进行CarSim/Simulink 硬件在环试验。根据设计的换道控制方法，进行前车紧急制动三种情况下的车辆换道硬件试验。采用基于LabVIEW/CarSim 联合的自动驾驶试验平台进行车辆换道的硬件在环试验研究。硬件在环试验台如图5 所示。

图5 硬件在环试验平台（换成LAC 的试验台）

试验中使用转向台架的实际转角代替计算到的期望方向盘转角，设置被控车辆车速为80 km/h、路面附着系数为0.85、仿真时间为6 s。在前方障碍车辆紧急制动情况下进行换道试验。

从图6（a）、（b）和（d）中可以看出，有动态轨迹规划的换道过程相较于无动态轨迹规划的换道过程，由于受到前方障碍车辆的椭圆危险包络椭圆约束，方向盘转角明显增大，车辆实际轨迹较大的偏离了换道参考轨迹，从而避免换道过程中自车与前车发生碰撞。从图6（c）中可以看出，在所设计的轨迹跟踪控制器的作用下，被控车辆的轨迹误差在换道过程中均保持在较小的范围内。另外，由图6（e）可知，换道过程中车辆的侧向加速度基本保持在约束的范围内，且变化平稳。

图6 硬件在环试验结果

从硬件在环试验的结果分析可知，在基于滚动优化的汽车换道动态轨迹规划和轨迹跟踪控制方法的作用下车辆具有良好的应对不同换道环境的能力。

5 结论

（1）针对换道过程中前车状态变化问题，本文在规划出换道参考轨迹的基础上，基于滚动优化思想，设计了以规划轨迹和参考轨迹偏差最小化为目标的动态轨迹规划方法，并基于模型预测控制设计了轨迹跟踪控制器。

（2）对所设计的换道控制方法在前车紧急制动工况下在基于CarSim/labview 的自动驾驶试验平台进行了硬件在环试验。硬件在环试验结果表明，本文设计的基于滚动优化的汽车换道动态轨迹规划和轨迹跟踪控制方法能使车辆有效的应对不同换道环境，在前车状态发生改变的情况下，能够保证车辆的换道安全性和舒适性。