所謂避障算法，主要目的就是“避障”（廢話），适用場合是并不複雜的 agent 自身周圍的障礙規避，甚至對移動障礙有一定的預判。

所以在常見的遊戲設計研發時，一般用于大型尋路算法（A*等）中，相對細微的近鄰狀态來做一些小距離的避障。

舉個例子，你是餓了麼騎手，你從你的當前位置，到達送餐地點的路線，主要由上層的尋路算法取得：

但是在路途中，你遇到車輛、建築以及其他騎手的時候，你如何駕駛車車在他們之間穿梭，就是使用的避障算法。

需要注意的是，此時的穿梭并不會大距離的偏移出既定的導航路線，比如你再怎麼閃避其他騎士，也不可能從人民大道跑到成華大道，一定還是大概在既定的路線上前進。

對于避障算法，現在遊戲開發中比較常用的就是 VO 相關的算法，因為它的計算是分布式的，而且過程簡單，非常适合大規模單位避障的場景。

目前它的算法優化已經到了 ORCA(RVO2)，但是在此之前，還是先從 VO 講起，這樣更容易理解算法逐漸優化演進的過程。

另外說明一點，避障算法使用的數學理論基本在高二之前我們就學習過，所以本身算法并沒有太高深。

之所以搞得看起來這麼複雜，我猜測是因為寫成論文，需要嚴謹和學（zhuang）術（b）。

所以本文會以最白話的方式來叙述算法，不整那些有的沒的（比如 Minkowski sum 一類的，我們就全部用幾何圖形來表示）。

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 < 0.0f，原因我在上面解釋的很清楚了，因為這下面的點，都滿足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。

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

另外這是我的技術群：573228201，有問題可以直接來群裡噴我，謝謝！

注：題圖由編輯添加，來自：AutoRVO: Reciprocal Collision Avoidance between Heterogeneous Agents and Applications to Autonomous Driving. 版權歸原作者所有。（https://gamma.umd.edu/researchdirections/autonomousdriving/autorvo/）

,

更多精彩资讯请关注tft每日頭條，我们将持续为您更新最新资讯!

查看全部