基于动力学模型预测控制的铰接车辆多点预瞄路径跟踪方法*

李斯旭，徐 彪，胡满江，边有钢，陈晓龙，孙 宁

（湖南大学机械与运载工程学院，汽车车身先进设计制造国家重点实验室，长沙410082）

前言

铰接车辆指前后车体间存在摆动环和铰接点的汽车［1］，其前后车体间夹角可达45°，机动能力好、越野能力强，加之其油耗小、成本低，在实际中被广泛应用。然而，铰接车辆操作困难，多用于矿山、山地等恶劣地形环境中，易引发事故，且这些作业环境往往伴随粉尘和噪声，也易对驾驶员的安全健康造成威胁。近年来，车辆自动驾驶技术快速发展，将其应用于铰接车辆，不但可替代驾驶员、避免人员损伤，还可有效提升工作效率和燃油经济性。

在车辆自动驾驶中，路径跟踪控制是核心技术之一［2］，而铰接车辆模型复杂、非线性强，其路径跟踪控制相比一般乘用车更加困难，也更具挑战性。目前，车辆路径跟踪控制技术已得到广泛研究，典型控制方法如模糊比例-积分-微分（PID）控制、线性二次调节控制、前馈-反馈控制、模型预测控制（model predictive control，MPC）等。Hayakawa等设计了PID路径跟踪控制器［3］，该方法结构简单，不依赖于精确的车辆模型，但无法对车辆未来状态进行预测，且对于非线性动力学的控制效果欠佳。Levinson等将线性二次调节器应用于路径跟踪控制［4］，提高了路径跟踪精度，但该方法难以显式满足车辆系统约束，在较复杂环境中难以达到理想的控制效果。Kritayakirana等针对赛车的路径跟踪问题设计了前馈-反馈控制器［5］，同样也有较高的跟踪精度，但其依赖于系统可逆性，应用范围有限。

近年来，MPC技术在车辆路径跟踪控制中得到了广泛应用。该技术能够将未来路径信息融入控制设计中，对速度和路径曲率的变化有良好的鲁棒性，且可实现多目标优化、显式处理状态变量约束及执行器饱和约束［6］，在车辆路径跟踪方面具有很大优势。基于MPC进行铰接车辆路径跟踪控制，可充分考虑其实际物理特性，显式处理前后车体间最大夹角、最大转向力矩等约束，提升控制方法实用性。

基于MPC的铰接车辆路径跟踪控制研究尚不多见。Nayl等首先将基于运动学的MPC应用于铰接车辆的路径跟踪问题［7］，但这种基于运动学模型的方法难以在路径曲率连续变化、低附着路面、高速行驶等复杂条件下保障良好的控制性能。窦凤谦将基于动力学的MPC应用于铰接车辆的路径跟踪控制［8］，取得了良好的控制效果，但该方法在模型线性化过程中作了较多简化，且未对线性化误差进行补偿，因此在较高车速、低附着和曲率连续变化情况下难以保证跟踪精度。为解决上述问题，本文中设计了一种新的基于DMPC（dynamic MPC）的路径跟踪控制方法，该方法对线性化误差进行补偿，从而得到一种改进动力学模型，并根据状态轨迹和实际状态量的偏差设计DMPC算法，以期提高路径跟踪控制精度。Nayl等设计了误差预瞄模型用以预测跟踪误差，但该方法基于预测时域内路径曲率不变的假设和前后车体间夹角为小角度的假设，实际的变化曲率较大及前后车体间夹角会导致跟踪精度的降低。为此，本文中直接根据路径函数生成参考路径点而非采用近似预瞄误差方程，提出了考虑多点预瞄误差的控制目标，以提高控制精度。

本文中首先构建铰接车辆路径跟踪动力学模型，而后设计DMPC控制器，最后通过Matlab/Simulink与Adams联合仿真验证所提出路径跟踪控制算法的性能。

1 研究方法

本节介绍预测模型构建和控制器设计方法。首先构建铰接车非线性车辆动力学模型，采用基于状态轨迹的线性化方法将其线性化，再近似离散化得到预测模型；而后基于预测模型，根据状态轨迹和真实状态变量的偏差设计约束条件、路径多点预瞄和目标函数，进而给出MPC优化问题。

1.1 预测模型

1.1.1 铰接车辆动力学模型

铰接车辆质量、尺寸参数如图1（a）所示，将铰接车辆液压转向系统等效为作用在前车和后车上的转向力矩Tn，铰接车辆受力情况如图1（b）所示。其中，x1、y1为前车局部坐标系坐标轴，Xw1、Xw2为前车轮胎受到的纵向力，Yw1、Yw2为前车轮胎受到的横向力，ω1、ω2为前、后车横摆角速度，T1、T2为前、后车在Tn作用下质心处受到的等效力矩，u1、v1分别为前车的纵向、横向速度，φ为前后车在铰接点处的夹角，φ̇为前后车夹角加速度，Xw3、Xw4分别为后车轮胎所受到的纵向力，Yw3、Yw4分别为后车轮胎所受到的横向力，u2、v2分别为后车的纵向、横向速度。

图1 铰接车辆动力学模型

如图1（b）所示，由达朗贝尔原理：

式中：Y1=Yw1+Yw2；Y2=Yw3+Yw4；X2=Xw3+Xw4；ta为半个轮距。

在路径跟踪过程中，两侧轮胎受力可近似为相等，且轮胎力可用近似的线性描述：

式中：Fs、Fα分别为轮胎纵向力和横向力；Cs、Cα分别为轮胎纵向刚度和侧偏刚度；s为轮胎纵向滑移率；α为轮胎侧偏角。

S2为后轮纵向滑移率；α1、α2分别为前、后轮侧偏角；Cs2为后轮纵向滑移刚度；Cα1、Cα2为前、后轮侧偏刚度；；u1为纵向速度，为定值，可控制铰接车辆的转向。

1.1.2 模型线性化

由于跟踪的目标为路径的X、Y坐标及路径的航向角，需要对式（6）模型进行扩维处理，使得车辆X、Y坐标及车辆航向角成为状态变量，选择χ=作为状态变量，u=Tn为系统输入控制量，其中X、Y为前车参考点全局坐标，θ为前车航向角，可得适用于模型预测控制的铰接车辆动力学模型：

式中：f=M-1F；

采用基于状态轨迹的线性化方法将铰接车辆动力学模型线性化［9］，该方法优点是不需要提供每一时刻的状态变量参考值。假设在某一采样时刻t，车辆状态变量为χ0，输入控制量为u0，假设铰接车辆在预测时域内的控制输入量为u0不变，得到一条状态轨迹͂，对前文推导的式（7）非线性模型在状态轨迹点进行泰勒展开，并忽略高阶项：

将式（8）重新整理，可得以χ作为状态变量、以u作为控制量的表达式：

1.1.3 模型离散化

取采样时间T，对铰接车辆动力学模型进行近似离散化［6］，取状态变量χ，控制量u，则k+1时刻状态变量χ(k+1)及用于表示目标函数需要的输出η(k)的表达式为

1.2 MPC控制器设计

1.2.1 约束条件

根据实际工况赋予以下约束：

式（12）表示铰接点处液压转向系统输入的转向力矩极值约束；式（13）表示铰接点处输入的转向力矩增量极值约束，其中ε≥0为松弛因子［6］，对控制量增量约束进行松弛，其能够对求解可行域进行扩展，避免在突然转弯等极限工况下无法在硬约束上下界内求得可行解，保证可行解存在。

1.2.2 路径多点预瞄

为消除基于预测时域内曲率不变假设和前后车体间夹角为小角度假设的误差模型产生的跟踪误差，提高路径跟踪精度，本文中直接根据期望路径函数生成路径点，由预测时域内期望路点的状态轨迹与预测的车辆行驶状态轨迹之差获得跟踪误差，设计考虑路径多点预瞄误差的目标函数。此外，基于文献［6］中方法，为综合考虑前方路径转向特性、提高铰接车辆转弯时的跟踪精度，计当前时刻 为t，以[t，t+N∙T]时域内路点位置及[t+k∙T，t+(k+N)∙T]时域内路点航向角为车辆期望状态轨迹，如图2所示，N为预测时域长度，各预瞄点间的距离为v∙T。

图2 跟踪误差示意图

将预瞄误差分为位置偏差ed与航向角偏差eh，其表达式为

式中：xr(t)为t时刻预瞄点的x坐标；x(t)为t时刻车辆的x坐标；yr(t)为t时刻预瞄点的y坐标；y(t)为t时刻车辆的y坐标；θr(t)为t时刻预瞄点的航向角；θ(t)为t时刻的车辆航向角。

1.2.3 MPC优化问题

为实现铰接车辆的精确跟踪，设置以下优化目标函数：

式中：ΔU(t)=[Δu(t)，Δu(t+1)，…，Δu(t+Np)]；Δη(t+i|t)=η(t+i|t)-ηref(t+i|t)，表示车辆的跟踪误差，可分解为如1.2.2节中的位置偏差ed和航向角偏差eh；ηref(t+i|t)为t+i时刻的系统输出量参考值，由该时刻参考路径的x坐标、y坐标、前车航向角组成；Δu(t+i|t)为在t时刻预测的t+i时刻的系统控制量增量；ε为1.2.1节中的松弛因子，为找到偏离硬约束最小的可行解，防止约束上下界被松弛因子过分放大，将松弛因子加入目标函数中进行限制；Q、R、ρ为权重矩阵。

设定新的状态量［6］：

可得预测时域内所有输出量的表达式为

将目标函数转变为标准二次型，在每个控制周期求解上述优化问题，得到最优控制增量序列：

取最优序列的第1项作为当前时刻的铰接车辆控制量增量。如此进行滚动时域优化控制至跟踪结束。

2 仿真分析

为验证所设计控制器的性能，建立Adams铰接车辆模型与Matlab/Simulink进行联合仿真，如图3所示，Adams模型各参数如表1所示，各参数含义见图1，轮胎选用UA轮胎，路面选用二维平路面。

图3 Adams铰接车辆模型

表1 Adams模型参数

仿真环境中设置螺旋线、左转、双圆3种仿真工况。铰接车辆初始X、Y坐标、前车航向角、前后车铰接点处夹角都为0，由于铰接车辆在不同前后车夹角、路径曲率等情况具有不同的速度上限，本文中选取纵向速度为3 m/s，可涵盖大部分转向情况速度上限。且每一种工况都将本文中提出的DMPC算法与文献［8］中提出的MPC算法进行比较。由于本文中只考虑铰接车辆的横向控制，故将用于对比的MPC控制器的输出量稍作调整，假设预测时域内车辆纵向速度不变，只对横向位置进行控制。仿真均使用i5⁃7200U处理器，采用内点法在Matlab中使用quadprog求解器进行MPC优化问题求解。

仿真中，设采样时间为0.05 s，两控制器参数均调节至最优，其中DMPC权重矩阵Q=5×1010I，R=I，ρ=1000I，预测时域Np=11，k=10。控制量Tn的约束设为-100 kN∙m≤u(k+t|t)≤100 kN∙m，控制 量 增 量 的 约 束 设 为-20 kN∙m≤Δu(k+t|t)≤20 kN∙m。轮胎纵向滑移率根据仿真测试数据采用估计值0.15，车辆纵向速度变化量在7%以内，本文控制器在每一采样时间都对车辆最近路点进行搜索并更新，以减小其影响。

2.1 螺旋线工况

图4为路径跟踪结果，可见两控制器均能控制铰接车辆跟踪参考路径。

图4 螺旋线工况路径跟踪结果

图5为位置误差及控制量随时间的变化。基于DMPC和MPC的控制器产生的最大位置误差分别为0.079和0.208 m，降低了62.0%，平均位置误差分别为0.041和0.143 m，下降了71.3%。基于DMPC和MPC的两控制器在仿真过程中最大控制量分别为32.486和48.594 kN·m，平均控制量分别为9.881和13.474 kN·m，平均控制量增量分别为0.770和1.189 kN·m。

图5 螺旋线工况位置误差及控制量变化

2.2 左转工况

图6为路径跟踪结果，可见两控制器均能控制铰接车辆跟踪参考路径。

图6 左转工况路径跟踪结果

图7为位置误差及控制量随时间的变化。基于DMPC和MPC控制器产生的最大位置误差分别为0.098和0.185 m，降低了47.0%，平均位置误差分别为0.043和0.094 m，降低了54.3%。基于DMPC和MPC的两控制器在仿真过程中最大控制量分别为42.515和63.627 kN·m，平均控制量分别为11.599和16.144 kN·m，平均控制量增量分别为0.968和1.502 kN·m。

图7 左转工况位置误差及控制量变化

2.3 双圆工况

图8为路径跟踪结果，可见两控制器均能控制铰接车辆跟踪参考路径。

图8 双圆工况路径跟踪结果

图9为位置误差及控制量随时间的变化。基于DMPC和MPC的控制器产生的最大位置误差分别为0.088和0.165 m，降低了46.7%，基于DMPC和MPC的控制器产生的平均位置误差分别为0.043和0.096 m，降低了55.2%。基于DMPC和MPC的两控制器在仿真过程中最大控制量分别为49.264和76.280 kN·m，平均控制量分别为6.235和8.590 kN·m，平均控制量增量分别为0.417和0.697 kN·m。

图9 双圆工况位置跟踪误差及控制量变化

2.4 统计结果

为直观地体现本文中提出算法与对比算法相比在跟踪精度上的提高，将各工况下两控制器产生的最大位置误差与平均位置误差进行统计，如图10和图11所示。

图10 各工况最大位置误差

图11 各工况平均位置误差

本文中提出的DMPC算法相比于MPC算法在各工况下的最大位置误差和平均位置误差均有效降低，说明本文中设计的基于状态轨迹的动力学模型和考虑路径多点预瞄误差的控制目标有效地提高了路径跟踪的控制精度。

3 结论

针对铰接车辆路径跟踪控制问题，建立了适用于模型预测控制的铰接车辆动力学模型，基于状态轨迹进行线性化，对线性化误差进行补偿。在此基础上，构建了考虑多点预瞄误差的控制目标，并设计了基于动力学模型预测控制的算法。Matlab/Simulink和Adams联合仿真结果显示，所设计的方法可有效提升控制精度和控制平顺性。

本文中假定铰接车辆纵向速度恒定且线性化误差在预测时域内不变。未来将进一步考虑变速工况与动态线性化误差，以提升控制方法的实用性。