预见性运动动力学规划器用于计算自动驾驶中的最优路径与速度轨迹

摘要

本文提出了一种利用预见性运动动力学规划器在动态复杂环境（如城市驾驶场景）中获取最优轨迹和最优速度曲线的方法。该规划器对道路搜索空间进行离散化，并在每个控制时间段内寻找最优车辆路径和速度曲线，前提是静态和动态物体已被检测到。本工作的主要贡献在于：预见性运动动力学规划器、一种快速生成路径的G2样条曲线方法，以及一种在满足车辆运动动力学约束并考虑乘客舒适性的前提下，为每条候选路径计算并选择最优速度曲线的方法。该方法已在MATLAB中开发并通过一系列仿真进行了测试，仿真场景包括不同典型情况下的静态障碍物和移动车辆。仿真结果表明，预见性运动动力学规划器在多种动态场景中均能正确运行，并保持平滑的加速度以确保乘客舒适性。

关键词 ：
自动驾驶，先进驾驶辅助系统，城市，预见性，运动动力学规划，路径规划，G2‐样条曲线，速度曲线

1. 引言

根据世界卫生组织的报告，每年全球有125万人死于交通事故。以欧洲为例，道路交通事故是年轻人死亡的主要原因。此外，几乎所有的事故都归因于驾驶员失误、酒后驾驶或分心驾驶。自动驾驶车辆有望大幅减少因疏忽和人为错误导致的交通事故。此外，它们还能为因残疾而无法驾驶的人提供个人出行便利。

然而，为了实现这些目标，自动驾驶车辆必须达到人类可接受的驾驶性能。这一性能水平尤其依赖于开发和改进满足要求的方法与算法，其中运动规划至关重要。

自动驾驶中运动规划的目标是基于环境预测、行为规划和轨迹规划来计算最优的驾驶操作。在本文中，我们介绍了一种新技术，该技术同时考虑轨迹规划和部分行为规划，旨在寻找完成特定驾驶操作的最优路径和速度曲线，同时保持乘客舒适性。

自动驾驶汽车的导航必须应对各种驾驶情况，例如同一车道内静止的车辆等静态障碍物（处于移动状态）或移动物体（例如，自行车、摩托车、过马路的行人或其他移动车辆）。在这些情况下，需要评估情况，预测静态和移动物体的未来位置，计算最佳路径轨迹并选择最佳方案。这些决策必须在不同层级上实时做出。

在本研究中，我们将范围限定为对车辆的轨迹和速度进行低层决策，仅考虑在同一或相反车道上可能存在静态障碍物和移动车辆的情况。我们的目标是提出一种能够在实时计算出运动规划器多种解决方案中的最优路径和最优速度的方法，以应对车辆前方不同的道路状况（例如接下来的100米内）。我们提出了一种预见性运动规划器框架，在每隔几毫秒的时间间隔内，计算出多条运动轨迹，并针对每条轨迹，通过最小化静态和动态成本函数来寻找最优速度曲线。该成本函数包括与路径几何和静态障碍物相关的静态成本，以及与时间维度（关联包含动态障碍物的速度曲线）相关的动态成本。需要强调的是，所提出的运动规划器框架能够处理动态环境，预见各种情况，并基于一组成本函数选择最优路径和速度。

本文第3节介绍了所提出的运动规划器框架；第4节介绍了路径生成的G2‐样条曲线方法；第5节介绍了速度曲线生成算法，以及在第6节中用于选择最优路径和速度曲线的代价结构。最后，在第7节中详细描述了一些代表性仿真，在第8节中给出了结论。本文是文章[1]的扩展。

2. 相关工作

自动驾驶车辆的运动规划需要具备快速性（快速决策）、连贯性（决策与长期目标保持一致）、预见性（需要有一定的时间视野以进行预测）以及可预测性（遵循驾驶规则和预定义行为）[2]。为了满足这些要求，目前尚无具有实际计算复杂度的精确算法。最流行的方法——状态格点规划器，试图以某种方式对工作空间进行离散化（取决于其建模方式），并搜索最优解[3][4]。例如，DARPA挑战赛的获胜者斯坦利（2005年）和BOSS均采用了此类框架 2007[5][6]。其他规划方法，如基于约束的预测规划和基于样条的规划方法也已被提出[7]。

为了能够预测其他车辆等移动障碍物，运动规划器需要对障碍物进行可靠检测，并获知或推断这些车辆的未来行为或运动。通常情况下，这一预测问题极为困难。然而，当这些动态障碍物是在城市环境中行驶的车辆时，可以利用此类环境中的固有结构更轻松地推断其可能的行为：在道路上行驶的车辆通常遵循通用道路规则[8][9]。

如[4],所述，规划器采用不同的方法来考虑移动障碍物和交通流，例如马尔可夫决策过程、状态机、代价结构、概率模型或博弈论。

运动规划器需要生成候选几何路径，供底层控制系统跟踪。自动驾驶的路径需要满足运动学和动力学约束。此外，在道路场景（结构化环境）中行驶时，路径还需符合道路形状。这些运动动力学约束是必要的，以实现良好的控制性能，并确保车辆能够精确地跟踪路径。更具体地说，这些约束与路径的几何连续性（Gn）相关。路径生成中最常用的技术包括圆弧、螺旋曲线、多项式螺旋线、样条曲线和B´ezier曲线。它们在参数数量和自由度方面有所不同，因此在复杂度水平和性能之间存在权衡。通常所需的高复杂度会导致出现非直观几何航路点以及大量需要调整的参数，从而带来较大的计算负载。在[10]中，通过曲率多项式和数值优化生成了曲率变化率连续的路径。文献[11]中的方法使用B´ezier曲线生成符合道路形状的路径。

然而，该生成算法由于几何约束以及其复杂性而存在局限性。文献[12]提出了一种对所用方法的推广五次G2‐样条曲线，进而得到G3‐样条曲线，即增加了曲率导数连续性的七次多项式样条曲线。

另一方面，生成速度曲线可能导致高阶多项式和复杂性，意味着较高的计算成本。与路径生成算法类似，施加的约束与函数参数相关，而这些参数通常难以调整，并且需要进行一些后优化。速度曲线生成方法通常使用梯形曲线，因其简单性，尽管存在动态限制，因为它们在加速度上有不连续性[6][13]。更好的方法是基于样条曲线计算随时间变化的速度轨迹，固定总操作时间[14][15][16]，或也基于道路长度[17]。然而，这些时间条件使得离散化过程变得困难，甚至需要进行后优化。其他方法，例如[13],，通过模拟控制器在特定时间范围内的输出，并考虑环境因素来生成速度曲线。

本工作相对于现有技术的优势有多个方面：首先是预见性运动动力学路径规划框架，该框架提出了一种路径规划方案，能够在动态环境中实时预测多种解决方案中的最优轨迹和最优速度曲线；其次，是一种快速计算G2‐样条曲线的方法，无需进行耗时的参数计算；第三，是一种同时选择最优路径和最优速度曲线的方法，综合考虑了车辆的运动动力学约束、静态障碍物和路径几何带来的约束，以及与包含动态障碍物的速度曲线相关的时间维度约束。

3. 预期运动规划器框架

图1 显示了所提出的运动规划器的总体方案。在上层，全局规划器负责确定任务目标，这些目标可以是完全自主的，也可以考虑用户输入。路线规划器选择驾驶指令，以引导车辆沿特定路线行驶（例如在特定交叉口转弯、选择街道、高速公路出口等）。它需要道路网络数据作为输入，并且可以考虑感知系统，例如用于检测封闭道路。

从期望的路线出发，运动规划器需要车道中心路径，这些路径可以使用规划器内部采用的相同路径生成算法进行计算，该算法在第4节中进行了说明。它利用道路网络数据以及感知系统的信息，以精确调整轨迹形状，并考虑诸如车道变窄等意外事件。

直接从感知系统中，规划器需要获知所有检测到的障碍物列表，包括静态和动态障碍物。静态障碍物是指任何静止不动的物体（如树木、停泊车辆、锥形标等），而动态障碍物则是指移动物体（如行驶车辆、行人等）。

图2描绘了运动规划器的内部阶段，其中箭头表示处理顺序。

首先，该规划器将环境进行离散化，从车道中心路径开始，选择多个端点。然后从主车状态到这些端点生成候选路径（第4.1 节）。运动规划器采用一种快速方法生成路径，G2‐样条曲线[18]。该方法仅需初始点和终点的基本几何状态。它能够近似任意类型的路径形状，保持曲率的平滑连续性并最小化曲率变化，从而确保车辆沿其行驶时的运动学可行性：如圆弧段、直线、螺旋曲线、完整的车道变换等（第 4.2节）。

规划器获得一组候选路径后，会为每条候选路径计算一组速度曲线，以应对动态障碍物（第5节）。每条速度曲线都有相应的成本，针对特定路径选择成本最小的速度曲线。最后，将所有候选路径及其对应的速度曲线根据代价结构进行比较（第6节），并选择最小成本解（路径和速度曲线）作为执行方案。速度曲线采用三阶样条计算，同时考虑候选路径的动态限制和道路路径形状，并经过运动学验证。

所提出的运动规划器的输出是一条具有相关速度曲线的运动学和动力学可行路径，为主车生成的文件，同时满足乘客的舒适性限制，例如加速度和加加速度的约束。

4. 路径生成

路径生成算法既用于创建运动规划器所需输入的车道中心路径，也用于生成候选路径。

4.1. 候选路径和端点生成

为了使用G2‐样条曲线计算候选路径，规划器从车道中心路径开始对环境进行离散化，并选择多个端点，方法类似于[17]中的做法。每个端点是一个状态配置X=[x， y， θ, κ]。变量x和y是位置坐标， θ是航向， κ是曲率，曲率通过自行车运动学模型与方向盘转角相关联。

与[17],类似，候选路径从主车状态生成至一定的前瞻距离。然而，该时域被划分为多个插值点，以确保跟随车道形状（图3）。然后，对于每个中间纵向位置，通过多个横向偏移（垂直于道路中心线）创建一组端点。

在第一个插值距离（图3中的点P）之前，采用更精细的离散化以提高采样的响应性（图4）。

最后，当考虑多个车道时，端点集还包含其他车道。

该方法的更多描述见[20]。

4.2. G2-样条曲线路径生成

G2样条曲线 是具有二阶几何连续性（G2）的五阶几何多项式，因此其曲率 κ是连续的[18]。为了构建一条通用路径p(u)，即起始端点任意定义的路径XA=[xA,yA, θA, κA], 以及终点 XB=[xB,yB, θB, κB]（图5），定义这些样条曲线的方程为：

$$
p(u)=[x(u) y(u)]:=[x_0+ x_1u+ x_2u^2+ x_3u^3+ x_4u^4+ x_5u^5 \
y_0+ y_1u+ y_2u^2+ y_3u^3+ y_4u^4+ y_5u^5] \quad (1)
$$

其中 $ u \in[0, 1] $ 以及：

$$
x_0= x_A \quad x_1= \eta_1 \cos\theta_A \
x_2= \frac{1}{2}(\eta_3 \cos\theta_A - \eta_2^1\kappa_A \sin \theta_A) \
x_3= 10(x_B - x_A)-(6\eta_1+ \frac{3}{2}\eta_3) \cos\theta_A -(4\eta_2 - \frac{1}{2}\eta_4) \cos\theta_B+ \frac{3}{2}\eta_2^1\kappa_A \sin \theta_A - \frac{1}{2}\eta_2^2\kappa_B \sin \theta_B \
x_4= -15(x_B - x_A)+(8\eta_1+ \frac{3}{2}\eta_3) \cos\theta_A+(7\eta_2 - \eta_4) \cos\theta_B- \frac{3}{2}\eta_2^2\kappa_B \sin \theta_B \
x_5= 6(x_B - x_A)-(3\eta_1+ \frac{1}{2}\eta_3) \cos\theta_A -(3\eta_2 - \frac{1}{2}\eta_4) \cos\theta_B+ \frac{1}{2}\eta_2^1\kappa_A \sin \theta_A - \frac{1}{2}\eta_2^2\kappa_B \sin \theta_B \
y_0= y_A \quad y_1= \eta_1 \sin \theta_A \
y_2= \frac{1}{2}(\eta_3 \sin \theta_A+ \eta_2^1\kappa_A \cos\theta_A) \
y_3= 10(y_B -y_A)-(6\eta_1+ \frac{3}{2}\eta_3) \sin \theta_A -(4\eta_2 - \frac{1}{2}\eta_4) \sin \theta_B- \frac{3}{2}\eta_2^1\kappa_A \cos\theta_A+ \frac{1}{2}\eta_2^2\kappa_B \cos\theta_B \
y_4= -15(y_B -y_A)+(8\eta_1+ \frac{3}{2}\eta_3) \sin \theta_A+(7\eta_2 - \eta_4) \sin \theta_B+ \frac{3}{2}\eta_2^1\kappa_A \cos\theta_A - \eta_2^2\kappa_B \cos\theta_B \
y_5= 6(y_B -y_A)-(3\eta_1+ \frac{1}{2}\eta_3) \sin \theta_A -(3\eta_2 - \frac{1}{2}\eta_4) \sin \theta_B- \frac{1}{2}\eta_2^1 \kappa_A \cos\theta_A+ \frac{1}{2}\eta_2^2 \kappa_B \cos\theta_B
$$

生成的路径取决于影响其形状的参数向量 η 和 $ \eta=[\eta_1, \eta_2, \eta_3, \eta_4] $。

生成的样条曲线的一个重要特性是其曲率 $ \kappa(u) $，可通过以下公式计算：

$$
\kappa(u)= \frac{\dot{x}(u)\ddot{y}(u)- \ddot{x}(u)\dot{y}(u)}{( \dot{x}(u)^2 + \dot{y}(u)^2 )^{3/2}} \quad (2)
$$

4.3. G2-样条曲线优化算法

优化 η值的通用情况需要进行数值优化[18]。然而，由于所需车辆运动操作的类型，我们可以采用一种更快的解决方案，该方案无需数值优化，而只需一个简短的迭代过程。我们已经意识到这些轨迹具有起止点之间的对称行为，如图6a所示，展示了一种常见的变道操作。然后我们可以将公式3的约束施加于η参数，从而只需调整2个参数。

$$
\eta_{12}= \eta_1= \eta_2 \quad \eta_{34}= \eta_3= -\eta_4 \quad (3)
$$

$ \eta_{34} \in(-\infty,+\infty) $，它会影响曲率变化。可以得出结论，在所有情况下该参数的最佳值均为0。在图6中，通过执行变道操作测试了不同的 $ \eta_{34} $值。可以看出，当 $ \eta_{34}> 0 $（绿色轨迹）时，曲率变化集中在轨迹的中间部分，而当 $ \eta_{34}< 0 $（蓝色轨迹）时，则集中在起点和终点的两端。红色轨迹代表$ \eta_{34}= 0 $，是最平滑且最均衡的轨迹，表现出最小的曲率变化性。

另一方面， $ \eta_{12} \in(0,+\infty) $，它迫使 θ和 κ保持接近初始值和最终值。研究发现，当$ \eta_{12} $接近轨迹长度（以米为单位）时，可得到最平滑的轨迹。因此，采用一种迭代方法，将 $ \eta_{12} $设为路径长度，以收敛到最优的轨迹。经过数次迭代（通常为3到4次）后，计算出的轨迹会收敛到最平滑的结果。

G2‐样条曲线优化算法可以总结为公式4。

$$
\eta_{12}= \text{length(path)} \
\eta_{34}= 0 \quad (4)
$$

该过程获得的结果与[18],中提出的数值优化方法类似，后者通过最小化曲率变化率 $ \min_\eta \left| \frac{d\kappa}{du} \right| $ 来实现，但本方法计算量显著降低，从而能够达到实时性能。

图7展示了路径生成方法试图用半径为r= 15 m的圆形路径进行近似，但在起点和终点处曲率为零（κ= 0）。该示例表示街道拐角处的一个90度急转弯。经过3次迭代后，转弯中段的路径曲率几乎保持恒定（图7b）。

图8展示了在实际环境中，优化算法进行1次迭代与选择的5次迭代之间的对比。该图显示了复杂交叉口内所有可能的中心车道路径组合。这些路径是根据起点和终点的几何信息生成的（外部点为红色，交叉点为品红色）。

5. 速度曲线生成

为了生成速度曲线，运动规划器在速度上使用三阶多项式样条 $ v(t) $。该样条曲线可在两个不同速度之间实现平滑过渡，并通过抛物线加速度曲线固定最大（或最小）加速度。由于加速度的导数即加加速度是连续的，因此速度之间的这种过渡是平滑且舒适的。

此外，能够直接调整轨迹中最大期望加速度这一参数对该应用非常有用。它与轨迹所需时间、乘客舒适性以及可行性直接相关。

速度曲线生成算法分为两种不同情况：有初始加速度和无初始加速度，如下所述。这能够始终提供解析解，从而降低计算成本。

5.1. 无初始加速度的三阶样条

所提出的在速度上的三阶样条有4个变量：a、b、c、d（公式5）。第五个未知数是轨迹的总时间T。该方程组（公式6）通过应用初始和终止条件，并利用速度样条是对称的这一事实来求解，因此最大加速度出现在时间为 T/2时。

$$
x(t)= \frac{a}{4} t^4+ \frac{b}{3} t^3+ \frac{c}{2} t^2+ dt+ x_0 \
v(t)= at^3+ bt^2+ ct+ d \
a(t)= 3at^2+ 2bt+ c \quad (5)
$$

$$
\begin{cases}
v(0)= v_0 \
a(0)= 0 \
v(T)= v_f \
a(T)= 0 \
a\left( \frac{T}{2} \right)= a_{\text{max}}
\end{cases}
\Rightarrow
\begin{cases}
b= \frac{4a_{\text{max}}^2}{3(v_f - v_0)} \
a= -\frac{b^2}{3a_{\text{max}}} \
c= 0 \
d= v_0 \
T= \frac{3(v_f - v_0)}{2a_{\text{max}}}
\end{cases} \quad (6)
$$

5.2. 具有任意初始加速度的三阶样条

该速度上的三阶样条与无初始加速度情况下的样条相同（公式5），但方程组略有不同，因为样条曲线不对称，且最大加速度在时间$ t_1 $时达到。6个未知数为样条曲线变量 a、b、c、d，以及总轨迹 $ T $和时间$ t_1 $。

$$
\begin{cases}
v(0)= v_0 \
a(0)= a_0 \
v(T)= v_f \
a(T)= 0 \
a(t_1)= a_{\text{max}} \
a’(t_1)= 0
\end{cases}
\Rightarrow
\begin{cases}
0= \left[ 4a_0^2 \right] T^2+ \left[6(v_f - v_0)- 12(v_f - v_0)\frac{a_0}{a_0 - a_{\text{max}}} \right] T+ \left[9(v_f -v_0)^2 \frac{a_0}{a_0 - a_{\text{max}}} \right] \
b= \frac{1}{T}\left[3(v_f -v_0) T - 2a_0 \right] \
a= \frac{b^2}{3(a_0 - a_{\text{max}})} \
c= a_0 \
d= v_0 \
t_1= -\frac{b}{3a}
\end{cases} \quad (7)
$$

我们需要求解一个二阶方程来找到时间$ T_1 $和$ T_2 $（公式7）。由于分母项（$ a_0 -a_{\text{max}} $）的存在，并非所有情况下都有解。$ a_{\text{max}} $在加速时必须始终大于$ a_0 $，而在减速时则必须更小。

当只有一个$ T_i $为正时，这是唯一的解决方案。当两个 $ T_i $都为正时，对应两种可能的解决方案：一种是增加加速度至$ a_{\text{max}} $并更快地完成操作，另一种是减小加速度从$ a_0 $并用更长时间完成操作。在这种情况下，总是选择最快的操作，因此$ T= \min(T_1,T_2) $。

当样条曲线无解时，对应于车辆正在加速的方向与期望最终速度方向相反的情况，或矛盾情况（例如，$ a_0> 0 $和$ v_f< v_0 $）。该问题通过添加一个线性加速度段来解决，该段从$ a_0 $开始，到加速度为0结束，并采用期望斜率（加加速度），以确保舒适性和可行性。

5.3. 速度曲线生成算法

生成速度曲线的完整算法详见算法1，一些示例显示在图9中。每种颜色代表算法的一次不同启动。蓝色曲线是无初始加速度的样条曲线。然后计算一个新的速度曲线，达到18 m/s（红色），但在某一时刻，算法重新启动至相同的最终速度（橙色），这对应于具有任意初始加速度的样条曲线。右侧的示例显示了一个矛盾情况。

算法1 生成速度曲线

if a0 = 0 then
    无 a0 的样条曲线情况
else if vf = v0 then
    添加从 a0 到 0 的线性加速度
    无 a0 的样条曲线情况
else if 矛盾情况 then
    从 a0 到 0 添加线性加速度
    不带 a0 的样条曲线情况
else
    带有任意 a0 的样条曲线情况
end if
end if
end if

5.4. 速度曲线候选集

对于每条候选路径，运动规划器会计算一组速度曲线候选集，对最终速度和加速度进行离散化处理，并考虑车辆的运动动力学特性。从主车当前状态开始，选择从0到道路/街道允许的最大速度之间的多个最终速度。然后，针对每个最终速度，也选择不同的加速度，范围从最大减速度限制到期望加速度值。这些速度和加速度的示例如图10所示。

为了减少计算量，我们使用样条曲线对速度曲线施加一些约束，以排除不可行候选并限制搜索空间。这些约束包括：

1. 通过主车在轨迹长度方向上的距离来计算与静态障碍物碰撞的距离。如果检测到碰撞，则唯一允许的最终速度为0。
2. 根据规划路径曲率所限制的允许最大速度。
3. 根据车道中心线路径曲率所限制的允许最大速度。该限制确保安全性，以更加保守的方式检查轨迹几何的静态属性，因为规划路径在重新启动时可能会被平滑处理。
4. 车辆的纵向动力学。该限制对加速度进行约束，以确保可行性。

采用这种方法，我们可以获得所需的加速度，然后通过选择最佳的速度曲线来缩小搜索空间并进行剪枝。

6. 最优路径和速度曲线的选择

在每次迭代中，根据总成本函数J选择最小成本解来确定运动规划器的输出，其中J是所有成本项的加权和（公式8）。

$$
J= J_{\text{static}}+ J_{\text{dynamic}}= \sum_i w_{s,i} \cdot c_{s,i}+ \sum_j w_{d,j} \cdot c_{d,j} \quad (8)
$$

运动规划器使用两类成本项：静态成本和动态成本。静态成本是指与路径几何和静态障碍物相关的项，它们与候选路径相关联（表1）。另一方面，动态成本涉及时间维度，与速度曲线相关，并包含动态障碍物（表2）。这些成本基于[17],中提出的方法，但经过调整以适应提出的方法。

Cost	公式
$ c_l $	候选路径的长度 / $ s_{\text{maneuver}} $
$ c_\kappa $	$ \max(\kappa) \cdot r_{\text{min}} $
$ c_{\dot{\kappa}} $	$ \max(\dot{\kappa}) \cdot r_{\text{min}} $
$ c_{\text{off}} $	$ o/o_{\text{max}} $
$ c_{\text{obs,s}} $	$ f \cdot e^{(-1/\lambda)\cdot d} $

表1: 静态成本

Cost	公式
$ c_v $	$ 1 - v_{f,vp}/v_{\text{max,desired}} $
$ c_a $	$ \left
$ c_{\text{obs,d}} $	$ f \cdot e^{(-1 / \lambda ) \cdot d} $

表2：动态成本

除障碍物项$ c_{\text{obs,s}} $和$ c_{\text{obs,d}} $外，所有项（静态的和动态的）均在0到1之间进行归一化。以下详细介绍各项：

• $ c_l $：路径长度项。它对效率有影响。$ s_{\text{maneuver}} $是操作的站点：从主车到候选路径终点沿道路轴线的距离。$ l $是候选路径的长度。
• $ c_\kappa $和$ c_{\dot{\kappa}} $：曲率项。与舒适性和运动学可行性相关。 $ \kappa $和 $ \dot{\kappa} $分别是候选路径中某一点的曲率和曲率导数值， $ r_{\text{min}} $为主车的最小转弯半径。
• $ c_{\text{off}} $：横向偏移项。它会影响行为（试图在车道中央行驶）。$ o_{\text{max}} $为横向所有路径中最远端点到车道中心的距离（最大允许横向偏移）以及$ o $是候选路径终点的横向偏移。
• $ c_v $：速度项。影响行为（系统会尝试以允许的最大速度行驶）。$ v_{f,vp} $是候选速度曲线的结束速度。$ v_{\text{max,desired}} $是当前时刻允许的最大速度。
• $ c_a $：加速度项。它对舒适性和效率有影响。$ a_{\text{max},vp} $是候选速度曲线的最大加速度值。$ a_{\text{max,braking}} $是主车允许的最大制动加速度值（始终为 < 0）。
• $ c_{\text{obs,s}} $和$ c_{\text{obs,d}} $：静态和动态项。f是缩放值， $ \lambda $是指数函数的衰减系数。d是障碍物（静态或动态）到主车的距离（两者均被建模为圆形）。如果d小于某个阈值，则使用P对成本进行惩罚：

$$
c_{\text{obs}}=
\begin{cases}
f \cdot e^{(-1/\lambda)\cdot d}+ P, & \text{if } d< \text{threshold} \
f \cdot e^{(-1/\lambda)\cdot d}, & \text{otherwise}
\end{cases} \quad (9)
$$

该函数在没有惩罚项的情况下的图像如图11所示。

与其他采用无限成本来惩罚碰撞的方法不同，[17],更倾向于保留该值。这样，在所有候选解的成本都很高（例如遇到无法避免的障碍物）的情况下，规划器总会给出最小成本解，因此该解是危险性最低的。事实上，在这种极端情况下，输出结果是在车道内进行最大制动直至停车，这是人类驾驶员所能采取的最安全操作。

每条候选路径选定的速度曲线是动态成本$ J_{\text{dynamic}} $最小的那条。然后，每条候选路径结合其关联的速度曲线构成一个候选动作，并计算静态成本函数$ J_{\text{static}} $。

最终选择的是最小成本操纵，该操纵使总成本函数 $ J $（公式 8）最小化。

7. 仿真

7.1. 仿真环境

运动规划器和车辆底层控制已在MATLAB中实现。斯坦利横向控制器[19]与一个考虑了车辆动力学的纵向控制器配合使用。具体而言，转向柱被建模为一阶系统，所用的纵向模型考虑了换挡和内燃机扭矩响应。该控制系统通过使用来自SEAT León Cupra汽车的实际数据进行仿真，完成了验证和调整。

所选的仿真场景因其代表了城市和道路驾驶中的现实生活常见情况而被选中，且这些场景是对所提出的运动规划器正确行为进行验证的具有挑战性的测试。这些场景是使用真实数据和参数（例如车辆参数、车道宽度、道路曲率半径、法定速度等）创建的。

主车如图12所示。外轮廓用红色表示，紫色线条为后轴和前轴。障碍物与主车类似，使用圆形（绿色）建模，但为了更清晰地显示，在移动车辆中未绘制出障碍物。在仿真过程中，主车以固定时间间隔绘制。这使得查看图片时能够方便地进行时间表示。主车行驶的路径用绿色线条表示（从后轴中心起始）。车道通过侧边线（黑色）及其（虚拟）中心线（蓝色）表示。

仿真在MATLAB中使用中等配置的计算机硬件进行，每个包含静态和动态障碍物的仿真周期的平均计算时间为 250毫秒。考虑到实际应用中C++代码的执行速度大约快 10倍，该方法即使不考虑实现优化，也能在25毫秒内运行。

7.2. 静态环境

图13展示了在无任何障碍物的环境中的仿真。允许的最大速度为15 m/s，主车从11.11 m/s（40 km/h）开始行驶。初始阶段，车辆加速至最高速度，直到遇到“S”形转弯，在此处车速降至7 m/s。随后由于左转弯，速度提升至12 m/s，在最终直线段，车辆再次加速至允许的最大速度。该环境为典型的城市场景，包含低速驾驶和多种转弯，需在转弯时降低速度。

图14展示了相同的环境，但车道内存在多个由圆形表示的静态障碍物。这模拟了城市环境中的一种突发事件，例如施工区域或双排停车的情况。可以看出，主车通过在车道内横向移动避开障碍物（第一个障碍物），并在可能的情况下通过换道来避让（第二个障碍物）。

7.3. 动态环境

本节中的仿真考虑了近期未来动态障碍物的影响。

7.3.1. 单向车道超车及对向来车情况

接下来的仿真展示了超车示例。在进行超车时，需要考虑的一个因素是规划视野范围。图15a展示了一次完整超车操作的规划示例。在这种情况下，假设规划视野范围足够长，能够支持完成整个操作的规划。规划器可以预测到操作完成前的所有动态障碍物，因此如果它发起超车，则意味着是安全的。如果左车道是对向车流车道，则必须满足该规划视野范围以确保安全。

现在考虑另一种情况，如图15b所示。在这种情况下，由于某些原因，感知系统范围小于超车所需的时域。规划器应该采取什么操作？如果相邻车道为同向车道，则仅规划变道操作到另一车道是足够安全的，因为主车只需注意后方来车即可。但在存在对向车流的情况下，则无法执行超车操作。如果障碍物是静态的，最优解是在当前车道内将主车安全地停止。

图16展示了一条单向车道（两条车道同方向）中的超车操作。主车初始速度为14 m/s，试图加速至最大速度 15 m/s，但随后遇到前方一辆以5 m/s行驶的慢速车辆。起初，运动规划器规划的速度曲线降至6.5 m/s，因为完整的操作需在100 m前无法进行超车。但当主车接近前方车辆时，规划器找到了可行的超车方案。

在图17的仿真中，主车行驶在双向车道上。前方车道被一些静态障碍物阻塞，且对向车道有对向车辆。规划器使主车在右车道平稳减速至1 m/s，并保持在此车道直到对向车辆（蓝色）通过。随后，主车变道至左车道并完成操作。

在双向车道上进行超车时，只有当该操作能够被预测至完成时才能执行，否则该操作将被放弃，主车会减速并跟随前方的动态障碍物（前车）。图18展示了一次仿真过程，其中主车可以利用其初始较高速度，在对向车流到达前返回右车道。然而，如果对向车流阻碍了该操作，主车将在后方等待。这种行为如图19所示。通过调整代价项权重以及修改公式9中的排斥函数参数，可以获得不同的响应结果。

7.3.2. T型交叉路口

进入T型交叉路口时，必须预测来自两个方向的对向车辆。图20展示了一种保守的仿真情况，主车减速至2 m/s，并让交通流先行通过。另一方面，图21展示了一种更为激进的情况，主车利用车辆之间的间隙，在横向舒适加速度允许的最大速度下驶入交叉路口，即8 m/s。

8. 结论

在分析仿真后，验证了所提出的路径和速度曲线生成方法能够正确运行，并成功应对所有呈现的情况，这些情况对应于道路驾驶中发生的代表性案例。

使用G2‐样条曲线进行路径生成的提出的方法表现超出预期，可实现实时计算。该路径仅需初始和终点端点的基本信息即可生成，有助于降低复杂性，并提升算法的性能、灵活性以及易用性。

所提出的用于速度曲线生成的方法运行平稳，并提供了能够应对任意情况的完全解析解。此外，该方法允许直接调整期望加速度，而非操作时间，从而有助于根据运动学和动力学约束来选择和剪枝候选速度曲线集合。