本文共 5787 字，大约阅读时间需要 19 分钟。

参考博文：https://blog.csdn.net/u012740992/article/details/89397714

论文文档：http://gamma.cs.unc.edu/ORCA/

1.问题定义

用简单的圆形或凸多边形代表机器

可被观察到的属性：

• 设机器A是圆形，半径$r_A$
• 机器位置$p_A$
• 当前速度$v_A$

观察不到的属性:

• 最大速度$v^{max}$
• 首选速度$V^{pref}$（大小等于最大速度，方向指向目标位置）

计算新速度$v^{new}$

• 保证所有机器在时间$\tau$内不碰撞
• 新速度尽可能的接近$V^{pref}$

2. 速度障碍区$V{O}_{A|B}^{\tau }$(velocity obstacle)

有机器AB

速度障碍区$V{O}_{A|B}^{\tau }$

• 把A化作质点，B可以当做半径$r_A+r_B$的圆
• B相对A的位置$p_B-p_A$
• A相对B的速度$v_r=v_A-v_B$
• 如果A与B的相对速度$v_A-v_B$在障碍速度$V{O}_{A|B}^{\tau }$范围内，在时间$\tau$内，AB一定会碰撞
  V O A ∣ B τ = { v ∣ ∃ t ∈ [ 0 , τ ] : : t v ∈ D ( p B − p A , r A + r B ) } VO_{A|B}^{\tau} = \{v|\exists t \in [0,\tau]:: tv \in D(p_B - p_A,r_A + r_B)\} VOA∣Bτ​={
  v∣∃t∈[0,τ]::tv∈D(pB​−pA​,rA​+rB​)}
• $V{O}_{A|B}^{\tau }$和$V{O}_{B|A}^{\tau }$关于原点对称

3.避免碰撞速度集合$C{A}_{A|B}^{\tau }(V_B)$(collision-avoiding velocities)

• 闵可夫斯基和
  $$X⨁Y=x+y|x\in X,y\in Y$$
• 如果$v_b\in V_B$，那么$v_A\in C{A}_{A|B}^{\tau }(V_B)$时，在时间$\tau$内AB不会碰撞
  C A A ∣ B τ ( V B ) = { v ∣ v ∉ V O A ∣ B τ ⨁ V B } CA_{A|B}^{\tau}(V_B) = \{v|v \notin VO_{A|B}^{\tau} \bigoplus V_B\} CAA∣Bτ​(VB​)={
  v∣v∈/​VOA∣Bτ​⨁VB​}

4. 相互最大化（reciprocally maximal）

$$\begin{array}{r}{V}_A=C{A}_{A|B}^{\tau }(V_B)\\ V_B=C{A}_{B|A}^{\tau }(V_A)\end{array}$$

5.ORCA(optimal reciprocal collision avoidance)

优化速度$v_A^{opt}$

(optimization velocities)，为了定义ORCA引出的概念。

• 虽然避免碰撞速度集合中有很多对速度$v_A,v_B$，但希望选择出最接近优化速$v_A^{opt}，v_B^{opt}$的速度对
• 一般来说优化速度$v_A^{opt}$等于当前速度$v_A$，这样机器尽可能少的偏离当前的轨道

ORCA几何定义

当$v_A-v_B\in V{O}_{A|B}^{\tau }$

• $u$是使点$v_A-v_B$到速度障碍区$V{O}_{A|B}^{\tau }$边缘最近的点的向量。即使AB避免碰撞需要做的最小改变。
• $n$是该点的法向量
• AB都是可移动的机器，每人承担一半的避免碰撞的责任
• $ORC{A}_{A|B}^{\tau }$是一个被直线分隔的半平面，这条直线通过点$v_A^{opt}+\frac{1}{2}u$，方向垂直于$n$。
• $ORC{A}_{B|A}^{\tau }$是一个被直线分隔的半平面，这条直线通过点$v_B^{opt}-\frac{1}{2}u$，方向垂直于$n$。
  O R C A A ∣ B τ = { v ∣ ( v − ( v A o p t + 1 2 u ) ) ⋅ n ≥ 0 } ORCA_{A|B}^{\tau} = \{v|(v - (v_A^{opt} + \frac{1}{2}u))\cdot n \geq 0\} ORCAA∣Bτ​={
  v∣(v−(vAopt​+21​u))⋅n≥0}
  ORCA定义
• $ORC{A}_{A|B}^{\tau }$和$ORC{A}_{B|A}^{\tau }$互相避免碰撞和最大化
  $$\begin{array}{r}C{A}_{A|B}^{\tau }(ORC{A}_{B|A}^{\tau })=ORC{A}_{A|B}^{\tau }\\ C{A}_{B|A}^{\tau }(ORC{A}_{A|B}^{\tau })=ORC{A}_{B|A}^{\tau }\end{array}$$
• 并且$ORC{A}_{A|B}^{\tau }$和$ORC{A}_{B|A}^{\tau }$区域比其他的避免碰撞速度集合$V_A\in C{A}_{A|B}^{\tau }(V_B),V_B\in C{A}_{B|A}^{\tau }(V_A)$更接近$v_A^{opt},v_B^{opt}$
  $$\begin{array}{rl}|ORC{A}_{A|B}^{\tau }\bigcap D(v_A^{opt},r)|& =|ORC{A}_{B|A}^{\tau }\bigcap D(v_B^{opt},r)|\\ & \ge \min (|V_A\bigcap D(v_A^{opt},r)|,|V_B|\bigcap D(v_B^{opt},r))\end{array}$$

6.多个机器避免碰撞

$$ORC{A}_A^{\tau }=D(0,v_A^{max})\bigcap _{B\ne A}ORC{A}_{A|B}^{\tau }$$

再从线性规划出的区域中选择最接近首选速度$V_A^{pref}$的速度 $$v_A^{new}=\arg \underset{v\in ORC{A}_A^{\tau }}{\min }\parallel v-v_A^{pref}\parallel$$

• 这时都是在$v_A^{opt}=v_A$的基础上规划出的区域。也有可能规划出的区间为空$ORCA=\varnothing$。如果选择合适的$v_A^{opt}$,ORCA永远不会是空，永远有解。并不明白
• 可以通过选择“附近”的机器，减少计算量(kd-tree寻找)

7.如何选择优化速度$v_A^{opt}$

• $v_A^{opt}=0$

所有机器的优化速度为0时，保证$ORC{A}_A^{\tau }$永远有解并不明白。但此时机器只考虑位置关系不考虑当前速度。机器密集时，也会导致死锁。

• $v_A^{opt}=v_A^{pref}$
  当首选速度的值很大时，线性规划不可行，导航不安全。
• $v_A^{opt}=v_A$
  一个折中选择，优化速度会自适应。在低密度情况下会接近首选速度；高密度情况下会接近0.

8.高密度情况

• 如果$ORC{A}_A^{\tau }=\varnothing$，不能保证$\tau$时间内不会发生碰撞。这时选择一个“最安全的速度”.
• $d_{A|B}(v)$代表速度$v$到$ORC{A}_{A|B}^{\tau }$的距离。（如果在平面范围内，值为负数）
• 使到所有平面的最大距离最小化
  $$v_A^{new}=\arg \underset{v\in D(0,v_A^{max})}{\min }\underset{B\ne A}{\max }{d}_{A|B}(v)$$

9.静态障碍物

• 用线段$O$表示静态障碍物
• 机器负责所有避障责任
• $V{O}_{A|O}^{\tau }$由静态障碍物造成的速度障碍区
  V O A ∣ O τ = { v ∣ ∃ t ∈ [ 0 , τ ]   : t v ∈ O ⨁ } VO_{A|O}^{\tau} = \{v |\exists t\in[0,\tau]\::tv \in O \bigoplus \} VOA∣Oτ​={
  v∣∃t∈[0,τ]:tv∈O⨁}
• $ORC{A}_{A|O}^{\tau }$是一个半平面，在A的速度$v_A^{opt}$距$V{O}_{A|O}^{\tau }$最近的点做切线，由切线分割平面
• 在遇到障碍物的时候，把所有机器的$v_A^{opt}=0$，保证存在有效速度
• 同时，对于障碍物的时间$\tau$可以更小

github上有代码，代码中处理静态障碍物时的$v_A^{opt}$并不是0.仍然是将A对障碍物的相对速度（其实就等于A的当前速度，因为静态障碍物没有速度）向$V{O}_{A|B}^{\tau }$的边缘最近的点做垂线，并在垂点做切线，这条切线就是A的$ORC{A}_{A|B}^{\tau }$（因为静态障碍物没有速度，所有避免碰撞做需要做出的改变$u$都需要机器A负责，而$v_A+u$就是垂点，因此$ORC{A}_{A|B}^{\tau }$就是通过此点的切线）。