韓 超楊 杰

(青島大學(xué)機(jī)電工程學(xué)院,山東 青島 266071)

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

1 PRM 算法描述

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

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

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

1.2 查詢階段

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

2 PRM 算法優(yōu)化

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

2.1 光照區(qū)域獲取

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

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

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

2.2 節(jié)點(diǎn)采樣

光照區(qū)域?yàn)榛疑糠?光照節(jié)點(diǎn)采樣過程如圖2所示。由圖2a可以看出,將光照區(qū)域分別存儲為多邊形,并將其添加到多邊形列表中。

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

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

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

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

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

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

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

3.1 單個(gè)狹窄通道地圖仿真

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

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

圖4 單個(gè)狹窄通道地圖仿真實(shí)驗(yàn)對比

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

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

3.2 多個(gè)狹窄通道地圖仿真

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

圖5 多個(gè)狹窄通道地圖仿真實(shí)驗(yàn)對比

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

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

4 結(jié)束語

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