# 進度報告(2024/01/08)

## 文獻閱讀
**Reciprocal Velocity Obstacles for Real-Time Multi-Agent Navigation**[^1]
當兩個或多個採用VO避障策略的agents相互閃避時，路徑會出現oscillation的現象。此篇paper便提出修改最初VO的策略，稱作RVO，目的是解決以上的狀況。

### oscillation
稱兩個採用VO避障策略的agents分別為A、B，預設在沒有任何干擾的狀況下，agents會朝目標點直線前進，所以A以$\mathbf{v_A}$的速度直線行駛，B也以$\mathbf{v_B}$的速度直線行駛。
此時A、B相遇，A在速度space中產生了一個$VO^A_B(\mathbf{v_B})$，A的速度向量不能踏進這區域，否則便會在未來的某的時間點撞上B，而$\mathbf{v_A}$因為落在VO裡，因此A會選擇一個VO以外的速度$\mathbf{v'_A}$以避開B。同理B也會產生$VO^B_A(\mathbf{v_A})$，並且選擇這個區間以外的速度$\mathbf{v'_B}$以避開A。下個時間點，A與B皆改變了原本的速度(論文上說，agents會選擇離preferred速度$\mathbf{v^{pref}}$最近的$\mathbf{v'}$，也就是相對於朝終點的直線速度，變化量最小的VO外之速度)。此時，A會發現由於B改變了速度，$VO^A_B(\mathbf{v_B})$也因此移動了，導致原本落在VO中的$\mathbf{v_A}$向量不再落在VO中，為了更快到達目標點，A便再次將它的速度向量改回$\mathbf{v_A}$，同理，B也再次將它的速度向量改回$\mathbf{v_B}$。
A、B的決策造成彼此再次相遇，因此又回到文章上一段落，如此，A與B又重新選擇了新的速度向量，然後又改回來，不斷重複。造成兩agents的路徑抖動。
簡單來說，設兩agents彼此的避障責任各為$\alpha_A$、$\alpha_B$，兩數和應為1。而前述的狀況，由於A、B兩agents都達到了100%的避障($\alpha_A=\alpha_B=1$)，也就是總合為2，結果便是兩者都避障過頭了，造成扭動的現象。

### RVO
為了避免扭動產生，需要讓兩個相遇的agents分擔避障責任，RVO理論中，分擔方法為移動碰撞錐。
從最早的VO文獻得知，當A節點遇到B節點時，A相對於B會產生一碰撞錐。
平移此碰撞錐，平移向量為B的速度向量$\mathbf{v_B}$，此平移過後的碰撞錐為VO。
如今，為了分擔避障責任，需要再次平移碰撞錐，也就是將VO變成RVO(Reciprocal Velocity Obstacle)
* 如果agent A面對B需要負擔100%的避障責任，在速度的space中，其碰撞錐之頂點便落在$\mathbf{v_B}$向量所指的那點，形成VO
* 如果agent A面對B僅僅需負擔0%的避障責任，代表A不用改變當前的速度。可以想像成碰撞錐之頂點落在原速度$\mathbf{v_A}$向量所指的那點。

將避障責任設為$\alpha$介於等於0~1之間，則碰撞錐的平移量為$\alpha\mathbf{v_B}+(1-\alpha)\mathbf{v_A}$。如此，$\alpha=1$，碰撞錐的平移量完全是$\mathbf{v_B}$形成原VO；$\alpha=0$，碰撞錐的平移量完全是$\mathbf{v_A}$。而平移過後的碰撞錐，便是RVO，如下圖:

### 多節點時的速度選擇策略
當在一個廣闊的空間有很多agents時，每一顆agent都要考慮所有其他agents相對於自己的碰撞追會耗費太多時間且沒效率，所以，文章作者幫agent定義了"Neighbor Region"，進入到這個鄰近區域的其他節點才會被認為是"相遇"，並考慮碰撞錐。儘管如此，當多個agents聚集在一起時，會出現屬於不同節點的碰撞錐幾乎把所有在速度space中可行的$\mathbf{v}$都覆蓋的情況，如圖:
此時，對節點i來說，任一個可以更靠近目標點的速度向量都落在RVO中，所以i要在被碰撞錐覆蓋的$\mathbf{v_i}$集合中找出一個可能最不會撞上的速度向量，同時又可以朝i的目標點前進，因此，定義一個penalty，公式如下:
$$
penalty_i(v'_i)=w_i \dfrac{1}{tc_i(v'_i)}+\| v_i^{pref}-v'_i \|
$$
$v_i^{pref}$是以最佳速度朝目標點直線前進的速度向量，$w_i$是權重參數。依此公式，agent A會選擇一個逞罰量最小的速度向量前進。

### VO vs RVO

## 參考RVO程式
找到一分matlab版本的RVO程式，[它發布在github上](https://github.com/jimas95/Multi-Agent-Navigation.git)，程式的作者在README上說是基於上面那篇RVO的論文而寫的。它的有些註解好像不是英文，導致我花了一段時間看懂他的程式碼，其中最主要的應該是一個定義robot的類別檔，所有agent應該有的參數及function幾乎集中在這個class中。
仔細檢查後，發現這程式跟原來的RVO論文還是有一些小出入，例如，選擇速度的penalty還額外考慮了速度變量(加速度大小應越小越好)，還有權重係數也應該是程式的作者自定義的，畢竟原論文好像沒有交代。儘管如此，大體架構應跟原RVO的論文大同小異。以下是其中幾個執行場景:
避障責任$\alpha_A=\alpha_B=0.5$
{%youtube gIjW3N4ggGU %}
避障責任$\alpha_A=0,\alpha_B=1$
{%youtube 7d9lCYpTgJk %}
多數agents的RVO
{%youtube oPsggGKP81w %}

### Reference
[^1]:J. van den Berg, Ming Lin and D. Manocha, "Reciprocal Velocity Obstacles for real-time multi-agent navigation," 2008 IEEE International Conference on Robotics and Automation, Pasadena, CA, 2008, pp. 1928-1935, doi: 10.1109/ROBOT.2008.4543489.
URL: https://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=4543489&isnumber=4543169