基于MPC的自动驾驶车辆横向路径跟踪控制

陈威，廖文浩，刘明春

(南昌大学机电工程学院，江西 南昌 330031)

考虑驾驶员的驾驶能力、先天生理差异、主管情绪等各方面的影响，驾驶员在“驾驶员-车辆-道路”闭环系统中被认为是最薄弱的环节，研究表明，将近55%～90%的交通事故是由于驾驶员因素造成的[1]。自动驾驶汽车能够部分甚至全部取代人类驾驶员的操作，降低驾驶员操作因素和失误概率，提高行驶安全性、舒适性和高效性，因而被认为是未来汽车发展和技术创新的主要方向[2]。自动驾驶车辆的运动控制是行驶安全性和舒适性的核心技术,其中横向运动控制主要实现车辆的横向路径跟踪，即控制方向盘转角，使车辆沿规划好的路径行驶，确保车辆在行驶过程中的安全性、舒适性和平稳性。

目前，横向路径跟踪控制的跟踪精度和稳定性是主要研究方向，并主要通过与参考路径的横向偏差、前轮转角及其增量的变化率等参数进行评价。控制算法上主要有PID控制、预瞄跟随(PF)、线性二次调节器(LQR)、模型预测控制(MPC)等。文献[3-4]采用改进型的PID控制器作为路径跟踪器，能够在一定程度上解决了路径跟踪问题，但需通过大量的测试数据和专家经验确定关键控制参数。文献[5]采用郭孔辉院士提出的“预瞄最优侧向加速度模型”建立驾驶员模型，实现对路径的跟踪，但是跟踪效果受驾驶员参数及环境变化因素影响较大。文献[6]运用LQR最优控制理论，提出了一种新型主动转向变传动比控制策略，并实现了理想转向特性。文献[7]运用LQR算法，利用状态反馈的方法实现了最优控制，解决了因外界条件变化引起的系统失稳问题，但是无法处理多变量控制过程的约束问题。文献[8]采用LQR算法考虑了横向稳定性，在规划控制上结合了直接横摆力矩控制，使跟踪更为平顺稳定，但并未考虑复杂的动力学约束问题。模型预测控制算法[9]因其能处理复杂约束条件下的多目标优化问题，被广泛应用于自动驾驶车辆的运动控制中。文献[10-11]针对车辆稳定性控制问题，采用MPC算法设计了主动转向的MPC控制器，取得较好的跟踪效果。文献[12]建立了轨迹跟踪动态的仿真车辆模型，并基于此模型提出了一种基于非线性车辆动态模型在线线性化的侧向MPC控制器，有效地减小跟踪误差，保证车辆的行驶平稳性。文献[13-14]采用MPC算法，将车辆简化为二自由度线性模型，在规划层考虑了车辆动力学约束问题，控制实时性较好；其中文献[14]通过连续线性化的误差模型，将多目标优化问题转化为一个二次规划问题，因其目标函数结构简单，较容易实现，但在处理对控制量变化有较高要求的问题时有一定的局限性。上述路径跟踪方法并未考虑真实驾驶员的跟踪特性，不能最优地兼顾安全性、稳定性和舒适性。目前自动驾驶车辆的控制研究主要以提高车辆动力学性能为主，以自动驾驶车辆满足设计条件下的安全性、稳定性等为依据，缺乏对驾驶员操作缺陷方面的改善分析，本文通过分析人工驾驶员的转向操作特性，以熟练驾驶员的转向操作为参考依据，设计自动驾驶车辆横向路径跟踪控制器，并分析其在改善驾驶员转向操作缺陷方面的有效性。

首先，为了分析预瞄时间和车速对驾驶转向操作特性的影响，根据预瞄跟随理论建立最优侧向加速度的驾驶员模型；其次，基于MPC控制理论，设计了自动驾驶车辆的横向路径跟踪MPC控制器，在MPC控制器中，采用反馈校正机制对预测模型进行改善，降低参数不完全确定和外部干扰对模型带来的影响；引入松弛因子对约束条件进行软化和扩展，以保证目标函数具有可行解；进一步地，将横向路径跟踪优化问题转化为带约束的二次规划问题，利用模型预测控制滚动优化的特点，求解跟踪参考路径所需的方向盘转角，作用于自动驾驶车辆。最后，通过仿真实验和实车测试，分析了熟练驾驶员的操作特性，验证了所设计MPC控制器的横向路径跟踪性能。

1 车辆动力学建模

本小节建立车辆二自由度线性动力学模型，以描述车辆横向运动中的关键状态变量——质心侧偏角、横摆角速度，以及车辆行驶轨迹。如图1所示，对车辆模型做如下假设：1)汽车在xv轴方向的速度vx保持不变；2)忽略车辆的转向系统，假设车辆的前轮转角保持一致，并将前轮转角作为系统的输入；3)忽略空气动力学、悬架作用和轮胎打滑对车辆的的影响。

(1)

(2)

式中:m为整车质量；vx为纵向速度；Iz为车辆绕zv轴的转动惯量；Fyf、Fyr分别表示地面对前、后轮的侧向作用力；lf、lr分别为前后轮与质心之间的距离；δ为前轮转角,它与方向盘转角δs的关系为δ=δs/is，其中ig为转向系统传动比。

当轮胎侧偏角和纵向滑移率处于较小值，可用轮胎侧偏角的线性函数描述轮胎侧向作用力，则前、后轮胎侧向作用力Fyf、Fyr可表示为[15]

(3)

(4)

式中:Cf、Cr分别为前、后轮的侧偏刚度。

综合式(1)～式(4),考虑到δ值较小，得到车辆动力学方程：

(5)

(6)

另外，为了描述车辆的行驶轨迹，将车身坐标系xv-ov-yv与大地坐标系Xg-Og-Yg之间进行转换，可得：

(7)

(8)

综上，得到车辆动力学模型如下：

(9)

2 驾驶员转向模型的建立

为了描述路径跟随过程中驾驶员对方向盘转角的操作特性，采用郭孔辉院士提出的预瞄跟随理论[16]建立最优侧向加速度驾驶员模型。

驾驶员在驾驶过程中，驾驶员以车辆坐标系xv-ov-yv作为参考系来获得前方的道路与车辆位置之间的关系，且驾驶员的视线一直处于前方道路上的某一点。设目标路径的函数在大地坐标系上为Y=f(Xg)(转换关系如式(7)所示)，前视距离为d，预瞄时间T较小，可假设在预瞄过程中车辆以恒定的车速行驶。在车辆坐标系xv-ov-yv中，车辆在t时刻的质心位置A0(xv(t),yv(t)),预瞄点A(xv(t+T),f(t+T))；经过预瞄时间T后，车辆质心的实际位置B(xv(t+T),yv(t+T))，根据运动学关系，有:

(11)

式中：Ti为行驶路径上每个预瞄点以t时刻为起点的时间增量。为了使整段预瞄轨迹的跟随误差处于最小值，建立二次型目标函数，则

(12)

(13)

图2中，P(s)为驾驶员对路径预瞄的传递函数，当预瞄时间为T时，有:

P(s)=eTs

(14)

(15)

式中:td为神经系统滞后时间常数;Th为动作及惯量滞后时间常数。

(16)

式中:Gavss为侧向加速度对方向盘转角的稳态增益，系数Tay1、Tay2、T1、T2与车辆系统参数有关。

c(s)为驾驶员的矫正函数，这是为了克服车辆的动态响应特性与驾驶员的反应滞后，而加上的一个串联微分矫正环节。根据预瞄跟随理论，预瞄跟随系统可被视为由预测器和跟随器组成的串联系统，预瞄跟随系统在预测器与跟随器传递函数的乘积为1时达到最佳跟随效果。由图2中的预瞄跟随系统，得到:

c(s)=(1+Tcs)/Gayss

(17)

式中:Tc为矫正时间，可表示为：

Tc=td+Th+T1-Tay1

(18)

3 MPC横向路径跟踪器设计

在车辆行驶过程中，驾驶员一般关注道路前方的一段，由预瞄理论可知，假设这段路程中存在若干个预瞄点，与预瞄区域处匹配的预瞄时间为tn，跟随误差为en，那么随着车辆的移动，不断更新预瞄区域的信息，驾驶员由此操纵车辆跟踪所预期的行驶路径。本节将基于模型预测控制(MPC)算法，建立与真实驾驶员横向路径跟踪特性相类似的横向路径跟踪控制器，建立目标函数及其约束条件，兼顾熟练驾驶员跟踪特性，以此达到模拟熟练驾驶员的横向路径跟踪特性。

MPC横向路径跟踪器的控制策略如图3所示，首先将控制对象输出的状态变量x(k)作为预测模型的输入，以系统的状态与跟踪预测状态在k时刻的状态误差来设计反馈校正预测模型；通过预测模型来预测车辆状态Y，选取跟踪性能、舒适性能等性能指标、约束条件，并进行预测性转化；然后，在采样周期内求解目标函数，计算预测输入；最后，将得到的一系列控制量的第1个分量u(k)施加到被控车辆上，并将被控车辆的输出状态y(k)作为下一次求解目标函数的输入，实现系统的滚动优化。

3.1 预测模型

为了建立MPC控制器，首先将式(9)中的线性车辆模型转化成状态空间表达式：

(19)

根据式(19)得到以下离散状态空间表达式：

(20)

式中:Ad、Bd、Cd分别为离散状态空间方程的状态矩阵、控制矩阵和输出矩阵，有：

(21)

式中:ΔT为系统的采样时间。

为进一步降低算法的复杂程度，在每个步长内，对时变系统的矩阵做如下假设：

A(k)=A(t),k=1,…,t+n-1
B(k)=B(t),k=1,…,t+n-1

(22)

在跟踪器设计过程中，通常将约束控制问题表示为一个实时优化问题，为防止系统执行过程中出现控制量发生突变，直接对控制指令的平顺性造成影响或者导致无可行解的情况，将式(20)中的控制量转化为控制增量形式，并令：

(23)

因此式(20)可由一个新的状态空间式表达成：

(24)

式中:Aa、Ba、Ca称为增广矩阵：

(25)

设定Np为预测时域，Nc为控制时域，且Nc≤Np，则系统的状态变量和输出变量为：

(26)

(27)

为了明确整个系统关系，以矩阵的形式表达系统未来时刻的输出，即：

Y(t)=Ψξ(t|t)+ΘΔU(t)

(28)

式中：

Y(t)=[η(t+1|t)η(t+2|t) …η(t+
NP|t)]T

(29)

(30)

(31)

由式(28)可知，由系统当前的状态量ξ(t|t)和控制增量ΔU(t)可得到在预测时域内的状态量和输出量。

3.2 优化求解

为了提高自动驾驶车辆的横向路径跟踪精度和跟踪稳定性，建立关于控制增量的目标函数如下：

(32)

目标函数中的第一项计算车辆行驶路径与参考路径之间的偏差，反应了车辆跟踪参考轨迹的能力；第二项计算在控制增量在控制时域内的大小，满足控制稳定性的要求[17]，最后，对上述两项指标分别设置权重矩阵Q和R，使得车辆快速、稳定地跟踪目标参考轨迹。

而在实际的车辆系统中，系统状态变量和控制变量受到一定的约束，其约束一般形式为：

(33)

考虑系统模型的时变性，为了保证优化问题存在最优可行解，采用向量松弛因子ε，同时加入权重系数ρ对目标函数进行软化处理：

(34)

将预测模型式(28)代入目标函数式(34)，并将系统的输出偏差表示为

(35)

经过矩阵运算，将目标函数整理为标准二次型表达式：

(36)

式中：

在上式中，Pt为与控制增量无关量，因此，模型预测控制的滚动优化求解问题就是求解二次规划问题：

(37)

式中:

3.3 反馈机制

在每个控制周期内求解式(37)，得到了一系列最优控制序列：

(38)

根据模型预测控制的原理，将最优控制序列中第1个元素作为输入控制增量作用在控制对象，即：

(39)

进入下一周期后，重复上述过程，进行循环滚动优化，实现车辆横向路径跟踪控制。

4 实验测试和结果分析

本节采用典型双移线避障工况，比较分析所设计的MPC控制器与熟练驾驶员模型在控制车辆转向中的性能。首先，基于驾驶员转向模型，对驾驶员跟踪特性进行分析，通过调整转向模型参数得到熟练驾驶员转向模型；其次，以熟练驾驶员转向模型为参考对象，对比分析在不同速度下基于MPC横向路径跟踪器的跟踪效果；最后，以真实驾驶员操纵效果为基础，通过实车验证MPC控制器的有效性。

本文所设计的MPC的控制参数如表1所示。

表1 MPC控制参数Tab.1 MPC control parameters

4.1 仿真测试1：驾驶员模型中横向路径跟踪特性分析

以跟随参考路径的最小横向偏差为目标，忽略驾驶员的决策偏差(即td、Th保持一致)，驾驶员模型在不同行驶速度和预瞄时间下的横向路径跟踪的仿真结果如图4所示。图4(a)所示为同一预瞄时间t=0.3 s，不同行驶车速下的横向路径跟踪结果，其中驾驶员启动转向操作的起始位置与参考轨迹保持一致，由图可见当预瞄时间为0.3 s时，车速为70 km·h-1可以得到较好的横向路径跟踪效果，而在60和80 km·h-1的车速下横向路径跟踪效果差异较大。

图4(b)所示为同一车速(70 km·h-1)，不同预瞄时间下的路径跟踪效果，由于不同预瞄时间对应预瞄距离不同，因此图中各条曲线对应的起始方向盘转角位置点不同，其中，参考路径的起始变道位置点为Xref=5 m，预瞄时间为0.2 s时起始方向盘转角位置点为Xref=2.35 m，预瞄时间为0.3 s时起始方向盘转角位置点为Xref=7.30 m，预瞄时间为0.8 s时起始方向盘转角位置点为Xref=12.85 m。由图可见，当预瞄时间为0.3 s时，可以得到较好的路径跟踪效果；当预瞄时间为0.2 s时，由于预瞄时间较短，驾驶员操作较为急促，产生了较大的路径跟踪超调量；预瞄时间为0.8 s时，产生了一定程度的路径跟踪延迟。

由以上分析可知，行驶速度和驾驶员预瞄时间对车辆路径跟踪效果起着决定性作用，熟练的驾驶员应该能够很好地调整车速和预瞄时间，以精确、稳定地跟踪预期的行驶路径。

4.2 仿真测试2：对比熟练驾驶员，不同车速下MPC控制器的横向路径跟踪

设置驾驶员模型中车速为90 km·h-1，预瞄时间t=1.5 s；对比MPC控制器作用下车速60和90 km·s-1的横向路径跟踪效果。仿真中道路附着条件良好，附着系数为μ=0.8。仿真结果如图5所示。

图5(a)所示为熟练驾驶员转向模型和MPC控制器作用下的车辆横向路径跟踪结果，可见MPC控制器可以保证车辆在不同车速下很好地跟踪参考轨迹，而熟练驾驶员控制的车辆在轨迹跟踪方面存在一定程度的延迟，这是因为驾驶员操作存在一定的反应时间和动作时间。图5(b)、(c)为MPC控制器和熟练驾驶控制下的方向盘转角及其增量的变化情况，可见，MPC控制下的方向盘转角波动次数更多，但转角幅度及其增量都较小，说明MPC控制器可以快速且进行转向操作，且转向动作较为缓和；熟练驾驶员的方向盘转向幅度和增量较大，意味着需要克服较大的驾驶负担。图5(d)、(e)分别为车辆横摆角速度和质心侧偏角的变化曲线，可见与熟练驾驶控制下的车辆状态相比，MPC控制下的车辆横摆角速度和质心侧偏角都较小，车辆行驶稳定性较好。

4.3 实车测试和分析

4.3.1 测试车辆介绍

测试车辆由汉腾汽车有限公司开发，如图6(a)所示，测试车辆采用6R1V的方案，包括6个BOSCH毫米波雷达、1个Mobileye EQ4摄像头，具备线控转向、线控驱动和制动功能，满足本文控制算法对转向和速度的控制需求。其传感器感知范围如图6(b)所示，车辆主要参数如表2所示。

表2 测试车辆主要参数Tab.2 Test vehicle main parameters

实车测试方案如图7所示，MicroAutoBox作为控制器搭载本文提出的MPC控制算法，求解作用于车辆的方向盘转角，通过CAN通讯将转角指令传输给线控转向系统实现车辆的转向控制；传感器向控制器提供周围交通信息，例如车辆经度和纬度以及和车身状态。在计算机中，主要对于车辆的控制信息和运动状态进行实时监测。

4.3.2 实车测试与分析

实车测试工况为ISO3888指导下的双移线工况(如图8所示)，入口车道宽a=(1.1D+0.25) m；相邻车道宽b=(D+1.0) m，其中D为整车宽度。

在实车测试中，路面状况良好，控制车速为50 km·h-1，分别采集熟练驾驶员操纵下的实车状态数据，以及MPC控制算法作用下的实车状态数据，测试结果如图9所示。

图9(a)所示为驾驶员操纵和MPC控制器作用于实车上的横向路径跟踪结果，由图可知，由于驾驶员存在生理上的反应迟滞造成车辆在换道后会产生一定的超调量，导致车辆实际行驶轨迹与参考轨迹之间产生横向偏差，其中最大横向偏差为0.357 m；而MPC控制器下的最大横向偏差为0.085 m，即MPC控制下的车辆可以更好地跟踪参考路径。图9(b)、(c)所示为方向盘转角及其增量的变化，从中可知驾驶员操纵下的方向盘转角在时间上相对延迟，幅度相对较小，而且较难维持方向盘在完全0°的位置；而MPC控制下的方向盘转角在车辆直线行驶时可以稳定保持在0°，相对于熟练驾驶员模型，MPC控制下的车辆转向起始时间提前0.89 s，且转向响应迅速、精确。图9(d)、(e)分别为车辆横摆角速度和质心侧偏角的变化情况，可见双移线测试过程中，驾驶员操纵和MPC控制下的车辆都可以维持较小的横摆角速和质心侧偏角，保持较好的车辆操纵稳定性；而与驾驶员相比，MPC控制器下的车辆横摆角速度响应时间较为提前，说明具备更好的响应速度。

5 结论

本文针对自动驾驶车辆的横向路径跟踪问题，建立了基于预瞄跟随理论的最优侧向加速度驾驶员模型，设计了基于模型预测控制(MPC)的车辆横向路径跟踪控制器，并通过仿真实验和实车测试，验证了所设计的MPC控制器的有效性，得到如下结论：

1) 预瞄时间、行驶车速等因素对驾驶员跟踪参考路径的影响较大，熟练驾驶员应该能够很好地调整车速和预瞄时间，以精确、稳定地跟踪预期的行驶路径。

2) 仿真实验和实车测试结果都说明，在中、高速行驶情况下，熟练驾驶员仍然存在响应时间相对滞后、方向盘转角幅度大、驾驶负担重、跟踪参考路径超调等问题；所设计的MPC控制器对方向盘转角的相应更为快速、精确，可以更好地跟踪参考路径，保持较好的车辆操纵稳定性。