基于优化预瞄距离的无人驾驶车辆轨迹跟踪控制

摘要

实现快速、准确的轨迹跟踪控制是自动驾驶技术发展的核心任务。为此，本文建立了车辆预瞄偏差动力学模型，基于模型预测控制算法构建了以前轮转角为控制量的轨迹跟踪控制器，以实现结合车辆状态和参考轨迹设计预瞄距离优化方法，提高跟踪精度。综合考虑车辆跟踪控制的准确性和行驶稳定性设计目标函数与约束条件，搭建Carsim⁃Simulink联合仿真平台，验证算法的有效性，在双移线工况下进行不同车速的仿真试验，探索预瞄距离对控制结果的影响并设计预瞄距离优化方法。结果表明：与无预瞄距离优化相比，本文设计的轨迹跟踪控制器能够实现高精度控制，在车辆中高速行驶时，横向位移平均误差小于10 cm，航向角平均误差小于1°，并使车辆稳定性显著提升。

关键词

自动驾驶; 轨迹跟踪; 模型预测控制; 预瞄距离

由于无人驾驶车辆技术在解决车辆主动安全性问题、提高交通效率、降低能耗等方面的巨大潜力，已经成为当前汽车领域的研发热点^［1］。轨迹跟踪控制作为无人驾驶系统的核心环节之一，其性能表现直接影响车辆行驶安全和用户体验。国内外学者在这个领域开展了大量的研究工作^［2］。

根据轨迹跟踪控制模型，可将控制算法分为几何学模型、车辆运动学模型及车辆动力学模型^［3］。基于几何学模型和运动学模型^［4-6］的控制算法在较低侧向加速度的情况下，具有较好的控制效果，但是未考虑车辆动力学特性与执行器动态响应特性。针对高速、曲率突变工况，几何学模型和运动学模型的失准以及控制算法的低鲁棒性导致控制结果出现较大偏差。基于动力学模型进行控制算法考虑了车辆动力学特性，有利于实现高精度的轨迹跟踪控制。MARINO等^［7］根据侧向位移误差得到期望横摆角速度，并通过 PI控制实现横摆角速度跟踪，该算法没有考虑执行器约束对控制效果的影响。Xu等^［8］提出了提出基于最优预瞄控制的LQR轨迹跟踪控制，把多点预瞄的道路曲率引入增广状态量，使转向控制更加平滑。GOODARZI等^［9］将道路曲率前馈引入LQR控制，提出了前轮主动转向和直接横摆力矩控制相结合的控制策略。Hwang等^［10］提出了分步递进的模糊动态滑模轨迹跟踪控制，该控制器由虚拟期望输入和轨迹跟踪控制两部分组成，并且还设计了一种改进的模糊动态滑模控制来处理系统例如有效载荷的不确定性，通过Lyapunov验证了闭环系统的稳定性。FALCONE等^［11］提出了基于模型预测控制算法的前轮主动转向和四轮独立制动相结合的集成控制方法，该控制策略实现了高速低附着路面的轨迹跟踪控制。NAM等^［12］考虑极限工况下对转向执行器的要求，在模型预测控制器中引入了执行器动态特性约束，在紧急转向工况下验证了算法的有效性。辛喆等^［13］结合二次型代价函数和安全约束构建了轨迹跟踪的开环最优预测控制问题，通过约束车辆的前后轮侧偏角，保持极限工况下智能汽车的行驶稳定性。但是上述方法并未考虑预瞄点的设置对于控制结果的影响，非预瞄式算法只关注车辆当前位置状态，对系统干扰敏感，因此不能满足如今复杂交通系统的需求。

对于预瞄式系统而言，预瞄距离的设置可以有效减小车辆在弯道处的超调，提高轨迹跟踪性能^［14］。现有的研究中预瞄距离通常设置为与纵向车速成线性相关^［15］；或将预瞄距离根据纵向车速设置为三段式的函数^［15］。以上方式均只考虑了预瞄距离与车速之间的简单函数关系。李爽等^［16］设计了一种根据车辆实际行驶路程获取预瞄点横向位移的弧长预瞄方法，提高了轨迹跟踪控制算法的可调节性。还有学者把模糊控制引入预瞄距离优化中，YUAN等^［17］提出了一种基于模糊控制的预瞄距离自适应方法，建立纵向车速、侧向车速与预瞄距离的隶属度函数实现自适应控制；CAO等^［18］建立了道路曲率、车辆侧向加速度和预瞄距离的隶属度函数实现了基于模糊控制的预瞄距离自适应控制。但是模糊规则的建立大多取决于经验或试凑，具有一定程度的不可解释性。并且这些方法没有充分考虑纵向车速和期望轨迹道路曲率对于预瞄距离的影响。

为了实现高精度的跟踪控制，本文在分析不同预瞄距离对轨迹跟踪控制器性能的影响的基础上设计预瞄距离优化方法，建立预瞄偏差动力学模型作为预测模型进行模型预测控制的轨迹跟踪控制方法的开发，最后基于联合仿真平台进行控制算法的仿真验证。

1 预测模型

轨迹跟踪控制系统控制目标为缩小横向位移误差及航向角误差，因此预测模型需正确反映车辆侧向运动及横摆运动特性。模型复杂程度较大，难以保证控制算法的实时性，如果模型过于简单，导致控制效果变差。综合考虑模型精度与计算效率，结合车辆与参考轨迹的运动学关系与车辆动力学特性，建立如图1所示的预瞄偏差动力学模型。

图1  预瞄偏差动力学模型

Fig.1  Dynamic model of preview deviation

图1中：（X，Y）为大地固定坐标系；（x，y）为车辆坐标系；（X_cg，Y_cg）为大地坐标系下车辆质心的坐标；φ为车辆横摆角，即车辆坐标系x轴与大地坐标系X轴之间的夹角；γ为车辆横摆角速度；l_f和l_r分别为车辆质心到前轴、后轴的距离；F_yf和F_yr分别为前后车轮侧偏力；δ_f为前轮转角；α_f和α_r分别为前后车轮的侧偏角；v_x和v_y分别为车辆纵向速度和侧向速度。车辆单轨动力学方程可表示为：

$$m{a}_y=F_{y\mathrm{f}}\mathrm{c}\mathrm{o}\mathrm{s}{\delta }_{\mathrm{f}}+F_{y\mathrm{r}}$$

（1）

$$I_z\stackrel{•}{\gamma }=l_{\mathrm{f}}{F}_{y\mathrm{f}}\mathrm{c}\mathrm{o}\mathrm{s}{\delta }_{\mathrm{f}}-l_{\mathrm{r}}{F}_{y\mathrm{r}}$$

（2）

式中：m为车辆的质量，a_y为车辆侧向加速度，$I_z$为车辆绕质心处z轴的转动惯量。根据模型假设，轮胎侧向力与轮胎侧偏角成线性关系，因此前后车轮的侧偏力可以表示为：

$$F_{y\mathrm{f}}=-\mathrm{2}{K}_{\mathrm{f}}{\alpha }_{\mathrm{f}}$$

（3）

$$F_{y\mathrm{r}}=-\mathrm{2}{K}_{\mathrm{r}}{\alpha }_{\mathrm{r}}$$

（4）

式中K_f和K_r分别为前后车轮的等效侧偏刚度。

根据几何关系，前后车轮的侧偏角可以表示为：

$${\alpha }_{\mathrm{f}}=\beta +\frac{l_{\mathrm{f}}\gamma }{v_x}-{\delta }_{\mathrm{f}}$$

（5）

$${\alpha }_{\mathrm{r}}=\beta -\frac{l_{\mathrm{r}}\gamma }{v_{\mathrm{x}}}$$

（6）

车辆侧向加速度可以表示为：

$$a_y={\dot{v}}_y+v_x\gamma$$

（7）

质心侧偏角β为车辆横向速度与纵向速度之比：

$$\beta =\frac{v_y}{v_x}$$

（8）

联立以上公式可得车辆操纵动力学模型表达式，并将其表示为以下状态空间表达式：

$$\left\{\begin{array}{c}\dot{\beta }=-\frac{\mathrm{2}(K_{\mathrm{f}}+K_{\mathrm{r}})}{m{v}_x}\beta +(-\frac{\mathrm{2}(K_{\mathrm{f}}{l}_{\mathrm{f}}+K_{\mathrm{r}}{l}_{\mathrm{r}})}{m{v}_x^{\mathrm{2}}}-\mathrm{1})\gamma +\frac{\mathrm{2}{K}_{\mathrm{f}}}{m{v}_x}{\delta }_{\mathrm{f}}\\ \dot{\gamma }=-\frac{\mathrm{2}(K_{\mathrm{f}}{l}_{\mathrm{f}}-K_{\mathrm{r}}{l}_{\mathrm{r}})}{I_z}\beta -\frac{\mathrm{2}(K_{\mathrm{f}}{l}_{\mathrm{f}}^{\mathrm{2}}+K_{\mathrm{r}}{l}_{\mathrm{r}}^{\mathrm{2}})}{I_z{v}_x}\gamma +\frac{\mathrm{2}{K}_{\mathrm{f}}{l}_{\mathrm{f}}}{I_z}{\delta }_{\mathrm{f}}\end{array}\right.$$

（9）

$$\left[\begin{array}{c}\dot{\beta }\\ \dot{\gamma }\end{array}\right]=\left[\begin{array}{cc}-\frac{\mathrm{2}(K_{\mathrm{f}}+K_{\mathrm{r}})}{m{v}_x}& -\frac{\mathrm{2}(K_{\mathrm{f}}{l}_{\mathrm{f}}+K_{\mathrm{r}}{l}_{\mathrm{r}})}{m{v}_x^{\mathrm{2}}}-\mathrm{1}\\ -\frac{\mathrm{2}(K_{\mathrm{f}}{l}_{\mathrm{f}}-K_{\mathrm{r}}{l}_{\mathrm{r}})}{I_z}& -\frac{\mathrm{2}(K_{\mathrm{f}}{l}_{\mathrm{f}}^{\mathrm{2}}+K_{\mathrm{r}}{l}_{\mathrm{r}}^{\mathrm{2}})}{I_z{v}_x}\end{array}\right]\left[\begin{array}{c}\beta \\ \gamma \end{array}\right]+\left[\begin{array}{c}\frac{\mathrm{2}{K}_{\mathrm{f}}}{m{v}_x}\\ \frac{\mathrm{2}{K}_{\mathrm{f}}{l}_{\mathrm{f}}}{I_z}\end{array}\right]{\delta }_{\mathrm{f}}$$

（10）

考虑车辆与参考轨迹之间的运动学关系，建立车辆与参考轨迹之间的运动学模型，如图2所示。图中：P为期望轨迹上的预瞄点；y_e为预瞄位置的侧向位移偏差；y_e0为车辆质心处相对于期望轨迹的偏差；L_p为预瞄距离。

图2  运动学模型

Fig.2  Kinematics model

预瞄点处的车辆航向角偏差${\phi }_{\mathrm{e}}$可表示为：

$${\phi }_{\mathrm{e}}=\phi -{\phi }_{\mathrm{d}}$$

（11）

式中：φ为车辆航向角，即大地坐标系横轴与车辆纵轴之间的夹角；${\phi }_{\mathrm{d}}$为预瞄点P处的期望航向角，即期望轨迹在预瞄点P处的切线与大地坐标系横轴之间的夹角。对式（11）求导得：

$${\dot{\phi }}_{\mathrm{e}}=\gamma -\kappa v_x$$

（12）

式中κ为目标路径在预瞄点P处的道路曲率。车辆质心与期望轨迹之间的侧向位移偏差相对于预瞄点的变化率为：

$${\dot{y}}_{\mathrm{e}\mathrm{0}}=v_y+v_x\mathrm{t}\mathrm{a}\mathrm{n}{\phi }_{\mathrm{e}}$$

（13）

在轨迹跟踪过程中，航向角偏差较小，因此可以将式（13）化简为：

$${\dot{y}}_{\mathrm{e}\mathrm{0}}=v_y+v_x{\phi }_{\mathrm{e}}=v_x\beta +v_x{\phi }_{\mathrm{e}}$$

（14）

根据运动学模型几何关系可以得到：

$$\mathrm{t}\mathrm{a}\mathrm{n}{\phi }_{\mathrm{e}}=\frac{y_{\mathrm{e}}-y_{\mathrm{e}\mathrm{0}}}{L_p}$$

（15）

$$y_e=y_{eo}+L_p{\phi }_e$$

（16）

对式（1.16）求导并代入式（1.14），可得：

$${\dot{y}}_e=v_x\beta +v_x{\phi }_e+L_p\gamma$$

（17）

综合式（17）和式（12），可得到车辆运动学轨迹跟踪模型：

$$\left\{\begin{array}{l}{\dot{y}}_{\mathrm{e}}=v_x\beta +v_x{\phi }_{\mathrm{e}}+L_p\gamma \\ {\dot{\phi }}_{\mathrm{e}}=\gamma -\kappa v_x\end{array}\right.$$

（18）

结合式（18）和式（9），可建立联合运动学-动力学的预瞄偏差预测模型：

$$\left\{\begin{array}{l}\stackrel{•}{\mathit{X}}=f(\mathit{X},\mathit{u},{\mathit{w}}_s)=\mathit{A}\mathit{X}+\mathit{B}\mathit{u}+\mathit{W}{\mathit{w}}_s\\ \mathit{Y}=\mathit{C}\cdot \mathit{X}\end{array}\right.$$

（19）

式中：状态向量$\mathit{X}={\left[\begin{array}{cccc}{y}_{\mathrm{e}}& {\phi }_{\mathrm{e}}& \beta & \gamma \end{array}\right]}^{\mathrm{T}}$；控制向量为前轮转角$\mathit{u}={\delta }_f$；目标路径的道路曲率${\mathit{w}}_s=\kappa$设为扰动；输出向量$\mathit{Y}={\left[\begin{array}{cc}{y}_{\mathrm{e}}& {\phi }_{\mathrm{e}}\end{array}\right]}^{\mathrm{T}}$；$\mathit{A}=\left[\begin{array}{cccc}\mathrm{0}& v_x& v_x& L_p\\ \mathrm{0}& \mathrm{0}& \mathrm{0}& \mathrm{1}\\ \mathrm{0}& \mathrm{0}& a_{\mathrm{11}}& a_{\mathrm{12}}\\ \mathrm{0}& \mathrm{0}& a_{\mathrm{21}}& a_{\mathrm{22}}\end{array}\right]$，$\mathit{B}=\left[\begin{array}{c}\mathrm{0}\\ \mathrm{0}\\ b_{\mathrm{1}}\\ b_{\mathrm{2}}\end{array}\right]$，$\mathit{W}=\left[\begin{array}{c}\mathrm{0}\\ -v_x\\ \mathrm{0}\\ \mathrm{0}\end{array}\right]$，$\mathit{C}=\left[\begin{array}{c}\begin{array}{cccc}\mathrm{1}& \mathrm{0}& \mathrm{0}& \mathrm{0}\end{array}\\ \begin{array}{cccc}\mathrm{0}& \mathrm{1}& \mathrm{0}& \mathrm{0}\end{array}\end{array}\right]$。

2 轨迹跟踪控制

考虑控制算法实时性，采用泰勒级数在参考点处展开并忽略高阶项，对式（19）预测模型进行线性化处理，设参考点$(X_{\mathrm{r}},u_{\mathrm{r}},w_{s\mathrm{r}})$：

$$\dot{X}\approx f(X_{\mathrm{r}},u_{\mathrm{r}},w_{s\mathrm{r}})+\frac{\partial f}{\partial X}(X-X_{\mathrm{r}})+\frac{\partial f}{\partial u}(u-u_{\mathrm{r}})+\frac{\partial f}{\partial w_s}(w_s-w_{s\mathrm{r}})$$

（20）

$$\left\{\begin{array}{l}\dot{\tilde{X}}(t)=\dot{X}(t)-{\dot{X}}_{\mathrm{r}}(t)=A(t)\tilde{X}(t)+B(t)\tilde{u}(t)+W(t){\tilde{w}}_s(t)\\ \dot{\tilde{Y}}=C\dot{\tilde{X}}(t)\end{array}\right.$$

（21）

式中：$\tilde{X}(t)=X(t)-X_{\mathrm{r}}(t)$；$\tilde{u}(t)=u(t)-u_{\mathrm{r}}(t)$；${\tilde{w}}_s(t)=w_s(t)-w_{s\mathrm{r}}(t)$。作为实际应用，需要将微分方程（21）进行离散化处理。

设采样周期为$T_s$，利用前向欧拉法对连续模型进行如下离散化：

$$\left\{\begin{array}{l}\tilde{X}(k+\mathrm{1})=A_k\tilde{X}(k)+B_k\tilde{u}(k)+W_k{\tilde{w}}_s(k)\\ \tilde{Y}(k)=C\cdot \tilde{X}(k)\end{array}\right.$$

（22）

式中：$A_k=I+T_{s}A(t)$；$B_k=T_{s}B(t)$；$W_k=T_{s}W(t)$。

由此可以得到系统的控制量模型，即通过控制前轮转角来控制系统运行，达到轨迹跟踪的目的。但是由于直接优化求解得到控制量，会出现控制量突变的情况，即前轮转角突变的情况。实际前轮转角的变化是缓变的，因此，需要将系统控制量模型转变成系统的控制增量模型，即通过优化求解得到车辆前轮转角增量来改变车辆前轮转角值。相比于控制量模型，控制增量模型不仅可使得系统控制量为缓变状态，还可同时对控制量和控制增量进行约束。转变形式如下，将原方程中的状态量、控制量和扰动量合并为新的状态变量并得到新的系统状态矩阵：

$$\mathit{\xi }(k)=\left[\begin{array}{c}\tilde{X}(k)\\ \tilde{u}(k-\mathrm{1})\\ {\tilde{w}}_s(k-\mathrm{1})\end{array}\right]$$

（23）

$$\left\{\begin{array}{l}\mathit{\xi }(k+\mathrm{1})={\tilde{A}}_k\xi (k)+{\tilde{B}}_k\mathrm{\Delta }u(k)\\ \mathit{\eta }(k)=\tilde{C}\xi (k)\end{array}\right.$$

（24）

式中：

${\tilde{\mathit{A}}}_k=\left[\begin{array}{ccc}{A}_k& B_k& W_k\\ O_{\mathrm{1}\times \mathrm{4}}& I& O\\ O_{\mathrm{1}\times \mathrm{4}}& O& I\end{array}\right]$；${\tilde{\mathit{B}}}_k=\left[\begin{array}{c}{B}_k\\ I\\ O\end{array}\right]$；$\tilde{\mathit{C}}=\left[\begin{array}{ccc}C& O_{\mathrm{2}\times \mathrm{1}}& O_{\mathrm{2}\times \mathrm{1}}\end{array}\right]$；$\mathrm{\Delta }\mathit{u}(k)=\left[u(k)-u(k-\mathrm{1})\right]$。

模型预测控制中由于预测时域较短，可假设车辆在预测时域内的状态是不变的：

$$\begin{array}{cc}{\tilde{A}}_i={\tilde{A}}_k,& i=k,k+\mathrm{1},\cdots ,k+N_p-\mathrm{1}\end{array}$$

（25）

$$\begin{array}{cc}{\tilde{B}}_i={\tilde{B}}_k,& i=k,k+\mathrm{1},\cdots ,k+N_p-\mathrm{1}\end{array}$$

（26）

$$\begin{array}{cc}{\tilde{C}}_i=\tilde{C},& i=k,k+\mathrm{1},\cdots ,k+N_p-\mathrm{1}\end{array}$$

（27）

通过迭代对预测时域内的状态量进行预测，可以得到预测时域内的$\xi$的变化：

$$\begin{array}{ll}\xi (k+\mathrm{1})& ={\tilde{A}}_k\xi (k)+{\tilde{B}}_k\mathrm{\Delta }u(k)\\ \xi (k+\mathrm{2})& ={\tilde{A}}_k\xi (k+\mathrm{1})+{\tilde{B}}_k\mathrm{\Delta }u(k+\mathrm{1})\\ & \begin{array}{l}={{\tilde{A}}_k}^{\mathrm{2}}\xi (k)+{\tilde{A}}_k{\tilde{B}}_k\mathrm{\Delta }u(k)+{\tilde{B}}_k\mathrm{\Delta }u(k+\mathrm{1})\\ ⋮\end{array}\\ \xi (k+N_c)& ={\tilde{A}}_k\xi (k+N_c-\mathrm{1})+{\tilde{B}}_k\mathrm{\Delta }u(k+N_c-\mathrm{1})\\ & \begin{array}{l}={{\tilde{A}}_k}^{N_c}\xi (k)+{{\tilde{A}}_k}^{N_c-\mathrm{1}}{\tilde{B}}_k\mathrm{\Delta }u(k)+{{\tilde{A}}_k}^{N_c-\mathrm{2}}{\tilde{B}}_k\mathrm{\Delta }u(k+\mathrm{1})+\cdots \\ +{\tilde{B}}_k\mathrm{\Delta }u(k+N_c-\mathrm{1})\end{array}\\ & ⋮\\ \begin{array}{l}\xi (k+N_p)\\ \\ \end{array}& \begin{array}{l}={\tilde{A}}_k\xi (k+N_p-\mathrm{1})+{\tilde{B}}_k\mathrm{\Delta }u(k+N_p-\mathrm{1})\\ ={{\tilde{A}}_k}^{N_p}\xi (k)+{{\tilde{A}}_k}^{N_p-\mathrm{1}}{\tilde{B}}_k\mathrm{\Delta }u(k)+{{\tilde{A}}_k}^{N_p-\mathrm{2}}{\tilde{B}}_k\mathrm{\Delta }u(k+\mathrm{1})+\cdots \\ +{{\tilde{A}}_k}^{N_p-N_c}{\tilde{B}}_k\mathrm{\Delta }u(k+N_c-\mathrm{1})\end{array}\end{array}$$

（28）

式中：$N_p$为预测时域；$N_c$为控制时域，且$N_p\ge N_c$。

同样地，可以得到预测时域内的控制输出$\eta$的变化：

$$\begin{array}{ll}\eta (k+\mathrm{1})& =\tilde{C}\xi (k+\mathrm{1})\\ & =\tilde{C}{\tilde{A}}_k\xi (k)+\tilde{C}{\tilde{B}}_k\mathrm{\Delta }u(k)\\ \eta (k+\mathrm{2})& =\tilde{C}\xi (k+\mathrm{2})\\ & =\tilde{C}{{\tilde{A}}_k}^{\mathrm{2}}\xi (k)+\tilde{C}{\tilde{A}}_k{\tilde{B}}_k\mathrm{\Delta }u(k)+\tilde{C}{\tilde{B}}_k\mathrm{\Delta }u(k+\mathrm{1})\\ & ⋮\\ \eta (k+N_c)& =\tilde{C}\xi (k+N_c)\\ & \begin{array}{l}=\tilde{C}{{\tilde{A}}_k}^{N_c}\xi (k)+\tilde{C}{{\tilde{A}}_k}^{N_c-\mathrm{1}}{\tilde{B}}_k\mathrm{\Delta }u(k)+\tilde{C}{{\tilde{A}}_k}^{N_c-\mathrm{2}}{\tilde{B}}_k\mathrm{\Delta }u(k+\mathrm{1})+\cdots \\ +\tilde{C}{\tilde{B}}_k\mathrm{\Delta }u(k+N_c-\mathrm{1})\end{array}\\ & ⋮\\ \eta (k+N_p)& =\tilde{C}\xi (k+N_p)\\ \begin{array}{l}\\ \end{array}& \begin{array}{l}=\tilde{C}{{\tilde{A}}_k}^{N_p}\xi (k)+\tilde{C}{{\tilde{A}}_k}^{N_p-\mathrm{1}}{\tilde{B}}_k\mathrm{\Delta }u(k)+\tilde{C}{{\tilde{A}}_k}^{N_p-\mathrm{2}}{\tilde{B}}_k\mathrm{\Delta }u(k+\mathrm{1})+\cdots \\ +\tilde{C}{{\tilde{A}}_k}^{N_p-N_c}{\tilde{B}}_k\mathrm{\Delta }u(k+N_c-\mathrm{1})\end{array}\end{array}$$

（29）

将预测时域内的控制输出转化成矩阵的形式：

$$\mathit{Y}(k)=\mathit{\psi }\tilde{\xi }(k)+\mathit{\Theta }\mathrm{\Delta }\mathit{U}(k)$$

（30）

式中：

$\mathit{Y}(k)=\left[\begin{array}{c}\eta (k+\mathrm{1})\\ \eta (k+\mathrm{2})\\ ⋮\\ \eta (k+N_p)\end{array}\right]$； $\mathit{\psi }=\left[\begin{array}{c}\tilde{C}{\tilde{A}}_k\\ \tilde{C}{{\tilde{A}}_k}^{\mathrm{2}}\\ ⋮\\ \tilde{C}{{\tilde{A}}_k}^{N_p}\end{array}\right]$； $\mathrm{\Delta }\mathit{U}(k)=\left[\begin{array}{c}\mathrm{\Delta }u(k)\\ \mathrm{\Delta }u(k+\mathrm{1})\\ ⋮\\ \mathrm{\Delta }u(k+N_c-\mathrm{1})\end{array}\right]$；

$$\mathit{\Theta }=\left[\begin{array}{cccc}\tilde{C}{\tilde{B}}_k& \mathrm{0}& \cdots & \mathrm{0}\\ \tilde{C}{\tilde{A}}_k{\tilde{B}}_k& \tilde{C}{\tilde{B}}_k& \cdots & \mathrm{0}\\ \tilde{C}{{\tilde{A}}_k}^{\mathrm{2}}{\tilde{B}}_k& \tilde{C}{\tilde{A}}_k{\tilde{B}}_k& \cdots & \mathrm{0}\\ ⋮& ⋮& ⋮& ⋮\\ \tilde{C}{{\tilde{A}}_k}^{N_c}{\tilde{B}}_k& \tilde{C}{{\tilde{A}}_k}^{N_c-\mathrm{1}}{\tilde{B}}_k& \cdots & \tilde{C}{\tilde{A}}_k{\tilde{B}}_k\\ ⋮& ⋮& ⋮& ⋮\\ \tilde{C}{{\tilde{A}}_k}^{N_p-\mathrm{1}}{\tilde{B}}_k& \tilde{C}{{\tilde{A}}_k}^{N_p-\mathrm{2}}{\tilde{B}}_k& \cdots & \tilde{C}{{\tilde{A}}_k}^{N_p-N_c}{\tilde{B}}_k\end{array}\right]$$。

以提高轨迹跟踪精度为目标建立优化目标函数，当目标函数取值最小时即系统的控制输出$\mathit{Y}(k)$与期望路径之间的横向位移误差和航向角误差达到最小值，实现对于参考路径的准确跟踪，同时还要确保车辆在跟踪过程中的横向稳定性。因此设计优化目标函数为：

$$\begin{array}{l}J={{\displaystyle \sum _{i=\mathrm{1}}^{N_p}}‖\eta (k+i)-{\eta }_{\mathrm{r}}(k+i)‖}_{\mathit{Q}}^{\mathrm{2}}+\\ {{\displaystyle \sum _{j=\mathrm{1}}^{N_c-\mathrm{1}}}‖\mathrm{\Delta }u(j)‖}_{\mathit{R}}^{\mathrm{2}}+\rho {\epsilon }^{\mathrm{2}}\end{array}$$

（31）

式中：${\eta }_{\mathrm{r}}(k+i)$为预测模型输出的参考值，由于预测模型的控制输出为系统的横向位置误差和航向角误差，因此$\begin{array}{cc}{\eta }_{\mathrm{r}}(k+i)=\mathrm{0},& i=\mathrm{0,1},\cdots ,N_p\end{array}$；Q和R分别为预测输出和控制增量的权重矩阵；由于系统模型参数是时变的，为了确保在每一个采样周期都能求解出最优的控制增量，在优化目标函数中引入了松弛因子$\epsilon$；$\rho$是其权重系数。

为保证车辆横摆运动平缓，设置前轮转角约束及转角增量约束：

$$u_{\mathrm{m}\mathrm{i}\mathrm{n}}\le u(k+i)\le u_{\mathrm{m}\mathrm{a}\mathrm{x}},i=\mathrm{0,1},\cdots ,N_c-\mathrm{1}$$

（32）

$$\mathrm{\Delta }{u}_{\mathrm{m}\mathrm{i}\mathrm{n}}\le \mathrm{\Delta }u(k+i)\le \mathrm{\Delta }{u}_{\mathrm{m}\mathrm{a}\mathrm{x}},i=\mathrm{0,1},\cdots ,N_c-\mathrm{1}$$

（33）

式中：$u_{\mathrm{m}\mathrm{i}\mathrm{n}}$和$u_{\mathrm{m}\mathrm{a}\mathrm{x}}$分别为前轮转角的最大值和最小值；$\mathrm{\Delta }{u}_{\mathrm{m}\mathrm{i}\mathrm{n}}$和$\mathrm{\Delta }{u}_{\mathrm{m}\mathrm{a}\mathrm{x}}$分别为前轮转角增量的最大值和最小值。

模型预测控制算法中，针对目标函数优化求解得到控制时域内系统最优的控制增量序列，因此约束条件只能以控制量或者控制增量与转换矩阵相乘的形式出现。因此，要想同时约束控制量和控制增量，需要对式（33）进行转换，获得相对应的转换矩阵。由于控制量和控制增量之间原本存在如下关系：

$$u(t+k)=u(t+k-\mathrm{1})+\mathrm{\Delta }u(t+k)$$

（34）

假设：  

$$\mathit{U}={\mathrm{1}}_{N_c}\otimes \mathit{u}(k-\mathrm{1})$$

（35）

$$\mathit{A}=\underset{N_c\times N_c}{\underbrace{\left[\begin{array}{ccccc}\mathrm{1}& \mathrm{0}& \cdots & \cdots & \mathrm{0}\\ \mathrm{1}& \mathrm{1}& \mathrm{0}& \cdots & \mathrm{0}\\ \mathrm{1}& \mathrm{1}& \mathrm{1}& \ddots & \mathrm{0}\\ ⋮& ⋮& \ddots & \ddots & \mathrm{0}\\ \mathrm{1}& \mathrm{1}& \cdots & \mathrm{1}& \mathrm{1}\end{array}\right]}}\otimes {\mathit{I}}_m$$

（36）

式中：${\mathrm{1}}_{N_c}$表示行数有$N_c$行的列向量；${\mathit{I}}_m$为维度为$m$的单位矩阵；$\otimes$表示克罗内克积（Kronecker product）算法；$\mathit{u}(k-\mathrm{1})$表示系统在$k-\mathrm{1}$时刻的实际控制量。

结合式（34）—式（36），将控制量约束条件式（32）转化为：

$${\mathit{U}}_{\mathrm{m}\mathrm{i}\mathrm{n}}\le \mathit{A}\cdot \mathrm{\Delta }\mathit{U}+\mathit{U}\le {\mathit{U}}_{\mathrm{m}\mathrm{a}\mathrm{x}}$$

（37）

式中：${\mathit{U}}_{\mathrm{m}\mathrm{i}\mathrm{n}}$和${\mathit{U}}_{\mathrm{m}\mathrm{a}\mathrm{x}}$分别表示系统在控制时域内求解的控制量最小极限值集合与控制量最大极限值集合。因此，可以实现同时约束控制量约束与控制增量约束。

$$\left\{\begin{array}{l}{\mathit{U}}_{\mathrm{m}\mathrm{i}\mathrm{n}}\le \mathit{A}\cdot \mathrm{\Delta }\mathit{U}+\mathit{U}\le {\mathit{U}}_{\mathrm{m}\mathrm{a}\mathrm{x}}\\ \mathrm{\Delta }{\mathit{U}}_{\mathrm{m}\mathrm{i}\mathrm{n}}\le \mathrm{\Delta }\mathit{U}\le \mathrm{\Delta }{\mathit{U}}_{\mathrm{m}\mathrm{a}\mathrm{x}}\end{array}\right.$$

（38）

式中：$\mathrm{\Delta }{\mathit{U}}_{\mathrm{m}\mathrm{i}\mathrm{n}}$和$\mathrm{\Delta }{\mathit{U}}_{\mathrm{m}\mathrm{a}\mathrm{x}}$分别表示系统在控制时域内求解的控制量增量最小极限值集合与控制量增量最大极限值集合。

将上述带有控制量和控制增量的优化问题转化为二次规划问题，采用内点法对该问题进行求解，得到控制时域内的最优控制序列：

$$\mathrm{\Delta }{\mathit{U}}^{\mathrm{*}}={\left[\mathrm{\Delta }{u}^{\mathrm{*}}(k+\mathrm{1}),\mathrm{\Delta }{u}^{\mathrm{*}}(k+\mathrm{2}),\cdots ,\mathrm{\Delta }{u}^{\mathrm{*}}(k+N_c-\mathrm{1})\right]}^{\mathrm{T}}$$

（39）

将最优的控制增量序列的第一项作为控制量的实际控制增量作用于系统：

$$u(k)=u(k-\mathrm{1})+\mathrm{\Delta }{u}^{\mathrm{*}}(k)$$

（40）

在新的采样时刻k+1，根据系统反馈的新状态变量重复上述过程，得到新的最优控制序列并将控制增量作用于系统，实现了模型预测控制的反馈校正。

3 预瞄距离优化

当车辆速度保持固定值时，探究预瞄距离对于轨迹跟踪控制的影响。设置车辆速度为36 km/h，路面附着系数为0.8，参考轨迹为双移线，预瞄距离分别设置为0 m、1 m和3 m。仿真结果如图3所示。

(a)  行驶轨迹

（b）  纵向速度

（c）  侧向位移误差

（d）  航向角误差

（e）  横摆角速度

（f）  质心侧偏角

图3  定速下不同预瞄距离轨迹跟踪仿真实验

Fig.3  Simulation experiment on trajectory tracking with different preview distances at fixed speed

从图3（a）车辆行驶轨迹的仿真结果中可以发现，当车速维持在10 m/s时，不同的预瞄距离均能完成轨迹跟踪控制任务。从图3（c）、3（d）以及表1中可以发现，随着预瞄距离的提升，车辆在弯道处的超调量在减小，侧向位移跟踪平均误差呈现先增大后减小的趋势。航向角误差平均值减小接近50%。通过图3（e）、3（f）车辆的横摆角速度及质心侧偏角的仿真结果可以看出，随着预瞄距离的增大，车辆的横摆运动变得更加稳定，减少了“画龙”现象，有助于提高车辆的操纵稳定性。这是由于当预瞄距离增大时，MPC模型预测控制器的预测范围扩大，对于前方道路的变化能够及时做出调整，使得控制量的变化变得较为平缓，系统稳定性增加。因此预瞄距离的设计应该兼顾车辆跟踪性能及车辆的操纵稳定性。

表1  定速下不同预瞄距离轨迹跟踪仿真实验

Tab.1  Simulation experiment on trajectory tracking with different preview distances at fixed speed

预瞄距离/m	侧向位移误差最大值/m	侧向位移误差平均值/m	航向角误差最大值/（°）	航向角误差平均值/（°）
0	0.092	0.001 0	3.23	0.011 1
1	0.102	0.006 4	2.74	0.009 1
3	0.093	0.003 1	4.72	0.005 7

车辆在不同速度下应选取不同的预瞄距离，选取预瞄距离为3 m，设置车辆初始速度为10 m/s，目标速度分别为5 m/s、10 m/s、15 m/s。进行3组仿真对比试验，路面附着系数为0.8，参考轨迹为双移线。仿真结果如图4所示。从仿真结果可以看到，相同预瞄距离下，车辆以不同速度均能完成轨迹跟踪任务，从侧面验证了所提出的轨迹跟踪控制器的性能。由于车辆速度不同，导致轨迹跟踪完成时间不一致。由图4（c）、4（d）及表2中可以看出，随着车速的增加，车辆在弯道处的最大超调量明显减小，相比较车速在5 m/s时，车速维持在15 m/s时，最大超调量减小近50%，航向角误差几乎保持不变，减小了0.03°；速度较大时，预瞄距离的提升有助于实现更高精度的轨迹跟踪。同时，图4（e）、4（f）的仿真结果表明，此时车辆也保持一定的操纵稳定性。综上，改变车辆的预瞄距离能够影响车辆的轨迹跟踪精度，合适的预瞄距离能够有效改善车辆在弯道处的超调量，减少抖震。仿真试验发现，当车辆速度较小时，预瞄距离应选择较小数值，当车速较大时，预瞄距离应该增加，以此为参考，对预瞄距离进行优化。

(a)  行驶轨迹

（b）  纵向速度

（c）  侧向位移误差

（d）  航向角误差

（e）  横摆角速度

（f）  质心侧偏角

图4  定预瞄距离下不同速度轨迹跟踪仿真实验

Fig.4  Simulation experiment on trajectory tracking at different speeds under fixed preview distance

表2  定预瞄距离下不同速度轨迹跟踪仿真实验

Tab.2  Simulation experiment on trajectory tracking at different speeds under fixed preview distance

纵向速度	侧向位移误差最大值	侧向位移误差平均值	航向角误差最大值	航向角误差平均值
5m/s	0.115m	0.0002m	5.06°	0.0051°
10m/s	0.093m	0.0031m	4.72°	0.0057°
15m/s	0.051m	0.0017m	5.03°	0.0190°

通过进行不同车速时不同预瞄距离的仿真试验发现，较长的预瞄距离能够更好发挥预瞄机制的优势，提高车辆的行驶稳定性；而较短的预瞄距离能够在轨迹复杂多变、道路曲率较大时更好地跟随期望路径，提高轨迹跟踪的跟踪精度。为了同时保证车辆的轨迹跟踪精度和行驶稳定性，在车速较低、道路曲率较大时，采用较短的预瞄距离；而在车速较高、道路曲率较小时，采用较长的预瞄距离，本文将基于规则设计预瞄距离优化方法实现上述预瞄距离的控制。

首先，根据车速计算直线行驶时的预瞄距离，将车辆直线行驶时预瞄距离处理为与纵向车速线性相关，考虑到预瞄系统采用视觉传感器的物理极限，对预瞄距离做饱和化处理，直线行驶时预瞄距离的表达式为：

$$L_v=\left\{\begin{array}{ll}{L}_{v\mathrm{m}\mathrm{i}\mathrm{n}}& v_x<v_{x\mathrm{m}\mathrm{i}\mathrm{n}}\\ L_{v\mathrm{m}\mathrm{i}\mathrm{n}}+\frac{L_{v\mathrm{m}\mathrm{a}\mathrm{x}}-L_{v\mathrm{m}\mathrm{i}\mathrm{n}}}{v_{x\mathrm{m}\mathrm{a}\mathrm{x}}-v_{x\mathrm{m}\mathrm{i}\mathrm{n}}}(v_x-v_{x\mathrm{m}\mathrm{i}\mathrm{n}})& v_{x\mathrm{m}\mathrm{i}\mathrm{n}}\le v_x\le v_{x\mathrm{m}\mathrm{a}\mathrm{x}}\\ L_{v\mathrm{m}\mathrm{a}\mathrm{x}}& v_x>v_{x\mathrm{m}\mathrm{a}\mathrm{x}}\end{array}\right.$$

（41）

式中：纵向车速$v_{x\mathrm{m}\mathrm{i}\mathrm{n}}=\mathrm{10}\mathrm{m}/\mathrm{s}$，$v_{x\mathrm{m}\mathrm{a}\mathrm{x}}=\mathrm{20}\mathrm{m}/\mathrm{s}$；直线行驶时的预瞄距离限值$L_{v\mathrm{m}\mathrm{i}\mathrm{n}}=\mathrm{3}\mathrm{m}$，$L_{v\mathrm{m}\mathrm{a}\mathrm{x}}=\mathrm{10}\mathrm{m}$。

其次，计算弯道行驶时的预瞄距离，道路曲率的计算公式为：

$$\kappa =\left|y\mathrm{\text{'}}\mathrm{\text{'}}\right|/(\mathrm{1}+y{\mathrm{\text{'}}}^{\mathrm{2}}{)}^{\frac{\mathrm{3}}{\mathrm{2}}}$$

（42）

先不考虑车速对车辆轨迹跟踪的影响，仅考虑道路曲率的大小对预瞄距离的影响。设置道路曲率预瞄距离系数，该系数在最大曲率限制内与预瞄点处的道路曲率线性相关，表达式为：

$$k_{\kappa }=\left\{\begin{array}{cc}\mathrm{1}-\frac{\mathrm{1}-k_{\kappa \mathrm{m}\mathrm{i}\mathrm{n}}}{{\kappa }_{\mathrm{m}\mathrm{a}\mathrm{x}}}|\kappa |& |\kappa |\le {\kappa }_{\mathrm{m}\mathrm{a}\mathrm{x}}\\ k_{\kappa \mathrm{m}\mathrm{i}\mathrm{n}}& |\kappa |>{\kappa }_{\mathrm{m}\mathrm{a}\mathrm{x}}\end{array}\right.$$

（43）

式中： ${\kappa }_{\mathrm{m}\mathrm{a}\mathrm{x}}$道路曲率的最大值； $k_{\kappa \mathrm{m}\mathrm{i}\mathrm{n}}\mathrm{、}{k}_{\kappa \mathrm{m}\mathrm{a}\mathrm{x}}$分别为道路曲率预瞄距离系数的最小值和最大值。

车辆在弯道行驶时也要考虑车速对预瞄距离的影响，设置车速预瞄距离系数，该系数的表达式为：

$$k_v=\left\{\begin{array}{ll}\mathrm{1}& v_x<v_{x\mathrm{m}\mathrm{i}\mathrm{n}}\\ \mathrm{1}+\frac{k_{v\mathrm{m}\mathrm{a}\mathrm{x}}}{v_{x\mathrm{m}\mathrm{a}\mathrm{x}}-v_{x\mathrm{m}\mathrm{i}\mathrm{n}}}(v_x-v_{x\mathrm{m}\mathrm{i}\mathrm{n}})& v_{x\mathrm{m}\mathrm{i}\mathrm{n}}\le v_x\le v_{x\mathrm{m}\mathrm{a}\mathrm{x}}\\ k_{v\mathrm{m}\mathrm{a}\mathrm{x}}& v_x>v_{x\mathrm{m}\mathrm{a}\mathrm{x}}\end{array}\right.$$

（44）

式中：纵向车速$v_{x\mathrm{m}\mathrm{i}\mathrm{n}}=\mathrm{10}\mathrm{m}/\mathrm{s}$，$v_{x\mathrm{m}\mathrm{a}\mathrm{x}}=\mathrm{20}\mathrm{m}/\mathrm{s}$；车速预瞄距离系数的最大值$k_{v\mathrm{m}\mathrm{a}\mathrm{x}}=\mathrm{2}$。

综合考虑直线和弯道行驶，预瞄距离的表达式为：

$$L=\left\{\begin{array}{cc}{L}_v& \kappa =\mathrm{0}\\ L_v\cdot k_{\kappa }\cdot k_v& \mathrm{e}\mathrm{l}\mathrm{s}\mathrm{e}\end{array}\right.$$

（45）

同时，不希望预瞄距离变化太快，故本文对预瞄距离的变化速度做出限制，因此预瞄距离的表达式为：

$$L_p=L_{p,\mathrm{l}\mathrm{a}\mathrm{s}\mathrm{t}}+\mathrm{s}\mathrm{g}\mathrm{n}(L-L_{p,\mathrm{l}\mathrm{a}\mathrm{s}\mathrm{t}})\cdot \mathrm{m}\mathrm{i}\mathrm{n}(\mathrm{a}\mathrm{b}\mathrm{s}(L-L_{p,\mathrm{l}\mathrm{a}\mathrm{s}\mathrm{t}}),\mathrm{\Delta }{L}_{p\mathrm{m}\mathrm{a}\mathrm{x}})$$

（46）

式中：$L_{P,\mathrm{l}\mathrm{a}\mathrm{s}\mathrm{t}}$为上一时刻的预瞄距离；$\mathrm{\Delta }{L}_{P\mathrm{m}\mathrm{a}\mathrm{x}}$为预瞄距离的最大变化值。

4 仿真验证

为验证所提出的预瞄距离优化策略的有效性，基于Simulink-Carsim联合仿真平台进行仿真试验。在Carsim中完成整车模型的建立，并且基于Matlab/Simulink平台进行轨迹跟踪控制算法模型的搭建。利用CarSim中的Matlab/Simulink 联合仿真接口，定义整车模型的输入输出端口，实现两者之间的联合仿真。如图5所示，在Carsim中完成车辆悬架、轮胎、等车辆尺寸参数设置。车辆动力学模型所需的基本参数配置如表3所示。

图5  Carsim仿真模型建立

Fig.5  Carsim Simulation model

表 3  车辆动力学模型参数

Tab.3  Vehicle Parameters

参数	符号	数值
整车质量/kg	$m$	1 290
质心到前轴的距离/m	$l_{\mathrm{f}}$	1.060
质心到后轴的距离/m	$l_{\mathrm{r}}$	1.245
绕z轴转动惯量/（kg·m2）	$I_z$	1 702.4
前轮等效侧偏刚度/—	$K_{\mathrm{f}}$	30 838
后轮等效侧偏刚度/—	$K_{\mathrm{r}}$	36 682

仿真试验中，设置车辆初始速度为10 m/s，车辆目标速度为15 m/s，道路附着系数为0.8，规划参考轨迹为双移线，预瞄距离分别设置为1 m、3 m及优化预瞄距离。进行仿真对比试验，仿真结果如图6所示.图6（a）、6（b）车辆轨迹及纵向速度实验结果验证了所提出轨迹跟踪算法的有效性，当车速为15 m/s时，采用不同的预瞄距离，均能完成轨迹跟踪控制任务。考虑车辆速度及道路曲率优化预瞄距离后，由图6（c）、6（d）及表4可以看出，基于优化预瞄距离的车辆跟踪过程中的最大侧向位移误差相较于预瞄距离1 m、3 m分别下降76%和17%。最大航向角误差增大，但从图6（e）、6（f）横摆角速度和质心侧偏角仿真结果可以看出，车辆的稳定性得到了提升。相比于较短距离的预瞄，优化预瞄距离后的轨迹跟踪控制使车辆的最大质心侧偏角降级接近50%，横摆角速度变化平稳，且不会在弯道处出现抖震，提升了车辆的操纵性能。

(a)  行驶轨迹

（b）  纵向速度

（c）  侧向位移误差

（d）  航向角误差

（e）  横摆角速度

（f）  质心侧偏角

图6  优化预瞄距离仿真试验

Fig.6  Optimization of preview distance simulation experiment

表4  优化预瞄距离轨迹跟踪仿真实验

Tab.4  Optimization of preview distance simulation experiment

预瞄距离	侧向位移误差最大值	侧向位移误差平均值	航向角误差最大值	航向角误差平均值
1m	0.175m	0.001m	5.89°	0.009°
3m	0.051m	0.0017m	5.03°	0.019°
优化	0.042m	0.0010m	6.12°	0.016°

5 结语

本文通过结合车辆二自由度车辆动力学方程及车辆轨迹跟踪与参考轨迹的运动学关系建立预瞄偏差模型，并基于车辆参数搭建了Carsim车辆动力学仿真模型。将预瞄偏差模型作为模型预测控制器的预测模型进行轨迹跟踪控制器的开发，以前轮转角为控制输入，道路曲率为干扰。

将轨迹跟踪控制问题转化为优化问题进行求解，设计以减小轨迹跟踪误差及控制量缓变为目标的目标函数，结合执行器特性设计约束条件。通过仿真试验验证了所提出的控制器的有效性，针对预瞄距离对控制结果的影响进行仿真分析，提出了基于规则的预瞄距离优化方法，基于实时纵向车速和预瞄位置的道路曲率通过规则设计实现预瞄距离的控制。通过仿真分析发现，预瞄距离优化能够解决中高车速下期望轨迹复杂道路曲率大时，固定预瞄距离的轨迹跟踪控制器不能同时保证轨迹的跟踪精度和行驶稳定性的问题，结合预瞄距离优化的轨迹跟踪控制器具有较好的行驶稳定性的同时提升了轨迹跟踪精度 。

本文在建立车辆动力学模型只考虑了侧向和横摆两个自由度的动力学特性，简化的车辆动力学模型能够减少计算量保证控制器的实时性要求，但实际上车辆的纵向运动和侧向运动存在很强的耦合关系，例如纵向加速度引起的前后轴载荷转移以及侧向加速度引起的左右轮载荷转移带来的轮胎垂直载荷重新分配，进而对纵侧向动力学产生影响。在以后的工作中会进一步展开此方向的工作，并考虑侧向控制与纵向控制的预瞄距离的差异及他们对控制结果的影响。