安全增强的SMPC自动驾驶控制

原创于 2025-10-15 12:53:38 发布 · 736 阅读

18 ·

CC 4.0 BY-SA版权

文章标签：

#SMPC # FTP # 自动驾驶 # 轨迹规划 # 安全保障

部署运行你感兴趣的模型镜像

面向自动驾驶的安全保障型随机模型预测控制

摘要

自动驾驶车辆在不确定环境中进行操纵时需要高效且安全的规划。这种不确定性主要来自其他交通参与者，例如周围车辆。周围车辆的未来运动通常难以预测。虽然鲁棒控制方法能够实现安全但保守的运动规划，而随机模型预测控制（SMPC）则能够在存在不确定性的情况下提供高效的规划。通过应用概率约束来确保最大风险保持在预定义水平以下。然而，由于概率约束可能被违反，因此无法完全保证安全性，这在自动驾驶车辆中是不可接受的。本文提出了一种具备安全保障的高效轨迹规划框架。采用SMPC在有限时域内获得高效的车辆轨迹。基于第一个优化的SMPC输入，利用可达集规划一条有保障的安全备用轨迹。在必要时，该备用轨迹将用于覆盖SMPC输入以确保安全性。所提出的安全SMPC算法的递归可行性得到了证明。高速公路仿真结果表明了所提出的方法在性能与安全性方面的有效性。

索引术语

模型预测控制，随机模型预测控制，故障安全轨迹规划，自动驾驶车辆

一、引言

在过去的几十年中，自动驾驶汽车领域的研究取得了显著进展。大多数道路交通事故仍由人为错误引起，因此，提高车辆自主性有望大幅减少事故总数。

自动驾驶车辆的安全性取决于车辆控制算法处理其他交通参与者和环境不确定性的能力。尽管存在多种用于规划车辆轨迹的控制方法，但模型预测控制（MPC）通过在有限预测时域上迭代求解最优控制问题，已被证明是一种合适的方案。预测模型中的不确定性通过鲁棒模型预测控制（RMPC）[1]来解决。

RMPC方法被设计用于自动驾驶车辆的轨迹规划 [2],[3],然而，鲁棒地处理不确定性会导致保守的车辆行为。通过随机模型预测控制（SMPC）[4],[5],，在该方法中将鲁棒约束重新表述为概率形式，从而降低了因在模型预测控制中鲁棒处理不确定性而带来的保守性约束。这种概率性重构使得在大多数场景中能够进行乐观的轨迹规划，但也允许存在小概率的约束违反，即车辆 [6],[7]存在碰撞的概率。

与随机模型预测控制（SMPC）相比，基于可达性分析的轨迹规划能够提供形式化安全保证[8],[9]。在此方法中，通过对周围其他车辆进行最坏情况预测，以规划故障安全轨迹，即容错轨迹规划（FTP）。

在本研究中，我们致力于解决自动驾驶车辆高效且安全轨迹规划的挑战。我们提出了一种新型的模型预测控制轨迹规划器，该规划器结合了随机模型预测控制与容错轨迹规划在不确定性环境中的优势。利用随机模型预测控制规划一条轨迹，以实现乐观且高效的规划。在常规情况下，将第一个优化的SMPC输入应用于车辆，并在下一个时间步长通过移动时域求解新的SMPC最优控制问题。此外，针对每个时间步长，基于第一个优化的SMPC输入规划一条故障安全轨迹。仅当应用第一个SMPC输入后仍能获得故障安全备用轨迹时，才将该乐观的SMPC输入应用于车辆。这确保了只要存在备用轨迹，即可执行高效的 SMPC轨迹，从而保证安全性。所提出的方法称为 Stochastic Model Predictive Control + fail-safe trajectory planning (SMPC+FTP)。

本工作的贡献如下

一种新颖的SMPC FTP方法，可高效且安全地进行轨迹规划，包括换道决策。
随机模型预测控制+FTP方法的递归可行性证明
在复杂的高速公路交通场景下的仿真研究。

所提出的SMPC+FTP在利用高效SMPC轨迹规划优势的同时，确保了车辆的安全性。SMPC+FTP方法的设计保证了递归可行性，即如果某一时间步长存在解，则下一时间步长也保证存在解。通过对两个复杂场景的仿真研究，展示了在常规高速公路场景中乐观轨迹规划的优势，同时在紧急场景中验证了SMPC+FTP保障安全性的能力。

这项工作的扩展版本可参见[10],，其中提供了更详细的推导和分析。

A. 相关工作

自动驾驶车辆的轨迹规划是一个被广泛研究的领域。在非模型预测控制相关领域中有多种方法，例如使用部分可观测马尔可夫决策过程（POMDP）[11]或强化学习[12]。基于学习的方法在自动驾驶赛车[13]–[16]中也很流行。在考虑自动驾驶道路车辆时，模型预测控制用于操纵和轨迹规划 [17]本,[1文8]的。主要研究重点是基于SMPC和FTP的轨迹规划。

容错轨迹规划被定义为在考虑周围车辆任何合法未来运动的情况下，规划无碰撞车辆轨迹[19]。针对实际应用中的有界不确定性，FTP基于最坏情况不确定性实现进行应用。结合可达性分析，提供了形式化安全保证[9]。这些可达集的计算与RMPC中的控制不变集相关，如[20]所述。一种将可达性分析纳入模型预测控制的方法在[21]中给出。

在[22]中提出了一种方法，用于计算交通参与者可能占据的所有未来位置的集合。剩余的安全空间可用于规划紧急轨迹。该故障安全轨迹方法在[8]中提出。首先，根据周围车辆最可能的运动状态，确定一条最优轨迹；然后，将一条紧急轨迹连接到最优轨迹的最后一个点。故障安全轨迹的生成方式可确保被控车辆最终进入静止状态。

在[19]中介绍了一种可实时生成故障安全轨迹的FTP方法。该方法基于CommonRoad基准框架[23]在多种仿真场景中进行了测试。在[24],中引入了一种运动规划框架，该框架结合了可达性分析与基于优化的轨迹规划。

随机模型预测控制（SMPC）在自动驾驶车辆背景下已得到广泛研究。这些研究工作聚焦于由概率约束（即所谓的机会约束[25]）定义的风险与保守性之间的权衡。SMPC的一个主要挑战是将概率机会约束重新表述为可处理约束，以便求解器能够处理。

在[26]中展示了一种SMPC粒子方法，该方法应用于简单的车辆制动场景，其中粒子用于逼近不确定性。在[27]中提出了一种针对存在固定障碍物情况下的自动驾驶车辆的SMPC轨迹规划器。在[6]中，基于周围车辆最可能的预测结果并假设存在高斯不确定性，使用SMPC规划车辆轨迹。研究了不同的风险参数，说明了风险与保守性之间的权衡。在[28]中提出了一种SMPC变道控制器，其中通过预测碰撞时间来考虑变道风险。

另一种SMPC方法被用于[7],[29],，重点关注场景随机模型预测控制（SCMPC）。在SCMPC中，抽取不确定性的样本，这些样本必须满足约束，以找到可处理的机会约束表达式。SCMPC能够处理任意概率分布，而标准的 SMPC通常需要高斯分布以便对机会约束进行解析重构。[29]专注于简单的变道场景，而该工作在[7]中得到扩展，并给出了实验结果。

结合了SMPC和SCMPC各自优势的组合方法在[30],中给出。在[31],中提出了一种进一步的SMPC方法，该方法采用基于网格的SMPC方法来规划车辆轨迹。基于占据网格[32],[33]。

总之，SMPC方法在常规情况下能够为大多数不确定性实现提供高效的车辆轨迹。然而，对于小概率不确定性事件，会出现安全问题。

在本研究中，将利用SMPC高效规划轨迹的优势与FTP的安全保证相结合。本文中的FTP受到[8],[19],[22]思想的启发。下文将介绍SMPC和FTP，并详细推导所提出的SMPC+FTP方法。

II. 预备知识

下文中，我们简要介绍用于随机模型预测控制和FTP的通用模型预测控制最优控制问题（OCPs）。

模型预测控制通过求解一个具有有限预测时域 N且满足输入和状态约束的最优控制问题来迭代执行。在求解模型预测控制最优控制问题后，仅将优化输入序列 U=(u0,…,uN−1) >中的第一个输入 u0施加到系统。在下一个时间步长，重新求解更新后的模型预测控制最优控制问题。我们区分常规的时间步长 h和模型预测控制最优控制问题内的预测步长 k。若上下文明确，我们将省略计算模型预测控制最优控制问题时的时间步长 h。下文中，我们仅显式地标记预测时间步长 k。

A. 带机会约束的随机模型预测控制

尽管标准的模型预测控制考虑硬约束，但在存在不确定性时，这种方法存在问题。受不确定性影响的硬约束可通过机会约束来考虑，从而得到SMPC最优控制问题。

$$
V^* = \min_U \sum_{k=0}^{N-1} l(\xi_k, u_k) + V_f(\xi_N) \quad (1a)
$$
$$
\text{s.t. } \xi_{k+1} = f(\xi_k, u_k) \quad (1b)
$$
$$
u_k \in U_k, \xi_{k+1} \in \Xi_{k+1} \quad \forall k \in {0,…, N - 1} \quad (1c)
$$
$$
\Pr(\xi_k \in \Xi’_k,\text{safe}(w)) \geq \beta \quad \forall k \in {1,…, N} \quad (1d)
$$

预测步长为 k，状态为 $\xi_k$，系统动态为 $f$，以及服从正态分布、零均值的不确定性 $w \sim N(0,\Sigma_w)$，其协方差矩阵为 $\Sigma_w$。成本函数由阶段成本 $l(\xi_k,u_k)$和终端代价 $V_f(\xi_N)$组成。状态和输入分别受状态约束集$\Xi_k$和输入约束集 $U_k$限制，安全性约束$\xi_k \in \Xi’_k,\text{safe}(w)$依赖于不确定性 $w$。概率型机会约束由(1d)给出。安全性约束$\xi_k \in \Xi’_k,\text{safe}(w)$需根据风险参数 $\beta$满足要求。对于$\beta < 1$，允许存在非零的约束违反概率。

B. 故障安全轨迹规划

我们还考虑了用于FTP的MPC OCP，即故障安全 MPC OCP。与SMPC不同，FTP考虑不确定性的最坏情况实现，从而得到安全但保守的优化输入。尽管总体 OCP仍与(1)类似，但机会约束(1d)被替换为
$$
\xi_k \in \Xi_k,\text{safe}(w) \quad \forall k \in {1,…, N - 1} \quad (2a)
$$
$$
\xi_N \in \Xi_N,\text{safe}(w). \quad (2b)
$$

FTP安全集$\Xi_k,\text{safe}(w)$基于可达性分析构建，以确保形式化安全保证。除了约束(2a)外，还需要终端约束(2b)，该约束确保终端预测状态 $\xi_N$能够在预测时域之外保持在安全状态。基于此安全终端集$\Xi_N,\text{safe}(w)$，可保证存在系统输入 $u_{k+}$，使得 $k+ > N$产生安全状态 $\xi_{k+}$。

III. 车辆模型

模型预测控制（MPC）需要被控车辆（即自车（ EV））以及周围车辆（称为目标车辆（TVs））的系统模型，以在最优控制问题（OCP）中预测未来的状态。

A. 自车模型

我们采用运动学自行车模型来预测有限时域内的电动汽车状态，如[34]中所述。连续时间系统由以下给出
$$
\dot{s} = v \cos(\varphi + \alpha), \quad (3a)
$$
$$
\dot{d} = v \sin(\varphi + \alpha), \quad (3b)
$$
$$
\dot{\varphi} = \frac{v}{l_r} \sin\alpha, \quad (3c)
$$
$$
\dot{v} = a, \quad (3d)
$$
$$
\alpha = \arctan\left( \frac{l_r}{l_r + l_f} \tan\delta \right), \quad (3e)
$$

其中 $l_r$和 $l_f$分别为车辆质心到后轴和前轴的距离。状态向量为 $\xi=[s, d, \varphi, v]^>$，输入向量为 $u=[a, \delta]^>$。车辆速度由 $v$给出，加速度和转向角分别用 $a$和 $\delta$表示。我们考虑车辆沿道路的纵向位置 $s$、车辆相对于右车道中心线的横向偏差 $d$以及车辆相对于道路的方位角 $\varphi$。非线性车辆模型(3)总结为 $\dot{\xi} = f_c(\xi,u)$。

每个模型预测控制最优控制问题（MPC OCP）都通过在当前车辆状态 $\xi^ = \xi_0$和输入 $u^ =[0, 0]^>$处对非线性预测模型(3)进行线性化来初始化。选择非零参考输入 $u^ $通常会导致较远的预测步长出现较大的差异$\Delta u= u_k -u^ $，从而增加线性化的不准确性。随后，线性化连续时间车辆模型表示为
$$
\dot{\xi}^ +\Delta\dot{\xi} = f_c (\xi^ , 0)+ A_l(\xi - \xi^*)+ B_l u \quad (4)
$$

使用雅可比矩阵
$$
A_l=\left[\frac{\partial f_c}{\partial \xi}\right] \bigg| {(\xi^ ,u^ )}, \quad B_l=\left[\frac{\partial f_c}{\partial u}\right] \bigg| {(\xi^ ,u^ )}. \quad (5)
$$

模型预测控制需要离散时间模型，因此线性化预测模型(4)以采样时间 T进行离散化。这得到了用于预测步长 k 的离散状态 $\xi_k =[s_k , d_k , \varphi_k , v_k]^>$和输入 $u_k =[a_k , \delta_k]^>$，以及线性化且离散化的系统
$$
\xi_{k+1} = \xi_0 + T f_c (\xi_0 , 0)+ A_d(\xi_k - \xi_0) + B_d u_k \quad (6a)
$$
$$
= f_d (\xi_0 , \xi_k ,u_k) \quad (6b)
$$

其中 $A_d$和 $B_d$是由 $A_l$、 $B_l$通过零阶保持法得到的线性化系统的矩阵。(4)中的非线性项$f_c(\xi^ ,u^ )$由于 $\xi_0$已知，采用前向欧拉法进行近似。线性化且离散化的矩阵 $A_d$和$B_d$在扩展版本[10]中给出。以下对于(6)中的$k= 0$，即 $\xi_k= \xi_0$，参数 $\xi_0$仅提及一次，即 $f_d(\xi_0, \xi_0,u_0)$简写为 $f_d(\xi_0,u_0)$。

后续章节推导了一种用于避免与周围车辆发生碰撞的随机模型预测控制方法及其约束条件。然而，即使不存在其他车辆，仍需要满足某些约束条件。加速度和转向角受到限制
$$
u_{\min} \leq u_k \leq u_{\max} \quad (7a)
$$
$$
\Delta u_{\min} \leq \Delta u_k \leq \Delta u_{\max} \quad (7b)
$$

其中 $\Delta u_{k+1}= u_{k+1} - u_k$和 $u_{\max}=[a_{\max}, \delta_{\max}]^>$， $u_{\min}=[a_{\min}, \delta_{\min}]^>$。此外，考虑了道路和速度约束，从而得到
$$
d_k \in D_{\text{lane}} \quad (8a)
$$
$$
0 \leq v_k \leq v_{\max} \quad (8b)
$$

其中 $D_{\text{lane}}$表示道路边界， $v_{\max}$是最大速度。不允许出现负速度，即 $v_k \geq 0$。

下文中，我们将输入约束称为可接受输入集 $U$，状态约束称为可接受状态集 $\Xi$。

B. 目标车辆模型

为了避免碰撞，电动汽车还需要预测周围交通车辆（ TVs）的未来状态。电动汽车所使用的交通车辆（TVs）的预测模型是一个由以下公式给出的线性离散时间点质量模型
$$
\xi_{\text{TV}, k+1}= A\xi_{\text{TV}, k}+ Bu_{\text{TV}} \quad (9a)
$$
$$
u_{\text{TV}, k}= \tilde{u} {\text{TV}, k}+ w {\text{TV}} \quad (9b)
$$

其中 $\xi_{\text{TV}, k} =[x_{\text{TV}, k} , v_{\text{TV}, x,k} , y_{\text{TV}, k} , v_{\text{TV}, y,k}]^>$是TV状态，包含纵向位置和速度 $x_{\text{TV}, k}, v_{\text{TV}, x,k}$，以及横向位置和速度 $y_{\text{TV}, k}, v_{\text{TV}, y,k}$。线性目标车辆模型可用于传播不确定性，这对于后续章节中的模型预测控制方法是必要的。本文所使用的TV模型仅为一种可行选择，也可采用其他线性目标车辆预测模型。

系统和输入矩阵是
$$
A= \begin{bmatrix}
1 & T & 0 & 0 \
0 & 1 & 0 & 0 \
0 & 0 & 1 & T \
0 & 0 & 0 & 1
\end{bmatrix}, \quad
B= \begin{bmatrix}
0.5T^2 & 0 \
T & 0 \
0 & 0.5T^2 \
0 & T
\end{bmatrix} \quad (10)
$$

采样时间为 $T$。目标车辆输入由反馈控制器 $\tilde{u} {\text{TV}, k}$ 和输入上的扰动组成，该扰动被假设为独立同分布的扰动向量 $w {\text{TV}, k}$。该设定假设目标车辆在跟踪给定参考轨迹的同时允许存在偏差。目标车辆反馈控制器由下式给出
$$
\tilde{u} {\text{TV}, k} = K(\xi {\text{TV}, k} - \xi_{\text{TV}, \text{ref},k}) \quad (11)
$$

与目标车辆参考$\xi_{\text{TV}, \text{ref},k}$。反馈矩阵$K$通过线性二次型调节器策略获得。如果目标车辆输入由(11)计算得出的目标车辆输入超过限值 $u_{\text{TV}, \max}=[a_{\max}, a_{y,\max}]^>$和 $u_{\text{TV}, \min}=[a_{\min}, a_{y,\min}]^>$，总结为 $U_{\text{TV}}$，将目标车辆输入限制在满足 $U_{\text{TV}}$的范围内。

我们假设 $w_{\text{TV}, k}$服从均值为零、协方差矩阵为 $\Sigma_{\text{TV}, w}$的高斯分布，记作 $w_{\text{TV}, k} \sim N(0,\Sigma_{\text{TV}, w})$。我们还考虑了交通车辆（TVs）状态测量中的传感器噪声，即
$$
\hat{\xi} {\text{TV}, 0}= \xi {\text{TV}, 0}+ w_{\text{sens}} \quad (12)
$$

其中$\hat{\xi} {\text{TV}, 0}$是电动汽车测得的电视初始状态。假设传感器噪声 $w {\text{sens}, 0}=[w_{\text{sens}, 0,x}, w_{\text{sens}, 0,vx}, w_{\text{sens}, 0,y}, w_{\text{sens}, 0,vy}]^>$为截断高斯噪声，其满足 $w_{\text{sens}, 0} \sim N(0,\Sigma_{\text{sens}})$ 和 $w_{\text{sens}, 0} \in W_{\text{sens}}$，其中 $W_{\text{sens}}$是一个紧致、凸且有界集合。

IV. 具有安全保证的随机模型预测控制

随机模型预测控制和容错轨迹规划各自具有独特的优势，即分别在不确定环境中实现高效轨迹和有保障的安全运动规划。下文提出了一种结合随机模型预测控制与 FTP的框架SMPC+FTP，该框架充分利用两种方法的优势，为自动驾驶车辆规划高效且安全的轨迹。本节介绍 SMPC+FTP框架的设置，并给出递归可行性的证明。

A. 随机模型预测控制+FTP 方法

在介绍SMPC+FTP方法之前，我们需要定义安全的自车状态以及由单个输入组成的安全输入序列 $U_{\text{safe}}=[u_{\text{safe},0},u_{\text{safe},1},…,u_{\text{safe},m}]^>$ with $m+1$。注意，$m$与模型预测控制预测时域无直接关系。

定义1（安全状态） 当自车完全位于一个车道内时，若其处于无横向车辆运动状态，即 $\varphi= 0$，且自车速度低于同一车道前方目标车辆的速度（或自车速度为零），则认为该自车状态是安全的。安全状态的集合用 $\Xi_{\text{safe}}$表示。

定义2（安全输入序列） 如果连续应用$U_{\text{safe}}$的所有元素能够产生一个避免碰撞并最终达到零速度的状态轨迹，则称输入序列 $U_{\text{safe}}$是安全的。

安全状态和安全输入序列的定义导致了对交通车辆（ TVs）的假设。

假设1（交通规则） 目标车辆遵守交通规则。

假设2(VehicleDeceleration) 自车减速度的最大绝对值至少与目标车辆减速度的最大绝对值相等。

给定一个安全的自车状态，存在一个由减速度和零转向组成的安全输入序列$U_{\text{safe}}$，可使自车在当前自车车道内达到零速度状态，即在 $x$方向和 $y$方向上均为零速度。这是基于假设1和假设2。交通车辆（TVs）违反交通规则的行为无法通过任何预测可靠地考虑，且必须采用减速度假设以避免与前方刹车的目标车辆发生碰撞。

下文中，在时间步 $h$和预测步长 $k= 0$的当前自车状态表示为 $\xi_0$。我们在一个最优控制问题中省略了对当前时间步 $h$的显式标注。在每个OCP的初始化阶段，电动汽车已知当前自车状态 $\xi_0$和当前目标车辆状态 $\xi_{\text{TV}, 0}$。此外，还存在一个来自前一时间步求解的SMPC+FTP问题的安全输入序列 $U_{\text{safe}}$。之后，我们将重点研究如何在给定当前时间步安全输入序列的情况下，获得下一时间步SMPC+FTP迭代的安全输入序列。

SMPC+FTP方法由两部分组成，即SMPC和FTP，也就是说在每个时间步长上分别求解一个SMPC最优控制问题和一个FTP最优控制问题。其基本思想是：只有在基于第一个SMPC输入$u_{\text{SMPC},0}$能够找到一条故障安全轨迹的情况下，才允许应用SMPC输入序列 $U_{\text{SMPC}}= [u_{\text{SMPC},0} ,…,u_{\text{SMPC},N−1}]^>$中的第一个输入$u_{\text{SMPC},0}$。与常规的SMPC方法相比，该方法保证了应用乐观的SMPC输入$u_{\text{SMPC},0}$不会导致不安全行为。算法框架如图1所示。

1) SMPC : 在SMPC+FTP的第一部分中，会在有限时域 $N_{\text{SMPC}}$上求解一个SMPC问题，得到输入序列 $U_{\text{SMPC}} =[u_{\text{SMPC},0} ,…,u_{\text{SMPC},N_{\text{SMPC}} − 1}]^>$。该SMPC优化考虑了由于其他交通参与者（即目标车辆）带来的不确定环境和约束。碰撞约束基于目标车辆的概率预测以概率约束的形式进行表述。因此，规划的SMPC轨迹提供了一个高效且乐观的未来电动汽车的轨迹，因为在最坏情况场景下无需避免与交通车辆（ TVs）发生碰撞。

2) FTP ：SMPC+FTP 的第二部分基于 FTP，以确保规划的电动汽车轨迹保持安全。首先进行最坏情况交通车辆预测。然后，在有限时域 $N_{\text{FTP}}$ 上求解一个故障安全MPC问题，得到输入序列 $U_{\text{FTP}}=[u_{\text{FTP},0},…,u_{\text{FTP},N_{\text{FTP}}−1}]^>$。该故障安全轨迹必须避免与最坏情况交通车辆预测发生碰撞，并且在应用完整的故障安全输入序列 $U_{\text{FTP}}$后，终端状态 $\xi_{N_{\text{FTP}}}$ 必须根据定义1成为一个安全状态。确切的 FTP 形式取决于 SMPC最优控制问题的可行性。

a) 可行SMPC（SMPC模式） ：如果SMPC最优控制问题产生一个解，则使用FTP来判断应用第一个SMPC输入$u_{\text{SMPC},0}$是否安全。因此，从应用第一个SMPC输入$u_{\text{SMPC},0}$后得到的电动汽车状态出发构建一个FTP最优控制问题，即初始FTP最优控制问题状态为
$$
\xi’ 0= f(\xi_0,u {\text{SMPC},0}) \quad (13)
$$

根据(6)，使用 $f(\xi_0,u_{\text{SMPC},0})$。如果可行，FTP最优控制问题将基于 $\xi’ 0$生成一个故障安全输入序列$U’ {\text{FTP}}$。因此，如图1中的蓝色路径所示，SMPC输入序列的第一个元素$u_{\text{SMPC},0}$被安全地应用。由此得到的新安全输入序列为
$$
U_{\text{safe}}=[U’ {\text{FTP}}, U {\text{brake}}] \quad (14a)
$$
$$
U_{\text{brake}}=[[a_{\min} 0],[a_{\min} 0],…] \quad (14b)
$$

其中 $a_{\min}$为最大减速度， $U_{\text{brake}}$为将电动汽车制动至静止状态的制动序列。安全输入序列 $U_{\text{safe}}$确保在施加完整的故障安全输入序列 $U’ {\text{FTP}}$后系统处于安全状态，随后启动制动以达到零速度。注意 $a {\min}$仅在 $U_{\text{brake}}$中应用直至达到静止状态，之后不再施加减速度。

b) 不可行SMPC（FTP模式） ：如果SMPC最优控制问题不可行，则以初始状态 $\xi_0$求解FTP最优控制问题。若找到 FTP解 $U_{\text{FTP}}$，则应用 $U_{\text{FTP}}$的第一个元素，即$u_{\text{FTP},0}$，如图1中的橙色路径所示。更新后的安全输入序列由此得出
$$
U_{\text{safe}}=[U_{\text{FTP},1:N_{\text{FTP}}}, U_{\text{brake}}] \quad (15)
$$

根据(14b)，其中 $U_{\text{brake}}$
$$
U_{\text{FTP},1:N_{\text{FTP}}} =[u_{\text{FTP},1},…,u_{\text{FTP},N_{\text{FTP}} −1}] \quad (16)
$$

由 $U_{\text{FTP}}$的所有输入元素组成，但不包括第一个输入 $u_{\text{FTP},0}$。

3) 不可行的FTP（备份模式） ：当FTP最优控制问题不可行时，当前时间步长$h$不会生成新的输入。然而，根据定义，在前一时间步 $h−1$获得的安全输入序列对当前时间步长 $h$仍然是安全的。因此，当FTP最优控制问题不存在解时，将应用仍然有效的安全输入序列$U_{\text{safe}}$的第一个元素，该元素表示为$u_{\text{safe},0}$。此过程在图1中以红色标出。持续应用 $U_{\text{safe}}$ 中的元素将根据定义2产生一条安全轨迹。如果FTP最优控制问题保持在连续时间步长内不可行的情况下，可能会应用单个安全输入序列的多个后续输入元素，直到FTP最优控制问题再次变为可行。

该过程要求在每次SMPC FTP迭代后进行转换$U_{\text{safe}}$，其中FTP最优控制问题不可行，即如果应用了 $U_{\text{safe}}$的第一个输入元素$u_{\text{safe},0}$。通过以下方式获得转换后的更新输入序列
$$
U_{\leftarrow \text{safe}}= U_{\text{safe}}[0_m I_m]=[u_{\text{safe},1},u_{\text{safe},2},…,u_{\text{safe},m}] \quad (17)
$$

其中 $U_{\text{safe}} \in \mathbb{R}^{2\times(m+1)}$、 $I_m \in \mathbb{R}^{m\times m}$为单位矩阵以及$0_m \in \mathbb{R}^{1\times m}$。转换后的安全输入序列 $U_{\leftarrow \text{safe}}$包含 $U_{\text{safe}}$的所有元素，除了已应用的输入$u_{\text{safe},0}$。在SMPC+FTP迭代结束时，通过选择来更新安全输入序列
$$
U_{\text{safe}}= U_{\leftarrow \text{safe}}, \quad (18)
$$

用于初始化下一次SMPC+FTP迭代的安全输入序列。

4) 随机模型预测控制总结+FTP : 在SMPC+FTP方法中，考虑了四种情况。这些情况总结如下。

a) SMPC和FTP可行（SMPC模式） ：应用第一个 SMPC输入$u_{\text{SMPC},0}$，并根据(14)获得新的安全输入序列$U_{\text{safe}}$。

b) SMPC不可行且FTP可行（FTP模式） ：应用第一个 FTP输入$u_{\text{FTP},0}$，并根据(15)获得新的安全输入序列$U_{\text{safe}}$。

c) SMPC可行且FTP不可行（备份模式） ：未获得新的输入序列。应用安全输入序列的首个输入元素$u_{\text{safe},0}$。该安全输入序列 $U_{\text{safe}}$在下一个时间步长仍然有效，并根据(18)进行更新。

d) SMPC 不可行且 FTP 不可行（备份模式） ：与之前的情况相同，未获得新的输入序列。应用输入 $u_{\text{safe},0}$，并根据 (18) 生成 $U_{\text{safe}}$ 用于下一个时间步长。

遵循此流程，在常规情况下应用SMPC输入，从而实现高效性能，而FTP则为所有可能情况（包括罕见事件）提供安全性保障。

B. 递归可行性

各种SMPC算法的一个缺点是无法保证最优控制问题的递归可行性。在本节中，证明了SMPC+FTP方法的递归可行性，即如果优化问题在时间步长$h$可以求解，则对于所有 $h \in \mathbb{N}$，在时间步长 $h+ 1$也可以求解。在本节中，需要对时间步长 $h$进行标记。在时间步长 $h$更新的安全输入序列记为$U_{\text{safe},h}$。

定义3（安全可行轨迹） 假设存在一个安全集 $\Xi_{\text{safe}}$，并设 $\Xi_f$为一个控制不变集。令 $\chi_{U_h}^h =[ \xi_h ,…, \xi_{h+N} ]$表示在时间步长 $h$时从初始状态 $\xi_h$ 出发，通过应用输入序列 $U_h =[u_h ,…,u_{h+N − 1}]$获得的具有 $N$个轨迹步长的轨迹，其中 $\xi_{h+1}= f(\xi_h,u_h)$。那么，通向集合 $\Xi_f$的安全可行轨迹集 $\Gamma_h$定义为
$$
\Gamma_h = \left{ \chi_h = U_h \mid \xi_{h+i} \in \Xi_{\text{safe}}, i \in {0,…, N}, \xi_{h+N} \in \Xi_f \right}. \quad (19)
$$

一条安全可行轨迹满足由$\Xi_{\text{safe}}$给出的所有约束，并终止于控制不变集 $\Xi_f$。

假设3(System Model) 自车系统模型(3)和(6)对应于实际系统的动力学。目标车辆模型(9)是对实际目标车辆动力学的过近似。

这里，过近似意味着使用TV模型可达的可能状态包含使用真实TV动力学得到的所有可能状态。

假设4（初始安全输入序列） 在初始时间步长 $h= 0$，初始自车状态是安全的，并且存在一个已知的初始安全输入序列 $U_{\text{safe},\text{init}}$，使得 $\chi_{U_{\text{safe}, \text{init}}}^0$ 是一条安全可行轨迹，即 $\chi_{U_{\text{safe}, \text{init}}}^0 \in \Gamma_0$。

我们现在展示所提出方法的递归可行性。

定理1. 假设3和假设4成立。那么，对于SMPC+FTP方法，存在一条可行轨迹$\chi_{U_h}^h \in \Gamma_h$，可保证在所有时间步长 $h \in \mathbb{N}$上都是安全的。

证明。 该证明的推导见附录A。

交通车辆（TVs）的最坏情况行为取决于交通规则。因此，如假设1所述，只有当周围的交通车辆（TVs）遵守基本交通规则时，才能保证SMPC+FTP方法的安全性与递归可行性。然而，证明定理1并不需要任何具体的交通规则。

V. 轨迹规划算法

两个模型预测控制最优控制问题（SMPC和FTP）被独立求解。接下来将分别推导对应的最优控制问题。

A. 随机模型预测控制

SMPC通过求解带有机会约束的最优控制问题（OCP）来考虑目标车辆不确定性，并依赖于一个风险因子 β。首先，在每个预测的TV状态周围定义一个安全区域，该区域考虑了电动汽车和TV的形状。然后，根据预定义的风险参数，扩大该安全区域以考虑TV不确定性。最终，根据电动汽车与TV的相对位置，为每辆TV生成一个线性约束。

1) 确定性目标车辆预测 ：对于SMPC，采用一种简单的TV预测方法，表示最可能的TV行为，即 $w_{\text{TV}, k} = 0$，也就是 $u_{\text{TV}, k} = \tilde{u} {\text{TV}, k}$。假设当前TV操纵在预测时域 $N {\text{SMPC}}$内持续进行。因此，应用目标车辆模型(9)，其中TV参考$\xi_{\text{TV}, \text{ref}, k}$取决于当前TV操纵。参考速度$v_{\text{TV}, x,\text{ref},k}$设置为当前TV速度$v_{\text{TV}, x,0}$。TV参考横向速度选择为$v_{\text{TV}, y,\text{ref},k} = 0$。参考横向位置$y_{\text{TV}, \text{ref},k}$为当前目标车辆车道中心。如果TV形状的一部分位于相邻车道，并且横向速度使TV朝向该相邻车道移动，则选择新的参考车道。

2) 目标车辆安全区域 ：通过确保电动汽车与目标车辆之间必要的距离来避免与交通车辆（TVs）发生碰撞。在此，基于直线高速公路道路，在TV周围定义一个长度为 $a_r$、宽度为 $b_r$的安全矩形。如果车辆中心之间的距离至少为车辆长度 $l_{\text{veh}}$和宽度 $w_{\text{veh}}$，则车辆形状不会相交。对于安全矩形的宽度，这得出
$$
b_r = w_{\text{veh}} + \varepsilon_{\text{safe}} \quad (20)
$$
其中 $\varepsilon_{\text{safe}}$是可能的额外安全性裕度。

计算安全矩形长度 $a_r$需要一个与速度相关的部分 $\tilde{a} r(\xi, \xi {\text{TV}})$，用于补偿电动汽车与电视之间的潜在速度差，从而导致
$$
a_r = l_{\text{veh}} + \varepsilon_{\text{safe}} + \tilde{a} r(\xi, \xi {\text{TV}}). \quad (21)
$$

速度相关的部分 $\tilde{a} r$需要考虑电动汽车和电视在同时启动最大制动时行驶距离的差异。它通过以下方式获得
$$
\tilde{a}_r(\xi, \xi {\text{TV}}) = -\frac{1}{2a_{\min}} \max{0, (v^2 - (v_{\text{TV}, x})^2)} \quad (22)
$$
其中最大值算子确保安全矩形的长度对于 $v_{\text{TV}, x} > v_{\text{EV}}$不会减小。

对于SMPC最优控制问题，安全矩形是基于第五节-A1部分描述的TV预测 $\xi_{\text{TV}, k}$，在预测时间步长 $k$上计算的。然而，在依赖于速度的部分 $\tilde{a} r$中仅考虑了电动汽车的初始状态$\xi_0$。这是为了生成线性安全约束所必需的。得到的安全矩形参数为
$$
b {r,k} = w_{\text{veh}} + \varepsilon_{\text{safe}} \quad (23a)
$$
$$
a_{r,k} = l_{\text{veh}} + \varepsilon_{\text{safe}} + \tilde{a} r(\xi_0, \xi {\text{TV}, k}). \quad (23b)
$$

3) 机会约束重构 ：由(23)给出的目标车辆安全矩形未考虑目标车辆不确定性。接下来，根据目标车辆不确定性和风险参数 β来扩大安全矩形。类似于(1d)的机会约束表示为
$$
\Pr(\xi_k \in \Xi’ {k,\text{safe}}(w {\text{TV}, k})) \geq \beta \quad (24)
$$
其中，电动汽车状态的安全集$\Xi’ {k,\text{safe}}(w {\text{TV}, k})$取决于先前定义的安全矩形参数(23)以及目标车辆不确定性 $w_{\text{TV}, k}$。

机会约束(24)无法直接求解。我们借鉴其他SMPC方法[6],[30]，为此概率表达式推导出一个确定性近似。

根据(9)，TV状态遵循
$$
\xi_{\text{TV}, k+1} = A\xi_{\text{TV}, k} + BK(\xi_{\text{TV}, k} - \xi_{\text{TV}, \text{ref},k}) + Bw_{\text{TV}, k}, \quad (25)
$$
而预测的TV状态由以下给出
$$
\hat{\xi} {\text{TV}, k+1} = A\hat{\xi} {\text{TV}, k} + BK(\hat{\xi} {\text{TV}, k} - \xi {\text{TV}, \text{ref},k}), \quad (26)
$$
产生预测误差
$$
e_k = \hat{\xi} {\text{TV}, k} - \xi {\text{TV}, k}. \quad (27)
$$

电视预测（26）现在被分为确定性部分和随机部分
$$
\hat{\xi} {\text{TV}, k+1} = \xi {\text{TV}, k+1} + (A + BK)e_k - Bw_{\text{TV}, k} = \xi_{\text{TV}, k+1} + e_{k+1} \quad (28)
$$
从而导致预测误差的更新
$$
e_{k+1} = (A + BK)e_k - Bw_{\text{TV}, k}. \quad (29)
$$

根据(12)中的传感器噪声 $w_{\text{sens}, 0}$，初始误差服从 $e_0 \sim N(0,\Sigma_{e_0})$，其中$\Sigma_{e_0}$。此可以递归计算预测误差协方差矩阵$\Sigma_{e_k}$，从而得到
$$
\Sigma_{e_{k+1}} = B\Sigma_{\text{TV}, w}B^\top + (A + BK)\Sigma_{e_k}(A + BK)^\top. \quad (30)
$$

基于预测误差协方差矩阵$\Sigma_{e_k}$，扩大TV安全矩形。给定预定义的SMPC风险参数 β，目标是找到一个围绕预测的TV状态的区域，使得该区域以概率 β包含真实TV状态。

由于TV安全矩形仅考虑位置，我们定义具有简化协方差矩阵的简化误差 $\tilde{e} k = [e {x,k}, e_{y,k}]^\top$
$$
\tilde{\Sigma} {e_k} = \text{diag}(\sigma^2 {x,k}, \sigma^2_{y,k}) \quad (31)
$$
其方差为$\sigma^2_{x,k}$和$\sigma^2_{y,k}$，分别对应纵向和横向TV位置，即$\Sigma_{e_k}$的第一个和第三个对角线元素。现在使用简化误差协方差矩阵$\tilde{\Sigma}_{e_k}$来扩大安全矩形，以考虑不确定性。

由 $\Sigma_{e_k}$描述的双变量高斯分布，其均值为$\mu = [\mu_x, \mu_y]^\top = 0$，包含纵向和横向位置的独立随机变量。这使得我们能够在预测的TV状态均值周围找到一个置信区域，该区域由包围最高密度区域的椭球等值线界定，如图2所示。目标是找到一条等值线，使其以风险参数 β对应的概率包含预测误差。该等值线椭圆方程表示为
$$
(\tilde{e} k - \mu)^\top (\tilde{\Sigma} {e_k})^{-1} (\tilde{e} k - \mu) = \kappa \quad (32a)
$$
$$
\frac{(e {x,k} - \mu_x)^2}{\sigma^2_{x,k}} + \frac{(e_{y,k} - \mu_y)^2}{\sigma^2_{y,k}} = \kappa \quad (32b)
$$
容差水平为 $\kappa$。容差水平 $\kappa$取决于风险参数 β，并表示为确保预测误差低于概率 β所需的约束收紧程度。容差水平 $\kappa$基于自由度为 $n$的卡方分布, $n$的累积分布函数 $F$确定。在此情况下， $n= 2$作为由两个元素组成的简化误差 $\tilde{e}_k$。给定风险参数 β以及卡方分布chi-square distribution $\chi^2_2$ 的分位函数 $F^{-1}$，可得
$$
\kappa = F^{-1}(\beta, 2), \quad (33)
$$

示意图0

这确保了真实TV状态位于等值线内的概率为 β · 100%。

椭圆的长半轴和短半轴由此给出
$$
e_{x,k,\kappa} = \sigma_{x,k}\sqrt{\kappa} \quad (34a)
$$
$$
e_{y,k,\kappa} = \sigma_{y,k}\sqrt{\kappa}. \quad (34b)
$$

尽管根据(32)椭圆描述了所需的置信区域，但本文所采用的约束生成方法需要矩形目标车辆安全区域。因此，我们通过矩形对椭圆进行过近似。为了将这种不确定性考虑纳入(23)中的矩形参数$ar,k$和$br,k$，基于椭圆长半轴$ex,k,\kappa$和短半轴$ex,k,\kappa$来增大矩形参数，从而得到
$$
b_{r,k} = w_{\text{veh}} + \varepsilon_{\text{safe}} + e_{x,k,\kappa} \quad (35a)
$$
$$
a_{r,k} = l_{\text{veh}} + \varepsilon_{\text{safe}} + \tilde{a} r(\xi_0, \xi {\text{TV}, k}) + e_{y,k,\kappa}. \quad (35b)
$$

更新后的安全矩形参数现在用于生成SMPC最优控制问题的安全性约束。

4) SMPC约束生成 ：针对每个TV的安全矩形，可以为每个预测步长和每个TV定义避免碰撞的线性约束。每个线性约束的形式为
$$
0 \geq q_y(\xi_0, \xi_{\text{TV}, k}) y_k + q_x(\xi_0, \xi_{\text{TV}, k}) x_k + q_t(\xi_0, \xi_{\text{TV}, k}) \quad (36)
$$
其中 $q_y$和 $q_x$是电动汽车状态 $y_k$和$x_k$的系数， $q_t$是截距。线性约束的系数 $q_y$、 $q_x$和 $q_t$取决于当前自车状态 $\xi_0$和预测的TV状态均值 $\xi_{\text{TV}, k}$。这导致了多种约束生成情况，扩展了[7]中的情况。

根据OCP开始时的初始车辆配置（即 $k= 0$）来区分不同情况。在针对特定情况于预测步长 $k$构建约束(36)时，仅考虑初始电动汽车状态 $\xi_0$，以允许生成线性约束，如第五章A节所述。

我们简要讨论了所考虑的约束情况的简化概述，总结见表I。示例情况如图3所示。(36)式中的各种情况、要求及约束参数 $q_x$、 $q_y$、 $q_t$的完整概述见扩展版本[10]。

总之，当电动汽车与电视之间的纵向距离大于 $r_{\text{lar}}$时，不会产生约束（情况A）。如果电动汽车距离电视足够近（纵向距离小于 $r_{\text{close}}$），则可通过采用倾斜约束实现超车（情况D和E）。如果电视位于后方

示意图1

5) SMPC 最优控制问题 ：在定义了安全约束后，用于替代SMPC问题的确定性最优控制问题由下式给出
$$
V^* = \min_U \sum_{k=1}^{N_{\text{SMPC}}} |\Delta\xi_k| Q + |u {k-1}| R + |\Delta u {k-1}| S \quad (37a)
$$
$$
\text{s.t. } \xi {k+1} = f_d(\xi_0, \xi_k, u_k) \quad (37b)
$$
$$
\xi_{\text{TV}, k+1} = A\xi_{\text{TV}, k} + B\tilde{u} {\text{TV}} \quad (37c)
$$
$$
u_k \in U, \xi {k+1} \in \Xi \quad \forall k \in {0,…, N_{\text{SMPC}} - 1} \quad (37d)
$$
$$
0 \geq q_y(\xi_0, \xi_{\text{TV}, k}) y_k + q_x(\xi_0, \xi_{\text{TV}, k}) x_k + q_t(\xi_0, \xi_{\text{TV}, k})
\quad \forall k \in {0,…, N_{\text{SMPC}}} \quad (37e)
$$

其中$\Delta\xi_k = \xi_k - \xi_{k,\text{ref}}$，电动汽车参考状态$\xi_{k,\text{ref}}$，以及根据(6)确定的线性函数 $f_d$。对于输入差值$\Delta u$，将 $u_{-1}$设为前一时间步应用的输入。成本函数求和限值经过转换以包含 $\xi_N$的终端代价。权重矩阵由 $Q$、 $S$和$R$给出。我们考虑根据 (7)设定的恒定输入约束 $U$，以及根据(8)设定的状态约束 $\Xi$。

所得的SMPC最优控制问题(37)是一个带有线性约束的二次规划，通过第五章A3节中描述的机会约束重构方法来考虑不确定性。该最优控制问题可以高效求解，其中为获得线性约束(37e)所需的主要计算步骤在优化开始前完成。

B. 故障安全轨迹规划

尽管SMPC算法仅考虑部分目标车辆不确定性以规划一条乐观轨迹，但备用FTP算法需要考虑最坏情况的不确定性实现。这是基于可达性分析实现的。首先，确定最坏情况电视占用预测。然后，生成线性安全约束。最终，在给定安全不变终端集的情况下，求解FTP最优控制问题。

1) 目标车辆占用预测 ：与随机模型预测控制算法类似，为每个目标车辆定义一个矩形安全区域。然而，对于 FTP需要确定最大可达区域。首先，根据假设1，需要定义目标车辆所遵守的某些交通规则：
- 道路边界适用。
- 禁止负速度。
- 必须避免与电视正前方同一车道内的车辆发生碰撞。
- 仅允许一次变道（在预测范围内）。
- 如果电视速度低于预定义的最小变道速度$v_{\text{LC},\text{min}}$，则不允许变道。
- 如果与新车道上车辆的距离过小，则不允许变道。

由于假设目标车辆运动为线性动力学，因此借鉴[8],[19],[22]的方法，利用目标车辆输入的最小值和最大值来确定最大可达集。在预测步长 $k$处目标车辆所有可能到达的位置集合称为可达集 $R_{\text{TV}, k}$，包括目标车辆及其形状。当将 $R_{\text{TV}, k}$称作目标车辆的可达集时，我们额外扩大该集合以考虑电动汽车的形状。这是必要的，因为后续将使用集合 $R_{\text{TV}, k}$通过保持电动汽车中心在 $R_{\text{TV}, k}$之外来避免碰撞。给定从初始状态$\hat{\xi} {\text{TV}, 0}$开始并施加输入序列 $U$求得的目标车辆动力学(9)的解$\zeta(\hat{\xi} {\text{TV}, 0}, U)$，我们定义该可达集
$$
R_{\text{TV}, k} = \left{ \zeta(\hat{\xi} {\text{TV}, 0}, U) \mid U^{(i)} \in U {\text{TV}} \forall i \in {0,…, k - 1}, \hat{\xi} {\text{TV}, 0} \in \Xi {\text{TV}, 0} \right}. \quad (38)
$$

可达集 $R_{\text{TV}, k}$的初始状态不是TV状态$\xi_{\text{TV}, 0}$，而是取决于传感器不确定性以及TV和电动汽车的形状。该初始集由以下给出
$$
\Xi_{\text{TV}, 0} = \left{ \hat{\xi} {\text{TV}, 0} \mid \xi {\text{TV}, 0} + \min{w_{\text{sens}, 0}} - [l_{\text{veh}}, 0, w_{\text{veh}}, 0]^\top \leq \hat{\xi} {\text{TV}, 0}, \hat{\xi} {\text{TV}, 0} \leq \xi_{\text{TV}} + \max{w_{\text{sens}, 0}} + [l_{\text{veh}}, 0, w_{\text{veh}}, 0]^\top \right}. \quad (39)
$$

由于我们假设采用线性目标车辆预测模型，因此在遵守交通规则的前提下，通过应用最大和最小输入 $u_{\text{TV}} \in U_{\text{TV}}$来计算预测步长 $k> 0$的可达集$R_{\text{TV}, k}$。

可达集仅在离散时间步上计算。为了考虑连续系统，最终的可达集 $R_{\text{TV}, k}$通过构建覆盖两个连续预测步长的矩形凸包获得，即
$$
R_{\text{TV}, k} = \text{conv}{R_{\text{TV}, k-1}, R_{\text{TV}, k}} \quad (40)
$$
其中$\text{conv}$表示凸包操作。

一种特殊情况是，当电视位于电动汽车后方的同一车道时。电视不得与同一车道上的电动汽车发生碰撞，但允许电视变道以便超越电动汽车。此处通过以下方式处理这一特殊情况：三个占位电视可达集描述了可能的电视行为。第一个占位电视可达集位于电动汽车车道内，以避免与电动汽车发生碰撞。另外两个占位电视可达集覆盖电动汽车左侧和右侧允许的相邻车道，表示潜在电视变道的可达集。

2) FTP 约束生成 ：一旦确定了每辆电视的可达集$R_{\text{TV}, k}$，则生成线性约束。我们再次考虑电动汽车和电视位置变化的不同情况。这些情况与第V-A4节中的类似，但有一些变化，如表I所示。FTP情况用星号表示。示例性的FTP情况如图4所示。同样，扩展版本中提供了FTP情况的完整概述 [10]。

我们简要讨论与SMPC约束生成情况的主要差异。在情况D∗和E∗中，由于存在纵向约束，不发起超车。如果交通车辆（TV）位于电动汽车（EV）后方，我们通过引入占位交通车辆（TVs）来考虑TV可能的变道（情况C∗和J∗）。此处使用 $r_{\text{FTP}, \text{close}}$代替 $r_{\text{close}}$。

总体而言，为FTP生成的约束比SMPC更加保守。这是由于FTP的目标是找到一条最终到达安全状态的轨迹。如果激励FTP规划超车操纵，则会使问题复杂化。有关在 FTP最优控制问题中寻找安全终态的细节将在下文给出。

3) 安全不变终端集 ：除了常规的安全约束外，还需要一个安全不变终端集，以确保在有限的模型预测控制预测时域之后电动汽车输入的安全性。故障保护规划输入的设计可保证其在预测时域内保持安全。然而，在 $N_{\text{FTP}}$输入被施加且未获得新的故障保护规划解后，必须采取紧急策略使车辆进入静止状态。这通过制动实现，同时保持转向角 $\delta= 0$恒定，如公式(14)和(15)所述。因此，故障保护规划最优控制问题的终端状态需要满足某些要求。首先，车辆方向必须与道路对齐，即 $\varphi= 0$。这保证了在制动并保持转向角 $\delta= 0$恒定的情况下，电动汽车仍保持在其当前车道内。其次，电动汽车与前方电视之间的距离必须足够大，以确保当两车同时开始最大减速度时不会发生碰撞。这一点通过以下方式予以考虑：
$$
x_N \leq x_{\text{TV}, N} - \Delta s_{N_{\text{FTP}}, \text{min}} \quad (41a)
$$
$$
v_N \leq v_{N_{\text{FTP}}, \text{max}} \quad (41b)
$$
其中$v_{N_{\text{FTP}}, \text{max}} = v_{\text{TV}, N_{\text{FTP}}, \text{min}} - \sqrt{2\Delta s_{N_{\text{FTP}}, \text{min}} a_{x,\text{min}}} \quad (42)$
其中$v_{\text{TV}, N_{\text{FTP}}, \text{min}}$为预测的TV纵向速度最低值。(41)和(42)共同确保最小终端安全距离$\Delta s_{N_{\text{FTP}}, \text{min}}$足够大，使得在电动汽车最大速度$v_{N_{\text{FTP}}, \text{max}}$下，电动汽车的最大减速度能够保证对 $k> N_{\text{FTP}}$的碰撞避免。这一较不直观的终端约束再次具有生成线性约束的优点。

4) FTP 最优控制问题 ：采用一个结构与(37)相似的最优控制问题用于FTP，从而得到
$$
V^* = \min_U \sum_{k=1}^{N_{\text{FTP}}} |\Delta\xi_k| Q + |u {k-1}| R + |\Delta u {k-1}| S \quad (43a)
$$
$$
\text{s.t. } \xi {k+1} = f_d(\xi_0, \xi_k, u_k) \quad (43b)
$$
$$
u_k \in U, \xi_{k+1} \in \Xi \quad \forall k \in {0,…, N_{\text{FTP}} - 1} \quad (43c)
$$
$$
0 \geq q_y(\xi_0, R_{\text{TV}, k}) y_k + q_x(\xi_0, R_{\text{TV}, k}) x_k + q_t(\xi_0, R_{\text{TV}, k}) \quad \forall k \in {0,…, N_{\text{FTP}}} \quad (43d)
$$
$$
x_N \leq x_{\text{TV}, N} - \Delta s_{N_{\text{FTP}}, \text{min}}, \quad v_N \leq v_{N_{\text{FTP}}, \text{max}} \quad (43e)
$$

根据(6)，使用线性函数 $f_d$。安全约束(37e)现在变更为约束(43d)，以考虑最坏情况下的交通车辆不确定性实现。与SMPC最优控制问题类似，(43)是一个具有线性约束的二次规划问题，可以高效求解。

VI. 结果

我们在不同设置下评估了所提出的SMPC+FTP算法。以下首先介绍仿真设置，然后在两个场景中对SMPC+FTP进行分析，并与SMPC方法和FTP方法进行比较。

A. 仿真设置

在本仿真部分中，我们分析了图5所示的场景。电动汽车位于三车道高速公路的右车道上。我们考虑了高速公路上围绕电动汽车的五辆交通车辆（TVs）。电动汽车的目标是安全高效地穿越交通。具体目标是在避免碰撞的同时，保持接近选定参考速度的速度。

我们考虑两种不同的场景：
1) 常规场景 ：所有交通车辆（TVs）保持其初始速度和车道。
2) 紧急场景 ：其中一辆交通车辆（TV5）执行紧急制动操纵。这导致TV4通过移动到中间车道来避让TV5。随后，TV1进行一次缓和制动操纵以应对可能的危险。最终，TV4再次移动到左车道以超越TV2。

第一种场景代表一种常规场景，其中电视行为没有意外情况。第二种场景涵盖了一种罕见情况，即一系列意外的电视动作导致自动驾驶电动汽车面临挑战性局面。

仿真在配备 AMD Ryzen 7 1700X 处理器的计算机上使用 Matlab 中的 fmincon求解器进行。算法基于 NMPC工具箱 [35]。以下介绍在不同仿真中保持不变的设置参数。所有量均以国际单位制单位给出。若上下文明确，则省略单位。

所有模型预测控制算法均采用采样时间 $T= 0.2$s，随机模型预测控制时域为 $N_{\text{SMPC}}= 10$，FTP时域为 $N_{\text{FTP}}= 10$。线性化离散时间电动汽车预测模型及约束条件遵循(6)-(8)，而电视预测模型由(9)-(11)给出。表II列出了其他主要仿真参数。车道边界由车道和车辆宽度确定。此外，安全性参数为
$$
\varepsilon_{\text{safe}}=0.01, r_{\text{lar}}= 200, r_{\text{close}}= 90, r_{\text{FTP}, \text{close}}= \max{10, |v_0 N_{\text{FTP}} T|}, v_{\text{LC}, \text{min}}= 10 \text{以及} \Delta s_{N,\text{min}}= 22.5.
$$

在所有场景中，电动汽车的初始参考轨迹设置为 $[d_{\text{ref}}, \varphi_{\text{ref}}, v_{\text{ref}}]=[0, 0, 27]$。虽然参考方向和速度在整个仿真过程中保持不变，但横向位置的电动汽车参考始终设置为当前电动汽车车道的中心。

尽管MPC最优控制问题使用线性化的离散时间预测模型(6)，但输入被应用于基于连续时间系统(3)的仿真。鉴于该仿真设置，我们现在研究各个场景并分析所提出的SMPC+FTP方法。

B. 常规高速公路场景

我们首先分析一个常规高速公路场景。车辆的初始状态如表III所示。图5中显示的五辆交通车辆（TVs）均保持其初始速度和车道，因此，$\xi_{\text{TV}, \text{ref},k} = \xi_{\text{TV}, 0}$。

下文将详细展示SMPC+FTP解，并与SMPC方法和FTP方法进行比较。

1) 随机模型预测控制+FTP : 将所提出的SMPC+FTP方法应用于常规高速公路场景，可实现电动汽车在交通中的高效行为。SMPC风险参数选择为 $\beta= 0.8$。输入和重要状态如图6所示，车辆运动情况如图7所示。

由于速度差异，电动汽车接近TV1。随后，电动汽车以 $\delta< 0.04$的适中转向角变道至中间车道。到达TV2后，电动汽车再次变道并最终超越TV2。车辆方向保持在有限范围内，即 $\varphi< 0.11$。通过-

示意图2

示意图3

示意图4

在整个场景中，电动汽车保持参考速度，且加速度输入较小。求解SMPC和FTP OCP的平均计算时间分别为0.11秒和0.15秒。使用其他求解器可能进一步降低计算量。如果应用于需要在线计算的场景，则认为计算时间超过要求的最优控制问题不可行。在此情况下，将使用先前计算的、仍然有效的安全输入序列。

接下来，我们将更详细地分析SMPC和FTP的约束条件。图8展示了时间步长 $h= 22$下SMPC的约束。对于与电动汽车处于同一车道的TV1，生成一个倾斜约束（情况 D）。在每个预测步长中，该约束连接电动汽车的初始形状与TV1在预测位置处的安全矩形。电动汽车的预测 SMPC轨迹保持在约束线之上。需要注意的是，仅相应的预测状态必须满足所示约束。更远未来的预测状态则根据更前方交通车辆预测位置对应的安全矩形，满足相应的约束。对于TV2，激活情况E，同样产生倾斜约束。TV4和 TV5均位于电动汽车左侧两车道，对应情况G和H，从而在交通车辆右侧形成水平约束。由于清晰性考虑，TV3未在图8中显示。

步骤 $h= 22$ 的 FTP 约束如图9所示。与SMPC约束相比，这些约束更保守。可达TV集向后延伸的程度大于向前延伸的程度，因为最大减速度大于最大

示意图5

示意图6

加速度。此外，还考虑了连续两个预测步长的可达集的凸包。TV1和TV2的约束分别根据情况D∗和B∗构建。TV4 和TV5的约束则根据情况H∗和G∗生成。当SMPC轨迹向中间车道移动以超车TV1时，FTP轨迹找到一种车辆运动，在最终预测步长中保持在当前车道，并与TV1保持 $\varphi= 0$的距离，即达到安全终态。由于FTP最优控制问题得到一个解，因此将规划的SMPC轨迹的第一个输入$u_{\text{SMPC},0}$应用。

2) 与SMPC和FTP的比较 ：在整个仿真过程中，SMPC和FTP最优控制问题始终保持可行。因此，SMPC输入始终被应用。在此常规场景下，仅应用 SMPC算法而不使用FTP将得到相同的结果。

与随机模型预测控制（SMPC）不同，仅应用FTP会产生不同的解。由于约束相比SMPC更为保守，电动汽车从不变道超车。如图9中的FTP预测所示，FTP约束使电动汽车保持在当前车道。

我们将使用以下指标来比较SMPC+FTP和FTP的性能。基于最优控制问题的成本函数，对整个仿真过程中的应用输入和产生的状态进行分析。
$$
J_{\text{sim}} = \sum_{k=1}^{N_{\text{sim}}} |\Delta\xi_k| Q + |u {k-1}| R + |\Delta u {k-1}| S \quad (44)
$$
随着仿真步数 $N {\text{sim}}$。

SMPC+FTP的总体成本为 $J_{\text{sim}}= 11.32$，而FTP总成本为 $J_{\text{sim}}= 4.03e4$。成本对比表明，SMPC+FTP方法比 FTP方法具有更高效的行为。在这种情况下，效率的提高源于将速度保持在接近参考速度的水平。

3) 风险参数变化 ：在之前讨论的仿真中，风险参数被设定为 $\beta= 0.8$。在此，我们简要分析不同风险参数对电动汽车性能的影响。所分析的风险参数范围从$\beta= 0.8$到 $\beta= 0.999$。根据(44)，各风险参数对应的总体仿真成本如表IV所示。仿真结果的总体成本表明，在此常规场景下，随机模型预测控制的行为和成本非常相似。然而，选择较大的风险参数可能在成本方面更有利，因为输入的变化更加平滑。在这五个示例中，电动汽车的行为几乎相同。

4) 变化仿真设置 ：在之前的分析中，仅考虑了一种车辆配置。为了表明SMPC+FTP方法适用于各种场景，我们进行了1000次仿真，每次仿真运行包含125个仿真步，并为每次仿真运行随机选择初始车辆位置和速度。电动汽车位于三条车道中的一个，即 $d_0 \in {0, 3.5, 7}$，初始纵向位置为 $s_0= 0$，速度为 $v= 27$。五辆交通车辆（TVs）被随机放置在三条车道中的任意一条上，初始纵向位置为 $x_{\text{TV}, 0} \in [-100, 200]$，恒定速度为 $v_{\text{TV}, x} \in [20, 32]$，以及恒定的 $v_{\text{TV}, y}= 0$。确保所有位于同一车道的车辆具有初始纵向距离 $\Delta x \geq 50$，并且交通车辆（TVs）的速度选择保证它们彼此之间不会发生碰撞。SMPC+FTP 方法成功处理了全部 1000 次仿真运行，且未发生碰撞。