基于改進(jìn)D*Lite的二維路徑連續(xù)動(dòng)態(tài)規(guī)劃算法

傅 亮,劉 峰,劉書(shū)勇*,韓新宇

(1.中國(guó)航空無(wú)線電電子研究所 民機(jī)航電系統(tǒng)部,上海 200241;2.哈爾濱工程大學(xué) 計(jì)算機(jī)科學(xué)與技術(shù)學(xué)院,黑龍江 哈爾濱 150001)

0 引言

路徑規(guī)劃[1]是讓目標(biāo)對(duì)象基于環(huán)境模型下尋找一條經(jīng)過(guò)起止節(jié)點(diǎn)的無(wú)碰撞障礙物的安全路線優(yōu)化問(wèn)題,它廣泛應(yīng)用于移動(dòng)機(jī)器人、空中無(wú)人機(jī)、自動(dòng)駕駛車輛、水面無(wú)人艇等領(lǐng)域[2-8]?；谝?guī)劃模式可劃分為靜態(tài)路徑規(guī)劃與動(dòng)態(tài)路徑規(guī)劃兩大類。靜態(tài)路徑規(guī)劃常用算法有圖搜索算法[9-11]和智能優(yōu)化算法。圖搜索算法如Dijkstra、A*、RRT等,智能優(yōu)化算法如蟻群算法、遺傳算法、粒子群算法、蝙蝠算法等[12-15]。靜態(tài)路徑規(guī)劃算法實(shí)施要求之一是需要全區(qū)域的地形信息,對(duì)未知地形則不具備規(guī)劃能力。為解決在未知領(lǐng)域進(jìn)行路線搜索的問(wèn)題,提出了動(dòng)態(tài)路徑規(guī)劃算法,如D*、LPA*、D*Lite等[16-18]。

D*Lite算法是融合D*和LPA*提出的一種反向搜索的動(dòng)態(tài)路徑規(guī)劃算法,應(yīng)用在未知環(huán)境下進(jìn)行快速路線搜索,可以對(duì)未知突變障礙進(jìn)行重規(guī)劃,是一種高效的規(guī)劃算法,但是它依然存在易受運(yùn)動(dòng)物體影響及路徑貼近障礙物等問(wèn)題,因此眾多研究學(xué)者對(duì)其進(jìn)行了改進(jìn)。吳濤等人[19]引入跳點(diǎn)搜索概念,僅對(duì)跳點(diǎn)進(jìn)行訪問(wèn),能夠有效優(yōu)化路徑長(zhǎng)度。杜軒等人[20]結(jié)合人工勢(shì)場(chǎng)(Artificial Potential Field,APF)法對(duì)D*Lite規(guī)劃路線進(jìn)行重規(guī)劃,使得目標(biāo)物體在移動(dòng)中能夠與障礙物保持有效的安全距離,但本身并未對(duì)D*Lite算法進(jìn)行改進(jìn)。張毅等人[21]針對(duì)原有Boustrophedon單元分解法進(jìn)行改進(jìn)重構(gòu),提取關(guān)鍵單元,引導(dǎo)正確搜索方向,有效提高規(guī)劃速度。戴年慧等人[22]進(jìn)入安全系數(shù)阻止危險(xiǎn)節(jié)點(diǎn)作為路徑優(yōu)先選擇節(jié)點(diǎn)以避免斜穿過(guò)障礙,導(dǎo)致冗余點(diǎn)過(guò)多。以上改進(jìn)算法僅對(duì)預(yù)規(guī)劃路徑進(jìn)行單次動(dòng)態(tài)重構(gòu),并沒(méi)有進(jìn)行多次重構(gòu)優(yōu)化,沒(méi)有體現(xiàn)動(dòng)態(tài)規(guī)劃的多樣性。

針對(duì)以上問(wèn)題,本文提出了一種改進(jìn)D*Lite的二維路徑連續(xù)動(dòng)態(tài)規(guī)劃算法(Continuous D*Lite,CD*Lite)。本文算法基于柵格法進(jìn)行環(huán)境建模,記n·m矩陣。優(yōu)化原始D*Lite算法的切比雪夫啟發(fā)估計(jì)函數(shù),使其在估計(jì)數(shù)值上更加貼近實(shí)際路線規(guī)劃。引入人工勢(shì)場(chǎng)的引力場(chǎng)概念,作為補(bǔ)充啟發(fā)估計(jì)函數(shù),并提出了一種全域啟發(fā)估計(jì)算子。引入人工勢(shì)場(chǎng)的斥力場(chǎng)概念,提出了一種方向禁忌矩陣,能夠避免斜穿柵格障礙節(jié)點(diǎn)。優(yōu)化重規(guī)劃模式,使其能夠在連續(xù)產(chǎn)生動(dòng)態(tài)障礙的條件下對(duì)路徑進(jìn)行持續(xù)重構(gòu)。最后引入冗余點(diǎn)刪除機(jī)制,使得規(guī)劃路線更加平滑。

1 相關(guān)工作

1.1 D*Lite算法

D*Lite[18]是由Sven Koenig和Maxim Likhachev于2002年提出的一種基于LPA*和D*的動(dòng)態(tài)路徑規(guī)劃算法。LPA*是一種基于A*和Dynamic SWSF-FP思想的正向傳播增量式動(dòng)態(tài)路徑規(guī)劃算法,但正向搜索會(huì)因?yàn)楹芏喾种?dǎo)致效率下降,因此D*Lite融入D*的逆向搜索思想,從目標(biāo)節(jié)點(diǎn)反向搜索起始節(jié)點(diǎn),能夠有效地減少節(jié)點(diǎn)訪問(wèn)次數(shù),減少搜索時(shí)間。D*Lite是一種起始節(jié)點(diǎn)可變化的反向規(guī)劃算法,因此更適合應(yīng)用于未知環(huán)境或預(yù)規(guī)劃路線突變障礙的情況。

D*Lite算法通過(guò)不斷更新節(jié)點(diǎn)值執(zhí)行路徑搜索過(guò)程,其定義如式(1)所示:

(1)

式中:keys是由k1和k2組成的二維矩陣,表示為s節(jié)點(diǎn)的鍵值;sstart為當(dāng)前開(kāi)始節(jié)點(diǎn),sgoal為目標(biāo)節(jié)點(diǎn),slast為重構(gòu)路徑之前的sstart節(jié)點(diǎn),g(s)為sgoal節(jié)點(diǎn)到s節(jié)點(diǎn)的歷史最小實(shí)際代價(jià)值;rhs(s)為sgoal節(jié)點(diǎn)到s節(jié)點(diǎn)的啟發(fā)估計(jì)值;km為鍵值修飾器,是slast節(jié)點(diǎn)到sstart節(jié)點(diǎn)的實(shí)際移動(dòng)距離。

D*Lite與LPA*最大的不同在于key值定義,通過(guò)引入km因子修飾key,使得啟發(fā)估計(jì)值的sstart節(jié)點(diǎn)是可變化的,可以減少不必要節(jié)點(diǎn)的計(jì)算操作,從而大大提高算法效率,也使得key值比較對(duì)于實(shí)際而言更有意義。由式(1)可知k1由最小實(shí)際代價(jià)值、啟發(fā)估計(jì)值、鍵值修飾器累和求得,其具體定義如式(2)～(5)所示:

(2)

(3)

h(s,sstart)=λ·max(|xs-xsstart|,|ys-ysstart|),

(4)

(5)

式中:Succ(s)為s節(jié)點(diǎn)的后繼節(jié)點(diǎn)集合;λ為單步移動(dòng)代價(jià);c(s,s′)為邊緣實(shí)際代價(jià)函數(shù),即s節(jié)點(diǎn)到邊緣s′節(jié)點(diǎn)的歐氏距離,xsysxs′ys′為s,s′的橫縱坐標(biāo)值;h(s,sstart)為s,sstart的切比雪夫距離,xsysxsstartysstart為s,sstart的橫縱坐標(biāo);

算法核心在于不斷遍歷節(jié)點(diǎn)并計(jì)算keys放入待檢測(cè)隊(duì)列(Priority Queue)。同時(shí)由式(2)～(3)可知,g(s)和rhs(s)定義相同,但區(qū)別為g(s)為歷史最小值、rhs(s)為當(dāng)前計(jì)算值,取二者最小值用于更新keys,共計(jì)3種不確定狀態(tài)如下所示:

① 若rhs(s)=g(s),局部一致?tīng)顟B(tài)。此狀態(tài)表示結(jié)點(diǎn)s處于預(yù)估與實(shí)際基本沒(méi)有差錯(cuò)的穩(wěn)定狀態(tài)。如無(wú)外界影響,將一直保持此狀態(tài)。

② 若rhs(s)

③ 若rhs(s)>g(s),則處于局部欠一致?tīng)顟B(tài)。此狀態(tài)表示節(jié)點(diǎn)s周圍節(jié)點(diǎn)發(fā)生障礙物突變導(dǎo)致舊有解失效,已經(jīng)收到了附近新障礙物直接或間接的影響,因此需要將其放入優(yōu)先隊(duì)列中進(jìn)行進(jìn)一步判斷。

D*Lite算法主要功能包括預(yù)規(guī)劃路徑和動(dòng)態(tài)規(guī)劃路徑,具體如下所示:

① 預(yù)規(guī)劃路徑:算法初期會(huì)通過(guò)更新全局地圖信息,根據(jù)式(1)計(jì)算相應(yīng)擴(kuò)散節(jié)點(diǎn)的keys,并將其置于優(yōu)先隊(duì)列中,然后計(jì)算優(yōu)先隊(duì)列最小keys,計(jì)算規(guī)則為先按照k1升序排序,再按照k2升序排序,最后優(yōu)先隊(duì)列中首項(xiàng)即為所求,更新該s節(jié)點(diǎn)的最小實(shí)際代價(jià)值,根據(jù)3種局部狀態(tài)執(zhí)行相應(yīng)操作,遍歷邊緣節(jié)點(diǎn)尋找最優(yōu)keys作為其前繼節(jié)點(diǎn),重復(fù)以上操作,直到遍歷到sstart節(jié)點(diǎn),功能結(jié)束。

② 動(dòng)態(tài)規(guī)劃路徑:當(dāng)預(yù)規(guī)劃路徑的節(jié)點(diǎn)突變?yōu)檎系K物,會(huì)重新計(jì)算突變節(jié)點(diǎn)的keys,將受到突變節(jié)點(diǎn)影響的所有邊緣節(jié)點(diǎn)全部置于優(yōu)先隊(duì)列中,再執(zhí)行預(yù)規(guī)劃路徑功能,完成單次動(dòng)態(tài)路徑規(guī)劃。

1.2 人工勢(shì)場(chǎng)

人工勢(shì)場(chǎng)算法是路徑規(guī)劃和局部避障運(yùn)動(dòng)中較為常用的方法,該算法的基本思想是在移動(dòng)點(diǎn)和障礙物周圍定義勢(shì)場(chǎng),移動(dòng)點(diǎn)的運(yùn)動(dòng)依靠勢(shì)場(chǎng)力進(jìn)行驅(qū)動(dòng)。人工勢(shì)場(chǎng)[23]是由Khatib于1986年提出的一種虛擬力場(chǎng),它將目標(biāo)與障礙物看作產(chǎn)生引力與斥力的物體,對(duì)其投入場(chǎng)內(nèi)物體使其沿著場(chǎng)內(nèi)合力方向進(jìn)行移動(dòng)。主要方法為建立引力場(chǎng)Uatt(q)和斥力場(chǎng)Urep(q),如式(6)～(7)所示:

(6)

(7)

式中:ξ為引力尺度因子,η為斥力尺度因子;ρ(q,qgoal)為物體與目標(biāo)的歐氏距離,ρ(q,qobs)為物體與障礙物的歐氏距離,ρ0為障礙物的影響半徑。

Uatt(q)和Urep(q)的負(fù)梯度方向即為引力和斥力的方向,負(fù)梯度值即為物體受到引力和斥力的值,如式(8)～(9)所示:

(8)

(9)

式中:ξ為引力尺度因子,η為斥力尺度因子,ρ(q,qgoal)為物體與目標(biāo)的歐氏距離,ρ(q,qobs)為物體與障礙物的歐氏距離,ρ0為障礙物的影響半徑。

2 CD*Lite改進(jìn)算法

2.1 改進(jìn)啟發(fā)估計(jì)算子

在進(jìn)行動(dòng)態(tài)路徑規(guī)劃時(shí),針對(duì)移動(dòng)點(diǎn)當(dāng)前位置附近的節(jié)點(diǎn),使用傳統(tǒng)的D*Lite算法來(lái)估計(jì)啟發(fā)式值。具體而言,使用切比雪夫距離作為計(jì)算方式,該距離是在向量空間中測(cè)量?jī)蓚€(gè)點(diǎn)之間的一種方法,它定義為兩個(gè)點(diǎn)在各個(gè)坐標(biāo)上數(shù)值差的絕對(duì)值中的最大值。切比雪夫距離也被稱為棋盤(pán)距離,因?yàn)樗愃朴谠谄灞P(pán)上移動(dòng)棋子的最短步數(shù)。在設(shè)定的場(chǎng)景中,目標(biāo)節(jié)點(diǎn)周圍的鄰接節(jié)點(diǎn)與目標(biāo)節(jié)點(diǎn)的切比雪夫距離都是1,這意味著在橫向、縱向和斜向移動(dòng)時(shí),到達(dá)鄰接節(jié)點(diǎn)的代價(jià)是相同的。然而,在實(shí)際情況中,當(dāng)目標(biāo)節(jié)點(diǎn)向預(yù)定節(jié)點(diǎn)移動(dòng)時(shí),“斜走”“先直走再橫走”到達(dá)預(yù)定節(jié)點(diǎn)的代價(jià)是不同的。因此,需要改進(jìn)傳統(tǒng)啟發(fā)估計(jì)算子以匹配實(shí)際路徑規(guī)劃的路徑代價(jià),基于式(10)～(12)對(duì)其進(jìn)行改進(jìn)。

h′(s,sstart)=λ1·diffmin+λ2·(diffmax-diffmin),

(10)

式中:

diffmin=min(|xs-xsstart|,|ys-ysstart|),

(11)

diffmax=max(|xs-xsstart|,|ys-ysstart|),

(12)

2.2 改進(jìn)Key鍵值算子

2.2.1 引入人工勢(shì)場(chǎng)引力算子

人工勢(shì)場(chǎng)法定義目標(biāo)點(diǎn)引力場(chǎng)如式(6)所示,引力場(chǎng)引導(dǎo)移動(dòng)點(diǎn)向目標(biāo)點(diǎn)運(yùn)動(dòng),在移動(dòng)點(diǎn)和目標(biāo)點(diǎn)間無(wú)障礙的理想條件下,移動(dòng)點(diǎn)和目標(biāo)點(diǎn)坐標(biāo)間連線距離始終為最短距離;算法不需要對(duì)全局軌跡進(jìn)行搜索,規(guī)劃時(shí)間短、效率高,滿足實(shí)時(shí)和軌跡生成安全性的要求。定義hatt(s)為移動(dòng)點(diǎn)與目標(biāo)點(diǎn)的啟發(fā)估計(jì)代價(jià)如式(13)所示,即引入人工勢(shì)場(chǎng)引力算子作為補(bǔ)充啟發(fā)代價(jià)算子。

(13)

式中:ξ為引力尺度因子,xsysxsgoalysgoal為s,sgoal的橫縱坐標(biāo)。

2.2.2 全域啟發(fā)估計(jì)算子

D*Lite算法的啟發(fā)估計(jì)算子如式(1)所示,通過(guò)計(jì)算起始點(diǎn)到目標(biāo)點(diǎn)的代價(jià)值標(biāo)識(shí)移動(dòng)點(diǎn)的啟發(fā)效果,在局部規(guī)劃中是一種盲目性的試錯(cuò)路徑規(guī)劃,在復(fù)雜環(huán)境中帶來(lái)更多的時(shí)間效率損耗。為改進(jìn)keys鍵值算子,本文提出全域啟發(fā)估計(jì)算子hwhole替換h(s,sstart),如式(14)～(15)所示:

(14)

其中:

hwhole=h′(s,sstart)+hatt(s),

(15)

式中:g(s)和rhs(s)定義如式(2)～(3)所示,h′(s,sstart)和hatt(s)定義如式(10)和式(13)所示。

式(1)中h(s,sstart)為s,sstart的切比雪夫距離,由2.1節(jié)可知hwhole內(nèi)h′(s,sstart)算子改進(jìn)h(s,sstart)算子的實(shí)際路徑規(guī)劃路徑代價(jià),提升規(guī)劃路徑精度;由2.2.1節(jié)可知,hwhole內(nèi)新增的hatt(s)算子引導(dǎo)移動(dòng)點(diǎn)始終向目標(biāo)點(diǎn)方向移動(dòng),不必通過(guò)分別計(jì)算鄰域8個(gè)方向的結(jié)果后做方向選擇,加快算法收斂速度。

2.3 引入方向禁忌矩陣

D*Lite算法在規(guī)劃路徑時(shí)會(huì)出現(xiàn)斜穿障礙物頂點(diǎn)的危險(xiǎn)行為[22],在實(shí)際路徑規(guī)劃當(dāng)中這種路線會(huì)對(duì)移動(dòng)物體產(chǎn)生威脅,需要將其修正為安全行為,如圖1所示。

圖1 修正危險(xiǎn)路線Fig.1 Correct the dangerous route

修正規(guī)劃路徑中的危險(xiǎn)路徑需要識(shí)別危險(xiǎn)節(jié)點(diǎn),即障礙物節(jié)點(diǎn)。遍歷環(huán)境內(nèi)每一格柵格,判定識(shí)別其是否為障礙物;同時(shí)對(duì)其鄰接8個(gè)方向柵格節(jié)點(diǎn)的路線選擇和變更情況按照?qǐng)D1的禁忌準(zhǔn)則進(jìn)行確認(rèn),并構(gòu)建鄰域方向禁忌矩陣d用于存儲(chǔ)危險(xiǎn)路線修正變更結(jié)果;移動(dòng)點(diǎn)根據(jù)所在節(jié)點(diǎn)8鄰域的規(guī)劃路線修正結(jié)果,減少重復(fù)計(jì)算,避免迂回搜索。

鄰域方向禁忌矩陣d定義如式(16)所示,在對(duì)目標(biāo)節(jié)點(diǎn)進(jìn)行鄰域搜索時(shí),基于其過(guò)濾危險(xiǎn)節(jié)點(diǎn)。

(16)

(17)

(18)

i=x×m+y,

(19)

式中:move(i)為i節(jié)點(diǎn)能夠經(jīng)過(guò)的安全鄰域節(jié)點(diǎn);d為p+1行8列的二維矩陣,p對(duì)應(yīng)n行m列柵格模型環(huán)境矩陣c內(nèi)最后一個(gè)序號(hào)節(jié)點(diǎn);方向禁忌矩陣d的行序號(hào)i與n×m柵格模型環(huán)境矩陣c內(nèi)(x,y)節(jié)點(diǎn)對(duì)應(yīng)關(guān)系如式(19)所示。禁忌矩陣d有8列,即8個(gè)分量對(duì)應(yīng)節(jié)點(diǎn)8個(gè)鄰域方向移動(dòng)有效位,對(duì)應(yīng)鄰域節(jié)點(diǎn)的標(biāo)識(shí)如圖2所示。

圖2 鄰域節(jié)點(diǎn)的標(biāo)識(shí)Fig.2 Identification of neighborhood node

在算法進(jìn)行鄰域節(jié)點(diǎn)搜索或動(dòng)態(tài)路徑重規(guī)劃尋找周圍影響節(jié)點(diǎn)后,基于d進(jìn)行安全行為判斷。從i～j節(jié)點(diǎn)尋找到一條路線,如果dij=-1說(shuō)明j節(jié)點(diǎn)不存在,該路線無(wú)效;dij=1說(shuō)明i節(jié)點(diǎn)可以移動(dòng)到j(luò)節(jié)點(diǎn),是一種安全行為;dij=0說(shuō)明i節(jié)點(diǎn)不可以移動(dòng)到j(luò)節(jié)點(diǎn),是一種危險(xiǎn)行為。dij初始化策略偽代碼如算法1所示。

算法1 UpdateD算法1 Input:S2Output:d3BEGIN:4 for eachs∈S5 if s is not existent ∥節(jié)點(diǎn)s為無(wú)效節(jié)點(diǎn)6 dij=-17 ∥ i～j節(jié)點(diǎn)的路線為安全路線8 else ifi do not cross j obliquely9 dij=110 elsedij=011 end if12 end for13 returnd ∥更新方向禁忌矩陣14END

2.4 連續(xù)動(dòng)態(tài)規(guī)劃模式

D*Lite算法由一次全局預(yù)規(guī)劃和一次動(dòng)態(tài)路徑規(guī)劃組成。算法首先執(zhí)行預(yù)規(guī)劃?rùn)C(jī)制,生成全局初始路線;接著進(jìn)行動(dòng)態(tài)路徑規(guī)劃,如果全局初始路線上出現(xiàn)突變障礙,會(huì)重新設(shè)定sstart起點(diǎn)直到新規(guī)劃路線再次經(jīng)過(guò)sgoal節(jié)點(diǎn),完成一次動(dòng)態(tài)重構(gòu)路徑,目標(biāo)點(diǎn)會(huì)按照更新路線進(jìn)行移動(dòng)。如果目標(biāo)點(diǎn)在完成D*Lite算法更新后的路線上移動(dòng)時(shí),在snow識(shí)別出新的突變障礙,重新使用D*Lite算法規(guī)劃路線,會(huì)導(dǎo)致運(yùn)算效率降低。本文改進(jìn)D*Lite算法的動(dòng)態(tài)路徑規(guī)劃過(guò)程,提出連續(xù)障礙突變動(dòng)態(tài)路徑規(guī)劃算法,即在一次動(dòng)態(tài)路徑規(guī)劃完成后,若識(shí)別出新的突變障礙,可繼續(xù)開(kāi)展動(dòng)態(tài)路徑規(guī)劃,省略全局預(yù)規(guī)劃過(guò)程,大幅提升運(yùn)算效率。算法輸入包括柵格節(jié)點(diǎn)集合以及經(jīng)UpdateD算法輸出的方向禁忌矩陣d。輸出一個(gè)滿足上述約束符合既定需求的動(dòng)態(tài)規(guī)劃局部路線節(jié)點(diǎn)集合E,偽代碼如算法2所示。

算法2 連續(xù)障礙突變動(dòng)態(tài)路徑規(guī)劃算法1 Input:S,d2Output:動(dòng)態(tài)規(guī)劃局部路線節(jié)點(diǎn)集合E3BEGIN:4 ∥既定路線有突變障礙5 If any obstacle changed near s do6 Update sstart and sgoal7 ∥更新全域8 Scan Global Grid and Update snow9 ∥動(dòng)態(tài)規(guī)劃路線10 Compute shortest path and Update E11 end if12 returnE13END

2.5 冗余點(diǎn)刪除機(jī)制

經(jīng)過(guò)動(dòng)態(tài)規(guī)劃后,路徑從起始節(jié)點(diǎn)sstart開(kāi)始順序連接鄰域節(jié)點(diǎn)sgoal,直至到達(dá)終止節(jié)點(diǎn),如圖3所示的“規(guī)劃路線”。規(guī)劃路徑中的節(jié)點(diǎn)與其相鄰節(jié)點(diǎn)符合鄰域8方向機(jī)制,即只能與其周圍8個(gè)方向中的某一個(gè)節(jié)點(diǎn)相連。然而,這樣的連接方式可能會(huì)導(dǎo)致節(jié)點(diǎn)冗余的問(wèn)題。為解決這一問(wèn)題,本文提出了冗余點(diǎn)刪除機(jī)制。具體操作是從起始節(jié)點(diǎn)開(kāi)始按順序遍歷路徑上的節(jié)點(diǎn),構(gòu)建一個(gè)“過(guò)渡路線”,如圖3所示。如果這個(gè)“過(guò)渡路線”不被認(rèn)為是“危險(xiǎn)路線”,則刪除當(dāng)前節(jié)點(diǎn)之前的節(jié)點(diǎn)。如果被認(rèn)為是“危險(xiǎn)路線”,則返回前一個(gè)節(jié)點(diǎn),將其所在的“過(guò)渡路線”定義為“最終路線”,并將該節(jié)點(diǎn)作為新的起始節(jié)點(diǎn),繼續(xù)遍歷和重復(fù)進(jìn)行冗余點(diǎn)刪除,直到達(dá)到終止節(jié)點(diǎn)。這樣構(gòu)建出的“最終路線”即為經(jīng)過(guò)冗余點(diǎn)刪除后優(yōu)化的規(guī)劃路徑,實(shí)現(xiàn)了路徑的平滑化,并且目標(biāo)點(diǎn)移動(dòng)的方向不再局限于鄰域8方向。

圖3 冗余點(diǎn)刪除流程Fig.3 Redundant point deletion process

3 CD*Lite算法實(shí)現(xiàn)

本文CD*Lite算法具體實(shí)現(xiàn)步驟如下:

步驟1:初始化參數(shù)包括rows、cols、snow、sstart、slast、sgoal、km、U、d;

步驟2:執(zhí)行預(yù)規(guī)劃路徑功能,令rhs(sgoal)=0,根據(jù)式(14)計(jì)算keysgoal,將sgoal和keysgoal作為子項(xiàng)插入U(xiǎn);

步驟3:反向搜索,遍歷s節(jié)點(diǎn),根據(jù)g(s)和rhs(s)三種不一致?tīng)顟B(tài)計(jì)算keys并對(duì)g(s)和rhs(s)進(jìn)行同步更新,找到相應(yīng)代價(jià)最小的鄰域節(jié)點(diǎn),基于方向禁忌矩陣d篩除危險(xiǎn)節(jié)點(diǎn),直到遍歷經(jīng)過(guò)sstart且U中子項(xiàng)最小的keys不小于keysstart時(shí)規(guī)劃結(jié)束,此階段延續(xù)采用D*Lite算法構(gòu)建路徑方式。

步驟4:執(zhí)行本文提出的連續(xù)動(dòng)態(tài)規(guī)劃模式,默認(rèn)目標(biāo)單次移動(dòng)單位柵格距離,在移動(dòng)過(guò)程中實(shí)時(shí)檢測(cè)突變障礙,若有異常則及時(shí)更新方向禁忌矩陣d,更新所有因突變障礙而受到影響的所有節(jié)點(diǎn)rhs(s)并執(zhí)行步驟2和步驟3同類操作,至此完成單次動(dòng)態(tài)路徑重構(gòu)。之后繼續(xù)實(shí)時(shí)檢測(cè)全域既定路線是否有突變障礙,重復(fù)以上流程以實(shí)現(xiàn)連續(xù)動(dòng)態(tài)路徑規(guī)劃,直至目標(biāo)移動(dòng)到sgoal,返回全局規(guī)劃路線與突變障礙節(jié)點(diǎn)。

步驟5:最后采用冗余點(diǎn)刪除機(jī)制對(duì)CD*Lite規(guī)劃的路徑進(jìn)行優(yōu)化,保留最優(yōu)路線,算法結(jié)束。

基于以上步驟,本文CD*Lite算法偽代碼如算法3所示。

算法3 CD*Lite算法1 Input:S,d2Output:全局規(guī)劃路線節(jié)點(diǎn)集合Ewhole3BEGIN:4procedure Initialize()5 initialize s∈S,snow=sstart=slast,U=?,km=0,d=0,rhs(s)=g(s)=∞,rhs(sgoal)=0;6 Calculate sgoals key and insert into U(sgoal,key)7procedure Calculate Key(s)8 return[min(g(s),rhs(s))+hwhole(s)+km, min(g(s),rhs(s))]9procedure Compute Shortest Path()10 while U.TopKeyg(sstart)11 u=U.Top();12 Calculate Key(u) and Update us near Vertex13 Update U and related g(u) & rhs(u)14procedure Continuous Dynamic Program (u)15 Move and Scan abrupt obstacles on preplanning path;16 If any obstacle changed:17 Update D and km=h'(slast,sstart)18 While True19 whilesstart≠sgoal20 Update sstart,slast21 Scan directed influenced s by abrupt obstacles22 rhs(s) = ∞ and insert into U(s,Calculate Key(s))23 rhs(obstacles)=g(obstacles)=∞24 Compute Shortest Path()25 If get valid path26 Update Ewhole27 Else28 spre-aiming=spre-aiming.Prec29procedure Main()30 Initialize()31 Update D(d)32 Compute Shortest Path()33 whilesnow≠sgoal34 Continuous Dynamic Programming (snow)35 Update Ewhole36 returnEwhole37END

4 實(shí)驗(yàn)分析

4.1 實(shí)驗(yàn)環(huán)境與參數(shù)設(shè)定

本實(shí)驗(yàn)的硬件環(huán)境:CPU為Intel(R) Core(TM) i7-9750H CPU @ 2.60 GHz 12線程;軟件環(huán)境:Windows 10平臺(tái)PyCharm 2021.3.3。CD*Lite算法在柵格地圖上進(jìn)行動(dòng)態(tài)路徑規(guī)劃結(jié)果驗(yàn)證。實(shí)驗(yàn)采用20×20、50×50、100×100三種規(guī)模柵格地圖,地圖內(nèi)黑色方塊為障礙地圖塊,白色方塊為目標(biāo)可行動(dòng)區(qū)域圖塊,黃色方塊為目標(biāo)行動(dòng)終止節(jié)點(diǎn)。為保證柵格地圖內(nèi)容隨機(jī)性,使用隨機(jī)算法對(duì)應(yīng)生成三種地圖節(jié)點(diǎn)集,如式 (20)～(21)所示:

ob=random.sample(range(0,a·b),c),

(20)

(21)

式中:ob為障礙節(jié)點(diǎn)集,a和b為生成柵格地圖的行高和列寬,c為障礙節(jié)點(diǎn)數(shù)目總和,fieldij為柵格地圖節(jié)點(diǎn)集。按照實(shí)驗(yàn)環(huán)境要求生成三種規(guī)模地圖節(jié)點(diǎn)集,為確保生成地圖普適性,設(shè)置障礙占比分別為 25.5%、40.16%、39.71%,如圖4所示。

(a) 20×20柵

(b) 50×50柵

(c) 100×100柵圖4 三種規(guī)模柵格地圖模型Fig.4 Three scale grid map models

在預(yù)規(guī)劃模式中,本文CD*Lite算法將與傳統(tǒng)路徑規(guī)劃算法LPA*、D*Lite進(jìn)行實(shí)驗(yàn)對(duì)比,參數(shù)設(shè)定單步移動(dòng)代價(jià)如表1所示。在動(dòng)態(tài)規(guī)劃模式中,首先基于單次動(dòng)態(tài)規(guī)劃,本文改進(jìn)的CD*Lite算法將與文獻(xiàn)[22]提出的改進(jìn)D*Lite算法從路線節(jié)點(diǎn)個(gè)數(shù)、路線長(zhǎng)度、運(yùn)行時(shí)間進(jìn)行對(duì)比。然后,進(jìn)行連續(xù)動(dòng)態(tài)規(guī)劃以驗(yàn)證本文改進(jìn)的CD*Lite算法。

表1 單步移動(dòng)代價(jià)Tab.1 Single step moving cost

4.2 預(yù)規(guī)劃實(shí)驗(yàn)結(jié)果分析

預(yù)規(guī)劃實(shí)驗(yàn)結(jié)果如圖5～圖7所示,分別為20×20、50×50、100×100三種規(guī)模下各算法所規(guī)劃的路徑,障礙占比分別為 25.5%、40.16%、39.71%,改進(jìn)效果對(duì)比如表2所示。

(a) LPA*20×20

(b) D*Lite20×20

(c) CD*Lite20×20圖5 三種算法在20×20規(guī)模下的預(yù)規(guī)劃路徑結(jié)果Fig.5 Three algorithms pre planning path results under 20×20 scales

(a) LPA*50×50

(b) D*Lite50×50

(c) CD*Lite50×50圖6 三種算法在50×50規(guī)模下的預(yù)規(guī)劃路徑結(jié)果Fig.6 Three algorithms pre planning path results under 50×50 scales

(a) LPA*100×100

(b) D*Lite100×100

(c) CD*Lite100×100圖7 三種算法在100×100規(guī)模下的預(yù)規(guī)劃路徑結(jié)果Fig.7 Three algorithms pre planning path results under 100×100 scales

表2 CD*Lite與LPA*、D*Lite實(shí)驗(yàn)結(jié)果數(shù)據(jù)對(duì)比Tab.2 Comparison of experimental results of CD*Lite in this paper,LPA* and D*Lite presented

CD*Lite算法在預(yù)規(guī)劃模式下不執(zhí)行冗余點(diǎn)刪除機(jī)制,在20×20、50×50和100×100的地圖中都能夠消除斜穿障礙邊界點(diǎn)的危險(xiǎn)行為。而傳統(tǒng)算法LPA*和D*Lite在路徑規(guī)劃時(shí)以最短距離為前提,不考慮實(shí)際障礙物的影響,因此會(huì)產(chǎn)生較多的危險(xiǎn)節(jié)點(diǎn)。本文算法通過(guò)引入方向緊急矩陣實(shí)現(xiàn)了目前的效果。

相比于LPA*和D*Lite算法,改進(jìn)的CD*Lite算法計(jì)算節(jié)點(diǎn)代價(jià)的次數(shù)減少約50%和30%,計(jì)算時(shí)間減少約50%和35%。這是因?yàn)楸疚奶岢龅娜騿l(fā)算子能夠更好地估計(jì)當(dāng)前節(jié)點(diǎn)的代價(jià)值,從而更有效地指導(dǎo)算法進(jìn)行全局路徑規(guī)劃。

改進(jìn)后的CD*Lite算法考慮到實(shí)際應(yīng)用中需要保持距離,因此在危險(xiǎn)節(jié)點(diǎn)處需要增加拐點(diǎn)。即使在20×20和50×50的地圖中,改進(jìn)后的算法仍能夠保持距離不增加。隨著地圖規(guī)模的增大,與LPA*和D*Lite算法相比,距離僅增加約10%。

動(dòng)態(tài)規(guī)劃定義為在預(yù)規(guī)劃路徑基礎(chǔ)上出現(xiàn)突變障礙之后繼續(xù)路徑規(guī)劃操作,根據(jù)實(shí)現(xiàn)模式可以分為單次動(dòng)態(tài)規(guī)劃與連續(xù)動(dòng)態(tài)規(guī)劃。單次動(dòng)態(tài)規(guī)劃如D*Lite,而本文所提出的CD*Lite可以連續(xù)進(jìn)行單次動(dòng)態(tài)規(guī)劃即連續(xù)動(dòng)態(tài)規(guī)劃。理論上可以連續(xù)進(jìn)行無(wú)窮次動(dòng)態(tài)規(guī)劃,為便于實(shí)驗(yàn)結(jié)果解析,本文設(shè)計(jì)的CD*Lite算法基于上述中的20×20規(guī)模柵圖選取連續(xù)3次動(dòng)態(tài)規(guī)劃結(jié)果如圖8所示。圖中黃色為移動(dòng)目標(biāo)搜索起點(diǎn),紅色為突變障礙節(jié)點(diǎn)。移動(dòng)目標(biāo)不斷更新snow,同時(shí)偵查全域既定路線上是否有突變障礙,當(dāng)運(yùn)動(dòng)到圖8(b)黃色節(jié)點(diǎn)時(shí)偵測(cè)到異常變化,初始化sstart=snow,slast=sstart并對(duì)既定路線進(jìn)行重構(gòu),目標(biāo)沿新路線移動(dòng)并更新snow,重復(fù)以上步驟的具體路線重構(gòu)結(jié)果如圖8(c)～(d)所示。

(a) 真實(shí)預(yù)規(guī)劃路線

(b) 突變1

(c) 突變2

(d) 突變3圖8 CD*Lite算法連續(xù)3次對(duì)突變障礙動(dòng)態(tài)路徑規(guī)劃Fig.8 CD*Lite algorithm performs dynamic path planning for abrupt obstacle nodes for three consecutive times

文獻(xiàn)[22]提出一種改進(jìn)D*Lite算法,通過(guò)引入安全系數(shù),使得危險(xiǎn)節(jié)點(diǎn)將不作為路徑的優(yōu)先選擇,能夠有效地避免斜穿過(guò)障礙物柵格頂點(diǎn)。本文將從路徑長(zhǎng)度、規(guī)劃曲線拐點(diǎn)個(gè)數(shù)、運(yùn)行時(shí)間三方面進(jìn)行對(duì)比以驗(yàn)證CD*Lite的連續(xù)動(dòng)態(tài)規(guī)劃模式的可行性,其具體實(shí)驗(yàn)結(jié)果如表3所示。

表3 CD*Lite與LPA*、D*Lite實(shí)驗(yàn)結(jié)果數(shù)據(jù)對(duì)比Tab.3 Comparison of experimental results and data between CD*Lite and LPA*,D*Lite

CD*Lite算法和文獻(xiàn)[22]在進(jìn)行首次動(dòng)態(tài)路徑規(guī)劃時(shí)的操作相同,都需要對(duì)所有相關(guān)節(jié)點(diǎn)keys進(jìn)行初始賦值。然而,CD*Lite算法與文獻(xiàn)[22]的不同之處在于CD*Lite會(huì)保留節(jié)點(diǎn)信息并進(jìn)行持續(xù)更新。

在持續(xù)既定路線節(jié)點(diǎn)變異3次后,文獻(xiàn)[22]算法沒(méi)有記憶能力,而CD*Lite算法相比文獻(xiàn)[22]算法的節(jié)點(diǎn)代價(jià)計(jì)算次數(shù)減少約80%,運(yùn)算時(shí)間減少約70%。這表明本文CD*Lite算法在面對(duì)連續(xù)既定路線突變障礙情況下能夠高效地動(dòng)態(tài)重構(gòu)路徑,展現(xiàn)了其可行性和高效性。

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

本文融入人工勢(shì)場(chǎng)的引力場(chǎng)思想提出了一種適用于連續(xù)既定路線突變障礙情況下的連續(xù)動(dòng)態(tài)路徑規(guī)劃CD*Lite算法,對(duì)原始切比雪夫代價(jià)估計(jì)函數(shù)進(jìn)行改進(jìn)區(qū)分行走代價(jià),使其更貼近真實(shí)仿真。引入引力算子提出了一種全域代價(jià)估計(jì)算子并改進(jìn)鍵值算子,能夠有效地對(duì)當(dāng)前規(guī)劃路線的節(jié)點(diǎn)進(jìn)行有效評(píng)估,提高了算法規(guī)劃效率。提出鄰域方向禁忌矩陣,有效避免產(chǎn)生危險(xiǎn)路線行為;引入冗余點(diǎn)刪除機(jī)制,使得規(guī)劃路徑更加平滑,使目標(biāo)移動(dòng)更加靈活;分別基于預(yù)規(guī)劃和連續(xù)動(dòng)態(tài)規(guī)劃模式下,對(duì)模擬柵格地圖進(jìn)行路徑規(guī)劃進(jìn)行仿真驗(yàn)證,成功驗(yàn)證了本文CD*Lite算法連續(xù)動(dòng)態(tài)路徑規(guī)劃模式的優(yōu)勢(shì)。