基于模型预测控制的无人车辆轨迹跟踪控制

李 畅，张 佳，孙中奇,3，夏元清

（1.北京理工大学自动化学院，北京 100081；2.中汽数据（天津）有限公司，天津 300393； 3.北京理工大学长三角研究院（嘉兴），嘉兴 314019）

1 引 言

无人车辆是指通过搭载先进传感器装置，运用人工智能等新技术，具有自动驾驶功能，逐步成为智能移动空间和应用终端的新一代汽车。随着5G 通信技术[1]、泛人工智能芯[2]片、新型传感器等技术的成熟，无人驾驶车辆越发成为热点研究方向。同时，它被广泛认为是一种验证视觉、听觉、认知以及人工智能技术的通用实验平台[3]。运动控制是无人车辆的核心技术之一，高效精准的运动控制系统是智能无人车辆实现功能行为的重要保障。在运动控制领域，应用比较成功的控制算法有：比例积分微分控制[4]、纯跟踪控制[5]、预瞄控制[6]、模糊控制[7]、滑模控制[8]、模型预测控制（Model Predictive Control, MPC）[9]及神经网络控制[10-11]等。无人车辆自身有着非线性和欠驱动性[12-13]、系统内外部存在诸多的约束条件。模型预测控制具有预测模型、滚动优化和反馈校正等基本特征，具有全面考虑预测信息和处理多约束优化问题的能力，被认为是最具应用前景的控制方法之一。

近年来，模型预测控制得到了很快的发展，产生了鲁棒模型预测控制算法、自适应模型预测控制算法[14]、数据驱动模型预测控制算法和分布式模型预测控制算法[15]等新方向。应用领域也从传统过程控制领域延伸到航空、车辆、交通、网络等众多应用领域。文献[16]设计了一种模型预测控制方法，使车辆在满足其物理约束的情况下沿期望路径稳定行驶。龚建伟等[17]介绍了模型预测控制在无人驾驶车辆运动控制的基础应用。但是大部分研究基于软件仿真实验和硬件在环仿真实验，缺少实车实验验证和测试。无人驾驶车辆往往有乘客乘坐，其乘坐体验是跟踪效果的重要考量。限于参考轨迹的质量、传感器的噪声和控制机器人性能，实际实验中往往存在着方向盘高频抖动，降低乘坐体验。由于经典模型预测控制器的成本函数中权重固定不变，难以在变化工况下得到最佳控制效果。往往出现高速状态控制量变化大、直线游动、大曲率弯道跟踪误差大等现象，造成乘坐体验不佳、跟踪效果不佳。

本文首先通过机理分析建立了车辆的运动学模型。在对模型进行合理假设后进行了线性化和离散化处理。基于上述模型和无人车辆的物理结构约束、控制量约束，设计了模型预测控制器，并对控制器的迭代可行性和系统稳定性进行了分析。针对实际乘坐体验不佳的问题，提出利用跟踪微分器处理参考轨迹的方法，针对经典模型预测控制变化工况的适配问题，提出了一种改进的MPC 策略，采用模糊自适应控制算法，自适应的改变成本函数的权值。通过软件仿真，验证了算法可行性。最后进行了实车实验，测试了算法的控制效果、鲁棒性和可靠性。

2 问题描述

通过对无人车辆进行运动学分析，得到无人车辆的运动学模型，同时得到车辆物理约束、控制量约束，给出了无人车辆轨迹跟踪问题的描述。

2.1 无人车辆运动学模型

无人车辆是具有非完整性约束的欠驱动系统。本文主要讨论的是具有阿卡曼转向结构的无人车辆。车辆结构及转向原理如图1 所示。车辆后轮驱动，前轮转向，由于阿克曼结构，4 个车轮转向圆共点。N是车辆前轴中点，M为后轴中点，φ是车身横摆角（航向角），δf是前轴中心偏转角，vf是前轴中心速度，vr是后轴中心速度，L是前后轴距。R是后轴中心转向半径，P是车辆的瞬时转向中心。

图 1 车辆运动原理示意图Fig.1 Principle of vehicle motion

车辆运动学模型可进一步简化：仅考虑前后轴的中点M和N，并将两点连线看作车身，忽略车宽。通过分析可以得到车辆运动学方程[17]：

模型可以写成一般非线性形式：

2.2 无人车辆轨迹跟踪问题

无人车辆在轨迹跟踪过程中，需要尽快跟踪上参考轨迹，同时需要尽可能降低能量的损耗。在跟踪过程中，要满足车辆物理约束和跟踪问题约束。车辆的给定轨迹可以由状态信息来描述，状态信息满足系统动态方程，状态量和控制量满足约束：

车辆实际的轨迹满足动态方程，状态量和控制量满足约束：

定义跟踪误差：

轨迹跟踪问题可以描述为：在满足各类约束条件下，通过建立并求解优化问题得到最优控制量，控制车辆状态量跟踪误差ξ˜ 收敛到零平衡点。

3 控制器设计

模型预测控制器主要由预测模型、目标函数和约束条件组成，通过构建并求解最优化问题，从而得到最优控制量。本节首先对车辆运动学模型进行了线性离散化处理，然后适当简化设计了模型预测控制器。

3.1 运动学车辆模型线性化与离散化

非线性的模型预测控制带来了控制算法的设计难度，同时使得计算量增加，加大了计算机的计算负担，使得系统控制的实时性减低，可靠性减弱。所以本节首先对模型进行了线性化和离散化处理。给定轨迹始终满足式(3)，在任意点(ξr,ur)处进行泰勒展开，保留一阶项，忽略高阶项得到：

将式(3)与式(10)做差，可以得到跟踪误差的状态方程：

对连续的状态方程(11)进行离散化得到线性离散状态方程：

式中，T是采样时间，L是车辆轴距。

3.2 预测模型设计

误差系统的离散状态方程由式(13)可知。已知当前时刻的实际状态量，利用状态方程，我们可以滚动预测未来一段时间的状态量。为了简化计算，假设单次迭代过程中系统方程和输入方程不变，即：

简化后的系统方程为可以表示为：

控制器的预测模型可以表示为(N是预测时域)：

3.3 目标函数设计

目标函数要同时反映跟随能力和控制量变化和系统的终端成本。采用如下形式的目标函数：

式中，N是预测时域，是误差项，表征了系统对参考轨迹的跟踪能力，表示控制量的变化表征控制的平稳性，是终端成本，和算法的迭代可行性和系统稳定性有关。

3.4 约束条件设计

对于实际的控制对象，有着多种约束，对于本节建立的控制器主要的约束是控制量极限约束和状态量极限约束。形式是：

4 迭代可行性与稳定性分析

本节将对模型预测控制器的迭代可行性和稳定性进行简要的说明。模型预测控制可行，意味着在tk时刻优化问题可以解得一个优化控制序列采用该优化控制序列控制无人车辆可以使其沿着一条在tk时刻的最优轨迹进入终端集中。稳定意味着代价函数不递增，状态最终收敛到平衡点。分析中涉及不同含义的控制量和状态量，在此进行说明：

系统动态方程由式(15)给出。在式(15)可稳定的条件下，在终端区域内可以确定线性状态反馈控制器：是渐进稳定的[18]。

控制器求解的最优问题可以如下：

式中，Q,R阵为加权矩阵，N是预测时域，T是采样控制周期，Ω是终端区域。是有限层控制成本，是惩罚有限层末端状态的终端成本，满足：

定理4.1：假设线性化后的系统(15)在原点是可稳定的[18]。那么有以下结论：

（1）Lyapunov 方程

（2）存在一个常数α∈ (0, ∞)来指定一个原点的一个邻域：

应用终端控制器后，在终端集内有

定理详细证明过程参考文献[18]。

定理4.2（迭代可行性）：对于具有完全状态测量且无扰动的标称系统，且采样时间足够小的情况下，优化问题在t= 0时刻可行，意味着它对所有t＞ 0时刻都是可行的。

证.假设在t时刻有最优控制序列满足约束。当开环应用该控制序列后，可以使得状态量沿着开环的最优轨迹进入终端集那么，在实际的控制中，最优控制序列在t到t+T时段被实际应用。由于考虑的系统是没有扰动的标称系统，所以实际的状态量等于最优轨迹中的状态量：为保证在t+T时刻问题仍然有解，可以预先设计可行解

式中，K是为用于确定P和Ω的局部线性状态反馈增益。由定理4.1 可知终端区域是不变的，当设计的可行解应用后，可以使得控制量是阶段执行的，每次只执行第一个控制量，所以几乎不会被应用。对于轨迹的连续性，我们可以假设对于一个足够小的T＞ 0来满足。至此算法的可行性得证。

定理4.3（稳定性）：假设优化问题在t=0 时是可行的，对于没有扰动的标称系统，由定理4.2可知，对于任何可行，且最优值函数满足：

证.在t时刻有最优控制序列开环控制后得到最优状态轨迹NT]，此时的目标函数的最优值可以写成：

由于是标称系统，将控制序列应用后得到实际状态与最优状态相等：

在t+T时设计可行的控制序列：

应用了可行控制序列后实际的轨迹除了[t+NT,τ+NT)之外和预测的最优轨迹相同：

对定理4.1 中的式(27)从t+NT到τ+NT进行积分：

在τ∈(t,t+T]中时：

对比式(30)、(31)可知：

进一步推得：

式中，Q＞0,R＞ 0。至此，我们证明了最优值函数J是不递增的。我们可以给出该闭环系统是渐近稳定的结论。

5 参考轨迹预处理

参考轨迹的质量直接影响跟踪控制效果。参考轨迹可由函数生成[19]，也可由驾驶员驾驶车辆进行采集。直接采集的数据高效便捷的同时满足车辆的动力学特性，这保证了轨迹在车辆极限状态的可行性。但采集数据同时存在着随机误差和传感器噪声干扰，导致驾驶机器人方向盘出现高频抖震乘坐体验变差。本节在参考轨迹的预处理过程中使用了跟踪微分器[20]进行处理，取得了不错的效果。跟踪微分器有去除噪声污染，去除异常值的能力，具体函数由式(38)给出：

式中，fhan(x1,x2,r,h)是最速控制综合函数，x1(k)是状态跟踪序列，x2(k)是跟踪序列的微分（跟踪速度），x1(k)r是原参考轨迹，r,h是跟踪微分器的两个参数，r是速度因子，h是跟踪步长。通过循环执行式(38)，序列x1(k)即表示处理后的参考轨迹。其他具体函数形式由式(39)给出，具体推导过程参考[20]。

离散参考轨迹点间距为2 cm，实验中采取r=5,h=0.02。图2 给出了跟踪微分器处理前后的参考轨迹对比。可以发现，处理后，新轨迹与原轨迹偏差较小（1～2 cm），在保留了原轨迹信息的同时使轨迹变得光滑，去除了传感器和人为因素的误差干扰和异常值点，大幅减少了机器人抖震，提高了跟踪控制效果。

图2 参考轨迹处理结果Fig.2 Reference trajectory processing results

6 权重自适应控制

经典的模型预测控制器在轨迹跟踪过程中存在效果不佳的问题，其中重要的原因之一是式(19)中权重矩阵Q和R固定不变，难以在多种不同的工况下达到最优的控制效果，本节为同时保证较高的跟踪精度和较好的乘坐体验，提出了基于模糊自适应控制的改进MPC 控制器。该控制器可以根据速度和地图曲率自适应调整代价函数的权值。

FLC 可以模拟人类驾驶员的程序性知识，实现自适应的控制。例如，在高速行驶状态下避免大幅度转向保证车身稳定，在直线行驶避免反复调整，在发生侧滑后迅速调整消除误差等。FLC主要可分为3 个阶段：模糊化、模糊推理和解模糊。第一步，根据相关隶属度函数，将当前输入值模糊化为具有一定真实性的语言或模糊值。第二步，确定模糊输入值转换为模糊输出值的模糊规则。第三步将模糊输出去模糊化为最终的实际输出。本节定义了两个模糊控制输入：车辆实时速度和地图曲率。考虑参考路径跟踪中车辆的行驶状态，将速度输入模糊化为2 个模糊集：VS（速度小）、VB（速度大），曲率输入模糊化为3 个模糊集：CB（曲率大）、CM（曲率中）、CS（曲率小）。隶属度函数采用双高斯混合函数。将式(19)中的权重矩阵元素Qy,Qφ,Rδ,作为模糊控制器的输出。分别将其模糊化为5 个模糊集：VSCB（低速大曲率）、VSCS（低速小曲率）、VBCB（高速大区率）、VBCM（高速中曲率）、VBCM（高速小曲率）。

模糊规则由表1 给出，Qy,Qφ,Rδ这3 个参数的模糊输出均采用了同一套模糊规则。去模糊化方法采用了重心法。参数Qx=Qy，Rv是常值。

表1 权重的模糊规则Table 1 Fuzzy rules of weight

图3 分别给出了Qy,Qφ,Rδ在模糊规则下输出和输入之间的关系的响应面。根据车身速度和道路曲率，可以自适应地调整参数。实验中，Qx=Qy，Rv是一个常数。

图3 模糊规则的响应面Fig.3 Response surface of fuzzy rules

7 仿真实验与实车实验

7.1 仿真轨迹跟踪控制实验

仿真实验搭建了一套联合仿真平台（图4）。采用Simulink 作为控制算法实现的平台，版本为Matlab2019a，同时联合了车辆动力学仿真软件Carsim，建立仿真控制车辆和仿真工况，版本为 Carsim8.02。

图4 联合仿真平台Fig.4 Union simulation platform

实验选取车型是B 型车，设定参数为：整车质量1723 kg；车身总长为4 m；车身宽2 m；质心至前轴1.232 m；质心至后轴1.468 m；绕Z轴转动惯量为4175 kg·m2；前轮胎侧偏刚度为66900；后轮胎侧偏刚度为62700。实验采用自定义的仿真环境，为1*1 km2面积，摩擦系数为0.8的平整水泥地面。Simulink 中模型控制算法的输入量为车辆位置信息(X,Y)、纵向速度vf、车身 横摆角φ等。输出量是纵向速度v与车辆前轮转角δ。

仿真实验1：参考轨迹为一圆心(0,30)，半径25 m 的圆形，期望速度vr为匀速10.8 km/h，初始状态为ξ0=[0,0,0]T，初始速度为v0=10.8 km/h 。

实验1 中，图5 是仿真软件Carsim 的仿真实验动画，图6 是圆形轨迹跟踪的仿真实验数据。由图6 可知，仿真车辆跟踪效果良好，控制量满足控制量约束且处于较小的波动范围内。在跟踪后期效果平稳，误差较小。

图5 仿真实验1 结果（Carsim 动画）Fig.5 Simulation results data1（Carsim）

仿真实验2：参考轨迹由驾驶员录制，包括位姿和速度信息，输入数据至Carsim 中可以构建仿真道路。本节截取了实验跟踪轨迹的一部分（掉头弯路）进行说明。

在实验2 中截取了跟踪轨迹的一段大曲率弯道。图5 展示了仿真实验的动画效果。图7 给出了实验的数据，该弯道的最大曲率是0.078，仿真通过速度在46 km/h 以上，最大横向误差小于0.43 m，整体跟踪效果良好。通过仿真实验验证了算法的可行性。

图7 仿真2 实验数据Fig.7 Numerical simulation data 2

7.2 实车轨迹跟踪控制实验

为进一步测试算法性能，搭建了无人车辆测试平台。参考轨迹由专业驾驶员采集。车辆位姿由高精组合导航给出，控制算法由车载工控机计算，控制量由自动机驾驶机器人执行。主要进行了高速直线、高速弯道、低速直线和低速弯道等测试。测试车辆为前轮驱动四座乘用车。实车实验中进行了不同速度、不同路况的全面测试，文中给出了具有代表性的实验结果。图8 展示了搭建的无人车辆测试平台。

图8 无人车辆测试平台Fig.8 Unmanned vehicle test platform

图9～10 给出了高速直线跟踪效果。可以发现，在最高速度95.7 km/h 情况下，跟踪误差始终较低，纵向误差在1.5 cm 内，最大横向偏差在7 cm 内，航行角偏差在 0.005 rad 内。车辆平均偏左 3 cm 是由转向机器人的2.6°静态偏差导致。图11～ 12 给出了高速弯道跟踪效果，弯道最大曲率为0.056（半径17.8 m）在平均速度55 km/h，最低速度44 km/h 过弯时，纵向误差在4 cm 内，横向偏差在28.2 cm 内，航向角偏差在0.056 rad 内。由图9～ 12 可以发现，控制算法具有良好的控制性能，并且在不同工况下有着不错的鲁棒性。

图9 高速直线轨迹跟踪对比Fig.9 High speed linear trajectory tracking

图11 高速弯道轨迹跟踪对比Fig.11 High speed curve trajectory tracking

对比权值固定的经典MPC 控制器，本文提出的权重自适应MPC 控制器控制效果有着明显的提升，主要表现为低速运行跟踪误差大大缩小、极限工况稳定性提升等。

图12 高速曲线跟踪误差Fig.12 Tracking error of high speed curve

图13是低速大曲率弯道的跟踪轨迹，行驶速度为8 km/h，曲率最大处为0.24。红色轨迹是权重自适应MPC 跟踪轨迹，蓝色是权重固定MPC。通过对比可以发现，权重自适应MPC 跟踪误差最大值在30 cm 内，明显小于权重固定MPC 效果。

图13 低速大曲率弯道控制轨迹对比Fig.13 Control trajectory comparison

图14是高速直线工况下前轮转角控制量的对比。黑色线条是当前道路曲率。可以发现，权重固定MPC在直线跟踪时出现反复调整，出现“游动现象”，而权重自适应MPC 在直线段主要通过自适应增大航向角权重使车辆姿态稳定，避免了“直线游动”，提高了乘坐体验。

图14 高速直线方向盘转角控制量对比Fig.14 Comparison of front wheel angle control

图15 是转弯滑移测试轨迹对比。车速在25 km/h 以上，要求在转弯处达到车辆转向极限（500°）。车辆会因惯性发生轮胎滑移。目的是测试滑移后车辆姿态恢复能力。通过对比可以发现，权重自适应MPC 在发生滑移后有更好的恢复速度，而固定权重MPC 出现了失稳现象，并且恢复慢。

图15 转弯滑移测试轨迹对比Fig.15 Turn slip test trajectory comparison

通过以上对比实验可以发现，本文提出的基于模糊控制的权重自适应MPC 控制器对比经典MPC 控制器有着更优的控制效果和乘坐体验。

文献[21]中提出了纵向模糊PID 和横向航向预估式的控制算法，在跟踪实验过程中，速度在

10 km/h 左右，跟踪轨迹波动较为明显在直线出现“游走”，弯道跟踪不够紧密，最大误差在0.8 m。通过对比可以发现，本文提出的模型预测轨迹跟踪器有明显优势。

8 结 论

本文主要基于模型预测控制的无人车辆的轨迹跟踪问题进行了相关的研究，通过机理分析对阿克曼结构的无人车辆进行基于运动学建模。设计了模型预测控制器，并通过设计终端约束的和终端集的方法对迭代可行性和系统稳定性进行了分析。针对参考轨迹的随机误差和噪声干扰导致的乘坐体验变差问题，提出来利用跟踪微分器处理参考轨迹的方法，有效消除了方向盘高频抖动问题，提高了乘坐体验。针对经典MPC控制器由于优化函数中权值固定导致变化工况适应性低的问题，提出了基于模糊控制的权重自适应MPC。通过搭建软件仿真平台，验证了算法的可行性，同时进行了大量实车实验。实验表明，本文设计模型预测控制器可以完成轨迹跟踪任务，同时在不同工况具有较强的鲁棒性。对比经典MPC，在控制精度和乘坐体验上均有明显提高。