论文阅读-Velocity obstacle guided motion planning method in dynamic environments

---

---

常见方法

📝

1. 最主要的方法在规划算法中使用距离度量作为安全约束

2. 基于速度障碍的移动机器人在移动障碍物环境中的运动规划方法

3. 动态避障的方法主要包括反应式方法、基于模型预测控制（MPC）的方法、基于速度障碍的方法和基于规划的方法

   1. 反应式方法是一种控制方法，它仅通过检查动态障碍物的当前状况来做出单步控制决策,常见的四种反应式方法包括动态窗口法（DWA）、人工势场法（APF）、社会力法和不可避免碰撞状态法（ICS） 。然而，当障碍物表现出非线性运动或环境过于复杂时，这些方法可能导致保守或振荡行为

      1. 基于速度障碍（VO）的方法:作为反应式方法的一个子集，速度障碍（VO）方法可以用于生成一系列无碰撞动作，并广泛应用于多智能体导航和动态避障 。速度障碍表示机器人和障碍物之间潜在碰撞的区域，它基于它们的相对速度和距离来确定 。通过选择VO区域外部的速度，机器人可以有效地实现避障任务 。然而，大多数基于VO的方法倾向于保守和短视，这不可避免地降低了效率和轨迹质量

   2. 基于MPC的方法将轨迹规划问题表述为带有安全约束的非线性最优控制问题，以确保避障 。与反应式方法相比，基于MPC的方法生成的轨迹更符合机器人动力学，并解决了反应式方法的短视问题。然而，求解非线性动态障碍物约束在缺乏适当初始猜测时，可能导致收敛到局部最优

   3. 在动态环境中，基于规划的方法往往比反应式方法更鲁棒、更安全，因为它们利用了动态障碍物的预测信息来评估未来时间域内规划轨迹的安全性 。许多这类工作将动态障碍物参数化为几何形状（如多面体或椭球体），搜索运动基元以获得可行路径，然后求解非线性优化问题来获得轨迹 。然而，大多数基于规划的方法在规划轨迹时仅考虑障碍物的未来位置而忽略了速度，这不足以确保在高度非线性动态场景中的机器人安全 。

   方法名称	核心思路	优点	缺点
   1. 反应式方法	只看当前障碍物状态，马上做出反应（如DWA、APF）	反应快（效率高）	短视（只看一步），容易导致保守、抖动或在复杂环境中失败
   2. 基于VO的方法	是反应式的一种，通过计算速度障碍区（VO）来确定哪些速度会撞上。VO区域是基于相对速度和距离来定义的 。	避障效果好（效率高）	同样有保守和短视的缺点
   3. 基于MPC的方法	把规划看作一个复杂的数学优化问题，用模型预测未来几步的运动	轨迹平滑，符合机器人动力学，不短视 。	计算量大（效率低），如果初始值选不好容易卡在局部最优解 。
   4. 基于规划的方法	利用障碍物的预测信息，提前规划未来较长时间的路线	鲁棒性高，更安全 。	计算量大（效率低）。而且通常只看障碍物未来的位置，没有充分利用“速度”信息，在高度动态环境中不够安全 。

1. 相对碰撞锥 (RCC) 的几何定义

首先，我们定义相对碰撞锥 ($RCC$) 。这是所有会导致机器人和障碍物碰撞的相对速度的集合。

公式 (1)

R C C r , b = { V r , b ∣ λ ( P , V r , b ) ∩ B ′ ≠ ∅ } RCC_{r,b}=\{V_{r,b}|\lambda(P,V_{r,b})\cap B^{\prime}\ne\emptyset\} RCCr,b​={Vr,b​∣λ(P,Vr,b​)∩B′=∅}

• $RC{C}_{r,b}$ ： 机器人 $r$ 相对于障碍物 $b$ 的相对碰撞锥 。

  $V_{r,b}$： 机器人 $r$ 和障碍物 $b$ 之间的相对速度 。

  $\lambda (P,V_{r,b})$： 从机器人位置 $P$ 发出，方向为 $V_{r,b}$ 的射线（即机器人的相对运动路径）。

  $B^{\prime }$： 经过膨胀后的障碍物 。障碍物被增大了机器人的半径，这样就可以把机器人视为一个点来简化计算 。

  判断条件 ($\ne \varnothing$)： 意味着如果这条射线与膨胀后的障碍物 $B^{\prime }$ 相交，则当前相对速度 $V_{r,b}$ 就会导致碰撞 。

理解：

想象机器人站在 P 点，如果它以$V_{r,b}$ 的相对速度移动，它的运动轨迹（射线）会穿过障碍物。所有这样的速度 $V_{r,b}$
构成的区域就是 $RCC$ 。

2. 有限相对碰撞锥 (RCC_F) - 考虑时间约束

为了减少避障的保守性 ，论文引入了时间约束 $T_f$（最小碰撞时间阈值） 。我们只关注那些在未来 $T_f$ 秒内就会发生的碰撞。

公式 (2)

R C C F = { V r , b ∣ V r , b ∈ R C C , ∣ ∣ V r , b ∣ ∣ 2 ≥ d m / T f } RCC_{F}=\{V_{r,b}|V_{r,b}\in RCC,||V_{r,b}||_{2}\ge d_{m}/T_{f}\} RCCF​={Vr,b​∣Vr,b​∈RCC,∣∣Vr,b​∣∣2​≥dm​/Tf​}

$RC{C}_F$： 有限相对碰撞锥 。
$V_{r,b}\in RCC：相对速度V_{r,b}必须是会导致碰撞的速度（在RCC$ 内部） 。
$d_m$： 机器人和障碍物之间的最小距离 。
$T_f$： 最小碰撞时间阈值 。
$||V_{r,b}|{|}_2\ge d_m/T_f$： 这个不等式等价于碰撞时间 $\tau$ 满足 $\tau \le T_f$。它限制了 $RCC$ 区域，只保留那些“很快就会撞上”（碰撞时间小于 $T_f$）的速度 。

作用： 使用 $RC{C}_F$ 而不是 $RCC$ 可以让机器人在规划轨迹时不那么保守 。它忽略了那些离得很远或者移动很慢、在可预见的未来不会造成威胁的障碍物 。

3. VO-基于安全度量 (VOSM) 的数学判断式

公式 (3) 是一个判别式，它将几何概念 $RCC$ 转化为一个用于判断碰撞状态的数值表达式 $F_{vo}^k$。这使得它能够被直接用于计算机算法（CheckVO()）和非线性优化。

公式 (3)

$$\begin{gathered} F_{vo}^k=\zeta f_{vo}^k>0 \\ {f}_{vo}^k=(I_k^{\top }{v}_k{)}^2+(R_k^{\prime 2}-||I_k|{|}_2^2)||v_k|{|}_2^2 \end{gathered}$$

关键变量：

• $\zeta$： 惩罚函数 $\zeta =\text{softmax}(-\cos \theta )$ 20202020，用于将成本映射到 0 或 1。

判断逻辑：

• $f_{vo}^k$ 是一个基于几何关系的表达式 。当 $f_{vo}^k>0$ 时，表明机器人与障碍物可能发生碰撞 。圆与射线相交判断的代数形式

  $\cos \theta <0$： 表示相对速度向量 $v_k$ 的方向指向相对位置向量 $I_k$ 的反方向，即机器人正在靠近障碍物。

  结论： 仅当 ($\cos \theta <0$) 且 ($f_{vo}^k>0$) 时，碰撞关系才成立，此时 $F_{vo}^k>0$ 。

  VOSM 违反： 如果 $F_{vo}^k>0$，则机器人的状态违反了VOSM（相对速度 $v_k$ 在 $RCC$ 区域内）。

第 1 步：拆解 $f_{vo}^k$ —— 核心碰撞几何检查

我们来逐项分析 $f_{vo}^k$ 的含义：$f_{vo}^k=\underset{\text{项 A: 相对位置在速度方向上的投影}}{\underbrace{(I_k^{\top }{v}_k{)}^2}}+\underset{\text{项 B: 几何距离与安全半径的比较}}{\underbrace{(R_k^{\prime 2}-||I_k|{|}_2^2)||v_k|{|}_2^2}}$

符号定义 (您的机器狗项目背景)

• $I_k$ (Relative Position): 机器狗与第 $k$ 个障碍物之间的相对位置向量。

  • 在您的 2D 项目中，这是一个 $(x,y)向量。||I_k|{|}_2$ 就是它们之间的当前距离。

• $v_k$ (Relative Velocity): 机器狗与第 $k$ 个障碍物之间的相对速度向量 ($V_{robot}-V_{obstacle}$)。

• $R_k^{\prime }$ (Inflated Safety Radius): 障碍物的半径 + 机器狗的半径 + 额外的安全裕度。

项 A：

• $I_k^{\top }{v}_k$： 这是 $I_k$ 和 $v_k$ 的点积，它衡量了相对位置和相对速度的一致性。

  • 在几何上，它等于 $||I_k||\cdot ||v_k||\cdot \cos \theta$，是相对位置在相对速度方向上的投影。

• 作用： 它是计算圆心到射线距离的关键部分。

项 B：

• 子项 $(R_k^{\prime 2}-||I_k|{|}_2^2)$：

  • 如果 $||I_k|{|}_2<R_k^{\prime }$（机器狗已经在安全半径内），这个子项是负数。

  • 如果 $||I_k|{|}_2>R_k^{\prime }$（机器狗在安全半径外），这个子项是正数。

• 作用： 这一项与距离的平方有关。如果它是一个很大的正数，表示机器狗离碰撞区域太远，碰撞风险极低。

关键的代数结论：

在几何中，当圆心到直线的垂直距离小于等于圆的半径时，直线与圆相交。代数判别式 $f_{vo}^k$ 就是来判断这个条件的：

• 如果 $f_{vo}^k>0$： 意味着几何上相交，即机器狗的相对运动路径会穿过膨胀后的障碍物。

• 如果 $f_{vo}^k\le 0$： 意味着几何上不相交，即路径是安全的。

• 【独家深度推导】这个公式是怎么来的？

  这其实是直线与圆相交判别式的变体。

  1. 设定场景：

     • 相对位置矢量：$I_k=P_{robot}-P_{obs}$。

     • 相对速度矢量：$v_k$。

     • 膨胀半径：$R_k^{\prime }$。

     • 时刻 $t$ 的相对位置：$P(t)=I_k+t\cdot v_k$。

  2. 碰撞条件：

     在未来某个时刻 $t>0$，相对位置进入了障碍物圆内，即距离平方小于半径平方：

     $||P(t)|{|}^2\le R_k^{\prime 2}$

     代入 $P(t)$ 展开：

     $||I_k+t{v}_k|{|}^2\le R_k^{\prime 2}$

     $(I_k+t{v}_k)\cdot (I_k+t{v}_k)\le R_k^{\prime 2}$

     $I_k\cdot I_k+2t(I_k\cdot v_k)+t^2(v_k\cdot v_k)\le R_k^{\prime 2}$

     整理为一个关于时间 $t$ 的一元二次不等式：

     $||v_k|{|}^2\cdot t^2+2(I_k^T{v}_k)\cdot t+(||I_k|{|}^2-R_k^{\prime 2})\le 0$

     令 $a=||v_k|{|}^2$, $b=2(I_k^T{v}_k)$, $c=||I_k|{|}^2-R_k^{\prime 2}$。即 $a{t}^2+bt+c\le 0$。

  3. 判别式分析：

     如果这个二次不等式有解（即存在时刻 $t$ 发生碰撞），那么对应的二次方程 $a{t}^2+bt+c=0$ 必须有实根。这意味着判别式 $\Delta \ge 0$。

     $\Delta =b^2-4ac$$

     $\Delta {=}^2-4||v_k|{|}^2(||I_k|{|}^2-R_k^{\prime 2})$

     $\Delta =4(I_k^T{v}_k{)}^2-4||v_k|{|}^2||I_k|{|}^2+4||v_k|{|}^2{R}_k^{\prime 2}$

     $\frac{\Delta }{4}=(I_k^T{v}_k{)}^2+(R_k^{\prime 2}-||I_k|{|}^2)||v_k|{|}^2$

     关键发现：请看上面推导出的 $\frac{\Delta }{4}$，这正是论文中的 Eq (3) $f_{vo}^k$！

     • 物理含义：$f_{vo}^k$ 本质上就是碰撞检测方程的判别式。

     • 当 $f_{vo}^k>0$，说明直线与圆相交（潜在碰撞）。

     • 当 $f_{vo}^k<0$ ，说明直线与圆相离（安全）。

     • 当 $f_{vo}^k=0$，说明直线与圆相切。

第 2 步：拆解 $\zeta$ —— 相对方向检查

仅仅 $f_{vo}^k>0$ 还不够！因为一条射线可以穿过一个圆，但这并不意味着我们一定会撞上。

举个例子： 假设人在 $5m$ 外，背对机器狗跑远。机器狗的路径虽然会“穿过”人当前位置所在的圆，但人也在移动，因此永远不会追上。

我们需要确保障碍物是迎面而来，或者至少不是在跑远。这就是 $\zeta$ 的作用。

$\zeta =\text{softmax}(-\cos \theta )$

• $\theta$ 是 $I_k$ 和 $v_k$ 之间的夹角。

• $\cos \theta$ 衡量了两个向量的方向关系：

  • $\cos \theta <0$ ($\theta >90^{\circ }$)：$v_k$ 指向 $I_k$ 的反方向，正在互相靠近。

    • $I_k$ 指向机器狗的位置–>$v_k$ 也指向机器狗的运动方向

    • 相对速度 $v_k$ 有一个指向机器狗位置 $I_k$ 方向的分量。这意味着：机器狗正在远离障碍物，或者说，障碍物正在追不上机器狗

  • $\cos \theta >0$ ($\theta <90^{\circ }$)：$v_k$ 指向 $I_k$ 的方向，正在互相远离。

    • $I_k$ 是从障碍物指向机器狗–>$v_k$ 指向机器狗的运动反方向

    • 相对速度 $v_k$ 有一个指向障碍物位置 $I_k$ 反方向的分量。这意味着：机器狗正在向障碍物靠近，或者说，碰撞距离正在减小。

    • 场景 1：迎面相撞 (风险极高，> 90 )

      • 行人： 向机器狗走来。

      • 机器狗： 向行人走去。

      • $I_k$： 从行人指向机器狗。

      • $v_k$ (相对速度)： 向量 $v_k$ 的方向是迎着行人冲过去的。

      • 夹角 $\theta$： $I_k$ 和 $v_k$ 几乎完全反向，$\theta \approx 180^{\circ }$。

      • $\cos \theta$： $\cos (180^{\circ })=-1$。

      • 判断结果： $\cos \theta <0$ 成立！正在互相靠近，碰撞风险高。

      场景 2：机器狗超车 (低风险， < 90)

      • 行人： 向前走，速度 $1.0m/s$。

      • 机器狗： 从行人后方超车，速度 $2.0m/s$。

      • $I_k$： 从行人指向机器狗（机器狗在行人前方）。

      • $v_k$ (相对速度)： $v_k=2.0-1.0=1.0m/s$，方向向前。

      • 夹角 $\theta$： $I_k$ 和 $v_k$ 几乎同向，$\theta \approx 0^{\circ }$。

      • $\cos \theta$： $\cos (0^{\circ })=1$。

      • 判断结果： $\cos \theta >0$ 成立！正在互相远离（机器狗正在拉开与行人的相对距离），风险低。

  • $I_k$ 是从障碍物指向机器狗的向量。机器狗相对于它在哪里

  • $v_k$ 是机器狗相对于障碍物的相对速度。机器狗正在以什么速度冲向/远离障碍物

  • 作者引入 $\zeta =\text{softmax}(-\cos \theta )$ 作为一个平滑的开关函数。当夹角指向物体时，$\zeta \approx 1$，激活惩罚；当背离时，$\zeta \approx 0$，忽略惩罚。

关键的逻辑：VO 导引的路径搜索

$F_{vo}^k=\zeta f_{vo}^k$ 只有在 $\cos \theta <0$（正在靠近）时才会有效惩罚 $f_{vo}^k>0$ 的状态。如果 $\cos \theta >0$（正在远离），$\zeta$ 会把惩罚项映射为 0 或一个极小值，忽略碰撞警告。

4. 碰撞时间 $\tau$ 的计算

这个公式主要用于计算碰撞时间，并与 $T_f$ 进行比较（即 $RC{C}_F$ 的逻辑）。

公式 (4)

${\tau }_k=\frac{d_m}{||v_k\cos \theta |{|}_2}=\frac{||l_k|{|}_2^2-||l_k|{|}_2{R}_k^{\prime }}{||l_k^{\top }{v}_k|}$

• ${\tau }_k$： 机器人与第 $k$ 个障碍物之间的碰撞时间 。

作用：

这个公式被用于构建轨迹优化中的 $S_d$ 惩罚项。在轨迹优化时，只有当 ${\tau }_{ijk}<T_f$ 时，才会计算惩罚 $F_{vo}^{ijk}$（见公式12） 31。这正是将有限碰撞锥 ($RC{C}_F$) 的概念融入到优化约束中的关键步骤。

感知：

首先应用体素网格滤波器来降低机载传感器点云的密度，使用基于密度的聚类算法（DBSCAN）来生成一组簇 A c l s = { C 1 , C 2 , . . . , C n } A_{cls}=\{C_{1},C_{2},...,C_{n}\} Acls​={C1​,C2​,...,Cn​}，其中每个簇 $C_i$ 对应一个障碍物。根据 $C_i$ 的质心和尺寸，计算最小包围球。随后，通过匈牙利算法将特定的跟踪轨迹分配给 $C_i$，以最小化赋值成本，该成本被公式化为簇质心与预测轨迹之间的欧几里得距离 $d_s$。如果 $d_s$ 超过设定的阈值 $D_{max}$，则会为该簇创建一个新的轨迹。每个障碍物 $C_i$ 的历史轨迹存储在相应的滑动窗口 W i t = { C i t − n △ t , . . . , C i t − △ t , C i t } W_{i}^{t}=\{C_{i}^{t-n\triangle t},...,C_{i}^{t-\triangle t},C_{i}^{t}\} Wit​={Cit−n△t​,...,Cit−△t​,Cit​} 中。最后，通过二次多项式拟合历史轨迹的位置，以获得预测轨迹，然后将其用于运动规划模块。

VO 导引的路径搜索

算法流程 (Algorithm 1) 解析：

这是一个典型的Kinodynamic A*（动力学 A*）算法的变体。

1. Initialize()：初始化优先队列。

2. PopMin()：取出当前代价最小的节点。

3. GetNeighbour()：根据机器人的动力学模型（如微分驱动模型），生成下一时刻可能到达的一组状态。

4. CheckVO()：这是关键步骤。对每一个生成的子状态，利用 Eq (3) 计算 $F_{vo}$。

   • 如果 $F_{vo}>0$，说明该动作不安全，continue（跳过/丢弃）。

   • 只有安全的动作才会被加入队列。

5. 代价计算 Eq (5)：

   $J(T)={\int }_0^T||u(t)|{|}^{2}dt+\rho T$

   代价由两部分组成：能量消耗（控制量 $u$ 的平方积分，越平滑越省力）和时间成本（$\rho T$，希望越快到达越好）。

B样条的数学魔力如果我们要检查轨迹是否撞墙，不需要检查曲线上的无数个点，只需要检查这几个控制点是否撞墙即可。如果控制点都在安全区域内，曲线一定安全。这极大地简化了碰撞检测的计算量。

3.4.2 优化目标函数

我们将轨迹生成转化为一个数学上的最小化问题：

$minS={\lambda }_e{S}_e+{\lambda }_v{S}_v+{\lambda }_a{S}_a+{\lambda }_d{S}_d$

这是一个多目标优化，每一项代表一种期望：

1. $S_e$ (Smoothness/Jerk) 平滑项：

   $S_e=\sum ||J_i|{|}^2$

   $J_i$ 是加加速度（Jerk）。最小化 Jerk 可以让机器人的运动如丝般顺滑，减少机械磨损，防止饮料洒出来。

2. $S_v,S_a$ (Limits) 动力学约束项：

   我们希望速度和加速度不超过最大值。作者使用了软约束（Soft Constraint） 的形式，即惩罚函数：

   $\eta (x)=\max (0,x{)}^3$

   • 如果速度小于最大值，$x<0$，惩罚为 0。

   • 如果速度超过最大值，$x>0$，惩罚值呈立方级数暴涨。这迫使优化器把速度压回安全范围内。

3. $S_d$ (VO Safety Penalty) —— 核心创新：

   $S_d=\sum \eta (F_{vo}^{ijk})$

   这里直接把 Eq (3) 计算出的判别式值 $F_{vo}$ 塞进了优化函数。

   • 原理：$F_{vo}$ 的值正比于“碰撞的危险程度”。

   • 梯度下降：优化器（如 L-BFGS）会计算 $S_d$ 相对于控制点位置 $P_i$ 的梯度（导数）。这个梯度告诉优化器：“把第 $i$ 个控制点往左移一点，判别式 $F_{vo}$ 就会变小，碰撞风险就会降低。”

   • 通过不断迭代，控制点会自动移动到 $F_{vo}<0$ 的区域，从而生成一条既平滑又避开动态障碍物的轨迹。