导航动态避让算法RVO的优化
ORCA（OptimalReciprocalCollision。。。
来源于⽂档的主要内容：
ORCA主页
⽂档来源
本⽂要解决的问题：
n(n>0)n(n>0)n(n>0)个个体导航向⽬标点移动过程中，对于其它个体或者障碍物进⾏动态避让，并寻找最佳路径向⽬标点移动。
和A星寻路算法有什么异同？
相对⽽⾔，
ORCA是局部导航，导航⽬标是在个体⾃⼰的周围，让个体⾃⾝避开与⾃⼰接近的其它个体⽬标和障碍，ORCA只能感知到靠近⾃⾝周围的
情况，没有全局环境的信息，所以它只管导航时不与⾃⼰周围其它个体⽬标和障碍避免碰撞，或者说重叠在⼀起，却不能为⾃⾝起点和⽬标
点之间找到最短路径，这刚好是A*星寻路解决的问题。
A星寻路算法刚好和ORCA形成互补：
A星是全局寻路算法，会根据配置最⼤可能的保证找到导航个体⾃⾝起点到⽬标点的最短路径，算法的全局信息中有着整个环境的障碍信
息。但A星没有感知所有导航个体的具体状态和周围的“交通状况”信息，所以A星算法不处理可能会碰撞问题，因此多个导航个体之间可
能会重叠在⼀起。这刚好是ORCA解决的问题。
所以可以把它们结合起来，形成互补，Unity有个插件A星PathfindingProjectPro就是将两者结合起来了，形成动态避让的全局导航。
我们在本⽂中讨论的问题正式定义如下：
在⼀个共享的空间环境下有n(n>0)n(n>0)n(n>0)个机器⼈，为了简单起见，我们假设机器⼈都是圆形，空间环境则为2D空间。（这样
我们在此中更容易提出定义和算法，也能适⽤于多维）。每个机器⼈AAA都有⼀个当前位置pAp_{A}p
A
(圆盘的中⼼点)、当前速度vAv_{A}v
A
和半径rAr_{A}r
A
，这些参数都是机器⼈的外在状态，它们可以被其他机器⼈观察到。此外，当每个机器⼈往⽬标点⽅向的路上没有其它机器⼈阻挡时，都
有最⼤速度vmaxAv_{A}^{max}v
A
max
和期望速度vprefAv_{A}^{pref}v
A
pref
（vprefAv_{A}^{pref}v
A
pref
直接指向⽬标点，它的长度等于vmaxAv_{A}^{max}v
A
max
），但这些事机器⼈的内部参数，⽆法被其它机器⼈观察到。
解决⽅案：
我们提出了⼀个严格的⽅案，假设有nnn个个体，他们之间使⽤相同的避免碰撞策略，提供充⾜条件下，他们相互在ttt时间内避免碰撞的⽅
案。
这个⽅案基于速度，这意味着每个机器⼈都会考虑到其它机器⼈的速度避免与他们碰撞，然后从他⾃⼰的可选速度空间区域中选择⾃⼰的新
速度，其它被标为"禁⽌"的区域则被其它机器⼈占据。对于每个其它机器⼈，当前机器⼈都有⼀个半平⾯(速度空间)的可选速度，⽤来与其
速度，其它被标为"禁⽌"的区域则被其它机器⼈占据。对于每个其它机器⼈，当前机器⼈都有⼀个半平⾯(速度空间)的可选速度，⽤来与其
它机器⼈避免碰撞。那么当前机器⼈可以从这些可选的多个平⾯的交集中选择⼀个最佳的速度，这个可以通过线性划分有效的完成。线性划
分在机器⼈密集的环境下可能不可⾏，在这种情况下，我们通过三维空间来选择“尽可能安全”的速度。
我们要做的是：
第⼀，为每个机器⼈A同步独⽴地为它们⾃⼰选择⼀个新的速度vnewAv_{A}^{new}v
A
new
，并且这个新速度vnewAv_{A}^{new}v
A
new
能够保证在预定的t时间内，持续的与别的机器⼈⽆碰撞移动。
第⼆，每个机器⼈选择新速度vnewAv_{A}^{new}v
A
new
时，都要尽可能的接近它们⾃⼰的期望速度vprefAv_{A}^{pref}v
A
pref
。
第三，每个机器⼈之间不允许进⾏沟通，所以它们只能够观察得到别的机器⼈的当前位置和当前速度。然⽽，每个机器⼈可以假设其它机器
⼈也是使⽤和⾃⼰⼀样的策略来选择新的速度vnewAv_{A}^{new}v
A
new
。
这个问题⽆法使⽤中⼼协调的⽅式解决，因为每个机器⼈的期望速度只有它们⾃⼰知道。
第四节，我们描述为机器⼈选择时间t内避免碰撞的新速度准备了⾜够多的条件。
第五节，展⽰了如何使⽤此原理循环多个机器⼈导航。
所以，整篇⽂章主要部分要解释的动态避让效果也就如下图所⽰：
也就是要达成如下效果图这般动态避让导航的样⼦，⾛位四不四很风骚？^^。
图解：
以红⾊圆为主⾓，我们假设他叫AAA，其它颜⾊的圆各叫BCDEFBCDEFBCDEF等。
图中各种颜⾊的直线就是上述的平⾯的分割线，分给A的半平⾯下⽂称为ORCAtA∣BORCA_{A|B}^{t}ORCA
A∣B
t
。⽩⾊区域是AAA避开多个其它颜⾊圆的多个半平⾯的交集区域，也就是⾃⼰的可选速度范围，下⽂称为ORCAtAORCA_{A}^{t}ORCA
A
t
(⽩⾊区域图中)。左边⿊⾊点位置是⽬的地点，主⾓AAA的期望速度vprefAv_{A}^{pref}v
A
pref
(图中灰⾊线段)总是指向它，毕竟那是⾃⼰要去的地⽅嘛。⽽主⾓AAA的当前速度voptAv_{A}^{opt}v
A
opt
(图中⿊⾊线段)也想指向它，却⽆奈被限制在⽩⾊区域⾥，但它不死⼼，所以它每在下⼀步选择新速度vnewAv_{A}^{new}v
A
new
(图中的⿊⾊线段的下⼀步状态)都会尽可能的向灰⾊速度靠近，直到到达⽬的地。由此，风骚⾛位图形成
4.1准备
有两个机器⼈AAA和BBB，障碍速度集合为VOtA∣BVO_{A|B}^{t}VO
A∣B
t
（解释为：A在此集合中选择此速度时，将在时间ttt内与BBB发⽣碰撞)。
设DDD为以ppp为圆⼼，rrr为半径的圆：
DDD(ppp,rrr)={qqq|||q−p||<$rrr}
那么：
VOtA∣BVO_{A|B}^{t}VO
A∣B
t
={vvv|∃ttt∈in∈[000,τττ]::tvtvtv∈in∈DDD(pBp_{B}p
B
−pAp_{A}p
A
,rAr_{A}r
A
+rBr_{B}r
B
)}
有：
（a）两个机器⼈A和B
（b）这⾥是速度空间坐标系，障碍速度集合VOtA∣BVO_{A|B}^{t}VO
A∣B
t
⼏何解释为（灰⾊区域）形成⼀个截头的圆锥形，顶点位于原点o（速度空间），它的两边与rAr_{A}r
A
+rBr_{B}r
B
相切，两边居中为pBp_{B}p
B
−pAp_{A}p
A
⽅向上。圆锥的半径由t决定，这⾥的障碍速度t=2。
（c）避免碰撞向量集合CAtA∣BCA_{A|B}^{t}CA
A∣B
t
：避免发⽣碰撞情况下，BBB的选择速度集合为VBV_{B}V
B
(深灰⾊)，这⾥为坐标系的第四象限，也就是说明BBB的速度集合⽅向为向右下，那么给AAA的选择的速度集合
CAtA∣BCA_{A|B}^{t}CA
A∣B
t
也就是VOtA∣BVO_{A|B}^{t}VO
A∣B
t
的Minkowskisum的补集。
（PS:临阵磨枪，脑补⼀下)Minkowskisum的⼤概定义如下：
设XXX⊕YYY表⽰为X和Y的Minkowskisum，绿⾊为XXX蓝⾊为YYY，红⾊为和：
XXX⊕YYY={xxx+yyy|xxx∈in∈XXX,yyy∈in∈YYY},
障碍速度的⼏何解释如(b)所⽰，记住：CAtA∣BCA_{A|B}^{t}CA
A∣B
t
和CAtB∣ACA_{B|A}^{t}CA
B∣A
t
相对于原点是对称的。
设vAv_{A}v
A
和vBv_{B}v
B
分别为机器⼈AAA和BBB的当前速度，根据障碍速度的定义意味着，如果vAv_{A}v
A
−vBv_{B}v
B
∈in∈VOtA∣BVO_{A|B}^{t}VO
A∣B
t
，那么AAA和BBB持续以当前速度移动将在时间ttt内碰撞。相反，如果vAv_{A}v
A
−vBv_{B}v
B
∉notin∈
/
VOtA∣BVO_{A|B}^{t}VO
A∣B
t
那么AAA和BBB在ttt时间内不会碰撞。
那么对应任何vBv_{B}v
B
，如果vBv_{B}v
B
∈in∈VBV_{B}V
B
和vAv_{A}v
A
∉notin∈
/
VOtA∣BVO_{A|B}^{t}VO
A∣B
t
⊕VBV_{B}V
B
，那么AAA和BBB以当前速度在ttt时间内是保证不会碰撞的，从⽽推导出避免碰撞速度集合CAtA∣BCA_{A|B}^{t}CA
A∣B
t
(VBV_{B}V
B
)，也就是在BBB选择vBv_{B}v
B
速度后AAA能够选择的速度集合。见图©：
CAtA∣BCA_{A|B}^{t}CA
A∣B
t
(VBV_{B}V
B
)={vvv|vvv∉notin∈
/
VOtA∣BVO_{A|B}^{t}VO
A∣B
t
⊕VBV_{B}V
B
}（图c公式）
对于AAA和BBB的速度集合VAV_{A}V
A
和VBV_{B}V
B
，如果VAV_{A}V
A
⊆subteq⊆CAtA∣BCA_{A|B}^{t}CA
A∣B
t
(VBV_{B}V
B
)和VBV_{B}V
B
⊆subteq⊆CAtB∣ACA_{B|A}^{t}CA
B∣A
t
(VAV_{A}V
A
)，那么，AAA和BBB相互不碰撞。
如果VAV_{A}V
A
=CAtA∣BCA_{A|B}^{t}CA
A∣B
t
(VBV_{B}V
B
)和VBV_{B}V
B
=CAtB∣ACA_{B|A}^{t}CA
B∣A
t
(VAV_{A}V
A
)，那么，我们称VAV_{A}V
A
和VBV_{B}V
B
互为最⼤化。
4.2
因为AAA和BBB是相对独⽴的机器⼈，所以它们应该在没有沟通的情况下，推断出⾃⼰被允许的速度范围，这⾥有⽆数对的VAV_{A}V
A
和VBV_{B}V
B
在遵循着这些要求。但在这些成对的VAV_{A}V
A
和VBV_{B}V
B
中，在他们互为最⼤化的可选的避免碰撞速度集合中选择⼀对接近最优的速度，称AAA的为voptAv_{A}^{opt}v
A
opt
，BBB的为voptBv_{B}^{opt}v
B
opt
。(opt:optimization)
我们称以上这些避免碰撞可选范围AAA和BBB相互最⼤化的速度集合AAA的为ORCAtA∣BORCA_{A|B}^{t}ORCA
A∣B
t
，BBB的为ORCAtB∣AORCA_{B|A}^{t}ORCA
B∣A
t
。
ORCAORCAORCA的具体描述
定义1(OptimalReciprocalCollisionAvoidance)
机器⼈A的ORCAtA∣BORCA_{A|B}^{t}ORCA
A∣B
t
，机器⼈B的为ORCAtB∣AORCA_{B|A}^{t}ORCA
B∣A
t
的定义是，两边A和B避免碰撞，并且他们可选新速度范围互为最⼤化，以下⽤等式描述：
CAtA∣BCA_{A|B}^{t}CA
A∣B
t
(ORCAtB∣AORCA_{B|A}^{t}ORCA
B∣A
t
)=ORCAtA∣BORCA_{A|B}^{t}ORCA
A∣B
t
和CAtB∣ACA_{B|A}^{t}CA
B∣A
t
(ORCAtA∣BORCA_{A|B}^{t}ORCA
A∣B
t
)=ORCAtB∣AORCA_{B|A}^{t}ORCA
B∣A
t
即以上等式解释为在时间ttt内避免与机器⼈BBB的ORCAtB∣AORCA_{B|A}^{t}ORCA
B∣A
t
碰撞的向量区域，就是的机器⼈A的ORCAtA∣BORCA_{A|B}^{t}ORCA
A∣B
t
，反则反之。
那么有，
|ORCAtA∣BORCA_{A|B}^{t}ORCA
A∣B
t
⋂bigcap⋂DDD(voptAv_{A}^{opt}v
A
opt
,rrr)|=|ORCAtB∣AORCA_{B|A}^{t}ORCA
B∣A
t
⋂bigcap⋂DDD(voptBv_{B}^{opt}v
B
opt
,rrr)|≥geq≥min(|VAV_{A}V
A
,DDD(voptAv_{A}^{opt}v
A
opt
,rrr)|,|VBV_{B}V
B
,DDD(voptBv_{B}^{opt}v
B
opt
,rrr)|)
这意味着ORCAtA∣BORCA_{A|B}^{t}ORCA
A∣B
t
和ORCAtB∣AORCA_{B|A}^{t}ORCA
B∣A
t
包含着更多速度接近voptAv_{A}^{opt}v
A
opt
和voptBv_{B}^{opt}v
B
opt
，超过任何其它成对的速度的互相避免碰撞速度集合。它们允许选择的速度分布是均匀的，接近最优速度的速度集合数量AAA和BBB的相
等。
我们构建ORCAtA∣BORCA_{A|B}^{t}ORCA
A∣B
t
和ORCAtB∣AORCA_{B|A}^{t}ORCA
B∣A
t
的⼏何图如下
假设AAA和BBB最优速度分别为voptAv_{A}^{opt}v
A
opt
和voptBv_{B}^{opt}v
B
opt
,假设AAA和BBB⼀定会碰撞上，即voptAv_{A}^{opt}v
A
opt
-voptBv_{B}^{opt}v
B
opt
∈in∈VOtA∣BVO_{A|B}^{t}VO
A∣B
t
。
设uuu是以voptAv_{A}^{opt}v
A
opt
-voptBv_{B}^{opt}v
B
opt
为起点，指向和到以VOtA∣BVO_{A|B}^{t}VO
A∣B
t
边界最近的点为终点的向量：
uuu=(argminargminargmin∣∣||∣∣vvv−(voptAv_{A}^{opt}v
A
opt
−voptAv_{A}^{opt}v
A
opt
)∣∣||∣∣)−(voptAv_{A}^{opt}v
A
opt
−voptAv_{A}^{opt}v
A
opt
))，vvv∈in∈∂VOtA∣B∂VO_{A|B}^{t}∂VO
A∣B
t
∂偏导符号∂偏导符号∂偏导符号
设n是∂VOtA∣B∂VO_{A|B}^{t}∂VO
A∣B
t
范围内向，以(voptAv_{A}^{opt}v
A
opt
−voptBv_{B}^{opt}v
B
opt
)+uuu为起点向外的法线，那么uuu是对于AAA和BBB在时间ttt内避免碰撞速度需要改变最⼩的值。为了避免碰撞，机器⼈以公平的⽅
式"分担责任"，机器应该适配⾃⼰的速度为1/2u1/2u1/2u。
（我们引⼊这些voptvoptvopt(优化速度)来概括ORCAORCAORCA的定义。实际上，这些voptvoptvopt等于当前的速度，机器⼈必须偏
离当前轨道来避免可能的碰撞，更多选择在5.2节讨论）
如图：
Fig.2Fig.2Fig.2
AAA的ORCAtA∣BORCA_{A|B}^{t}ORCA
A∣B
t
跟B避免碰撞的向量集合是⼀个半平⾯，被垂直于向量uuu且经过点voptAv_{A}^{opt}v
A
opt
+1/2u1/2u1/2u的线分割。
uuu是以voptAv_{A}^{opt}v
A
opt
-voptBv_{B}^{opt}v
B
opt
为起点，指向和到以VOtA∣BVO_{A|B}^{t}VO
A∣B
t
边界最近的点为终点的向量。
nnn是VOtA∣BVO_{A|B}^{t}VO
A∣B
t
范围内向，以(voptAv_{A}^{opt}v
A
opt
-voptBv_{B}^{opt}v
B
opt
)+uuu为起点⽅向向外的法线。
ORCAtA∣BORCA_{A|B}^{t}ORCA
A∣B
t
允许A的避免碰撞速度集合是以voptAv_{A}^{opt}v
A
opt
+1/2u1/2u1/2u为起点，以nnn为⽅向的半平⾯；BBB的允许速度集合同理。
ORCAtA∣BORCA_{A|B}^{t}ORCA
A∣B
t
={vvv∣|∣(vvv−(voptAv_{A}^{opt}v
A
opt
+1/2u1/2u1/2u))·nnn≥000}.
以上等式在可能不会碰撞时也有效，即voptAv_{A}^{opt}v
A
opt
−voptBv_{B}^{opt}v
B
opt
∉notin∈
/
VOtA∣BVO_{A|B}^{t}VO
A∣B
t
时。
以上说明每个机器⼈之间不通过沟通就能观察到彼此之间的⼤概位置、半径和速度。
这种情况下两个机器⼈个承担⼀半保持⽆碰撞的责任。
5n个个体避免碰撞
在本节中，我们将展⽰如何应⽤上⾯定义的ORCA原理来实现多个移动机器⼈之间的n体碰撞避免，并讨论如何在这个框架中加⼊静态障
碍。
d流程图
感知其它机器⼈的位置和速度
计算机器⼈彼此之间的ORCA
使⽤线性划分选择新的速度
将速度应⽤于更新机器⼈位置
Fig.3Fig.3Fig.3
每个机器⼈的感知和反应循环⽰意图
5.15.15.1基本⽅案
每个机器⼈AAA在时间ttt内执⾏连续循环的感知和做反应。在每次循环中，机器⼈需要知道⾃⼰的和其它机器⼈的半径、当前位置和当前
速度，基于这些信息，机器⼈AAA推断出⾃⼰的对于机器⼈BBB的ORCAtA∣BORCA_{A|B}^{t}ORCA
A∣B
t
半平⾯。在这个半平⾯内允许的速度范围内，AAA⼜和其它机器⼈产⽣半平⾯，如此之间不断影响，我们定义此为
ORCAtAORCA_{A}^{t}ORCA
ORCAtAORCA_{A}^{t}ORCA
A
t
，(见Fig.4Fig.4Fig.4)
ORCAtAORCA_{A}^{t}ORCA
A
t
=DDD(((000,,,vmaxAv_{A}^{max}v
A
max
)))⋂bigcap⋂ORCAtA∣BORCA_{A|B}^{t}ORCA
A∣B
t
其中(((BBB≠neq
=AAA)))（7）
请注意，此定义还包括机器⼈A的最⼤速度限制。
接下来，避免碰撞允许速度范围内所有的速度，机器⼈选择⼀个最接近它的期望速度vprefAv_{A}^{pref}v
A
pref
的新速度vnewAv_{A}^{new}v
A
new
：
vnewAv_{A}^{new}v
A
new
=argmin∣∣v−argmin||v-argmin∣∣v−vprefAv_{A}^{pref}v
A
pref
∣∣，v∈||，v∈∣∣，v∈ORCAtAORCA_{A}^{t}ORCA
A
t
（8）
我们将在下⾯展⽰如何计算这个实际应⽤的新速度。更新机器⼈
达到新的位置；
pnewAp_{A}^{new}p
A
new
=pAp_{A}^{}p
A
+vnewAv_{A}^{new}v
A
new
new
Δt∆t∆t,（9）
所有的感知-反应都是（7）（8）（9）这个循环过程。
上述过程中的关键步骤是计算新的速度vnewAv_{A}^{new}v
A
new
,也就是（7）和（8）的定义。这可以使我们有效的使⽤线性规划算法完成，如ORCAtAORCA_{A}^{t}ORCA
A
t
是受多个机器⼈影响产⽣多个半平⾯共同内由线性规划约束引起的交集区域，见下图Fig.4Fig.4Fig.4。最优的解就是此区域速度集合中的
某个速度到期望速度vprefAv_{A}^{pref}v
A
pref
的距离，甚⾄这是个⼆次最优函数也不影响线性规划算法的特性，因为它只有⼀个局部最⼩值。
我们使⽤[3]的有效算法，它逐个添加约束
随机顺序，同时跟踪当前最佳新速度
Fig.4Fig.4Fig.4
(a)8个机器⼈的，它们各⾃的速度⽤箭头表⽰。
(b)A的受多个机器⼈影响的多个半平⾯t=2t=2t=2和vopt∗v_{*}^{opt}v
∗
opt
=v∗v_{*}^{}v
∗
)。EEE和CCC的半平⾯重合，虚线区域是AAA的多个半平⾯交集区域，也就是避免碰撞被允许的新速度选择区域范围。
算法⽬的是在ORCAtAORCA_{A}^{t}ORCA
A
t
中找到最接近vprefAv_{A}^{pref}v
A
pref
的新速度vnewAv_{A}^{new}v
A
new
，如果线性规划算法不可⾏，即ORCAtAORCA_{A}^{t}ORCA
A
t
=0=0=0.，那么返回失败。vmaxAv_{A}^{max}v
A
max
的调整不会影响算法的运⾏。
如果谨慎选择机器⼈的优化速度（我们将在5.2节讨论），ORCAτA将不会是空的，因此，总有⼀种解决⽅案可以保证机器⼈在t时间内⽆
碰撞。
我们可以通过不包括所有其他机器⼈的约束来提⾼选择速度的效率，仅仅考虑那些“靠近”的机器⼈。事实上，机器⼈B离机器⼈A的距离
远远超过（vmaxA+vmaxB）那么时间t内是不会与机器⼈发⽣碰撞，因此在计算机器⼈A的新速度时可以安全地将它们排除在线性规划算
法之外。还有⼀个⼩问题是机器⼈A不知道其他机器⼈的最⾼速度，但这可以通过“猜测”来解决，其它机器⼈的最⾼速度等于A的⾃⾝。
关于附近机器⼈的约束影响，可以使⽤kD树来寻找附近的机器⼈。
图
Fig.5Fig.5Fig.5
(a)3个机器⼈BCDBCDBCD密集的向机器⼈AAA移动。
(b)机器⼈彼此之间的参数t=2t=2t=2和vopt∗v_{*}^{opt}v
∗
opt
=v∗v_{*}^{}v
∗
，那么区域ORCAtA是空的，所以ttt时间内⽆法保证没有碰撞。
©机器⼈彼此之间的参数t=2t=2t=2和vopt∗v_{*}^{opt}v
∗
opt
=000，那么得出灰⾊区域是ORCAtAORCA_{A}^{t}ORCA
A
t
。
5.2选择最优速度
还有⼀个问题，如何为每个机器⼈AAA选择voptAv_{A}^{opt}v
A
opt
。为了让机器⼈在没有通信的情况下推断出半平⾯，voptAv_{A}^{opt}v
A
opt
必须是可被其它机器⼈观察得到的。
在这⾥，我们讨论⼀些合理的可能性：
voptAv_{A}^{opt}v
A
opt
=000对应所有的机器⼈AAA：
这就保证了ORCAtAORCA_{A}^{t}ORCA
A
t
对于机器⼈A⾮空。如上所述，那么线性规划算法将最快为所有机器⼈找到保证时间t内避免碰撞的速度。对应多个机器⼈B，点0始终位于
障碍速度VOtA∣BVO_{A|B}^{t}VO
A∣B
t
之外，因此半平⾯ORCAtAORCA_{A}^{t}ORCA
A
t
总是包含最⼩速度000。实际上，这条线界定了ORCAtAORCA_{A}^{t}ORCA
A
t
垂直于连接AAA和BBB的线。
将优化速度设置为000的缺点是机器⼈的⾏为看起来不⾃然，因为它们只考虑了别的机器⼈的当前位置，⽽不是它们的速度，在密集的情况
下也可能导致全局僵硬，因为机器⼈的速度彼此⾮常接近000。
voptAv_{A}^{opt}v
A
opt
=vprefAv_{A}^{pref}v
A
pref
对应所有的机器⼈AAA：
期望速度是机器⼈的内部状态，因此别的机器⼈⽆法观察得到。为了能讨论下去，我们假设某种程度上可以推断出其它机器⼈的期望速度，
然后让所有机器⼈的优化速度等于期望速度，这在低密度条件下运⾏很好，但随着优化速度的幅度递增，线性规划算法变得越来越不可⾏。
在⼤多数情况下，⽆论密度环境如何，期望速度都具有恒定（⼤）长度，这将导致即使在均匀的密度环境中导航看起来也不⾃然。
voptAv_{A}^{opt}v
A
opt
=vAv_{A}^{}v
A
对应所有机器⼈AAA：
这⾥的优化速度是以上两种的理想权衡，在低密度环境下选择偏向期望速度，在⾼密度环境下选择偏向000速度，当然，当前速度要被其它
机器⼈观察得到。
尽管如此，在⾼密度环境下，线性规划算法依然有可能不可⾏（见Fig.5(b)Fig.5(b)Fig.5(b)），这种情况下不能保证选择出⼀个避免碰撞
的速度。为此，我们使⽤3-D线性规划算法为机器⼈选择“尽可能安全”的速度（我们在5.3节讨论）。
5.3密集条件环境
对于所有机器⼈AAA，我们选择voptAv_{A}^{opt}v
A
opt
=vAv_{A}^{}v
A
，在机器⼈密度极⾼的情况下，可能会ORCAtAORCA_{A}^{t}ORCA
A
t
为空（见图5(b)），并且5.1的算法返回是不可⾏的，在这种情况下⽆法保证有⽆碰撞速度。
这种情况下，我们为每个机器⼈选择“尽可能安全”的速度，即速度最低限度的“穿透”其它机器⼈引起的约束。
正式点的说，就是设dA∣B(v)d_{A|B}^{}(v)d
A∣B
(v)为速度vvv到半平⾯ORCAtA∣BORCA_{A|B}^{t}ORCA
A∣B
t
的边界的距离。如果v∈v∈v∈ORCAtA∣BORCA_{A|B}^{t}ORCA
A∣B
t
，那么dA∣B(v)d_{A|B}^{}(v)d
A∣B
(v)为负。我们要选择最⼩的可⾏速度，选的也就是速度到多个机器⼈影响的各个平⾯的距离中最⼤的⼀个。
vnewAv_{A}^{new}v
A
new
=argminargminargminmaxmaxmaxdA∣B(v)d_{A|B}^{}(v)d
A∣B
(v)其中v∈D(0,v∈D(0,v∈D(0,vmaxAv_{A}^{max}v
A
max
))),,,BBB≠neq
=AAA
⼏何上，这可以解释为以相同的速度，向外垂直移动半平⾯ORCAtA∣BORCA_{A|B}^{t}ORCA
A∣B
t
的边缘，直到恰好得到⼀个有效的速度，这也就是我们要选的最⼩可⾏速度。
我们可以使⽤三维线性规划算法找到这个速度。对于每⼀个其它机器⼈BBB，距离函数dA∣B(v)d_{A|B}^{}(v)d
A∣B
(v)在三维(v,d)(v,d)(v,d)空间中是⼀个平⾯。我们通过距离函数来寻找⼀个点(v∗,d∗)(v*,d*)(v∗,d∗)，它位于所有的平⾯之上，找到它的
最⼩值ddd，然后设置vnewAv_{A}^{new}v
A
new
===v∗v_{*}^{}v
∗
。
实际上，我们可以将问题投射到v平⾯上，这样所有的⼏何形状操作可以⼆维进⾏，三维线性规划算法总是可⾏的，所以它总是返回⼀个解
决⽅案。
请注意，在这些密集的情况下，为机器⼈选择的新速度不会取决于机器⼈的期望速度。这意味着机器⼈’顺其⾃然流动’，其⾏为完全由
机器⼈周围的机器⼈决定。
效果如图：
关于静态障碍：
Fig.6Fig.6Fig.6
（a）机器⼈AAA和线段障碍物OOO的配置。
（b）障碍速度的⼏何形状VOtA∣OVO_{A|O}^{t}VO
A∣O
t
，t=2t=2t=2。
（c）半平⾯切割线ORCAtA∣OORCA_{A|O}^{t}ORCA
A∣O
t
与VOtA∣OVO_{A|O}^{t}VO
A∣O
t
相切于边上到voptAv_{A}^{opt}v
A
opt
最近的点，等于0时为碰到障碍OOO。
5.4静态障碍物
到⽬前为⽌，我们只讨论过机器⼈如何避免相互碰撞，但是
典型的多机器⼈环境也包含（静态）障碍物。我们可以很容易地将它们纳⼊上述框架中。我们基本上遵循以上相同的⽅法，⼀个关键的区
别是障碍物不移动，所以机器⼈应该完全负责避免与他们发⽣碰撞。
我们通常可以假设障碍物被建模为线段的集合。设OOO是这样的线段之⼀，AAA是半径为rAr_{A}^{}r
A
，位于点pAp_{A}^{}p
A
的机器⼈。那么由OOO影响的障碍速度VOtA∣OVO_{A|O}^{t}VO
A∣O
t
定义如下：
VOtA∣OVO_{A|O}^{t}VO
A∣O
t
==={v|∃t∈[0,τ]::tv∈O⊕−D(pAp_{A}^{}p
A
,rAr_{A}^{}r
A
)}.
如果其速度vAv_{A}^{}v
A
在VOtA∣OVO_{A|O}^{t}VO
A∣O
t
内，则代理AAA将在τττ时间内与障碍物OOO碰撞，反之则在t时间内避免碰撞。因此，对应障碍OOO，AAA允许的速度区域为
VOtA∣OVO_{A|O}^{t}VO
A∣O
t
的补集。这个补集是⼀个⾮凸区域，所以⽆法使⽤5.1的划线算法，为此，对于障碍O，我们定义了A可允许的速度集合
ORCAtA∣OORCA_{A|O}^{t}ORCA
A∣O
t
的半平⾯：划分线为为切线过VOtA∣OVO_{A|O}^{t}VO
A∣O
t
边界上⼀个最接近voptAv_{A}^{opt}v
A
opt
的点(见图Fig.6(c)Fig.6(c)Fig.6(c))。
图
Fig.7Fig.7Fig.7
两个机器⼈的踪迹模拟，机器⼈显⽰为彩⾊磁盘它们的初始位置很轻，随着时间的推移变暗。
（a）两机器⼈通过彼此的踪迹。
(b)五个机器⼈去到彼此对⾯点的踪迹。
本文发布于:2022-11-25 08:59:36，感谢您对本站的认可！
本文链接：http://www.wtabcd.cn/fanwen/fan/90/17526.html
版权声明：本站内容均来自互联网，仅供演示用，请勿用于商业和其他非法用途。如果侵犯了您的权益请与我们联系，我们将在24小时内删除。
| 留言与评论（共有 0 条评论） |
