張 毅,施明瑞
(重慶郵電大學 國家信息無障礙工程研發(fā)中心,重慶 400065)
移動機器人規(guī)劃路徑能力的大小,決定了機器人可勝任工作難度的高低。
在構建好的環(huán)境模型中,規(guī)劃一條從初始點到目標點的無碰撞最優(yōu)路徑,是路徑規(guī)劃的主要內容。在構建環(huán)境模型時,柵格法應用較多,該方法具有直觀簡潔、分辨率可變、容易創(chuàng)建和存儲等優(yōu)點,適用于室內環(huán)境路徑規(guī)劃地圖模型的建立,魯棒性強[1]。圖搜索算法是路徑規(guī)劃中的一類算法,它被廣泛應用于柵格地圖中的路徑規(guī)劃問題。A*算法是圖搜索算法中的經(jīng)典算法,它是一種啟發(fā)式搜索方法。啟發(fā)式搜索會評估狀態(tài)空間中的每個搜索位置,找出下一步要搜索的最好位置G,再從G進行類似搜索直到確定目標位置。該搜索方法能節(jié)省大量搜索空間,提高搜索效率[2]。在A*算法的基礎上,研究者們先后提出了多種改進算法,例如D*,LPA*與D*lite等算法。
D*lite算法是文獻[3]提出的。該算法采用反向搜索,從目標點向當前點擴展,在進行重規(guī)劃時極大地提高了效率。文獻[4]通過在啟發(fā)估價值中加入與障礙物相關的值,從而避免規(guī)劃出靠近障礙物尖角或穿越兩相鄰障礙物的不安全或不可達路徑。文獻[5]通過融合D*lite算法和RRT*算法提高了收斂速率。文獻[6]采用模型預測控制MPC,優(yōu)化了D*lite算法在動態(tài)環(huán)境和特殊地形中的路徑規(guī)劃能力。文獻[7]通過引入備選路徑,提高了D*lite算法中路徑重規(guī)劃的速度。文獻[8]通過在D*lite算法中引入搜索樹的思想,在需要重規(guī)劃時立即砍斷搜索樹,從而提高路徑重規(guī)劃的速度。
學者們對D*lite算法規(guī)劃出的路徑的安全性、平滑度和算法重規(guī)劃策略進行了諸多研究,但是在路徑搜索中仍然存在問題。
D*lite算法在進行路徑規(guī)劃時,會給網(wǎng)格設置優(yōu)先級,每次循環(huán)中選取優(yōu)先級最高的網(wǎng)格進行擴展,啟發(fā)信息是優(yōu)先級設置中的重要參考,但是啟發(fā)信息只是估計代價,無法將障礙物對全局移動代價的影響完全包含。當障礙物將較大的搜索空間分隔成多個較小的自由區(qū)域時,優(yōu)先級高的網(wǎng)格指引的搜索方向可能最終會被障礙物阻隔而無法繼續(xù)擴展,導致正確的搜索方向被隱藏起來,往往需要經(jīng)過多次嘗試才能找到,在錯誤的搜索方向上擴展的網(wǎng)格是無效網(wǎng)格,增加了計算次數(shù)而對規(guī)劃路徑?jīng)]有幫助,影響了路徑規(guī)劃的效率。因此本文提出一種基于單元分解的改進D*lite路徑規(guī)劃算法,將室內環(huán)境分解為若干相互連通的單元,這些單元的相互連通區(qū)域,即連接兩個相鄰單元的可通行的自由區(qū)域,是移動機器人導航時必須經(jīng)過的網(wǎng)格,可以在這些網(wǎng)格中選取合適網(wǎng)格作為核心網(wǎng)格,用來引導路徑規(guī)劃算法的搜索方向,這樣能夠快速找到繞過障礙物的正確路徑,從而減少無效網(wǎng)格的擴展,提高規(guī)劃速度。
本文在原始D*lite算法的基礎上進行優(yōu)化,在原有的搜索模型中加入核心網(wǎng)格進行改進,核心網(wǎng)格會引導搜索算法在正確的方向上進行搜索和擴展。
改進流程如下。
1)在原有Boustrophedon[9]單元分解法的基礎上設計基于新的分解規(guī)則,分割環(huán)境的柵格地圖。
2)設計雙向圖搜索算法,找出單元順序。
3)設計核心網(wǎng)格設置方法,得到核心網(wǎng)格組成搜索鏈表。
4)以搜索鏈表引導搜索算法完成路徑規(guī)劃。
改進流程如圖1。
圖1 改進流程Fig.1 Improved processes
利用Boustrophedon單元分解法以及新增的分解規(guī)則,可以將移動機器人所在的室內空間分解成若干單元,這些單元是內部沒有障礙物的有界區(qū)域。
Boustrophedon單元分解法是用一條垂直線從左至右掃描柵格地圖。當掃描線掃入障礙空間時,掃描線的連通性增加,當掃描線掃出障礙空間時,掃描線的連通性降低。當掃描線的連通性增加時,一個舊單元結束,生成兩個新單元。當掃描線的連通性降低時,兩個舊單元結束,生成一個新單元。
當障礙空間的外形產生變化時,此時掃描線的連通性不變,但單元內仍然存在阻擋網(wǎng)格擴展的障礙,算法難以快速找到繞過障礙的正確路徑。針對此問題設計了新的分解規(guī)則,當掃描線的連通性不變但某段連通線長度發(fā)生了變化,若變化的大小超過當前單元最大寬度的一半,則舊單元結束,新單元生成如圖2。
圖2 連通線長度變化較大時產生新單元Fig.2 Generating new unit when the length of the slice changes greatly
其余情況只增加單元大小,而不產生或結束單元。掃描結束后,環(huán)境地圖被分割成若干獨立的有界單元,每個單元內部都沒有障礙物,如圖3。
圖3 地圖分割成若干單元Fig.3 Map is segmented into several units
可以把這些單元看作節(jié)點,單元之間如果有直接的連通區(qū)域則可以看作相應節(jié)點間有邊連接,這些邊和節(jié)點可以組成圖。需要將兩單元間的移動代價作為代價值賦給相應的邊,在二維柵格地圖中曼哈頓距離的計算簡單,且能夠較準確地比較兩段距離長短,所以可以將單元補全為長方形,計算邊的兩端單元中心之間的曼哈頓距離,此距離作為邊的代價值。若兩單元之間沒有直接的自由連通區(qū)域,則兩單元節(jié)點之間沒有邊連接,兩單元之間的代價值設置為無窮大。計算完成后可得到單元節(jié)點圖如圖4。
圖4 單元節(jié)點圖Fig.4 Node graph of units
把節(jié)點圖中的代價值用矩陣表示可以得到鄰接矩陣edge,其中,edge[i][j]代表節(jié)點i與節(jié)點j之間的直接代價值。
設定起始網(wǎng)格所在節(jié)點為起始節(jié)點,目標網(wǎng)格所在節(jié)點為目標節(jié)點,需要找到起始節(jié)點到目標節(jié)點代價總值最短的路徑,此路徑表示了從起始網(wǎng)格到目標網(wǎng)格的最短路徑需要依次經(jīng)過哪些單元。
利用圖搜索方法可以在節(jié)點圖中找到兩個節(jié)點之間的最短路徑。給每個節(jié)點設置兩個值,cost值表示與搜索起始節(jié)點的代價值,pre表示父節(jié)點,每次從中取出cost值最小的節(jié)點,然后更新其余節(jié)點的cost和pre,直至取出搜索目標節(jié)點。此搜索方法每次只選取代價值最小的節(jié)點,所以在找到起始節(jié)點到目標節(jié)點的最短路徑前,會先計算一些無法到達目標節(jié)點但代價值更小的路徑。為了提高算法的速度,可以采用雙向搜索的方法。同時進行兩個搜索,分別以起始節(jié)點和目標節(jié)點作為搜索起始節(jié)點。
節(jié)點圖雙向搜索方法的算法步驟如下。
1)初始化。用集合c保存已經(jīng)確認最短路徑的節(jié)點,把搜索起始節(jié)點s放入c中。用集合w保存還沒有確認最短路徑的節(jié)點,把其余節(jié)點都放在w中。將w中所有節(jié)點j的cost值設為edge[s][j],pre設為s。
2)從集合w中選出cost最小的節(jié)點i,由于cost非負,所以此cost為s到i的最小代價值,將i移出w,移入c。
3)對于w中的每個節(jié)點j,若有costj>costi+edge[i][j],則更新costj為costi+edge[i][j],并把prej設為i。
4)在兩個搜索中分別重復步驟2、步驟3,直到兩個搜索中的集合c出現(xiàn)交集,或者任意一個搜索中集合w中節(jié)點的cost值都為無窮大,此時循環(huán)結束。
5)若兩個搜索中的集合c有交集,則根據(jù)c中節(jié)點的pre值可以得到起始節(jié)點到目標節(jié)點代價總值最短的路徑,即可得到起始網(wǎng)格到目標網(wǎng)格最短路徑經(jīng)過的單元順序,否則就說明起始節(jié)點無法到達目標節(jié)點。
D*lite算法采用反向搜索方法,即在路徑規(guī)劃時,從目標網(wǎng)格開始搜索,向起始網(wǎng)格擴展。在D*lite算法中每個網(wǎng)格s需要維護3個值:g(s)為目標網(wǎng)格到s網(wǎng)格的實際代價;h(s)是啟發(fā)值,為s網(wǎng)格到起始網(wǎng)格的估計代價,取s網(wǎng)格與起始網(wǎng)格橫坐標之差和縱坐標之差中的最大值,計算如(1)式;rhs(s)保存s網(wǎng)格的父網(wǎng)格s′的g值加上父網(wǎng)格到s網(wǎng)格的代價值c(s′,s),rhs(s)的計算如(2)式。由于機器人在柵格地圖中,從所在網(wǎng)格可以向相鄰的8個網(wǎng)格中任意一個直接移動,所以D*lite算法中父網(wǎng)格到8個子網(wǎng)格的代價值都為1,(1)式的計算結果可以作為網(wǎng)格s的啟發(fā)值h(s)。
h(s)=max(abs(xstart-xs),abs(ystart-ys))
(1)
(2)
(2)式中,pred(s)表示s網(wǎng)格的父網(wǎng)格,是從目標網(wǎng)格向起始網(wǎng)格搜索時搜索方向上的父網(wǎng)格。
當網(wǎng)格的g值等于rhs值時,稱為局部一致,反之則為局部不一致,只有局部一致的網(wǎng)格才是擴展完畢可以通行的。D*lite算法的優(yōu)勢在于動態(tài)環(huán)境中可以通過g值和rhs值的關系判斷網(wǎng)格的狀態(tài),從而找出受環(huán)境變化影響的網(wǎng)格。本文暫時只討論初始環(huán)境下的路徑規(guī)劃。
算法使用一個優(yōu)先隊列保存待更新的局部不一致網(wǎng)格,以k值作為優(yōu)先級,k值越小優(yōu)先級越高,k值由k1和k2組成,k(s)=(k1(s),k2(s)),k1和k2的計算方法為
(3)
比較k值的大小時,優(yōu)先比較k1,如果k1值相等再比較k2值。
D*lite算法的核心步驟如下。
1)初始化所有網(wǎng)格,將所有網(wǎng)格的g值和rhs值設置為無窮大,然后將目標網(wǎng)格的rhs值設置為0,根據(jù)(3)式計算其k值后插入優(yōu)先隊列。
2)從優(yōu)先隊列中移出優(yōu)先級最高的網(wǎng)格,設置其g值等于rhs值,使該網(wǎng)格成為局部一致。然后根據(jù)(2)式計算該網(wǎng)格的鄰近網(wǎng)格的rhs值,若這些網(wǎng)格的rhs值與g值不等,則計算它們的k值并將它們插入優(yōu)先隊列。
3)重復第2步直到起始網(wǎng)格成為局部一致。從起始網(wǎng)格開始按照g值最小的規(guī)則搜索四周鄰近網(wǎng)格,直到到達目標網(wǎng)格,即可得到最終路徑。
D*lite算法以k值作為優(yōu)先級來決定從優(yōu)先隊列中取出哪個網(wǎng)格,以求得8個鄰域方向的最優(yōu)解。然而很多優(yōu)先級較高的網(wǎng)格引導的搜索方向并不能進入正確路徑必須經(jīng)過的單元中,通過這些網(wǎng)格無法找到正確路徑,勢必造成算法在計算過程中耗費大量時間做無效工作。若在起始網(wǎng)格到目標網(wǎng)格的路徑必須經(jīng)過的單元中設置核心網(wǎng)格,首先從目標網(wǎng)格向核心網(wǎng)格進行擴展,則在一開始就能找到繞過障礙物的正確路徑。
為了找出核心網(wǎng)格,需要先設置核心區(qū)域,再在核心區(qū)域中設置核心網(wǎng)格。核心網(wǎng)格的設置算法如下。
1)為每一個網(wǎng)格添加一個編號n,n等于網(wǎng)格所在單元的編號。因為D*lite算法采用反向搜索,需要把節(jié)點圖雙向搜索算法得到的單元順序顛倒方向,使之成為從目標單元到起始單元的單元順序。
2)根據(jù)單元順序,從第2個單元開始,找出與前一個單元相鄰的網(wǎng)格作為核心區(qū)域,直到起始單元。設當前單元為u,前一單元為u′,核心區(qū)域設置如(4)式,其中,Ss.n=u.n表示單元u中的網(wǎng)格,succ(s′)表示與網(wǎng)格s′相鄰的網(wǎng)格,(4)式表示在當前單元u中,所有與前一單元u′相鄰的網(wǎng)格共同組成了單元u的核心區(qū)域。以xmax,xmin,ymax,ymin表示核心區(qū)域內的網(wǎng)格中的最大和最小橫縱坐標,則核心區(qū)域中心點可由(5)式得到。
(4)
(5)
3)設置目標網(wǎng)格為第1個核心網(wǎng)格,起始網(wǎng)格為最后一個核心網(wǎng)格。給每一個核心區(qū)域內的網(wǎng)格設置一個kc值,由kc1和kc2組成,kc(s)=(kc1(s),kc2(s)),kc1和kc2的計算如 (6) 式。其中s為當前核心區(qū)域內的網(wǎng)格,cgrid為上一個核心網(wǎng)格,coremid為下一個核心區(qū)域的中心點。每個核心區(qū)域中選取kc值最小的網(wǎng)格作為核心網(wǎng)格,比較kc值大小時,首先比較kc1值,如果kc1相同再比較kc2值。設置核心網(wǎng)格如圖5。
(6)
圖5 設置核心網(wǎng)格Fig.5 Main grids are set
圖5中星標志為目標網(wǎng)格,圓標志為起始網(wǎng)格,由節(jié)點圖雙向搜索算法可計算出目標單元到起始單元的最短路徑為5→2→1→4→9,灰色網(wǎng)格為核心區(qū)域,圓環(huán)標志為核心網(wǎng)格。
將目標網(wǎng)格、起始網(wǎng)格和設置完成的核心網(wǎng)格按照已經(jīng)計算完成的單元順序依次插入搜索鏈表中,用搜索鏈表引導D*lite算法完成多段路徑規(guī)劃。首先設置一個地址P指向搜索鏈表的首個網(wǎng)格。執(zhí)行每段路徑規(guī)劃時,以地址P當前指向的網(wǎng)格作為單次搜索中的目標網(wǎng)格,以P的下一個地址指向的網(wǎng)格作為單次搜索中的起始網(wǎng)格,為了增加搜索效率,限定只有編號n與當前搜索目標網(wǎng)格或者當前搜索起始網(wǎng)格的編號相同的網(wǎng)格才能被遍歷,當本次搜索中的起始網(wǎng)格成為局部一致后,將P向后移一位,繼續(xù)執(zhí)行下一段路徑規(guī)劃。當搜索鏈表的最后一個網(wǎng)格即真正的起始網(wǎng)格成為局部一致時,從起始網(wǎng)格開始循著鄰接網(wǎng)格中g值最小的網(wǎng)格移動,直到到達目標網(wǎng)格,就可完成從起始網(wǎng)格到目標網(wǎng)格的路徑規(guī)劃。
為了驗證算法的有效性,以MATLAB R2016b作為軟件平臺進行仿真,以Intel Core i3-6100 CPU作為硬件平臺,主頻為3.70 GHZ,4 GByte內存。
在相同的硬件平臺和軟件平臺下,對本文算法和DLD*lite算法[10]進行相同柵格地圖下的對比實驗,DLD*lite算法在D*lite算法的基礎上引入了視線算法和距離變換作為改進。實驗采用15×15的柵格地圖進行仿真,得到如圖6的路徑規(guī)劃對比實驗圖。
圖6 對比實驗Fig.6 Comparative experiment
圖6中星形標志為目標網(wǎng)格,圓形標志為起始網(wǎng)格,黑色網(wǎng)格為障礙物。深灰色網(wǎng)格和淺灰色網(wǎng)格為更新函數(shù)遍歷過的網(wǎng)格,計算了rhs值和k值并插入優(yōu)先隊列中。深灰色網(wǎng)格為搜索函數(shù)從優(yōu)先隊列中取出,計算完g值的局部一致網(wǎng)格。
圖6a為DLD*lite算法仿真圖,該算法在啟發(fā)函數(shù)中加入障礙物距離值,用視線算法更新父節(jié)點,使得規(guī)劃出的路徑平滑且安全,但是由于該算法每次只選取k值最小的網(wǎng)格進行擴展,從而擴展了很多無效網(wǎng)格,對這些網(wǎng)格的g值和rhs值的計算耗費了大量時間卻收效甚微,這樣大大降低了搜索效率。圖6b為本文算法的仿真圖,方形標志為算法計算出的核心網(wǎng)格,通過單元分解、雙向圖搜索和設置核心網(wǎng)格得到了搜索鏈表,用搜索鏈表引導分段搜索,最終找到起始網(wǎng)格到目標網(wǎng)格的路徑。由于以核心網(wǎng)格組成的搜索鏈表作為導向,使得搜索算法朝著正確的方向進行擴展,大大降低了遍歷網(wǎng)格的數(shù)量和計算次數(shù),從而提高了路徑規(guī)劃的速度。
表1為路徑規(guī)劃對比實驗中已遍歷網(wǎng)格數(shù)與最終路徑長度的統(tǒng)計。本文算法中更新函數(shù)已遍歷的網(wǎng)格數(shù)比DLD*lite算法中減少了43%,搜索函數(shù)已計算的網(wǎng)格數(shù)減少了70%??梢钥闯霰疚乃惴p少了網(wǎng)格的遍歷次數(shù)和計算次數(shù)。
表1 路徑規(guī)劃對比實驗相關數(shù)據(jù)
為了進一步驗證本文算法提高路徑規(guī)劃效率的有效性,統(tǒng)計本文算法、DLD*lite算法和改進多步長蟻群算法[11]路徑規(guī)劃所用時間并進行對比,為了消除偶然現(xiàn)象,提高實驗結果的精確性,統(tǒng)計了多次對比實驗。改進多步長蟻群算法路徑規(guī)劃實驗如圖7。3種算法規(guī)劃出的路徑長度通過計算比較并沒有明顯差別。
改進多步長蟻群算法將每次迭代產生的最優(yōu)路徑作為引導路徑,提高了算法的收斂速度,該算法規(guī)劃路徑時需要進行多次迭代,當其收斂后即完成了路徑規(guī)劃,所以統(tǒng)計收斂用時作為改進多步長蟻群算法的所用時間。表2為10次對比實驗中3種算法的規(guī)劃時間統(tǒng)計。由于改進多步長蟻群算法具有一定的隨機性,每次收斂時的迭代次數(shù)有起伏造成了所用時間有波動。DLD*lite算法平均用時701.2 ms,改進多步長蟻群算法平均用時654 ms,本文算法平均用時504.4 ms??梢钥闯?,雖然本文算法需要進行節(jié)點圖雙向搜索計算以及核心網(wǎng)格計算,但是總規(guī)劃平均時間仍然比DLD*lite算法減少了大約28%,比改進多步長蟻群算法減少了大約23%,驗證了本文算法在提高路徑規(guī)劃效率上的有效性。
圖7 改進多步長蟻群算法Fig.7 Improved multi-step ant colony algorithm
表2 多次對比實驗用時統(tǒng)計
本文提出了一種基于地圖分割的移動機器人路徑規(guī)劃方法,在原始D*lite算法的基礎上改進了搜索策略,通過單元分解和雙向圖搜索方法找出從起始網(wǎng)格到目標網(wǎng)格的必經(jīng)單元,按照核心網(wǎng)格設置策略在這些必經(jīng)單元中選出核心網(wǎng)格,將若干核心網(wǎng)格依次插入目標網(wǎng)格和起始網(wǎng)格之間組成搜索鏈表,用D*lite算法的反向搜索針對搜索鏈表完成分段搜索,即可得出起始網(wǎng)格到目標網(wǎng)格的路徑。將原始D*lite算法中直接的、低效的搜索策略改進為分段的、高效的搜索策略,能夠在室內環(huán)境中快速地找到繞過障礙物的正確路徑。通過實驗比較證實了本文算法對于提高路徑規(guī)劃效率的有效性。然而移動機器人所在環(huán)境一般是動態(tài)的,已規(guī)劃好的路徑可能因為移動過程中出現(xiàn)新的障礙物而被阻斷,此時需要路徑重規(guī)劃。D*lite算法進行路徑重規(guī)劃時可以保留初次規(guī)劃中未受環(huán)境變化影響的網(wǎng)格的全部信息,但是為了找到導向新起點的受環(huán)境變化影響的網(wǎng)格并將它們處理成能夠繼續(xù)擴展的狀態(tài),程序需要進行多次循環(huán),將會耗費大量的時間。如何快速找到這部分網(wǎng)格并插入優(yōu)先隊列以此來提高D*lite算法路徑重規(guī)劃的速率將是未來的研究目標。