变权重矩阵下的MPC轨迹跟踪研究

李培庆，杨正龙，陈一锴，李昊

(1.浙江科技学院机械与能源工程学院，浙江杭州 310023；2.浙江大学机械工程学院，浙江杭州 310058；3.合肥工业大学汽车与交通工程学院，安徽合肥 230009)

0 前言

目前无人驾驶车辆面临三大技术难题：环境感知、路径规划、轨迹跟踪，轨迹跟踪便是其中之一。无人驾驶车辆基于定位和高精度地图，通过激光雷达、毫米波雷达、摄像头等来收集数据，将这些数据融合，然后传递给路径规划层;规划层根据数据规划出一条符合当前时刻的参考路径，再传递给控制层;控制层接收到信号后，控制车辆的速度和转向尽可能地跟随参考轨迹。

针对轨迹跟踪，国内外学者进行了大量研究，目前主流的轨迹跟踪控制算法主要有：预瞄控制算法、模型预测控制算法、PID控制、LQR控制跟踪、纯跟踪控制(Pure Pursuit)等。FALCONE等提出了一种非线性的MPC路径跟踪控制算法，并建立了完整的十阶车辆模型和简化的自行车模型验证了算法的有效性。汪佳兴等提出了一种基于B样条曲线的MPC轨迹重规划算法，利用B样条曲线的局部性和凸包性的优势，提高规划路径的平滑性。CHEN等建立了一种考虑道路宽度和车辆几何结构的新型车路模型ribbon model，并在此基础上提出了一种新的带转向的轨迹跟踪控制器，通过仿真和实车验证了车辆可以根据道路情况自主调整安全、舒适、快速等控制策略。吴飞龙、郭世永对控制器参数进行优化，加入侧偏角软约束，从而保证了跟踪精度和行驶稳定性。梁忠超等设计了一种自适应MPC，通过不断更新卡尔曼状态估计器相关增益系数矩阵以及控制器的状态来保证车辆跟随的精度和稳定性。唐传茵等采用PSO算法对模型预测控制器中的预测时域和控制时域进行了优化处理，得到了不同工况和不同速度下的最优控制参数，从而使路径跟踪效果更进一步提高。章仁燮等提出了一种基于条件积分算法的无人驾驶车辆轨迹跟踪鲁棒控制方法，分别设计了运动学控制器和动力学控制器，通过仿真验证了能够实现无人车辆良好的轨迹跟踪效果。宋晓华等提出了基于车辆二自由度动力学模型的车辆轨迹预测改进方法，采用改进控制增量约束的方式，解决了车辆轨迹预测求解过程中二次规划方程变量上下限设置不当导致的车辆预测轨迹偏离期望轨迹的问题。邵毅明等设计了一种带有侧偏角软约束的线性时变MPC控制器，保证了车辆在稳定的前提下实现轨迹跟踪。

基于以上研究，本文作者在传统MPC理论的基础上研究了一种基于参考轨迹曲率的自适应模型预测控制方法。首先建立阿克曼转向特性的车辆运动学模型，通过速度和前轮转角来控制车辆，然后建立目标路径，根据轨迹曲率的变化动态改变权重矩阵大小。最后通过MATLAB/CarSim进行对比仿真，验证控制效果。

1 车辆运动学模型的建立

车辆在自动驾驶场景下，按照规划模块规划好的轨迹行驶，控制模块控制车辆尽可能地按照规划好的轨迹行驶，这就要求车辆的行驶要符合实际情况，满足车辆的运动学约束。采用运动学模型进行建模分析，可以满足车辆在轨迹跟踪过程中的运动学约束。

1.1 阿克曼转向

轮式车辆的转向形式一般有独立转向、阿克曼转向、铰链转向、差速转向。一般常用的转向方式是阿克曼转向，阿克曼转向是通过连杆机构实现左右两侧车轮在运动学上的协调实现转向。阿克曼转向的特点就是车辆沿着弯道行驶时利用四连杆的相等曲柄使内侧车轮的转向角比外侧车轮要大，从而使4个车轮行驶轨迹的圆心大致交汇于后轴延长线上的瞬时转向中心，即每个车轮都绕同一转向中心做纯滚动，使车辆可以顺畅地转弯。从而解决了车辆在转向的过程中由于两个前轮转角不同而具有不同的转向中心，引起转向不便和轮胎磨损的问题。

1.2 运动学模型

基于上述的阿克曼转向特性在大地坐标系()下建立车辆运动学模型，如图1所示。假设车辆是前轮转向、后轮驱动，并忽略空气阻力、悬架的作用。对于前轮转向、后轮驱动的车辆来说，在实际行驶过程中，其前轮的转向不一定等于当前车辆的速度方向。为了简化建模过程，采用单轨二自由度，这样其前轮方向就是当前车辆的速度方向。

图1 车辆运动学模型

用3个状态量(,,)表示当前车辆的位置信息，两个控制量(,)控制车辆的运动。故可以得到运动学方程：

(1)

其中:(，)是车辆后轴中心坐标;为当前车辆的横摆角;为后轴车速;为前轮转角;为轴距。

将式(1)表示为状态方程的形式：

(2)

2 模型预测控制

模型预测控制是一种最优控制算法，优势在于它是一个多变量控制器，通过考虑所有因素同时控制输出系统变量之间的交互，并且还可以处理多约束条件问题。模型预测控制主要分为三要素：预测模型、滚动优化、反馈校正。预测模型指的是根据被控模型的历史信息和将来的信息，预测控制对象未来的输出响应。滚动优化是指利用某一选定的参数指标达到最优化，来求解将来有限时刻的最优控制率。反馈校正是指根据被控对象的实际输出来对基于模型的预测输出进行校正并进行下一时刻的优化。模型预测控制原理如图2所示。

图2 MPC控制原理示意

2.1 预测模型

选取车辆作为被控对象，将其状态量表示为=(,,)，控制量表示为=(,)。此时整个系统可以表示成一个一般形式的控制系统：

(3)

轨迹跟踪的参考轨迹用下标r代表参考量，此时一般形式变为

(4)

接下来进行线性化处理，线性的目的是为了使最终的误差线性化。将式(3)在参考点采用泰勒级数展开并只展开到一阶，于是得到：

(5)

再将式(5)与式(4)相减，用·表示误差，于是得到：

(6)

(7)

(8)

然后再对式(8)中的车辆状态和控制量用前向欧拉法进行离散化处理，得到：

(9)

(10)

得到离散化的结果后再进行预测处理，用表示预测时域，表示控制时域得出系统的预测输出表达式：

=()+Δ()

(11)

2.2 优化求解

每一时刻，都会进行求解，要使当前的轨迹趋近参考轨迹，必须对控制量和当前车辆的状态进行约束和优化，故根据式(15)建立代价函数，使得当前车辆的行驶轨迹和参考轨迹的误差越小越好，更好地追踪轨迹。参考文献[10]采用如下的代价函数：

(12)

式中：和为权重矩阵；为权重系数；为松弛因子。此代价函数利于将问题转换成二次规划(QP)求解问题，利用MATLAB中的quadprog函数求解，此函数数学模型为

(13)

式中：、和为矩阵；、、、、和为向量。

将式(12)转换成式(13)中的二次型标准形式，得到：

(14)

式中:为预测时域内的跟踪误差。

2.3 反馈校正

在每一个控制周期内，由式(14)进行求解，得到当前控制时域的控制输入增量Δ()并作用于系统：

(15)

车辆随之运动变化，重新获得当前车辆的状态量(,,)，再进行预测、优化控制、反馈。重复上述过程，如此循环便实现了对车辆的轨迹跟踪控制。

2.4 约束条件

车辆在每个控制时域内都需要满足以下的约束条件：

(16)

式中：、分别为控制量的最小值与最大值；Δ、Δ分别为控制增量的最小值和最大值；、分别为轮胎侧偏角的最小值与最大值；、分别为质心侧偏角的最小值与最大值。

3 自适应MPC控制器

式(12)中权重矩阵和的选取非常重要。、一般选取对角矩阵，=diag(,…,=×)为状态误差矩阵，反映了对参考轨迹的跟随能力；=diag(,…,=×)为控制矩阵，反映了对控制量变化的约束或要求。其中为状态量的个数，为控制量的个数；和分别是状态误差权重系数和控制权重系数，文中都为同一值，都为同一值。

3.1 不同权重系数下的轨迹跟踪对比仿真

不同权重系数、的双移线轨迹跟踪对比仿真结果如图3和图4所示。

图3 不同r时的轨迹跟踪

图4 不同q时的轨迹跟踪

从图3中可以看出：当参考轨迹较趋于直线，曲线曲率较小时，不同的值下都可以比较好地跟踪轨迹，但是当参考轨迹由直线变为曲线，曲线曲率变大时，较小的值跟踪精度更高，横向误差更小；当值太大超过一定范围时，跟踪效果变差，跟踪轨迹出现震荡现象，行驶稳定性变差。从图4中可以看出：当参考轨迹由直线变为曲线，曲线曲率变大时，不同值下都可以相对较好地跟踪轨迹；当值和值相等时，跟踪轨迹到高点时出现了较大误差，并且在轨迹趋于直线时出现了波动；当曲线曲率较小，轨迹趋于直线时，值越大，跟踪效果越差，上下波动震荡越大，车辆稳定性越差。因此，可以考虑当参考轨迹曲率较小时，适当减小权重系数值，当参考轨迹曲率较大时，应该减小权重系数值。

3.2 变权重系数

基于以上分析，提出一种改进的算法，根据参考轨迹曲率更新权重矩阵、中的权重系数和。曲率是曲线弯曲程度的一种度量，轨迹曲率越大，曲线越弯曲越陡，表示车辆跟踪更为困难。当车辆速度较低时，转向容易，转向能力好；当车辆速度较大时，转向能力变弱，向心力变大，容易发生侧滑。车辆最大转向曲率应满足以下公式：

(17)

式中:为最大前轮转角;为车长。

将参考轨迹双移线拟合成以下函数：

=·sin(·+)+·sin(·+)+·

sin(·+)+·sin(·+)+·sin(·+

)+·sin(·+)+·sin(·+)+·

sin (·+)

(18)

拟合后的曲线和原参考轨迹相比，拟合程度较好，参考轨迹和拟合后的曲线基本吻合，如图5所示。

图5 拟合曲线

车辆在惯性坐标系下的纵向速度可以表示为

_DOT=·cos-·sin

(19)

当前时刻的参考轨迹的纵向位置为

()=+_DOT·

(20)

并根据以下公式算出参考轨迹曲率：

(21)

式中：()为当前时刻参考轨迹的纵向位置。将当前参考轨迹的纵向位置信息代入式(21)，算出当前时刻的参考轨迹曲率，调整权重矩阵的大小，从而影响控制量的大小，实时改变前轮转角和车速，使车辆能够较好地跟随参考轨迹，并提高行驶稳定性。

为了简便优化处理，首先将参考轨迹曲率进行归一化处理：

(22)

式中：为参考轨迹曲率的最小值；为参考轨迹曲率的最大值。

当权重系数较小时，控制系统响应整体趋于平缓，控制量变化小；当权重系数较小时，控制系统响应更加灵敏，控制量变化大。根据曲率的大小调整权重矩阵中权重系数、的大小。当较小时，表明参考轨迹曲率较小，轨迹趋于直线，较不弯曲，不需要大幅度调整前轮转向和速度，因此权重系数应适当减小一些；当较大时，表明参考轨迹曲率较大，轨迹较弯曲，跟踪困难，前轮转角和速度应及时调整，因此权重系数应减小。故采用如下的算式调整权重系数、：当曲率越小时，值减少得越多；当曲率越大时，值减少得越多。

(23)

(24)

式中：、、、为权重系数基本值；、为常数。

4 仿真实验

为了验证文中所提出的控制器的效果，基于MATLAB/CarSim进行联合仿真，通过CarSim搭建无人驾驶车辆运动仿真工况，设置主要输入及输出参数，利用MATLAB建立仿真模型以及S-Function函数编写。CarSim的输出作为MATLAB的输入，MATLAB的输出作为CarSim的输入，从而实现MPC控制算法，形成一个闭环反馈控制系统。

文中仿真车辆选取CarSim中E-Class/SUV车型，具体参数如表1所示。

表1 整车参数

为模拟车辆在现实中真实的行驶状况，选取双移线工况进行轨迹跟踪仿真实验。

4.1 两种控制器下的对比仿真

仿真实验中设定车速为36 km/h，控制器采用的参数为：预测时域=30，控制时域=15。MPC控制器权重系数经过多次仿真测试后选取轨迹跟随效果较好的一组数据：=2 000,=50 000。改进的MPC控制器自适应调整值和值。

图6为两种控制器下的车辆轨迹跟踪对比结果，可以看出：改进的MPC控制器最大横向误差为0.09 m，MPC控制器最大横向误差为0.17 m，两种控制器都能较好地跟踪参考轨迹，但是在参考轨迹曲率较大处、曲线较弯曲时，改进的MPC控制器能够更好地跟踪轨迹，精度更高，横向误差减小了0.08 m。这是由于当曲率较大时，改进的MPC控制器权重系数值减小，从而能更好地跟踪轨迹。图7为横摆角速度对比结果，可以看出：MPC控制器最大横摆角速度为8.19°/s，改进的MPC控制器的最大横摆角速度为6.80°/s，减小了1.39°/s，并且在参考轨迹由曲线变为直线、曲率变小时，改进的MPC控制器更加稳定，车辆行驶更加平稳舒适。这是因为权重系数值减小的原因，从而减少了波动。图8为质心侧偏角对比结果，可以看出：两种控制器的质心偏角的最大值均在极限范围内，小于8.91°，在稳定范围内，说明车辆在两种控制器下都能较好地跟随轨迹并保持稳定性。

图6 车辆轨迹跟踪对比

图7 横摆角速度

图8 质心侧偏角

4.2 变权重矩阵控制器对速度的鲁棒性

在不同的速度下对变权重矩阵控制器进行仿真实验，分析它对速度的鲁棒性。控制器采用的参数为：预测时域=25，控制时域=10。

图9—图10为变权重矩阵控制器在不同速度下的轨迹跟踪对比结果，可以看出：控制器在不同速度下能够较好地完成轨迹跟踪，并且在低速下精度更高，对速度具有较好的鲁棒性。图11为控制器在不同速度下的横摆角速度，可以看出：随着车速的增加，横摆角速度也随之增大。图12为控制器在不同车速下的质心侧偏角，3种车速下的质心侧偏角都在约束范围内，并且随着速度的增加，质心侧偏角随之变小。综上所述，变权重矩阵控制器对速度具有较好的鲁棒性。

图9 车辆轨迹跟踪对比

图10 车辆轨迹最高点局部放大

图11 横摆角速度

图12 质心侧偏角

5 结论

针对无人驾驶车辆轨迹跟踪问题，基于模型预测控制算法提出了一种改进优化的算法，即根据参考轨迹曲率来动态调整权重矩阵大小。MATLAB/CarSim仿真验证表明改进的MPC控制器相比权重矩阵是固定值的传统MPC控制器具有更高的跟踪精度和稳定性。