基于單元分解的改進(jìn)D*lite路徑規(guī)劃算法

張 毅，施明瑞

(重慶郵電大學(xué) 國(guó)家信息無(wú)障礙工程研發(fā)中心，重慶 400065)

0 引 言

移動(dòng)機(jī)器人規(guī)劃路徑能力的大小，決定了機(jī)器人可勝任工作難度的高低。

在構(gòu)建好的環(huán)境模型中，規(guī)劃一條從初始點(diǎn)到目標(biāo)點(diǎn)的無(wú)碰撞最優(yōu)路徑，是路徑規(guī)劃的主要內(nèi)容。在構(gòu)建環(huán)境模型時(shí)，柵格法應(yīng)用較多，該方法具有直觀簡(jiǎn)潔、分辨率可變、容易創(chuàng)建和存儲(chǔ)等優(yōu)點(diǎn)，適用于室內(nèi)環(huán)境路徑規(guī)劃地圖模型的建立，魯棒性強(qiáng)[1]。圖搜索算法是路徑規(guī)劃中的一類算法，它被廣泛應(yīng)用于柵格地圖中的路徑規(guī)劃問(wèn)題。A*算法是圖搜索算法中的經(jīng)典算法，它是一種啟發(fā)式搜索方法。啟發(fā)式搜索會(huì)評(píng)估狀態(tài)空間中的每個(gè)搜索位置，找出下一步要搜索的最好位置G，再?gòu)腉進(jìn)行類似搜索直到確定目標(biāo)位置。該搜索方法能節(jié)省大量搜索空間，提高搜索效率[2]。在A*算法的基礎(chǔ)上，研究者們先后提出了多種改進(jìn)算法，例如D*，LPA*與D*lite等算法。

D*lite算法是文獻(xiàn)[3]提出的。該算法采用反向搜索，從目標(biāo)點(diǎn)向當(dāng)前點(diǎn)擴(kuò)展，在進(jìn)行重規(guī)劃時(shí)極大地提高了效率。文獻(xiàn)[4]通過(guò)在啟發(fā)估價(jià)值中加入與障礙物相關(guān)的值，從而避免規(guī)劃出靠近障礙物尖角或穿越兩相鄰障礙物的不安全或不可達(dá)路徑。文獻(xiàn)[5]通過(guò)融合D*lite算法和RRT*算法提高了收斂速率。文獻(xiàn)[6]采用模型預(yù)測(cè)控制MPC，優(yōu)化了D*lite算法在動(dòng)態(tài)環(huán)境和特殊地形中的路徑規(guī)劃能力。文獻(xiàn)[7]通過(guò)引入備選路徑，提高了D*lite算法中路徑重規(guī)劃的速度。文獻(xiàn)[8]通過(guò)在D*lite算法中引入搜索樹(shù)的思想，在需要重規(guī)劃時(shí)立即砍斷搜索樹(shù)，從而提高路徑重規(guī)劃的速度。

學(xué)者們對(duì)D*lite算法規(guī)劃出的路徑的安全性、平滑度和算法重規(guī)劃策略進(jìn)行了諸多研究，但是在路徑搜索中仍然存在問(wèn)題。

D*lite算法在進(jìn)行路徑規(guī)劃時(shí)，會(huì)給網(wǎng)格設(shè)置優(yōu)先級(jí)，每次循環(huán)中選取優(yōu)先級(jí)最高的網(wǎng)格進(jìn)行擴(kuò)展，啟發(fā)信息是優(yōu)先級(jí)設(shè)置中的重要參考，但是啟發(fā)信息只是估計(jì)代價(jià)，無(wú)法將障礙物對(duì)全局移動(dòng)代價(jià)的影響完全包含。當(dāng)障礙物將較大的搜索空間分隔成多個(gè)較小的自由區(qū)域時(shí)，優(yōu)先級(jí)高的網(wǎng)格指引的搜索方向可能最終會(huì)被障礙物阻隔而無(wú)法繼續(xù)擴(kuò)展，導(dǎo)致正確的搜索方向被隱藏起來(lái)，往往需要經(jīng)過(guò)多次嘗試才能找到，在錯(cuò)誤的搜索方向上擴(kuò)展的網(wǎng)格是無(wú)效網(wǎng)格，增加了計(jì)算次數(shù)而對(duì)規(guī)劃路徑?jīng)]有幫助，影響了路徑規(guī)劃的效率。因此本文提出一種基于單元分解的改進(jìn)D*lite路徑規(guī)劃算法，將室內(nèi)環(huán)境分解為若干相互連通的單元，這些單元的相互連通區(qū)域，即連接兩個(gè)相鄰單元的可通行的自由區(qū)域，是移動(dòng)機(jī)器人導(dǎo)航時(shí)必須經(jīng)過(guò)的網(wǎng)格，可以在這些網(wǎng)格中選取合適網(wǎng)格作為核心網(wǎng)格，用來(lái)引導(dǎo)路徑規(guī)劃算法的搜索方向，這樣能夠快速找到繞過(guò)障礙物的正確路徑，從而減少無(wú)效網(wǎng)格的擴(kuò)展，提高規(guī)劃速度。

1 D*lite算法改進(jìn)

1.1 改進(jìn)流程

本文在原始D*lite算法的基礎(chǔ)上進(jìn)行優(yōu)化，在原有的搜索模型中加入核心網(wǎng)格進(jìn)行改進(jìn)，核心網(wǎng)格會(huì)引導(dǎo)搜索算法在正確的方向上進(jìn)行搜索和擴(kuò)展。

改進(jìn)流程如下。

1)在原有Boustrophedon[9]單元分解法的基礎(chǔ)上設(shè)計(jì)基于新的分解規(guī)則，分割環(huán)境的柵格地圖。

2)設(shè)計(jì)雙向圖搜索算法，找出單元順序。

3)設(shè)計(jì)核心網(wǎng)格設(shè)置方法，得到核心網(wǎng)格組成搜索鏈表。

4)以搜索鏈表引導(dǎo)搜索算法完成路徑規(guī)劃。

改進(jìn)流程如圖1。

圖1 改進(jìn)流程Fig.1 Improved processes

1.2 單元分解

利用Boustrophedon單元分解法以及新增的分解規(guī)則，可以將移動(dòng)機(jī)器人所在的室內(nèi)空間分解成若干單元，這些單元是內(nèi)部沒(méi)有障礙物的有界區(qū)域。

Boustrophedon單元分解法是用一條垂直線從左至右掃描柵格地圖。當(dāng)掃描線掃入障礙空間時(shí)，掃描線的連通性增加，當(dāng)掃描線掃出障礙空間時(shí)，掃描線的連通性降低。當(dāng)掃描線的連通性增加時(shí)，一個(gè)舊單元結(jié)束，生成兩個(gè)新單元。當(dāng)掃描線的連通性降低時(shí)，兩個(gè)舊單元結(jié)束，生成一個(gè)新單元。

當(dāng)障礙空間的外形產(chǎn)生變化時(shí)，此時(shí)掃描線的連通性不變，但單元內(nèi)仍然存在阻擋網(wǎng)格擴(kuò)展的障礙，算法難以快速找到繞過(guò)障礙的正確路徑。針對(duì)此問(wèn)題設(shè)計(jì)了新的分解規(guī)則，當(dāng)掃描線的連通性不變但某段連通線長(zhǎng)度發(fā)生了變化，若變化的大小超過(guò)當(dāng)前單元最大寬度的一半，則舊單元結(jié)束，新單元生成如圖2。

圖2 連通線長(zhǎng)度變化較大時(shí)產(chǎn)生新單元Fig.2 Generating new unit when the length of the slice changes greatly

其余情況只增加單元大小，而不產(chǎn)生或結(jié)束單元。掃描結(jié)束后，環(huán)境地圖被分割成若干獨(dú)立的有界單元，每個(gè)單元內(nèi)部都沒(méi)有障礙物，如圖3。

圖3 地圖分割成若干單元Fig.3 Map is segmented into several units

可以把這些單元看作節(jié)點(diǎn)，單元之間如果有直接的連通區(qū)域則可以看作相應(yīng)節(jié)點(diǎn)間有邊連接，這些邊和節(jié)點(diǎn)可以組成圖。需要將兩單元間的移動(dòng)代價(jià)作為代價(jià)值賦給相應(yīng)的邊，在二維柵格地圖中曼哈頓距離的計(jì)算簡(jiǎn)單，且能夠較準(zhǔn)確地比較兩段距離長(zhǎng)短，所以可以將單元補(bǔ)全為長(zhǎng)方形，計(jì)算邊的兩端單元中心之間的曼哈頓距離，此距離作為邊的代價(jià)值。若兩單元之間沒(méi)有直接的自由連通區(qū)域，則兩單元節(jié)點(diǎn)之間沒(méi)有邊連接，兩單元之間的代價(jià)值設(shè)置為無(wú)窮大。計(jì)算完成后可得到單元節(jié)點(diǎn)圖如圖4。

圖4 單元節(jié)點(diǎn)圖Fig.4 Node graph of units

把節(jié)點(diǎn)圖中的代價(jià)值用矩陣表示可以得到鄰接矩陣edge，其中，edge[i][j]代表節(jié)點(diǎn)i與節(jié)點(diǎn)j之間的直接代價(jià)值。

1.3 節(jié)點(diǎn)圖雙向搜索

設(shè)定起始網(wǎng)格所在節(jié)點(diǎn)為起始節(jié)點(diǎn)，目標(biāo)網(wǎng)格所在節(jié)點(diǎn)為目標(biāo)節(jié)點(diǎn)，需要找到起始節(jié)點(diǎn)到目標(biāo)節(jié)點(diǎn)代價(jià)總值最短的路徑，此路徑表示了從起始網(wǎng)格到目標(biāo)網(wǎng)格的最短路徑需要依次經(jīng)過(guò)哪些單元。

利用圖搜索方法可以在節(jié)點(diǎn)圖中找到兩個(gè)節(jié)點(diǎn)之間的最短路徑。給每個(gè)節(jié)點(diǎn)設(shè)置兩個(gè)值，cost值表示與搜索起始節(jié)點(diǎn)的代價(jià)值，pre表示父節(jié)點(diǎn)，每次從中取出cost值最小的節(jié)點(diǎn)，然后更新其余節(jié)點(diǎn)的cost和pre，直至取出搜索目標(biāo)節(jié)點(diǎn)。此搜索方法每次只選取代價(jià)值最小的節(jié)點(diǎn)，所以在找到起始節(jié)點(diǎn)到目標(biāo)節(jié)點(diǎn)的最短路徑前，會(huì)先計(jì)算一些無(wú)法到達(dá)目標(biāo)節(jié)點(diǎn)但代價(jià)值更小的路徑。為了提高算法的速度，可以采用雙向搜索的方法。同時(shí)進(jìn)行兩個(gè)搜索，分別以起始節(jié)點(diǎn)和目標(biāo)節(jié)點(diǎn)作為搜索起始節(jié)點(diǎn)。

節(jié)點(diǎn)圖雙向搜索方法的算法步驟如下。

1)初始化。用集合c保存已經(jīng)確認(rèn)最短路徑的節(jié)點(diǎn)，把搜索起始節(jié)點(diǎn)s放入c中。用集合w保存還沒(méi)有確認(rèn)最短路徑的節(jié)點(diǎn)，把其余節(jié)點(diǎn)都放在w中。將w中所有節(jié)點(diǎn)j的cost值設(shè)為edge[s][j]，pre設(shè)為s。

2)從集合w中選出cost最小的節(jié)點(diǎn)i，由于cost非負(fù)，所以此cost為s到i的最小代價(jià)值，將i移出w，移入c。

3)對(duì)于w中的每個(gè)節(jié)點(diǎn)j，若有costj>costi+edge[i][j]，則更新costj為costi+edge[i][j]，并把prej設(shè)為i。

4)在兩個(gè)搜索中分別重復(fù)步驟2、步驟3，直到兩個(gè)搜索中的集合c出現(xiàn)交集,或者任意一個(gè)搜索中集合w中節(jié)點(diǎn)的cost值都為無(wú)窮大，此時(shí)循環(huán)結(jié)束。

5)若兩個(gè)搜索中的集合c有交集，則根據(jù)c中節(jié)點(diǎn)的pre值可以得到起始節(jié)點(diǎn)到目標(biāo)節(jié)點(diǎn)代價(jià)總值最短的路徑，即可得到起始網(wǎng)格到目標(biāo)網(wǎng)格最短路徑經(jīng)過(guò)的單元順序，否則就說(shuō)明起始節(jié)點(diǎn)無(wú)法到達(dá)目標(biāo)節(jié)點(diǎn)。

1.4 基于核心網(wǎng)格的路徑規(guī)劃

D*lite算法采用反向搜索方法，即在路徑規(guī)劃時(shí)，從目標(biāo)網(wǎng)格開(kāi)始搜索，向起始網(wǎng)格擴(kuò)展。在D*lite算法中每個(gè)網(wǎng)格s需要維護(hù)3個(gè)值：g(s)為目標(biāo)網(wǎng)格到s網(wǎng)格的實(shí)際代價(jià)；h(s)是啟發(fā)值，為s網(wǎng)格到起始網(wǎng)格的估計(jì)代價(jià)，取s網(wǎng)格與起始網(wǎng)格橫坐標(biāo)之差和縱坐標(biāo)之差中的最大值，計(jì)算如(1)式；rhs(s)保存s網(wǎng)格的父網(wǎng)格s′的g值加上父網(wǎng)格到s網(wǎng)格的代價(jià)值c(s′,s)，rhs(s)的計(jì)算如(2)式。由于機(jī)器人在柵格地圖中，從所在網(wǎng)格可以向相鄰的8個(gè)網(wǎng)格中任意一個(gè)直接移動(dòng)，所以D*lite算法中父網(wǎng)格到8個(gè)子網(wǎng)格的代價(jià)值都為1，(1)式的計(jì)算結(jié)果可以作為網(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)格，是從目標(biāo)網(wǎng)格向起始網(wǎng)格搜索時(shí)搜索方向上的父網(wǎng)格。

當(dāng)網(wǎng)格的g值等于rhs值時(shí)，稱為局部一致，反之則為局部不一致，只有局部一致的網(wǎng)格才是擴(kuò)展完畢可以通行的。D*lite算法的優(yōu)勢(shì)在于動(dòng)態(tài)環(huán)境中可以通過(guò)g值和rhs值的關(guān)系判斷網(wǎng)格的狀態(tài)，從而找出受環(huán)境變化影響的網(wǎng)格。本文暫時(shí)只討論初始環(huán)境下的路徑規(guī)劃。

算法使用一個(gè)優(yōu)先隊(duì)列保存待更新的局部不一致網(wǎng)格，以k值作為優(yōu)先級(jí)，k值越小優(yōu)先級(jí)越高，k值由k1和k2組成，k(s)=(k1(s),k2(s))，k1和k2的計(jì)算方法為

(3)

比較k值的大小時(shí)，優(yōu)先比較k1，如果k1值相等再比較k2值。

D*lite算法的核心步驟如下。

1)初始化所有網(wǎng)格，將所有網(wǎng)格的g值和rhs值設(shè)置為無(wú)窮大，然后將目標(biāo)網(wǎng)格的rhs值設(shè)置為0，根據(jù)(3)式計(jì)算其k值后插入優(yōu)先隊(duì)列。

2)從優(yōu)先隊(duì)列中移出優(yōu)先級(jí)最高的網(wǎng)格，設(shè)置其g值等于rhs值，使該網(wǎng)格成為局部一致。然后根據(jù)(2)式計(jì)算該網(wǎng)格的鄰近網(wǎng)格的rhs值，若這些網(wǎng)格的rhs值與g值不等，則計(jì)算它們的k值并將它們插入優(yōu)先隊(duì)列。

3)重復(fù)第2步直到起始網(wǎng)格成為局部一致。從起始網(wǎng)格開(kāi)始按照g值最小的規(guī)則搜索四周鄰近網(wǎng)格，直到到達(dá)目標(biāo)網(wǎng)格，即可得到最終路徑。

D*lite算法以k值作為優(yōu)先級(jí)來(lái)決定從優(yōu)先隊(duì)列中取出哪個(gè)網(wǎng)格，以求得8個(gè)鄰域方向的最優(yōu)解。然而很多優(yōu)先級(jí)較高的網(wǎng)格引導(dǎo)的搜索方向并不能進(jìn)入正確路徑必須經(jīng)過(guò)的單元中，通過(guò)這些網(wǎng)格無(wú)法找到正確路徑，勢(shì)必造成算法在計(jì)算過(guò)程中耗費(fèi)大量時(shí)間做無(wú)效工作。若在起始網(wǎng)格到目標(biāo)網(wǎng)格的路徑必須經(jīng)過(guò)的單元中設(shè)置核心網(wǎng)格，首先從目標(biāo)網(wǎng)格向核心網(wǎng)格進(jìn)行擴(kuò)展，則在一開(kāi)始就能找到繞過(guò)障礙物的正確路徑。

為了找出核心網(wǎng)格，需要先設(shè)置核心區(qū)域，再在核心區(qū)域中設(shè)置核心網(wǎng)格。核心網(wǎng)格的設(shè)置算法如下。

1)為每一個(gè)網(wǎng)格添加一個(gè)編號(hào)n，n等于網(wǎng)格所在單元的編號(hào)。因?yàn)镈*lite算法采用反向搜索，需要把節(jié)點(diǎn)圖雙向搜索算法得到的單元順序顛倒方向，使之成為從目標(biāo)單元到起始單元的單元順序。

2)根據(jù)單元順序，從第2個(gè)單元開(kāi)始，找出與前一個(gè)單元相鄰的網(wǎng)格作為核心區(qū)域，直到起始單元。設(shè)當(dāng)前單元為u，前一單元為u′，核心區(qū)域設(shè)置如(4)式，其中，Ss.n=u.n表示單元u中的網(wǎng)格，succ(s′)表示與網(wǎng)格s′相鄰的網(wǎng)格，(4)式表示在當(dāng)前單元u中，所有與前一單元u′相鄰的網(wǎng)格共同組成了單元u的核心區(qū)域。以xmax，xmin，ymax，ymin表示核心區(qū)域內(nèi)的網(wǎng)格中的最大和最小橫縱坐標(biāo)，則核心區(qū)域中心點(diǎn)可由(5)式得到。

(4)

(5)

3)設(shè)置目標(biāo)網(wǎng)格為第1個(gè)核心網(wǎng)格，起始網(wǎng)格為最后一個(gè)核心網(wǎng)格。給每一個(gè)核心區(qū)域內(nèi)的網(wǎng)格設(shè)置一個(gè)kc值，由kc1和kc2組成，kc(s)=(kc1(s),kc2(s))，kc1和kc2的計(jì)算如 (6) 式。其中s為當(dāng)前核心區(qū)域內(nèi)的網(wǎng)格，cgrid為上一個(gè)核心網(wǎng)格，coremid為下一個(gè)核心區(qū)域的中心點(diǎn)。每個(gè)核心區(qū)域中選取kc值最小的網(wǎng)格作為核心網(wǎng)格，比較kc值大小時(shí)，首先比較kc1值，如果kc1相同再比較kc2值。設(shè)置核心網(wǎng)格如圖5。

(6)

圖5 設(shè)置核心網(wǎng)格Fig.5 Main grids are set

圖5中星標(biāo)志為目標(biāo)網(wǎng)格，圓標(biāo)志為起始網(wǎng)格，由節(jié)點(diǎn)圖雙向搜索算法可計(jì)算出目標(biāo)單元到起始單元的最短路徑為5→2→1→4→9，灰色網(wǎng)格為核心區(qū)域，圓環(huán)標(biāo)志為核心網(wǎng)格。

將目標(biāo)網(wǎng)格、起始網(wǎng)格和設(shè)置完成的核心網(wǎng)格按照已經(jīng)計(jì)算完成的單元順序依次插入搜索鏈表中，用搜索鏈表引導(dǎo)D*lite算法完成多段路徑規(guī)劃。首先設(shè)置一個(gè)地址P指向搜索鏈表的首個(gè)網(wǎng)格。執(zhí)行每段路徑規(guī)劃時(shí)，以地址P當(dāng)前指向的網(wǎng)格作為單次搜索中的目標(biāo)網(wǎng)格，以P的下一個(gè)地址指向的網(wǎng)格作為單次搜索中的起始網(wǎng)格，為了增加搜索效率，限定只有編號(hào)n與當(dāng)前搜索目標(biāo)網(wǎng)格或者當(dāng)前搜索起始網(wǎng)格的編號(hào)相同的網(wǎng)格才能被遍歷，當(dāng)本次搜索中的起始網(wǎng)格成為局部一致后，將P向后移一位，繼續(xù)執(zhí)行下一段路徑規(guī)劃。當(dāng)搜索鏈表的最后一個(gè)網(wǎng)格即真正的起始網(wǎng)格成為局部一致時(shí)，從起始網(wǎng)格開(kāi)始循著鄰接網(wǎng)格中g(shù)值最小的網(wǎng)格移動(dòng)，直到到達(dá)目標(biāo)網(wǎng)格，就可完成從起始網(wǎng)格到目標(biāo)網(wǎng)格的路徑規(guī)劃。

2 實(shí)驗(yàn)結(jié)果與分析

為了驗(yàn)證算法的有效性，以MATLAB R2016b作為軟件平臺(tái)進(jìn)行仿真，以Intel Core i3-6100 CPU作為硬件平臺(tái)，主頻為3.70 GHZ，4 GByte內(nèi)存。

在相同的硬件平臺(tái)和軟件平臺(tái)下，對(duì)本文算法和DLD*lite算法[10]進(jìn)行相同柵格地圖下的對(duì)比實(shí)驗(yàn),DLD*lite算法在D*lite算法的基礎(chǔ)上引入了視線算法和距離變換作為改進(jìn)。實(shí)驗(yàn)采用15×15的柵格地圖進(jìn)行仿真，得到如圖6的路徑規(guī)劃對(duì)比實(shí)驗(yàn)圖。

圖6 對(duì)比實(shí)驗(yàn)Fig.6 Comparative experiment

圖6中星形標(biāo)志為目標(biāo)網(wǎng)格，圓形標(biāo)志為起始網(wǎng)格，黑色網(wǎng)格為障礙物。深灰色網(wǎng)格和淺灰色網(wǎng)格為更新函數(shù)遍歷過(guò)的網(wǎng)格，計(jì)算了rhs值和k值并插入優(yōu)先隊(duì)列中。深灰色網(wǎng)格為搜索函數(shù)從優(yōu)先隊(duì)列中取出，計(jì)算完g值的局部一致網(wǎng)格。

圖6a為DLD*lite算法仿真圖，該算法在啟發(fā)函數(shù)中加入障礙物距離值，用視線算法更新父節(jié)點(diǎn)，使得規(guī)劃出的路徑平滑且安全，但是由于該算法每次只選取k值最小的網(wǎng)格進(jìn)行擴(kuò)展，從而擴(kuò)展了很多無(wú)效網(wǎng)格，對(duì)這些網(wǎng)格的g值和rhs值的計(jì)算耗費(fèi)了大量時(shí)間卻收效甚微，這樣大大降低了搜索效率。圖6b為本文算法的仿真圖，方形標(biāo)志為算法計(jì)算出的核心網(wǎng)格，通過(guò)單元分解、雙向圖搜索和設(shè)置核心網(wǎng)格得到了搜索鏈表，用搜索鏈表引導(dǎo)分段搜索，最終找到起始網(wǎng)格到目標(biāo)網(wǎng)格的路徑。由于以核心網(wǎng)格組成的搜索鏈表作為導(dǎo)向，使得搜索算法朝著正確的方向進(jìn)行擴(kuò)展，大大降低了遍歷網(wǎng)格的數(shù)量和計(jì)算次數(shù)，從而提高了路徑規(guī)劃的速度。

表1為路徑規(guī)劃對(duì)比實(shí)驗(yàn)中已遍歷網(wǎng)格數(shù)與最終路徑長(zhǎng)度的統(tǒng)計(jì)。本文算法中更新函數(shù)已遍歷的網(wǎng)格數(shù)比DLD*lite算法中減少了43%，搜索函數(shù)已計(jì)算的網(wǎng)格數(shù)減少了70%。可以看出本文算法減少了網(wǎng)格的遍歷次數(shù)和計(jì)算次數(shù)。

表1 路徑規(guī)劃對(duì)比實(shí)驗(yàn)相關(guān)數(shù)據(jù)

為了進(jìn)一步驗(yàn)證本文算法提高路徑規(guī)劃效率的有效性，統(tǒng)計(jì)本文算法、DLD*lite算法和改進(jìn)多步長(zhǎng)蟻群算法[11]路徑規(guī)劃所用時(shí)間并進(jìn)行對(duì)比，為了消除偶然現(xiàn)象，提高實(shí)驗(yàn)結(jié)果的精確性，統(tǒng)計(jì)了多次對(duì)比實(shí)驗(yàn)。改進(jìn)多步長(zhǎng)蟻群算法路徑規(guī)劃實(shí)驗(yàn)如圖7。3種算法規(guī)劃出的路徑長(zhǎng)度通過(guò)計(jì)算比較并沒(méi)有明顯差別。

改進(jìn)多步長(zhǎng)蟻群算法將每次迭代產(chǎn)生的最優(yōu)路徑作為引導(dǎo)路徑，提高了算法的收斂速度，該算法規(guī)劃路徑時(shí)需要進(jìn)行多次迭代，當(dāng)其收斂后即完成了路徑規(guī)劃，所以統(tǒng)計(jì)收斂用時(shí)作為改進(jìn)多步長(zhǎng)蟻群算法的所用時(shí)間。表2為10次對(duì)比實(shí)驗(yàn)中3種算法的規(guī)劃時(shí)間統(tǒng)計(jì)。由于改進(jìn)多步長(zhǎng)蟻群算法具有一定的隨機(jī)性，每次收斂時(shí)的迭代次數(shù)有起伏造成了所用時(shí)間有波動(dòng)。DLD*lite算法平均用時(shí)701.2 ms，改進(jìn)多步長(zhǎng)蟻群算法平均用時(shí)654 ms，本文算法平均用時(shí)504.4 ms?？梢钥闯觯m然本文算法需要進(jìn)行節(jié)點(diǎn)圖雙向搜索計(jì)算以及核心網(wǎng)格計(jì)算，但是總規(guī)劃平均時(shí)間仍然比DLD*lite算法減少了大約28%，比改進(jìn)多步長(zhǎng)蟻群算法減少了大約23%，驗(yàn)證了本文算法在提高路徑規(guī)劃效率上的有效性。

圖7 改進(jìn)多步長(zhǎng)蟻群算法Fig.7 Improved multi-step ant colony algorithm

表2 多次對(duì)比實(shí)驗(yàn)用時(shí)統(tǒng)計(jì)

3 結(jié)束語(yǔ)

本文提出了一種基于地圖分割的移動(dòng)機(jī)器人路徑規(guī)劃方法，在原始D*lite算法的基礎(chǔ)上改進(jìn)了搜索策略，通過(guò)單元分解和雙向圖搜索方法找出從起始網(wǎng)格到目標(biāo)網(wǎng)格的必經(jīng)單元，按照核心網(wǎng)格設(shè)置策略在這些必經(jīng)單元中選出核心網(wǎng)格，將若干核心網(wǎng)格依次插入目標(biāo)網(wǎng)格和起始網(wǎng)格之間組成搜索鏈表，用D*lite算法的反向搜索針對(duì)搜索鏈表完成分段搜索，即可得出起始網(wǎng)格到目標(biāo)網(wǎng)格的路徑。將原始D*lite算法中直接的、低效的搜索策略改進(jìn)為分段的、高效的搜索策略，能夠在室內(nèi)環(huán)境中快速地找到繞過(guò)障礙物的正確路徑。通過(guò)實(shí)驗(yàn)比較證實(shí)了本文算法對(duì)于提高路徑規(guī)劃效率的有效性。然而移動(dòng)機(jī)器人所在環(huán)境一般是動(dòng)態(tài)的，已規(guī)劃好的路徑可能因?yàn)橐苿?dòng)過(guò)程中出現(xiàn)新的障礙物而被阻斷，此時(shí)需要路徑重規(guī)劃。D*lite算法進(jìn)行路徑重規(guī)劃時(shí)可以保留初次規(guī)劃中未受環(huán)境變化影響的網(wǎng)格的全部信息，但是為了找到導(dǎo)向新起點(diǎn)的受環(huán)境變化影響的網(wǎng)格并將它們處理成能夠繼續(xù)擴(kuò)展的狀態(tài)，程序需要進(jìn)行多次循環(huán)，將會(huì)耗費(fèi)大量的時(shí)間。如何快速找到這部分網(wǎng)格并插入優(yōu)先隊(duì)列以此來(lái)提高D*lite算法路徑重規(guī)劃的速率將是未來(lái)的研究目標(biāo)。