论文笔记《Reciprocal n-body Collision Avoidance》

简介

ORCA是经典的分布式底层避障算法，其任务是（对于A）：

输入：A与B的形状（$r_A$、$r_B$）、位置（$p_A$、$p_B$）、速度（$v_A$、$v_B$），以及A的偏好量（$v_A^{pref}$）和限制量（$v_A^{max}$）。
输出：不会发生碰撞的$v_A^{\prime}$。
特点：双方只要都采用ORCA，那么双方无需通信，分布式地求出来的$v_A^{\prime}$与$v_B^{\prime}$也不会发生碰撞。

跟RVO一样，ORCA也是基于VO的，但更加实用，因为：

效果上：考虑了速度的大小，使得筛选粒度更细。不像VO和RVO只考虑速度方向。
效率上：求解过程基本只用到了线性规划，比较高效。不像VO和RVO有大量的非线性求解。

引入时间窗口$\tau$

如下图(a)，在空间坐标上，有
- 位于$\bf{p_B}$，半径为$r_B$的$B$
- 位于$\bf{p_A}$，半径为$r_A$的$A$
如下图(b)，引入时间$\tau$，将空间坐标转换为速度坐标
1. 相对化空间坐标：将图(a)化为以$A$为原点，并将$A$化为质点
2. 转为速度坐标：假设$B$静止，那么当$v_A * \tau$达到红色弧线时，$A$与$B$会发生碰撞，所以VO内，速度上限为$\frac{p_{弧线上的点}}{\tau}$，得到绿色弧线。
3. 至此得到$VO^{\tau}_{A|B}$，即绿色弧线与两射线组成的类扇形
如下图(c)，考虑$B$取的速度集合$V_B$，对假设$B$静止得到的$VO^{\tau}_{A|B}$求闵氏和，得$CA^{\tau}_{A|B}(V_B)$

下面给出各变量规范的数学定义：

$D({\bf{p}},r)$：以$p$为圆心$r$为半径的圆
$$D({\bf{p}},r)=\{ {\bf{q}} | \| {\bf{q}} - {\bf{p}} \| < r \}$$
$VO^{\tau}_{A|B}$：绿色弧线和两条射线组成的类扇形
$$VO^{\tau}_{A|B} = \{ {\bf{v}} | \exists t \in [0, \tau], t {\bf{v}} \in D({\bf{p_B}} -{\bf{p_A}}, r_A+r_B) \}$$
$CA^{\tau}_{A|B}(V_B)$：考虑$B$取速度集合$V_B$
$$CA^{\tau}_{A|B}(V_B)=\{ {\bf{v}} | {\bf{v}} \notin VO^{\tau}_{A|B} \oplus V_B \}，　其中\oplus为闵氏和$$

至此，成功在VO的基础上引入了时间窗口，避障的时候不仅考虑速度方向，还考虑速度大小。

p.s. 关于VO，闵氏和等内容可以参考上一篇文章论文笔记《Reciprocal Velocity Obstacles for Real-Time Multi-Agent Navigation》

ORCA定义

ORCA全称为Optimal Reciprocal Collision Avoidance，含义就是在上述的$CA^{\tau}_{A|B}(V_B)$中选择最优的一个。

论文对最优的定义大概可以理解为在$v^{opt}$附近的合法值越多越好，即为满足下列条件的$V_A$与$V_B$对：

Reciprocally collision avoiding：很好理解，在对方的$CA$内即可，即
$$V_A \subseteq CA^{\tau}_{A|B}(V_B) \land V_B \subseteq CA^{\tau}_{B|A}(V_A)$$
Reciprocally maximal：两个速度集是最大的不碰撞速度集，$V_A$或$V_B$都没有更多的避撞速度可选
$$V_A = CA^{\tau}_{A|B}(V_B) \land V_B = CA^{\tau}_{B|A}(V_A)$$
包含最多$v^{opt}$附近的避撞速度：$v^{opt}$是一个预设值，可以理解为偏好值，论文中的最优希望：
- $v^{opt}$附近的值越多越好
双方在各自$v^{opt}$附近的避撞速度数量一样：$A$和$B$各自有$v^{opt}_A$和$v^{opt}_B$，论文中的最优希望：
- $A$求出来在$v^{opt}_A$附近的避撞速度的数量$n_A$
- $B$求出来在$v^{opt}_B$附近的避撞速度的数量$n_B$
- $n_A = n_B$

那么用数学语言来描述上面几条性质就是：
$$| ORCA^{\tau}_{A|B} \cap D({\bf{v^{opt}_A}}, r) | = | ORCA^{\tau}_{B|A} \cap D({\bf{v^{opt}_B}}, r) | \ge min(|V_A \cap D({\bf{v^{opt}_A}}, r)|, |V_B \cap D({\bf{v^{opt}_B}}, r)|)$$

其中

$ORCA^{\tau}_{A|B}$与$ORCA^{\tau}_{B|A}$：表示要求的最优速度集对，且满足reciprocally maximal，即
$$ORCA^{\tau}_{A|B} = CA^{\tau}_{A|B}(ORCA^{\tau}_{B|A}) \land ORCA^{\tau}_{B|A} = CA^{\tau}_{B|A}(ORCA^{\tau}_{A|B})$$
$V_A$与$V_B$：表示任意reciprocally collision avoiding的速度集对，即
$$V_A \subseteq CA^{\tau}_{A|B}(V_B) \land V_B \subseteq CA^{\tau}_{B|A}(V_A)$$
$r$：表示了“附近”的大小
$D({\bf{v^{opt}}},r)$：表示$v^{opt}$附近的速度

ORCA求解

论文采用构造法求解出满足上述性质的ORCA，下面先说怎么做，再说为什么这样做就能满足性质。

下面从$A$的角度介绍构造过程（图示如下），这里假设$A$知道$\bf{v^{opt}_B}$

求解相对速度${\bf{v^{opt}_A}}-{\bf{v^{opt}_B}}$
求解$VO^{\tau}_{A|B}$边界上离相对速度${\bf{v^{opt}_A}}-{\bf{v^{opt}_B}}$最近的点$\bf{x}$
求解从相对速度${\bf{v^{opt}_A}}-{\bf{v^{opt}_B}}$指向$\bf{x}$的向量$\bf{u}$
求解与${\bf{v^{opt}_A}} + \frac{1}{2}{\bf{u}}$垂直的直线，将空间分为两个平面
直线${\bf{v^{opt}_A}} + \frac{1}{2}{\bf{u}}$指向的平面即为所求$ORCA^{\tau}_{A|B}$

下面逐条解释为什么符合性质：

Reciprocally collision avoiding：
- 由构造过程可知${\bf{v^{opt}_A}}-{\bf{v^{opt}_B}}+{\bf{u}}$不在$VO^{\tau}_{A|B}$内
- $A$取新速度${\bf{v^{opt}_A}} + \frac{1}{2}{\bf{u}}$
- $B$取新速度${\bf{v^{opt}_B}} - \frac{1}{2}{\bf{u}}$
- 此时，相对速度为${\bf{v^{opt}_A}} + \frac{1}{2}{\bf{u}} - ({\bf{v^{opt}_B}} - \frac{1}{2}{\bf{u}}) = {\bf{v^{opt}_A}}-{\bf{v^{opt}_B}}+{\bf{u}}$，所以不在$VO^{\tau}_{A|B}$内
Reciprocally maximal：
- 要证$ORCA^{\tau}_{A|B} = CA^{\tau}_{A|B}(ORCA^{\tau}_{B|A})$
- 先求$CA^{\tau}_{A|B}(ORCA^{\tau}_{B|A})$：
  - 回忆之前引入时间窗口$\tau$部分，构造$CA^{\tau}_{A|B}(ORCA^{\tau}_{B|A})$分两步
    1. 先将类扇形往${\bf{v^{opt}_B}}-\frac{1}{2}{\bf{u}}$移动
    2. 沿直线$ORCA^{\tau}_{B|A}$方向扫
  - 先将类扇形往$\bf{v^{opt}_B}$方向平移（此时，类扇形中的${\bf{v^{opt}_A}}-{\bf{v^{opt}_B}}$到了$\bf{v^{opt}_A}$，且$\bf{x}$与$\bf{v^{opt}_A}$的中点在直线$ORCA^{\tau}_{A|B}$上）
  - 别忘了还要将类扇形往$-{\frac{1}{2}\bf{u}}$方向移动（此时，$\bf{x}$刚好位于直线$ORCA^{\tau}_{A|B}$上）
  - 最后再沿直线$ORCA^{\tau}_{B|A}$方向扫，得$CA^{\tau}_{A|B}(ORCA^{\tau}_{B|A})$
- 而显然$CA^{\tau}_{A|B}(ORCA^{\tau}_{B|A})$正是$ORCA^{\tau}_{A|B}$
包含最多$v^{opt}$附近的避撞速度：
- 选择了最近的$\bf{x}$进行“突围”，那“舍弃”的速度应该是最少的
- p.s. 这一点的证明不太严谨，有更好的证明欢迎留言探讨
双方在各自$v^{opt}$附近的避撞速度数量一样：
- $\bf{v^{opt}_A}$到$ORCA^{\tau}_{A|B}$的距离等于$\bf{v^{opt}_B}$到$ORCA^{\tau}_{B|A}$的距离
- 定义“附近”用的是圆

最后给出ORCA的数学定义

$$ORCA^{\tau}_{A|B} = \{ {\bf{v}} | ({\bf{v}} - ({\bf{v^{opt}_A}} + \frac{1}{2}{\bf{u}})) \cdot {\bf{n}} \ge 0 \}$$

其中

${\bf{u}}$是最小偏移
$${\bf{u}} = (\mathop{argmin}_{ {\bf{v}} \in \partial VO^{\tau}_{A|B}}{\| {\bf{v}} - ( {\bf{v^{opt}_A}} - {\bf{v^{opt}_B}} ) \|}) - ({\bf{v^{opt}_A}} - {\bf{v^{opt}_B}})$$
${\bf{n}}$是跟${\bf{u}}$同向的法向量
用${\bf{v}} \cdot {\bf{n}} \ge 0$来表示一个平面

ORCA基本用法

对其他所有agents的$ORCA$求交（线性规划），再与自己可选速度求交集，得候选速度集$ORCA^{\tau}_{A}$
$$ORCA^{\tau}_{A}=D({\bf{0}}, {\bf{v^{max}_A}}) \cap \mathop{\bigcap}_{B \ne A}{ORCA^{\tau}_{A|B}}$$
在候选集中求解跟自己偏好速度最近的一个速度${\bf{v^{new}_A}}$
$${\bf{v^{new}_A}} = \mathop{argmin}_{ {\bf{v}} \in ORCA^{\tau}_A}{\| {\bf{v}} - {\bf{v^{opt}_A}} \|}$$

可见ORCA的求解，就是在$ORCA^{\tau}_{A}$内，优化目标函数$\| {\bf{v}} - {\bf{v^{opt}_A}} \|$，所以计算效率很高。

关于${\bf{v^{opt}}}$的选择

为方便阐述，假设${\bf{v_A}} = {\bf{0}}$，这样$ORCA^{\tau}_{A|B}$就只是从原点移$\frac{1}{2}{\bf{u}}$

讨论${\bf{v^{opt}}}$的选择跟$ORCA^{\tau}_{A}$的关系

${\bf{0}}$
- 一定有解，如下图(c)。很好理解，这相当于假设了别人都静止，那只要自己也静止，必然有解。
- 密集情况下容易死锁，因为这样相当于只考虑位置信息，而不考虑速度信息
${\bf{v^{pref}}}$
- 密集情况下容易无解，如下图(b)。因为一般来说${\bf{v^{pref}}}$都比较大
- 而且实际上也不可能观察得到别人的${\bf{v^{pref}}}$
${\bf{v_{curr}}}$
- 两者的折中

无解情况的处理

基本思路跟线性代数里面的投影差不多：选择离合法速度最近的速度

$$ {\bf{v^{new}_{A}}} = \mathop{argmin}_{ {\bf{v}} \in D({\bf{0}}, {\bf{v^{max}_A}})} {\mathop{max}_{B \ne A}{ d_{A|B}({\bf{v}})}} $$

下面慢慢拆分来看

$d_{A|B}({\bf{v}})$：表示${\bf{v}}$的“违规”程度。定义为${\bf{v}}$到ORCA分割线的有向距离，${\bf{v}}$在ORCA外则取正
${\mathop{max}_{B \ne A}{ d_{A|B}({\bf{v}})}}$：对每个速度${\bf{v}}$，取“违规”最多的值作为它的“违规程度”
$\mathop{argmin}_{ {\bf{v}} \in D({\bf{0}}, {\bf{v^{max}_A}})} {\mathop{max}_{B \ne A}{ d_{A|B}({\bf{v}})}}$：在可行速度内，求”违规程度”最小的速度

收获

改进可以从以下角度考虑：
- 效果：只考虑了方向，没有考虑速度
- 效率：计算复杂度

References

Reciprocal n-body Collision Avoidance