复杂路况下无人驾驶路径跟踪模型预测控制研究*

李 骏，万文星，郝三强，秦 武，，刘霏霏

（1. 华东交通大学机电与车辆工程学院，南昌 330000；2. 建新赵氏科技有限公司，宁波 315000）

前言

近年来，传感器技术、通信技术和软硬件的快速发展，为无人驾驶技术的研究和工程应用领域的落地提供了有效的支持。无人驾驶车辆技术被誉为最终解决交通拥挤、减少交通事故、提高道路通行能力的有效方案，该技术主要包括感知融合、决策规划和运动控制等3大关键技术，其中车辆运动控制是无人驾驶领域内的核心技术之一。而在具有直角转弯、连续弯道和弧形弯等路径曲率变化较大的复杂路况下，保证路径跟踪精度和行驶稳定性是无人车驾驶车辆能够应用于各种场景所需要解决的一个难点。

无人驾驶车辆路径跟踪原理是在车辆上选取一点作为控制点，跟踪决策层规划出的几何曲线或给定的参考路径，使控制点与期望路径间的横向误差和航向角偏差尽可能小或者逐渐收敛到零。无人驾驶车辆路径跟踪控制已经成为了无人驾驶车辆技术的研究热点，国内外学者针对无人驾驶路径跟踪控制的精度、稳定性和安全性问题，开展了大量的研究。

目前，应用较多的路径跟踪控制方法有经典的PID反馈控制、预瞄控制、最优控制和基于机器学习的控制方法等。PID 控制不需要建立精确的物理模型，广泛应用于工程领域，但在车辆运动控制过程中存在多变量系统和时变系统，使用PID控制效果难以得到保证。预瞄控制以纯跟踪算法为代表，该算法的关键在于前视距离和预瞄点的确定。最优控制对建立的模型精确度要求较高，根据建立的精确模型，能够达到控制目标最优，但同样因为车辆运动过程中参数时变和模型非线性，模型的准确性无法保证，因此最优控制在车辆运动控制中的应用存在局限性。机器学习的方法是基于大量的实验数据驱动，通过大数据建立控制模型，得到和实测值相近的控制输出。该算法的不足在于控制效果完全依赖于所采集数据的完整度。

以上控制算法对参数和环境的依赖度较高，在变化的环境中不能完全适应新的车辆状态，且在试验验证时未考虑执行器和车辆的状态约束。模型预测控制算法（MPC）具有对未来轨迹的预测和处理多约束条件的优势，在车辆路径跟踪控制方面得到了广泛的应用。文献［14］中提出了一种基于非线性模型预测的横纵向综合控制的路径跟踪方法，通过所提出的控制算法计算出纵向力、侧向力和横摆力矩，利用障碍函数法求解轮胎力的最优分配。仿真结果表明，该方法能显著提高跟踪精度和行驶稳定性，但非线性模型会增加控制器的计算时间，实时性得不到保证。文献［15］中提出了线性时变模型预测控制算法，减少了控制器的计算时间，提高了车辆运行的实时性，保证了跟踪精度。文献［16］中为兼顾路径跟踪精度和实时性，改进了3 自由度车辆模型，在模型预测控制算法中将横摆速度跟踪误差加入到二次规划性能指标，同时考虑车辆质心侧偏角对车辆行驶稳定性的影响，由此在提高跟踪精度的同时保证了行驶稳定性。文献［17］中在传统模型预测控制算法的基础上，利用遗传算法求解车辆行驶最优时域参数和加入前轮侧偏角约束，改善了高速行驶、低附着系数时车辆跟踪性能和平稳性。Ju等在研究分布式电动汽车路径跟踪问题时，提出了利用横摆力矩对跟踪偏差补偿和车辆稳定性进行协调控制的方法，采用MPC 控制算法给出前轮转角，将跟踪偏差和横摆角速度偏差的两个滑动面设置为控制目标，最后将两个组合面合成一个具有权重系数的协调滑动面，算出横摆力矩，对跟踪误差进行修正。文献［19］中提出了一种将模型预测控制算法与PID 速度控制器相结合的协调控制方法，以8 自由度的车辆模型作为预测模型，通过MPC 控制器生成最优前轮转角，以嵌入模型预测控制优化求解中的PID 控制器生成车辆的驱动力/制动力，协调控制车辆的路径跟踪和速度跟踪性能。

综上所述，MPC 算法在简单或双移线路况下的仿真与试验验证能得到较好的路径跟踪效果和行驶稳定性。但是，目前基于MPC 算法的无人驾驶路径跟踪的研究还存在以下问题：在车辆横向运动控制时，通常假定车辆纵向速度为恒定值，而忽略了纵向车速对横向控制的影响；在直角转弯、连续弯道和弧形弯的复杂路况下，考虑无人驾驶车辆路径跟踪的研究较少；绝大多数控制算法研究未考虑道路曲率对行驶速度的约束。

因此，本文基于驾驶员开车时会根据道路弯曲度调整车辆行驶速度的驾驶习惯，提出了一种改进的MPC 算法，提高MPC 算法在复杂路况下的跟踪性能和行驶稳定性。该算法根据路径弯曲度设计车辆纵向行驶速度，并将车辆的位置偏差、前轮转角偏差和速度偏差作为二次规划的性能指标。在测试场地进行了实车验证，试验结果表明：与纵向速度恒定的MPC 算法相比，改进MPC 算法时车辆路径跟踪精度提高了61.80%，验证了改进模型预测控制算法的可行性和优越性，显著改善了车辆平稳性。

1 无人驾驶控制问题描述

1.1 无人驾驶汽车平台简介

无人驾驶车辆平台由某新能源汽车有限公司提供，该平台是在某款纯电动汽车的基础上建立，其设备的布置实物图如图1 所示。该试验平台在保留原车车载传感器的同时还后装了无人驾驶所需要的位姿设备和环境感知传感器，包括：双GPS天线、前视摄像头、4 线前向激光雷达、64 线激光雷达、工控机、惯导RT2000 等（见图1（a））。车辆的定位采用实时动态载波相位差分技术，使用双GPS天线和RT2000组成组合导航，让无人驾驶车辆“知道”自己的位置和状态；无人驾驶车辆的“眼睛”由前视摄像头、4 线前向激光雷达、64线激光雷达以及车辆前装的77 GHz毫米波雷达代替，通过多传感器融合技术对车辆周围环境进行实时感知；无人车的“大脑”由安装在车辆后备箱的工控机代替，其对各传感器传输过来的数据进行实时分析处理，并下达相应的控制指令；工控机下发控制指令时，首先通过USB 转CAN 盒接入车身CAN 总线（见图1（b）），再通过车身CAN 总线传输控制指令，以实现车辆的加减速、转向等操作，因此CAN 总线相当于是无人驾驶车辆的“神经网络”。

图1 无人驾驶试验平台

1.2 无人驾驶汽车的控制结构

无人驾驶车辆运动控制主要分为横向控制和纵向控制，其控制原理如图2所示。无人驾驶车辆正常行驶时，车身传感器感知周围环境信息，将感知的数据输入给工控机进行数据分析计算，得出车辆行驶的期望速度和期望位姿，同时车辆运动传感器实时检测车辆当前的速度和位姿信息，并将测量信息传输到工控机。工控机将计算得到的期望速度和期望位姿信息与传感器测量的信息进行对比，得出相应加速踏板、制动或者转向盘的控制量，最后通过CAN 总线控制车辆的执行机构以达到路径跟踪的目的。

图2 无人驾驶运动控制原理图

在无人驾驶运动控制中，一般认为纵向控制是对速度的控制，横向控制主要是对转向盘进行控制，研究时往往将两者分开研究。在研究横向控制时通常设定纵向速度（行驶速度）为恒定值，只对车辆的横向控制进行研究，但对于复杂路况，不同的纵向速度会对横向控制性能产生较大的影响。因此在复杂路况下，综合考虑车辆的纵向控制和横向控制对提高路径跟踪性能与车辆稳定性具有重要意义。

本文在研究路径跟踪控制时主要运用了无人驾驶车辆技术中的车辆定位和控制技术，图3 所示为所搭建的路径跟踪试验平台。

图3 路径跟踪试验平台

2 基于改进模型预测控制的无人驾驶车辆路径跟踪设计

一般情况下，以纵向速度恒定的模型预测控制（CMPC）算法设计的无人驾驶车辆控制器，在直线、双移线等简单路况下进行试验，可以得到较好的路径跟踪效果。但在直角弯、连续弯道、弧形弯道等复杂路况下，无人驾驶车辆路径跟踪会出现较大的横向误差。针对复杂路况，驾驶员驾驶车辆时会根据行驶路径弯曲程度进行相应的速度调整，以便更好地控制车辆沿着车道行驶。为了提高复杂行驶工况下的无人驾驶车辆路径跟踪精度，本文根据路径的弯曲度设计车辆的行驶速度，即行驶速度是取决于路径的弯曲度，而不是恒定值，因此研究改进的模型预测控制（MMPC）算法。

基于MMPC 算法的无人驾驶车辆路径跟踪设计，包括路径弯曲度确定、参考速度设定和MPC 算法设计3部分。

2.1 路径弯曲度确定

根据规划好的期望路径信息，将其等距离采样，如图4所示。在路径上每间隔距离取一个点，每连续的3 个点进行一次弯曲度计算。在图4 中从路径的起始点D1 开始，先与D2、D3 点计算一次弯曲度；然后以D2 点作为基点与D3、D4 再计算一次弯曲度。依次循环计算每3个点之间的弯曲度。

图4 期望路径等距离采样下弯曲度确定

每个点处的弯曲度计算公式可表示为

式中θ表示第段折线到第+ 1 段折线的弯曲度。根据采集或者规划的期望路径，通过离散化，计算得到每连续线段处的弯曲度值。为避免连续线段之间弯曲度变化过大，引起匹配的车速不连续或者突变而造成车辆行驶不稳定情况，需要对全程期望路径的弯曲度值进行平滑处理：

2.2 参考点速度设定

弯道实际限速一般采用简化计算，如Pomerleau等提出的弯道限速公式为

式中：为重力加速度；为道路半径；μ为路面附着系数；为路面超高角度。当车辆在平坦的道路（= 0）上不发生滑移时，车辆转弯行驶最大速度是根据向心力公式推导。

根据道路半径可计算出路径参考点的最大参考速度，当采用距离的值取不大时，可将连续的3 个点之间2点的连线看作3点所确定圆的切线，此时每3 个点连成两段折线的中垂线所交的圆心角就等于弯曲度值，即

式中：d为采样时间；为车辆绕着等速圆心的角速度；为车辆的行驶速度。由式（5）可得到每一段处车辆的转弯半径。联合式（3）～式（5）可得到在平坦道路上车辆不发生滑移的最大速度为

式中μ为路面附着系数，可以根据工程经验或文献［21］获得，通常情况下车辆正常行驶的路面附着系数范围为0.7 ≤μ≤1。

在低速（30 km/h）下弯曲度小于3°时，一般可以认为是直线路段，可以按照设定的目标速度行驶；在弯曲度大于3°的路况下，道路弯曲度只限制最大速度，并没有最小速度的限制，通过大量仿真对比并综合考虑无人驾驶车辆的跟踪效果，可将参考速度设定为

式中为常数。

根据式（6）和式（7）可计算出期望路径下不同弯曲度所匹配的参考速度，并且能够实现参考速度的连续变化。不需要通过离散的坐标信息拟合，计算出每一段路径的道路半径，以设置行驶速度。改进的模型预测控制算法可以在给定的期望路径上根据路径弯曲度匹配相应的期望行驶速度，实现在复杂工况下的变速行驶。

2.3 模型预测控制算法设计

模型预测控制的主要原理是利用建立的模型、系统的当前状态和未来的控制量预测系统未来的输出，通过在线滚动求解带约束优化问题来实现对系统的控制目的。对于无人驾驶车辆的运动控制，控制算法的实时性是至关重要的。因此，本文采用线性时变模型预测控制算法作为车辆路径跟踪控制方法。该算法基于车辆动态的线性状态模型，通过预测模型、滚动优化和反馈校正，实现了车辆路径的自动跟踪。

因此模型预测控制算法的设计可由车辆模型的建立、预测模型、滚动优化和反馈校正等4步构成。

（1）车辆模型的建立

车辆运动学模型从简化的阿卡曼转向几何模型和车辆位姿关系的角度研究车辆的运动规律。因为其不涉及车辆的物理性质（如质量）和受力情况，所以当车辆低速（≤30 km/h）行驶在路况良好的路面上时，不需要考虑复杂的轮胎滑移及车辆侧向动力学特性对车辆的影响，此时基于运动学模型设计的路径跟踪控制器也具备可靠的控制性能。建立车辆运动学模型，如图5 所示，在大地坐标系下，设定车身与大地坐标系轴的夹角为车体的横摆角，其中(，)和(，)分别为车辆前后轴中心坐标，为车辆后轴中心的速度，为等效前轮转角，为车辆轴距。

图5 车辆运动学关系示意图

根据车辆运动与几何关系，得到车辆的运动学模型表达式为

（2）预测模型

为减小计算量，保证控制量的实时性，将式（9）进行线性化和离散化处理。离散化处理，得到离散状态空间方程：

为避免系统控制量出现突变的情况，对控制增量进行精确约束，将状态量与控制量进行组合，重新构建新的状态空间表达式：

通过观察式（13），可看出在预测时域内任意时刻系统的状态量和输出量都可以通过系统的当前状态量(|)和控制时域内的控制增量Δ()计算得到，即可实现模型预测控制算法中的“预测”功能。

（3）滚动优化

车辆在进行路径跟踪控制的过程中，模型预测控制器的每一采样时刻都要求状态偏差和控制增量最小，以获得最佳的路径跟踪性能和车辆行驶稳定性。在设计路径跟踪控制器时采用如下形式的目标函数：

式中：、和ℓ 分别表示控制量、控制增量和松弛因子的权重矩阵，通过调节不同权重矩阵得到不同的车辆跟踪效果；为松弛因子，将松弛因子添加到目标函数中，是为了防止目标函数出现无解的情况。式（14）中等号右边的第1 项反映了系统对参考路径的跟踪能力，即当权重矩阵较大时，车辆的跟踪误差更小；第2 项反映了系统对控制量平缓变化的要求，即当权重矩阵较大时，计算出的控制量变化更平缓，车辆运行的过程也会更加平稳。

在实际控制过程中，需要将控制量和控制增量限定在合理范围之内，即无人驾驶车辆的控制量和控制量增量的约束表达式为

为后续优化求解将约束条件中的变量统一，式（15）可以转换为以下形式：

式中：U= I⊗(- 1)，I表示行数为N的列向量，(- 1)为上一时刻的控制量；、分别表示控制时域内控制量的最小值和最大值集合；Δ、Δ分别为控制时域内控制增量的最小值和最大值集合；A为N× N的单位下三角矩阵。

将目标函数转化成标准的二次型，结合式（16）约束条件，求解以下优化问题：

在下一个控制周期，将上一控制周期的状态信息作为系统的依据来预测下一控制时域内的输出，再次通过上述优化求解得到式（19）序列。重复上述过程，完成在控制时域内向前采样周期内的滚动优化，实现车辆前轮转角和行驶速度的最优控制输入。

3 试验分析

3.1 试验参数设置

本文试验采用某新能源汽车有限公司提供的测试场地作为跟踪路径，利用定位设备采集厂区测试场地经纬度数据，得到的测试场地路径如图6 所示。图6（a）为JOSM 软件中处理得到的厂区测试地图。从图可知，该测试场地有直线行驶、直角转弯、连续弯道和弧形弯复杂路况。为便于试验验证，将路径经纬度坐标转换成以红点为原点的大地坐标，得到的大地坐标系下厂区路径如图6（b）所示。图中箭头表示车辆测试时的起始方向。

图6 测试场地路径

根据采集点的坐标信息，计算不同路段处的弯曲度，得到如图7（a）所示的道路弯曲度变化特性；实车试验的道路为干燥的水泥道路，路面附着系数取μ= 0.85；考虑到实时计算量和道路曲率精度，根据实车调试效果，采样距离取=1 m；计算弯曲度ˉ时，为避免采样路段间弯曲度变化过大而造成参考速度跳变，式（2）中取=5。将各参数具体值代入式（6），最大行驶速与弯曲度的关系式为

利用式（20）计算出无人驾驶车辆路径跟踪的全程参考速度。图7（b）为道路弯曲度与最大参考速度的对应关系。

图7 路径弯曲度及弯曲度与最大参考速度的对应关系图

根据文献［22］和文献［23］，考虑车辆加减速过程的平稳性和乘坐的舒适性，并结合调试效果，权重矩阵、和ℓ选择为

式中：为车辆期望速度；Δ为每个控制周期的速度变化量；Δ为每个控制周期前轮转向角的变化量。

根据式（21）和式（22），得到控制量的约束条件和控制量增量约束条件：

3.2 模型预测控制下路径跟踪试验分析

为验证MMPC算法在复杂路况下的路径跟踪效果，根据车辆运动学模型和MMPC 算法，利用Python语言编写控制程序，并将采集的测试场地地图信息导入到编写程序中作为无人驾驶车辆的期望路径。在进行实车试验时，MMPC 算法下的车辆期望行驶速度根据道路弯曲度进行设计，具体数值设定根据式（7），式中的常数取=0.75。而在CMPC 算法下的车辆期望行驶速度设置为恒定值5.6 m/s（20 km/h）。试验得到CMPC 算法和MMPC 算法下的速度、航向角和横向误差，对比分析在复杂路况下CMPC 算法和MMPC算法的路径跟踪效果。

图8（a）和图8（b）分别为CMPC 和MMPC 算法下的实车路径跟踪和行驶速度的对比结果。由图8（a）可知，两种控制均能准确地实现车辆跟踪期望路径；但在直角弯、连续弯道和弧形弯的路况下，两种控制算法下车辆出现一定的跟踪偏差。从图8（b）可知，在CMPC 算法下车辆行驶速度为恒定值，而在MMPC 控制下车辆的行驶速能够根据行驶的路径弯曲度进行相应调节。

图8 CMPC和MMPC算法下实车路径跟踪和行驶速度

图9（a）为CMPC 和MMPC 算法下的航向角变化规律。由图可知，在弯道处，车辆行驶速度越小，车辆横摆角速度越小，车辆稳定性和乘坐舒适性得到提高。图9（b）为CMPC 和MMPC 算法下的横向误差变化规律。由图中可知：与直线路况相比，车辆在直角弯、连续弯道等大弯曲度处横向偏差明显增大，表明不同行驶路况对无人驾驶车辆路径跟踪效果具有较大的影响；CMPC 算法下车辆最大路径跟踪误差为0.89 m，而MMPC 算法下车辆最大路径跟踪误差只有0.34 m，与CMPC 算法跟踪误差相比，减少了0.55 m，最大跟踪程度提高了61.80%，车辆具有更高的路径跟踪精度。

图9 CMPC和MMPC算法下航向角和横向误差

为更好地分析在直线行驶、直角转弯、连续弯道和弧形弯等复杂路况下车辆路径跟踪效果，将图8中对应的复杂工况局部放大，如图10 所示。在直线路况下（图10（a）），CMPC 算法和MMPC 算法下车辆具有较好的跟踪效果，两种控制算法下最大横向误差只有0.31 m。而在直角转弯路况下（图10（b）），CMPC 算法和MMPC 算法下车辆通过直角弯时都会出现提前入弯的情况，这是因为两种算法中的预测时域N值不可调，N值较大时，车辆在较远处的误差权重较大，使得车辆横向误差均呈现先增大、再减小、再增大、后减小的变化趋势；在CMPC 算法下车辆最大横向误差达到0.68 m，而在MMPC 算法下车辆横向误差维持在0.25～0.32 m，这是由于该算法的纵向速度根据路径弯曲度进行相应调整所至。在连续弯道路况下（图10（c）），MMPC 算法下车辆行驶速度相对较小，车辆横摆角速度小、稳定性好，路径跟踪横向误差最大为0.31 m。这与CMPC 算法下车辆最大横向误差为0.68 m 相比，MMPC 算法下的车辆跟踪误差减小了0.37 m，提高了入弯、出弯时的跟踪精度。MMPC 算法能够提高跟踪精度的原因是：连续弯道处的弯曲度值变化较大，车辆行驶速度比CMPC 算法下的车辆行驶速度要小，缩短了车辆达到期望转角的调整距离，从而减小了跟踪误差。而在弧形路况下（如图10（d）），CMPC 算法和MMPC算法均能对过弯时产生的横向偏差进行快速调节，但MMPC 算法能进一步减小跟踪误差，提高车辆的跟随效果。

图10 图8（a）中直线行驶、直角转弯、连续弯道和弧形弯下局部放大图

4 结论

本文针对复杂路况下无人驾驶车辆路径跟踪控制问题，提出了一种改进的模型预测控制算法（MMPC）。与纵向速度恒定的模型预测控制算法（CMPC）相比，MMPC 算法是根据路径弯曲度设计变化的纵向速度，故缩短车辆收敛到期望航向角时的调整距离，减小了跟踪误差。通过无人驾驶车辆实车试验，对比分析了在复杂路况下MMPC 算法和CMPC算法的路径跟踪效果，得到了以下结论：

（1）在弯曲度较小的路况下，MMPC 算法和CMPC 算法下无人驾驶车辆具有相同的跟踪效果，在弯曲度较大的路况下，CMPC 算法下最大跟踪误差为0.89 m，而MMPC 算法下最大跟踪误差仅有0.34 m，相比于CMPC 算法跟踪误差减小了0.55 m，跟踪精度提高了61.80%，实现了无人驾驶车辆具有更好的路径跟踪效果；

（2）车辆在入弯或过弯时，MMPC 算法能够调节车辆行驶速度，使车辆的航向角变化率更加平缓，提高了车辆行驶稳定性和乘坐舒适性；

（3）车辆在入弯或者过弯时，MMPC 算法下车辆能够减速行驶，更加符合人类驾驶员的驾驶行为。