基于PRM優化算法的移動機器人路徑規劃

程 謙 高 嵩 曹 凱 陳超波

(西安工業大學電子信息工程學院 陜西 西安 710021)

0 引 言

近些年移動機器人技術得到了飛躍性的進步，而路徑規劃作為移動機器人技術的重要基礎成為了研究的熱點。目前，路徑規劃算法被應用于工業、服務、娛樂等各個方面，例如無人車、掃地機器人、餐廳機器人等。機器人在執行任務時都依賴路徑規劃算法，而有效的路徑規劃技術可以使機器人完成任務時節省大量的物力財力，同時也可以提高執行任務時的安全性[1]。

根據路徑規劃在規劃過程中搜索的方式不同主要分為三大類：基于圖論的路徑搜索算法，基于離散采樣的規劃算法，智能路徑規劃算法。基于圖論的搜索算法于1959年被提出，主要思想是將狀態空間定義為占據網格，將障礙定義為不可訪問的網格點，然后算法搜索可用的網格點，如果路徑存在，則給出解決方案。如果網格分辨率足夠，基于圖論的算法可以保證解決方案。Dijkstra算法[2]作為圖論經典的算法被廣泛應用，隨后又發展出A*算法[3]、DFS算法[4]等。文獻[5]將圖論的規劃方法與其他算法進行了比較，在解決兩點之間最短距離的情況下，圖論的方法較其他算法有較大的優勢。基于離散采樣的規劃算法的主要思想是在配置空間中選擇非結構有限數量的點，并在這些點之間建立連接。基于采樣的方法是簡單、通用、高效和概率完備的(當時間接近無限時一定有解)，如隨機路徑圖法(PRM)[6]、快速隨機擴展樹(RRT)[7]等。基于采樣的規劃算法雖然一定能夠規劃出路徑，但是因為其離散隨機的特性解不一定是最優的。以PRM算法為例，在面對含有狹窄通道時算法的效率和成功率會明顯下降。針對這個問題文獻[8]提出了一種長短期存儲器(LSTM)模型，通過預測障礙物位置，將障礙物軌跡劃分為時間序列，并將下一時刻的位置估計引入到規劃算法的狀態有效性測試中，以生成最優路徑。通過對障礙物模型的預測，可以減少穿越和重編程的時間，通過優化采樣階段，也可以增加狹窄通道中采樣點的數量，從而提高算法的效率。智能路徑規劃算法是近些年慢慢發展起來的一個熱門方向，因為人工智能的興起，越來越多的智能算法被應用在移動機器人路徑規劃上，如遺傳算法[9]、粒子群優化算法[10]、蟻群算法[11]和人工勢場法[12]等。文獻[13]將人工勢場法和遺傳算法進行結合，通過引入指數因子來模擬斥力，從而解決了機器人在尋找路徑過程中易陷入局部極小值的問題，然后利用遺傳算法進行尋優，找出最優路徑。

目前，隨著機器人應用范圍越來越廣，機器人所處環境的障礙物也越來越復雜。基于圖論搜索的傳統規劃算法需要精確定位障礙物，其算法計算量過大，在現實中非常不適用。而基于離散采樣規劃算法的PRM算法可以有效避免對位姿空間精確定位，并且算法的復雜度并不依賴于地圖的復雜度，所以可以有效地應用于實際。但是PRM算法在實際的應用中也會有各種問題，例如在處理狹窄通道方面有明顯的不足。針對這個問題，文獻[14-16]提出了利用高斯采樣、橋測試法和自適應采樣研究方法，旨在提高狹窄通道中的采樣點個數，從而提高對狹窄通道的適應性。此外，PRM算法因為采樣點個數較多，所花費的時間較長，大大限制了其應用性。文獻[17]提出了減少碰撞檢測的思想來加大算法的效率，但是在一定程度上又增加了算法復雜性。

本文對PRM算法進行深入研究，提出一種基于連接點的優化方法，剔除算法在學習階段構建的無效路徑，使路徑總數減少，從而縮短在查詢階段的搜索時間，有效地提高了PRM算法的規劃速度。同時因為規劃出的路徑連接了更多的采樣點，使路線在彎道有更好的適應性，顯著提升了路線的安全性。

1 PRM算法基本原理

通過在環境中建立路徑網絡圖，將連續的空間轉化為離散的空間，然后再進行路徑規劃，使算法的復雜程度只依賴于搜索路徑的復雜度，從而使復雜程度降低。PRM算法也可以用在多自由度的移動機器人路徑規劃中，而且可以有效地克服局部極小值的缺點，在解決高維空間和復雜障礙物的情況下有計算量小的優勢。

PRM算法的基本思想是用概率圖來表示移動機器人所處環境的自由空間。在自由空間內進行隨機采樣，把采樣點加入到圖集中，利用局部規劃器把各個采樣點進行連接，構成路徑網絡圖，然后在這個隨機網絡中使用搜索算法來找到移動機器人的可行路徑。該算法可以分兩個階段完成，即離線學習階段和查詢階段。

1.1 學習階段

在自由空間中進行盡可能隨機且均勻的采樣，然后連接各個點，構建出一幅路徑網絡圖。

學習階段具體步驟如下：

1)創建采樣點集x

2)在地圖中采用均勻采樣隨機采取一點ni

3)while:采集到足夠的采樣點跳出

4) if:ni在障礙物內

5)舍棄該點

6)else:將ni加入到點集x

7)end

8)創建路徑集f

9)while:分別依次連接點集x中的兩點xinit,xgoal

10)if:兩點之間連線碰撞到障礙物

11)舍棄該路線

12)else:將兩點之間路線加入路徑集f

13)end

14)建立無向網絡圖G(x,f)

15)if:G(x,f)為空

16)不存在路徑

17)else:查找最優路徑

PRM算法學習階段是在地圖內進行隨機采樣，并對采樣點進行碰撞檢測，從而保證采樣點不會落在地圖障礙物內，使采樣點隨機均勻地分布在自由無障礙空間內。采樣完成后通過連接各個點，構建路徑網絡圖。構建網絡圖時，采用從一個點開始，向周圍點擴散的方法，如果兩個點之間可以連接，并且沒有碰到障礙物，就把路線添加到網絡圖中，反之則舍棄，從而構建出整幅路徑網絡圖，如圖1所示。其中：左上角大圓點代表起始點；右下角大圓點代表終點；黑色為障礙物；小點為采樣點；細線為路徑網絡圖中路徑。

(a)采樣圖 (b)網絡路徑圖

1.2 查詢階段

查詢階段步驟利用學習階段建立好的無向路徑網絡圖，在無向網絡圖中設置好起點和終點，根據設置好的起點和終點搜索出一條符合需求的路徑，具體可以分為以下三步：

(1)把地圖中離起始點和目標點最近的兩個點分別進行連接；

(2)在網絡圖中搜索路徑，找到起始點到目標點的路線；

(3)找到路徑后，通過平滑處理，優化路徑。

最終結果如圖2所示，深色粗線為規劃出的路線。

圖2 PRM查詢階段

2 PRM算法優化

PRM算法在學習階段通過對隨機地圖進行均勻采樣，使采樣點在障礙物地圖中均勻分布，通過連接各個采樣點構建出路徑網絡圖，隨后在查詢階段中利用A*算法進行路徑查詢，從而尋找出最優路徑。在PRM算法整個階段中學習階段花費的時間最少，影響算法整體效率的主要是查詢階段，而路徑網絡圖中的路徑數量直接影響查詢階段花費的時間。

為了有效地減少路徑網絡圖中的路徑數量，對學習階段進行改進。通過改變連接點的距離，使一些連接距離較遠且不合理的路徑大大減少，導致算法在查詢階段查找的路徑減少，從而減少花費的時間，進而提高運算效率。以未優化前一個點所構成的路徑網絡圖為例，如圖3(a)所示，A點在學習階段連接各個點，構建一幅不接觸障礙物的路徑網絡圖(如果路線接觸障礙物自動舍棄不連接)。為了減少路徑的數量，限定采樣點的連接距離，經過優化后A點的連接如圖3(b)中實線所示，距離A點較遠的E點和F點不進行連接，使構成的路徑網絡的路徑數量大大減少。

因為限定了連接點的距離，優化后的路線圖中路線較長的路徑消失，為了尋找到最優路徑，路線圖會連接更多的采樣點，使得規劃出的路徑在彎道處更加靈活，加大路線離障礙物的距離，如優化前A-F和A-E路線(圖3(a)中所示)優化后變為A-H-F和A-C-D-E路線(圖3(b)中虛線所示)。雖然優化后的路線在一定程度上加大了最終規劃出來路徑的長度，但是使得路線更加安全可靠，加大了在實際中的應用性和實用性。

(a)優化前 (b)優化后

優化過程中連接點限定距離的取值可以根據實際地圖情況進行選取。限定距離取值越大越接近傳統PRM算法，優化效果越弱；限定距離越小，網絡路徑圖中路線的數量越少，并且構成最優路徑時連接的采樣點越多，效果越顯著。但當限定距離過小時，因為連接不到采樣點，可能使路徑規劃失敗，所以設置最小值，在最小值上依次疊加，直到取到第一個采樣點，進而找到能保證規劃成功的最小限定距離。

3 優化算法仿真及結果分析

為了驗證改進算法的有效性，在如圖4所示的兩種地圖中進行仿真，同時與優化前PRM算法進行對比。在仿真過程中為計算方便，設置連接點最小距離為1米，仿真實驗的采樣點數量為150和300，對比在不同采樣點情況下優化算法的優化效果。

(a)簡單障礙物地圖 (b)復雜障礙物地圖

3.1 算法仿真

當采樣點N=50時，簡單地圖優化前后結果如圖5(a)和圖5(b)所示，能夠規劃出路徑，并且經優化后路徑網絡圖中的路徑數量明顯減少，并且路線較傳統相比離障礙物較遠。復雜障礙物地圖優化前后如圖5(c)和圖5(d)所示，因為采樣點過少，在建立出的路徑網絡圖中沒有可以到達目標點的路徑，規劃失敗。

(a)簡單障礙物地圖優化前 (b)簡單障礙物地圖優化后

當采樣點N=150時，簡單地圖優化前后結果如圖6(a)和圖6(b)所示，優化前所構成的路徑網絡圖都有較多的路徑，優化后路徑顯著減少，都能夠合理地規劃出路徑，但優化后的路徑在彎道處離障礙物較遠，安全性比優化前增強。復雜地圖優化前后結果如圖6(c)和圖6(d)所示，優化后路徑網絡圖中路徑的數量明顯減少，并且路徑安全顯著增強。

(a)簡單障礙物地圖優化前 (b)簡單障礙物地圖優化后

當采樣點N=300時，優化前圖像如圖7(a)和圖7(b)所示，優化后結果如圖7(c)和圖7(d)所示。

(a)簡單障礙物地圖優化前 (b)簡單障礙物地圖優化后

3.2 算法優化前后仿真成功率分析

成功的路徑規劃才是路線安全的根本，因此為了驗證改進算法的成功率，對復雜地圖進行改進，增加多個障礙物，使障礙物密度大大提高，能夠充分地測試算法的有效性和成功率，改進的復雜地圖如圖8所示。在地圖中固定采樣點為150個，對算法優化前后分別進行150次重復性實驗。改進的復雜地圖仿真結果如圖9所示。

圖8 改進的復雜地圖

(a)優化前PRM算法 (b)優化后PRM算法

3.3 結果分析

仿真結果如表1所示。采樣點個數可以決定能否規劃出路徑，采樣點數量越多，成功規劃出路線的概率越大，特別是復雜地圖中，如果采樣點過少，則可能規劃不出路徑，導致任務失敗。在采樣點相同時，優化后的PRM算法與PRM算法相比，時間最大縮短了近50%，且隨著采樣點數量的上升愈發明顯。在安全性上對比，改進后算法距障礙物最短的安全距離要遠遠大于改進前，使得路線更加可靠。由表2可知，優化后的PRM算法在同一幅地圖相同采樣點時，規劃路徑的成功率有所提升，增強了算法的適應性。綜上可得，優化后的PRM算法在時間、成功率和安全性上都有了顯著的提高，增強了PRM算法在實際中的實用性。

表1 不同地圖和采樣點優化前后結果

表2 算法優化前后仿真成功率對比

4 基于ROS的仿真分析

為了驗證算法實際的應用性，在Linux操作系統上的ROS環境中使用物理仿真平臺 Stage 進行機器人路徑規劃仿真。仿真過程采用的機器人是一個搭載激光雷達可編程的基于 ROS 的移動機器人。在Stage下構建障礙物地圖，如圖10所示，黑色為障礙物，圓形物體為移動機器人。機器人上搭載激光雷達，可以實時地避障和構建地圖，在可視化工具Rviz中可以實時觀察到機器人激光雷達實時構建地圖，如圖11所示，黑色為障礙物。

(a) (b)

圖11 激光雷達實時構建地圖

機器人起始點設為(-1.8，-5.4)，位置如圖12(a)所示。終點設置為(-2，3),位置如圖12(b)所示。在相同采樣點N=150的情況下分別使用PRM算法和優化的PRM算法進行路徑優化，PRM算法路徑規劃結果如圖13所示，其中灰線表示所構成的路徑網絡圖。優化前PRM算法的規劃結果如圖13(a)所示，機器人花費的時間為1.5 s，優化后PRM算法規劃結果如圖13(b)所示，機器人花費的時間為0.9 s。

(a)起始點(-1.8,5.4) (b)終點(-2,3)

(a)優化前PRM算法 (b)優化后PRM算法

優化后PRM算法較優化前在機器人實時路徑規劃時所構成的路徑網絡圖明顯減少，在搜索效率上有明顯改觀，并且路徑規劃的結果較之前在彎道處遠離障礙物，安全性明顯提升。

為了驗證優化算法的成功率，進行了30次重復性實驗，實驗結果如表3所示，在機器人實際路徑規劃時優化后的PRM算法較優化前，在花費時間較少的前提下，成功率也有所提升。但實際機器人路徑規劃成功率因為要考慮數據的誤差，以及機器人自身的物理體積，與MATLAB純算法仿真相比成功率明顯下降。

表3 機器人物理仿真結果

5 結 語

本文分析研究PRM算法的原理和過程，針對PRM算法運行速度和安全性進行改進，提出基于連接點距離的改進方法，通過改變連接點的連接距離，減少不合理(如連接點間距離遠的情況)的路徑，使運行速度有明顯的提升，并增加了障礙物的安全距離。本文優化方法應用方便，結構簡單，優化后的算法能夠有效地提高實時性和安全性，更好地運用在機器人實時路徑中。