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

官方文档：ORCA.pdf

序言

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

正文

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

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

文中提到速度和速率的区别：
速度：有方向有长度，即向量，用v\bf vv表示。
速率：无方向有长度，即常量，用vvv表示。
-----------------------------------------------------------------------------------------------
在一个2D平面R2R{^2}R2上定义以下内容：
AAA ：机器人A（设想机器人是2D平面上的一个圆）。
VAV_{_A}VA ：机器人A的任意速度集合。
------------AAA 的外部属性----------------
PAP_{_A}PA : 机器人A的位置（机器人形状的中心点）。
rAr_{_A}rA : 机器人A形状的半径。
vA\bf v_{_A}vA : 机器人A当前速度。
------------AAA的内部属性-----------------
vAmaxv{^{max}_A}vAmax : 机器人A的最大速率（常量）。
vApref{\bf v}{^{pref}_A}vApref : 机器人A的最倾向速度。
vAnew{\bf v}{^{new}_A}vAnew : 机器人A新选择的速度。
------------评估运算相关属性-----------
D(p,rD({\bf p}, rD(p,r) : 以点p\bf pp为圆心，半径为rrr的圆。
τττ ：某一小段时间，常量。
VAV_{_A}VA : AAA的任意速度集合，未具体指定。
VOA∣BτVO{^τ_{A|B}}VOA∣Bτ : AAA对BBB的τττ时间内有碰撞速度集合（不考虑BBB的速度影响，设BBB为静态）。
CAA∣BτCA{^τ_{A|B}}CAA∣Bτ : AAA对BBB的τττ时间内无碰撞速度集合（有考虑BBB的速度影响，设BBB为动态）。
ORCAA∣BτORCA{^τ_{A|B}}ORCAA∣Bτ : AAA对BBB的τττ时间内最理想互惠无碰撞的速度集合。
ORCAAτORCA{^τ_{A}}ORCAAτ : AAA对其它任何机器人的τττ时间内最佳互让无碰撞的速度集合。
vAopt{\bf v}{^{opt}_A}vAopt : AAA对BBB最优化避免碰撞自适应速度，这是个动态的速度，跟周围机器人数量自动适，在周围其它机器人数量密度低时，vAopt{{\bf v}^{opt}_A}vAopt偏向vApref{\bf v}{^{pref}_A}vApref，周围机器人数量密度高时vAopt{{\bf v}}{^{opt}_A}vAopt偏向000。
-----------------------------------------------------------------------------------

算法大体思路图

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

4、互让避免碰撞

4.1、准备工作

图1

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

所以有：
设DDD是以p\bf pp为圆心，rrr为半径的圆。
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：
VOA∣Bτ={v∃t∈[0,τ]::tv∈D(pB−pA,rA+rB)}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)}
说明：
∃∃∃：存在的意思。
:::::: :等比的意思。
VOA∣BτVO_{_{A|B}}^{τ}VOA∣Bτ表达式解释为：时间段τττ范围内存在一个时间ttt，使得t∗vt*\bf vt∗v的结果速度集合为DDD，即上图1(b)的截头圆锥体的截头范围由τττ决定。

那么有表达式2：
CAA∣Bt(VB)=v∣v∉VOA∣Bt⊕VBCA_{_{A|B}}^{t}(V_{_B}) = {{\bf v}|{\bf v}\notin VO_{_{A|B}}^{t}⊕V_{_B}} CAA∣Bt(VB)=v∣v∈/VOA∣Bt⊕VB
表达式说明：
⊕⊕⊕ : 明科夫斯基和的意思，在这里的几何意义是，AAA在找出障碍速度时，把BBB的速度考虑进去，而不再设想BBB是静态的。
CAA∣BtCA_{_{A|B}}^{t}CAA∣Bt(VBV_{_B}VB)表达式解释为：AAA对应BBB避免碰撞的速度集合为VOA∣BtVO_{_{A|B}}^{t}VOA∣Bt⊕VBV_{_B}VB的补集。

图2

图解：

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

4.2、求ORCAA∣BτORCA{^τ_{A|B}}ORCAA∣Bτ：

步骤如下：
1、求u\bf uu:
则有表达式：
u=(arg min⁡v∈∂VOA∣Bτ∣∣v−(vAopt−vBopt)∣∣)−(vAopt−vBopt){\bf u}=( \argmin_{{\bf v}\in ∂VO{^τ_{A|B}}}|| {\bf v}−({\bf v}_{_A}^{opt}−{\bf v}_{B}^{opt})||)−({\bf v}_{A}^{opt}−{\bf v}_{B}^{opt})u=(v∈∂VOA∣Bτargmin∣∣v−(vAopt−vBopt)∣∣)−(vAopt−vBopt)
说明：
argmin\rm arg minargmin : 表达式函数，当范数为最小值时得到的v\bf vv的值。
∣∣n∣∣||n||∣∣n∣∣ : n的范数。
∂∂∂ : 偏导数符号。
表达式解释为：
假设AAAh和BBB的自适应速度分别为vAopt{\bf v}{^{opt}_A}vAopt和vBopt{\bf v}{^{opt}_B}vBopt，如果AAA会撞上BBB碰撞，则vAopt−vBopt∈VOA∣Bτ{\bf v}^{opt}_{_A}-{\bf v}^{opt}_{_B}∈VO{^τ_{A|B}}vAopt−vBopt∈VOA∣Bτ。
u\bf uu是以vAopt−vBopt{\bf v}^{opt}_{_A}-{\bf v}^{opt}_{_B}vAopt−vBopt为起点，指向最接近ORCAA∣BτORCA{^τ_{A|B}}ORCAA∣Bτ边界上的点的向量。

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

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

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

有流程图：

5.1、基本解决方案

AAA对于其它所有BBB共同的ORCAORCAORCA:
ORCAAτ=D(0,vAmax)⋂(⋂A≠BORCAA∣Bτ)ORCA_{A}^{τ} = D(0,v_{A}^{max})\bigcap (\bigcap_{A\neq B} ORCA_{A|B}^{τ}) ORCAAτ=D(0,vAmax)⋂(A=B⋂ORCAA∣Bτ)
表达式解释为：
ORCAAτORCA_{A}^{τ}ORCAAτ的集合v{\bf v}v为AAA的最大速率为半径的圆集合与相对于其它BBB的ORCAORCAORCA的交集。

AAA得到的新速度：
vAnew=arg min⁡v∈ORCAAτ∣∣v−(vApref)∣∣{\bf v}^{new}_{_A}=\argmin_{{\bf v}\in ORCA{^τ_{A}}}|| {\bf v}−({\bf v}^{pref}_{_A}) ||vAnew=v∈ORCAAτargmin∣∣v−(vApref)∣∣
表达式解释为：
求得AAA的速度vAnew{\bf v}^{new}_{_A}vAnew为ORCAAτORCA{^τ_{A}}ORCAAτ中最接近vApref{\bf v}^{pref}_{_A}vApref的速度。
（说明：求vnew{\bf v}^{new}vnew需要用到线性规划法提高效率。）
AAA得到的新位置：
pAnew=pA+vAnew∆t{\bf p}^{new}_{_A}={\bf p}^{}_{_A}+{\bf v}^{new}_A∆tpAnew=pA+vAnew∆t
表达式解释为：
AAA的新位置为AAA的旧位置加上速度乘以时间。

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

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

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

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

5.3、密集条件下

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

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

通过3D线性规划法求出的vAnew{\bf v}^{new}_{_A}vAnew表达式：
vAnew=arg min⁡v∈D(0,vAmax)max⁡B≠AdA∣B(v){\bf v}^{new}_{_A}= \argmin_{{\bf v}\in D({0,v^{max}_{_A})}} \max_{B\neq A} d_{}{_{A|B}}({\bf v})vAnew=v∈D(0,vAmax)argminB=AmaxdA∣B(v)
表达式解释为：
几何上，各个半平面以相同的速度向外垂直移动半平面的边，直到得出一个有效的速度。

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

5.4、静态障碍

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

可以假设障碍物是由多条线段组成的模型。设OOO为这些线段中的其中一条，AAA是以pA{\bf p}_{}{_A}pA为原点rAr_{}{_A}rA为半径的机器人，那么对OOO的障碍速度为VOA∣OτVO{^τ_{A|O}}VOA∣Oτ:

VOA∣Oτ={v∃t∈[0,τ]::tv∈O⊕−D(pA,rA)}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)}

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

ORCA(RVO2)算法优化整理版相关推荐

【AI PC端算法优化】二，一步步优化自然饱和度算法
上一节的RGB转灰度图算法我又做了两个相关优化,加入了多线程以及去掉了上次SSE计算中的一些重复计算,现在相对于传统实现已经可以获得4倍加速.同时我也在做一个AVX2的优化,所以不久后我将发布一个RG ...
避障算法 - VO、RVO 以及 ORCA (RVO2)[转载]
本文转载于:http://k.sina.com.cn/article_3743149912_df1bef5801901cp6s.html 所谓避障算法,主要目的就是"避障"(废话) ...
KM算法（DFS版，优化DFS版，BFS版）
KM算法的前提是图存在一个完备匹配,因此用于二分图的最佳匹配问题.如果是最大权匹配问题,可以通过加权值为0的边来可以将图的最佳匹配与最大全匹配统一起来:如果是最小权匹配问题,可以通过加权值为-INF的 ...
高效算法，B*寻路算法，python版，思维优化（3），python实现B*寻路算法
接下来我们对路径进行优化椭圆属性,圆上的每个点到 a.b 两点的距离都是一样的,我们选择爬过障碍的最短路径,计算每个点到起点和穿透点的距离之和 ,最大说明该障碍点离两个点最远,也就是绕过障碍的 ...
智源青年科学家袁洋：机器学习可靠性与算法优化的方法探索
2020年2月11日,在"智源论坛Live | 青年科学家线上报告会"上,智源青年科学家.清华大学助理教授袁洋作了题为<机器学习可靠性与算法优化>的演讲.袁洋,2018 ...
【AI PC端算法优化】一，一步步优化RGB转灰度图算法
欢迎关注AI PC端算法优化代码库:https://github.com/BBuf/Image-processing-algorithm-Speed . 0. 资源获取公众号输入高性能计算关键词 ...
李宏毅深度学习笔记——呕心整理版
李宏毅深度学习笔记--呕心整理版闲谈叨叨叨: 之前看过吴恩达的一部分课程,所以有一定理论基础,再看李宏毅的课程会有新的理解.我先以有基础的情况写完学习过程,后续再以零基础的角度补充细节概念(估计不会 ...
初等数论--同余--MILLER-RABIN素性检测算法优化
初等数论--同余--MILLER-RABIN素性检测算法优化 Euler theorem.Fermat's little theorem Primality Test: Fermat.MILLER-R ...
与金山云的樊博士聊了聊AV1算法优化以及如何提升沉浸式视频的沉浸感
讲师介绍樊鸿飞,金山云高级研发总监,北京大学计算机科学与技术专业博士,负责视频云的VR.视频编码.人工智能等技术线研发,近年来主要从事沉浸式视频.视频编码.图像处理.计算机视觉方向上的研究与技术落地 ...

ORCA(RVO2)算法优化整理版

序言

正文

算法大体思路图

4、互让避免碰撞

4.1、准备工作

图解：

4.2、求ORCAA∣BτORCA{^τ_{A|B}}ORCAA∣Bτ：

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

5.1、基本解决方案

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

5.3、密集条件下

5.4、静态障碍

ORCA(RVO2)算法优化整理版相关推荐

最新文章

热门文章

ORCA(RVO2)算法优化整理版

序言

正文

算法大体思路图

4、 互让避免碰撞

4.1、准备工作

图解：

4.2、求ORCAA∣BτORCA{^τ_{A|B}}ORCAA∣Bτ​：

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

5.1、基本解决方案

5.2、剩下的问题，关于vApref{\bf v}^{pref}_{_A}vA​pref​的选择：

5.3、密集条件下

5.4、静态障碍

ORCA(RVO2)算法优化整理版相关推荐

最新文章

热门文章

4、互让避免碰撞

4.2、求ORCAA∣BτORCA{^τ_{A|B}}ORCAA∣Bτ：

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