基于MMPC的无人汽车轨迹跟踪控制①

卢红生 刘 斌 蒋 峥 胡慧中

(*武汉科技大学冶金自动化与检测技术教育部工程研究中心 武汉 430080) (**冶金工业过程系统科学湖北省重点实验室 武汉 430080)

0 引 言

随着科技发展，人们对行车安全的要求越来越高，希望汽车能在无人操作的情况下完成自动行驶、自动避障等一系列自动驾驶行为。无人汽车的概念逐渐成为研究热点。

对于无人汽车，轨迹跟踪控制是必不可少的。首先无人汽车是一个典型非完整约束的系统[1]，其次车辆系统是一个高度耦合的复杂非线性系统，并且存在着相当多的不确定因素[2]，因此，无人汽车轨迹跟踪控制面临严峻挑战。国内外学者已经进行了相关研究，提出了用于无人地面车辆(unmanned ground vehicle，UGV)轨迹跟踪的多种方法，包括常规的PID控制，模糊控制[3]，滑模控制[4]，模型参考自适应控制[5]，神经网络控制[6]等。需要注意的是，这些控制方法均假设计算得出的控制量不会超过方向盘转角的物理限制，也忽略了车辆与道路的相互作用导致的侧滑可能性。无人汽车轨迹跟踪控制必须考虑车辆行驶过程的物理约束特性以及车辆与道路的相互作用。模型预测控制(model predictive control，MPC)是解决这一问题很好的方法[7]。2007年Falcone等人[8]将模型预测控制分别应用于无人汽车的线性模型和非线性模型的轨迹跟踪控制。近年来有很多学者将非线性预测控制应用于无人汽车轨迹跟踪[9]，虽然能减少跟踪误差，但其中的非线性滚动时域优化问题导致算法的实时性较差。因此，线性预测控制方法在无人汽车控制中仍占优势。文献[10]提出一种轨迹优化算法，将参考轨迹上车辆横向制导任务描述为有约束最优控制问题，并通过线性时变模型预测控制方案实现轨迹跟踪控制。Kim等人[11]提出了一种基于模型预测控制的轨迹跟踪控制算法，应用二次规划优化方法来计算最优转向控制输入。Zanon等人[12]为了验证MPC控制无人汽车的安全性、平稳性，在结冰路面上进行了轨迹跟踪实验。以上的研究均强调无人汽车在行驶过程中不违反物理限制，从而保证安全性及平稳性，但未关注无人汽车在载人情况下的舒适性。文献[13]提及了舒适性的概念，以及舒适性的判断标准，但并未将此项纳入无人汽车的轨迹跟踪控制。

本文采用多约束模型预测控制(multi-constraint model predictive control，MMPC)方法实现无人汽车的轨迹跟踪控制。将无人汽车的轨迹跟踪过程中舒适性标准纳入系统模型，将安全性描述为系统约束。为提高有约束滚动时域优化问题的求解效率，提出一种倒序乘子搜索算法，并通过CarSim-Matlab平台模拟实车情况下的轨迹跟踪控制，验证了本文所提出的多约束模型预测控制方法的有效性。

1 车辆动力学模型

实际车辆是一个复杂的运动体，为了方便设计控制器，本文采用2自由度车辆模型[14]。首先做出如下假设：

(1)车辆的纵向速度是恒定的；

(2)忽略悬架运动，只考虑X-Y轴的平面运动以及空气动力影响；

(3)把车辆的方向盘转角直接作为系统输入，并且假定左右轮转角相等。

基于以上假设，根据牛顿定律获得了如图1所示的车辆的2自由度动力学模型。需要注意的是，图1中x-y轴是根据ISO8855规定的车辆坐标系，以车辆质心o为原点，x轴指向车辆前方，y轴指向车辆左侧，与大地坐标系X-Y不同。图1中各物理量的含义如表1所示。

对于较小的轮胎侧偏角，横向轮胎力可以近似为关于轮胎侧偏角的线性函数。前后轮轮胎侧偏角αf、αr和前后轮的横向轮胎力Fxf、Fxr定义为

(1)

(2)

图1 车辆2自由度动力学模型

表1 车辆参数

参数含义o车辆质心Fxf、Fxr前后轮的横向轮胎力δ前轮转角lf、lr质心到前后轴的距离β质心侧偏角(速度方向与车身夹角)ψ横摆角αf、αr前后轮胎侧偏角(车辆轮胎在横向轮胎力的作用下,使车轮的中心线与垂直于地面的车轮平面错开了一定距离而形成的倾斜角)Cf、Cr前后轮胎侧偏刚度V纵向速度m车的质量IZ车辆绕Z轴的转动惯量

根据图1，可以得到汽车所受到的沿y轴的合外力以及绕质心的合力矩分别为

Fy=cosδFxf+Fxr

(3)

MZ=lfcosδFxf-lrFxr

(4)

在短时采样周期内前轮转角δ非常小，所以可近似认为cosδ≈1，可得：

Fy=Fxf+Fxr

(5)

MZ=lfFxf-lrFxr

(6)

Vy=Vsinβ

(7)

+V′cos(β+Δβ)sinΔψ

=V′sin(β+Δβ+Δψ)

(8)

由于β在实际中较小，Δβ、Δψ均为微分量，且模型假设车辆纵向速度为恒定大小，取ΔVy为两时刻的横向车速差，则由式(7)和式(8)可得：

=V(Δβ+Δψ)

(9)

由式(9)微分可得车辆的横向位移加速度ay为

(10)

根据式(5)、(6)和(10)可得车辆横向动力学方程：

(11)

(12)

联立式(2)、(11)、(12)可以得到车辆横向运动以及横摆运动的动力学方程为

(13)

图2 车速随时间变化的微分示意图

根据上述车辆动力学模型，并取系统控制输入量uc=δ得到连续的系统状态方程为

(14)

其中，

为了方便设计多约束模型预测控制器，首先将上述连续模型进行离散化得到如下离散状态方程：

X(k+1)=AX(k)+Bu(k)

Y(k)=CX(k)+Du(k)

(15)

其中A、B分别为矩阵Ac、Bc离散后得到的矩阵，C=Cc，D=Dc。

在轨迹跟踪的MMPC控制中，为保证各个输出变量的硬约束和软约束，以及控制量和控制量增量的硬约束均得到满足，需要对状态方程式(15)进行增广变换。

首先定义状态向量、输入量的增量形式：

ΔX(k)=X(k)-X(k-1)

Δu(k)=u(k)-u(k-1)

可得：

Y(k)-Y(k-1)=CΔX(k)+DΔu(k)

Za(k+1)=AaZa(k)+BaΔu(k)

Ya(k)=CaZa(k)+DΔu(k)

(16)

其中，

2 多约束模型预测控制器设计

2.1 预测方程

MMPC控制器设计的一个重要环节是基于预测模型对车辆在未来若干时刻的行为进行预测。

设当前时刻为k(k≥0)，预测时域为NP，控制时域为Nc(Nc≤NP)。用ΔUm(k)表示在k时刻计算所得未来输入增量序列；用Ym(k)表示在k时刻计算所得预测输出序列。

ΔUm(k)=[Δu(k),…,Δu(k+Nc-1)]T

Ym(k)=[Ya(k+1),…,Ya(k+Np)]T

基于模型预测控制原理[17]，可以推导出预测方程的表达式如下：

Ym(k)=FZa(k)+GΔUm(k)

(17)

其中，

G=

2.2 输出变量的参考目标值

(1)车辆质心侧偏角

(2)车辆横摆角角速度

(3)车辆横向位移加速度

按照以上计算方法得到的参考目标值也不能直接应用到参考目标向量中，因为这些参考目标值可能超出了车辆行驶的物理约束，或安全性和舒适性要求引起的约束。因此在参考给定向量化的过程中需要对每个时刻的每个参考目标值都进行约束校验，最终得到预测控制目标函数中的参考目标向量。以下是以车辆质心侧偏角为例的校验方法。

其中βmax为车辆质心侧偏角的最大限制。

2.3 目标函数及约束

取k时刻的目标函数为以下形式：

J(k)=[Rr(k)-Ym(k)]TQ[Rr(k)-Ym(k)]

+ΔUm(k)TRΔUm(k)

(18)

其中Q、R矩阵为适维加权矩阵。

在无人汽车轨迹跟踪控制过程中系统主要服从3类约束，即控制量、控制增量和输出量的约束。将控制量及其增量约束描述为

-C1Δδmax≤ΔUm(k)≤C1Δδmax

(19)

-C1δmax≤C1u(k-1)+C2ΔUm(k)≤C1δmax

(20)

将输出量约束描述为

(21)

其中δmax和Δδmax分别为车辆转向角的最大值和单个采样周期内的最大变化值。

MΔUm(k)≤N

(22)

其中，

2.4 二次规划求解问题

基于以上预测方程、目标函数及约束对未来控制量(前轮转角)增量序列求解。将式(18)形式的目标函数改写为二次规划标准形式，由式(17)、(18)、(22)得：

(23)

其中，

Em=2(GTQG+R)

Fm=-2GTQ[Rr(k)-FZa(k)]

显然，Em为正定对称的矩阵。为了最小化不等式约束下的目标函数值，本文考虑下面的拉格朗日表达式：

+λT(MΔUm(k)-N)

(24)

其中，λ为拉格朗日乘子向量，其维数为4Nc+8Np。 因此，原问题被转化为如下对偶问题：

(25)

对式(24)求ΔUm的偏导得：

(26)

将式(26)代入式(25)可将式(25)指出的对偶问题转换为以λ为决策变量的二次规划问题：

(27)

其中，

本文提出一种倒序乘子求解法求取次优解，减少算法的计算时间，增强控制系统实时性。将式(27)展开为

(28)

其中n=4Nc+8Np为λ的维数，hij,ki分别为矩阵Hm,Km中的元素，且由式(27)可得Hm为正定对称矩阵。

将(28)进一步展开为

+(h23λ3+h24λ4+…+h2nλn)λ2+k2λ2

…

(29)

由式(29)看出，JL(k)可分解为n个开口向上的二次函数，各二次函数的对称轴λi(i∈(1,n))对应该函数的最小值。式(29)中前n-1个二次函数的对称轴计算如下：

(30)

如果λn为一确定值，式(30)从下往上逆序求解可得由对称轴组成的λ向量。因为λn可唯一确定λi(i∈(1,n-1))，从而将求解多维向量λ的问

3 数值仿真

3.1 倒序乘子求解法求解二次规划问题

在进行无人汽车轨迹跟踪控制仿真前，首先分别用本文提出的倒序乘子求解法与有效集法对二次规划问题求解，从而对两者的求解精度以及求解速度进行对比。实验采用Matlab仿真平台，计算机CPU频率为2.3 GHz。求解的二次规划问题形式为

(31)

其中，JT为目标函数值，X为二次规划问题的解序列，ET、FT、MT及NT为系数矩阵。

图3 倒序乘子求解法流程图

表2所示为本文倒序乘子求解法与有效集法的求解结果对比。与有效集法相比，本文倒序乘子求解法是将二次规划问题转化为以一个拉格朗日乘子为决策变量的单变量寻优问题。而有效集法则是从初始解出发不断调整有效集，每次有效集的调整都需要按照多项规则判断，最终求得最优解。因此本文算法可大幅提高求解效率，由表2看出本文倒序乘子求解法的计算速度是有效集法的4倍左右。另外，本文求取最优解与有效集法求解精度接近。上表中目标函数值JT存在负数的原因是，在性能指标函数转化为式(31)的二次规划标准形式的过程中去掉了不影响解序列的无关正项。

表2 二次规划求解结果对比

然后，用这2种方法求解的MMPC控制器分别进行无人汽车轨迹跟踪控制实验。国内外也有较多学者以双移线轨迹进行无人汽车的轨迹跟踪能力测试。因此本次实验也采用双移线作为参考轨迹。它的表达形式如下：

(32)

式中，z1=(2.4/25)×(X-27.19)-1.2，z2=(2.4/21.95)×(X-56.46)-1.2，dy1=4.05，dy2=5.7，dx1=25，dx2=21.95，X为车的纵向位移。

取预测时域Np=20，控制时域Nc=5，权重矩阵Q=INp×Np、R=50×INc×Nc，控制器采样周期T=0.05 s，车速为36 km/h，用本文提出的MMPC轨迹跟踪控制对式(14)的2自由度线性汽车动力学模型进行控制，模型具体参数见表3。令基于倒序乘子求解法的MMPC控制器为控制器A，有效集法求解的为控制器B。

表3 无人汽车仿真实验参数表

图4、图5分别为用本文倒序乘子求解法和有效集法求解的MMPC控制器对无人汽车轨迹跟踪系统的仿真结果。图6为2种求解方法的计算时长。图7为约束条件下基于MMPC(倒序乘子求解法)的无人汽车轨迹跟踪系统仿真图。

图4 基于控制器A的无人汽车轨迹跟踪系统仿真图

图5 基于控制器B的无人汽车轨迹跟踪系统仿真图

图6 控制器A、B的求解时长

图7 约束条件下控制器A的无人汽车轨迹跟踪系统仿真图

图中X、Y轴分别为大地坐标系下的横、纵坐标。由图4、图5的控制效果可看出本文提出的控制器的跟踪性能较好。由图6的计算时长对比结果可看出，在实时性方面本文提出的倒序乘子求解法更好一些。需要说明的是，图7中设计的约束条件(δmax=0.0684 rad)不是实际无人汽车转向输入量的物理限制，是为了测试本文设计的倒序乘子求解法在约束条件下的控制效果。可看出本文提出的方法在满足约束条件情况下也能达到较好的控制效果。

3.2 CarSim-Matlab环境下的无人汽车轨迹跟踪控制系统仿真

为了验证本文提出的控制器的有效性，必须进行实车实验，但考虑到安全性和经济性等问题，本文采用CarSim-Matlab仿真来模拟实车的轨迹跟踪。CarSim是专门针对车辆动力学的仿真软件，可以详细定义整车各系统的特性参数。

将表3中汽车参数以及控制器所需的状态量设置到CarSim中，并在CarSim-Matlab平台上按图8所示的系统框图搭建基于MMPC控制器的无人汽车轨迹跟踪控制系统。参考轨迹仍为双移线轨迹，设定预测时域Np=25，控制时域Nc=7，权重矩阵Q=INp×Np、R=50×INc×Nc，控制器周期T=0.05 s，车速为72 km/h，得到相应的控制状态和跟踪曲线见图9和图10。

图8 基于MMPC控制器的无人汽车轨迹跟踪控制系统框图

图9 无人汽车行驶过程各状态图

图10 基于MMPC控制器的无人汽车轨迹跟踪控制系统仿真图

综上，本文设计的MMPC控制器能使用于无人汽车轨迹跟踪控制，具有良好的实时性，且无人汽车行驶过程中的安全性、舒适性均可以保障。

4 结 论

本文着重研究了基于MMPC方法的无人汽车轨迹跟踪控制，及车辆行驶过程中的安全性、舒适性问题。为解决行车舒适性问题，本文将横向位移加速度纳入系统模型，并加以约束；为改善算法实时性，提出了倒序乘子求解法对二次规划问题求解。通过与有效集法的对比实验，验证了本文倒序乘子求解法的有效性。最终，在CarSim-Matlab平台实现了无人汽车轨迹跟踪控制，检验了本文提出的MMPC控制器的实用性。