路徑規划算法簡介

原文連結：https://blog.csdn.net/weixin_39594441/article/details/101243168

目錄

涉及的問題

算法簡介

遺傳算法； 

模擬退火；

人工勢場；

蟻群算法；

RRT；

PRM；

Dijkstra；

A-Star(A)搜索算法*；

Field D***

1. 涉及問題這裡的路徑規劃是指如何尋找一條從給定起點到終點的路徑，使機器人沿該路徑移動的過程中不與障礙物發生碰撞且路程最短或移動代價最小。2. 算法簡介遺傳算法（Genetic Algorithm）是一類借鑑生物界的進化規律（適者生存，優勝劣汰遺傳機制）演化而來的隨機化搜索方法。它是由美國的J.Holland教授1975年首先提出，其主要特點是直接對結構對象進行操作，不存在求導和函數連續性的限定；具有較好的全局尋優能力；採用概率化的尋優方法，能自動獲取和指導優化的搜索空間，自適應地調整搜索方向，不需要確定的規則。遺傳算法的這些性質，已被人們廣泛地應用於組合優化、機器學習、信號處理、自適應控制和人工生命等領域。就遺傳算法運用於路徑規劃而言，可簡單概括為如下步驟：a. 隨機選取節點組成一組初始路徑群（即許多條路徑構成的集合）；c. 選擇路徑群中任意兩條路徑進行交叉和變異，路徑長度越短的路徑有更高的被選擇的概率，繼續選擇然後進行交叉和變異形成新的子代路徑群；d. 按照路徑長度淘汰掉初始路徑群和子代路徑群中路徑較長的路徑，然後形成新的路徑群；

e. 回到步驟1繼續下一輪迭代，制定迭代到達一定次數或路徑長度變化非常緩慢的情況時停止迭代，此時路徑群中最短的路徑即為遺傳算法求得的優化路徑。

2.2 模擬退火

擬退火是S. Kirkpatrick, C. D. Gelatt和M. P. Vecchi在1983年提出。模擬退火算法（Simulated Annealing），簡稱SA，是一種適用於大規模組合優化問題的有效近似算法。

它模仿固體物質的退火過程，通過設定初溫、初態和降溫率控制溫度的不斷下降，結合概率突跳特性，利用解空間的鄰域結構進行隨機搜索。具有描述簡單、使用靈活、運行效率高、初始條件限制少等優點，但存在著收斂速度慢、隨機性等缺陷，參數設定是應用過程中的關鍵環節。

應用於路徑規劃問題中可簡單描述其運行步驟如下：

a. 進行若干次的隨機選擇路徑，從中選擇出路徑長度最小的路徑作為模擬退火迭代方法的初始解；

b. 對初始路徑進行隨機的元素置換然後依據Metropolis準則判斷轉換後的新解是否可以被接受；

c. 重複步驟2直至Metropolis準則中定義的溫度係數T低到一定程度後停止迭代，此時當前解即為模擬退火法求得的最優路徑。

關於Metropolis準則可簡單概括如下式：

上式(3.1)中i為當前狀態，Ei和Ej分別代表i狀態和j狀態的溫度，ε為0 到1間的隨機數。
通過上面分析可以看出其實遺傳算法和模擬退火算法有許多類似的地方，它們都是通過隨機抽樣的辦法在解空間中選取可能的解然後按照評估函數進行解的評價，關鍵不同點在於遺傳算法通過建立一個比較龐大的解的群（集合），並用這個群不斷進行迭代從而保證其不易陷入局部最優解。相對而言模擬退火算法雖然僅用一個樣本（或者稱一個解）進行迭代，但是其引入Metropolis準則從而避免在抽樣過程中陷入局部最優解這也正是模擬退火算法的核心所在。工勢場法路徑規劃是由Khatib提出的一種虛擬力法（Oussama Khatib，Real-Time obstacle Avoidance for Manipulators and Mobile Robots）。它的基本思想是將機器人在周圍環境中的運動，設計成一種抽象的人造引力場中的運動。人工勢場包括引力場和斥力場，其中目標點對物體產生引力，引導物體朝向其運動（這一點有點類似於A*算法中的啟發函數）。障礙物對物體產生斥力，避免物體與之發生碰撞。物體在路徑上每一點所受的合力等於這一點所有斥力和引力的和。對於二維平面裡面路徑規劃的問題，人工勢場法就如同在這個二維平面內建立起一個三維的勢場，機器人不斷從勢能高（位置高）的地方向勢能低（位置低）的地方移動，直至到達最低的地方。應用勢場法規劃出來的路徑一般是比較平滑並且安全，但是這種方法存在局部最優點問題。同時，當物體離目標點比較遠時，引力將變的特別大，相對較小的斥力在甚至可以忽略的情況下，物體路徑上可能會碰到障礙物；其次當目標點附近有障礙物時，斥力將非常大，引力相對較小，物體很難到達目標點；最後，在某個點，引力和斥力剛好大小相等，方向想反，則物體容易陷入局部最優解或震蕩。
（上圖來源於網際網路）
群算法是受到對真實螞蟻群覓食行為研究的啟發而提出。生物學研究表明：一群相互協作的螞蟻能夠找到食物和巢穴之間的最短路徑，而單只螞蟻則不能。生物學家經過大量細緻觀察研究發現，螞蟻個體之間的行為是相互作用相互影響的。螞蟻在運動過程中，能夠在它所經過的路徑上留下一種稱之為信息素的物質，而此物質恰恰是螞蟻個體之間信息傳遞交流的載體。螞蟻在運動時能夠感知這種物質，並且習慣於追蹤此物質爬行，當然爬行過程中還會釋放信息素。一條路上的信息素蹤跡越濃，其它螞蟻將以越高的概率跟隨爬行此路徑，從而該路徑上的信息素蹤跡會被加強。因此，由大量螞蟻組成的蟻群的集體行為便表現出一種信息正反饋現象。某一路徑上走過的螞蟻越多，則後來者選擇該路徑的可能性就越大。螞蟻個體之間就是通過這種間接的通信機制實現協同搜索最短路徑的目標的。快速擴展隨機樹（RRT / rapidly exploring random tree）的路徑規划算法是通過對狀態空間中的隨機選取的採樣點進行碰撞檢測，避免了相對複雜的對空間的建模，能夠有效地解決高維空間和複雜約束的路徑規劃問題。它以一個初始點作為根節點，通過隨機採樣增加葉子節點的方式，生成一個隨機擴展樹，當隨機樹中的葉子節點包含了目標點或進入了目標區域，便可以在隨機樹中找到一條由從初始點到目標點的路徑。該方法的特點是能夠快速有效地搜索高維空間，通過狀態空間的隨機採樣點，把搜索導向空白區域，從而尋找到一條從起始點到目標點的規劃路徑，適合解決多自由度機器人在複雜環境下和動態環境中的路徑規劃。PRM（Probabilistic Roadmap）是一種基於圖搜索的方法。算法主要思想是將連續空間轉換成離散空間，即在給定圖的自由空間裡隨機撒點（自定義個數），構建一個路徑網格圖，然後在利用A*等搜索算法 在路線圖上尋找路徑，以提高搜索效率。這種方法能用相對少的隨機採樣點來找到一個解，對多數問題而言，相對少的樣本足以覆蓋大部分可行的空間，並且找到路徑的概率為1（隨著採樣數增加，P（找到一條路徑）指數的趨向於1）。顯然，當採樣點太少，或者分布不合理時，PRM算法是不完備的，但是隨著採用點的增加，也可以達到完備。所以PRM是概率完備且不最優的。
迪科斯徹（Dijkstra）算法是由荷蘭計算機科學家艾茲格•W•迪科斯徹 （Edsger Wybe Dijkstra）於1959 年提出的。Dijkstra算法從地圖上設定的起點開始進行算法的初始化，並建立一個特殊的節點集合（這個集合的大小是動態的），這裡暫且把這個集合記著Set-f（fringe）。在初始化的最後再建立一個與地圖上節點矩陣大小一樣的矩陣P，用於存放與地圖矩陣上同樣（索引）位置節點的父節點的索引（這個父節點的索引在下面會提到）。在算法的第一次迭代中將起點的路徑信息傳遞給與起點相鄰的節點，就網狀的地圖而言相當於將路徑信息傳遞給與起點存在直接路徑的節點，對於二維平面裡面的Y-X軸狀柵格地圖而言即試圖將路徑信息傳遞給與其相鄰的8個節點（當起點不位於地圖邊界上）。這裡所謂傳遞路徑信息主要是指，被傳遞路徑信息節點到地圖起點的路徑長度。在第一次迭代中將哪些被傳遞路徑信息的節點插入到節點集合Set-f中，並且將這些被傳遞路徑信息節點的父節點定義為傳遞路徑信息的節點，即被傳遞路徑信息節點在P矩陣中的索引位置上存放的數據是被傳遞路徑信息節點父節點的索引。在進行下一次迭代時，則是從集合Set-f中選取距離起點路徑長度最短的節點作為本次迭代過程中傳遞路徑信息的起點，待傳遞路徑信息的起點被選定後即可將路徑信息傳遞給與傳遞路徑信息節點相鄰的節點（當然這些與傳遞路徑信息節點相鄰的節點中已經曾經作為過傳遞路徑信息起點的節點無需再被傳遞），同時在矩陣P中將被傳遞路徑信息節點的父節點更新為傳遞路徑信息的節點。在完成這樣的路徑信息傳遞後需要將向上面這樣周而復始地進行傳遞，最終直至路徑信息被傳遞到路徑的終點時結束迭代。最後從路徑的終點開始在矩陣P中進行向後的索引，即索引到某個節點的父節點，該父節點再索引其父節點直至索引到路徑起點為止即可生成一條「最短的」的路徑。將Dijkstra算法應用於二維平面內柵格地圖上的算法偽碼如下：

# Procedure Calculate_estimate_value(i)Return  line_distance(i,g);# Procedure Initialize()[r c]=size(Map);State=zeros(r,c);G=inf(r,c);P=inf(r,c);P(s)=s;Set-f=null;Set-f.insert(s,calculate_estimate_value(s));Procedure Main()Initialize();While(State(g)≠closed )cv=Set-f.getMin();   for all negh_cv∈ neighbor(cv)     if(state(negh_cv) ≠closed )           temp_esti=G(cv)+line_distance(negh_cv,cv)           if(temp_esti<G(negh_cv))                 P(negh_cv)=cv;                 G(negh_cv)=temp_esti;     state(negh_cv)=closed;   Set-f.remove(cv);   if(Is_empty(Set_f))   break;

偽碼第1行中函數line_distance(i,g)返回節點i到地圖目標節點g(goal)間的直線距離。障礙物地圖由矩陣Map表示。S表示路徑起點，Set-f.getMin()用於反饋集合Set-f中距離起點路徑長度最短的節點的索引。偽碼12行中neighbor(cv)表示與節點cv相鄰的節點組成的集合，在二維柵格地圖上若cv節點不位於地圖邊界上時與其相鄰的節點有且僅有8個。矩陣G用於存放節點距離地圖上路徑起點的路徑長度。

上圖3-4中標有S的方格代表地圖上的起點，標有G的方格代表路徑的目標點，即終點。圖3-4中深黑色部分的方格代表地圖上的障礙物，灰色部分的方格代表被算法計算區域，黃色部分方格代表在算法結束時該部分的方格仍然還在集合Set-f中，綠色部分方格表示算法搜索得到的路徑。每個方格上的箭頭均指向其父方格（或稱算法中的節點）。每個方格的左下方的兩組數據，由上至下分別表示該方格距離地圖上起點的路徑長度和該方格在Set-f中的評估數值，對於Dijkstra即為G值，對於A-Star即為F值。
A-Star(下稱A*)算法的整體結構和Dijkstra基本一致，關鍵的地方在於A算法為提高算法向目標點搜索的能力，即避免一些不必要的搜索方向而引入啟發函數使得搜索效率的到很大提高。在原有Dijkstra算法的中，當算法即將進行下一次迭代時，是從Set-f集合中選距離起點最近的節點(cv)作為即將進行的迭代過程中的傳遞路徑信息的節點，即G(cv)滿足下式：G(cv)≤G(i)        ∀ i ∈Set-f                  (3.2)相比之下A僅是將Dijkstra算法中上式3.2變換成下式：F(cv)≤F(i)        ∀ i ∈Set-f                  (3.3)F(i)=G(i)+H(i)                      (3.4)式 (3.4)中G(i)依然為節點i到地圖上路徑起點的距離，而H(i)為節點i到地圖上路徑目標點的估計距離。H函數數值的大小通常可以由以下幾類距離中的一類來計算， 曼哈頓距離(Manhattan distance) 、 切比雪夫距離(Chebyshev distance) 、 歐式距離(Euclidean distance)和Octile 距離 。H(i) = (abs(i.x – g.x) + abs(i.y – g.y))           (3.5)H(i) = Max(abs(i.x – g.x) ,abs(i.y – g.y))          (3.6)H(i) =(abs(i.x–g.x) + abs(i.y–g.y))              (3.7)H(i) = sqrt((i.x–g.x)^2 +(i.y–g.y)^2)             (3.8)H=√2Min(abs(i.x – g.x),abs(i.y – g.y)+Max(abs(i.x – g.x) ,abs(i.y – g.y))          (3.9)上式(3.5-3.9)中i.x表示點i的x軸坐標值，abs()為求取絕對值的函數。
對比式(3.2)和(3.4)不難發現Dijkstra與A算法差別在於從Set-f中選擇作為下一次迭代過程中使用的主動傳遞路徑信息節點的方式不樣。Dijkstra幾乎是以起點為圓心在任何一個方向上都進行主動傳遞路徑信息節點的選擇，這樣就導致路徑信息最終幾乎也是以起點為圓心逐漸向圓外進行拓展，沒有明顯的指向性。然而A算法在執行過程中Set-f中靠近路徑起點與路徑終點的節點將會被優先選取，這樣就使得A算法在迭代過程中逐漸向路徑終點靠近，從而明顯提高算法的搜索效率。Field D* 算法是由 Dave Ferguson 和Anthony Stentz提出的一種Any-Angle Path Planning algorithm算法（以下稱無角度約束路徑規劃）。所謂無角度約束路徑規劃主要是指，許多路徑規划算法在迭代過程中傳遞路徑信息的方式是以主動傳遞路徑信息的節點僅向其相鄰的8個節點進行路徑信息傳遞，而且在算法迭代過程中所有節點的父節點一定是與其相鄰的某個節點，這樣將導致算法最終得到的路徑上轉向點轉過的角度一定是45°的倍數（對於橫縱等間距的柵格地圖）。如下圖3-7所示這類有角度約束的算法生成的路徑通常如圖3-7(a)而無角度約束路徑規划算法的目的是找到圖3-7(b)中虛線的路徑。圖3-7中灰色方塊代表地圖上的障礙區域，Sstar和Sgoal分別代表地圖上的起點和終點。

圖3-6 受約束的路徑規劃和無角度約束路徑規劃受到角度約束的路徑規划算法中被傳遞路徑節點的父節點一定是與其相鄰的某個節點之一。
圖3-7 受約束的路徑規劃和無角度約束路徑規劃總的來說在計算靜態地圖時Field D算法的主體結構與3.3節中Dijkstra算法比較相似，主要區別在於Field D算法傳遞路徑信息的方式。與存在角度約束的算法中通過節點向其相鄰節點傳遞路徑信息的方式不同，Field D算法是通過兩個相鄰節點向與這兩個節點均相鄰的節點傳遞路徑信息。如下圖3-8 (a)所示某時刻算法擬對節點s進行路徑信息傳遞（即求出該點至地圖上路徑起點的距離），為確保節點s的路徑長度最短，顯然將s與路徑起點進行直接連線（這裡先假定這條直線不穿越障礙物）節點s可獲得最短的距離，這樣的連線必然穿越與s節點相鄰的兩個節點組成的連線。如圖3-8 (a)所示由s點出發的箭頭與S1和S2組成的線段相交。此時可抽象為S1和S2組成的線段向與S1和S2均相鄰的節點S傳遞路徑信息，具體計算S節點路徑長度的方法可見圖3-9中Field D算法計算路徑信息（路徑長度）的偽碼。
圖3-8 Field D*算法傳遞路徑信息分析下圖3-9中偽碼不難發現Field D算法，首先假定在節點Sa和Sb間的連線上距離起點的路徑長度呈線性分布（這其實與地圖上的實際情況差別比較大，也最終導致Field D算法不能準確地傳遞路徑信息，因為只有很少的情況下才滿足呈線性分布的關係）。過分析不難發現對於在二維地圖中設定起點和終點，尋找最短路徑的問題，使用遺傳算法和模擬退火算法不太恰當。這是因為這兩種算法都是在已知且固定的解空間維度中搜索最優解，而顯然對於尋找最短路徑問題，組成最優解的節點的個數並不是確定的。人工市場法建模方法相對複雜，不恰當的建模方法容易引發算法震蕩和陷入局部最優，而且在差異比較大的地圖環境中使用同一套建模方法算法還有可能出現與障礙物相碰撞或震蕩。蟻群算法雖然具有較好的搜索最優解的能力，但是它和A算法一樣應用在二維柵格地圖上時都存在搜索步長的約束，而無法實現無角度約束路徑規劃（Any-Angle Path Planning），而在求解最優路徑的所花的時間上A算法相比於蟻群算法有非常大的優勢。Field D算法是一種相對誕生比較早的無角度約束路徑規划算法，雖然後面有湧現出其它的同類算法（Any-Angle Path Planning），例如Theta-star、Block A-star 和Incremental Phi-star，這些算法的運行效率或求解的最優路徑相比Field D*並沒有明顯的提高。