基于目標導向采樣的機器人改進概率路圖法研究

陳志勇 吳精華

(福州大學機械工程及自動化學院, 福州 350108)

0 引言

為提高工作效率、保證任務質量,機器人路徑規劃問題研究一直深受人們的重視。目前,路徑規劃方法主要包括Dijkstra 算法[1-3]、A*算法[4-6]、人工勢場法[7-10]、快速擴展隨機樹(Rapidly exploring random tree, RRT)[11-13]、概率路圖法(Probabilistic roadmap method, PRM)[14-16]等。

Dijkstra算法[17]雖然在地圖中可以找到一條全局最優路徑,但會擴展出大量冗余的節點,導致搜索時間大量增加。而A*算法、人工勢場法等算法雖在二維路徑規劃中有著不錯的效果,但隨著維度增加、空間復雜度提升,算法整體計算量驟增,并可能陷入局部最優值。RRT算法是一種基于隨機采樣且可用于高維空間下的路徑搜索法,但RRT算法存在偏離最優解、局部擴展緩慢等問題,如文獻[18]提出了基于雙向搜索樹的RRT-Connect算法,該算法雖可有效提高搜索效率,但在復雜環境中仍然存在隨機性強、難以找到路徑等問題。與RRT算法類似,PRM算法亦是一種基于隨機采樣的路徑規劃算法,該方法同樣有助于機器人在高維空間中尋找到一條可行路徑,且具有計算量小、實時性好等優點,已被廣泛應用于各類機器人的路徑規劃。不過值得注意的是,傳統PRM算法采用的是完全隨機的采樣策略,為保證算法搜索效率,當采樣點數量一定時,若其中的多數隨機采樣點剛好落在障礙物空間中,則會使自由空間中的采樣點數變少,地圖連通性降低,可能會導致搜索路徑失敗;尤其當遇到狹窄通道或者大量障礙物時,傳統PRM算法將更難在工作空間中規劃出一條可行路徑。為了解決這一問題,最直接的方法就是重新采樣并增加更多的采樣點,使之在自由空間的采樣點數量增多;但該方法難以確定有效增加在狹窄區域的采樣點,反而會使路徑規劃整體時間呈指數性增長,并大大降低其實時性。為此,近年來許多學者針對PRM算法又提出了各種改進方法。如文獻[19]提出了高斯采樣方法,高斯采樣是基于障礙物的密度使得狹窄區域中的采樣點有效增加,但高斯采樣法會在障礙物附近增加許多不必要的節點,造成了節點浪費。文獻[20]提出了一種橋梁測試方法,該方法從障礙物中提取兩個采樣點,如果兩個采樣點的中點位于自由空間中,則將該點加入到自由點集;該方法雖可有效增加狹窄區域的采樣點密度,但會有較多的路標分布在障礙物凹陷處和拐角處,導致路徑規劃效率變低。

為實現機器人在具有狹窄通道工作環境下的有效路徑規劃,本文提出一種融合全局目標導向采樣、局部節點增強的Improved PRM算法。該算法采用基于全局目標導向采樣及隨機采樣的混合采樣方式來提高全局采樣點落在狹窄通道內的概率,以實現啟發式地圖增強;利用節點權重思想提取位于狹窄區域的節點,并在提取的節點周圍通過高斯分布來擴展其附近的節點,以增強地圖連通性,提高算法路徑規劃成功率;通過冗余節點剔除策略,有效減少路徑節點數及路徑代價,實現算法的路徑優化。通過仿真對比實驗,對Improved PRM算法在機器人路徑規劃上的可行性及有效性進行驗證。

1 PRM基本原理及六自由度機器人模型

1.1 PRM基本原理

作為解決高維空間下機器人路徑規劃問題的主要方法之一,基于全局隨機采樣及圖搜索的PRM算法的實現主要分為離線學習階段和查詢階段。

查詢階段是指在給定的起始位置和目標位置的條件下,根據離線學習階段得到的無向路徑圖上,采用A*算法在起始點和目標點之間搜索到一條可行的無碰撞路線。

1.2 六自由度機器人運動學模型

本文以一種六自由度機器人作為 Improved PRM算法的應用對象。該機器人具有6個旋轉自由度θj(j=1,2,…,6),且θj表示機械臂連桿j相對于前一個連桿j-1的關節角。據文獻[22],機器人末端操作器連體坐標系相對于基座連體坐標系的齊次變換矩陣為

(1)

其中

αj-1——連桿j-1與連桿j間的連桿扭角

aj-1——連桿j-1的長度

dj——連桿j-1與連桿j公法線間的距離

n、o、a——機器人末端操作器的姿態

p——機器人末端操作器的空間位置

利用式(1)對機器人進行逆運動學求解,可容易將其末端操作器位姿映射到系統各關節轉角θj(j=1,2,…,6)上(即反解出機器人各關節轉角)。顯然,機器人末端操作器的路徑規劃問題可以轉換成關節空間層面上的路徑規劃問題。

2 融合全局目標導向采樣、局部節點增強的Improved PRM算法

受采樣隨機性的限制,傳統PRM算法在狹窄通道的通過性往往較差。考慮到機器人在特殊情況下又需要經過指定的狹窄通道進行操作,若直接采用傳統PRM算法對此類機器人進行規劃,則算法的成功率將難以得到保證。為此,本文將在傳統PRM算法框架的基礎上,著重對其學習階段的無向圖構建方式進行改進,提出一種基于全局目標導向采樣、局部節點增強、冗余節點剔除的機器人Improved PRM算法,并將該算法應用于平面柵格地圖場景及六自由度機器人路徑規劃。

2.1 基于全局目標導向采樣的啟發式地圖增強方法

為構建一幅無向圖,傳統PRM算法采用全局隨機采樣,由于此種采樣方式過于隨機、單一,在機器人工作空間的狹窄通道中很難獲取足夠的采樣點,因此所得的無向圖連通性較差,難以保證后續路徑搜索成功率。為此,本文引入了一種目標區域導向采樣規則,并將其與隨機采樣方式相結合,作為Improved PRM算法的全局采樣方式。首先,以機器人在給定工作空間中的始末位置分別作為算法采樣的起始節點及目標節點;接著,從起始節點往目標節點方向確定采樣目標區域,并在該區域進行合理的導向采樣預處理,以獲得一定數量的全局導向采樣點,提高采樣點落在狹窄通道中的概率;最后,再在工作空間中進行慣常的隨機采樣,來獲得剩下的全局采樣點。

qguid=qstart+D0Frf

(2)

其中

Frf=(r1f1,r2f2,…,rnfn)T

式中D0——從起始節點往目標節點方向輻射采樣的算法調控系數

Frf——導向性采樣函數列向量

ri——第i(i=1,2,…,n)維方向上變量的采樣步長

fi——第i(i=1,2,…,n)維方向上變量的采樣函數

通過設計不同形式的采樣步長ri及采樣函數fi來獲得各種滿足特定采樣規則的導向性采樣節點。在獲得導向采樣點后,對采樣點進行是否處于自由空間的判定。若該采樣點剛好落在障礙物空間中,則舍去該節點;反之,則將其加入到自由節點集R中。

剩下的全局采樣點,采用隨機采樣方式,即

qrand=qmin+(qmax-qmin)rand[0,1]

(3)

其中

式中qrand——通過隨機采樣得到的n維采樣節點

qmin——由節點各維變量在工作空間中所能取到的最小值組成的列向量

qmax——由節點各維變量在工作空間中所能取到的最大值組成的列向量

rand[0,1]——在[0,1]內選取的隨機數

類似地,在獲得全局隨機采樣點后,對采樣點進行是否處于自由空間的判定。若該采樣點剛好落在障礙物空間中,則舍去該節點;反之,則將其繼續加入到自由節點集R中。

2.1.1方法在平面柵格地圖場景中的實現

文獻[23]提出了一種目標導向采樣方法,該方法可實現無人機在特定起始點和特定目標點連線方向上的目標區域外擴輻射圓導向采樣;不過由于受采樣規則限制,該算法采樣時引導方向單一,應用靈活性受到限制。為此,擬通過式(2)中調控系數D0來對原采樣規則進行修正,使修正后算法的導向性更加靈活、實用。

為實現平面柵格地圖場景下目標區域的導向采樣,定義地圖左上角位置為原點,地圖水平方向為x軸方向(水平朝右為正向),垂直方向為y軸方向(垂直朝下為正向),起始節點、目標節點及導向采樣節點分別為qstart=(xstart,ystart)T、qgoal=(xgoal,ygoal)T及qguid=(xguid,yguid)T。于是,始末兩節點連線方向與x軸正向的偏角α可表示為

(4)

其中,α∈(-π/2,π/2),以順時針轉向為正,且當xstart=xgoal時,α取π/2。

為實現在柵格地圖下的均勻外擴輻射圓采樣,設計采樣函數列向量Frf為

(5)

其中r1=r2=R0m=-i0+1,-i0+2,…,i0-1

式中R0——輻射圓的外擴步長

m——輻射圓心角控制系數(i0≤k,且i0選取越大,圓心角就越大;反之亦然)

i0——m取值范圍的調節參數

k——輻射圓上的采樣點數(k取值越大,則在起始點和目標點連線附近的采樣點越密集,在自由空間中的采樣占比越大;反之亦然),取k>0

n0——外擴的輻射同心圓序號,n0=1,2,…,N

其中輻射圓總數N計算式為

(6)

式中 Int[ ]——對計算結果的取整函數

為使采樣算法可以自動根據始末節點在平面柵格地圖中的具體位置進行方向調控,選取調控系數D0為

(7)

其中A=(xgoal-xstart,ygoal-ystart)TB=(1,0)T

式中A——從起始節點qstart到目標節點qgoal的方向矢量

B——x軸基矢量

在隨機采樣方面,平面柵格地圖的隨機采樣點可表示為qrand=(xrand,yrand)T,取式(3)中的qmin=(0,0)T,qmax=(V1,V2)T。其中,V1、V2分別為柵格地圖水平及垂直方向上的維度。

2.1.2方法在六自由度機器人中的實現

(8)

式中rj——機械臂第j個關節的采樣步長

uj——用于第j個關節始末角度等分的正整數

rand[0,uj]——隨機在0～uj中選取的整數,主要用于控制第j個關節在始末范圍內的定向采樣位置

2.2 基于高斯分布的局部節點增強策略

傳統PRM算法利用局部規劃器對自由節點集R中各節點與其鄰域半徑ρ0范圍內的其他節點作碰撞檢測,以獲得各節點的鄰域點集,進而得到無向圖。前述基于全局目標導向采樣的地圖增強方法雖可在工作空間狹窄通道內增加一定的采樣點,但數量仍有限且各節點間的連通性依然較差。為進一步提高算法連通性,有必要利用狹窄通道內已有節點來擴展出更多的狹窄通道局部新節點。為此,本文結合節點權重思想[24]對位于狹窄區域的節點進行提取,并給出一種基于高斯分布的局部節點增強策略。

設全局采樣得到的自由節點集R包含有N1個節點,qγ表示第γ(γ=1,2,…,N1)個節點。首先,利用局部規劃器在自由節點集R中所有采樣點鄰域內作碰撞檢測;其次,根據碰撞檢測結果,計算qγ位于狹窄區域的權重w(qγ)為

(9)

其中

式中P(qγ)——節點qγ的局部規劃失敗率

s(qγ)——局部規劃器嘗試連接節點qγ到其他節點的總次數

設狹窄區域權重判定閾值為w0,當w(qγ)>w0時,則可認為節點qγ位于狹窄區域內;反之亦然。最后,將所有位于狹窄區域的點放入狹窄區域節點集W中。

若狹窄區域節點集W共有N2個節點,則在點集W的各節點qζ(ζ=1,2,…,N2)周圍通過高斯分布規律來擴展一定數量的新節點,即

qnew=qζ+gGaussian[0,1]

(10)

式中 Gaussian[0,1]——標準正態分布的隨機數

g——n維高斯分布采樣的范圍幅值列向量

以此來增加狹窄通道中的節點數,增強狹窄通道的連通性。

對于新擴展出來的局部節點,去除位于障礙物空間的節點,保留位于自由空間中的節點,并將這些節點再次加入自由節點集R中。利用局部規劃器再次對更新后的自由節點集R進行碰撞檢測,得到各采樣點的鄰域點集Eγ,并最終得到更新后的無向圖。

2.3 基于冗余節點剔除的路徑優化策略

鑒于前述無向圖中的多數節點仍是通過隨機方式產生的,所以改進PRM算法若直接利用A*算法進行路徑搜索,得到的初始路徑將存在有較多的冗余節點。為了減少路徑節點、縮短路徑長度,Improved PRM算法通過剔除冗余節點、提取關鍵節點來優化A*算法得到的初始路徑。

以圖1為例進行簡單說明。設圖中黑色表示障礙物,A*算法得到的初始路徑點集為Q={qstart,q1,q2,q3,q4,q5,q6,qgoal}(圖中的黑色實線路徑)。首先,以起始節點qstart為基礎建立關鍵節點集Q′={qstart};之后,將關鍵節點qstart與初始點集Q中的后續節點進行連接,如果兩個路徑節點連線沒有與障礙物干涉,則繼續連接下一個節點;如果碰撞到障礙物,則把前一個節點放入到關鍵節點集Q′中,并把該節點作為新的關鍵節點與Q中剩余節點逐個進行嘗試連接并進行相應的碰撞檢測;重復以上步驟,直至遍歷到目標節點qgoal;最后將得到的關鍵節點集Q′={qstart,q2,q5,qgoal}中的各節點依次連接起來,即可得到最終的優化路徑(圖中的虛線路徑)。

圖1 冗余節點剔除示意圖Fig.1 Redundant node removal diagram

2.4 路徑規劃實現步驟

Improved PRM算法的路徑規劃步驟如下:

(1)在工作空間中,設qstart為起始點,qgoal為目標點,算法鄰域半徑為ρ0。

(2)根據目標導向采樣規則qguid=qstart+D0Frf,在工作空間中獲得一定數量的目標區域采樣點,并將其中位于自由空間的采樣點加入到自由節點集R中。

(3)根據隨機采樣規則qrand=qmin+(qmax-qmin)rand[0,1],在工作空間中進行一定數量的隨機采樣,并將位于自由空間的采樣點也加入到自由節點集R中。

(4)利用局部規劃器對自由節點集R中每個采樣點qγ和其鄰域半徑ρ0范圍內的其他采樣點的連線與障礙物作碰撞檢測;若連線與障礙物干涉,則舍棄qγ與其鄰域ρ0范圍內采樣點的連線;反之,則保留;遍歷完qγ鄰域范圍內的所有采樣點后,得到采樣點qγ的鄰域點集Eγ。

(6)計算自由節點集R中每個采樣點qγ位于狹窄區域的權重w(qγ),若w(qγ)>w0,則將該點加入到狹窄區域節點集W;反之亦然。

(7)不斷重復步驟(6),待遍歷完R中所有節點后,得到最終更新后的狹窄區域節點集W。

(8)利用節點增強策略qnew=qζ+gGaussian[0,1]對點集W中的節點qζ進行周圍新節點擴展,并將位于自由空間的新節點也加入到自由節點集R中。

(9)不斷重復步驟(8),待遍歷完狹窄區域節點集W中每個節點后,得到更新后的自由節點集R。

(12)使用冗余節點刪除策略,刪除初始路徑中冗余的節點,得到最終路徑。

3 仿真實驗

首先對傳統PRM算法、Gaussian PRM算法、RRT-Connect算法、Improved PRM算法在平面柵格地圖場景中進行仿真實驗,通過不同算法在路徑規劃成功率、時間、路徑質量的分析來檢驗Improved PRM算法的有效性。其次,在ROS仿真平臺上建立六自由度機器人仿真模型及具有狹窄通道的仿真環境,并將Improved PRM算法應用于機器人路徑規劃仿真實驗。通過Improved PRM算法與傳統PRM算法的仿真結果對比,證實所提算法的可行性。

3.1 平面柵格地圖仿真實驗

如圖2所示,給定的平面柵格地圖尺寸為500 mm×500 mm,圖中符號“×”表示機器人起始點qstart=(1 mm,1 mm)T,符號“△”代表目標點qgoal=(450 mm,400 mm)T,狹窄通道用橢圓形框框出。實驗中,RRT-Connect算法的采樣步長設為50 mm,傳統PRM、Gaussian PRM、Improved PRM算法在地圖中的總采樣點數均設為300個,鄰域半徑ρ0設為70 mm。同時,Gaussian PRM 算法中的高斯分布采樣的范圍幅值列向量設為g=(25.0 mm,25.0 mm)T;Improved PRM算法的其他參數設為R0=50 mm,g=(25.0 mm,25.0 mm)T,w0=0.015。在相同環境下,每種算法各進行150次仿真實驗,仿真結果見表1。圖2分別給出了傳統PRM、Gaussian PRM、RRT-Connect、Improved PRM算法仿真中某次成功規劃的路徑圖;需要指出的是,圖2中綠色點均表示各算法通過隨機采樣得到的采樣點,圖2d中藍色點表示全局目標導向采樣得到輻射圓采樣點,紅色點為狹窄通道內通過局部高斯擴展出的采樣點。在前述Improved PRM算法仿真中,利用狹窄區域節點集W中節點擴展出來的滿足高斯分布的部分增強節點參數見表2。

表1 4種算法仿真結果數據對比Tab.1 Comparison of simulation results of four algorithms

表2 基于高斯分布擴展的部分節點參數Tab.2 Parameters of some nodes extended by Gaussian distribution mm

圖2 4種算法得到的路徑圖Fig.2 Path graphs obtained by four algorithms

為了判斷算法規劃路徑的性能,定義綜合評價指數E1為

(11)

式中S——算法路徑規劃成功率

Lmin——起始點到目標點連線的始末長度

Lnow——路徑代價

由表1可知,RRT-Connect算法的路徑規劃成功率最低,只有29.6%,這說明RRT-Connect算法在這種存在狹窄通道的工作環境中較難規劃出一條可行的路徑。Gaussian PRM算法的成功率較傳統PRM算法高24.4個百分點,說明Gaussian PRM算法可在一定程度上提高路徑規劃的成功率。由于采樣密度不一樣(即參數k選取),Improved PRM算法的成功率最高值為100%,最低值為89.3%,均高于其他算法,這說明Improved PRM算法可以大幅提高路徑規劃成功率。

在算法平均規劃時間上,4種算法的平均規劃用時差距不明顯,傳統PRM算法用時最多,RRT-Connect算法用時最少。所以在相同的環境下,Improved PRM算法在不多耗費時間的基礎上,可以有效地提高機器人路徑規劃的成功率,保證算法路徑規劃的穩定性。

從綜合評價指數來看,Improved PRM的綜合評價指數均比其他算法高。Improved PRM綜合評價指數最高值36.465比其他算法最高值25.754高41.59%,則說明Improved PRM較其他算法能夠更有效地在具有狹窄通道的復雜環境中找到一條無碰撞的較優路徑。從表1可知,當全局目標區域導向采樣占比為32%～42%時,路徑規劃的成功率較高,綜合評價指數也較高。

傳統PRM、Gaussian PRM、RRT-Connect、Improved PRM算法均涉及隨機采樣,尋找的路徑隨機性較強,所以規劃出來的路徑會存在較多的冗余節點,導致機器人的運行時間會隨之增加,因此優化路徑質量顯然很有必要。本文路徑質量評價指數E2計算式為

(12)

式中Nsum——路徑節點數

表3為4種算法得到的路徑質量對比情況。由表3可以看出,RRT-Connect的平均路徑代價最大,路徑節點數最多,路徑質量評價指數最低,僅有25.0%。Gaussian PRM的路徑質量評價指數比傳統PRM的路徑質量評價指數高0.2個百分點,Improved PRM的路徑質量評價指數最高值83.6%比Gaussian PRM高38.5個百分點,這說明Improved PRM找到的路徑質量更優。

表3 路徑質量對比Tab.3 Path quality comparison

上述分析可知,本文所提Improved PRM算法可以大幅地提高路徑規劃的成功率,有效減少路徑節點數,降低路徑代價,比傳統的PRM、Gaussian PRM及RRT-Connect算法能夠更容易地找到一條無碰撞的可行路徑。

3.2 基于ROS平臺的六自由度機器人仿真實驗

為進一步驗證Improved PRM算法的可行性,本文在ROS仿真平臺中選用MOTOMAN-GP7型六自由度機器人作為仿真對象,該機器人的D-H參數見表4,同時構造具有狹窄通道的復雜仿真環境,見圖3a;并繼續利用Improved PRM及傳統PRM算法對機器人做路徑規劃仿真對比實驗。在圖3a中,參考坐標系建立在機器人基座中心處,各障礙物形狀、尺寸及位置信息見表5。

表4 MOTOMAN-GP7的D-H參數Tab.4 D-H parameters of MOTOMAN-GP7

表5 障礙物信息Tab.5 Obstacles’ information

圖3 傳統PRM算法仿真結果Fig.3 Simulation results of traditional PRM algorithm

仿真時,要求機器人末端操作器從起始點P1(位置坐標為(0.17 m,-0.45 m,0.43 m)、RPY角為(0.13 rad,-3.13 rad,2.02 rad))盡可能經由障礙物2、4、5所形成的狹窄通道抵達目標點P2(位置坐標為(0.65 m,0.29 m,0.35 m)、RPY角為(-3.08 rad,-0.06 rad,0.41 rad))。為此,利用ROS平臺的KDL逆運動學求解器將機器人末端始末位姿轉換為關節空間下各關節的始末位置,進而得到算法的起始節點qstart=(-1.27 rad,-0.15 rad, -0.69 rad,0.28 rad,0.57 rad,-0.37 rad)T和目標節點qgoal=(0.41 rad,0.43 rad,-0.22 rad,0 rad,0.72 rad,-0.06 rad)T。

仿真時,傳統PRM、Improved PRM算法的鄰域半徑ρ0均設為2 rad,此外,Improved PRM算法進行全局導向性采樣概率設為37%,uj=6(j=1,2,…,6),w0=0.002,g=(0.8 rad,0.8 rad,0.8 rad,0.8 rad,0.8 rad,0.8 rad)T。

利用傳統PRM算法及Improved PRM算法分別對ROS平臺中的機器人進行50次成功的路徑規劃仿真實驗,并記錄下2種算法每次路徑規劃所得到的路徑代價、規劃時間及狹窄通道的通過頻次。此外,為保證機器人能夠平穩地從初始位置運動到目標位置,采用三次樣條插值法來生成機器人各個關節的運動曲線[25]。圖3為利用傳統PRM進行某次路徑規劃得到的機器人運動過程圖及各關節轉角變化曲線,圖4為利用Improved PRM進行某次路徑規劃得到的機器人運動路徑圖及各關節轉角變化曲線。

圖4 Improved PRM算法仿真結果Fig.4 Simulation results of Improved PRM algorithm

利用曼哈頓距離定義機器人總路徑代價為

(13)

式中Nz——每次路徑規劃得到的總節點數計算兩種算法50次仿真實驗的平均路徑代價、平均耗時及通過狹窄通道次數,見表6。

表6 兩種算法仿真結果對比Tab.6 Comparison of simulation results of two algorithms

由表6可知,在50次仿真實驗中,兩種算法在平均路徑規劃時間上并無太大差別,但ImprovedPRM算法得到的平均路徑代價比傳統PRM算法降低42.7%,成功通過狹窄通道的概率也比傳統PRM提高68個百分點。顯然,融合目標導向采樣及局部節點增強的Improved PRM比傳統PRM算法更適于機器人在具有狹窄通道的復雜環境中進行有效地路徑規劃。

4 結束語

針對具有狹窄通道的工作環境,本文提出了一種融合全局目標導向、局部節點增強的機器人Improved PRM算法。該算法中基于全局目標導向采樣及隨機采樣的混合采樣方式,可提高全局采樣點落在狹窄通道內自由空間的概率,實現啟發式地圖增強;基于高斯分布的局部節點增強策略,可有效增強算法在狹窄空間下連通性,提高路徑規劃的成功率;冗余節點刪除策略又可在一定程度上減少路徑節點及路徑代價,對算法規劃的路徑起到優化作用。仿真結果表明,文中所提算法在平面柵格場景下的路徑規劃成功率可達89.3%以上,且綜合評價指數及路徑質量評價指數亦高于其他算法;在六自由度機器人路徑規劃仿真場景下,Improved PRM算法得到的平均路徑代價比傳統PRM算法降低42.7%,成功通過狹窄通道概率也比傳統PRM提高68個百分點。