基于预瞄跟随理论MPC的无人驾驶车辆路径跟踪控制

Path Tracking Control of Unmanned Vehicle Based on Preview Following Theory MPC

摘要: 为提高无人驾驶车辆在变曲率条件下的路径跟踪精度和稳定性，提出一种基于预瞄跟随理论(PFT)的MPC横向控制器(PFT-MPC)。PTF-MPC包括参考路径点生成模块和MPC控制器模块。参考路径点生成模块通过PFT计算预瞄点的横摆角，为MPC控制器提供参考横摆角，在不增加MPC控制器计算量的同时提高预测范围。在双移线工况下，验证了PFT-MPC控制器的准确性和稳定性。

Abstract: In order to improve the path tracking accuracy and stability of unmanned vehicles under variable curvature conditions, an MPC lateral control controller (PFT-MPC) based on look-ahead following theory (PFT) was proposed. PTF-MPC includes a reference waypoint generation module and a MPC controller module. The reference path point generation module calculates the yaw angle of the pre-view point through PFT, provides the reference yaw angle for the MPC controller, and improves the prediction range without increasing the calculation amount of the MPC controller. The accuracy and stability of the PFT-MPC controller are verified under the condition of double-line shifting.

1. 引言

自动驾驶汽车是科学技术领域的一个热门研究课题，对社会和经济发展具有重大影响 [1] 。互联和全自动或自动车辆作为一种技术正变得越来越可行，并可能很快主导汽车行业，自动驾驶汽车的市场需求和经济价值日益突出 [2] 。

无人驾驶车辆路径跟踪控制是车辆自动化发展的重点研究方向之一，目前已经有多种控制方法可以实现车辆的自动驾驶 [3] [4] 。

在当前无人驾驶路径跟踪控制研究的文献中，大多采用线性二次最优控制(LQR)、模型预测控制、鲁棒控制及其相互结合的控制方法来设计无人驾驶车辆的路径跟踪器。王银等 [5] 提出一种自适应预测时域的模型预测控制(MPC)的轨迹跟踪控制算法，在满足车辆动力学模型的基础上加入侧偏角约束，可以保证智能车辆在低附着且变速条件下跟踪控制的精确性和稳定性。Lin F等 [6] 提出了一种模型预测控制和模糊比例积分微分控制相结合的综合控制方法，模型预测控制用于控制车辆偏航稳定性，并在路径跟踪过程中考虑前轮角、侧滑角、轮胎侧滑角和偏航率来跟踪目标路径，采用模糊比例–积分–微分算法，通过控制每个轮胎的制动力来保持车辆侧倾稳定性，确保了跟踪目标路径时的操纵稳定性。Fan S [7] 等从三自由度车辆动力学模型出发，通过分析车辆横摆稳定性，在模型中加入包络约束，导出了线性时间，具有包络约束的MPC控制器比普通MPC控制器具有更高的跟踪精度。吴海东等 [8] 提出基于结合线性矩阵不等式(LMI)鲁棒控制的智能车轨迹跟踪控制方法，将车辆侧向动力学状态空间模型进行坐标变换，得到基于跟踪误差的车辆侧向动力学状态空间模型，采用饱和线性轮胎得到车辆侧向动力学多胞型模型，该控制器在保证车辆稳定性的基础上具有较高的跟踪精度。

目前大多数无人驾驶车辆路径跟踪控制过程中考虑车辆模型约束条件和道路附着系数对路径跟踪精度的影响，但是参考路径的曲率变化也是影响路径跟踪精度的一个重要因素。本文考虑无人驾驶车辆在变曲率条件下的路径跟踪精度和稳定性，提出一种基于预瞄跟随理论(PFT)的MPC横向控制控制器(PFT-MPC)，在不增加MPC控制器计算量的同时提高预测范围，提高变曲率道路上的跟踪精度和稳定性。

2. 路径跟踪模型

车辆动力学模型

车辆的动力学模型，是MPC控制器预测方程的基础，需要在较为准确描述动力学的基础上进行简化，以提高MPC控制器的计算速度。本文采用3-DOF车辆动力学模型，如图1所示。

Figure 1. Vehicle dynamics model

图1. 车辆动力学模型

图1中， $X$ ， $Y$ ， $\phi$ 是车辆在惯性坐标系下的横向位置、纵向位置和航向角； $F_{lf}$ ， $F_{lr}$ 分别为前后轮胎上的纵向力； $F_{cf}$ ， $F_{cr}$ 分别为前后轮上的侧向力， $F_{xf}$ ， $F_{xr}$ 分别为作用在车辆前轴和后轴上的轮胎纵向力的合力， $F_{yf}$ ， $F_{yr}$ 分别为作用在车辆前轴和后轴上的轮胎侧向力的合力。

基于小角度假设和线性轮胎模型的车辆动力学非线性模型可表示为：

$m\ddot{y}=-m\stackrel{\dot{}}{x}\stackrel{\dot{}}{\phi }+2\left[C_{cf}\left({\delta }_f-\left(\stackrel{\dot{}}{y}+a\stackrel{\dot{}}{\phi }\right)/\stackrel{\dot{}}{x}\right)+C_{cr}\left(b\stackrel{\dot{}}{\phi }-\stackrel{\dot{}}{y}\right)/\stackrel{\dot{}}{x}\right]$ (1)

$m\ddot{x}=-m\stackrel{\dot{}}{y}\stackrel{\dot{}}{\phi }+2\left[C_{lf}{s}_f+C_{cf}\left({\delta }_f-\left(\stackrel{\dot{}}{y}+a\stackrel{\dot{}}{\phi }\right)/\stackrel{\dot{}}{x}\right){\delta }_f+C_{lr}{s}_r\right]$ (2)

$I_z\ddot{\phi }=2\left[a{C}_{cf}\left({\delta }_f-\left(\stackrel{\dot{}}{y}+a\stackrel{\dot{}}{\phi }\right)/\stackrel{\dot{}}{x}\right)-b{C}_{cr}\left(b\stackrel{\dot{}}{\phi }-\stackrel{\dot{}}{y}\right)/\stackrel{\dot{}}{x}\right]$ (3)

$\stackrel{\dot{}}{Y}=\stackrel{\dot{}}{x}\sin \left(\phi \right)+\stackrel{\dot{}}{y}\cos \left(\phi \right)$ (4)

$\stackrel{\dot{}}{X}=\stackrel{\dot{}}{x}\cos \left(\phi \right)-\stackrel{\dot{}}{y}\sin \left(\phi \right)$ (5)

式中： ${\delta }_f$ ， $\stackrel{\dot{}}{\phi }$ ，m和 $I_z$ 是前轮转向角，车辆横摆角速度，车辆质量和车辆绕z轴的转动惯量； $\stackrel{\dot{}}{x}$ ，y是车辆质心处沿x，y方向的速度；Y和X是惯性坐标系下的车辆坐标； $C_{cf}$ ， $C_{cr}$ 是车辆前轮和后轮的侧偏刚度； $s_f$ ， $s_r$ 是车辆前轮和后轮的滑移率；a，b是车辆质心处到前后轴的距离。

3. PFT-MPC控制器设计

3.1. PFT-MPC控制器框架

图2显示了MPC和PFT的结合，其中t表示车辆的当前时刻，Y，X表示惯性坐标系下的纵向和横向位置，p，T分别表示MPC的预测时域和PFT的预瞄时间。MPC在预测时域范围内计算预测状态，PFT在MPC预测状态的基础上计算出最佳参考横摆角，作为MPC控制器的参考横摆角。最佳参考横摆角可以利用PFT在 $X\left(t+T\right)~X\left(t+p+T\right)$ 范围内计算出来。

Figure 2. Prediction range of combined MPC and PFT

图2. MPC和PFT相结合的预测范围

3.2. 预瞄跟随理论(PFT)

本节介绍预瞄跟随理论(PFT)的实现过程，车辆的运动遵循阿克曼机构的原理：

$a_y=\frac{V^2}{R}$ (6)

式中： $a_y$ 为车辆的侧向加速度；R，V为道路曲率半径和车辆速度。

在图3中， $F\left(X\right)$ 是参考路径的表达式，t为当前时刻，d为驾驶员预瞄距离， $T=d/V$ 为预瞄时间。

Figure 3. Preview follow theory

图3. 预瞄跟随理论

在t时刻，车辆的侧向位置坐标y和横向速度 $v_y$ 分别为：

$y=Y\left(t\right)$ (7)

$v_y=\stackrel{\dot{}}{Y}\left(t\right)$ (8)

在图3中，在T时间后，车辆侧向速度 $\stackrel{\dot{}}{Y}\left(t\right)$ 和侧向加速度 $\ddot{Y}\left(t\right)$ 的作用下，到达预瞄点 $F\left(X\left(t+T\right)\right)$ 。其侧向位移为 $Y\left(t+T\right)$ 的表达式可以描述为：

$Y\left(t+T\right)=Y\left(T\right)+T\stackrel{\dot{}}{Y}\left(t\right)+\frac{T^2}{2}\ddot{Y}\left(t\right)$ (9)

在期望的车辆侧向速度 $\stackrel{\dot{}}{Y}\left(t\right)$ 和侧向加速度 $\ddot{Y}\left(t\right)$ 作用下，侧向位移为 $Y\left(t+T\right)$ 和预瞄点 $F\left(X\left(t+T\right)\right)$ 重合，即 $Y\left(t+T\right)=F\left(X\left(t+T\right)\right)$ 。期望侧向加速度为：

$\ddot{y}\left(t\right)=\frac{2}{T^2}\left[F\left(X\left(t+T\right)\right)-Y\left(t\right)-T\stackrel{\dot{}}{Y}\left(t\right)\right]$ (10)

由公式(6)可以求得车辆的转弯半径为：

$R=\frac{d^2}{2\left[F\left(X\left(t+T\right)\right)-Y\left(t\right)-T\stackrel{\dot{}}{Y}\left(t\right)\right]}$ (11)

车辆在转弯过程中，横摆角速度 $\stackrel{\dot{}}{\phi }$ 由车辆速度和转弯半径确定：

$\stackrel{\dot{}}{\phi }=\frac{V}{R}$ (12)

在MPC控制器的采样周期 $t_s$ 间隔内，可以由车辆横摆角速度 $\stackrel{\dot{}}{\phi }$ 数值积分求得MPC的参考横摆角 $\phi$ ：

$\phi ={\displaystyle \underset{t}{\overset{t+t_s}{\int }}\stackrel{\dot{}}{\phi }\text{d}t}={\displaystyle \underset{t}{\overset{t+t_s}{\int }}\frac{V}{R}\text{d}t}$ (13)

这里注意我们求得的参考横摆角 $\phi$ 是由预瞄点计算得来，相当于是在当前参考路径中加入未来的路径点信息。

3.3. 参考路径点生成模块

在k时刻，可以计算在 $k+i$ 时刻的参考路径点 $r\left(k+i\right)={\left[{\phi }_{ref}\left(k+i\right),Y_{ref}\left(k+i\right),X_{ref}\left(k+i\right)\right]}^{\text{T}}$ ， $i=1,2,\cdots ,N_p$ ， $N_p$ 为MPC控制器的预测时域。算法1中描述了获得参考路径点的方法。

Algorithm 1. Generate reference path points

算法1. 生成参考路径点

3.4. MPC横向控制器设计

MPC常用于路径跟踪控制。基于小角度假设的车辆动力学线性模型如式(15)所示，状态空间方程可以写为：

${\stackrel{\dot{}}{x}}_c\left(k\right)=f_c\left(x_c\left(k\right),u\left(k\right)\right)$ (14)

式中： $x_c$ 为系统状态量；u为系统的控制输入。

本文采用泰勒级数在状态点 $x_0\left(k\right)$ 处进行线性化，其线性化过程如下所示：

${\stackrel{\dot{}}{x}}_c\left(k\right)=A_0\left(k\right)x_c\left(k\right)+B_0\left(k\right)u\left(k\right)$ (15)

式中： $A_0=\frac{\partial f_c}{\partial x}|x_0,u_0$ ， $B_0=\frac{\partial f_c}{\partial u}|x_0,u_0$ 。

对于MPC控制器，通过求解满足目标函数以及各种约束的优化问题，在预测时域内求解最优控制序列，并将该控制序列的第一个元素作为受控对象的实际控制量，重复上述的求解过程，实现对被控对象的持续控制。为了将模型(公式14)应用于MPC控制器的设计中，需要对公式(14)进行离散化处理：

$x_c\left(k+1\right)=A_1{x}_c\left(k\right)+B_{1}u\left(k\right)$ (16)

式中： $x={\left[\begin{array}{cccccc}\stackrel{\dot{}}{y}& \stackrel{\dot{}}{x}& \stackrel{\dot{}}{\phi }& \phi & Y& X\end{array}\right]}^{\text{T}}$ ； $u=\delta f$ ； $A_1={\left(I-\frac{t_s{A}_0}{2}\right)}^{-1}\left(I+\frac{t_s{A}_0}{2}\right)$ ； $B_1={\left(I-\frac{t_s{A}_0}{2}\right)}^{-1}{t}_s{B}_0$ ； $t_s$ 为系统采样时间。

在MPC控制器中控制变量的变化速度对实际的被控系统有较大影响，需要对控制量的增量进行约束限制。将状态量作以下变换：

$x\left(k\right)=\left[\begin{array}{c}{x}_c\left(k\right)\\ u\left(k-1\right)\end{array}\right]$ (17)

可以得到新的状态空间表达式：

$x\left(k+1\right)=Ax\left(k\right)+B\Delta u\left(k\right)$ (18)

$y\left(k\right)=Cx\left(k+1\right)$ (19)

式中： $A=\left[\begin{array}{cc}{A}_1& B_1\\ 0_{m\times n}& I_m\end{array}\right]$ ； $B=\left[\begin{array}{c}{B}_1\\ I_m\end{array}\right]$ ； $C={\left[\begin{array}{cccccc}0& 0& 0& 1& 1& 1\end{array}\right]}^{\text{T}}$ ； $\Delta u\left(k\right)=u\left(k\right)-u\left(k-1\right)$ ；n为状态量的维度；m为控制量的维度。

目标函数需要加入系统状态量的偏差和控制量的大小作为优化目标，以保证无人驾驶车辆快速且平稳的跟踪上参考路径。目标函数可以采用以下形式：

$J\left(k\right)={\displaystyle \underset{i=1}{\overset{N_p}{\sum }}{\Vert y\left(k+i\right)-r_{ref}\left(k+i\right)\Vert }_Q^2}+{\displaystyle \underset{i=1}{\overset{N_c-1}{\sum }}{\Vert \Delta u\left(k+i\right)\Vert }_R^2}$ (20)

式中： $Q$ ， $R$ 为权重矩阵； $N_p$ ， $N_c$ 分别为MPC的预测时域和控制时域。

本文主要考虑控制过程中的控制量极限约束和控制增量约束，控制量极限约束为：

$u_{\min }\left(t+k\right)\le u\left(t+k\right)\le u_{\max }\left(t+k\right),k=0,1,\cdots ,N_c-1$ (21)

控制增量约束为：

$\Delta u_{\min }\left(t+k\right)\le \Delta u\left(t+k\right)\le \Delta u_{\max }\left(t+k\right),k=0,1,\cdots ,N_c-1$ (22)

二次优化问题可以用约束条件来求解。计算出最优转向角后，仅第一个输出用作系统下一次采样时间的控制。优化问题在每个步骤内递归求解。

4. 仿真实验与验证

为了验证本文所提出的MPC横向控制控制器和PID纵向速度控制器有效性和可行性，在Carsim-Simulink联合仿真平台上进行试验仿真和验证。

为了验证本文的控制方法在变曲率的道路环境的路径跟踪效果，本文的采用双移线形式的参考路径如图4所示，表达式如式(23)和(24)所示。

Figure 4. Reference path

图4. 参考路径

$Y_{ref}\left(X\right)=\frac{d_{y1}}{2}\left[1+\tanh \left(z_1\right)\right]-\frac{d_{y2}}{2}\left[1+\tanh \left(z_2\right)\right]$ (23)

${\phi }_{ref}\left(X\right)=\arctan \left[d_{y1}{\left(\frac{1}{\cosh \left(z_1\right)}\right)}^2\left(\frac{1.2}{d_{x1}}\right)-d_{y2}{\left(\frac{1}{\cosh \left(z_2\right)}\right)}^2\left(\frac{1.2}{d_{x2}}\right)\right]$ (24)

式中： $z_1=\frac{2.4}{25}\left(X-27.19\right)-1.2$ ； $z_2=\frac{2.4}{21.95}\left(X-56.46\right)-1.2$ ； $d_{x1}=25$ ； $d_{x2}=21.95$ ； $d_{y1}=4.05$ ； $d_{y2}=5.7$ 。

PFT-MPC横向控制器的参数如表1所示，车辆的基本参数如表2所示。

Table 1. PFT-MPC controller parameters

表1. PFT-MPC控制器参数

Table 2. Longitudinal controller and vehicle parameters

表2. 纵向控制器和车辆参数

在我们的仿真实验中，采用极限速度路径跟踪工况径跟踪工况。考虑到本文所提供的双移线参考路径的曲率较大，为了保证无人驾驶车辆在路径跟踪过程中不发生侧滑和侧翻，跟踪双移线的极限速度设定为20 m/s。为了验证本文所提供的路径跟踪方法的有效性，在相同的路径跟踪工况下，对比了PFT-MPC和MPC路径跟踪方法。

本文在大曲率道路条件下，贴近道路极限速度情况下的仿真结果，如图5所示。由图5(b)所示：在贴近极限速度工况下，无人驾驶车辆采用本文所提供的PFT-MPC控制方法的横向误差最大为0.137 m，而无人驾驶车辆采用MPC方法的横向误差最大可以达到0.188 m。本文所提供的PFT-MPC的控制方法相较于MPC方法在极限速度工况的横向控制性能上可以提高27%，但是PFT-MPC相较于MPC计算量并没有提高。横向控制性能提升是因为PFT-MPC控制方法是将预瞄点范围的道路信息提前加入当前参考路径点中，可以在每个控制时域中使车辆的横向控制性能达到最优，可以实现提前转弯的效果。

Figure 5. Double line shift simulation results at 20 m/s

图5. 20 m/s双移线仿真结果

5. 结论

本文针对无人驾驶车辆在大曲率道路环境的跟踪问题，设计了基于预瞄跟随理论(PFT)的MPC横向控制控制器(PFT-MPC)，相较于MPC方法可以提高无人驾驶车辆在横向控制上的性能，PFT-MPC的控制方法相较于MPC方法在极限速度工况的横向控制性能上最大可以提高27%。另外，PFT-MPC可以在不增加计算量的同时提高控制器的预测范围，使横向控制器(PFT-MPC)可以使用更远的路径信息，在弯道上的路径跟踪效果有明显提升。仿真结果表明：在变曲率道路工况下，本文所提供的方法(PFT-MPC)可以保证较高的控制精度，从横向误差和航向误差可以看出，本文所提供的方法在无人驾驶车辆的路径跟踪上保证较好的稳定性和准确性。

NOTES

^*通讯作者。