基于FSGP-MPC考虑不确定性的轨迹规划方法

A Trajectory Planning Method Considering Uncertainties Based on FSGP-MPC

摘要: 在自动驾驶和机器人领域，局部轨迹规划中的不确定性处理是影响技术落地和安全的关键。本文针对外部环境障碍物预测结果的不确定性，提出了一种局部轨迹规划方法。该方法使用高斯过程模型预测障碍物信息，并基于高斯分布建模不确定性。通过量化障碍物预测结果的不确定性，并将其通过风险分配系数纳入控制系统，方法考虑了不确定性对轨迹规划的影响。同时，通过构建安全状态集和机会约束并结合模型预测控制(MPC)的动态约束处理能力，生成安全的局部轨迹。针对高斯过程模型样本有限的问题，本文结合高斯分布的矩集中界限理论收紧机会约束，确保即使在样本有限的情况下也能生成安全轨迹。

Abstract: In the fields of autonomous driving and robotics, handling uncertainties in local trajectory planning is crucial for the deployment and safety of these technologies. This paper proposes a local trajectory planning method that addresses the uncertainties in the predicted positions of external environmental obstacles. The method employs Gaussian Process (GP) models to predict obstacle information and uses Gaussian distributions to model uncertainties. By quantifying the uncertainties in obstacle predictions and incorporating them into the control system via risk allocation coefficients, the method accounts for the impact of these uncertainties on trajectory planning. Additionally, by constructing a safe state set and chance constraints, and leveraging the dynamic constraint handling capabilities of Model Predictive Control (MPC), the method generates safe local trajectories. To address the issue of limited samples in GP models, the paper tightens chance constraints using concentration bounds of Gaussian distribution moments, ensuring safe trajectory planning even with a finite number of samples.

1. 引言

在机器人和自动驾驶汽车等自主系统的实际应用中，一个主要挑战是处理环境中的不确定性。从自动驾驶汽车的角度来看，附近车辆，行人等外部环境中的障碍物行为在很大程度上是不可完全预测的。在机器人搜救中，目标和障碍物的位置也都是先验不确定的[1]。另外通过摄像头或激光雷达等传感器测量的数据集也是有限的。因此需要一种方法能够基于这些有限样本考虑潜在的不确定性并规划出确保安全的轨迹，同时允许实时计算。

过去在轨迹规划方面的工作考虑了两个来源的不确定性：(1) 自主智能体的动态模型和测量模型；(2) 环境中的障碍安全设置。Jha等人[2]考虑了多面体不确定障碍物，假设随机性为高斯分布并具有已知矩和确定性车辆模型。他们在假设高斯分布的情况下重新表述机会约束，然后将避障问题编码为混合整数形式处理。但是这种方法是在完全了解高斯矩的假设下工作的，而在现实中人们只能根据传感器测量或过去的数据来估计不确定性模型。Sessa等人[3]考虑了动态对噪声的非线性依赖，他们不假设噪声的分布而是考虑有足够数量的样品可用，使用场景方法为所设计的干扰反馈控制提供了概率保证。虽然他们的工作产生了在一般干扰模型下可以坚持安全的控制策略，但该方法需要大量的样本并且该方法的计算量太大无法实时实现。Edward等人[4]收集了大量的竞赛车之间相互超车等交互行为的数据训练高斯过程模型预测对手车辆的动态行为达到了很好的效果，但是这样的训练样本在现实环境中基本不可能完全获得。

处理预测外部环境的不确定性，高斯过程预测模型是一个很好的选择。高斯过程模型在机器学习和复杂动力学建模控制中得到了很好的认可[5]。主要的优点是GP模型也对它们自己的无知进行编码，提供了关于模型对于特定输入是否可靠的信息。通过高斯过程可以对函数进行预测并对其不确定性进行量化。高斯分布通常用于近似传感器噪声，并且在自动驾驶应用中越来越多地用于在预测行为时对标称原型机动的扰动进行建模其他车辆[6]。不确定环境下的轨迹规划在许多自主系统应用中出现，将环境的不确定性纳入优化框架的一种方法是通过机会约束规划[7]。不确定性的处理方法主要是随机规划，鲁棒优化和机会约束。与鲁棒优化和随机规划相比，该方法的优点在于允许较小的约束违反导致不太保守的轨迹，而且还可以处理未知或无界支持集的不确定性，并且计算的复杂性较低，灵活性更高。

模型预测控制(MPC)其固有的在线再规划能力使其适用于动态环境，另外由于其预测性能、对于约束处理、能实时调整、可多目标优化对于处理不确定问题有很好的能力。在[8]中认为最适合L4级自动驾驶的控制设计技术是MPC方法，因为它允许非线性模型，提供建模灵活性，并支持允许用户做出明确保证的约束。另外它还可以处理具有输入和状态约束的多输入多输出系统，同时考虑到车辆的非线性动力学。

本文使用高斯过程模型对于障碍物动态行为进行预测，并且利用高斯过程模型输出的预测结果得到相应的不确定性量化信息，构建了考虑动态不确定障碍物的安全状态集，并且通过机会约束引入到模型预测控制中。本文结合了高斯过程和模型预测控制在处理不确定性方面的优势，也考虑到样本分布的矩是未知的并且通过有限样本估计的现实情况，同时引用了[9]中的矩集中界限理论提出了一种考虑不确定性的轨迹规划方法。本文主要有以下贡献：把障碍物视为质点构建了系统安全状态集合，并将其对应的机会约束转化成二阶锥约束。利用高斯过程模型预测结果设计风险分配系数引入到约束中考虑了不确定性的传播。提出了一种考虑不确定性的轨迹规划方法FSGP-MPC，并验证了算法的有效性。

2. 矩集中界限理论

假设1. 样本数据的估计是符合以下条件：

$\widehat{\mu }=\frac{1}{N_s}{\displaystyle \sum _{i=1}^{N_s}d}$ (1)

$\widehat{\Sigma }=\frac{1}{N_s-1}{\displaystyle \sum _{i=1}^{N_s}\left(d_i-\widehat{\mu }\right){\left(d_i-\widehat{\mu }\right)}^{\top }}$ (2)

假设2. 参数$d^t$ 是服从高斯分布的$d^t~N\left({\mu }^t,{\Sigma }^t\right)$ ，其均值${\mu }^t$ ，方差${\Sigma }^t\succ 0$ 。

引理1. [9]在满足假设1和假设2的条件下，并且满足概率条件为$1-\beta$ ，$\beta \in \left(0,1\right)$ 有：

${\Vert \mu -\widehat{\mu }\Vert }_2\le r_1:=\sqrt{\frac{T_{n,N_s-1}^2\left(1-\beta \right)}{N_s{\lambda }_{\min }\left({\widehat{\Sigma }}^{-1}\right)}}$ (3)

其中$T_{n,N_s-1}^2\left(1-\beta \right)$ 表示的是霍特林T分布的第$1-\beta$ 个分位数，该分布参数$n$ 表示随机变量的维度和$N_s-1$ 表示自由度。常用于多变量假设检验，特别是用于检验两个或多个变量的均值是否相等。在本文中这个分布被用来制定估计样本均值和真实均值的误差边界。

引理2. [9]在假设1和假设2的条件下，定义变量$r_2$ ：

$r_2:=\max \left\{\left|1-\frac{N_s-1}{{\chi }_{N_s-1,1-\beta /2}^2}\right|,\left|1-\frac{N_s-1}{{\chi }_{N_s-1,\beta /2}^2}\right|\right\}$ (4)

其中${\chi }_{k,p}^2$ 具有$k$ 个自由度的卡方分布的第$p$ 个分位数，在概率为$1-\beta$ 条件下，$\beta \in \left(0,1\right)$ 有：

$\left|x^{\top }\left(\Sigma -\widehat{\Sigma }\right)x\right|\le x^{\top }\widehat{\Sigma }x{r}_2$ (5)

卡方分布是一种统计分布，常用于检验观察到的数据与预期数据之间的差异是否显著。该引理被用来建立关于样本估计协方差和真实的协方差的误差边界。这个值被用来确定在给定置信水平下，样本协方差与总体协方差之间的最大可能差异。

引理3. [9]在满足假设1和假设2的条件下，机会约束：

$\mathrm{Pr}\left(d^{\top }\tilde{x}+Mz>0\right)\ge 1-ϵ$ (6)

其中成立的概率最少是$1-2\beta$ ，则有：

${\Psi }^{-1}\left(1-ϵ\right)\sqrt{1+r_2}{\Vert {\widehat{\Sigma }}^{1/2}\tilde{x}\Vert }_2+r_1{\Vert \tilde{x}\Vert }_2\le {\widehat{\mu }}^{\top }\tilde{x}+Mz$ (7)

3. 不确定性界限

3.1. 安全状态集合

本文将障碍物视作一个质点，只考虑障碍物的x轴方向和y轴方向的位置信息。安全状态集合设计如下：

$X_t:=\left\{x_t\in {ℝ}^{n_x}:{\wedge }_{j=1}^{N_o}{\vee }_{i\in \left\{x,y\right\}}^{F_j}{a}_{ij}^t{}^{\top }{x}_t+b_{ij}^t>0\right\}$ (8)

其中$x_t$ 表示在时间$t$ 的系统状态。$N_o$ 是障碍物的总数，$j$ 是障碍物的索引。$F_j$ 是第$j$ 个障碍物的面，本文将障碍物看成质点只考虑他的横向和纵向只考虑两个面，$F_j$ 的取值是$x$ 或者$y$ 。$a_{ij}^t$ 是第$j$ 障碍物的第$i$ 个面在时间步$t$ 的法向量。那么$a_{ij}^t{}^{\top }{x}_t$ 的含义是系统状态$x$ 在障碍物面法向量方向上的投影(内积)。$b_{ij}^t$ 是第$j$ 个障碍的第$i$ 个面在时间步$t$ 的偏移量，也就是障碍物的状态位置信息与该面的法向量的内积。${\wedge }_{j=1}^{N_o}$ 表示逻辑与的关系，即所有障碍物都需要满足表达式后面的条件，${\vee }_{i\in \left\{x,y\right\}}^{F_j}$ 表示逻辑或的关系，即障碍物的各个面至少有一个面满足条件。$a_{ij}^t{}^{\top }{x}_t+b_{ij}^t$ 实际上计算了状态$x_t$ 相对于障碍物面的位置。如果这个值大于0，意味着系统位置处于在障碍物面的外部。如果等于0，意味着系统位置正好处于在障碍物面上。如果小于0，意味着系统位置处于在障碍物面的内部。

3.2. 机会约束转化

由于参数的随机性，考虑到所做决策在不利情况发生时可能不满足约束条件而采用的一种原则：即允许所做决策在一定程度上不满足约束条件，但该决策使约束条件成立的概率不小于某一置信水平。本文通过使用联合机会约束的方式，应用到模型预测控制的设计上从而实现安全约束。

$\mathrm{Pr}\left({\wedge }_{t=1}^N{x}_t\in {\mathcal{X}}_t\right)\ge 1-ϵ$ (9)

联合公式(8)和公式(9)得到：

$\mathcal{X}:=\left\{x\in {ℝ}^{n_x}:{\wedge }_{t=1}^N{\wedge }_{j=1}^{N_o}{\vee }_{i\in \left\{x,y\right\}}^{F_j}{a}_{ij}^t{}^{\top }{x}_t+d_{ij}^t>0\right\}$ (10)

联合概率机会约束是不能直接进行优化器求解计算的，因为需要进行处理。第一步我们使用sigmoid函数处理了公式(10)中的逻辑或关系，第二步使用布尔不等式[10]将联合概率分布给分解成单个概率约束，从而简化计算。

(1) 使用sigmoid函数可以将公式(10)中的逻辑或关系近似写成：

$\sigma (z)=\frac{1}{1+{\text{e}}^{-kz}}\in \left(0,1\right)$ (11)

${\vee }_{i\in \left\{x,y\right\}}^{F_j}{a}_{ij}^t{}^{\top }{x}_t+d_{ij}^t>0\Rightarrow {\displaystyle \sum _{i\in \text{{}x,y\text{}}}^{}\sigma \left(a_{ij}^t{}^{\top }{x}_t+d_{ij}^t\right)}>\text{1}$ (12)

因此上面的概率约束公式(9)可以写成：

$\mathrm{Pr}\left({\wedge }_{t=1}^N{\wedge }_{j=1}^{N_o}{\displaystyle \sum _{i\in \text{{}x,y\text{}}}^{}\sigma \left(a_{ij}^t{}^{\top }{x}_t+d_{ij}^t\right)}>\text{1}\right)\ge 1-ϵ$ (13)

(2) 依据布尔不等式将联合概率事件分成单个事件：

$\mathrm{Pr}\left({\displaystyle \underset{i}{\cup }{A}_i}\right)\le {\displaystyle \sum _i\mathrm{Pr}}\left(A_i\right)$ (14)

其含义是事件的并集概率是小于单个事件概率之和。因此$Pr\left({\displaystyle \underset{i}{\cup }{A}_i}\right)$ 等价于公式(12)，那么可以理解单个事件的表示：

${\wedge }_{t=1}^N{\wedge }_{j=1}^{N_o}\mathrm{Pr}\left({\displaystyle \sum _{i\in \text{{}x,y\text{}}}^{}\sigma \left(a_{ij}^t{}^{\top }{x}_t+d_{ij}^t\right)}>\text{1}\right)$ (15)

即某个时刻某个障碍物某个面并且是同时满足条件的事件为单个事件。因此可以得出：

$Pr\left({\wedge }_{i=1}^N{\wedge }_{j=1}^N{\displaystyle \sum _{i\in \text{{}x,y\text{}}}^{}\sigma \left(a_{ij}^t{}^{\top }{x}_t+d_{ij}^t\right)}>\text{1}\right)\le {\displaystyle \sum _{t=1}^N{\displaystyle \sum _{j=1}^{N_o}{\wedge }_{t=1}^N{\wedge }_{j=1}^{N_o}\mathrm{Pr}\left({\displaystyle \sum _{i\in \text{{}x,y\text{}}}^{}\sigma \left(a_{ij}^t{}^{\top }{x}_t+d_{ij}^t\right)}>\text{1}\right)}}$ (16)

就此联合概率转化为单个约束：

${\wedge }_{t=1}^N{\wedge }_{j=1}^{N_o}\mathrm{Pr}\left({\displaystyle \sum _{i\in \text{{}x,y\text{}}}^{}\sigma \left(a_{ij}^t{}^{\top }{x}_t+d_{ij}^t\right)}>1\right)\ge 1-{ϵ}_{ij}^t$ (17)

其中${\displaystyle \sum _{t=1}^N{\displaystyle \sum _{j=1}^{N_o}{\displaystyle \sum _{i=1}^{F_j}{ϵ}_{ij}^t}}}\le ϵ$ ，这意味着将轨迹规划时间区间内的风险系数分配到去计算单个时刻的概率约束，将风险系数$ϵ$ 拆分。

3.3. 风险分配

在机会约束问题中，总的风险水平$ϵ$ 表示在整个规划时间范围内允许的约束违反概率。为了确保整体的安全性，需要将这个风险分配到每个时间步上。传统的均匀风险分配方法是将总风险$ϵ$ 均匀分配到所有约束上，但这种方法较为保守。考虑将高斯过程模型预测障碍物动态行为位置信息的各个预测步长的方差和均值引入到风险系数分配中。在MPC的每个滚动优化时域内，根据最新预测的协方差矩阵和均值实时更新风险系数，单个的事件的风险系数如下：

${ϵ}_{ij}^t=ϵ\cdot \frac{\text{Tr}\left({\Sigma }_{ij}^t\right)+\gamma \cdot \Vert {\mu }_{ij}^t-{\mu }_{ij}^{t-1}\Vert }{{\displaystyle \sum _{j,i,t}\left(\text{Tr}\left({\Sigma }_{ij}^t\right)+\gamma \cdot \Vert {\mu }_{ij}^t-{\mu }_{ij}^{t-1}\Vert \right)}}$ (18)

${\Sigma }_{ij}^t$ 是预测协方差矩阵，${\mu }_{ij}^t$ 是预测均值。引入均值变化量$\Vert {\mu }_{ij}^t-{\mu }_{ij}^{t-1}\Vert$ ，捕捉障碍物运动趋势(如加速、转向)。$\gamma$ 是运动趋势权重。这种方法能够更合理地分配风险，减少保守性，提高安全性并适应动态环境。

3.4. 概率约束转化为确定性约束

公式(17)是一种概率约束涉及随机变量和概率分布，导致优化问题非凸且计算复杂。为了方便求解需要将机会约束转化为确定性约束，它要求某个条件必须始终满足不涉及概率。为了方便计算令：

$D_{ij}^t:={\left[\begin{array}{ll}{a}_{ij}^t\hfill & b_{ij}^t\hfill \end{array}\right]}^{\top }$ (19)

因次令$Y=a_{ij}^{t^{\top }}{x}_t+b_{ij}^t$ 。$D_{ij}^t$ 是服从高斯分布的，所以$Y$ 也是服从高斯分布的。为了方便计算定义增广向量$\tilde{x}:={\left[\begin{array}{ll}{x}^{\top }\hfill & 1\hfill \end{array}\right]}^{\top }$ 那么公式(15)可以改写成：

${\wedge }_{t=1}^N{\wedge }_{j=1}^{N_o}\mathrm{Pr}\left({\displaystyle \sum _{i\in \text{{}x,y\text{}}}^{}\sigma \left(D_{ij}^t{\tilde{x}}_t\right)}-1>0\right)\ge 1-{ϵ}_{ij}^t$ (20)

因此可以得到$Y=D_{ij}^t{\tilde{x}}_t$ ，其中$Y$ 的均值为${\mu }_{ij}^t{\tilde{x}}_t$ ，方差为：${\tilde{x}}_t{}^{\top }{\Sigma }_{ij}^t{\tilde{x}}_t$ 。

将$Y$ 转化成标准的正态分布：

$Y=\frac{D_{ij}^t{\tilde{x}}_t-{\mu }_{ij}^{\top }{\tilde{x}}_t}{\sqrt{{\tilde{x}}_t^{\top }{\displaystyle {\sum }_{ij}^t{\tilde{x}}_t}}}~N\left(0,1\right)$ (21)

那么结合公式(20)和公式(21)可以得到：

$P\left(Y>\frac{-{\mu }_{ij}^{tT}{\tilde{x}}_t+1}{\sqrt{{\tilde{x}}_t^T{\displaystyle {\sum }_{ij}^t{\tilde{x}}_t}}}\right)\ge 1-{ϵ}_{ij}$ (22)

根据[10]中的定理，如果${ϵ}_{ij}^t<0.5$ ，通过累计分布函数的逆向函数可以得到：

$\frac{-{\mu }_{ij}^t{\tilde{x}}_t+1}{\sqrt{{\tilde{x}}_t^{\top }{\displaystyle {\sum }_{ij}^t{\tilde{x}}_t}}}\le {\Phi }^t\left(1-{ϵ}_{ij}^t\right)$ (23)

进一步可以将公式(23)写成二阶锥约束的形式：

${\Phi }^{-1}\left(1-{ϵ}_{ij}^t\right){\Vert \sqrt{{\tilde{x}}_t^T{\displaystyle {\sum }_{ij}^t{\tilde{x}}_t}}\Vert }_2\le {\mu }_{ij}^{tT}{\tilde{x}}_t-1$ (24)

4. 算法设计

本文考虑是线性时变系统，一般表达式如下：

$x_{t+1}=A_t{x}_t+B_t{u}_t$ (25)

其中$x_t$ 是系统状态，$u_t$ 是系统的输入，$A_t$ 和$B_t$ 是在时间$t$ 时刻系统的动态矩阵。给定一个时间范围$N>0$ ，用$u=\left(u_0,\cdots ,u_{N-1}\right)$ 表示有限长度的输入序列。假设给定初始状态$x_0$ 和输入序列$u$ ，状态轨迹的演化表示为$x:=\left(x_1,\cdots ,x_N\right)$ 。控制输入被限制在一个凸集$U$ 中，也就是说对于所有的$t\in \left[0,N-1\right]$ ，有$u_t\in U$ 。那么有模型预测控制方案的一般形式如下：

${\min }_{u,z_{ij}^t}J\left(x_0,u\right)$ (26)

$s.t{x}_{t+1}=A_t{x}_t+B_t{u}_t,x=x_0$ (27)

$u\in U$ (28)

${\Psi }^{-1}\left(1-ϵ\right)\sqrt{1+r_2}{\Vert {\widehat{\Sigma }}^{1/2}\tilde{x}\Vert }_2+r_{1,ij}{\Vert \tilde{x}\Vert }_2\le {\widehat{\mu }}^{\top }\tilde{x}-1$ (29)

本文中假设考虑的系统和障碍物的状态为：$x={\left[x,y,\varphi ,v_x,v_y,\omega \right]}^{\top }$ ，其中$x$ 、$y$ 为系统的笛卡尔坐标位置，$\varphi$ 为航向角，$v_x$ 、$v_y$ 分别为重心的纵、横速度，$\omega$ 为偏航角速度。控制输入为$u={\left[a,\delta \right]}^{\top }$ ，控制输入$u$ 是加速度$a$ 和转向角$\delta$ 。则可以设置MPC成本函数的形式如下：

$J\left(x_0,u\right)={\displaystyle \sum _{t=0}^{N-1}\left({\Vert \Delta u_t\Vert }_R^2+{\Vert u_t\Vert }_P^2+{\Vert \Delta x_t\Vert }_Q^2\right)}-k{V}_x{}^N$ (30)

MPC预测步长也为N (这里的步长跟高斯过程的预测步长没有必然联系可以根据计算的复杂程度适当调整)。其中控制输入项${\Vert u_t\Vert }_R^2$ 和${\Vert \Delta u_t\Vert }_R^2$ ，用来控制输入$u_t$ 的大小以及变化量，能够避免过大的控制输入(如急加速或急转向)从而提高驾驶的舒适性和安全性。$-k{V}_x{}^N$ 这一项的目的是希望受控车辆的速度尽可能快。在存在预测误差或环境不确定性的情况下，松弛变量可以提高MPC的鲁棒性，确保生成的轨迹在实际环境中仍然可行。${\Vert \Delta x_t\Vert }_Q^2$ 为系统状态项，这一项是希望横向位置的变化和转向角变化不要太大从而增加系统的危险系数。$k,R,P,Q$ 均为权重矩阵，这些目标项都可以通过调整权重矩阵系数来调节各个变量目标项的重要性程度。

结合前面的分析，本文提出FSGP-MPC算法。该算法在每个时间步$k$ 生成一个预测障碍物时间步长为N的运动轨迹。为了生成这个轨迹，算法在每个时间步$k$ 进行$M$ 次采样，每次采样生成一个可能的障碍物运动轨迹状态。高斯过程模型的输入的是预测步长$N$ ，采样次数$M$ ，当前障碍物的状态$Z_K$ 并且帮当前系统状态作为每次采样时状态的初始值。在当前时刻的每次采样，根据此次的采样结果都会给出障碍物$N$ 个时间步的轨迹预测。求取$M$ 次的轨迹预测结果的平均值作为最后的轨迹。使用这个均值结果跟每次采样生成的轨迹做差求出其方差，得出每个预测时间步的方差和均值。计算风险分配系数值，最后计算MPC优化问题规划出智能体未来时间N步的轨迹。具体算法流程如图1。

Figure 1. FSGP-MPC algorithm

图1. FSGP-MPC算法

5. 案例研究

为了验证算法的有效性，参考[3]中的开环控制案例，一辆受控的汽车以初始速度50 km/h在双向道路的右侧车道上行驶，初始位置为(4.5 m, −1.75 m)。另一辆来自相反方向的对手车辆开始左转进入一条支路。对手车辆的轨迹是不确定的，并遵循独轮车动力学模型：

$\dot{\chi }:={\left[\begin{array}{ccc}{y}_1& y_2& \theta \end{array}\right]}^{\top }={\left[\begin{array}{ccc}v\cos \left(\theta \right)& v\sin \left(\theta \right)& \omega \end{array}\right]}^{\top }$ (31)

对手车辆的恒定前进速度是$v=22\text{ km/h}$ ，其初始状态已知为$\left(49\text{ m},1.75\text{ m},0^{°}\right)$ ，假设所有可能的值在给定区间内是等概率的，设置角速度$\omega$ 的分布为均匀分布：

${\omega }_t~\text{Unif}\left(\frac{\pi -0.66}{2\left(N+1\right)}\min \left\{\frac{\pi +0.66}{2\left(N+1\right)}\cdot \frac{\pi }{2}-{\displaystyle \sum _{\tau =0}^{t-1}{\omega }_{\tau }}\right\}\right)$ (32)

单位为弧度/秒，确保对手车辆在规划时间范围$N$ 结束时完成${90}^{°}$ 的逆时针转弯。受控车辆的模型为双积分器：

${\left[{\dot{x}}_1{\dot{x}}_2\right]}^{\top }={\left[x_3{x}_4\right]}^{\top },{\left[{\dot{x}}_3{\dot{x}}_4\right]}^{\top }={\left[u_1{u}_2\right]}^{\top }$ (33)

其中状态$x\in {ℝ}^4$ 包含车辆的位置$\left(x_1,x_2\right)$ ，和相应的速度$\left(x_3,x_4\right)$ 。控制输入为加速度$\left(u_1,u_2\right)$ ，并受到约束集$\mathcal{U}:=\left\{u\in {ℝ}^2:-3\le u_1\le 10,\left|u_2\right|\le 5\right\}$ ，单位为m/s²。同样速度也受到约束集：

$\mathcal{X}:=\left\{x\in {ℝ}^4:\left|\frac{x_3}{40\text{ km/h}}-1\right|+\left|\frac{x_4}{20\text{ km/h}}-1\right|\le 1\right\}$ (34)

目标函数是最大化车辆的终端前进位置，成本函数为$J=-x_{1.N}$ 。为了处理对手车辆姿态${\chi }_t$ 的不确定性，施加了联合机会约束：

$\mathrm{Pr}\left({\wedge }_{t=1}^N{\vee }_{i=1}^2{d}_i^t{\left({\chi }_t\right)}^{\top }\left[\begin{array}{c}{x}_{1,t}\\ x_{2,t}\\ 1\end{array}\right]>0\right)\ge 1-ϵ$ (35)

其中系数$d_i^t\left({\chi }_t\right)$ 的具体形式：

$\begin{array}{l}{d}_{\text{1}}^t={\left[+\sin \left(\theta \right),+\cos \left(\theta \right),-\sin \left(\theta \right)y_1-\cos \left(\theta \right)y_2-W/2\right]}^{\top },\\ d_{\text{2}}^t={\left[-\cos \left(\theta \right),+\sin \left(\theta \right),+\cos \left(\theta \right)y_1-\sin \left(\theta \right)y_2-L/2\right]}^{\top }。\end{array}$ (36)

其中$L$ 和$W$ 分别是对手车辆的长度和宽度。两辆车的长度均为4.5米，宽度为2米，这些参数被纳入相关的不等式约束中。

高斯过程预测模型的核函数的选择需要考虑输出轨迹应该是平滑的。本文中高斯过程模型核函数选择的是径向基函数(RBF)核，其表达式为公式(38)。因为通常在处理时间序列数据或动态系统建模时，它能够较好地捕捉数据的平滑变化和局部相关性。

$K\left(x,y\right)=\exp \left(-\frac{{\Vert x-y\Vert }^2}{2{\sigma }^2}\right)$ (37)

在该案例研究中，采样时间$T_s=0.4$ 秒和规划时间范围$N=10$ (即4秒)，使用CPLEX求解器。考虑了三种不同的情况。联合机会约束(37)在整个时间范围内施加总的风险系数为$ϵ=0.05$ 置性水平为$\beta ={10}^{-3}$ 。样本的来源是使用数据生成的方式产生。考虑了三种情况，分别使用的样本数为$N_s^A=\text{5000 }{N}_s^B=\text{2}000$ (情况A和情况B)和$N_s^C=\text{1}00\text{0}$ (情况C)。高斯过程模型的超参数统一使用3000个样本去训练得到相应的超参数。

Figure 2. Experimental results (Case A, B, C)

图2. 实验结果(情况A，B，C)

根据图2可以观察到，在样本数量有限的情况下(如情况B和C)，所提出的方法依然能够生成安全的轨迹规划。由于样本数量较少，对障碍物位置的估计不确定性较高，受控车辆被迫采取更保守的策略。例如急刹车，以避免与对手车辆的不确定位置发生碰撞。相比之下在样本数量较多的情况下(如情况A)，对障碍物位置的估计精度更高，生成的轨迹能够以较小的安全距离避开对手车辆。此外情况B使用了比情况C更多的样本，因此矩的不确定性较小，生成的轨迹规划结果略优，表现为受控车辆的最终位置在情况B中比情况C略靠右。

另外本文生成了对手车辆100个新的行驶轨迹如图3，来评估该算法违背概率的情况。

Figure 3. 100 new driving trajectories of opponent vehicles

图3. 100个新的对手车辆行驶轨迹

Figure 4. Experimental results (Case D)

图4. 实验结果(情况D)

在这100个测试案例中产生的违背概率大概只有约3%，满足了预期的概率约束要求。

为了验证风险分配系数的有效性，情况D是在情况A的基础上增加一辆并列行驶的对手车辆，该车辆起初是做了慢速动作，并且有小幅靠近受控车辆方向动作，然后修正前进方向，开始做加速动作，并且做了并且有靠近受控车辆方向动作。情况D轨迹如图4。

可以看到当对手车辆在起初的减速阶段时，即使车辆有方向上的轻微波动，受控车辆也只是在方向上有所避让，并没有减速。当对手车辆有速度加速状态时，受控车辆不仅在方向做出更多的避让也进行减速。

6. 结论

本文结合了高斯过程模型和MPC处理不确定性的优势还有矩集中理论提出了FSGP-MPC算法。构建了避障的系统安全状态集合，将高斯过程预测结果通过风险分配系数引入机会约束中，很好地结合了MPC与高斯过程模型在处理不确定性方面的优势，并且给出了相关案例证明了算法的有效性。在工程实践方面可能需要考虑在线升级高斯过程模型的超参数问题。另外该算法只是考虑了对于外部的不确定性。那么算法内部不确定性主要有模型本身的不确定性，还有算法本身参数的不确定性。因此后面的工作可以通过引入强化学习的方式去弥补算法本身的这种不确定性。