所謂避障演算法，主要目的就是“避障”（廢話），適用場合是並不複雜的 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 都有自己的社交安全距離，這個距離是一個圓形的範圍，半徑分別是 和 ，那自然而然想到的演算法就是：我們這一幀的相對移動要保證兩個人的最近距離要大於等於 。
這是顯而易見的，實現這個目標的方法也很簡單，在 A 的位置放置一個質點，然後在 B 的位置放置一個半徑為 的大圓，這個大圓以外的位置都是 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 把自己的速度只改變 ，也就是說，我們假設 A 和 B 想要錯開，總共需要錯開 ，VO 演算法中 A 和 B 都會各自錯開 ，而在 RVO 演算法中 A 只錯開一半，也就是 ，同時 A 假設 B 會錯開另外一半，B 也是這麼想的，因此兩者不謀而合，第二幀的時候，兩個人各自錯開了一半，並且發現此時轉回最佳速度依然是會碰撞的（因為每個人只轉了一半嘛），因此有效避免了上述抖動的現象。
究極進化 - ORCA（Optimal Reciprocal Collision Avoidance）
RVO 雖然解決了 VO 演算法出現的問題，並且在單對單的避障中幾乎總是表現良好，但當智慧體的數量增多時，還是會出現不符合預期的現象。
假設下面一種情況，A 和 B 一左一右的與 C 相向而行，在 RVO 中會遇到什麼情況呢？
很簡單，A 認為 C 會向右轉，因此自己向左轉了 ，而 B 認為 C 會向左轉，因此自己向右轉了 ，而 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 是如何得到這個平面的呢？我們看圖。
首先我們還是得理解這張圖，這張圖很重要， 是兩點的位置， 是兩智慧體半徑， 是單位時間（我也不知道他為什麼要用 這個字母，可能因為是 的字源？）
圖 是位置域，智慧體 A 和 B 在這裡偶遇了。
這時候我們從 A 的角度來看問題，把 A 的位置放到原點，那麼 VO 演算法中不可達區域在位置域中的位置就如同圖 中大圓的部分及其後的錐形空間，即以 為圓心，半徑為 的圓。
而在速度域中，單位時間τ內移動的距離（也就是設定的幀間隔，同時也算是開始避障的預警時間）其實就可以理解為速度，所以保證 時間後不會進入上述位置的 VO 區域，即為圖 中的灰色區域，即 VO。
這個不難理解吧？簡單來講就是你用小圓上的速度，剛好τ時間後會到大圓的位置，所以 VO 就如圖 。
圖 我們不用管它，其實就是為了方便我們理解的，不過我覺得其實想弄懂它完全沒啥意義，而且對我們理解也沒啥幫助，而且程式碼裡其實完全沒用上。
理解了這個之後，如果把這個 VO 解釋成一個錐體，其實就是類似前面說的 VO，但是 ORCA 中是把它解釋成了一個二分平面，這個平面大概如下圖所示：
就是紅色箭頭指著的這個平面，馬賽克掉的地方不用去管它，那是智慧體 B 的平面，我們現在是智慧體 A，所以看不到。
首先我解釋一下圖中的變數， 指的是兩智慧體的期望速度，一般就是指向目的地的速度，u 是相對期望速度到上面計算出的 VO 區域外的最短向量， 是 指向的 VO 表面的位置的法線。
這個時候平面就確定了，就是位於 的期望速度 + 的方向為 的半平面。
為啥這樣確定咧？我相信聰明的小夥伴已經反應過來了， 是什麼， 其實就是相對速度需要修改的值，相當於上面有一個例子裡面的那 ，所以 其實就相當於 A 與 B 爭端中的最佳修改後速度，到這裡其實跟 RVO 是差不多的。
但不同的是，我們得到這個值後並不是用它作為最終答案的一部分，而是由它確定了這個半平面。
這個首先我們要說明為什麼要確定一個半平面，其實之前說過了，使用半平面可以把問題轉化為簡單的線性規劃問題，計算量斷崖式下降。
那麼我們為什麼要選擇這個半平面呢？原因其實很簡單，這是包含當前這次衝突最佳解決方案的半平面，相信聰明的小夥伴可以理解這句話。
所以，當 A 在完成多個平面的切割後，每次平面切割剩下的那個可選的半空間，都一定包含了當此衝突最佳解決方案的速度，比如與 B 之間的速度一定包含在第一個半空間內，與 C 之間的最佳速度一定包含在第二個半空間內
這樣可以保證，在這麼些半空間的交集內，出現與每個對手解決方案的最優速度的可能性是最大的，即使最後最佳速度被切走了，也總能選到相對更好的速度。
個人認為這個結論還是挺容易理解的，所以此處就不贅述證明了，因為這違背了大白話講演算法的初衷。
特殊情況
好的那麼空間切割完畢之後，如果最佳速度處於當前空間內，就選擇最佳速度，否則，就選擇距離最佳速度最近的切割後空間內速度，這個速度一般位於空間的邊界頂點處，也就轉化為了一個簡單的線性規劃問題。
這是論文中的圖，注意圖 中有一條曲線，這是因為論文中還有一個 ，即智慧體能達到的最大速度，這也是合理的，不過不屬於演算法核心，所以前面沒講。
當然需要注意的是，當對手非常多時，ORCA 是非常容易把整個空間全部切光的，因為前面也說了，每次都切掉近乎一半的空間，所以很容易發生這種情況
這種情況的解決方案其實也很簡單，就是增加了一個 變數，它表示速度到當前半平面的距離， 為正時表示當前速度在這個半平面以內，為負時表示被半平面切掉了，我們就是要找 的最大和時對應的速度，也就相當於是違規最小的速度。
從幾何上看就直觀很多，可以怎麼理解呢，其實就是把半平面向外挪，讓半平面少切一部分，使得這些個半平面剛好能切出一個點，同時對所有半平面的平移最少，就選這個點作為目標速度。
靜態障礙物
除此之外，論文中還討論了關於靜態障礙物的躲避，其實也是合理的，因為你在送餐的時候不能光躲會動的東西嘛，也要躲電線杆才行。
這個的邏輯其實很簡單，就是因為障礙物本身沒有速度是靜止的，所以作為智慧體要負責全部的躲避責任，而不是 了。
程式碼中比較複雜主要是因為障礙物的構成不是一個會移動的圓，而是一條條線段連成的多邊形，所以計算上覆雜了很多。
程式碼解析
官方的程式碼做了很多為了效率上的讓步，比如經常直接把平方值作為開根後的向量長度來用，或者直接使用 magic number 一類的操作，我們不要過分糾結，差不多就行~
本文主要解析的是官方 github 中的 c# 程式碼，為什麼選擇 c# 程式碼主要有兩點，一是因為我對 c++ 已經很久沒有實際應用了，所以害怕因為自己的生疏對命令有誤解，影響對程式碼的理解，二是因為我們主要目的是理解並復現這個演算法，所以選擇 c#，因為可以減少很多不必要的程式碼，讀起來也會覺得邏輯更清晰。
程式碼中主要圍繞 類在執行遊戲迴圈，每一幀執行 函式，函式內容也很簡單，就是先構建一大堆 ，然後重構 樹， 樹其實是有 和 兩棵，但是 的只需要構建一次， 則需要每幀構建，因為 會跑嘛，其實個人認為這裡應該是個最佳化點，因為其實很多遊戲中並不需要真實的獲得離我最近的智慧體有哪些，理論上因為有軍團的存在，所以應該有比較方便的獲取方法，目前還沒細想。
構建完 之後就是一個模組一個模組的執行 的 和 邏輯， 是主要計算邏輯， 則是更新 狀態的邏輯，內容也很簡單：
可以看到主要就是遍歷所有的 然後呼叫其中的函式， 主要就是樹操作，從 樹中取出離自己最近的智慧體， 就是更新自身狀態，速度啊，位置啊一類的。
關鍵在於 ，我們上文所講的 ORCA 的邏輯主要就在這個函式中。
不過這個函式相當長，我們分塊來分析。
首先是 的整體結構：
可以看到其實主要的邏輯都集中在生成靜態障礙物的半平面和其他智慧體形成的半平面兩個迴圈中，外面的邏輯相對簡單。
第一行就是把之前儲存的半平面清空，重置本次計算的狀態。
迴圈前對 的計算則是為了減少迴圈中的計算數量，因為這其實是個常量值，放在迴圈中的話要多跑 遍。
最後就是用線性規劃來求解最後的速度，這個是比較通用的內容，本文就不展開了，感興趣自己去看，內容也很短，一個函式大概就 50 行左右的樣子。
所以其實計算半平面的邏輯還是集中在了兩個迴圈中，我們此處主要解析智慧體的這個迴圈，靜態障礙物的這個跟智慧體的邏輯很類似，程式碼我也加註釋了，感興趣自己下下來看吧：
首先是做了一些變數的準備，大概代表的含義都如我的註釋，其中 代表當前 ， 代表當前 （下面簡稱 A 和 B）。
就是我們最後要算出來的半平面的那條切割線，這個類裡面封裝了它的位置和方向。
則是上面演算法中介紹的 ，是一個向量，我們要用它來計算 的位置。
接下來是一個分支語句，可以看到它用距離的平方和半徑和的平方去比較，因為距離和半徑和本身都是正的，所以比較平方也可以比較出兩者的大小。
所以這個分支相當於是判斷 A 和 B 是否已經碰撞。
最後結束的兩行，是在用上面演算法中的公式來計算 的位置，並壓入 。
好的我們先看如果兩者已經接觸的邏輯，因為比較簡單容易理解：
這裡計算了一個特殊的 ，它跟上面的警戒時間不同，這個是切實的幀間隔，這套邏輯裡是支援警戒時間和幀間隔不同的，也就是可以提前幾幀就做出避障動作。
對於 的理解我們可能得藉助幾何圖形，我們還是看上面這張圖：
我們先稱這個圖為雪人圖，因為它很像一個倒掛的雪人（指大小兩個圓），後面會用到好幾次。
從雪人圖裡可以很容易的看出來，所謂 其實就是此圖中小圓的圓心位置，所以用相對速度減去它，得到的就是從小圓圓心指向當前相對速度的一條向量。
接下來的兩行比較簡單，就是計算了 的長度和 方向的單位向量。
接下來使用 來計算 的方向，使用的是常用的向量順時針旋轉 的計算公式，這個公式可以很容易的用座標系旋轉的公式來推導，即 和 的性質。
下面一行是計算 ，這裡需要把公式拆解一下，把 乘進括號裡，就會好理解很多。
是小圓半徑，乘以 可以理解為就是 到小圓圓心的連線與小圓圓周的交點，而 就是 自身，所以用前者減去後者，就是從 指到小圓圓周的向量，這不就是 嘛~
我們要求的就是這個嘛~
接下來再看尚未碰撞的情況，註釋比較長，可以輔助理解：
這裡的 、 都比較簡單，不用多解釋
需要解釋一下，這裡參考雪人圖改良版，如上圖， 就是圖中大圓的圓心，也就是從原點指向大圓圓心的向量，圖中紅色的這條，所以 與此向量相乘，得到的就是 投向圖中紅色向量的投影。
可以看到下面的分支語句就是用 的正負來作為判斷條件之一的，其實相對來說比較容易理解，向量點乘，兩向量夾角為銳角時為正，鈍角時為負，垂直時為 ，所以此處第一個判斷條件意思就是 和紅色向量的夾角為鈍角，那其實在速度域中就如下圖：
圖中的紅線與上圖的紅色向量垂直，這條紅線以下的點，都符合 ，原因我在上面解釋的很清楚了，因為這下面的點，都滿足 與紅色向量成鈍角。
這個條件主要是為了第二個判斷條件做第一部分篩選。
第二個判斷條件需要做一點準備計算，基本的推理操作，這個不等式其實本身跟 是等價的，而 可以理解為上上圖紅色向量到 投影的標量長度（長度只能為正）乘上 ，不明白的自己去看向量點乘公式。
所以兩邊可以同時消掉 ，這裡的條件就變成了“紅色向量到 投影的標量長度 > ”，那這個條件什麼時候滿足呢？說真的想破腦瓜也想不出個所以然，但是用幾何來表現還是很清晰的。
我們可以先來看，什麼時候左邊等於右邊，顯而易見的：
當 與 VO 的兩條腿（腿指的是那兩條射線）垂直時，即紫色向量的方向時，紅色向量在 的投影是與 相等的，原因我覺得就不用解釋了吧？不明白的請去複習向量點乘的幾何意義。
這個時候可能就會有記憶力比較差的小朋友問了，關於大圓圓心與紫色向量對稱的兩條向量呢？紅色向量在他們兩個上面的投影也是 啊？
雖然很不情願，但是我還是得提醒一下，第一個條件，幫我們篩選掉了與紅色向量夾角為銳角的向量，所以本來的四條，只剩下圖中紫色的兩條了。
而這兩條的方向恰恰是什麼呢？恰恰就是兩條腿與大圓小圓的切點與圓心的連線，為什麼這麼說呢，因為切點與圓心的距離就是半徑，不明白請回爐重造。
我們做這麼多的意義是啥呢，可能聰明的朋友已經發現了，其實 VO 的整個區域是可以分成兩部分的，一個部分的外邊緣是小圓的圓周，另外一部分的外邊緣則是兩條腿的一部分。
而我們現在透過這兩個條件篩選出來的，就是下圖紫色向量之間的區域，這個區域的 VO 就是以小圓的圓周為外邊緣的：
那麼下面要做的事就顯而易見了：
用 的方向來計算 ，計算過程跟已經碰撞的情況是完全一樣的，此處就不贅述了。
那麼 的部分，半平面其實就是腿的方向。
首先透過勾股定理計算出原點到大圓切點的長度，這裡稱為腿長。
接下來的一個分支語句，稍微有一個小知識點，就是行列式的幾何含義，就是有向體積，在二位空間中是有向面積。
所以可以透過計算 和 組成的行列式的值的正負，來判斷 相對於向量 的位置，即此處的左腿右腿。
分開左腿和右腿後，接下來透過一個相對複雜一點的式子來計算腿的方向，我們只看一下左腿吧，右腿是完全類似的。
首先如上圖中的三角形，我們稱為“黃金三角形”，它的三邊長度分別是 、 以及 。
我們先記住這個概念，然後來看公式。
line。direction=newVector2（relativePosition。x（）*leg-relativePosition。y（）*combinedRadius，relativePosition。x（）*combinedRadius+relativePosition。y（）*leg）/distSq；
其中 是前面計算的 長度的平方，所以可以理解為 ，這裡我們可以先除一個 進來，整個式子就變成了：
line。direction=newVector2（relativePosition。x（）*leg/relativePositionLength-relativePosition。y（）*combinedRadius/relativePositionLength，relativePosition。x（）*combinedRadius/relativePositionLength+relativePosition。y（）*leg/relativePositionLength）/relativePositionLength；
相信 X 大的小朋友已經發現了 就是黃金三角型下面這個角的 值，而 就是 了，所以此時，前面 出來的這個 就昭然若揭了，他就是把 沿原點逆時針旋轉黃金三角型下方角度所得到的向量！
這個向量恰好與左腿重合，且長度與向量 相同。
注意了，剛才我們只除進來一個 ，所以在 外面還有一個 要除，因此，我們得到了左腿的單位向量。
只能說，牛逼！
然後下面再用點乘，獲得 在左腿上的投影長度 ，最後在用向量減法得出 。
完結撒花~*★，°*：。☆（￣▽￣）/$：*。°★* 。
另外這是我的技術群：，有問題可以直接來群裡噴我，謝謝！
注：題圖由編輯新增，來自：AutoRVO： Reciprocal Collision Avoidance between Heterogeneous Agents and Applications to Autonomous Driving。 版權歸原作者所有。（https：//gamma。umd。edu/researchdirections/autonomousdriving/autorvo/）