基于光節(jié)點模型的PRM 路徑規(guī)劃優(yōu)化算法研究

韓 超楊 杰

(青島大學機電工程學院,山東 青島 266071)

近年來科學技術迅速發(fā)展,自主移動機器人的基礎研究課題——機器人的路徑規(guī)劃獲得了更廣闊的發(fā)展空間,得到了更多學者的關注。它能夠在有障礙物的環(huán)境下,于起始點和目標點之間生成一條無碰撞軌跡。在手術機器人[1]、智能汽車[2]、農(nóng)業(yè)采摘[3]、船舶[4]等各個方面有廣泛應用。目前常用的算法有A*算法[5-7]、快速隨機搜索樹算法[8--10]、概率路線圖算法[11-13]人工勢場法[14-15]、蟻群算法[16]等。其中,PRM 算法在多數(shù)情況下能用較少的采樣點,規(guī)劃出起始點至目標點的無碰撞軌跡。然而,仍存在無法通過狹窄區(qū)域,導致結果大幅偏離最優(yōu)路徑或規(guī)劃失敗的問題[17]。程謙等人[18]提出了一種基于連接點的優(yōu)化方法,剔除算法在學習階段構建的無效路徑,使路徑總數(shù)減少,縮短在查詢階段的搜索時間,有效地提高了PRM 算法的規(guī)劃速度,但路徑平滑度未改善,算法仍存在可能無解的問題;鄒善席等人[19]引入隨機節(jié)點生成函數(shù)和改進的節(jié)點增強法,減少了原始路徑上節(jié)點數(shù)目,縮短路徑長度,有效提高了路徑平滑度,但引入隨機節(jié)點生成函數(shù)并未改變在狹窄地圖中可能無解的問題;譚建豪等人[20]提出將障礙物邊界點作為PRM 算法的確定采樣點,并在自由空間中構建最優(yōu)可行區(qū)域,降低隨機采樣點的分散性,提高算法的時間和空間利用率,解決了在狹窄通道地圖中可能無解的情況,但該算法運行時間增加,降低了運行效率。由此可知,上述研究無法同時解決路徑平滑度低、在狹窄地圖中可能無解、程序運行效率低的問題。針對這種情況,本文基于光照方法提出了一種優(yōu)化PRM 算法,將起始點、目標點和采樣的節(jié)點視為光源,提取未照亮范圍并在其中進行采樣,優(yōu)化了采樣方法,既減少了采樣節(jié)點的數(shù)量,又解決了路徑無解的問題,在狹窄直線通道地圖具有良好的應用前景。

1 PRM 算法描述

1.1 學習階段

PRM 算法是一種基于圖搜索的方法,傳統(tǒng)的PRM 算法分為學習和查詢兩個階段。

學習階段任務是構建無向網(wǎng)絡路徑圖G＝(V,E),其中V代表節(jié)點集,E代表兩節(jié)點之間的邊集。首先,在整個地圖中隨機產(chǎn)生N個采樣節(jié)點,保留在自由空間中的n個節(jié)點,構成路徑圖中的V;其次,將V中的每個節(jié)點與其鄰居節(jié)點相連,并去除與障礙物碰撞的連線,構成路徑圖中的E。

1.2 查詢階段

查詢階段的任務是通過A*算法、深度優(yōu)先搜索算法等搜索算法,在學習階段構建的無向網(wǎng)絡路徑圖中,找到一條連接起始點和目標點的無碰撞軌跡。傳統(tǒng)PRM 算法為隨機采樣,存在采樣點分布不均勻的情況,特別是在有狹窄通道的地圖中,采樣點會集中于空曠的自由空間,而在狹窄通道處不易產(chǎn)生采樣點,容易出現(xiàn)路徑無解的情況。

2 PRM 算法優(yōu)化

為了解決傳統(tǒng)PRM 算法路徑平滑度低、在狹窄地圖中可能無解、程序運行效率低的問題,優(yōu)化后的采樣方法將采樣節(jié)點視為光源,在未照亮區(qū)域進行采樣,可以減少采樣節(jié)點的數(shù)量。

2.1 光照區(qū)域獲取

將起始點Ps與目標點Pg視為兩個光源,獲取Ps和Pg的光照區(qū)域。光照區(qū)域獲取過程如圖1所示。由圖1a可以看出,地圖中的障礙物為黑色,以起始點Ps為光源中心,向周圍360°釋放射線。由圖1b可以看出,沿著以光源為起點的射線,間隔相同步長多次取點,直至找到射線與障礙物碰撞的所有點,將這些點存儲為多邊形的頂點,構成表示光照區(qū)域的多邊形。

圖1 光照區(qū)域獲取過程

獲取光照區(qū)域的偽代碼如下:

2.2 節(jié)點采樣

光照區(qū)域為灰色部分,光照節(jié)點采樣過程如圖2所示。由圖2a可以看出,將光照區(qū)域分別存儲為多邊形,并將其添加到多邊形列表中。

圖2 光照節(jié)點采樣過程

若多邊形列表中所有多邊形存在連接起始點與目標點的光路,則找到連通Ps和Pg的軌跡;若未找到,則在未照亮的自由區(qū)域進行采樣,得到節(jié)點P1,并將P1的光照范圍存儲為新的多邊形。重復上述步驟,將新的多邊形添加到多邊形列表中,繼續(xù)檢測多邊形列表中所有元素構成的多邊形是否存在連接起始點與目標點的光路,若不存在則繼續(xù)采樣,直至照亮的區(qū)域能夠連通起始點與目標點。由圖2b可以看出,構成起始點與目標點之間的光路,通過該方式采樣得到的節(jié)點只會出現(xiàn)在未照亮區(qū)域,同時節(jié)點數(shù)量大幅減少,繼而提升了路徑平滑度、提高了程序運行效率。

由圖2c可以看出,在多邊形列表中光照范圍的重合部分繼續(xù)進行節(jié)點采樣,得到Ps1和P1g。將Ps、Pg以及所有采樣點作為節(jié)點,將所有光照范圍有交集的節(jié)點兩兩連接,作為無向網(wǎng)絡路徑圖的邊。由圖2d可以看出,將兩個節(jié)點之間的歐氏距離作為對應邊的代價值,形成的無向網(wǎng)絡路徑。構建無向網(wǎng)絡路徑圖的流程如圖3所示。

圖3 構建無向網(wǎng)絡路徑圖的流程

2.3 選擇無向圖中最優(yōu)路徑

在上一步生成的無向網(wǎng)絡路徑圖中,使用Dijkstra算法獲取無向有權圖中Ps至Pg的最短路徑。最后優(yōu)化路徑,遍歷路徑中的所有節(jié)點,嘗試依次刪除每個節(jié)點,并將所刪除節(jié)點前后的兩個節(jié)點相連,生成無向有權圖的邊。若新添加的邊未與障礙物發(fā)生碰撞,則更新路徑,獲取相對更短路徑。

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

3.1 單個狹窄通道地圖仿真

為了測試優(yōu)化PRM 算法的性能,本文將其與傳統(tǒng)PRM 算法進行測試對比。電腦處理器為Intel Core i5-7200U,主頻2.50 GHz,內(nèi)存8 GB。

單個狹窄通道地圖大小為20 m×20 m,起點位置設置為(4,10),終點位置設置為(2,1)。將傳統(tǒng)PRM算法依次取300,500和1 000個采樣節(jié)點,在每組節(jié)點數(shù)進行50次實驗的基礎上,與優(yōu)化PRM 算法進行對比,并取優(yōu)化PRM 算法的射線數(shù)量和射線步長分別為36和0.5 m。實驗生成無向網(wǎng)絡路徑圖和最終路徑,單個狹窄通道地圖仿真實驗對比如圖4所示。

圖4 單個狹窄通道地圖仿真實驗對比

單個狹窄通道地圖仿真50次實驗數(shù)據(jù)對比如表1所示,實驗結果表明,采樣節(jié)點數(shù)分別為300,500和1 000個的傳統(tǒng)PRM 算法與優(yōu)化PRM 算法,能夠規(guī)劃出無碰撞路徑的概率分別為68%,72%,96%和100%。相較于這3種條件下的傳統(tǒng)PRM 算法,優(yōu)化PRM 算法的規(guī)劃平均時間分別降低58.97%,72.49%和88.38%;平均路徑平滑度使用路徑轉角之和量化,分別提升60.35%,68.35%和77.11%;平均路徑長度基本保持不變。

表1 單個狹窄通道地圖仿真50次實驗數(shù)據(jù)對比

3.2 多個狹窄通道地圖仿真

多個狹窄通道地圖大小為20 m×20 m,起點位置設置為(1,1),終點位置設置為(19,19)。將傳統(tǒng)PRM算法依次取500,1 000和2 000個采樣節(jié)點,在每組節(jié)點數(shù)進行100次實驗的基礎上,與優(yōu)化PRM 算法進行對比,并取優(yōu)化PRM 算法的射線數(shù)量和射線步長分別為36和0.5 m。實驗生成無向網(wǎng)絡路徑圖和最終路徑,單個狹窄通道地圖仿真實驗對比如圖5所示。

圖5 多個狹窄通道地圖仿真實驗對比

多個狹窄通道地圖仿真100次實驗數(shù)據(jù)對比如表2所示,實驗結果表明,采樣節(jié)點數(shù)分別為500,1 000和2 000個的傳統(tǒng)PRM 算法與優(yōu)化PRM 算法能夠規(guī)劃出無碰撞路徑的概率分別為14%,52%,96%和100%。與這3種條件下的傳統(tǒng)PRM 算法相比,優(yōu)化PRM 算法的規(guī)劃平均時間分別降低50.49%,66.57%和83.14%;平均路徑平滑度分別提升42.81%,54.57%和64.62%;平均路徑長度基本保持不變。

表2 多個狹窄通道地圖仿真100次實驗數(shù)據(jù)對比

4 結束語

本文基于傳統(tǒng)PRM 算法,加入光照采樣節(jié)點的方法,提出了一種優(yōu)化PRM 算法,可應用于狹窄直線通道地圖。該算法不會在光照范圍內(nèi)采樣,不僅減少了采樣節(jié)點數(shù)量,而且提高了采樣節(jié)點利用率,在路徑長度基本保持不變的情況下,縮短了路徑規(guī)劃時間。與目前廣泛采用的改變隨機節(jié)點生成函數(shù)的方法相比,優(yōu)化后的PRM 算法路徑節(jié)點大幅減少,因而提高了路徑平滑度,同時具有完備性,極大地提高了運行效率。仿真實驗已經(jīng)證明了優(yōu)化PRM 算法的可行性。但是,該算法的適用場合僅限于狹窄直線通道地圖,而在普通地圖和環(huán)形狹窄通道地圖中的仿真實驗效果有待提升,因此建議后續(xù)研究將本文算法與其他算法相結合,提出適用于多種地圖的新算法。