一种新型基于多点预瞄的最优路径跟踪控制方法

摘要：为在嵌入式控制器开发环境下提高智能车辆的路径跟踪精度，采用车辆动力学模型和多点道路预瞄模型，以 预瞄窗口内的跟踪偏差为目标函数，结合 LQR 最优控制原理，提出了一种基于多点预瞄最优控制路径跟踪控制方法。针对实车应用，通过离线计算最优增益的方法，提高算法实时性。在仿真及红旗 H7 实车环境下进行试验，结果显示，该方法在保证跟踪精度的同时具有良好的算法实时性。

1.前言

路径跟踪作为自动驾驶系统中的关键执行层控制技术，是影响智能车辆安全性与舒适性的关键技术[1-3]。常见的路径跟踪方法按照使用模型不同可分为基于几 何/运动学模型的方法和基于动力学模型的方法，其中 基于几何/运动学模型的方法，通常将车辆简化为四轮 机器人刚性结构，由于相对简单计算量小，已有较多实 车应用，如名古屋大学的自动驾驶开源项目Autoware 中使用的纯跟踪方法[4]。

基于动力学模型的方法，考虑车 辆轮胎侧偏等动力学特性，通常结合最优控制理论，如线性二次型调节器（LQR）[5]、模型预测控制（MPC）[6]等。百度的自动驾驶开源项目Apollo 中横向控制器即提供了LQR 和MPC 两种方法[7]。LQR 方法未考虑预瞄前方目标路径，易出现跟踪偏差较大问题；MPC方法需要消耗大量计算资源，在车辆嵌入式控制器的计算环境中难以实现。而基于预瞄的LQR控制方法[8]，既能够综合考虑前方路径特征，又不需要在线优化求解占用大量计算 资源，具有较强的嵌入式环境实车应用价值。

因此，本文在文献[8]LQR预瞄控制方法的基础上， 提出了一种新型基于多点预瞄的最优路径跟踪控制方 法，改进了道路模型中的预瞄偏差计算方式，以及采用 离线计算最优增益的方法，提高了算法的适应性及实时 性，更加适合实车路径跟踪控制应用。

2.车辆及道路预瞄模型

2.1车辆动力学模型

本文采用的车辆动力学模型如图1 所示，假设车辆是一个在平面内沿一定速度向前行驶的刚体，可通过前轮转角进行横摆旋转和侧向平移运动。

图1 车辆动力学模型

通过如图1 所示的几何关系，可推导出前后轴的侧偏角αf、αr关系式：

式中，y为车辆横向位置；a、b分别为车辆质心到前后轴的距离；ψ为车辆航向角；u为车辆纵向速度；δsw为转向盘转角；isteer为转向传动比。

然后由车辆前后轴的侧偏刚度Cf、Cr ,可知前后轴的侧向力Fvf、Fvr:

最后，根据牛顿第二定律进行侧向力和横摆力矩分析即可得到式（5）所示的动力学状态方程。

式中：

其中，M为整车质量；I为车辆绕 Z轴横摆的转动惯量。

式（5）状态方程为连续量，在控制器中采用的是离散数字控制，需要根据实际采样时间T 对状态方程进行离散化处理，如式（6）所示：

式中：

由此状态方程即可确定车辆在一定速度下转向盘转角作为控制量，对车辆横向位置及横摆角速度状态产生的影响。

2.2 道路预瞄模型

图2（a）所示的为文献[8]所使用的道路预瞄模型，以道路在局部坐标系下的未来n个周期的参考y轴方向横向偏移作为道路状态量，当前车辆在局部坐标系中的横向坐标为y，朝向角为ψ，假设车辆纵向速度u恒定，当前车辆坐标为 y，第一个道路状态量即为 yr0，第二个状态量即车辆沿 x 轴方向移动 uT 后，对应的道路 y 坐标，即为图中的yr1，以此类推再下一周期同样x向移动uT，状态量变为yr2，yr3，yrn。

此模型采用的局部坐标系方向固定不变，车体坐标 系与局部坐标系夹角位置关系随着车辆运动不断变化， 当车辆朝向角与x轴夹角过大时，车辆的每一时刻x向参考位置的变化将与uT 差别较大，使得算法对于这种情况的适应性较差。因此本文的局部坐标系直接采用车体坐标系作为参考，车辆的初始y向坐标为0，航向角ψ也为0，这样车辆沿x轴的参考位置uT对应横向参考坐标随着车体坐标系的不断变化而不断变化，纵向速度假设更加合理，算法适应性更强。

图2 道路预瞄关系图

对于车辆而言，k时刻的道路状态yrn即为k+1时刻的道路状态yrn-1，只需将前一时刻的道路状态整体向前移动一个周期，同时补充最后一时刻的状态yr_n+1，可将道路状态整理为如下矩阵形式：

式中：

3.LQR最优预瞄控制

整合第2 章的车辆模型和道路预瞄模型的状态方程（6）和（7），可得下式：

此状态方程仅描述了车辆和道路间状态量的变化关系，而车辆和道路则需要通过式（9）所示的目标函数产生关联：

式中：

该目标函数为LQR 标准的二次型形式，一共包含两项，第一项目标为路径跟踪的横向与角度的综合偏差，C矩阵的第一行与式（8）的状态相乘，即连接了车辆的y轴坐标与道路参考点的y 轴坐标差，得到车辆横向偏差；第二行连接了车辆的航向角与道路朝向角的差， 得到车辆角度偏差，同时Q矩阵中的两个元素q1和q2决定了横向偏差与角度偏差的权重；第二项表示控制量转 向盘转角的惩罚项，目的为减少转向盘转角输出，提高系统稳定性，R为该项权重标量。

由上述目标函数和线性状态方程，即可构成标准的离散LQR 控制问题，从而求得由最优增益 K 和状态量[X(k),yr(k)]T构成的最优控制量u(k)：

最优增益K=[R+BTPB]-1BTPA，其中P矩阵可通过代数黎卡提方程求解：

当道路预瞄窗口较大时，上述矩阵维数将明显增多， 求解黎卡提方程将消耗较多计算资源，在嵌入式硬件环 境中实时求解难度较大。通过对最优增益矩阵中的元素 进行分析，可以发现只有车速u是动态变化的，其他元素均为车辆固有参数，因此，本文采用离线计算不同速度u下的增益矩阵K，然后通过在线查表的方式使用与当前车速u最邻近的离线增益K，然后与当前车辆状态、道路状态[X(k),yr(k)]T按式（10）计算即可获得控制输出转向盘转角，大幅提升算法实时性。

4.仿真及实车验证

4.1仿真结果

为验证本文算法的跟踪精度与算法实时性，在CarSim 和Simulink 联合仿真的环境下，将本文方法与文献[9]所提出的模型预测控制方法进行了对比测试，仿真对比结果见图3 和图4。

仿真所使用的为CarSim中的车辆，其质量为1 723 kg，转动惯量为4175kg·m2，质心距前后轴距分别为1.232m和1.468m，前后轴侧偏刚度分别为119552N/rad和109 548 N/rad。

仿真试验工况选用双移线轨迹进行车辆路径跟踪能力测试。分别选用常规小侧向力工况和达到路面附着极限的工况进行仿真，纵向速度分别为 60km/h和80km/h，路面附着系数为1，控制周期为20ms，MPC控制器的预测时域和控制时域均为50，权重矩阵Q和R的取值与文献[9]中保持一致；本文LQR控制器预瞄周期也为1s，权重矩阵系数q1=0.001，q2=0，R=1。

图4 侧向加速度仿真结果曲线

从路径跟踪结果中可以看出60km/h时LQR控制器与MPC控制器均能较好的跟踪目标路径，其中LQR控制器的最大跟踪偏差为0.36m，而MPC控制器的最大跟踪偏差为0.64m；而图4中可以看出最大侧向加速度约为0.7g，尚未达到路面附着极限，提出的LQR控制器能够较好地跟踪目标路径；而在80km/h的仿真试验可以看出最大侧向加速度达到1g左右，已经达到了路面附着极限，LQR控制器由于不具有MPC控制器的侧偏角约束，未能保持车辆稳定性，出现侧滑现象。另一方面，本文提出的LQR控制器与MPC控制器仿真单步运行时间分别为8.5ms和89ms，LQR方法具有明显的实时性优势。由此可见，本文提出的LQR控制器由于具有与MPC 控制器相同的优化目标，较为适合实车在附着良好的且未达到附着极限的行驶工况下进行实时控制。

4.2实车试验

实车控制器采用的是MicroAutoBox快速原型，LQR控制方法可在该平台下良好地实时运行，传感器为RTK- IMU 组合导航记录位姿，目标路径采用人工驾驶过程中录制组合导航的位置姿态数据，试验场地为一汽NBD 园区内，试验车辆为红旗H7 挚途试验车，如图5 所示。

图5 红旗H7 挚途试验车

园区内试验最大车速为30km/h，覆盖了长直线、长弯、环岛和直角弯等城市道路典型工况，试验结果如图6所示，整车最大侧向加速度在0.2 g 以内，横向控制偏差在0.35 m 以内，具有良好的跟踪精度；以120 s 附近的过环岛工况为例，转向盘最大转角为281°，路径跟踪的最大横向偏差为0.28m，转向盘转动较为平稳。

图6 实车路径跟踪结果曲线

图7所示为LQR控制器在弯曲道路换道工况下的实车路径跟踪结果图，图7a 所示为参考路径和跟踪路径，车速及跟踪的横向偏差如图7b所示，换道时车速约为60km/h，整体跟踪偏差在0.3m以内，最大侧向加速度约3.2m/s2。说明本文方法适合在实车嵌入式实时环境下，平稳地控制车辆实现良好精度的路径跟踪功能。

图7 弯曲道路换道工况路径跟踪结果图

5.结束语

本文针对嵌入式控制器环境下的智能车辆路径跟踪问题，基于LQR 最优控制方法并采用离线优化的方式，提出了一种基于多点预瞄最优路径跟踪控制方法， 构建了车辆及道路预瞄模型。仿真及实车试验表明该 方法能够适应常规工况下实车路径跟踪控制，具有良好 的跟踪精度。下一步研究将针对轮胎和路面附着极限 工况下，优化轮胎模型，约束最终控制量输出保证车辆 稳定性，提高算法对极限工况的适应能力。