基於搜索的路徑規劃算法

基於搜索的路徑規劃算法是較爲成熟和廣泛使用的一種路徑規劃算法，常常被用於移動機器人或者遊戲中的人物的路徑規劃。
主要分爲下面幾個部分進行介紹：

1. 圖搜索基礎
2. Dijkstra and A*算法
3. 跳點搜索算法（Jump Point Search）
4. 作業

圖搜索基礎

Configuration Space / 構形空間

Configuration Space / 構形空間是一種特殊的空間，機器人的每一種位姿都代表空間中的一個點，這些點的集合就稱爲構型空間，也叫C-space。爲什麼要提出這樣一種奇怪的空間概念呢？我個人理解有以下幾種原因：

1. 各種各樣的機器人的形狀不一，而在構型空間內機器人的位姿都簡化成一個點，便於用統一的方法描述障礙物並且用統一的方法做碰撞檢測
2. 各種各樣的機器人的運動學方程都不一樣，例如有的能側移有的則不能，大大提高了路徑規劃的複雜度。而在構型空間內都內描述成一條連續的曲線，使得所有路徑規劃的問題都無需考慮運動學。

總之，C-space空間的提出有利於使路徑規劃問題簡化，使複雜的、多樣的問題變得簡單且單一。這個空間的概念不是特別容易理解，下面舉幾個簡單的例子便能理解了。

現在有一個機器人，只能在水平空間內平移而無法旋轉，我們只用它的座標（x，y）便能描述它的位姿。於是這樣一個機器人的C-space便是R2。

若這個機器人能夠旋轉，則增加了一個自由度，它的C-space便可以用（x，y，theta）來表達。當然更加高維的問題則需要用其他的數學工具去表達如se(3)等。

圖搜索理論

首先，什麼是圖。在計算機科學中，一個圖就是一些頂點的集合，這些頂點通過一系列邊結對（連接）。頂點用圓圈表示，邊就是這些圓圈之間的連線。頂點之間通過邊連接。

圖可以分爲有向圖和無向圖，同時有的圖還有權重，這部分概念和數據結構的內容一致可參考
https://www.cnblogs.com/w-wanglei/p/figure.html 。

圖搜索主要的流程如下：

1. 創建一個容器來儲存所有要訪問的節點

2. 初始化容器，將起始位置放入

3. 進入循環

   1. 移除：根據規則訪問一個節點並將其移出容器
   2. 擴展：獲取改節點的所有鄰節點
   3. 存儲：把符合條件的鄰節點放入容器中

4. 結束

下面看兩種經典的圖搜索方法，深度優先搜索和廣度優先搜索。

深度優先搜索

如下圖，從節點0出發首先搜索節點1，然後節點3，最後到節點4後走到盡頭了才接着走節點3和2。

將如上過程轉換成樹的形式則如下圖，可以清晰的看到深度優先搜索的特點，即一條路走到黑再走另外一條路。

廣度優先搜索

和深度優先算法相反，廣度優先算法則是一層層的搜索的過程。

更多的動圖可以訪問網站 https://visualgo.net/zh/dfsbfs?slide=4-2 幫助理解。

算法實現

在深入講解兩種搜索算法的實現前需要先提及兩種較爲常見的數據結構：隊列和棧。其特點也非常簡單，隊列的特點是先進先出而棧的特點是先進後出。

下面根據這兩種數據結構編寫算法的基本流程。

深度優先搜索

1. 創建一個棧來儲存所有要訪問的節點

2. 初始化棧，將起始位置放入

3. 進入循環

   1. 移除：訪問節點s後，將初始節點s移除
   2. 擴展：獲取初始節點s的所有鄰節點d、e、p。
   3. 存儲：把未訪問過的d、e、p節點放入容器中
   4. …一直循環下去直到所有點被訪問

4. 結束

廣度優先搜索

1. 創建一個隊列來儲存所有要訪問的節點

2. 初始化隊列，將起始位置放入

3. 進入循環

   1. 移除：訪問節點s後，將初始節點s移除
   2. 擴展：獲取初始節點s的所有鄰節點d、e、p。
   3. 存儲：把未訪問過的d、e、p節點放入容器中
   4. …一直循環下去直到所有點被訪問

4. 結束

兩者的不同之處僅僅在於使用的數據結構不同，堆棧返回最後進入的則使整個搜索一直朝一條路走。而隊列返回最先進入的節點則會使整個搜索呈現一層一層的搜索。

那在實際的地圖上這兩種方法的效果是怎麼樣的呢？

可以看到廣度優先要搜索的區域很大，但是可以找到一條最優的路徑，而深度優先則會找到很多的錯誤的路徑，但是它的好處在於即使在未知全局地圖的情況下也能找到一條路徑儘管這條路徑不是最優的。

啓發式算法

除了上面兩種圖搜索的方法，還有一類經典的搜索算法較爲常用叫做啓發式搜索，也叫做貪心搜索。貪心算法的思路就是每一次迭代都朝着局部最優的值靠近。相比與DFS和BFS根據壓入數據結構的先後來確定下一次訪問的節點，貪心搜索則對每一個數據結構內的節點用一個函數去估計其代價，找到代價最小的即局部最優值去訪問，其中這個預估代價的函數便稱作啓發函數。

例如，常見的啓發函數有歐式距離和馬式距離。

若採用距離來作爲啓發函數則每次都會朝着離目標點最近的方向去搜索，如下圖。

可以看到，在沒有障礙物的情況下該算法能非常快找到一條可行路徑，但是我們再看一下有障礙物的情況。

可以看到在一開始的時候算法陷入了局部的極值，沒有找到一條最好的路徑反而走向了死衚衕。

啓發式算法的實現和之前的兩種沒有太大的區別，主要在於容器的數據結構不一樣，這裏採用優先級隊列作爲容器，定義不同的啓發函數作爲排序依據，每次彈出優先級最高（即最近）的節點並訪問、迭代。

Dijkstra and A*算法

前面介紹的三種算法，廣度優先和啓發式搜索較爲實用但是依舊存在着明顯的缺點：廣度優先搜索需要訪問大量的節點才能找到最優路徑，啓發式搜索能很快的進行搜索但是容易陷入局部極值導致路徑並非最優的。於是有人提出將這兩種方法的有點結合起來，於是就有了Dijkstra and A*算法。

第一個問題是如何確保找到的路徑是最優的，可以從之前的例子看到路徑不只一條且有的是最優路徑有的不是。在之前廣度優先搜索中認爲第一次搜索到的終點的路徑最短但是如果機器人能斜着移動，如下圖

若機器人能斜着移動，則左右前後平移和斜着移動的距離明顯不一樣兩者之前的權重是不一樣的。對於這種情況我們在圖的邊上加上權重，直走的權重爲1而斜着走爲2或者根號2。更通常的情況下則將這樣的地圖抽象成權重圖。

每一個節點在訪問的時候都會累計得到從起點到該節點的路徑上所有的權重和，若之前這個節點被訪問過，則比較兩者的權重和大小，取小的從而保證路徑是最優的。而每次從隊列中取出的不再是先進入的而是權重和最小的。當然可以看到，如果機器人無法斜着運動，每一個節點的權重都是1，則算法退化爲廣度優先算法，因爲權重和最小的和最先進入的是同一個節點。

接下來就可以詳細的描述算法的流程。

1. 創建一個優先級隊列來儲存所有要訪問的節點

2. 初始化優先級隊列，將起始節點放入

3. 進入循環

   1. 如果優先級隊列爲空，則跳出循環

   2. 移除：移除權重和最小的節點，並訪問它

   3. 標記改節點已經被訪問了

   4. 如果該節點是目標節點則，跳出循環

      對於該n節點的所有鄰居m進行循環

      1. 如果m的權重和爲infinite
      2. m節點的權重和爲n的權重和加上n到m路徑的權重
      3. 把m壓入到隊列中
      4. 如果m的權重>n的權重和加上n到m路徑的權重
      5. m節點的權重和爲n的權重和加上n到m路徑的權重

   5. 存儲：把未訪問過的d、e、p節點放入容器中

   6. …一直循環下去直到所有點被訪問

4. 結束

詳細的流程可以參考這個圖，和之前最大的區別在於它的優先級隊列的排序規則是之前路徑的權重和。這樣就能保證在各種圖中搜索到一條最優的路徑。

那麼這樣一個算法在地圖中搜索的表現如何呢？

可以看到，比起廣度優先算法以一種正方形的方式擴展Dijkstra算法是以圓形的方式擴展，更爲合理且能保證找到一條最優的路徑。但是依舊保留廣度優先算法的最大的缺點——需要搜索大量的節點後才能得到一條路徑，算法效率太低。

所以第二個問題是如何解決算法效率低下，其中一種思路就是將啓發式搜索和Dijkstra算法結合。思路也很簡單除暴，Dijkstra的優先級隊列的排序規則是節點的權重和而啓發式搜索的排序規則是距離目標點的距離。所以結合兩者算法，優先級隊列的規則設置爲節點的權重和與距離目標點的距離兩者的和。即
$f(n)=g(n)+h(n)$

A*和Dijkstra算法的流程一模一樣,只是將排序的規則改成了上面的和的方式。那麼這種算法在地圖中搜索是什麼情況？

可以看到改算法可以兼顧兩者的優點，既訪問較少的節點，目的性比較強又不至於陷入局部最優而找到一條不好的路徑。

當然也有人提出將$f(n)=g(n)+h(n)$更改成加權和，通過修改兩者的權重比來調整算法的貪心程度。
大家可以和如下圖所示的調整權重來對比算法，可以在網站 http://qiao.github.io/PathFinding.js/visual/ 中進行嘗試。

似乎所有的問題都解決了，但是在實際的工程實現中A*並不是總是能表現良好。其中如何選擇啓發函數便尤爲重要，之前大多采用歐式距離作爲啓發函數實際上啓發函數多種多樣，那啓發函數需要滿足哪些要求呢？還有有沒有一個理論最優的啓發函數呢？

實際上只要滿足m節點的啓發函數值大於或等於實際上從m到目標點的最短路徑即可。顯然當啓發函數和m到目標點的最短路徑相等是最好的，也就是啓發函數估計的和實際的一毛一樣，當然能找到最好的路徑了。但是現實是障礙物的千變萬化，無法做到這一點，於是退而求其次要求啓發函數在沒有障礙物的環境中和實際的路徑是一樣的即可。那麼找到一個最優的啓發函數的問題轉換成如何在無障礙物情況下找到一個最短路徑了，這當然是一個很簡單的事情，如下圖。

於是得到最優的最短路徑的計算方法如下：
$\begin{aligned} &d x=abs(node.x-goal.x)\\ &\mathrm{d} y=\operatorname{abs}(\text { node. } y-\text { goal. } y)\\ &h=(d x+d y)+(\sqrt{2}-2) * \min (d x, d y) \end{aligned}$

跳點法（Jump Point Search）

A*算法是目前較爲成熟且使用的較多的一種路徑搜索的算法，後來的學者又對其進行了很多的修改和完善使其適應不同的應用情況，而跳點法則是其中的佼佼者。

跳點法的提出主要目的在於解決如下的問題，在較爲空曠的環境中搜索路徑時常常有不只一條最優路徑，而A*則無法區分這些路徑導致所有的最優路徑都會被搜索。但是實際情況是我們只需要其中的任意一條即可，而對其他的最優路徑的搜索實則可以看成一種浪費資源的行爲。如下圖，有很多的路徑，而我們只需要其中一條即可。

基於以上的考慮，有學者提出了跳點法的概念，他可以根據一種規則在許多的等價的路徑中找出一條並一直跟隨直到抵達目標點。

因此跳點法最關鍵的點在於如何確定這個規則，它的規則稱爲向前看規則（Look Ahead Rule），主要的規則如下：

1、若是通過平移的方式抵達該節點的且四周無障礙物，則只需要查詢和抵達該節點的平移方向的唯一節點，如圖。

爲什麼需要這樣一個奇怪的規則呢？我們來仔細分析一下，從4->x，x按照A*的規則理論上要訪問1，2，3，5，6，7，8節點，但是我們發現了一個漏洞。從4到1明顯比4->x->1要短很多，既然我從4走到了x，說明1這個節點無需訪問也會在其他的路徑下走到1並取得更短的路徑。這個過程相當於減枝的過程，抵達1的方法有很多而從4->x->1明顯不是最短的，這個步驟我就直接減去了。同理，我們對2，3，4，6，7，8逐個分析可以得到，僅有從4->x->5這樣一條路徑是最短的。其他的節點都有其他的更短的方法抵達，因此我們跳到節點5。

2、若是通過斜移的方式抵達該節點且四周無障礙物，則訪問如圖三個節點。分析方法和第一種是一模一樣的，對每個點分析從6到該點的最短路徑，若最短路徑需要經過x，則該點需要訪問，反之則直接丟棄。

考慮完沒有障礙物的簡單情況，那麼有障礙物則如何去取這個規則呢？

3、若是平移且障礙物在對角某處，則和第一種情況一樣。

4、若是平移且障礙物在節點的側面，則對應側的對角節點也要壓入優先級隊列。可以通過之前同樣的方法來分析，可以發現由於障礙物的出現，導致該對角節點的最短路徑發生了改變也必須通過x了，所以也要訪問了。

5、類似的，如圖自行分析

知道了規則之後，我們來看JPS的具體流程

首先在初始位置我們需要訪問右側三個點，首先是水平方向的節點，然後由於沒有障礙物繼續方法其水平的點，最後一直調到障礙物上，結束水平跳躍。然後訪問上方的點，同理會一直向上訪問，直到地圖邊界然後結束，最後訪問右上角的節點。對於右上角節點，又有三個節點需要被訪問，和之前相似的步驟首先水平跳躍，遇到障礙物然後向上跳躍遇到邊界，之後一直重複如上過程。

直到上圖所示，發生了變化。在向右跳躍的過程中遇到了有障礙物的情況，這時候在該點有了兩個需要訪問的節點。

同樣，首先訪問右側水平的節點，遇到邊界結束。再次訪問右下角的節點，然後不斷重複，直到找到目標節點。

從上面的步驟中，我們可以提取出算法的步驟。而我們會發現和A*居然驚人的一致，僅僅在尋找當前節點的鄰居節點時，JPS算法會按照Look Ahead rule，大大減少了壓入隊列的節點。

說了這麼多，那麼在地圖中實際的搜索效果如何呢？

雖然jps能減少A*訪問的節點，且找到的路徑也是最優的，但是並不總是能取得很好的效果。例如在一個十分空曠的環境中，JPS往往需要查詢很多節點才能找到路徑，也就是說這個規則實際上削弱了算法的貪心程度，目的性不強導致效率在有些情況下變低了。

作業

最後的最後便是學習這一節課的作業了
待更新…