路徑規劃算法--PRM，從原理到Matlab實現

注：本篇博客轉載自運動規劃入門 | 3. 白話PRM，從原理到Matlab實現，老師講的太好了，我這裏轉載留存下。

上一回，我們講完了A的工作原理，與Dijkstar相比A確實有一定程度上的優化，但是我們最後也提到了，即便如此A和Dijkstar一樣，依舊還是在逐一遍歷地圖中的每一個柵格，這對於算法的實際使用是十分不友好的，畢竟我們不可能使用一張2020或者100*100的地圖來描述實際的物理世界，往往實際上的地圖是會遠遠大於這個尺寸的。

在座的小夥伴們一定知道在概率統計中，有一個概念叫做抽樣調查，對大量的數據進行抽樣的話，可以大大地壓縮數據量，並且抽樣的結果依然可以代表整體的特徵。PRM（概率路線圖）便是具有相似的原理。PRM就是通過對一張大地圖進行稀疏採樣，從而將原本很巨大的柵格地圖簡化爲較少的採樣點和它們之間的邊組成的“無向圖”。沒錯，就是圖論意義上的那個圖。從而我們可以拿着這張簡化了的地圖進行路徑規劃。

Dijkstar、A*和PRM之間的那點關係

其實在路徑規劃中，我們把生成一個計劃稱爲“規劃階段”，而依賴規劃階段的結果來找到一條從A到B的路徑則稱爲“查詢階段”。在前兩篇中我並未闡明這兩者之間的區別，因爲在Dijkstar和A中，主要是屬於查詢階段，這兩個算法都是在執行在A點到B點之間尋找路徑這麼一件事。而PRM和前者不一樣，PRM的主要任務是將原本的柵格地圖轉化爲一張稀疏無向圖。而且在後面的介紹中你會發現，實際上PRM和Dijkstar以及A之間並不是互斥的，相反他們經常被結合起來使用，首先由PRM簡化地圖，然後將簡化得到的無向圖交給Dijkstar或者A*進行查詢。所以他們是各司其職的，並不矛盾。在本文中，我會以Dijkstar爲例，展示整套流程。

PRM是如何運作的？
2.1 基本原理

實際上PRM的原理非常好理解，想象着面前有一張平鋪的地圖，現在我們拿着一堆點往上撒。如下圖所示，藍色幾何塊代表着障礙物，灰色部分表示自由空間，起初圖中只有綠色的起點和黃色的終點，現在我們往地圖上撒第一個點，那麼顯然會有兩種可能，一個是很不巧，把點撒在了障礙物上，那麼這種情況我們必須要捨棄這個點，重新撒一次，直到我們成功地把點撒到灰色區域。假設這時候我們發現這個點可以連接到起點，於是我們就得到了第一條邊。

當我們重複這個過程，不斷地往地圖上撒點，當然，並不是每一個點都能和相鄰的點連接起來的，比如圖中的虛線就代表着這兩點之間被障礙物阻隔了，這種情況我們就必須捨棄掉這條邊

假設我們設置採樣點爲10個點，那麼結果就會得到像下面這樣的一張稀疏的無向圖。這張無向圖即保留了原本柵格地圖的特徵，而且大大壓縮了數據量。

在第一篇中，我在講解Dijkstar的原理的時候就講過無向圖，並以此爲例說明了Dijkstar的運作過程，所以我們知道這種稀疏的無向圖其實是對Dijkstar和A*這種規劃算法十分友好的，我們將PRM的輸出——稀疏無向圖，作爲輸入交給Dijkstar進行規劃，很快就能找到從起點到終點之間的最優路徑。

2.2 步長限制
在實際的算法實現中，往往我們會添加一個步長的限制，或者叫連接距離限制，比如我們設置將步長限制在10以內，也就是說如果一個點與它的所有相鄰點的距離超過10的話，該點就會被算法放棄。在某些場景下我們可能不希望機器一直走大直線，我們希望它的路線更加光滑，那麼我們就可以將這個步長限制在一個小範圍內，那麼最終得到的路徑便會是一小段一小段的直線路徑連接起來的樣子，後期對該路徑進行光滑處理，便可以得到一條平滑的路徑。

2.3 碰撞檢測
碰撞檢測是PRM算法中非常非常重要的一環，沒有碰撞檢測的話，我們將無法正確構建一張避開障礙的無向圖。

碰撞檢測最常見的思路便是：如下圖所示，假設我們要對A，B兩點之間的連線做碰撞檢測，那麼我們會對矢量AB進行等距採樣，從中挑選出若干個相互等距的採樣點，從而逐個檢測其是否處於障礙物的位置。至於要從中取多少個採樣點，這取決於你對其檢測精度的要求。顯而易見採樣點越多，碰撞檢測就越精確，但是當採樣點數量到達一定高度後，再往上增加的話，檢測精度的提高不再明顯，反而會增大計算壓力。