近些年，沿规定导航路径行驶的运输车以及园区观光游览车逐步成为企业研究的热点^[1]，无人车在商用运输^[2-3]以及未来军事领域都有广阔的应用前景。作为自动驾驶核心技术之一，路径制导算法以及控制器的设计具有很强的理论研究价值和现实意义，国内外学者在这个领域开展了广泛而深入研究。目前主流的制导方法有两种：第一种是纯追踪(Pure Pursuit)算法^[4]，该方式在车辆以及移动机器人的循迹领域应用广泛，但在高速行驶工况下会出现超调。第二种是Stanley算法，该方法对环境中的随机干扰有较弱的鲁棒性，转弯时极易冲出跑道。目前循迹车辆的控制方法大多基于现代控制理论，主流方法有LQR控制^[5]，PID控制^[6]和模型预测控制^[7](model predictive control, MPC)等。LQR控制选取了车辆运动学模型作为研究对象，但忽略车辆动力学约束，快速转弯时极易侧偏。经典PID控制简化了控制问题，鲁棒性高，控制参数需要反复地修改、调试，耗时较长。模型预测控制能实时处理多变量约束优化问题，权重系数选取灵活多样，在提高循迹实时性和稳定性方面有不错的效果。

针对无人车路径跟踪问题的不足之处，本文总结了文献[8]中传统视线(line-of-sight, LOS)制导法动态响应慢的缺点，提出了时变前视距离的自适应LOS算法，把目标轨迹的跟踪转化为航向的跟踪，并基于车辆横向动力学模型以及模型预测控制方式求解最优反馈的方向盘控制指令。最后，为验证上述所提跟踪策略的有效性与优越性，在Simulink仿真环境分别进行直线路径和曲线路径追踪测试，验证了改进LOS制导算法能够基于当前的横向误差及调整前视距离，更出色地完成路径跟踪任务。

1 基于时变前视距离的视线(LOS)制导法 1.1 传统LOS的基本原理

视线(line-of-sight，LOS)制导法^[9]是在地对空导弹的多目标拦截，自动驾驶循迹领域应用较为广泛的经典制导算法，不仅能追踪曲率k为0的直线路径还可以追踪曲率k>0的曲线路径，但是变化曲率的存在会干扰车辆当前位置和目标位置的横向误差的大小，使得误差不能稳定的收敛。虽然曲率k不为零，但可以把曲线轨迹切分为许多段曲率为0的直线路径，本文选取细分后的直线作为追踪的目标路径。视线制导算法的原理如图 1所示。

LOS引导车辆收敛于目标直线P_w-1P_w上的LOS点P_los(x_los, y_los)，其中，P_w-1=[x_w-1, y_w-1]∈R², P_w=[x_w, y_w]∈R²并保持车辆的方位角ψ和目标直线的航向角α_w一致。车辆在世界坐标系(OXY)中的位置坐标为P=[x, y]∈R², 则循迹过程中确定P_los(x_los, y_los)的方法为：作车辆位置至期望直线的垂线，P_w-1P_w上与垂足F相距前视距离Δ的位置即为LOS点。设定一个以P_w-1为原点的正切参考坐标系(OX_PY_P)，此时参考路径在世界坐标系(OXY)中的航向角可以表示成

$ a_{w}=\arctan 2\left(y_{w}-y_{w-1}, x_{w}-x_{w-1}\right) $

(1)

循迹过程中的沿向误差x_e和横向误差y_e经过坐标转换可以表示为

$ \left[\begin{array}{l} x_{e} \\ y_{e} \end{array}\right]=\boldsymbol{R}^{\mathrm{T}}\left(a_{w}\right)\left[\begin{array}{c} x-x_{w-1} \\ y-y_{w-1} \end{array}\right] $

(2)

式中，R^T为世界坐标系(OXY)到参考路径坐标系(OX_PY_P)的转换矩阵, 表示为

$ \boldsymbol{R}\left(a_{w}\right)=\left[\begin{array}{cc} \cos a_{w} & -\sin a_{w} \\ \sin a_{w} & \cos a_{w} \end{array}\right] $

(3)

展开公式(2)可得追踪的横向距离误差

$ y_{e}=-\left(x-x_{w-1}\right) \sin a_{w}+\left(y-y_{w-1}\right) \cos a_{w} $

(4)

车辆需要快速且稳定地收敛于期望的路径上的LOS点P_los(x_los, y_los)并沿着目标轨迹稳定的追踪，因此，制导系统有两个操纵目标：

1) 横向控制：随着车辆接近目标路径，横向误差y_e逐渐收敛于0，即

$ \lim \limits_{t \rightarrow \infty} y_{e}(t)=0 $

(5)

2) 纵向控制：循迹车在纵向的实际速度与期望速度之间的误差逐步减小，即

$ \lim \limits_{t \rightarrow \infty}\left[u(t)-u_{\mathrm{d}}(t)\right]=0 $

(6)

式中，u_d为沿着车体x坐标轴的纵向期望线速度。

此外，无人车接近参考目标点P_w时，对应的目标直线由P_w-1P_w换为P_wP_w+1。一般情况下，当车辆驶入以P_w点为圆心，R₀为半径的接纳圆形区域，即(x－x_w)²+(y－y_w)² < R₀²时，可判定车辆的目标路径点需要切换，并对下一个目标直线进行跟踪。

车辆刚进入半径为R₀的接纳圆，目标航向角ψ_los角以及车辆方位角ψ产生突变，造成车辆无法避免地偏离参考直线路径。彼此相邻路径的夹角θ∈(0, $\frac{\pi }{2}$)时，较小的折角要求车辆提前发出转向指令，从而有足够的时间改变方位并通过弯曲地带。如果接纳圆半径R₀的取值过小，车辆转弯会相对延迟，造成循迹追踪的控制效果不理想；相反地，如果θ取值过大，同时接纳圆半径取较大值，车辆会提前追踪下一个目标路径直线，造成对上一个直线追踪的不完整。所以，R₀与相邻路径折角θ之间存在着对应的最优函数关系，使得追踪过程中车辆的操纵特性发挥的更为出色。R₀有必要满足R_min < R₀ < R_max。R_min可以保证车辆不会因为接纳圆过小而无法正常切换至接下来的目标直线。R_max则能可以避免接纳圆半径R₀过大所产生的循迹点追踪不完全。

无人车长度为L, 接纳圆半径R₀与相邻路径的夹角θ成反比关系，设计如下的公式：

$ R_{0}=\left\{\begin{array}{l} l\left(\frac{{\rm{ \mathsf{ π} }}}{\theta}-1\right) L+R_{\min }, \theta \geqslant \frac{{\rm{ \mathsf{ π} }}}{\sqrt{\frac{R_{\max }-R_{\min }}{l L}+1}} \\ R_{\max }, 0 \leqslant \theta \leqslant \frac{{\rm{ \mathsf{ π} }}}{\sqrt{\frac{R_{\max }}{l L} R_{\min }+1}} \end{array}\right. $

(7)

上式中的l数值取决于车辆的操纵性能，确定l变量的具体方法如下所示：

1) 取n+2个期望点，生成的轨迹会有n个不同数值的航向夹角，分别为{θ₁, θ₂, …, θ_n}。设计期望轨迹时尽可能满足相邻目标直线之间的航向夹角θ彼此不同。

2) 应用恒定的接纳圆半径R₀进行循迹，计算不同航向夹角下各个目标直线路径的平均横向追踪误差e_a，其公式为

$ e_{\mathrm{a}}=\frac{1}{N} \sum\limits_{i=1}^{N}|e(i)| $

(8)

式中，N代表整个循迹过程的总步长，e(i)代表第i个目标直线的横向追踪误差，e_a取较小数值时说明追踪效果更为出色。

3) 设计一系列接纳圆半径R₀，令它们的集合为{L, 2L, …, nL}，分别在上述半径下完成路径追踪，获取各个航向夹角θ_i所匹配的最佳R₀集合序列{R₀₁, R₀₂, …, R_0n}，原则上采集的目标点越丰富，辨识的效果越理想(l的估计值更为准确)，与此同时也增加了工作内容，于是常取4≤n≤6。

4) 基于Matlab最小二乘估计函数获取的l估计数值$\hat l$，对应的估计公式为

$ \hat{l}=\left(\boldsymbol{x}^{\mathrm{T}} \boldsymbol{x}\right)^{-1} \boldsymbol{x}^{\mathrm{T}} \boldsymbol{y} $

(9)

式中：

$ \boldsymbol{x}=\left[\left(\frac{{\rm{ \mathsf{ π} }}}{\theta_{1}}-1\right)^{2},\left(\frac{{\rm{ \mathsf{ π} }}}{\theta_{1}}-1\right)^{2}, \cdots \cdots,\left(\frac{{\rm{ \mathsf{ π} }}}{\theta_{n}}-1\right)^{2}\right]^{\mathrm{T}} $

$ \boldsymbol{y}=\left[\frac{\bar{R}_{01}-R_{\min }}{L}, \frac{\bar{R}_{02}-R_{\min }}{L}, \cdots \cdots, \frac{\bar{R}_{0 n}-R_{\min }}{L}\right]^{\mathrm{T}} $

1.2 基于可变前视距离的自适应LOS

传统的视线(LOS)制导法结合无人车实际位置与理想位置之间的状态误差，按照一定的制导算法计算出控制命令，发送给底层控制器，改变车辆运动状态，消除横向和纵向偏差。无人车的控制目标是无限接近于直线路径上目标点P_los(x_los, y_los)，令参考路径方向P_w-1P_w与车辆方位o_bP_los的夹角ψ_los逐渐收敛到零，因此车辆期望航向角度的公式如下:

$ \psi_{\mathrm{d}}=a_{w}-\psi_{\text {los }} $

(10)

式中

$ \psi_{\mathrm{los}}=\arctan \left(\frac{y_{e}}{\varDelta}\right) \in\left[-\frac{{\rm{ \mathsf{ π} }}}{2}, \frac{{\rm{ \mathsf{ π} }}}{2}\right] $

(11)

通常情况下，前视距离Δ为一个固定的数值。循迹初始阶段的横向误差y_e过大，车辆本应迅速地收敛于LOS点，但是固定的前视距离Δ无法快速调整方位，因此收敛缓慢；随着车辆接近参考路径，y_e数值逐渐变小，平稳地追踪成为系统的首要目标，但是固定的Δ使得循迹存在震荡，抖动的问题，因此本文根据横向误差y_e的数值大小，在线调整前视距离Δ，提出一种基于可变前视距离的LOS制导法，使得车辆可以更加迅速且平稳地实现路径追踪。

本文所提出的时变前视距离公式如下:

$ \varDelta\left(y_{c}\right)=\left(\varDelta_{\max }-\varDelta_{\min }\right) \mathrm{e}^{-y\left|y_{e}\right|}+\varDelta_{\min } $

(12)

从上述公式(12)可以看出，前视距离Δ为横向距离误差函数，与车速和目标路径曲率无关。其中，Δ_min和Δ_max为循迹过程中前视距离的阈值，γ为收敛速率。为取得出色的路径追踪效果，前视距离的最小、最大值Δ_min与Δ_max依据经验分别取车身长度的4倍与8倍，γ取0.1。当车辆远离目标路径时，Δ取较小值，此时收敛速度加快；当车辆靠近目标路径时，Δ取较大值，此时车辆的平稳性提高，有效避免了震荡；上述的远和近的概念都是相对，当设定Δ_min、Δ_max以及γ的数值时应考虑车辆的动力学约束。

1.3 制导法稳定性分析

车辆在世界坐标系下的运动学方程式可以写成

$ \left\{\begin{array}{l} \dot{x}=u \cos \psi-v \sin \psi \\ \dot{y}=u \sin \psi+v \cos \psi \end{array}\right. $

(13)

式中: u，v为车辆纵向和横向的线速度，ψ是车辆当前时刻的方位角。

对公式(4)两边求导，可得

$ \begin{aligned} \dot{y}_{e}=&-\dot{x} \sin a_{w}+\dot{y} \cos a_{w}=\\ & u \sin \left(\psi-a_{w}\right)+v \cos \left(\psi-a_{w}\right) \approx \\ & U \sin \left(\psi-a_{w}\right) \end{aligned} $

(14)

其中，$U = \sqrt {{u^2} + {v^2}} $为车辆前进的合速度。

为使LOS所规划的轨迹尽可能贴近实际情况，考虑横向误差y_e的运动学约束，接下来对制导系统在平衡点y_e=0附近的轨迹收敛稳定性进行分析。假设无人车循迹过程中精准地沿着期望路径行驶，此时实际方位角ψ满足

$ \psi=\psi_{\mathrm{d}}=a_{w}-\arctan \left(\frac{y_{e}}{\varDelta}\right) $

(15)

在sin ψ_los取值很小的情况下，sin ψ_los=ψ_los，将公式(15)代入到公式(14)，得到

$ \dot{y}_{e}=U \frac{-y_{e}}{\sqrt{y_{e}^{2}+\varDelta^{2}}} $

(16)

本文使用李雅普诺夫(Lyapunov)稳定性判据来证明车辆LOS制导系统的横向跟踪误差收敛可以到平衡性状态y_e=0，设置一个正定的李雅普诺夫(Lyapunov)函数

$ V_{1}\left(y_{e}\right)=\frac{1}{2} y_{e}^{2}>0 $

(17)

对横向误差y_e求导，并代入公式(16)可得

$ \dot{V}_{1}\left(y_{e}\right)=-\frac{U y_{e}^{2}}{\sqrt{y_{e}^{2}+\varDelta^{2}}} $

(18)

式中：前进速度U大于0，${\dot V_1} < 0$，说明该制导系统在平衡点y_e=0具有全局渐进稳定特性(Uniform Global Asymptotic Stability，UGAS)。在球空间D₁={y_e∈R| y_e ≤Φ, Φ>0}内部，李雅普诺夫函数的导数满足

$ \dot{V}_{1}\left(y_{e}\right)=-\frac{U y_{e}^{2}}{\sqrt{y_{e}^{2}+\varDelta^{2}}} \leqslant-k_{1} y_{e}^{2} $

(19)

式中：$0 < {k_1} < \frac{U}{{\sqrt {{\mathit{\Phi }^2} + {\mathit{\Delta }^2}} }}$，上式证明可车辆制导系统在平衡点y_e=0具有半全局一致指数稳定性(Uniform Local Exponential Stability，ULES)^[10]。综上所述，LOS制导法不仅保证车辆最后趋近于平衡点y_e=0，而且以较小的衰减速率完成轨迹收敛。

2 自适应LOS制导结合模型预测控制(MPC)的跟踪方法设计 2.1 车辆路径跟踪控制的原理

针对自动驾驶车辆的路径跟踪问题，本文设计了一种基于系统跟踪模型的预测控制方法。原理如下：首先利用自适应LOS把对目标路径的跟踪简化为对航向的跟踪，然后基于系统跟踪模型以及模型预测控制方法实现车辆对期望方位的收敛，跟踪系统的架构如图 2所示。

该跟踪控制系统由3个部分组成，即环境感知子系统，制导子系统以及底层控制子系统。环境感知系统主要利用激光雷达、GPS等传感器获取无人车的位置，速度以及姿态信息。制导系统按照一定的制导算法生成期望路径与目标方位角。底层控制系统的任务是接受制导系统的规划指令，改变车辆运动状态，消除偏差。

循迹控制的具体过程如下：

1) 上位机记录GPS的经纬度坐标并生成参考路径点。一般情况下，参考路径是相邻点连接形成的直线段。

2) 自适应LOS制导律依据参考路径，车身姿态，位置等信息计算出车辆与参考路径的横向距离误差以及方位角的相位差。

3) 横向控制采用模型预测控制的方式，依据横向距离误差、方位角相位差以及车速等信息计算最优的方向盘指令。纵向控制采用传统比例-积分-微分这种闭环控制方式(PID控制)，生成油门和刹车的指令，实现汽车的定速巡航。

4) 将线控油门，方向盘转角指令输送到车辆的底盘。

5) 环境感知子系统得到车辆行驶过程中的位置，车速，车身姿态等信息，再把数据及时的传送到上层制导系统，生成期望的状态参数。

2.2 无人车的动力学模型

汽车动力学模型^[11]是车辆实现仿真控制的基础，可以更好地分析车辆稳态响应以及瞬态响应。然而，车辆具有非线性的特性，考虑到仿真的计算效率和耗时，本文使用单车模型进行动力学分析并做如下假设：

1) 单纯考虑轮胎的线性侧偏特性，忽略轮胎里的横纵耦合关系。

2) 忽略纵向空气动力学。

3) 假设汽车只有前轮转向，并且忽略悬架对稳定性的影响。

4) 忽略汽车载荷左右的转移。

根据上述所列的假设，建立如图 3所示的汽车动力学模型，O-XYZ是世界惯性坐标系，o-xyz是车体坐标系。

基于车辆动力学模型和牛顿第二定律，车辆沿着车体坐标系x轴、y轴和绕z轴的方程分别为：

在x轴方向上

$ m a_{x}=F_{x f}+F_{x r} $

(20)

在y轴方向上

$ m a_{\mathrm{y}}=F_{y f}+F_{y r} $

(21)

在z轴方向上

$ I_{z} \ddot{\psi}=a F_{t f}-b F_{y r} $

(22)

其中, m为整车整备质量，I_z为绕z轴的转动惯量，a、b分别为重心到前后轴的距离。经过分析，y轴的加速度a_y由两部分组成：分别是y轴方向的位移加速度和绕z轴转动的向心加速度，表示如下：

$ a_{y}=\dot{v}+u \dot{\psi} $

(23)

代入到公式(21), 可得

$ m(\dot{v}+u \dot{\psi})=F_{y f}+F_{y r} $

(24)

轮胎由于受到横向的载荷压力，会产生一个很小的侧偏角，前后轮胎所受横向力F_yf, F_yr与对应侧偏角ρ_f, ρ_r的线性关系为

$ \left\{\begin{array}{l} F_{y f}=2 C_{f} \rho_{f} \\ F_{y r}=2 C_{r} \rho_{r} \end{array}\right. $

(25)

式中, C_f和C_r分别为汽车前后轮胎的侧偏刚度。当前后轮的侧偏角都取较小值时, tan ρ=ρ，ρ_f和ρ_r计算公式为

$ \left\{\begin{array}{l} \rho_{\mathrm{f}}=\delta-\frac{v+a \dot{\psi}}{u} \\ \rho_{\mathrm{r}}=\frac{b \dot{\psi}-v}{u} \end{array}\right. $

(26)

式中，δ为前轮转角。结合公式(24)~(26)得到横向位移y与方位角ψ的状态空间模型。

$ \left[\begin{array}{l} \ddot{y} \\ \ddot{\psi} \end{array}\right]=\left[\begin{array}{ll} a_{11} & a_{12} \\ a_{21} & a_{22} \end{array}\right]\left[\begin{array}{l} \dot{y} \\ \dot{\psi} \end{array}\right]+\left[\begin{array}{c} 2 C_{f} / m \\ 2 a C_{f} / I_{z} \end{array}\right] \delta $

(27)

其中：

$ a_{11}=-\left(2 C_{f}+2 C_{r}\right) / m u $

$ a_{12}=-u-\left(2 a C_{f}-2 b C_{r}\right) / m u $

$ a_{21}=\left(2 a C_{f}-2 b C_{r}\right) / I_{z} u $

$ a_{22}=-\left(2 a^{2} C_{f}+2 b^{2} C_{r}\right) / I_{z} u $

2.3 无人车的路径跟踪系统模型

自适应LOS制导获取车辆实时位置信息并与期望路径作比较，在线计算出期望的方位角度，使用模型预测控制器改变车辆状态。结合公式(14)和(27)，设计跟踪系统的非线性数学模型，可得

$ \left\{\begin{array}{l} \dot{y}_{e}=U \sin \bar{\psi} \\ \dot{\bar{\psi}}=r \\ \dot{r}=-\frac{2 b C_{r}-2 a C_{f}}{I_{z} u} v-\frac{2 a^{2} C_{f}+2 b^{2} C_{r}}{I_{z} u} r+\frac{2 a C_{f}}{I_{z}} \delta \end{array}\right. $

(28)

式中, ψ=ψ－a_w，r为车辆绕z轴旋转的角速度。从状态空间表达式来观察，系统状态变量为：x =[y_e, ψ, r]^T，u =δ为控制输入变量，则该数学模型经过转换得到如下的表达式:

$ \dot{\boldsymbol{x}}=f(\boldsymbol{x}, \boldsymbol{u})=\left[\begin{array}{l} U \sin \bar{\psi} \\ r \\ -\frac{2 b C_{r}-2 a C_{f}}{I_{z} u} v- \\ \frac{2 a^{2} C_{f}+2 b^{2} C_{r}}{I_{z} u} r+\frac{2 a C_{f}}{I_{z}} \delta \end{array}\right] $

(29)

上述公式(29)中不仅包含状态变量的一次项，还有三角函数项sin ψ，函数f(x, u)为非线性方程，因此该数学模型具有非线性特性。导致系统计算量增加，实时性变差，有必要对该系统模型进行进一步的简化。车辆航向角误差ψ在小角度范围内浮动，假设sin ψ =ψ，并把横向速度加入到状态变量提升模型维度，最终的路径跟踪模型为

$ \left[\begin{array}{l} \dot{y}_{e} \\ \dot{\bar{\psi}} \\ \dot{v} \\ \dot{r} \end{array}\right]=\left[\begin{array}{cccc} 0 & U & 0 & 0 \\ 0 & 0 & 0 & 1 \\ 0 & 0 & a_{11} & a_{12} \\ 0 & 0 & a_{21} & a_{22} \end{array}\right]\left[\begin{array}{l} y_{e} \\ \bar{\psi} \\ v \\ r \end{array}\right]+\left[\begin{array}{l} 0 \\ 0 \\ 2 C_{f} / m \\ 2 a C_{f} / I_{z} \end{array}\right] \delta $

(30)

2.4 基于自适应模型预测控制的跟踪策略设计

模型预测控制(MPC)使用多步预测^[12]，滚动实时优化^[13]，反馈校正^[14]等策略缩小系统模型中状态变量的误差。本文设计的控制器能估算未来预测步长系统状态，修正车辆方位角误差。为书写方便，公式(30)要表示为状态方程，可得

$ \left\{\begin{array}{l} \dot{x}=\boldsymbol{A} x+\boldsymbol{B} u \\ y=\boldsymbol{C} x \end{array}\right. $

(31)

式中，x为状态变量，代表无人车当前状态的数值与目标数值的误差量；u为系统输入变量，y为系统输出变量，系统矩阵为

$ \boldsymbol{A}=\left[\begin{array}{cccc} 0 & U & 0 & 0 \\ 0 & 0 & 0 & 1 \\ 0 & 0 & a_{11} & a_{12} \\ 0 & 0 & a_{21} & a_{22} \end{array}\right], \boldsymbol{B}=\left[\begin{array}{l} 0 \\ 0 \\ 2 C_{f} / m \\ 2 a C_{f} / I_{z} \end{array}\right], $

$ \boldsymbol{C}=\left[\begin{array}{llll} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \end{array}\right] $

数值计算器需要处理离散数据信息，但是上式的状态方程是连续，有必要对系统进行离散化。本文使用欧拉算法实现状态方程的离散化，可以得到

$ \left\{\begin{array}{l} \boldsymbol{A}_{\mathrm{k}}=\boldsymbol{I}+T \boldsymbol{A} \\ \boldsymbol{B}_{\mathrm{k}}=T \boldsymbol{B} \end{array}\right. $

(32)

式中: A_k以及B_k分别表示经过欧拉算法离散化后的系统状态矩阵，T为离散间隔步长。综合公式(31)和(32)，可得

$ \left\{ {\begin{array}{*{20}{l}} {x(k + 1) = {\mathit{\boldsymbol{A}}_{\rm{k}}}x(k) + {\mathit{\boldsymbol{B}}_{\rm{k}}}u(k)}\\ {y(k) = {C_{\rm{k}}}x(k)} \end{array}} \right. $

(33)

为了精确地约束前轮转角的增量，把状态变量x(k)与输入量u(k-1)封装成一个新的状态变量

$ \varepsilon(k)=\left[\begin{array}{l} x(k) \\ u(k-1) \end{array}\right] $

(34)

更新后的系统模型为

$ \left\{ {\begin{array}{*{20}{l}} {\varepsilon (k + 1) = {{\mathit{\boldsymbol{\bar A}}}_{\rm{k}}}\varepsilon (k) + {{\mathit{\boldsymbol{\bar B}}}_{\rm{k}}}\mathit{\Delta }(k)}\\ {y(k) = {{\mathit{\boldsymbol{\bar C}}}_{\rm{k}}}\varepsilon (k)} \end{array}} \right. $

(35)

式中：${\mathit{\boldsymbol{\bar A}}_{\rm{k}}} = \left[ {\begin{array}{*{20}{c}} {{\mathit{\boldsymbol{A}}_{\rm{k}}}}&{{\mathit{\boldsymbol{B}}_{\rm{k}}}}\\ 0&1 \end{array}} \right], {\mathit{\boldsymbol{\bar B}}_{\rm{k}}} = \left[ \begin{array}{l} {\mathit{\boldsymbol{B}}_{\rm{k}}}\\ 1 \end{array} \right], {\mathit{\boldsymbol{\bar C}}_{\rm{k}}} = \left[ {\begin{array}{*{20}{c}} {{\mathit{\boldsymbol{C}}_{\rm{k}}}}&0 \end{array}} \right]$。

MPC可以获取系统某未来N_P步长内的预测输出，N_P预测时域内的离散状态变量方程可表示为

$ \left\{\begin{array}{l} \varepsilon(k+1 \mid k)=\overline{\boldsymbol{A}}_{\mathrm{k}} \varepsilon(k)+\overline{\boldsymbol{B}}_{\mathrm{k}} \varDelta u(k) \\ \varepsilon(k+2 \mid k)=\overline{\boldsymbol{A}}_{\mathrm{k}}^{2} \varepsilon(k)+\overline{\boldsymbol{A}}_{\mathrm{k}} \overline{\boldsymbol{B}}_{\mathrm{k}} \varDelta u(k)+ \\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \overline{\boldsymbol{B}}_{\mathrm{k}} \varDelta u(k+1) \\ \ \ \ \ \ \ \ \ \vdots \\ \varepsilon\left(k+N_{P} \mid k\right)=\overline{\boldsymbol{A}}_{\mathrm{k}}^{N_{P}} \varepsilon(k)+ \\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \overline{\boldsymbol{A}}_{\mathrm{k}}^{N_{P}-1} \overline{\boldsymbol{B}}_{\mathrm{k}} \varDelta u(k)+\cdots+ \\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \overline{\boldsymbol{A}}_{\mathrm{k}}^{N_{P}-N_{C}} \overline{\boldsymbol{B}}_{\mathrm{k}} \varDelta u\left(k+N_{\mathrm{C}}-1\right) \end{array}\right. $

(36)

上式中，N_P、N_C分别为预测时域和控制时域。N_C < N_P说明N_C时刻后控制输入Δu(k)已经达到稳定的状态，N_P时域内系统的输出表达为

$ \boldsymbol{Y}(k)=\boldsymbol{F} \boldsymbol{\varepsilon}(k)+\boldsymbol{K} \varDelta \boldsymbol{U}(k) $

(37)

式中，ΔU为控制时域N_C内的输入增量，F、K为系统矩阵，它们的表达式为

$ \left\{\begin{array}{l} \boldsymbol{Y}=\left[\begin{array}{llll} \boldsymbol{y}^{\mathrm{T}}(k+1) & \boldsymbol{y}^{\mathrm{T}}(k+2) & \cdots & \boldsymbol{y}^{\mathrm{T}}\left(k+N_{P}\right) \end{array}\right]^{\mathrm{T}} \\ \varDelta \boldsymbol{U}=\left[\begin{array}{llll} \varDelta \boldsymbol{u}^{\mathrm{T}}(k+1) & \varDelta \boldsymbol{u}^{\mathrm{T}}(k+2) & \cdots & \varDelta \boldsymbol{u}^{\mathrm{T}}\left(k+N_{P}-1\right) \end{array}\right]^{\mathrm{T}} \\ \ \ \ \ \boldsymbol{F}=\left[\begin{array}{llll} \left(\overline{\boldsymbol{C}}_{\mathrm{k}} \overline{\boldsymbol{A}}_{\mathrm{k}}\right)^{\mathrm{T}} & \left(\overline{\boldsymbol{C}}_{\mathrm{k}} \overline{\boldsymbol{A}}_{\mathrm{k}}{ }^{2}\right)^{\mathrm{T}} & \cdots & \left(\overline{\boldsymbol{C}}_{\mathrm{k}} \overline{\boldsymbol{A}}_{\mathrm{k}}{ }^{N_{P}}\right)^{\mathrm{T}} \end{array}\right]^{\mathrm{T}}\\ \boldsymbol{K}=\left[\begin{array}{cccc} \overline{\boldsymbol{C}}_{\mathrm{k}} \overline{\boldsymbol{B}}_{\mathrm{k}} & 0 & \cdots & 0 \\ \overline{\boldsymbol{C}}_{\mathrm{k}} \overline{\boldsymbol{A}}_{\mathrm{k}} \overline{\boldsymbol{B}}_{\mathrm{k}} & \overline{\boldsymbol{C}}_{\mathrm{k}} \overline{\boldsymbol{B}}_{\mathrm{k}} & \cdots & 0 \\ \vdots & \vdots & \cdots & \vdots \\ \overline{\boldsymbol{C}}_{\mathrm{k}} \overline{\boldsymbol{A}}_{\mathrm{k}}^{N_{P}-1}{ }_{\mathrm{k}} \overline{\boldsymbol{B}}_{\mathrm{k}} & \overline{\boldsymbol{C}}_{\mathrm{k}} \overline{\boldsymbol{A}}_{\mathrm{k}}^{N_{P}-2}{ }_{\mathrm{k}} \overline{\boldsymbol{B}}_{\mathrm{k}} & \cdots & \overline{\boldsymbol{C}}_{\mathrm{k}} \overline{\boldsymbol{A}}_{\mathrm{k}}^{N_{P}-N_{C}}{}_{\mathrm{k}} \overline{\boldsymbol{B}}_{\mathrm{k}} \end{array}\right] \end{array}\right. $

构建考虑横向误差以及控制量增量的惩罚函数，从控制序列中找最优解，惩罚函数的表达式为

$ \begin{aligned} \boldsymbol{J}(k)=& \sum\limits_{i=1}^{N_{P}}\left\|y_{\mathrm{d}}(k+i)-y(k+i)\right\|_{Q}^{2}+\\ & \sum\limits_{i=1}^{N_{C}}\|\varDelta u(k+i-1)\|_{R}^{2} \end{aligned} $

(38)

式中，J为惩罚函数，Q与R分别是预测时域权重系数和控制时域权重系数。由于y(k)已经代表当前状态数值与期望状态数值误差，所以期望状态y_d为0。为方便求解带约束二次规划问题，结合公式(37)和(38), 惩罚函数转化为标准的二次型，可得

$ \boldsymbol{J}(\varDelta \boldsymbol{U}(k))=\frac{1}{2} \varDelta \boldsymbol{U}(k)^{\mathrm{T}} \boldsymbol{H} \varDelta \boldsymbol{U}(k)+\boldsymbol{M}^{\mathrm{T}} \varDelta \boldsymbol{U}(k) $

(39)

式中：

$ \left\{ {\begin{array}{*{20}{l}} {\mathit{\boldsymbol{H}} = {\mathit{\boldsymbol{K}}^{\rm{T}}}\mathit{\boldsymbol{\bar QK}} + \mathit{\boldsymbol{\bar R}}}\\ {\mathit{\boldsymbol{M}} = {\mathit{\boldsymbol{K}}^{\rm{T}}}\mathit{\boldsymbol{\bar Q}}(\mathit{\boldsymbol{F}}\varepsilon (k))}\\ {\mathit{\boldsymbol{\bar Q}} = \left[ {\begin{array}{*{20}{l}} \mathit{\boldsymbol{Q}}&{}&{}\\ {}& \ddots &{}\\ {}&{}&\mathit{\boldsymbol{Q}} \end{array}} \right]}\\ {\mathit{\boldsymbol{\bar R}} = \left[ {\begin{array}{*{20}{l}} \mathit{\boldsymbol{R}}&{}&{}\\ {}& \ddots &{}\\ {}&{}&\mathit{\boldsymbol{R}} \end{array}} \right]} \end{array}} \right. $

实际控制过程中，系统的控制量和状态变量满足以下约束条件：

$ \left\{\begin{array}{l} \varDelta u_{\min }(k+i) \leqslant \varDelta u(k+i) \leqslant \varDelta u_{\max }(k+i), \\ \qquad \text { 式中 } i=0,1, \cdots, N_{C}-1 ; \\ u_{\min }(k+i) \leqslant u(k+i) \leqslant u_{\max }(k+i), \\ \qquad \text { 式中 } i=0,1, \cdots, N_{C}-1 ; \\ y_{\min }(k+i) \leqslant y(k+i) \leqslant y_{\max }(k+i), \\ \qquad \text { 式中 } i=0,1, \cdots, N_{P} \end{array}\right. $

(40)

上式中，Δu_max和Δu_min为N_C步长内控制增量约束阈值，u_max和u_min为N_C步长内控制量约束阈值，y_max和y_min为系统输出约束阈值。

综上所述，自适应视线LOS制导结合MPC的跟踪优化过程可转化为如下带有约束的二次规划^[15](Quadratic Programming, QP)问题。

$ \left\{\begin{array}{l} \min \boldsymbol{J}(\varDelta \boldsymbol{U})=\frac{1}{2} \varDelta \boldsymbol{U}^{\mathrm{T}} \boldsymbol{H} \varDelta \boldsymbol{U}+\boldsymbol{M}^{\mathrm{T}} \varDelta \boldsymbol{U} \\ \ \ \ \text { s.t. } \\ \boldsymbol{Y}(k)=\boldsymbol{F} \varepsilon(k)+\boldsymbol{K} \varDelta \boldsymbol{U}(k) \\ \varDelta u_{\min }(k) \leqslant \varDelta u(k) \leqslant \varDelta u_{\max }(k) \\ u_{\min }(k) \leqslant u(k) \leqslant u_{\max }(k) \\ y_{\min }(k) \leqslant y(k) \leqslant y_{\max }(k) \end{array}\right. $

(41)

对于实际循迹来讲，如何满足MPC控制器对轨迹跟踪精度与行驶稳定性的要求十分重要。传统MPC控制器中的预测时域步长N_P通常为一个恒定的数值，但该步长参数应当依据车道的几何形状及时进行调整，从而获得到较好的控制效果。降低预测时域步长N_P时，控制器对路径的跟踪精度会有所降低(尤其在曲率特别大的转弯处，追踪精度降低的更加明显)。目前主流的预测步长N_P自适应控制器原理如下：选取参考路径曲率的绝对值k来判定道路的几何信息，N_P可以依据跟踪轨迹曲率k的变化整定，实现自适应控制，达到较好的跟踪效果。MPC控制器中本身就等同包含前视过程，自适应也约等同于前视自适应，后续仿真会比较变参数N_P与自适应LOS的路径追踪效果。

预测步长与曲率之间的关系可表示为

$ N_{P}=\operatorname{Round}\left(c_{1}|k|+c_{2}\right) $

(42)

式中：c₁为曲率增益系数，c₂为预测步长N_P的最小值，本文取c₁=400, c₂=5。

3 仿真实验

本文提出了自适应视线(LOS)制导法结合MPC控制的循迹优化策略，为了检测该方法的有效性，需要通过Simulink仿真平台进行模拟：将自适应LOS制导结合传统MPC的路径跟踪策略应用到循迹控制过程中，同时设计了传统LOS +传统MPC以及传统LOS+自适应MPC的路径跟随方法，并将上述三种轨迹追踪仿真结果进行比较。车辆大多数时间沿直线路径行驶，偶尔遇到需要更换车道以及加速的工况。因此，本文分别设计了直线路径和曲线路径工况下的路径追踪仿真。考虑到计算机硬件的运算能力，设定传统MPC控制器的参数为：N_P=5, N_C=2, T=1 s；纵向PID控制器参数为：K_P=0.2, K_I=0.1, K_D=0，车辆模型参数见表 1。

3.1 直线路径的跟踪结果

车辆追踪直线路径时，设定期望车速28 km/h, 起步时的位置坐标为(-10, 40)并处于静止状态，横向初始偏差为-20 m，初始航向角度与目标路径方位角的差值为0 rad。仿真实验结果见图 3，图 3(a)清晰地表明传统LOS+传统MPC，传统LOS+变预测步长MPC以及自适应LOS+传统MPC这三种路径追踪策略均可实现直线的良好跟踪。经过对比，自适应LOS制导律机动灵活，响应迅速，可以更快收敛到目标直线，整体的行驶平顺性也更加出色，主要是因为初始阶段车辆与目标直线距离较大，前视距离Δ相应地缩短，自适应LOS发出更为“激进”的追踪指令。随着车辆接近目标直线，自适应LOS的前视距离相应地增加，让震荡有所缓解，追踪也更加地平稳。考虑到直线参考路径的曲率k=0，两种MPC控制器根据公式(42)取相同的预测步长N_P，因此传统LOS+传统MPC与传统LOS+自适应MPC在追踪直线过程中有着相同的轨迹，这两种循迹策略在之后四个评价指标中也有着重合的曲线。图 3(b)与图 3(c)分别为横向误差以及前视距离随时间变化的对比结果。虽然在车辆到达目标直线附近时，三种追踪策略的横向偏差会有稍许的波动，但是都可以稳定地收敛于0。其中，自适应LOS+传统MPC的横向误差比变预测步长的MPC控制器收敛得更加迅速，延迟相对较小，跟踪精度也更为理想。图 3(d)为前轮转角对比图, 能够看出自适应LOS的方向盘转向幅度较大，横向误差收敛变快，说明系统起步阶段对跟踪性能更加侧重。由图 3(d)可以看出，对于设计的自适应LOS，前期的航向角收敛速度快，达到稳态航向角后的震荡、抖动与自适应MPC相比得到了大幅度改善。图 3(f)为纵向速度变化曲线。三种循迹策略的纵向控制都是采用相同的PID控制器，因此，纵向速度收敛曲线重合。车辆由最开始的静止状态经过40 s的加速后平稳地收敛到期望值，有效地避免了超调以及静态误差，纵向车速跟踪效果较好；综上所述，本文所设计的横纵向控制器满足循迹的要求。

3.2 曲线路径的跟踪结果

1) 假设车辆在S型无障碍物赛道上行驶，该赛道由曲率半径分别为10 m和15 m的半圆以及其他直线构成。起始位置坐标为(-10, 25)，横向初始偏差为5 m，初始航向角为0 rad，循迹车由静止加速到目标车速30 km/h，随后保持纵向的匀速运动。仿真结果见图 4，从图 4(a)能够看出三种追踪系统都可以在有限的时间内出色地完成曲线路径追踪任务。尽管第二种循迹策略的预测时域N_P会随着赛道曲率k的变化自动调节，但自适应LOS在初始阶段的转弯更“激烈”，收敛更快，转弯期间以一个弧度相对更小的曲线沿着圆形赛道行驶，具有更加出色的动态追踪能力。图 4(b)为横向误差随时间变化的对比结果。随着转弯处曲率k的增加，第二种循迹策略中控制器的预测步长N_P调整变长，追踪精度有所上升，因此比传统LOS+传统MPC策略的横向误差指标表现得更为出色。图 4(c)与图 4(d)给出了三种策略的前视距离与转向角变化曲线。可以看出当车辆与参考路径误差较大时，自适应LOS对追踪的时效性有着更高的要求，因此误差收敛速度变快；当位置误差变小时，行驶平稳性占更高的权重，此时的前视距离随之增加，车辆的行驶也更加平稳。无论是传统LOS还是自适应LOS，前轮转角一直处于约束阈值内，自适应LOS要求的转向操纵指令更迅速，转向幅度更大。分析图 4(e)的航向角变化曲线可得：与传统LOS相比，采用自适应LOS制导算法的航向角前期响应迅速，这意味着更为优秀的动态追踪能力，随着车辆接近参考曲线，车辆的平稳性提高，有效避免了超调。图 4(f)中的实际车速与期望车速对比图可看出纵向速度收敛地迅速且平稳，跟踪效果好；再结合图 4(f)中的横向误差以及而图 4(e)中的航向角均控制在合理范围之内，说明本文的横纵综合控制器可在曲线工况下保证车辆完成追踪任务，再次验证了其可行性与可靠性。

4 结论

1) 围绕着如何提高自主循迹车辆路径跟踪的响应速度和平稳性，本文提出了基于可变前视距离视线(LOS)制导结合模型预测控制(MPC)的思想，为无人车提供充足的操纵和响应时间，保证汽车更加灵活地追踪路径。

2) 基于传统LOS制导算法, 将前视距离Δ当成一个随横向误差不断优化的时变量，提升追踪的灵活度与稳定性，并基于李雅普诺夫稳定性理论证明LOS制导算法满足横向跟踪误差y_c最后趋近于0。

3) 采用单车模型作为研究的对象，建立能够分析车辆稳定性与操纵性的动力学模型，并结合自适应LOS的状态方程建立系统追踪控制模型，为模型预测控制器的设计做好准备。

4) 基于模型预测控制(MPC)理论设计带约束的惩罚函数，预测系统未来时域内的状态变量，并通过滚动优化以及反馈校正求取方向盘的最优输入量，从而快速且平稳地跟踪参考轨迹。

5) 在Simulink仿真环境分别进行直线路径和曲线路径的追踪测试，验证了自适应LOS制导算法能够基于当前的横向误差及时调整前视距离，有效提高无人车路径跟踪的响应速度和平稳性。