MPC与Lyapunov的轨迹重规划与跟踪控制

孙小松，段 敏，汉红彪，杨 岐

(辽宁工业大学 汽车与交通工程学院， 辽宁 锦州 121001)

自从无人驾驶汽车被公认为将对未来汽车设计产生重大影响以来，其轨迹规划与跟踪就一直是研究热点。目前，国内外大部分研究仅考虑了原规划轨迹无影响下的跟踪控制，对轨迹重规划的跟踪控制问题的研究比较有限。汪佳兴等[1]利用B样条曲线局部性和凸包性对重规划算法规划出的离散点进行拟合以提高路径平滑性。王银等[2]采用自适应模型预测控制提高轨迹的跟踪与稳定性。潘公宇等[3]将魔术公式轮胎与单轨车辆模型结合，提出一种基于预测轮廓的控制策略。周维等[4]采用目标导向、节点修剪、曲线拟合和最优路径选择方法对基础RRT规划算法改进的同时联合MPC(模型预测控制)完成轨迹的规划与控制。姜立标等[5]以3自由度模型进行线性时变模型预测跟踪控制。周龙辉等[6]在模型预测控制中加入了LQR方法以保证跟踪横摆稳定性。宋晓华等[7]采用机械特性极限等数据对跟踪偏差进行了优化。杨博等[8]、邓涛等[9]对轨迹的规划与跟踪皆采用模型预测控制，虽此控制方式的精确性有所提高，但对执行机构的可靠性有一定负面影响。Wang Huiran等[10]采用可变预测视域的模型预测控制的路径跟踪方法。邹启杰等[11]通过对RRT算法进行强化学习(RT)改进来实现路径重规划。苏凯[12]在轨迹跟踪控制阶段采用模糊PID控制，虽然提高了控制的可靠性，但忽略了对横摆稳定性的控制。

因此，本文在引入障碍物干扰原轨迹的跟踪下进行研究，采用MPC与Lyapunov(李雅普诺夫)第二法设计控制器以实现轨迹重规划跟踪的目的，并通过仿真试验其性能、可行性，防止由环境因素引起不必要的事故发生。

1 无人驾驶汽车建模

1.1 汽车动力学模型

为满足控制性能与计算速度的要求，忽略悬架作用与空气动力学的影响，不考虑车辆的翻滚、俯仰运动，建立线性双自由度动力学模型(如图1所示)，转化为微分方程：

(1)

将式(1)转化为矩阵形式：

(2)

考虑车辆动力性能、稳定性与小角度模型，假设汽车纵向速度不变[13]，基于干燥、潮湿路面立建如下约束：

(3)

图1 双自由度动力学模型

1.2 汽车点质量模型

轨迹重规划(避障)模块中忽略车辆尺寸与加速运动中的轴荷转移应用点质量模型，如图2所示。

图2 点质量模型

其加入约束的数学模型为：

(4)

s.t.δg>ay|

(5)

2 轨迹重规划与跟踪控制器设计

本文控制结构采用双模块控制器协同控制，如图3所示，轨迹重规划模块通过接收障碍信息与原规划路径的Yref、Xref、φref实现轨迹重规划调整，将重规划后当下理想的跟踪参数输入自适应跟踪模块进行跟踪，跟踪过程该模块可调节前、反馈控制器参数值Ka、Kb，使理想跟踪参数的偏差e尽量小后完成无人驾驶汽车模型输入的精确取值δw。

图3 双模块控制器结构

2.1 MPC轨迹重规划控制器

2.1.1参考点与障碍物处理

在惯性坐标系XOY下取车身模型后轴中心点，做穿过此点平行于X、Y轴分别交原期望轨迹于点g1、g2。为使期望轨迹与重规划轨迹间偏移量Δη小，将取离目标终点距离最近的点为初始参考点gi(i=1,2,…)。

对障碍物的处理可将障碍点连结为一个密封点整体，防止重规划轨迹出现穿过两障碍点时车身尺寸不能通过障碍间隙的情况。在车身坐标系xoy做i个障碍点，则任意一点的横、纵坐标为：

(6)

式中：Yγ、Xγ分别为惯性坐标系下障碍点的横纵坐标与车辆质心点横纵坐标之间的差值；γ为x轴与障碍点间夹角。

2.1.2评价函数与控制器形式

车辆通过读取传感器得到的障碍信息，分别根据障碍物之间、障碍物与目标点的距离差进行聚类膨胀与评价函数值的调节，同时介于车速影响，设计任意采样时刻ti下的避障功能函数：

(7)

式中：xi,0、yi,0为车身坐标系下障碍物与质心坐标之间的差平方；Qobs为权重系数;z取极接近于0。

评价函数与障碍点坐标的关系如图4所示。

以避障为优先条件，降低原轨迹∂与实际重规划轨迹∂ref之间的偏移量设计MPC控制器形式为：

(8)

式中：Q为Y误差的评价矩阵；R为控制序列的评价矩阵；Np为预测时域；Ui为控制量合集。

图4 评价函数示意图

运用Matlab中opti工具箱式(8)进行非线性求解。

2.1.3轨迹拟合

为使轨迹跟踪控制模块有良好的跟踪效果，有必要设计不同的控制、预测时域应用于2个控制器模块。但为避免控制器不规范化的设计，满足车辆位置连续、横摆角连续等运动学约束，采用5次多项式法对重规划轨迹离散点进行拟合：

(9)

2.2 基于Lyapunov第二法的自适应轨迹跟踪控制器

采用车辆双自由度动力学模型建立实际轨迹与重规划轨迹之间误差状态方程，用矩阵形式表示：

(10)

为确保通过Lyapunov第二法设计的能量函数Ve(t)有解且满足控制系统稳定性，引入前、反馈矩阵，建立方程：

(11)

由上可得出新的Lyapunov方程：

(D1Ka1+D)J-1+J-1(D1Ka1+D)T=-J-1GJ-1

(12)

式中：Ka1+Ka2=Ka；J-1、G皆为正定矩阵。

通过D、D1矩阵的调控的同时利用Matlab控制系统工具箱中的dlyap函数可对J-1、Ka1进行直接求解、设计，并借此对能量函数Ve(t)进行详细构造：

(13)

因为：

(14)

(15)

式中：η1、η2为权重系数；J1=JD1Ka2,J2=JD1Kw。

显然此条件下：

(16)

式中，二者初值为零。

3 Carsim/simulink联合仿真

3.1 仿真工况与主要参数

原轨迹采用双移线工况，使车辆分别在附着系数为0.5的潮湿平坦路面与附着系数为0.85的干燥平坦路面进行基于速度38、58、78 km/h的仿真试验，验证控制器性能。主要控制器参数、整车主要参数如表1、2所示。

表1 主要控制器参数

表2 整车主要参数

3.2 仿真结果分析

3.2.1轨迹重规划性能仿真

试验场景为以躲避在原期望轨迹(预定行驶路径)处出现与其交汇且经膨胀处理后的障碍物为目的构造的，以增加障碍物对原路径的“干扰”程度，坐标如图5所示，取车速为38、58、78 km/h时控制器所规划的多数路径并决定新的满足约束条件且最终收敛于原期望轨迹的跟踪轨迹，与原轨迹对比，体现出二者间偏差与车速成正增长。

3.2.2轨迹跟踪性能仿真

图6为基于附着系数为0.85的干燥平坦路面下的轨迹跟踪仿真试验。图6(a)仿真结果显示，在试验开始阶段，车速在38、58、73 km/h转向时，侧向加速度的数值出现了0.005～0.3可控范围内的起伏且为提高路面可利用附着力使车辆转弯顺利，随速度增加其转向所需的侧向加速度越大，在其余阶段侧向加速度趋于平缓，体现了控制器在保证跟踪精度的同时满足了横摆稳定性。

图6(b)～(d)对不同车速下跟踪过程的纵向、侧向位移与全局位置变化进行导出，可以看出，其满足了对重规轨迹的精确跟踪且峰值偏差控制在0.25以内。随着车速的提高，跟踪偏差有微小的上升趋势，但最终的状态误差皆趋于0，这是由于控制器在时间无穷大时误差趋于0的设计原理决定的。

图6(c)～(f)显示，在各个车速下，横摆角无突出的波动，趋于平滑，体现车辆在重规划轨迹跟踪过程中处于稳定。轮胎非线性影响横摆角速度图像出现毛刺，但不影响车辆对行驶稳定性、控制器对控制精度的要求。

图5 不同车速双移线工况下轨迹重规划仿真结果

图6 u=0.85时不同车速双移线工况下轨迹跟踪仿真结果

图7为基于附着系数为0.5的潮湿平坦路面下的轨迹跟踪仿真试验。图7(a)(c)所示不同车速跟踪位置变化、横摆角速度与干燥路面下的仿真结果对比无明显变化，轨迹跟踪的性能度依旧保持高的精确度。图7(b)显示与对应干燥路面下的仿真结果相比，横摆角在采样时间为0.28 s左右时的稳定程度反而增加，这是由于控制结构中前、反馈控制器调节参数值Ka、Kb所致。

图7 u=0.5时不同车速双移线工况下轨迹跟踪仿真结果

4 结论

以车辆点质量模型与双自由度模型为基础设计双模块控制器，基于MPC算法与Lyapunov第二法确立了控制量与状态变量的同时借由实际轨迹与重规划轨迹之间误差状态方程设计能量函数，并引入避障评价函数和前、反馈矩阵进行轨迹的重规划与跟踪控制。通过联合仿真试验结果揭示此控制器具有较强的鲁棒性，能模拟多数轨迹的重规划，并尽可能降低轨迹偏移量，实现精确的轨迹跟踪，满足横摆稳定性要求。在轨迹跟踪阶段，本文为简化模型采用仅考虑横摆稳定性的双自由度模型，为进一步实现实用多元化，未来研究将引入侧倾、俯仰对重规划轨迹跟踪的影响。