基于多约束随机模型预测控制的无人车运动规划与控制

摘要

提出了一种基于随机模型预测控制的无人车运动规划方法。在道路坐标系下，采用质点运动模型和高斯分布对周围动态车辆的预测轨迹进行位置不确定性表征，并使用随机模型预测控制（SMPC）中的机会约束进行描述，以此建立车辆空间位置的约束。通过变步长求解方式获得基于运动学模型的初始控制序列。基于此初始解，考虑车辆动力学信息，引入基于横摆角速度和质心侧偏角关系的稳定性约束，求解最优控制量。在多种工况下，通过仿真试验验证了所提出方法的有效性和稳定性。

关键词

无人车; 运动控制; 随机模型预测控制; 最优控制

传统的无人驾驶车辆架构体系中，轨迹规划和轨迹跟踪控制分别属于决策规划系统和运动控制系统。然而，随着对无人驾驶技术研究的深入，研究者们发现了将轨迹规划与跟踪控制相结合的优势和必要性。考虑到轨迹规划与轨迹跟踪的特点，模型预测控制（model predictive control， MPC）凭借其在利用预测信息和考虑多约束方面的巨大优势，成为了最适合于设计规划控制结合框架的算法^［1］。基于MPC的规划控制结合方法中，可以通过添加约束条件处理车辆周围环境和障碍物信息。

建立约束的方法是指在MPC的优化问题中直接添加约束条件，包括等式和不等式约束，这些约束条件可以限制车辆在执行规划与跟踪控制过程中的空间位置。除此之外，约束条件中通常会加入车辆的动力学特性约束。Schildbach等^［2-3］提出了场景模型预测控制（SCMPC）的概念，应用于高速公路换道工况的轨迹规划与跟踪控制，将车辆的换道路径统一建模为一个非线性函数，并考虑换道过程中车辆位置的不确定分布，再根据障碍车辆与自车相对速度的关系确定出当前车道和目标车道上安全的区域，执行换道或跟车。

文献［4］建立了行人的运动模型，并将对行人的运动预测结果转化为MPC问题的约束，使用MPC设计了一个包含轨迹规划和横纵向控制的通用框架，并在仿真和实车试验中进行了验证，车辆可以做出减速和停车等行为。

文献［5］提出的规划控制框架中，包括基于简单纵向运动学预测模型的基于多点控制的纵向运动规划模块、基于低维横向动力学预测模型的基于多点控制的集成横向规划控制模块和前馈纵向运动跟踪模块，该方法优先考虑纵向的运动规划，更接近于人类驾驶行为。

通过对相关文献的调研和总结，当前轨迹局部规划与跟踪控制的一体化算法的研究领域虽然已经有较多成果，但依然存在以下问题： ①算法中基本只考虑了确定性的信息，没有对不确定性进行系统地建模与处理； ②算法中多使用固定预测步长，或者没有使用合适的模型适配变化预测步长的算法； ③算法中对车辆行驶的舒适性考虑不足。

本文设计的基于多约束随机模型预测控制的无人车运动规划与控制的一体化算法，能够描述周围动态障碍车辆的轨迹预测信息的不确定性，考虑车辆动力学特性和不同车速工况下的控制需求，把轨迹局部规划和跟踪控制转化为统一的最优控制问题并求解，在实现无人驾驶车辆轨迹规划的同时，进行相应跟踪控制，以保证车辆行驶的安全性和平稳性。

1 SMPC算法介绍

1.1　MPC算法

MPC已经在复杂系统的高性能控制领域证明了其优秀的性能。MPC在概念上简单直观，但可以有效地处理具有多个控制输入和输出、状态约束和控制输入约束以及多控制目标的复杂动力学系统的控制问题。MPC的基本思想即是以滚动时域的方式反复求解开环的带约束最优控制问题（OCP），每次得到动态系统当前状态在当前时刻最优的控制序列，输入到被控平台，被控平台按照当前控制量进行控制，然后将当前状态量输入到MPC控制器，再次进行最优求解。

对于一个动态离散时间系统， 可以将系统在第k个采样时间步中的动态表示为：

$$\left\{\begin{array}{l}{\mathit{x}}_{k+\mathrm{1}}=f\left({\mathit{x}}_k,{\mathit{u}}_k\right)\\ {\mathit{y}}_k=g\left({\mathit{x}}_k,{\mathit{u}}_k\right)\end{array}\right.$$

（1）

式中： ${\mathit{x}}_{\mathit{k}}\in R^{n_x}$、 ${\mathit{u}}_{\mathit{k}}\in R^{n_u}$、 ${\mathit{y}}_{\mathit{k}}\in R^{n_y}$分别表示系统的状态、控制输入和控制输出，$f$和$g$分别描述系统的动力学和输出。

当系统的动力学和输出都为线性函数时，可以将式（1）中的$f$和$g$替换为如下矩阵形式来表达：

$$\left\{\begin{array}{l}{\mathit{x}}_{k+\mathrm{1}}={\mathit{A}}_k{\mathit{x}}_k+{\mathit{B}}_k{\mathit{u}}_k\\ {\mathit{y}}_k={\mathit{C}}_k{\mathit{x}}_k+{\mathit{D}}_k{\mathit{u}}_k\end{array}\right.$$

（2）

在模型预测控制中，向前选取有限的采样步数作为预测时域$N_P$，同时在不超过预测时域的范围内选取控制时域$N_C$，表示控制输入会在未来的$N_C$步作用于系统，再通过系统的动态方程对未来的$N_P$步的系统状态进行预测，得到在控制时域$\{\mathrm{0},\mathrm{1},\dots ,N_c-\mathrm{1}\}$内的控制序列$\{{\mathit{u}}_{\mathrm{0}},{\mathit{u}}_{\mathrm{1}},\dots ,{\mathit{u}}_{N_C-\mathrm{1}}\}$作用下系统未来的系统状态$\{{\mathit{x}}_{\mathrm{1}},{\mathit{x}}_{\mathrm{2}},\dots ,{\mathit{x}}_{N_P}\}$。根据控制要求，设定相应的优化目标，通常为未来状态序列与参考量序列$\{{\mathit{x}}_{\mathrm{r}\mathrm{e}\mathrm{f},\mathrm{1}},{\mathit{x}}_{\mathrm{r}\mathrm{e}\mathrm{f},\mathrm{2}},\dots ,{\mathit{x}}_{\mathrm{r}\mathrm{e}\mathrm{f},N_P}\}$之间的误差和控制序列的大小，并以此为依据建立最优控制问题的目标函数。这种典型的MPC目标函数可以表示如下：

$$J={\displaystyle \sum _{k=\mathrm{1}}^{N_P-\mathrm{1}}}{\left({\mathit{x}}_k-{\mathit{x}}_{k,\mathrm{r}\mathrm{e}\mathrm{f}}\right)}^{\mathrm{T}}Q\left({\mathit{x}}_k-{\mathit{x}}_{k,\mathrm{r}\mathrm{e}\mathrm{f}}\right)+{\left({\mathit{x}}_{N_P}-{\mathit{x}}_{N_P,\mathrm{r}\mathrm{e}\mathrm{f}}\right)}^{\mathrm{T}}{\mathit{Q}}_{\mathrm{T}}\left({\mathit{x}}_{N_P}-{\mathit{x}}_{N_P,\mathrm{r}\mathrm{e}\mathrm{f}}\right)+{\displaystyle \sum _{k=\mathrm{0}}^{N_c-\mathrm{1}}}{u}_k^{\mathrm{T}}R{\mathit{u}}_k$$

（3）

式中： ${\mathit{x}}_k$、${\mathit{x}}_{\mathrm{r}\mathrm{e}\mathrm{f},k}$ 和${\mathit{u}}_k$分别表示第k个采样时间步的系统状态向量、系统参考状态向量及控制输入向量；$\mathit{Q}$和$\mathit{R}$分别表示非终端系统状态和控制输入的权重矩阵，${\mathit{Q}}_{\mathrm{T}}$表示终端系统状态的权重矩阵。

当动态系统为线性系统时，可以将MPC转化为线性OCP问题，甚至进一步转化为二次规划（quadratic programming， QP）问题。这种形式的OCP结构简单，可以使用合适的方法实现快速求解，这对于无人驾驶车辆这类高实时性要求的系统具有重要意义。

综合式（3）给出的优化目标函数，将构建MPC问题时对系统状态、控制输入及控制输出设置约束条件和式（2）所示的线性系统动态方程分别作为等式约束和不等式约束的部分，并综合式（3）给出的优化目标函数，可以得到如式（4）给出的线性OCP：

$$\begin{array}{l}\mathrm{m}\mathrm{i}\mathrm{n}J={\displaystyle \sum _{k=\mathrm{1}}^{N_P-\mathrm{1}}}{\left({\mathit{x}}_k-{\mathit{x}}_{k,\mathrm{r}\mathrm{e}\mathrm{f}}\right)}^{\mathrm{T}}Q\left({\mathit{x}}_k-{\mathit{x}}_{k,\mathrm{r}\mathrm{e}\mathrm{f}}\right)+{\left({\mathit{x}}_{N_P}-{\mathit{x}}_{N_P,\mathrm{r}\mathrm{e}\mathrm{f}}\right)}^{\mathrm{T}}{\mathit{Q}}_{\mathrm{T}}\left({\mathit{x}}_{N_P}-{\mathit{x}}_{N_P,\mathrm{r}\mathrm{e}\mathrm{f}}\right)+{\displaystyle \sum _{k=\mathrm{0}}^{N_c-\mathrm{1}}}{u}_k^{\mathrm{T}}R{\mathit{u}}_k;\\ \mathrm{s}.\mathrm{t}.{\mathit{x}}_{k+\mathrm{1}}={\mathit{A}}_k{\mathit{x}}_k+{\mathit{B}}_k{\mathit{u}}_k,k=\mathrm{1,2},\dots ,N_P-\mathrm{1},\\ {\mathit{y}}_k={\mathit{C}}_k{\mathit{x}}_k+{\mathit{D}}_k{\mathit{u}}_k,k=\mathrm{1,2},\dots ,N_P,\\ {\mathit{x}}_{lb}\le {\mathit{x}}_k\le {\mathit{x}}_{ub},k=\mathrm{1,2},\dots ,N_P,\\ {\mathit{u}}_{lb}\le {\mathit{u}}_k\le {\mathit{u}}_{ub},k=\mathrm{0,1},\dots ,N_C-\mathrm{1},\\ {\mathit{y}}_{lb}\le {\mathit{y}}_k\le {\mathit{y}}_{ub},k=\mathrm{0,1},\dots ,N_P-\mathrm{1}\end{array}$$

（4）

式中： ${\mathit{x}}_{lb}$、 ${\mathit{u}}_{lb}$、 ${\mathit{y}}_{lb}$分别表示第k个采样时间步的系统状态、控制输入及控制输出的下限； ${\mathit{x}}_{ub}$、 ${\mathit{u}}_{ub}$、 ${\mathit{y}}_{ub}$分别表示第k个采样时间步的系统状态、控制输入及控制输出的上限。以上即标准MPC的一般表达形式。

1.2　SMPC算法

随机模型预测控制（SMPC）相比于标准MPC的区别就是引入与概率相关的机会约束来对随机系统的不确定性进行描述，其余目标函数、硬约束的建立和系统动态方程等基本与1.1中一致。因此，在引入机会约束后，可以将由SMPC构造的OCP问题统一表示为如下形式：

$$\begin{array}{l}\mathrm{m}\mathrm{i}\mathrm{n}J={\displaystyle \sum _{k=\mathrm{1}}^{N-\mathrm{1}}}{J}_{\mathrm{P}}\left({\mathit{x}}_k,{\mathit{u}}_k\right)+J_{\mathrm{T}}\left({\mathit{x}}_N\right);\\ \mathrm{s}.\mathrm{t}.{\mathit{x}}_{k+\mathrm{1}}=f\left({\mathit{x}}_k,{\mathit{u}}_k\right),k=\mathrm{0,1},\dots ,N-\mathrm{1},\\ {\mathit{y}}_k=g\left({\mathit{x}}_k,{\mathit{u}}_k\right),k=\mathrm{1,2},\dots ,N,\\ {\mathit{x}}_k\in {\mathit{X}}_k,k=\mathrm{1,2},\dots ,N,\\ {\mathit{u}}_k\in {\mathit{U}}_k,k=\mathrm{0,1},\dots ,N-\mathrm{1},\\ {\mathit{y}}_k\in {\mathit{Y}}_k,k=\mathrm{0,1},\dots ,N-\mathrm{1},\\ \mathrm{P}\mathrm{r}\left({\mathit{y}}_k\in {\mathit{Y}}_{k,\mathrm{s}\mathrm{a}\mathrm{f}\mathrm{e}}^{\mathrm{\text{'}}}\left(\omega \right)\right)\ge p,k=\mathrm{1,2},\dots ,N\end{array}$$

（5）

式中：为方便表示，将非终端状态的目标函数和终端状态的使用$J_{\mathrm{P}}$和$J_{\mathrm{T}}$分别表示，在第k个采样时间步的系统状态、控制输入及控制输出${\mathit{x}}_k$、 ${\mathit{u}}_k$、 ${\mathit{y}}_k$的约束集分别用${\mathit{X}}_k$、 ${\mathit{U}}_k$、 ${\mathit{Y}}_k$表示。约束的最后一项为机会约束，表示满足${\mathit{y}}_k\in {\mathit{Y}}_{k,\mathrm{s}\mathrm{a}\mathrm{f}\mathrm{e}}(\omega )$这一不等式约束的条件概率Pr的下界为$p$，这里的$p$为风险参数且满足$p\in (\mathrm{0},\mathrm{1})$，约束集${\mathit{Y}}_{k,\mathrm{s}\mathrm{a}\mathrm{f}\mathrm{e}}(\omega )$取决于不确定性$\omega$。

对比SMPC和标准MPC的一般表示形式（5）和（4）可以看出，SMPC与标准MPC最关键的差异就在最后的机会约束项，SMPC中使用概率的方法来描述特定的约束条件，使得SMPC在处理与不确定性相关的状态量时准确性更高。

2 SMPC初始解生成算法及求解方法

本文设计的规控一体化初始解生成算法的作用是为下层横向与纵向的规划控制算法提供一个具有引导作用的初值轨迹，以使得无人驾驶车辆在执行规划控制一体化算法求解时可以提高求解成功率和求解效率，从而得到更好的控制量结果，并下发给无人驾驶车辆执行。

初始解生成算法使用了随机模型预测控制算法来构建初始解生成的非线性最优控制问题（nonlinear optimal control problem， NOCP），考虑周围动态障碍车辆轨迹预测的不确定性，在该最优控制问题的目标函数和约束条件中对无人驾驶车辆周围的道路结构信息、动态障碍车辆的运动状态以及规划控制初始解的优化目标进行了描述，最终求解该最优控制问题得到的状态量和控制量序列将作为横向与纵向运动的规划控制的初值，应用于算法的下层模块进行进一步的优化求解。

2.1　Frenét坐标系下车辆运动学预测模型

进行SMPC问题的构建时，对系统进行采样时间为T的离散时间采样，结合常用的笛卡尔坐标系下的车辆运动学模型，可以推导出如式（6）的Frenét坐标系下的车辆运动学模型在离散空间的状态方程（见图1）：

$$\left\{\begin{array}{l}{s}_{k+\mathrm{1}}=s_k+\left(\frac{v_k}{\mathrm{1}-{\kappa }_k{l}_k}\mathrm{c}\mathrm{o}\mathrm{s}\left({\phi }_k+\mathrm{t}\mathrm{a}{\mathrm{n}}^{-\mathrm{1}}\left(\frac{l_r\mathrm{t}\mathrm{a}\mathrm{n}{\delta }_{f,k}}{l_f+l_r}\right)\right)\right)T\\ l_{k+\mathrm{1}}=l_k+\left(v_k\mathrm{s}\mathrm{i}\mathrm{n}\left({\phi }_k+\mathrm{t}\mathrm{a}{\mathrm{n}}^{-\mathrm{1}}\left(\frac{l_r\mathrm{t}\mathrm{a}\mathrm{n}{\delta }_{f,k}}{l_f+l_r}\right)\right)\right)T\\ {\phi }_{k+\mathrm{1}}={\phi }_k+\left(\frac{v_k}{l_r}\mathrm{s}\mathrm{i}\mathrm{n}\left(\mathrm{t}\mathrm{a}{\mathrm{n}}^{-\mathrm{1}}\left(\frac{l_r\mathrm{t}\mathrm{a}\mathrm{n}{\delta }_{f,k}}{l_f+l_r}\right)\right)-{\kappa }_k\frac{v_k}{\mathrm{1}-{\kappa }_k{l}_k}\mathrm{c}\mathrm{o}\mathrm{s}\left({\phi }_k+\mathrm{t}\mathrm{a}{\mathrm{n}}^{-\mathrm{1}}\left(\frac{l_r\mathrm{t}\mathrm{a}\mathrm{n}{\delta }_{f,k}}{l_f+l_r}\right)\right)\right)T\\ v_{k+\mathrm{1}}=v_k+a_{k}T\end{array}\right.$$

（6）

图1  Frenét坐标系下的车辆运动学

Fig.1  Vehicle kinematics in the Frenét coordinate system

式中： $v$和$a$分别表示车辆当前的速度和加速度，$\kappa$表示车辆当前位置所对应的根点处的曲率；同时，还引入了前轮转角${\delta }_f$，和加速度$a$共同作为系统的控制量。式（6）表示的系统动态可以用来作为本文初始解生成算法的预测模型，模型中定义${\mathit{\xi }}_k=[s_k,l_k,{\phi }_k,v_k{]}^{\mathrm{T}}$为系统的状态向量，${\mathit{u}}_k=[{\delta }_{f,k},a_k{]}^{\mathrm{T}}$为系统的控制输入向量。

2.2　规划控制初始解的最优控制问题

本文将整个预测时域划分为两部分：5.0 s之前的部分需要精确控制，即需要为横向和纵向的算法提供一个精确度较高的初始解；5.0~10.0 s部分只需要关注长时间的未来状态对规划控制的影响，而对这个范围内初始求解结果的质量要求不高。

本文选择了变步长的方法来建立系统的动态方程，其中前5.0 s的预测步长为$T_{\mathrm{S}}=\mathrm{0.2}\mathrm{s}$，预测步数为$N_{\mathrm{S}}=\mathrm{25}$；后5.0 s的预测步长为$T_{\mathrm{L}}=\mathrm{1.0}\mathrm{s}$，预测步数为$N_{\mathrm{L}}=\mathrm{5}$，总预测步数$N_{\mathrm{t}\mathrm{o}\mathrm{t}\mathrm{a}\mathrm{l}}=N_{\mathrm{S}}+N_{\mathrm{L}}=\mathrm{30}$。如此，可以在充分考虑未来运动状态的同时保证短时间内初始解优化结果的精确性，故可以为后续的横向和纵向运动的进一步优化提供较好的参考。在变步长的条件下，结合式（6）给出初值轨迹，OCP约束条件的表达式部分如下：

$$\begin{array}{ll}\mathrm{\Delta }{s}_k=\frac{v_k}{\mathrm{1}-{\kappa }_k{l}_k}\mathrm{c}\mathrm{o}\mathrm{s}\left({\phi }_k+\mathrm{t}\mathrm{a}{\mathrm{n}}^{-\mathrm{1}}\left(\frac{l_r\mathrm{t}\mathrm{a}\mathrm{n}{\delta }_{f,k}}{l_f+l_r}\right)\right)& \forall k\in \left\{\mathrm{0},\dots ,N_S+N_L-\mathrm{1}\right\}\\ s_{k+\mathrm{1}}=s_k+\mathrm{\Delta }{s}_k{T}_S& \forall k\in \left\{\mathrm{0},\dots ,N_S-\mathrm{1}\right\}\\ l_{k+\mathrm{1}}=l_k+\left(v_k\mathrm{s}\mathrm{i}\mathrm{n}\left({\phi }_k+\mathrm{t}\mathrm{a}{\mathrm{n}}^{-\mathrm{1}}\left(\frac{l_r\mathrm{t}\mathrm{a}\mathrm{n}{\delta }_{f,k}}{l_f+l_r}\right)\right)\right)T_S& \forall k\in \left\{\mathrm{0},\dots ,N_S-\mathrm{1}\right\}\\ {\phi }_{k+\mathrm{1}}={\phi }_k+\left(\frac{v_k}{l_r}\mathrm{s}\mathrm{i}\mathrm{n}\left(\mathrm{t}\mathrm{a}{\mathrm{n}}^{-\mathrm{1}}\left(\frac{l_r\mathrm{t}\mathrm{a}\mathrm{n}{\delta }_{f,k}}{l_f+l_r}\right)\right)-{\kappa }_k\mathrm{\Delta }{s}_k\right)T_S& \forall k\in \left\{\mathrm{0},\dots ,N_S-\mathrm{1}\right\}\\ v_{k+\mathrm{1}}=v_k+a_k{T}_S& \forall k\in \left\{\mathrm{0},\dots ,N_S-\mathrm{1}\right\}\\ s_{k+\mathrm{1}}=s_k+\mathrm{\Delta }{s}_k{T}_L& \forall k\in \left\{N_S,\dots ,N_L-\mathrm{1}\right\}\\ l_{k+\mathrm{1}}=l_k+\left(v_k\mathrm{s}\mathrm{i}\mathrm{n}\left({\phi }_k+\mathrm{t}\mathrm{a}{\mathrm{n}}^{-\mathrm{1}}\left(\frac{l_r\mathrm{t}\mathrm{a}\mathrm{n}{\delta }_{f,k}}{l_f+l_r}\right)\right)\right)T_L& \forall k\in \left\{N_S,\dots ,N_L-\mathrm{1}\right\}\\ {\phi }_{k+\mathrm{1}}={\phi }_k+\left(\frac{v_k}{l_r}\mathrm{s}\mathrm{i}\mathrm{n}\left(\mathrm{t}\mathrm{a}{\mathrm{n}}^{-\mathrm{1}}\left(\frac{l_r\mathrm{t}\mathrm{a}\mathrm{n}{\delta }_{f,k}}{l_f+l_r}\right)\right)-{\kappa }_k\mathrm{\Delta }{s}_k\right)T_L& \forall k\in \left\{N_S,\dots ,N_L-\mathrm{1}\right\}\\ v_{k+\mathrm{1}}=v_k+a_k{T}_L& \forall k\in \left\{N_S,\dots ,N_L-\mathrm{1}\right\}\end{array}$$

（7）

式中的初始状态向量为${\mathit{\xi }}_{\mathrm{0}}=[s_{\mathrm{0}},l_{\mathrm{0}},{\phi }_{\mathrm{0}},v_{\mathrm{0}}{]}^{\mathrm{T}}$。

将状态向量、控制输入向量和预测步长之间的关系简写为一个函数${\mathit{f}}^{\mathrm{d}}$表示，可以将上式简写为如下式（8）形式：

$$\begin{array}{cc}{\mathit{x}}_{k+\mathrm{1}}={\mathit{f}}^{\mathrm{d}}\left({\mathit{x}}_k,{\mathit{u}}_k,T_S\right)& \forall k\in \left\{\mathrm{0,1},\dots ,N_S-\mathrm{1}\right\}\\ {\mathit{x}}_{k+\mathrm{1}}={\mathit{f}}^{\mathrm{d}}\left({\mathit{x}}_k,{\mathit{u}}_k,T_L\right)& \forall k\in \left\{N_S,\dots ,N_L-\mathrm{1}\right\}\end{array}$$

（8）

将预测时域内所有时刻的各优化目标项相加，得到最终的优化目标函数如下：

$$\begin{array}{l}J={\displaystyle \sum _{k=\mathrm{1}}^{N_S+N_L}}\left(J_{l,k}+J_{\phi ,k}+J_{v,k}\right)+{\displaystyle \sum _{k=\mathrm{0}}^{N_S+N_L-\mathrm{1}}}\left(J_{{\delta }_f,k}+J_{a,k}\right)\\ ={\displaystyle \sum _{k=\mathrm{1}}^{N_S+N_L}}\left(q_l{\left(l_k-l_{ref}\right)}^{\mathrm{2}}+q_{\phi }{\phi }_k^{\mathrm{2}}+q_v{\left(v_k-v_{ref,k}\right)}^{\mathrm{2}}\right)+{\displaystyle \sum _{k=\mathrm{0}}^{N_S+N_L-\mathrm{1}}}\left(r_{{\delta }_f}{\delta }_{f,k}^{\mathrm{2}}+r_a{a}_k^{\mathrm{2}}\right)\end{array}$$

（9）

再结合每一时刻对应的系统状态量和控制输入约束，将目标函数和约束条件都以向量和矩阵的形式表示，可以得到以下表达的基于SMPC构建的最优控制问题：

$$\begin{array}{l}\begin{array}{ll}\underset{{\mathit{u}}_k}{\mathrm{m}\mathrm{i}\mathrm{n}}& {\displaystyle \sum _{k=\mathrm{1}}^{N_S+N_L}}{‖{\mathit{\xi }}_k-{\mathit{\xi }}_{ref}‖}_{\mathit{Q}}^{\mathrm{2}}+{\displaystyle \sum _{k=\mathrm{0}}^{N_S+N_L-\mathrm{1}}}{‖{\mathit{u}}_k‖}_{\mathit{R}}^{\mathrm{2}}\end{array}\\ \begin{array}{lll}s.t.& {\mathit{x}}_{k+\mathrm{1}}={\mathit{f}}^{\mathrm{d}}\left({\mathit{x}}_k,{\mathit{u}}_k,T_S\right)& \forall k\in \left\{\mathrm{0,1},\dots ,N_S-\mathrm{1}\right\}\\ & {\mathit{x}}_{k+\mathrm{1}}={\mathit{f}}^{\mathrm{d}}\left({\mathit{x}}_k,{\mathit{u}}_k,T_L\right)& \forall k\in \left\{N_S,\dots ,N_L-\mathrm{1}\right\}\\ & \mathrm{P}\mathrm{r}\left({\mathit{\xi }}_k\in {\mathit{\Xi }}_{safe,k}^{\mathrm{\text{'}}}\left(\omega \left({\mathit{u}}_{obj,k}\right)\right)\right)\ge p& \forall k\in \left\{\mathrm{1,2},\dots ,N_S+N_L\right\}\\ & {\mathit{\xi }}_k\in \mathit{\Xi }& \forall k\in \left\{\mathrm{1,2},\dots ,N_S+N_L\right\}\\ & {\mathit{u}}_k\in \mathit{U}& \forall k\in \left\{\mathrm{0,1},\dots ,N_S+N_L-\mathrm{1}\right\}\end{array}\end{array}$$

（10）

3 仿真实验

本章设计了超车与跟车及行人避让两种较为典型的无人驾驶车辆测试场景来进行仿真试验，对本文设计的规划控制一体化算法的期望功能进行验证，并对各工况的仿真试验结果分别进行了分析。

本文使用的仿真试验平台是在Ubuntu 18.04系统上搭建的，平台主体为自动驾驶场景仿真软件VTD 2021，VTD与算法程序之间使用ZeroCM（ZCM）进行数据通信，所有相关程序的开发都是基于C⁺⁺完成的。

3.1　跟车与超车仿真试验

本小节设计了一个典型场景的双车道跟车与超车的试验来对整体算法的有效性和可行性进行验证，如图2所示。自车初始速度为35 km/h，右边车道前方有两辆动态障碍车辆，速度均为25 km/h，左侧车道有一辆动态障碍车，速度为30 km/h，自车在双车道的范围内行驶，参考线为右边车道的中心线。

图2  双车道跟车与超车仿真

Fig.2  Dual-lane following and overtaking simulation

图3给出了整个仿真试验过程中车辆速度、纵向加速度和转向轮转角的变化，其中自车从开始至15.183 s工作在“目标跟随”模式，车辆的速度最低降至24.0 km/h左右，这个过程中的减速度最高达到0.7 m/s²左右由于该过程为换道跟车，跟随左边车道内的车速较高的目标，所以进行了一次换道，前轮转角由零变化至-2.5°左右；之后的过程车辆都工作在“速度保持”模式，车速逐渐增加到期望速度40.0 km/h左右，并完成换道超车，这个过程中车辆的前轮转角最小为-2.5°左右，最大为2.5°左右，分别出现在超车换至左边车道和换回原车道的过程中。

图3  车辆速度、加速度和转角的变化

Fig.3  Changes in vehicle speed， acceleration and turning angle

整个仿真试验过程中车辆横向距离和航向偏角的变化如图4所示，其中横向距离为3.5 m左右时表示车辆在左车道内行驶，。

图4  车辆横向距离和航向偏角的变化

Fig.4  Changes in vehicle lateral distance and heading declination angle

3.2　行人避让仿真试验

本小节设计了一个典型场景的无信号灯路口行人避让的试验来验证初始解对规划控制算法的影响，如图5所示。自车初始速度为20 km/h，在即将通过的无信号灯路口有一行人穿越，行人的速度为5 km/h，自车在路口的右边车道的范围内行驶，需要对行人进行避让。该仿真试验在其它条件完全相同的情况下，分别对使用初始解生成模块和不使用初始解生成模块的两种算法进行对比。

图5  无信号灯路口行人避让仿真

Fig.5  Pedestrian avoidance simulation test at unsignalized intersections

将两组仿真试验进行对比，可以直观地看到图6所示的无初始解算法仿真过程中，自车减速的位置更靠近路口且减速较为急促，图7所示的有初始解算法仿真过程中，自车减速的位置更远离路口且减速较为平缓。根据图8和图9对比也可以看出，无初始解的算法在避让过程中，速度由20.0 km/h降低至12.0 km/h左右，这个过程中最大减速度达到 3.0 m/s²，有初始解的算法在避让过程中，速度由20.0 km/h降低至15.2 km/h左右，这个过程中最大减速度约 1.5 m/s²，相比无初始解的算法变化较为平缓。

图6  无初始解的规划控制算法避让过程的车辆位

Fig.6  Vehicle position for planning control algorithm avoidance process without initial solution

图7  有初始解的规划控制算法避让过程的车辆位置

Fig.7  Vehicle position for planning control algorithm avoidance process with initial solution

图8  避让过程中的车速

Fig.8  Speed during avoidance process

图9  避让过程中的加速度

Fig.9  Acceleration during avoidance process

3 结语

本文设计的基于多约束模型预测控制的无人车轨迹局部规划与跟踪控制一体化算法可以在不同的路况中选择最合适的模式工作，并优化得到出安全无碰撞的状态轨迹，再将相应的控制指令发送给无人驾驶车辆执行，从而保证车辆高效安全地行驶。同时算法的初始解对规划控制算法有提升作用，可以通过长时域充分考虑未来信息，以避免其它交通参与者的运动对自车运动在一个较长时域内的潜在影响造成的急促减速甚至停车的情况，从而保持了较为平稳的加速度和速度的变化。