可调运动规划框架设计

最新推荐文章于 2025-11-25 14:32:13 发布

原创最新推荐文章于 2025-11-25 14:32:13 发布 · 617 阅读

29 ·

CC 4.0 BY-SA版权

文章标签：

#自动驾驶 #运动规划 #增强图 #级联排序

面向自动驾驶的运行时限可调运动规划

摘要

针对道路自动驾驶的轨迹规划方法通常采用通过累加多个加权特征项计算的单目标（SOMWF）进行公式化方法优化。此类公式化方法通常存在规划可调性不足的问题。其主要原因有两个：一是由于SOMWF的复杂性，特别是当特征数量较多时，缺乏物理直觉；二是缺乏特征相对优先级。本文通过提出一种包含多个可调规划阶段的框架以及两种新颖技术来解决这一问题：
- 无需优化的轨迹平滑/微调
- 基于采样的级联排序轨迹搜索

一、引言

自动驾驶载客车辆（APV）已展现出其在消除人为驾驶错误、实现更安全高效的交通系统方面的潜力。轨迹规划器（TP）在APV系统中起着重要作用，因为它决定了主车的具体运动。TP的共同目标是找到一条轨迹，以优化定义为多个加权特征项累加的单一目标，即单目标多加权特征（SOMWF）公式化方法。

然而，由于随着特征数量增加而缺乏物理直觉，SOMWF目标的调参变得困难。同时，也难以合理解释特定的权重配置，特别是当需要提供明确的语义解释和/或显式指定特征间的优先级时。我们致力于通过一个包含三个可调的规划阶段的框架以及两种新颖技术来解决这些问题：
- 基于增强图的无迭代轨迹平滑/微调。
- 基于基元采样和级联排序的轨迹搜索。

二、相关工作

轨迹规划器大致可分为基于优化的和基于搜索的。基于优化的方法通过迭代变形轨迹来优化代价目标函数 [1],[2],[3],[4],[5]。基于搜索的方法利用特定应用的采样模式构建图以搜索路径或轨迹。根据采样性质，规划器可进一步分为随机的或预定义的。随机的方法以随机方式探索环境，例如概率路线图（PRM）[6],[7]和快速探索随机树（RRT）[8],[7]算法。预定义的方法则采用固定模式进行探索，例如局部动态窗口法（DWA）[9]或状态格点[10]。图构建完成后，可使用A*/D* [11],[12]等启发式搜索算法来获取最优轨迹。例外的是，RRT类算法通常将图构建与搜索过程结合起来。

两种规划方法已应用于自动驾驶领域，Tran [13] 和 Kelly [14] 均采用基于优化的轨迹规划器，通过应用序列凸规划实现赛车的时间最优性。Ziegler[4]开发了一种基于优化的轨迹规划器，通过优化一个包含五项特征的目标函数，并满足轨迹的内部和外部约束。Li [15]提出了一种用于道路导航的规划器，使用支持向量机进行 modeling。另一方面，Urmson[16]和 Montemerlo[17]在2007年DARPA城市挑战赛中开发的道路内规划器采用了预定义的基于搜索的方法，该方法受DWA[9]启发，通过生成短时视野局部轨迹候选来跟踪车道中心线并避开路边障碍物。近期的道路规划器[18],[19],[20],[21],[22]构建了一个符合道路结构的时空搜索空间。Chen [23]提出了一种引导式启发式搜索方法，采用先探索后搜索策略以减小规划器的搜索空间。

自动驾驶规划器的一个主要需求是保证算法运行时间。对于基于优化的方法，其优化过程的连续特性无法保证在有时间限制的情况下收敛到最优解。对于基于搜索的方法，其运行时间很大程度上依赖于可接受启发式函数的质量，而该质量可能依赖于环境。我们希望采用一种运行时间预先确定的算法，即使只能得到分辨率完备结果1，也能在固定时间界限内终止。

先前的方法旨在通过最小化由多个加权特征项求和定义的最优性目标，即SOMWF公式化方法。例如，Ziegler[4]优化了一个目标函数，该函数考虑了中心线偏移、速度差异、加速度、急动度和偏航率，同时将无碰撞行为作为外部约束。另一个例子是McNaughton[19]优化由十三个特征成本项组成的目标函数。SOMWF定义中缺乏物理直觉，使其难以直观地进行调优。局部极小值的存在进一步为基于优化的和基于搜索的求解器带来了困难。对于前者，局部极小值使得解高度依赖于初始优化条件；对于后者，缺乏可接受的启发式方法使得搜索过程效率低下，有时产生非最优解。

另一类研究工作将物理直觉引入运动规划。经典方法将规划问题表述为动力系统，并旨在找到此类系统的平衡状态作为解，例如人工力场 [24],[25],[26],[27],[28]。然而，这些规划器的求解器均为基于优化的，且存在上述运行时间问题。

本文旨在开发一种可调且运行时有界的自动驾驶轨迹规划器。该规划器由多个可调的规划阶段组成，并包含两种新颖的规划技术。下文第三节阐述所提出的规划方法，第四节展示评估结果，第五节进行总结并展开进一步讨论。

III. 规划方法

整体规划方案包括三个步骤：
1) 生成平滑的参考轨迹以优化原始车道几何形状。
2) 修改参考轨迹以避开静态障碍物。
3) 从围绕先验参考轨迹采样的局部参数化轨迹中进行搜索，以避开动态障碍物。

前两个规划步骤均采用相同的增强图基技术进行表述。该技术是无迭代求解器的核心，能够数值逼近用于传统轨迹平滑和轨迹微调的基于优化的方法。

最后的规划步骤采用了基于轨迹采样的原始搜索方法，并结合一种新颖的级联轨迹排序机制。这种公式化方法使得规划器能够进行优先轨迹选择，并对其输出结果提供清晰的语义解释。

A. 增强图上的无迭代求解器

道路驾驶的运动规划任务可以抽象为在由车道边界自然限定的通道内进行规划。该通道内的构型空间被离散化为 N层横向采样的空间节点n S，构成如图1a所示的空间图 GS。通过连接相邻层上的节点生成空间边 e S ，并对每条空间边进一步进行碰撞检测。

与在 GS上进行规划不同，我们通过将 GS上的每个空间节点与其输入/输出空间边相结合，构建增强图 GA，如图1b所示。其优势在于，每个增强节点 n A不仅包含一个空间节点，还包含关于可能的组合信息通过空间边连接邻近空间节点。这使得可以计算依赖于空间节点及其邻居的目标函数。当一个增强节点的入边与另一个增强节点的出边相同时，这两个增强节点相连。

每个增强节点都具有成本 c(nA)。规划问题就是找到一条在 GA 上的增强节点序列 n∗A 1 → n∗A 2 → · · · → n∗A N，使其每个节点的累积成本最小。
$$
\arg \min_{n^ _A 1,n^ A 2,…,n^*_A N} \sum {i=1}^{N} c(n_iA)
$$

示意图0

GA是一个有向无环图，因此使用Dijkstra算法进行图搜索等同于应用动态规划，该方法是计算高效的。

总体而言，该技术采用预定义的基于采样公式化方法和穷尽搜索技术，因此能够在预定义的运行时间内终止。

规划结果是 GA上的一系列增强节点。将投影回 GS 的空间节点连接起来，我们得到由一系列空间边组成的分段线性路径。为了进一步生成平滑的参考轨迹，采用包含运动学半车模型和纯追踪控制器的车辆模型，前向仿真跟踪该分段线性路径的过程，并记录仿真车辆状态。

在获得一条运动学可行的平滑路径后，可进一步使用基于约束的速度曲线生成方案来生成参考轨迹[29]。在下一小节中，将基于增强节点成本的不同定义，开发两种规划器。

1) 参考路径平滑

可以利用车道边界的全部宽度对原始车道中心线进行进一步平滑。如图2所示，对于每个增强节点 n A，其成本计算如下：
$$
c(n_A) = \omega_l \cdot l + \omega_{\Delta h} \cdot \Delta h
$$
其中 l是相对于空间节点 nC对应 nA的中心线的横向偏移量，$\Delta h$是nA在连接 nL的输入边与连接 nR的输出边之间的航向变化量：
$$
\Delta h = \left| \frac{n_C - n_L}{|n_C - n_L|} \cdot \frac{n_R - n_C}{|n_R - n_C|} \right|
$$

示意图1

2) 参考路径推移

当观察到静态障碍物干扰正常道路行驶时，应修改或微调参考路径。可在增强图上构建一种受弹性带方法[24],[30],启发的推移例程。如图3所示，在扭曲的道路坐标系中，为每个增强（弹性）节点 nA计算三个力。它们的合力定义为 nA的残余力 fres。从概念上讲，较大的 fres会使nA变得不稳定，这与原始弹性带公式化方法中的情况一致：
$$
f_{res} = \omega_{fa} \cdot f_a + \omega_{fc} \cdot f_c + \omega_{fr} \cdot f_r
$$
其中 fa和 fr是通过人工力场获得的吸引力和排斥力，如[30]中所述。fc计算每个扩展节点的收缩力。
$$
f_c = (n_L + n_R - 2 * n_C) \cdot \rho
$$
其中 $\rho$ 是用于计算收缩力的人工弹簧常数。因此，每个 nA 的总成本定义为残余力的范数：
$$
c(n_A) = |f_{res}|
$$

示意图2

B. 局部参数化轨迹规划

局部规划器生成可执行轨迹以控制主车运动。本文阐述了可执行运动基元、采样模式以及一种新颖的级联排序方案的设计。

1) 轨迹基元

采用一对三次曲率和线性加速度（二次速度曲线）多项式来简洁地参数化轨迹基元：
$$
\kappa_c(s, P) = p_0 + p_1 \cdot s + p_2 \cdot s^2 + p_3 \cdot s^3 \
a_c(t, Q) = q_0 + q_1 \cdot t
$$
其中 κ是曲率，a是加速度，曲率多项式P ={p0,p1,p2,p3}和加速度多项式Q={q0, q1, q2}。

该轨迹基元被解释为带时间戳的几何曲线。使用运动学自行车模型(3)来评估轨迹基元：
$$
\dot{X}(t; P, Q) :=
\begin{cases}
\dot{x}(s) = \cos\theta(s) \
\dot{y}(s) = \sin\theta(s) \
\dot{\theta}(s) = \kappa_c(s, P) \
\dot{s}(t) = v(t) \
\dot{v}(t) = a_c(t, Q)
\end{cases}
$$
其中 x、 y和 θ为全局位姿， v为标量速度， s为弧长。

轨迹生成解决的是一个边界满足问题，其中找到P和 Q，使得轨迹从起始状态 X s开始，并在目标状态 X g结束：
$$
{P^ , Q^ , t_f^*} = \arg \min_{P,Q,t_f} |X_g - X_s|
$$
其中动学由以下给出：
$$
X_g = X_s + \int_0^{t_f} \dot{X}(t; P, Q) \cdot dt
$$

2) 时空采样模式

基于采样的时空规划生成一组候选轨迹以供评估。每条轨迹由一系列路径段和速度曲线组成，其中路径段指定空间曲线，速度曲线指定沿路径的速度。采样被解耦为空间和时间上的独立采样。

示意图3

示意图4

空间采样通过利用状态格点，在先前生成的参考路径周围进行。对于车道保持和变道场景，均采用两种采样模式。对于车道保持（图4a），构建了一个由前方采样节点分层组成的格点主车在选定的决策参考线周围，横向采样逐渐减少。对于变道（图4b），构建一个格点，该格点由出发车道和目标车道中沿决策参考线的节点组成。完整的空间规划是从起始节点到终止节点层在格点上由多个路径段组成的路径序列。

速度(米/秒)
-2 0 2 4 6 8 10 12 14 16 18
Time(s)
0
0.5
1
1.5
2
2.5
3

示意图5

对于每条路径序列，通过均匀改变公式4中的Q来采样一组速度曲线，从而将路径转换为轨迹。线性加速度速度参数化支持采样丰富的具有稳定/增加/减小加速度的短期速度曲线（图5）。

3) 分桶级联评估

采样的可行轨迹必须根据某些指标进行排序，排名最高的轨迹即为最优执行轨迹。传统规划方法定义了一个包含加权特征项的累积目标成本函数（SOMWF），并用它来对所有候选轨迹进行排序：
$$
C = \sum_{i=1}^{M} \omega_{x_i} \cdot f_{x_i}
$$
其中 xi表示 ith特征，$\omega_{x_i}$ 和 $f_{x_i}$ 分别是特征 xi的权重和成本函数，M是特征项数量。

当 M较大时，调参较为困难。同时，也无法对特征间的优先级进行编码。因此，我们提出了一种针对候选轨迹的级联排序机制：对轨迹候选集进行排序，使得具有较低成本的高优先级特征的轨迹始终排在前面，而不论其低优先级特征的成本如何。这种优先级排序过程为轨迹选择提供了清晰的语义解释。

优先级	特征
1	fS		m到静态物体的最小距离
2	fM		m到移动物体的最小距离
3	flat	m/s²	最大横向加速度
4	flon	m/s²	最大纵向加速度
5	fRv	m/s	与参考时间差的平均值
6	fRp	m	与参考空间差的平均值

Unit说明

示意图6

IV. 结果

所提出的规划方案在仿真环境中进行了广泛评估，其中主车以及行人、自行车骑行者和其他车辆等周围物体均被分别建模。在接下来的讨论中，所提出的规划器用P表示，而两个基准规划器分别表示为：
- B-I：Boss 的道路内规划器 [16]。
- B-II：在 [19] 中开发的时空格子规划器。

A. 计算复杂度

参考平滑和nudging规划器在80米的纵向范围和2米的分辨率下采样40个空间节点层（Nlayer）。每层在一个4米的横向跨度上以0.2米的分辨率采样20个空间节点（Ns−node/layer）。任意两个相连空间节点之间的横向/纵向偏移量的最大比值为0.3，因此出入度（Ndegree）的数量为5。对于计算成本较高的碰撞检测操作，将评估Nedge条边：
$$
N_{edge} = N_{layer} \cdot N_{s-node/layer} \cdot N_{degree} \approx 4,000
$$
因此，为每个空间节点构造了25个增强节点（Na−node/s−node）。增强节点的代价计算数量由以下公式给出：
$$
N_{a-node} = N_{layer} \cdot N_{s-node/layer} \cdot N_{a-node/s-node} \approx 20,000
$$
然而，由于其纯算术计算特性，这在计算上并不密集。

在轨迹采样与搜索中，构建了一个三层的局部空间格网轨迹。我们采用三层格网设计，每层的横向采样数量依次从 $N_{L1}= 5$ 减少到 $N_{L2}= 3$ 再到 $N_{L3}= 1$。然后创建一组路径序列，将起始节点连接到第三层的终止节点：
$$
N_{path} = N_{L1} \cdot N_{L2} \cdot N_{L3} + N_{L1} \cdot N_{L3} + N_{L2} \cdot N_{L3} + N_{L3} = 22
$$
然后，对于每条路径，我们在加速度范围从 $a_{min}= -4.0$ 到 $a_{max}= 3.0$、分辨率为 $\Delta a= 0.5$ 的情况下，均匀采样一组线性加速度曲线，使其开始和停止。因此，每条路径上的轨迹总数（$N_{traj/pathway}$）最多为14。因此，轨迹总数 $N_{traj}$ 为：
$$
N_{traj} = N_{traj/pathway} \cdot N_{pathway} = 308
$$
与B-I相比，P的计算更复杂，但无需行为规划器提供动作层级监督来触发车辆跟随、避让和变道等基本动作。与需要超过20万次显式轨迹评估的B-II相比，P由于采用了更简单的物理直观公式，所需计算量显著降低。

B. 实验

图7b展示了该场景的初始设置。主车位于右车道，前方有一名慢速前行的自行车骑手。左车道有三辆快速行驶的车辆以恒定速度行驶，形成两个间隙，其中间隙1小于间隙2。图7b显示的是车道保持情况，规划器能够利用较短的间隙1绕行前方慢速骑行的自行车骑手。在图7c所示的变道情况下，规划器则利用较长的间隙2变道至目标车道。规划器做出这一选择的原因是，变道规划器知晓在终止状态时，车辆需要与前车保持安全距离，而选择间隙1无法满足这一要求。

示意图7

示意图8

示意图9

进一步评估了一个具有挑战性的单车道场景（图8）：主车在存在多个周围物体的环境中沿单车道行驶，这些物体包括两辆停放的汽车、一个垃圾桶、一个过街行人以及一个慢速前行的自行车骑手（5米/秒）。评估了目标函数参数的三种配置（表二）。图8a至8c展示了在所有三种配置下的规划器执行复杂机动序列：绕行静态物体、为行人减速以及与前方骑行者保持距离。需要注意的是，车辆无法保证在期望间距处进行绕行。例如，在密集静态物体区域，由于最大间距低于期望距离，车辆必须灵活通过；或由于横向约束限制，车辆不允许过多进入相邻车道。

P首先独立评估每个特征，保持其原始含义，然后使用级联排序，确保特征间的优先级。相比之下，B-I和B-II使用权重来调整规划器的行为，而P使用特征独立参数和分桶大小。这种公式化方法提供了清晰的语义解释，以实现直接的调优结果。

特征
		配置1	配置2	配置3
$f^*_{Obin}$	m	[0.2,∞)	[0.4,∞)	[0.8,∞)
$f^*_{Opcar}$	m	[0.2,∞)	[0.4,∞)	[0.8,∞)
$f^*_{Oped}$	m	[2.0,∞)	[4.0,∞)	[8.0,∞)
$f^*_{Obike}$	m	[5.0,∞)	[10.0,∞)	[20.0,∞)
$f^*_{lat}$	m/s²	[0, 0.5)	[0, 0.5)	[0, 0.5)
$f^*_{lon}$	m/s²	[0, 1.0)	[0, 1.0)	[0, 1.0)
$f^*_{Rv}$	m/s	[0, 1.0)	[0, 1.0)	[0, 1.0)
$f^*_{Rp}$	m	[0, 0.2)	[0, 0.2)	[0, 0.2)