VO避障

原文地址：https://www.researchgate.net/publication/2672667_Motion_Planning_in_Dynamic_Environments_Using_the_Relative_Velocity_Paradigm

1. 简介

动态环境下的运动规划始终是一个难题，这是因为它需要在状态空间下进行规划，也即需要同时解决路径规划和速度规划两个问题。其中，路径规划是一个运动学问题，旨在计算出一条从出发点到目标点的无碰撞路径。而速度规划由于需要考虑机器人的动态和执行器约束，天然地是一个动态规划问题。事实表明，在有限速度和多障碍条件下对平面内一点的动态运动规划是很困难的，这其中不仅包括计算的难度，还包括由于不确定的环境特性导致的解的不确定性，比如，在时间 t₀ 的可行解在之后的某个时间可能会变得不可行。

对于如何解决动态环境下的运动规划问题，最初的思路是在假定速度边界和已知障碍物运动路径的情况下，在构型空间中增加时间维度，从而形成连续时间下的构型空间。Reif和Sharir在1985年提出了不断搜索可视图的方案，以解决由单个多边形机器人和多个多边形障碍组成的平面动态运动规划问题。1987年，Erdmann和Lozano-Perez提出将连续的构型空间离散化成由一系列的微小时间段的构型空间组成的序列的方式来求解，其本质是分别解决连续片段下的静态运动规划问题，然后将所有片段的解决方案组合起来得到最终的方案。1989年Fujimura和Samet也提出了网格法解决动态运动规划问题的思路。

另外一个解决动态运动规划问题的方向是将其分解成小的子问题：即路径规划问题和速度规划问题。按照这个方法的思路，首先解决路径规划的问题，寻找出一条从出发点到目标点的可行路线。然后按照可行路线依次寻找能够避开移动障碍物的速度。Kant和Zucker首先于1986年通过可视图的方式成功得到可行的路径和速度分布。一年以后，Lee和Lee（没错，原文就是两个李）也独立地提出以类似的方式为两个协同机器人解决运动规划的问题，并且，他们还比较了在避障时采取延迟和减速两种方案对运动时间的不同影响。1993年，Fraichard将加速度边界考虑进去，通过在连续状态空间（state-time space）下搜索来求出最段路径下的速度分布。Fraichard和Laugier还在同年考虑了选定路径在被移动障碍物阻挡时切换到邻近路径的情况。1995年，Fujimura（上文提到的网格法大佬）也提出了定时网格（fixed time-dependent network）下解决网格被移动障碍物阻塞的方案。

另一种不同的思路是基于对可视图的延申，尝试创建当前环境的无障碍图。Fujimura和Samet将其定义为当机器人以最大速度运动时会与障碍物发生碰撞的具体点的轨迹。这些点就形成了一条碰撞边界，并且可以从出发点连接至目标点。无障碍图的一个特性是，如果机器人以比障碍物更快的速度运行，那么通过图中搜索得到的路径就是耗时最少的路径。（这段没看懂，最好看原文）

线上动态运动规划问题更多地关心决策的做出，而不是机器人的动态。Sanborn和Hendler曾在1988年提出交通领域的机器人应对环境的规则。机器人通过预测相关非智能体与自身移动路径的发生交叉的时间段来对外部环境做出反应。这种方式考虑了环境的物理特性，而忽略了机器人自身的物理局限性。之后又在以时间为轴的计算碰撞的笛卡尔坐标系中增加了机器人速度的条件。与之相关的另一个问题，即对未来发生碰撞的预测和排序，也开始成为研究课题。

以上所有前期研究的一个共同点，就是他们都是基于位置信息对机器人和障碍之间的碰撞进行预测。由于当前算法都是通过位置信息对潜在碰撞进行预测，所以它们被统称为零阶算法。

在本文中，我们提出一种通过速度信息直接预测随时间变化的环境中的潜在碰撞的算法，即一阶算法。这种方法利用了速度障碍的概念（Velocity Obstacle, VO），将动态环境映射到机器人的速度空间中。VO是在给定时间范围内，机器人与障碍物会在将来某个时刻发生碰撞的速度的一阶近似值。通过这种表示，可以简单地通过选择VO区域之外的速度作为避障策略。

通过在离散时间间隔下计算生成的避障策略树，然后对该树进行搜索得到一系列的避障策略，最终组成一条路径。对于线上应用，可以根据不同的设计意图的优先级，比如避免碰撞、达到目标、最大速度等，对树进行启发式搜索以进行优化。

2. VO避障说明

本节我们将介绍对于单个障碍物和多个障碍物的VO。为了简单起见，我们会将分析的物体抽象成平面上的圆圈。由于多面体也可以用多个圆圈表示，所以这是一种相对合理的抽象。同时 ，我们也假设障碍物都以特定的轨迹运动，即，在某个特定的时间节点上，我们对障碍物的瞬间状态（位置和速度）是已知或者可预测的。

图1：

现在我们考虑一个由A和B两个圆圈组成的系统（如上图）。A和B的速度分别是V_A和V_B，半径分别是R_A和R_B，此时时间为t₀。其中A代表寻路的机器人，B代表障碍物。为方便计算VO，我们首相将B映射到A的构型空间。我们可以将A抽象成一个点，同时将B的半径增加R_A,从而得到新的A’和B’。他们的状态表示各自的位置和圆心的速度。

为了方便理解，我们首先考虑相对速度。用V_AB表示A’相对于B’的相对速度，即V_AB = V_A’ - V_B’, 入_AB 表示 V_AB对应的向量。那么，我们把所有会导致A’B’发生碰撞的V_AB 的集合定义为CC_AB，其形状如下图所示。

图2：

可见，这是一个以A’为顶点的扇形，A’到B’的两条切线 入_r 和 入_f 分别形成了这个扇形的两个边界。那么显而易见，在B’保持当前形状和速度都不变的情况下，任何落在扇形区域CC_AB内的相对速度都会导致A’和B’发生碰撞，相反，任何落在扇形区域以外的相对速度都不会使二者发生碰撞。

由于我们采用的是相对速度，所以这个扇形只适用于A’和B’这一对物体的特定情况。为了将其推广，使之适用于多个碰撞体，就需要将CC_AB 转化为与之等价的由A’的绝对速度形成的区域。很简单，只要将CC_AB中的每个相对速度加上B’的速度V_B即可。又或者，只要将整个扇形区域沿B’的速度向量移动一下即可。最终得到的结果如下图所示。

图3：

最终，我们将VO定义为：
VO = CC_AB ⊕ V_B
其中⊕代表明可夫斯基向量加。
VO将A’的绝对速度分成了会碰撞和不会碰撞两个区域，只要选择VO以外的速度，就不会发生碰撞，也即满足下式：
A(t) ∩ B（t） = Φ ，if V_A(t) ∉ VO(t)
位于VO边界上的速度，将导致A’和B’刚刚好擦到。而对于固定的障碍物，VO也依然适用，此时可以将B’的速度V_B看做是0。

对于多障碍物的避障，我们可以将其视为多个独立VO的联合：

其中m表示障碍物的总个数。那么，对于这种情况，不会引发碰撞的速度则是那些在所有VO区域外部的速度，如下图所示。

图4：

对于多个障碍物的情况，对障碍物按照碰撞发生的时间顺序进行优先级排序是一个很有效的优化思路。而且，由于VO是基于对物体的速度做线性模拟得出的，这对于远处的障碍物可能并不适用，因为障碍物实际并不是完全做线性运动，因此碰撞发生时间越远的情况越可能出现误差。如果碰撞发生在给定的时间T_h之内，即 t < T_h，那么我们便称之为紧迫的（imminent）。这其中时间T_h根据不同的实际情况可能采取不同的值。我们用VO_h定义那些发生碰撞的时间超过T_h的速度的集合，用d_m表示机器人和障碍物之间的相对距离，则VO_h可定义为下式：

VO_h = { V_A | V_A ∈ VO, || V_AB || <= d_m / T_h }

从整个VO区域中将VO_h的部分去除掉，就可得到我们认为紧迫的碰撞的速度区域（下图）。

图5：

3. 避让机动

在本节中，我们将介绍能够避免在给定的一段时间内发生碰撞的避让机动，它包括一系列一步的速度变化。

3.1 可达速度

对于指定状态下的机器人A，其在给定时间Δt后的可达速度通过将执行器约束映射到加速度约束中可计算。
首先我们定义FA(t) 为机器人A在时间时的可达加速度集合，其具体表示如下：

其中X为位置向量，u为加速度影响，U代表所有可接受的控制条件。
根据上式，可以将可达速度（ RV(t + Δt) ）表示为：

这个式子很好理解，就是将速度变化按照时间进行缩放，然后与当前速度相加，得到受影响后的速度。其表示如下图所示：

图6：

而可达避障速度（RAV）定义如下：

也就是从可达速度中将属于VO区域的那部分速度扣除出去，其结果如下图所示：

图7：

上图对于RAV的表示只是一种简单的示意图，从图中可见，此时RAV区域为可达速度RV被VO从中间分开后的两段区域，很显然，对于有多个障碍物的情况，RV可能会被多个VO区域进行阻隔出现多个分散不相连的区域的结果。

3.2 避让机动的结构

避让机动可以根据它们相对于移动障碍物的位置进行区域划分。通常，避让速度会被VO区域划分为两块不相连的独立区域，如上图所示。碰撞速度和非碰撞速度区域的边界分别为A’到B’的两条切线 入_f和 入_r。只要选择的速度位于这两块独立的区域 ——也就是RAV ——之中，就可以避免和障碍物发生碰撞，但是要从哪块区域中选择避让速度呢，这首先就要先决定要从障碍物的哪一边进行避让。
首先我们如下图构建坐标系。

图8：

以B’的中心点为原点，B’的运动方向为X轴正方向，与X轴垂直且远离A’的方向为Y轴正方向。Y轴与B’的两个交点Y_f和Y_r将B’分为前后两个半圆，分别用∂B_f和∂B_r表示。

当A’相对与B’的相对速度位于入_f或入_r的时候，A‘会与B’擦肩而过，姑且称之为切线机动吧（ tangent maneuvers）。但实际上，切线机动的切点却并不会像上图中所画的那样一定落在T_f和T_r上，这是因为这与二者的绝对速度有关。下图简单表示了在采取切线机动时，从时间t₀到t₁，A’和B’二者运行到相切位置的具体情况。

图9：

可见，实际的切点并不是发生在T_f点，而是在P点。那么切点的实际位置在哪里呢，下面的定理对切点的实际位置进行了描述。

定理1：
当且仅当机器人A’以相对于B’的相对速度为V∈{入_f， 入_r}时，二者会发生切线机动。其中入_f为A’到B’前半圆的切线（切点T_f），入_r为A’到B’后半圆的切线（切点T_r）。并且，切点只会落在T_f和Y_f将B’圆周切割出的最短弧，以及T_r和Y_r将B’圆周切割出的最短弧上。

该定理的具体证明在原文附录A中有，感兴趣的老板可以自己看原文。
上文中我们提到过，VO区域的两条边界线是基于相对速度下的切线入_f和入_r转换而来，因此可知这两条边界线也就是发生切线机动的速度。由于切线机动完全由入_f和入_r决定，我们根据VO的边界以及A’到VO区域顶点的连线，将可达区域划分为如下图所示的S_r S_f 和 S_d三部分（如下图），并得到定理2。

图10：

定理2：
对于单个障碍物的可达速度区域（RAV）最多可被分割为三块互不覆盖的子区域 S_r S_f 和 S_d，它们分别代表后向区域，前向区域和远离区域。

定理2的证明也在原文附录A中（万能的附录A），感兴趣的老板依然可以自己去看原文。
前向区域表示机器人会在障碍物前方穿过障碍物的路径，从而避免碰撞，同理，后向区域表示机器人会在障碍物后方穿过其路径，而远离区域表示机器人会渐渐远离机器人而不会发生碰撞。

另外，视具体情况不同，三块区域并不一定全都存在，可能存在一种，也能存在两种（图7），也可能一种都不存在（可达速度区域完全被VO覆盖，也就是无法避障）。

对于有多个障碍物的情况，我们可以用其VO区域依次对RAV进行切割，并用S_ss表示具体的切割后区域，比如S_fr即表示第一个障碍物的前向区域和第二个障碍物的后向区域的共同区域（如下图）。

图11：

定理2在多障碍物的情况下依然适用，其证明也依然在附录A中。

4.计算避障路径

这一部分概括来讲就是，由于这个模型是基于障碍物状态可预测的前提下进行的，因此可以在事前先按照一个时间间隔，在离散的时间点上计算出机器人的当前位置以及障碍物的当前位置，然后以机器人位置为节点，按照不同的避让机动做分支，从而形成一颗树，最终达到目标点。当然，这里的避让机动可以只做几种。而且，这里也可以在进行避让机动的时候增加一些条件，进行启发式的选择，从而达到更好的效果。

由于在实际游戏开发中，我们很少会拿VO做路径规划，一般只是做寻路过程中的动态避障，所以这部分就不展开了。有感兴趣的老板，依然可以去看原文。

5. 例子

略。