适应道路曲率多变的前馈-预测LQR横向控制

孙福昌，邵金菊，单少飞，谢生龙

（山东理工大学交通与车辆工程学院，山东 淄博 255000）

0 引言

路径跟踪控制是自动驾驶关键核心技术之一，其控制效果是保证车辆安全稳定行驶的关键，是近年来自动驾驶研究的热点之一［1－2］。目前常用的路径跟踪控制方法主要有比例－积分－微分（proportional-integral-derivative，PID）控制［3］、线性二次型最优控制（linear quadratic regulator，LQR）［4－5］、纯跟踪控制［6］、模糊控制（fuzzy control，FC）［7］、滑模控制（sliding mode control，SMC）［8］、前馈-反馈控制［9］、模型预测控制（model predictive control，MPC）［10］等。其中LQR控制对于高中低速工况下均有优异的表现，尤其是对城市路况有着很强的适应性，且对硬件的要求并不高，应用广泛，但在某些极端的路况和大幅度偏离跟踪路径的情况下仍存在响应迟滞等问题［4］。早在2009年，美国斯坦福大学的Snider等［11］考虑道路曲率将LQR应用于路径跟踪控制中，但由于缺少前馈控制的修正，系统仍存在稳态误差。Kapania等［12］提出反馈＋前馈的方法，增强了LQR控制在固定道路上的跟踪稳定性，消除了稳态误差，但当道路变化明显增大时，由于Q、R矩阵无法调节，跟踪效果并不理想。马思群等［13－14］通过模糊控制的方法实现了对Q、R矩阵的实时更新，有效地提高了控制器面对多变道路时的性能表现，但由于计算较为繁琐，更新速度存在一定的滞后性。当道路连续变化时，很难及时更新出适合当前位置时刻的Q、R矩阵。为此，在考虑道路曲率变化的前提下，本文中提出了一种动态预测模块，以解决Q、R矩阵更新不及时的问题。对于预测点和预测时间的确定，大多数都是采用单点预测和固定预测时间，故在面对不同曲率的道路时，控制效果适应性较低［15］。本文中采用两点加权的方式，根据道路曲率，实时调节两点的权重，计算出适合当前道路的预测点以及预测时间。

综上，考虑道路曲率变化，采用误差动力学模型取代传统动力学模型，提出一种前馈-预测LQR反馈控制，实现了曲率多变道路的跟踪，解决了传统LQR方法Q、R矩阵更新不及时的问题。在给定规划路径的基础上进行了硬件在环实验，测试了单、双移线多车速工况下传统LQR和本文中设计的前馈-预测LQR的路径跟踪效果。

1 数学建模

1.1 汽车动力学模型

考虑侧向平移和横摆2个自由度，根据牛顿力学定律建立车辆动力学微分方程如式（1），车辆的受力分析如图1所示。

图1 汽车2自由度模型受力分析示意图

式中：m为汽车质量；ay、δ分别为汽车横向加速度和前轮转角；y为汽车坐标系下的横向位移；x为纵向位移；Fyf、Fyr分别为前、后车轮所受的侧向力；Iz为汽车绕垂直于地面的z轴的转动惯量；为横摆角加速度；a、b分别为汽车前后轴到质心的距离。

假设δ较小，cosδ≈1，式（1）则可以化简为：

式中：Cαf、Cαr分别为前、后车轮的侧偏刚度；αf、αr为前、后轮的侧偏角，由于侧偏角的方向与侧向力的方向相反，因此定义为负数。

为了进一步简化式（2），根据图2刚体运动的合成与分解，对后轮运动进行分析，可得到ay与y的关系，以及αf、αr具体表达式。

图2 后轮运动分解示意图

图2中v为质心处的速度，β为质心侧偏角。vx和vy分别为车辆坐标系下的纵向速度和横向速度。前轮与后轮原理相同，可得到式（3）。

求得αf，αr为：

1.2 误差模型

相较于经典动力学模型，基于误差模型的跟踪控制精度更高、控制目标更易实现。其基本控制原理是通过调节合适的控制量，使输出的实际值与参考规划值的偏差尽可能地小，从而达到精准控制的效果。

本文中假定参考规划值（xryrvrθr）已知，其分别为路径的纵向位移xr、横向位移yr、速度vr、航向角θr。实际位置与参考轨迹的位置关系如图3所示。

图3 实际位置与规划轨迹位置关系示意图

图3中蓝色点划线为参考轨迹，车辆实际速度为v，在参考轨迹上的投影点的切线和法线方向分别为τr和nr，投影点的速度为˙s，车辆与投影点的距离为d，d即为横向误差，表示为式（6）。

式中，k为曲率，由于本文中主要关注横向控制，因此纵向速度vx在计算过程中视为常数。令ed＝d，eφ＝φ－θr，可将2.1中建立的汽车动力学模型（5）转化为误差模型式（9），令u＝δ，为前轮转角。

其中：

2 横向LQR控制器设计

在误差模型的基础上，设计LQR控制器，通过不断控制前轮转角来减小实际位置与规划位置的横向误差、航向角误差，最终实现对规划路径的跟踪。

2.1 反馈控制

LQR求解原理来源于拉格朗日乘数法，将上文求得的误差线性微分方程式（9）作为约束条件，在此约束条件下求得代价函数，将代价函数进行离散化后求解其极小值，求解出极小值处的解即为此约束下的最佳的前轮转角控制量。

此处忽略误差模型式（9）中的，建立的代价函数如式（10）：

式中：Q和R为对角阵，分别代表误差状态E和控制量u的惩罚力度，Q或R的值越大，代表对该变量的惩罚力度越大、重视程度越高。

由于式（10）中＝AE＋Bu是连续的微分方程，而给定的规划路径点为离散点，因此需要对其进行离散化，利用积分中值定理化简得：

采用中点欧拉法对E（ε）进行化简，同时采用向前欧拉法对u（ε）进行化简，并忽略较小值1／（I－Adt／2），进而式（11）简化为式（12）：

式中：I为单位矩阵；E（t）为当前时刻的误差；E（t＋dt）为下一时刻的误差；u（t）为当前时刻的控制量即前轮转角。

依据离散后的约束方程式（13），并考虑约束范围：E0～En，u0～un－1，可得到目标函数如下：

根据拉格朗日乘数法，由式（14）可构造代价函数如式（15）：

引入哈密顿方程对式（15）进行处理：

对代价函数式（16）求偏导：

根据向量导数求解法，对式（17）进行求解可得：

由式（18）可知，最终求解的控制量为：

式中只有λk＋1为未知量，R－和B－皆为已知量。联立式（18）方程可推导出λk的表达通式为λk＝2PkEk。其中Pk符合Riccati方程：

Pn代表末尾时刻的Pk，且Pn＝Q。因此可由已知的Pn逆求此时刻的Pk，又因为该迭代在经过几十次迭代后就会迅速收敛，即Pk－1＝Pk，Riccati方程（20）可转化为方程（21）：

在此基础上即可求解出最终所需要的LQR控制的反馈控制量uk：

2.2 前馈控制

根据误差模型式（9），将式（22）求得的控制量uk代入的误差模型为：

分析式（23）可以发现无论l取何值，和E无法同时为零，即误差和误差变化率始终不能同时为零，势必存在较大的稳态误差。因此在此处引入前馈控制量δf以抵消稳态误差，式（23）可表示为式（24）：

通过使E＝0且＝0求解出的前馈控制量δf为：

式中，l3为矩阵l的第三列。δf的值与反馈控制中的l有关，因此在求解控制量时需先求解反馈控制再求解前馈控制。由此可得LQR控制器的最终控制量u为：

2.3 Q、R矩阵更新

LQR算法通过优化状态量和控制量的加权之和来实现控制目的。但在道路曲率多变时，若不能根据道路的特征对Q、R矩阵进行实时调整，系统容易失稳。因此本文中根据路径跟踪的横向位置误差ed和航向误差eφ建立模糊规则，实时调整Q、R矩阵，使算法更适应当前道路的变化，如图4所示。其中，权重因子η为模糊规则输出，R矩阵为1，Q矩阵为10η的对角矩阵。

图4 模糊控制原理图

建立的模糊规则如表1所示。当航向误差和横向位置误差都比较大时，无论是正大还是负大都是远离参考路径，因此需要较大的指数因子η，以增加对误差的重视程度，放宽对控制量的限制。当横向位置误差较大，而航向误差较小时，此时的横向位置误差可通过较小的航向误差自主修正。同样，当航向误差较大，横向位置误差较小时也可以实现自主修正，不需要过度重视误差，此时只需要较小的指数因子η。

表1 模糊规则表

隶属度函数如图5所示，横向位置误差和航向误差与η的关系如图6所示。

图5 变量隶属度函数

图6 横向位置误差以及航向误差与η的关系

模糊控制器依据实时的横向位置误差、航向误差和制定的模糊规则计算得出Q矩阵的指数因子η，通过不同的指数因子对Q、R矩阵比例关系进行调节，进而实现对Q、R矩阵的实时更新。

调节指数因子η对跟踪效果的影响如图7所示。当η较大时，系统更加注重跟踪误差，因此会以更大的控制量来保证跟踪效果。当η较小时，对误差的关注度减小，对控制量大小的重视程度增大，因此跟踪效果会有所下降。

图7 η变化对跟踪效果的影响

2.4 动态预测模块

传统的LQR以当前时刻的位置和状态作为输入量进行求解，缺少预测性，在连续变化的道路条件下会存在一定的控制滞后性。因此本文中增加一个预测模块如图8所示，通过预测点与轨迹点之间的误差进行预测控制，并根据道路曲率的变化对预测点以及预测时间进行实时调整，解决传统LQR响应迟滞问题。

图8 预测模块

图8中，最佳的预测位置是通过N和F两个固定预测点加权之后得到，其中N表示近预测点，F表示远预测点，最佳预测点可表示为：

μ∈［0，1］为决定最佳预测点位置的权重参数，与道路的曲率k有关，μ＝μ（k）。μ是关于k的正相关函数，当道路曲率越大时，为提高算法对规划路径的跟踪性能，较大的μ会增大近预测点的权重，使最佳预测点较近，减小预测时间，提高预测频率。相反，当在曲率较小乃至直线道路下，较远的预测点和较长的预测时间可保证行驶过程更加平稳。权重μ的具体表达式为：

式中，tp（k）为驾驶员预瞄时间估计模型。选取5名不同年龄段的驾驶员操作模拟驾驶器，以20 km／h的车速进行实验，记录预瞄时间与道路曲率，最终将记录数据进行拟合，拟合曲线如图9所示，得到的驾驶员预瞄时间估计模型如式（29）所示。

图9 驾驶员预瞄时间拟合曲线

预测时间t可通过最佳预测点计算得出：

根据计算得到的预测时间t，预测模块可根据当前时刻状态（x，y，φ，vx，vy，˙φ）计算得到t秒后预测状态（xp，yp，φp，vxp，vyp，˙φp），其中（x，y，φ，vx，vy，˙φ）分别表示纵向位置、横向位置、横摆角、纵向速度、横向速度和横摆角速度，下标p表示预测点。

3 硬件在环实验

3.1 硬件在环实验验证

本文中通过硬件在环实验来验证所设计的前馈-预测LQR算法的有效性。根据图10所示的算法流程搭建控制仿真模型，基于图11所示的硬件在环实验平台实现Carsim和Simulink的联合仿真。将Carsim测得的航向角及前轮转角等数据传输到Simulink，经本文中设计的前馈-预测LQR算法计算得到所需的前轮转角，并将其发送给台架进而实现方向盘的控制，然后通过NI-XNET读取台架传感器信息获得实际方向盘转角、转矩等信息，实现完整的硬件在环实验，实验原理如图12所示。

图10 算法流程

图11 硬件在环实验台架

图12 硬件在环实验原理

依据40、60、80 km／h三种车速下单、双移线工况的跟踪效果来进行评价分析。本实验采用的整车参数如表2所示，路面附着系数设为0.8。

表2 整车参数

3.2 多工况轨迹跟踪仿真分析

分别用传统LQR控制和本文中设计的前馈-预测LQR控制在40、60、80 km／h，单移线工况下进行轨迹跟踪测试，测试效果如图13—图15所示。

图13 40 km／h跟踪效果

图14 60 km／h工况跟踪效果

图15 80 km／h工况跟踪效果

为验证本文中所设计的前馈-预测LQR控制面对复杂道路时的跟踪效果，采用双移线工况进行测试，测试效果如图16—图24所示。

图16 40 km／h工况横摆角曲线

图17 40 km／h工况前轮转角曲线

图18 40 km／h工况跟踪效果

图19 60 km／h工况横摆角曲线

图20 60 km／h工况前轮转角曲线

图21 60 km／h工况跟踪效果

图23 80 km／h工况前轮转角曲线

图24 80 km／h工况跟踪效果

3.3 仿真结果总结和分析

如表3所示在单移线多车速工况下，本文中设计的前馈-预测LQR控制在跟踪效果方面优于传统LQR，能够更好地跟踪目标轨迹。在40 km／h的工况下，本文中设计的预测LQR控制器跟踪效果高达99.2%，传统LQR为96.7%，误差减小2.5%。在60 km／h的工况下，本文中设计的预测LQR控制器跟踪效果高达98.9%，传统LQR为95.5%，误差减小3.4%。在80 km／h的工况下，本文中设计的预测LQR控制器跟踪效果高达98.4%，传统LQR为93.9%，误差减小4.5%。

表3 单移线多车速下传统LQR与预测LQR %

在更复杂的双移线多车速工况下的跟踪测试效果如表4所示。在40 km／h的工况下，本文中设计的预测LQR控制器跟踪效果高达96.7%，传统LQR为91.3%，误差减小5.4%。在60 km／h的工况下，本文中设计的预测LQR控制器跟踪效果高达92.9%，传统LQR为83.4%，误差减小9.5%。在80 km／h的工况下，本文中设计的预测LQR控制器跟踪效果高达84.5%，传统LQR为75.6%，误差减小8.9%。

表4 双移线多车速下传统LQR与预测LQR %

4 结论

针对传统LQR横向控制对高速工况、曲率较大道路适用性较差的问题，本文中提出了一种前馈-预测LQR反馈横向控制方法。

1）基于误差模型，根据航向和横向位置误差，通过模糊规则实时调节Q、R矩阵，解决了传统LQR在连续变化道路上稳定性差的问题，扩大了应用范围。

2）设计了基于2点预瞄的预测模块，实时更新预测点及预测时间，解决了传统LQR响应迟滞问题。

3）通过Simulink-CarSim的联合仿真和硬件在环实验对本文中所提算法进行了验证。结果表明，在单移线多车速工况下，本文中设计的预测LQR相比于传统LQR轨迹跟踪误差分别减少2.5%、3.4%、4.5%；在双移线多车速工况下，本文中设计的预测LQR相比于传统LQR轨迹跟踪误差分别减少5.4%、9.5%、8.9%，较好地实现轨迹跟踪。