基于PRM優(yōu)化算法的移動(dòng)機(jī)器人路徑規(guī)劃

程 謙 高 嵩 曹 凱 陳超波

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

0 引 言

近些年移動(dòng)機(jī)器人技術(shù)得到了飛躍性的進(jìn)步，而路徑規(guī)劃作為移動(dòng)機(jī)器人技術(shù)的重要基礎(chǔ)成為了研究的熱點(diǎn)。目前，路徑規(guī)劃算法被應(yīng)用于工業(yè)、服務(wù)、娛樂等各個(gè)方面，例如無人車、掃地機(jī)器人、餐廳機(jī)器人等。機(jī)器人在執(zhí)行任務(wù)時(shí)都依賴路徑規(guī)劃算法，而有效的路徑規(guī)劃技術(shù)可以使機(jī)器人完成任務(wù)時(shí)節(jié)省大量的物力財(cái)力，同時(shí)也可以提高執(zhí)行任務(wù)時(shí)的安全性[1]。

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

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

本文對PRM算法進(jìn)行深入研究，提出一種基于連接點(diǎn)的優(yōu)化方法，剔除算法在學(xué)習(xí)階段構(gòu)建的無效路徑，使路徑總數(shù)減少，從而縮短在查詢階段的搜索時(shí)間，有效地提高了PRM算法的規(guī)劃速度。同時(shí)因?yàn)橐?guī)劃出的路徑連接了更多的采樣點(diǎn)，使路線在彎道有更好的適應(yīng)性，顯著提升了路線的安全性。

1 PRM算法基本原理

通過在環(huán)境中建立路徑網(wǎng)絡(luò)圖，將連續(xù)的空間轉(zhuǎn)化為離散的空間，然后再進(jìn)行路徑規(guī)劃，使算法的復(fù)雜程度只依賴于搜索路徑的復(fù)雜度，從而使復(fù)雜程度降低。PRM算法也可以用在多自由度的移動(dòng)機(jī)器人路徑規(guī)劃中，而且可以有效地克服局部極小值的缺點(diǎn)，在解決高維空間和復(fù)雜障礙物的情況下有計(jì)算量小的優(yōu)勢。

PRM算法的基本思想是用概率圖來表示移動(dòng)機(jī)器人所處環(huán)境的自由空間。在自由空間內(nèi)進(jìn)行隨機(jī)采樣，把采樣點(diǎn)加入到圖集中，利用局部規(guī)劃器把各個(gè)采樣點(diǎn)進(jìn)行連接，構(gòu)成路徑網(wǎng)絡(luò)圖，然后在這個(gè)隨機(jī)網(wǎng)絡(luò)中使用搜索算法來找到移動(dòng)機(jī)器人的可行路徑。該算法可以分兩個(gè)階段完成，即離線學(xué)習(xí)階段和查詢階段。

1.1 學(xué)習(xí)階段

在自由空間中進(jìn)行盡可能隨機(jī)且均勻的采樣，然后連接各個(gè)點(diǎn)，構(gòu)建出一幅路徑網(wǎng)絡(luò)圖。

學(xué)習(xí)階段具體步驟如下：

1)創(chuàng)建采樣點(diǎn)集x

2)在地圖中采用均勻采樣隨機(jī)采取一點(diǎn)ni

3)while:采集到足夠的采樣點(diǎn)跳出

4) if:ni在障礙物內(nèi)

5)舍棄該點(diǎn)

6)else:將ni加入到點(diǎn)集x

7)end

8)創(chuàng)建路徑集f

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

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

11)舍棄該路線

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

13)end

14)建立無向網(wǎng)絡(luò)圖G(x,f)

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

16)不存在路徑

17)else:查找最優(yōu)路徑

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

(a)采樣圖 (b)網(wǎng)絡(luò)路徑圖

1.2 查詢階段

查詢階段步驟利用學(xué)習(xí)階段建立好的無向路徑網(wǎng)絡(luò)圖，在無向網(wǎng)絡(luò)圖中設(shè)置好起點(diǎn)和終點(diǎn)，根據(jù)設(shè)置好的起點(diǎn)和終點(diǎn)搜索出一條符合需求的路徑，具體可以分為以下三步：

(1)把地圖中離起始點(diǎn)和目標(biāo)點(diǎn)最近的兩個(gè)點(diǎn)分別進(jìn)行連接；

(2)在網(wǎng)絡(luò)圖中搜索路徑，找到起始點(diǎn)到目標(biāo)點(diǎn)的路線；

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

最終結(jié)果如圖2所示，深色粗線為規(guī)劃出的路線。

圖2 PRM查詢階段

2 PRM算法優(yōu)化

PRM算法在學(xué)習(xí)階段通過對隨機(jī)地圖進(jìn)行均勻采樣，使采樣點(diǎn)在障礙物地圖中均勻分布，通過連接各個(gè)采樣點(diǎn)構(gòu)建出路徑網(wǎng)絡(luò)圖，隨后在查詢階段中利用A*算法進(jìn)行路徑查詢，從而尋找出最優(yōu)路徑。在PRM算法整個(gè)階段中學(xué)習(xí)階段花費(fèi)的時(shí)間最少，影響算法整體效率的主要是查詢階段，而路徑網(wǎng)絡(luò)圖中的路徑數(shù)量直接影響查詢階段花費(fèi)的時(shí)間。

為了有效地減少路徑網(wǎng)絡(luò)圖中的路徑數(shù)量，對學(xué)習(xí)階段進(jìn)行改進(jìn)。通過改變連接點(diǎn)的距離，使一些連接距離較遠(yuǎn)且不合理的路徑大大減少，導(dǎo)致算法在查詢階段查找的路徑減少，從而減少花費(fèi)的時(shí)間，進(jìn)而提高運(yùn)算效率。以未優(yōu)化前一個(gè)點(diǎn)所構(gòu)成的路徑網(wǎng)絡(luò)圖為例，如圖3(a)所示，A點(diǎn)在學(xué)習(xí)階段連接各個(gè)點(diǎn)，構(gòu)建一幅不接觸障礙物的路徑網(wǎng)絡(luò)圖(如果路線接觸障礙物自動(dòng)舍棄不連接)。為了減少路徑的數(shù)量，限定采樣點(diǎn)的連接距離，經(jīng)過優(yōu)化后A點(diǎn)的連接如圖3(b)中實(shí)線所示，距離A點(diǎn)較遠(yuǎn)的E點(diǎn)和F點(diǎn)不進(jìn)行連接，使構(gòu)成的路徑網(wǎng)絡(luò)的路徑數(shù)量大大減少。

因?yàn)橄薅诉B接點(diǎn)的距離，優(yōu)化后的路線圖中路線較長的路徑消失，為了尋找到最優(yōu)路徑，路線圖會(huì)連接更多的采樣點(diǎn)，使得規(guī)劃出的路徑在彎道處更加靈活，加大路線離障礙物的距離，如優(yōu)化前A-F和A-E路線(圖3(a)中所示)優(yōu)化后變?yōu)锳-H-F和A-C-D-E路線(圖3(b)中虛線所示)。雖然優(yōu)化后的路線在一定程度上加大了最終規(guī)劃出來路徑的長度，但是使得路線更加安全可靠，加大了在實(shí)際中的應(yīng)用性和實(shí)用性。

(a)優(yōu)化前 (b)優(yōu)化后

優(yōu)化過程中連接點(diǎn)限定距離的取值可以根據(jù)實(shí)際地圖情況進(jìn)行選取。限定距離取值越大越接近傳統(tǒng)PRM算法，優(yōu)化效果越弱；限定距離越小，網(wǎng)絡(luò)路徑圖中路線的數(shù)量越少，并且構(gòu)成最優(yōu)路徑時(shí)連接的采樣點(diǎn)越多，效果越顯著。但當(dāng)限定距離過小時(shí)，因?yàn)檫B接不到采樣點(diǎn)，可能使路徑規(guī)劃失敗，所以設(shè)置最小值，在最小值上依次疊加，直到取到第一個(gè)采樣點(diǎn)，進(jìn)而找到能保證規(guī)劃成功的最小限定距離。

3 優(yōu)化算法仿真及結(jié)果分析

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

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

3.1 算法仿真

當(dāng)采樣點(diǎn)N=50時(shí)，簡單地圖優(yōu)化前后結(jié)果如圖5(a)和圖5(b)所示，能夠規(guī)劃出路徑，并且經(jīng)優(yōu)化后路徑網(wǎng)絡(luò)圖中的路徑數(shù)量明顯減少，并且路線較傳統(tǒng)相比離障礙物較遠(yuǎn)。復(fù)雜障礙物地圖優(yōu)化前后如圖5(c)和圖5(d)所示，因?yàn)椴蓸狱c(diǎn)過少，在建立出的路徑網(wǎng)絡(luò)圖中沒有可以到達(dá)目標(biāo)點(diǎn)的路徑，規(guī)劃失敗。

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

當(dāng)采樣點(diǎn)N=150時(shí)，簡單地圖優(yōu)化前后結(jié)果如圖6(a)和圖6(b)所示，優(yōu)化前所構(gòu)成的路徑網(wǎng)絡(luò)圖都有較多的路徑，優(yōu)化后路徑顯著減少，都能夠合理地規(guī)劃出路徑，但優(yōu)化后的路徑在彎道處離障礙物較遠(yuǎn)，安全性比優(yōu)化前增強(qiáng)。復(fù)雜地圖優(yōu)化前后結(jié)果如圖6(c)和圖6(d)所示，優(yōu)化后路徑網(wǎng)絡(luò)圖中路徑的數(shù)量明顯減少，并且路徑安全顯著增強(qiáng)。

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

當(dāng)采樣點(diǎn)N=300時(shí)，優(yōu)化前圖像如圖7(a)和圖7(b)所示，優(yōu)化后結(jié)果如圖7(c)和圖7(d)所示。

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

3.2 算法優(yōu)化前后仿真成功率分析

成功的路徑規(guī)劃才是路線安全的根本，因此為了驗(yàn)證改進(jìn)算法的成功率，對復(fù)雜地圖進(jìn)行改進(jìn)，增加多個(gè)障礙物，使障礙物密度大大提高，能夠充分地測試算法的有效性和成功率，改進(jìn)的復(fù)雜地圖如圖8所示。在地圖中固定采樣點(diǎn)為150個(gè)，對算法優(yōu)化前后分別進(jìn)行150次重復(fù)性實(shí)驗(yàn)。改進(jìn)的復(fù)雜地圖仿真結(jié)果如圖9所示。

圖8 改進(jìn)的復(fù)雜地圖

(a)優(yōu)化前PRM算法 (b)優(yōu)化后PRM算法

3.3 結(jié)果分析

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

表1 不同地圖和采樣點(diǎn)優(yōu)化前后結(jié)果

表2 算法優(yōu)化前后仿真成功率對比

4 基于ROS的仿真分析

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

(a) (b)

圖11 激光雷達(dá)實(shí)時(shí)構(gòu)建地圖

機(jī)器人起始點(diǎn)設(shè)為(-1.8，-5.4)，位置如圖12(a)所示。終點(diǎn)設(shè)置為(-2，3),位置如圖12(b)所示。在相同采樣點(diǎn)N=150的情況下分別使用PRM算法和優(yōu)化的PRM算法進(jìn)行路徑優(yōu)化，PRM算法路徑規(guī)劃結(jié)果如圖13所示，其中灰線表示所構(gòu)成的路徑網(wǎng)絡(luò)圖。優(yōu)化前PRM算法的規(guī)劃結(jié)果如圖13(a)所示，機(jī)器人花費(fèi)的時(shí)間為1.5 s，優(yōu)化后PRM算法規(guī)劃結(jié)果如圖13(b)所示，機(jī)器人花費(fèi)的時(shí)間為0.9 s。

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

(a)優(yōu)化前PRM算法 (b)優(yōu)化后PRM算法

優(yōu)化后PRM算法較優(yōu)化前在機(jī)器人實(shí)時(shí)路徑規(guī)劃時(shí)所構(gòu)成的路徑網(wǎng)絡(luò)圖明顯減少，在搜索效率上有明顯改觀，并且路徑規(guī)劃的結(jié)果較之前在彎道處遠(yuǎn)離障礙物，安全性明顯提升。

為了驗(yàn)證優(yōu)化算法的成功率，進(jìn)行了30次重復(fù)性實(shí)驗(yàn)，實(shí)驗(yàn)結(jié)果如表3所示，在機(jī)器人實(shí)際路徑規(guī)劃時(shí)優(yōu)化后的PRM算法較優(yōu)化前，在花費(fèi)時(shí)間較少的前提下，成功率也有所提升。但實(shí)際機(jī)器人路徑規(guī)劃成功率因?yàn)橐紤]數(shù)據(jù)的誤差，以及機(jī)器人自身的物理體積，與MATLAB純算法仿真相比成功率明顯下降。

表3 機(jī)器人物理仿真結(jié)果

5 結(jié) 語

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