避障算法 - VO、RVO 以及 ORCA (RVO2)[转载]

本文转载于：http://k.sina.com.cn/article_3743149912_df1bef5801901cp6s.html

所谓避障算法，主要目的就是“避障”（废话），适用场合是并不复杂的 agent 自身周围的障碍规避，甚至对移动障碍有一定的预判。

所以在常见的游戏设计研发时，一般用于大型寻路算法（A*等）中，相对细微的近邻状态来做一些小距离的避障。

举个例子，你是饿了么骑手，你从你的当前位置，到达送餐地点的路线，主要由上层的寻路算法取得：

但是在路途中，你遇到车辆、建筑以及其他骑手的时候，你如何驾驶车车在他们之间穿梭，就是使用的避障算法。

需要注意的是，此时的穿梭并不会大距离的偏移出既定的导航路线，比如你再怎么闪避其他骑士，也不可能从人民大道跑到成华大道，一定还是大概在既定的路线上前进。

对于避障算法，现在游戏开发中比较常用的就是 VO 相关的算法，因为它的计算是分布式的，而且过程简单，非常适合大规模单位避障的场景。

目前它的算法优化已经到了 ORCA(RVO2)，但是在此之前，还是先从 VO 讲起，这样更容易理解算法逐渐优化演进的过程。

另外说明一点，避障算法使用的数学理论基本在高二之前我们就学习过，所以本身算法并没有太高深。

之所以搞得看起来这么复杂，我猜测是因为写成论文，需要严谨和学（zhuang）术（b）。

所以本文会以最白话的方式来叙述算法，不整那些有的没的（比如 Minkowski sum 一类的，我们就全部用几何图形来表示）。

万物之始 - VO (Velocity Obstacle)

VO 其实本身并不能算是一种算法，他只是一种解决避障问题的思路，而后续的 RVO 以及 ORCA 其实都是基于这个设定。

它提出了一种叫做 Velocity Obstacle 的东西，其实它也非常见文知意，其实它就相当于将一系列可能会发生碰撞的速度在速度域（几何空间内表示为以横纵速度为基的坐标系）中划分出来作为一个需要避免的区域，在选择下一帧的速度时，避免这个区域内的速度就好了。

它其实本身就像是处于速度域中的“障碍物”一样，所以 VO 这个名字其实起的真的很好 233。

拥有 VO 这个概念以后，我们最容易想到的算法其实很简单，这也是我们一般称为“VO 算法”的最基础的逻辑。

假设一个场景，A 和 B 在空间中偶遇，因为 VO 算法本身是分布式的，也就是每个物体只考虑物体自身，这样做的好处和意义这里就不赘述了，所以，我们从 A 的角度来考虑这次偶遇。

我们假设 A 和 B 都有自己的社交安全距离，这个距离是一个圆形的范围，半径分别是 RA 和 RB，那自然而然想到的算法就是：我们这一帧的相对移动要保证两个人的最近距离要大于等于 RA+RB。

这是显而易见的，实现这个目标的方法也很简单，在 A 的位置放置一个质点，然后在 B 的位置放置一个半径为 RA+RB 的大圆，这个大圆以外的位置都是 A 结束时安全的相对位置，所以这一帧的相对移动只要位于圆外就可以。

但是又因为，虽然结束位置只要在圆外就可以，但是因为移动过程是连续的，所以我们移动的路径实际上是初始点到终点的一条直线，因此我们还应该保证我们不应该“穿过”这个大圆而到达大圆后方，即大圆相对于质点位置的后方的锥形区域也是不可达的。

我们画出几何图形其实相当显而易见：

如上图，灰色区域就是不可达区域，但是为了计算简单，同时有一定的预判效果，VO 算法把整个三角区域全部视为“危险”区域。

需要联想到，此时所说的这一帧质点的相对位移，其实就是质点的单帧速度，而质点的单帧速度，其实就是 A 和 B 的相对速度，所以其实我们可以非常容易的把上图映射到速度域内，其坐标系原点即为质点的坐标。

可以看出来图形和上面那张坐标图的几何结构基本是一样的，只不过这已经映射到速度域了（红色区域在坐标域为危险区域，在速度域即为 VO）。

理论上当 A 和 B 的相对速度选择红色三角以外的点，即可安全错开。

经典形态 - RVO（Reciprocal Velocity Obstacle）

VO 的思想非常简单，漏洞也是非常明显的，就是会发生抖动，且两个 agent 的情况就有可能发生，让我们看看原文的例子。

当 A 和 B 相向而行时，一开始 A 和 B 会错开，但当下一帧对方的速度发生变化时，他们又会把速度转回来，因为这个时候 A 会认为 B 就是向左上走的，所以我还是保持最佳的向下走也不会撞上，B 也是这么想的，所以他们就转回来了。

实际上这可能也不会造成最终的碰撞，因为在第二帧的时候，它们的速度确实是错开的，所以由于预判了碰撞，最终还是会很危险的错开。

但这个过程中两个 agent 的反复横跳的过程还是不够优雅的，而原因也显而易见，VO 算法中智能体总是默认其他智能体是稳定的恒速前进，这导致当对方的速度发生变化时，自己的行为就变得不符合预期。

所以在 RVO 中最大的改进，就是我们假设对方也会使用和我们相同的策略，而非保持匀速运动。

在基础的 VO 算法中，产生抖动的原因是 A 在第 2 帧选择新速度之后，发现 B 的速度也变化了很多，A 就会认为改回最佳速度（即直接指向目的地的速度）似乎也不会碰撞了，因为 B 的新速度其实就是假设 A 保持最佳速度也能不碰撞的情况下改变的，所以 A 就会认为 B 允许他转回来，但同时 B 也是这么想的。

然而在 RVO 中，A 把自己的速度只改变 1/2，也就是说，我们假设 A 和 B 想要错开，总共需要错开 10cm，VO 算法中 A 和 B 都会各自错开 10cm，而在 RVO 算法中 A 只错开一半，也就是 5cm，同时 A 假设 B 会错开另外一半，B 也是这么想的，因此两者不谋而合，第二帧的时候，两个人各自错开了一半，并且发现此时转回最佳速度依然是会碰撞的（因为每个人只转了一半嘛），因此有效避免了上述抖动的现象。

终极进化 - ORCA（Optimal Reciprocal Collision Avoidance）

RVO 虽然解决了 VO 算法出现的问题，并且在单对单的避障中几乎总是表现良好，但当智能体的数量增多时，还是会出现不符合预期的现象。

假设下面一种情况，A 和 B 一左一右的与 C 相向而行，在 RVO 中会遇到什么情况呢？

很简单，A 认为 C 会向右转，因此自己向左转了 1/2，而 B 认为 C 会向左转，因此自己向右转了 1/2，而 C 实际上两种做法都没有选择，因为在 VO 图中，这实际上是两个锥体摆在自己的面前，所以 C 选择非常努力的向左或者向右转向。

这个时候 A、B、C 三个人就都炸了，因为没有一个的预料是正确的，所以他们在第 3 帧时就会根据一个预料之外的对方速度修改自己的速度，从而形成抖动。

其实原因也很简单，在 RVO 中，所有的智能体都假设对方只考虑自己。

所以这次事故的原因就是，A 认为 C 爱着自己，B 也认为 C 爱着自己，但实际上 C 同时爱着对面两个人，就像是 A 找 C 约会，然后 C 把 B 带上一起了。

虽然这种情况下实际上也不会发生真正的碰撞，因为 A 和 B 终究会向左右挪开，但过程中的抖动也是不符合预期的。

后来 ORCA 就出现了，它切实的解决了上面这种抖动的问题（虽然我认为是碰巧解决了，因为 ORCA 相对于 RVO 的改进其实主要是效率原因）。

ORCA 与 RVO 最大的区别，在于 VO 的形状差异，在以往的 VO 算法中，VO 都是以无限高度的锥形出现的，二维中就是同源的两条射线的夹角，但在 ORCA 中，我们使用一个有向平面来分割空间。

因此求解最佳速度的计算也得到了优化，从一个由一堆尖角切出的空间变成了高考必考内容的线性规划问题。

而 ORCA 是如何得到这个平面的呢？我们看图。

首先我们还是得理解这张图，这张图很重要，p 是两点的位置，r 是两智能体半径，τ 是单位时间（我也不知道他为什么要用 τ 这个字母，可能因为是 t 的字源？）

图 a 是位置域，智能体 A 和 B 在这里偶遇了。

这时候我们从 A 的角度来看问题，把 A 的位置放到原点，那么 VO 算法中不可达区域在位置域中的位置就如同图 b 中大圆的部分及其后的锥形空间，即以 pb-pa 为圆心，半径为 ra+rb 的圆。

而在速度域中，单位时间τ内移动的距离（也就是设定的帧间隔，同时也算是开始避障的预警时间）其实就可以理解为速度，所以保证 τ 时间后不会进入上述位置的 VO 区域，即为图 b 中的灰色区域，即 VO。

这个不难理解吧？简单来讲就是你用小圆上的速度，刚好τ时间后会到大圆的位置，所以 VO 就如图 b。

图 c 我们不用管它，其实就是为了方便我们理解的，不过我觉得其实想弄懂它完全没啥意义，而且对我们理解也没啥帮助，而且代码里其实完全没用上。

理解了这个之后，如果把这个 VO 解释成一个锥体，其实就是类似前面说的 VO，但是 ORCA 中是把它解释成了一个二分平面，这个平面大概如下图所示：

就是红色箭头指着的这个平面，马赛克掉的地方不用去管它，那是智能体 B 的平面，我们现在是智能体 A，所以看不到。

首先我解释一下图中的变量，vopt 指的是两智能体的期望速度，一般就是指向目的地的速度，u 是相对期望速度到上面计算出的 VO 区域外的最短向量，n 是 u 指向的 VO 表面的位置的法线。

这个时候平面就确定了，就是位于 v 的期望速度 + u/2 的方向为 n 的半平面。

为啥这样确定咧？我相信聪明的小伙伴已经反应过来了，u 是什么，u 其实就是相对速度需要修改的值，相当于上面有一个例子里面的那 10cm，所以 vAopt + u/2 其实就相当于 A 与 B 争端中的最佳修改后速度，到这里其实跟 RVO 是差不多的。

但不同的是，我们得到这个值后并不是用它作为最终答案的一部分，而是由它确定了这个半平面。

这个首先我们要说明为什么要确定一个半平面，其实之前说过了，使用半平面可以把问题转化为简单的线性规划问题，计算量断崖式下降。

那么我们为什么要选择这个半平面呢？原因其实很简单，这是包含当前这次冲突最佳解决方案的半平面，相信聪明的小伙伴可以理解这句话。

所以，当 A 在完成多个平面的切割后，每次平面切割剩下的那个可选的半空间，都一定包含了当此冲突最佳解决方案的速度，比如与 B 之间的速度一定包含在第一个半空间内，与 C 之间的最佳速度一定包含在第二个半空间内

这样可以保证，在这么些半空间的交集内，出现与每个对手解决方案的最优速度的可能性是最大的，即使最后最佳速度被切走了，也总能选到相对更好的速度。

个人认为这个结论还是挺容易理解的，所以此处就不赘述证明了，因为这违背了大白话讲算法的初衷。

特殊情况

好的那么空间切割完毕之后，如果最佳速度处于当前空间内，就选择最佳速度，否则，就选择距离最佳速度最近的切割后空间内速度，这个速度一般位于空间的边界顶点处，也就转化为了一个简单的线性规划问题。

这是论文中的图，注意图 b 中有一条曲线，这是因为论文中还有一个 vmax，即智能体能达到的最大速度，这也是合理的，不过不属于算法核心，所以前面没讲。

当然需要注意的是，当对手非常多时，ORCA 是非常容易把整个空间全部切光的，因为前面也说了，每次都切掉近乎一半的空间，所以很容易发生这种情况

这种情况的解决方案其实也很简单，就是增加了一个 d 变量，它表示速度到当前半平面的距离，d 为正时表示当前速度在这个半平面以内，为负时表示被半平面切掉了，我们就是要找 d 的最大和时对应的速度，也就相当于是违规最小的速度。

从几何上看就直观很多，可以怎么理解呢，其实就是把半平面向外挪，让半平面少切一部分，使得这些个半平面刚好能切出一个点，同时对所有半平面的平移最少，就选这个点作为目标速度。

静态障碍物

除此之外，论文中还讨论了关于静态障碍物的躲避，其实也是合理的，因为你在送餐的时候不能光躲会动的东西嘛，也要躲电线杆才行。

这个的逻辑其实很简单，就是因为障碍物本身没有速度是静止的，所以作为智能体要负责全部的躲避责任，而不是 1/2 了。

代码中比较复杂主要是因为障碍物的构成不是一个会移动的圆，而是一条条线段连成的多边形，所以计算上复杂了很多。

代码解析

官方的代码做了很多为了效率上的让步，比如经常直接把平方值作为开根后的向量长度来用，或者直接使用 magic number 一类的操作，我们不要过分纠结，差不多就行~

本文主要解析的是官方 github 中的 c# 代码，为什么选择 c# 代码主要有两点，一是因为我对 c++ 已经很久没有实际应用了，所以害怕因为自己的生疏对命令有误解，影响对代码的理解，二是因为我们主要目的是理解并复现这个算法，所以选择 c#，因为可以减少很多不必要的代码，读起来也会觉得逻辑更清晰。

代码中主要围绕 Simulator 类在执行游戏循环，每一帧执行 doStep 函数，函数内容也很简单，就是先构建一大堆 worker，然后重构 kd 树，kd 树其实是有 obstacle 和 agent 两棵，但是 obstacle 的只需要构建一次，agent 则需要每帧构建，因为 agent 会跑嘛，其实个人认为这里应该是个优化点，因为其实很多游戏中并不需要真实的获得离我最近的智能体有哪些，理论上因为有军团的存在，所以应该有比较方便的获取方法，目前还没细想。

构建完 KDtree 之后就是一个模块一个模块的执行 worker 的 step 和 update 逻辑，step 是主要计算逻辑，update 则是更新 agent 状态的逻辑，内容也很简单：

可以看到主要就是遍历所有的 agent 然后调用其中的函数，computeNeighbors 主要就是树操作，从 kd 树中取出离自己最近的智能体，update 就是更新自身状态，速度啊，位置啊一类的。

关键在于 computeNewVelocity，我们上文所讲的 ORCA 的逻辑主要就在这个函数中。

不过这个函数相当长，我们分块来分析。

首先是 computeNewVelocity 的整体结构：

可以看到其实主要的逻辑都集中在生成静态障碍物的半平面和其他智能体形成的半平面两个循环中，外面的逻辑相对简单。

第一行就是把之前存储的半平面清空，重置本次计算的状态。

循环前对 invTimeHorizon 的计算则是为了减少循环中的计算数量，因为这其实是个常量值，放在循环中的话要多跑 n 遍。

最后就是用线性规划来求解最后的速度，这个是比较通用的内容，本文就不展开了，感兴趣自己去看，内容也很短，一个函数大概就 50 行左右的样子。

所以其实计算半平面的逻辑还是集中在了两个循环中，我们此处主要解析智能体的这个循环，静态障碍物的这个跟智能体的逻辑很类似，代码我也加注释了，感兴趣自己下下来看吧：

首先是做了一些变量的准备，大概代表的含义都如我的注释，其中 me 代表当前 agent，you 代表当前 neighbor（下面简称 A 和 B）。

line 就是我们最后要算出来的半平面的那条切割线，这个类里面封装了它的位置和方向。

u 则是上面算法中介绍的 u，是一个向量，我们要用它来计算 line 的位置。

接下来是一个分支语句，可以看到它用距离的平方和半径和的平方去比较，因为距离和半径和本身都是正的，所以比较平方也可以比较出两者的大小。

所以这个分支相当于是判断 A 和 B 是否已经碰撞。

最后结束的两行，是在用上面算法中的公式来计算 line 的位置，并压入 list。

好的我们先看如果两者已经接触的逻辑，因为比较简单容易理解：

这里计算了一个特殊的 timestep，它跟上面的警戒时间不同，这个是切实的帧间隔，这套逻辑里是支持警戒时间和帧间隔不同的，也就是可以提前几帧就做出避障动作。

对于 w 的理解我们可能得借助几何图形，我们还是看上面这张图：

我们先称这个图为雪人图，因为它很像一个倒挂的雪人（指大小两个圆），后面会用到好几次。

从雪人图里可以很容易的看出来，所谓 invTimeStep * relativePosition 其实就是此图中小圆的圆心位置，所以用相对速度减去它，得到的就是从小圆圆心指向当前相对速度的一条向量。

接下来的两行比较简单，就是计算了 w 的长度和 w 方向的单位向量。

接下来使用 unitW 来计算 line 的方向，使用的是常用的向量顺时针旋转 90° 的计算公式，这个公式可以很容易的用坐标系旋转的公式来推导，即 cos90°=0 和 sin90°=1 的性质。

下面一行是计算 u，这里需要把公式拆解一下，把 unitW 乘进括号里，就会好理解很多。

combinedRadius * invTimeStep 是小圆半径，乘以 unitW 可以理解为就是 relativeVelocity 到小圆圆心的连线与小圆圆周的交点，而 wLength * unitW 就是 relativeVelocity 自身，所以用前者减去后者，就是从 relativeVelocity 指到小圆圆周的向量，这不就是 u 嘛~

我们要求的就是这个嘛~

接下来再看尚未碰撞的情况，注释比较长，可以辅助理解：

这里的 w、wLengthSq 都比较简单，不用多解释

dotProduct1 需要解释一下，这里参考雪人图改良版，如上图，relativePosition 就是图中大圆的圆心，也就是从原点指向大圆圆心的向量，图中红色的这条，所以 w 与此向量相乘，得到的就是 w 投向图中红色向量的投影。

可以看到下面的分支语句就是用 dotProduct1 的正负来作为判断条件之一的，其实相对来说比较容易理解，向量点乘，两向量夹角为锐角时为正，钝角时为负，垂直时为 0，所以此处第一个判断条件意思就是 w 和红色向量的夹角为钝角，那其实在速度域中就如下图：

图中的红线与上图的红色向量垂直，这条红线以下的点，都符合 dotProduct1 ，原因我在上面解释的很清楚了，因为这下面的点，都满足 w 与红色向量成钝角。

这个条件主要是为了第二个判断条件做第一部分筛选。

第二个判断条件需要做一点准备计算，基本的推理操作，这个不等式其实本身跟 abs(dotProduct1)>combinedRadius * wLength 是等价的，而 abs(dotProduct1) 可以理解为上上图红色向量到 w 投影的标量长度（长度只能为正）乘上 wLength，不明白的自己去看向量点乘公式。

所以两边可以同时消掉 wLength，这里的条件就变成了“红色向量到 w 投影的标量长度 > combinedRadius”，那这个条件什么时候满足呢？说真的想破脑瓜也想不出个所以然，但是用几何来表现还是很清晰的。

我们可以先来看，什么时候左边等于右边，显而易见的：

当 w 与 VO 的两条腿（腿指的是那两条射线）垂直时，即紫色向量的方向时，红色向量在 w 的投影是与 combinedRadius 相等的，原因我觉得就不用解释了吧？不明白的请去复习向量点乘的几何意义。

这个时候可能就会有记忆力比较差的小朋友问了，关于大圆圆心与紫色向量对称的两条向量呢？红色向量在他们两个上面的投影也是 combinedRadius 啊？

虽然很不情愿，但是我还是得提醒一下，第一个条件，帮我们筛选掉了与红色向量夹角为锐角的向量，所以本来的四条，只剩下图中紫色的两条了。

而这两条的方向恰恰是什么呢？恰恰就是两条腿与大圆小圆的切点与圆心的连线，为什么这么说呢，因为切点与圆心的距离就是半径，不明白请回炉重造。

我们做这么多的意义是啥呢，可能聪明的朋友已经发现了，其实 VO 的整个区域是可以分成两部分的，一个部分的外边缘是小圆的圆周，另外一部分的外边缘则是两条腿的一部分。

而我们现在通过这两个条件筛选出来的，就是下图紫色向量之间的区域，这个区域的 VO 就是以小圆的圆周为外边缘的：

那么下面要做的事就显而易见了：

用 w 的方向来计算 u，计算过程跟已经碰撞的情况是完全一样的，此处就不赘述了。

那么 else 的部分，半平面其实就是腿的方向。

首先通过勾股定理计算出原点到大圆切点的长度，这里称为腿长。

接下来的一个分支语句，稍微有一个小知识点，就是行列式的几何含义，就是有向体积，在二位空间中是有向面积。

所以可以通过计算 relativePosition 和 w 组成的行列式的值的正负，来判断 relativeVelocity 相对于向量 relativePosition 的位置，即此处的左腿右腿。

分开左腿和右腿后，接下来通过一个相对复杂一点的式子来计算腿的方向，我们只看一下左腿吧，右腿是完全类似的。

首先如上图中的三角形，我们称为“黄金三角形”，它的三边长度分别是 combinedRadius、relativePositionLength 以及 leg。

我们先记住这个概念，然后来看公式。

line.direction =newVector2(relativePosition.x()* leg - relativePosition.y()* combinedRadius, relativePosition.x()* combinedRadius + relativePosition.y()* leg)/ distSq;

其中 disSq 是前面计算的 relativePosition 长度的平方，所以可以理解为 relativePositionLength * relativePositionLength，这里我们可以先除一个 relativePositionLength 进来，整个式子就变成了：

line.direction =newVector2(relativePosition.x()* leg / relativePositionLength - relativePosition.y()* combinedRadius / relativePositionLength, relativePosition.x()* combinedRadius / relativePositionLength + relativePosition.y()* leg / relativePositionLength)/ relativePositionLength;

相信 X 大的小朋友已经发现了 leg / relativePositionLength 就是黄金三角型下面这个角的 cos 值，而 combinedRadius / relativePositionLength 就是 sin 了，所以此时，前面 new 出来的这个 vector2 就昭然若揭了，他就是把 relativePositionLength 沿原点逆时针旋转黄金三角型下方角度所得到的向量！

这个向量恰好与左腿重合，且长度与向量 relativePosition 相同。

注意了，刚才我们只除进来一个 relativePositionLength，所以在 vector2 外面还有一个 relativePositionLength 要除，因此，我们得到了左腿的单位向量。

只能说，牛逼！

然后下面再用点乘，获得 relativeVelocity 在左腿上的投影长度 dotProruct2，最后在用向量减法得出 u。

完结撒花~★,°:.☆(￣▽￣)/$:.°★ 。

---

以上是转载的部分。

最后贴上自己的理解图（学到就是赚到）：