浅析自动驾驶4WS车辆路径跟踪最优控制算法仿真

模型预测控制算法设计

模型预测的机理可以描述为：在每一个采样时刻，根据获得的当前测量信息，在线求解一个有限时域开环优化问题 ，并将得到的控制序列的第一个元素作为被控对象，在下一个采样时刻，重复上述过程，用新的测量值刷新优化问题并重新求解。故将此算法分为三部分进行设计。 

预测方程建立

由（9）式可看出，本文所建立的车辆动力学模型是非线性的，对于复杂系统来说，非线性模型的在线求解难度比较大,很难满足自动驾驶车辆控制实时性的要求。为了计算简便 ，需要对此模型进行线性化近似。

模型预测方程求解

在自动驾驶车辆高速行驶过程中，往往会发生控制量突变的情况，使MPC控制量发散从而造成跟踪轨迹完全偏离目标轨迹，为了防止出现此现象在目标函数中加人松弛系数进行控制量软约束，故采用如下形式设计模型预测控制器的目标函数。

上式中的第一项反映了车辆对目标路线的跟踪效果，第二项反映了对误差的控制效果，第三项为加人的松弛系数，以调整约束范围，解决在硬性约束下该方程无法得到最优解的问题。

控制反馈机制

结合以上约束，将目标函数转化为矩阵二次型形式，利用内点法求解最优化问题，这样就可得到预测步长内的控制 (12) (13) (14) 序列的输人增量

权系数LQR后轮转角控制算法设计

权系数车辆状态方程

前文在前轮轮胎小侧偏角假设下建立了车辆模型，而在四轮自动驾驶车辆的实际运行过程中，在极限工况下轮胎侧偏角会超出线性假设的范围，这会导致原有的车辆运动学模 型不准确从而影响路径跟踪精度。由图2可以看出，当轮胎侧偏角小于0.15rad时,侧偏角与侧向力成线性关系且侧偏刚度较大。当侧偏角大于0.15rad时,侧偏角与侧向力成线 性关系且侧偏刚度较小。

图2 轮胎侧偏特性

为了使控制区域能够覆盖所有轮胎侧偏角。根据这两个区域的线性特性分别建立两个不同参数的二自由度车辆方程，并基于这两个方程设计权系数后轮最优控制算法。

线性二次型最优控制算法设计

LQR算法通过状态量与理想状态量之间的误差来构造评价函数，通过求解该评价函数来得到最优控制规律，也就是最优的后轮输出转角。

仿真结果

综上所述，整个算法流程如图3所示。

图3 算法联合仿真流程图

将参考轨迹输入到模型预测算法中，基于车辆状态构建目标函数，解算目标函数后，由“预测-跟踪 ”的流程输出每一步的最优前轮转角控制量。然后将前轮转角、y 轴速度和横摆角速度输入到权系数LQR算法中得到最优的后轮转向角，将得到的前、后轮转角一同输入到二自由度车辆模型中得到车辆状态反馈量，从而形成路径跟踪闭环控制。

为了验证算法效果，采用双移线工况进行路径跟踪仿真，路面附着系数μ取 0.8，模型预测的预测周期取 0.05，预测步长取 20，控制步长取 5，松弛因子取 10，权重系数取 1000。

分别在 30km/h 、60km/h 、110km/h 车速下进行仿真，路径跟踪效果如图所示，从图中可以看出，在 60km/h 以内的路 径跟踪效果基本没有变化，对速度的鲁棒性很好，而在高车速 110km/h 工况下，在转弯处跟踪路径出现了些许波动，出现此现象的原因是模型线性化的累积误差，使预测路径偏离。综合三种工况下的仿真结果可以得出此算法的路径跟踪效果良好，且对速度有较强的鲁棒性。

结合图 5-7 , 可以看到车辆在不同工况下的动态响应，随着车速增大，控制增量也在增大，但始终保持在约束范围内，满足高速下自动驾驶车辆操纵稳定性的要求。

图4 路径跟踪仿真效果图

图5 前轮侧偏角随时间变化图

图6 车辆横摆角随时间变化图

图7 纵向加速度随时间变化图

由图8中可以看出，考虑非线性轮胎侧偏特性的权系数 LQR后轮转角控制方法比基于线性特性设计的LQR方法横摆角速度更小。另外，在6s处车辆进行U型弯路径跟踪时，线性的LQR控制方法横摆角速度达到 5.5deg/s,这会增加车辆的驾驶难度，而权系数LQR方法将横摆角速度减小到了 5deg/s以下，有效地提高了四轮转向车辆路径跟踪时的操纵稳定性。 

图8 110km/h 车速下车辆横摆角速度变化图

本文所设计的模型预测控制算法对非线性车辆模型做了离散线性化处理，在每个控制步长内进行滚动优化，基于与参考轨迹的误差输出最优的前轮转角权系数LQR后轮转向控制算法基于前轮转角输入，考虑大侧偏角下轮胎的侧偏特性，设计权函数来对其进行线性拟合，输出最优的后轮转角。经过联合仿真表明，此路径跟踪算法误差最大为 0.34m (110km/h)，考虑车辆建模的精度，该误差在合理的范围内。相较于传统的反馈控制，对车辆横摆角、纵向加速度等车辆操纵稳定性参数有较好的控制效果，并且对车速有很好的鲁棒性。权系数LQR算法相较于线性LQR算法在 110km/h 工况下对横摆角速度的控制效果提升了15%。因此，本文设计的算法可以满足自动驾驶4WS车辆高速循迹性能要求。