很久以前半翻译加描述的写过一篇关于群体避障算法 导航动态避让算法RVO的优化ORCA(Optimal Reciprocal Collision Avoidance)。
英语的语境直译成中文的文章十分不好理解，所以打算重新整理一遍，这篇算是精简优化版的吧，只讲要点不详细描述推导过程。
需要说明的是，ORCA算法是RVO算法的优化，所以有些地方也称ORCA算法为RVO2。

官方文档 ：ORCA.pdf

序言

这个ORCA.pdf 文档上使用了大量的数学表达式来帮助描述问题，一眼看去显得十分深奥，晦涩难懂。但！实际上这些数学表达式，它们几乎都是为了把文档上描述的情况如何用在数学表达式上定义下来，描述的内容主要还是普通的逻辑。这样一来这些公式显得“温和”多了，所以如果熟悉了一些数学符号，那么会发现并没有想象中的那么难，相反，这些数学等式还十分有助于帮助理解）。

正文

---

文中用了大量的英文或数学符号来表示特定的属性，阅读时不好分清，因此这里把它们都一一罗列出来，可以先往下阅读，等遇到不明白的英文字母或者数学符号属性时再来查找。

这是定义在一个2D平面上的动态避让，称平面上的运动单位为机器人，每个机器人的形状大小一致。
机器人的属性分为内部属性和外部属性。任何一个机器人只能观察到其它机器人的外部属性。
对于其它机器人的内部属性，则是根据设想其它机器人跟自己用同样的避障策略而推算出来。

文中提到速度和速率的区别：
速度：有方向有长度，即向量，用$\mathbf{v}$表示。
速率：无方向有长度，即常量，用$v$表示。
-----------------------------------------------------------------------------------------------
在一个2D平面$R{}^2$上定义以下内容：
$A$ ： 机器人A（设想机器人是2D平面上的一个圆）。
$V_{{}_A}$ ：机器人A的任意速度集合。
------------$A$ 的外部属性----------------
$P_{{}_A}$ : 机器人A的位置（机器人形状的中心点）。
$r_{{}_A}$ : 机器人A形状的半径。
${\mathbf{v}}_{{}_{\mathbf{A}}}$ : 机器人A当前速度。
------------$A$的内部属性-----------------
$v{}_A^{max}$ : 机器人A的最大速率（常量）。
$\mathbf{v}{}_A^{pref}$ : 机器人A的最倾向速度。
$\mathbf{v}{}_A^{new}$ : 机器人A新选择的速度。
------------评估运算相关属性-----------
$D(\mathbf{p},r$) : 以点$\mathbf{p}$为圆心，半径为$r$的圆。
$\tau$ ： 某一小段时间，常量。
$V_{{}_A}$ : $A$的任意速度集合，未具体指定。
$VO{}_{A|B}^{\tau }$ : $A$对$B$的$\tau$时间内有碰撞速度集合（不考虑$B$的速度影响，设$B$为静态）。
$CA{}_{A|B}^{\tau }$ : $A$对$B$的$\tau$时间内无碰撞速度集合（有考虑$B$的速度影响，设$B$为动态）。
$ORCA{}_{A|B}^{\tau }$ : $A$对$B$的$\tau$时间内最理想互惠无碰撞的速度集合。
$ORCA{}_A^{\tau }$ : $A$对其它任何机器人的$\tau$时间内最佳互让无碰撞的速度集合。
$\mathbf{v}{}_A^{opt}$ : $A$对$B$最优化避免碰撞自适应速度，这是个动态的速度，跟周围机器人数量自动适，在周围其它机器人数量密度低时，${\mathbf{v}}_A^{opt}$偏向$\mathbf{v}{}_A^{pref}$，周围机器人数量密度高时$\mathbf{v}{}_A^{opt}$偏向$0$。
-----------------------------------------------------------------------------------

算法大体思路图

图解：
以红色圆为$A$，其它颜色的圆各叫$BCDEF$等。
图中各种颜色的直线就是上述的平面的分割线，$A$对于$B$的半平面有效的一侧下文称为$ORC{A}_{A|B}^{\tau }$。白色区域是$A$避开多个其它颜色圆的多个半平面的交集区域，也就是自己的可选速度范围，下文称为$ORC{A}_A^{\tau }$(白色区域图中)。左边黑色点位置是目的地点，$A$的期望速度${\mathbf{v}}_A^{pref}$(图中灰色线段)总是指向它，毕竟那是自己要去的地方嘛。而$A$的当前速度 ${\mathbf{v}}_A^{}$(图中黑色线段)的下一步选择新速度${\mathbf{v}}_A^{new}$(图中的黑色线段的下一步状态)时也想指向它，却无奈被限制在白色区域里，但它不死心，所以它每在下一步选择新速度${\mathbf{v}}_A^{new}$都会尽可能的向灰色速度靠近，直到到达目的地。由此，风骚走位图形成

4、 互让避免碰撞

4.1、准备工作

图1

图1a：在方位空间上，有2个机器人A和B。
图1b：在速度空间上，障碍速度$VO{}_{A|B}^{\tau }$(灰色部分)几何形状为一个截头圆锥体，它的顶点位于原点，它的两边与以$P{}_A-P{}_B$为中心$r_{{}_A}+r_{{}_B}$为半径的圆相切。圆锥体的截头范围由$(r_{{}_B}+r_{{}_A})$$\tau$决定，上图为$\tau =2$时的范围。
说明：这个坐标系是速度空间坐标系，假设$B$为静止状态，如果$A$移动时不撞上$B$，那么$A$的速度不能为$VO{}_{A|B}^{\tau }$，几何意义可以解释为$A$在$\tau$时间段内不能往$B$方向走超过$(P_{{}_B}-P_{{}_A})-(r_{{}_B}+r_{{}_A})$的距离，否则发生碰撞。
图1c：在速度空间上，$A$不与$B$发生碰撞的速度为$CA{}_{A|B}^{\tau }$。
说明：$VO{}_{A|B}^{\tau }$与$V_{{}_B}$的明科夫斯基和（图1（c）亮灰色部分)）， 其几何意义可以解释为，这是把$V_{{}_B}$（图1（c）暗灰色部分）考虑在内后得出的$A$相对于$B$在$\tau$时间内会碰撞的速度集合，那么它们的补集为$CA{}_{A|B}^{\tau }$。

所以有：
设$D$是以$\mathbf{p}$为圆心，$r$为半径的圆。
D ( p , r ) = { q ∣    ∣ ∣ q − p ∣ ∣ < r } D(\bf p, r) = {\lbrace{\bf q} |\; ||q-p||<r \rbrace} D(p,r)={q∣∣∣q−p∣∣<r}
那么有表达式1：
V O A ∣ B τ = { v ∃ t ∈ [ 0 , τ ] : : t v ∈ D ( p B − p A , r A + r B ) } VO_{_{A|B}}^{τ} = {\lbrace{\bf v}∃t \in [0,τ] :: t{\bf v} \in D({\bf p}_{_B} −{\bf p}_{_A}, r_{_A} + r_{_B})\rbrace} VOA∣B​τ​={v∃t∈[0,τ]::tv∈D(pB​​−pA​​,rA​​+rB​​)}
说明：
$\exists$：存在的意思。
$::$ :等比的意思。
$V{O}_{{}_{A|B}}^{\tau }$表达式解释为：时间段$\tau$范围内存在一个时间$t$，使得$t\ast \mathbf{v}$的结果速度集合为$D$，即上图1(b)的截头圆锥体的截头范围由$\tau$决定。

那么有表达式2：
$$C{A}_{{}_{A|B}}^t(V_{{}_B})=\mathbf{v}|\mathbf{v}\notin V{O}_{{}_{A|B}}^t\oplus V_{{}_B}$$
表达式说明：
$\oplus$ : 明科夫斯基和的意思，在这里的几何意义是，$A$在找出障碍速度时，把$B$的速度考虑进去，而不再设想$B$是静态的。
$C{A}_{{}_{A|B}}^t$($V_{{}_B}$)表达式解释为：$A$对应$B$避免碰撞的速度集合为$V{O}_{{}_{A|B}}^t$⊕$V_{{}_B}$的补集。

图2

图解：

$ORCA{}_{A|B}^{\tau }$是被一条直线分割开的一个半平面集合，这个半面集合为$\mathbf{n}$指向的一侧（这个条件确定半平面的方向）。
其中这条线垂直于$\mathbf{u}$，且穿过点${\mathbf{v}}_{{}_A}^{opt}+\frac{1}{2}$（这两个条件就可以确定一条直线）。
$\mathbf{u}$为以${\mathbf{v}}_{{}_A}^{opt}-{\mathbf{v}}_{{}_B}^{opt}$为起点，指向最接近$ORCA{}_{A|B}^{\tau }$边界上的点的向量。

4.2、求$ORCA{}_{A|B}^{\tau }$：

步骤如下：
1、求$\mathbf{u}$:
则有表达式：
$$\mathbf{u}=(\underset{\mathbf{v}\in \partial VO{}_{A|B}^{\tau }}{\mathrm{argmin}}||\mathbf{v}-({\mathbf{v}}_{{}_A}^{opt}-{\mathbf{v}}_B^{opt})||)-({\mathbf{v}}_A^{opt}-{\mathbf{v}}_B^{opt})$$
说明：
$\mathrm{a}\mathrm{r}\mathrm{g}\mathrm{m}\mathrm{i}\mathrm{n}$ : 表达式函数，当范数为最小值时得到的$\mathbf{v}$的值。
$||n||$ : n的范数。
$\partial$ : 偏导数符号。
表达式解释为：
假设$A$h和$B$的自适应速度分别为$\mathbf{v}{}_A^{opt}$和$\mathbf{v}{}_B^{opt}$，如果$A$会撞上$B$碰撞，则${\mathbf{v}}_{{}_A}^{opt}-{\mathbf{v}}_{{}_B}^{opt}\in VO{}_{A|B}^{\tau }$。
$\mathbf{u}$是以${\mathbf{v}}_{{}_A}^{opt}-{\mathbf{v}}_{{}_B}^{opt}$为起点，指向最接近$ORCA{}_{A|B}^{\tau }$边界上的点的向量。

2、求$\mathbf{n}$:
$$\mathbf{n}=({\mathbf{v}}_{\mathbf{A}}^{\mathbf{o}\mathbf{p}\mathbf{t}}-{\mathbf{v}}_{\mathbf{B}}^{\mathbf{o}\mathbf{p}\mathbf{t}})+\mathbf{u}$$
表达式解释为:
$\mathbf{n}$是以$VO{}_{A|B}^{\tau }的边界上的点$$(({\mathbf{v}}_A^{opt}-{\mathbf{v}}_B^{opt})+\mathbf{u})$为起点向外延伸作的法线。
3、求$ORCA{}_{A|B}^{\tau }$
得到
O R C A A ∣ B τ = { v   ∣   ( v − ( v A o p t + 1 2 u ) ) ⋅ n ≥ 0 } ORCA{^τ_{A|B}} = \lbrace{\bf v} \,|\, ({\bf v-({\bf v}_{A}^{opt}+\frac12{\bf u}))·{\bf n}≥0}\rbrace ORCAA∣Bτ​={v∣(v−(vAopt​+21​u))⋅n≥0}
表达式解释为：
$ORCA{}_{A|B}^{\tau }$是被一条直线分割开的一个半平面集合，这个半面集合为$\mathbf{n}$指向的一侧（这个条件确定半平面的方向）。
其中这条线垂直于$\mathbf{u}$，且穿过点${\mathbf{v}}_{{}_A}^{opt}+\frac{1}{2}$（这两个条件就可以确定一条直线）。

图4a：8个机器人的方位，箭头所指的方向为它们的速度。
图4b：图中阴影半平面为$A$z在其他机器人的影响下可选的速度，当前所有机器人$\tau =2$，${\mathbf{v}}_{{}_{\ast }}^{opt}={\mathbf{v}}_{\ast }$(表达式解释为${\mathbf{v}}_{{}_{\ast }}^{opt}$选择任意一个可选的速度，在[0-${\mathbf{v}}_{{}_{\ast }}^{pref}$]内)。图中切碎的阴影块区域就是$ORC{A}_A^{\tau }$。箭头则是$A$当前的速度。

5、对于多个机器人碰撞的情况

有流程图：

5.1、基本解决方案

$A$对于其它所有$B$共同的$ORCA$:
$$ORC{A}_A^{\tau }=D(0,v_A^{max})\bigcap (\bigcap _{A\ne B}ORC{A}_{A|B}^{\tau })$$
表达式解释为：
$ORC{A}_A^{\tau }$的集合$\mathbf{v}$为$A$的最大速率为半径的圆集合与相对于其它$B$的$ORCA$的交集。

$A$得到的新速度：
$${\mathbf{v}}_{{}_A}^{new}=\underset{\mathbf{v}\in ORCA{}_A^{\tau }}{\mathrm{argmin}}||\mathbf{v}-({\mathbf{v}}_{{}_A}^{pref})||$$
表达式解释为：
求得$A$的速度${\mathbf{v}}_{{}_A}^{new}$为$ORCA{}_A^{\tau }$中最接近${\mathbf{v}}_{{}_A}^{pref}$的速度。
（说明：求${\mathbf{v}}^{new}$需要用到线性规划法提高效率。）
$A$得到的新位置：
$${\mathbf{p}}_{{}_A}^{new}={\mathbf{p}}_{{}_A}^{}+{\mathbf{v}}_A^{new}∆t$$
表达式解释为：
$A$的新位置为$A$的旧位置加上速度乘以时间。

性能优化：
其实对于$A$只关心与自己接近的其它机器人，即超出距离$(v{}_A^{max}+v{}_B^{max})\tau$的 则忽略，可以用$kD-tree$找出范围内的机器人，然后用线性规划法求出最合适的速度。

5.2、剩下的问题，关于${\mathbf{v}}_{{}_A}^{pref}$的选择：

图5a：当三个机器人如图向$A$移动时，$A$能选择的速度为0。
图5b：$A$可选择的半平面速度集合，在所有机器人的$\tau =2$，${\mathbf{v}}_{{}_{\ast }}^{opt}={\mathbf{v}}_{\ast }$条件下，$ORCA{}_A^{\tau }$有可能为空，这就无法保证在$\tau$时间内不发生碰撞。
图5c：$A$可选择的半平面速度集合，在所有机器人的$\tau =2$，${\mathbf{v}}_{{}_{\ast }}^{opt}=0$条件下，如图区域${\mathbf{v}}_{{}_{\ast }}^{opt}={\mathbf{v}}_{\ast }$为0，结果可能会导致看起来机器人行动僵硬，不自然。

如何选择${\mathbf{v}}_{{}_A}^{pref}$：
1、${\mathbf{v}}_{{}_A}^{opt}={\mathbf{v}}_{{}_A}$（对于所有机器人）：根据机器人当前情况，在$[0,{\mathbf{v}}^{pref}]$范围内自适应选择合适的速度，周围低密度时选择偏向${\mathbf{v}}^{pref}$，高密度时选择偏向$0$。但关于$ORCA{}_A^{\tau }$高密度时依然可能为空，这是使用线性规划法求解最佳速度会失败，所以需要用到3D线性规划法求解最佳速度，以此保证有解，接下来下文会讲到。

5.3、密集条件下

当所有机器人选择${\mathbf{v}}_{{}_A}^{opt}={\mathbf{v}}_{{}_A}$时（即根据环境选择自适应速度），在机器人高密度的情况下时$ORCA{}_A^{\tau }$可能为空，线性规划法求最最新速度会失败，为了足够的安全，使用3D线性规划法。
3D线性规划法：
设$d_{}{}_{A|B}(\mathbf{v})$为$\mathbf{v}$到半平面$ORCA{}_{A|B}^{\tau }$边上的欧氏距离（非垂直距离），如果$\mathbf{v}\in ORCA{}_{A|B}^{\tau }$，那么$d_{}{}_{A|B}(\mathbf{v})$为负数。那么选择的新速度${\mathbf{v}}_{{}_A}^{new}$则是在同时与多个半平面的欧式距离最大化时的速度中，选择一个长度最短的速度。这个理解起来有些绕，画个图：

图解：
图中橙色和绿色的箭头就是点$0$分别同时到各个半平面的最大化欧式距离，那么选择其中最短的欧式距离所指向的点作为新的速度，即图中橙色的箭头指向的点。

通过3D线性规划法求出的${\mathbf{v}}_{{}_A}^{new}$表达式：
$${\mathbf{v}}_{{}_A}^{new}=\underset{\mathbf{v}\in D(0,v_{{}_A}^{max})}{\mathrm{argmin}}\underset{B\ne A}{\max }{d}_{}{}_{A|B}(\mathbf{v})$$
表达式解释为：
几何上， 各个半平面以相同的速度向外垂直移动半平面的边，直到得出一个有效的速度。

以上可以用3D线性规划法求解，对于其它每个机器人B，距离函数$d_{}{}_{A|B}(\mathbf{v})$是第三维$(\mathbf{v},d)$平面空间。要查找的新的点$({\mathbf{v}}^{\ast },d^{\ast })$则是各个平面距离函数算出的结果中在所有平面上的交集部分（即上图中橙色和绿色所指向的目标点的速度），选择它们中$d$值最小的$\mathbf{v}$为${\mathbf{v}}_{{}_A}^{new}$赋值。不管机器人密度情况如何，3D线性规划总能有解。

5.4、静态障碍

图6a：$A$和障碍线段$O$。
图6b：$\tau =2$时的障碍速度$VO{}_{A|O}^{\tau }$几何图。
图6c：$ORCA{}_{A|O}^{\tau }$的半平面切割线相切于$VO{}_{A|O}^{\tau }$的边界且与$\mathbf{v}{}_A^{opt}$距离最短。其中$\mathbf{v}{}_A^{opt}$总是等于$0$。

可以假设障碍物是由多条线段组成的模型。设$O$为这些线段中的其中一条，$A$是以${\mathbf{p}}_{}{}_A$为原点$r_{}{}_A$为半径的机器人，那么对$O$的障碍速度为$VO{}_{A|O}^{\tau }$:

V O A ∣ O τ = { v ∃ t ∈ [ 0 , τ ] : : t v ∈ O ⊕ − D ( p A , r A ) } VO_{_{A|O}}^{τ} = {\lbrace{\bf v}∃t \in [0,τ] :: t{\bf v} \in O⊕- D({\bf p}_{_A}, r_{_A} )\rbrace} VOA∣O​τ​={v∃t∈[0,τ]::tv∈O⊕−D(pA​​,rA​​)}

$V{O}_{{}_{A|O}}^{\tau }$是一个非凸平面，是一个平头的锥体，因此不能用上述的线性规划算法高效算出新的速度。
对于障碍$O$，所有机器人选择$\mathbf{v}{}_A^{opt}=0$，这能保证总存在有效速度在$\tau$时间内避免碰撞。