吳 瓊,袁 杰,馬圣山,郭振宇

(新疆大學(xué) 電氣工程學(xué)院,新疆 烏魯木齊 830017)

0 引言

路徑規(guī)劃[1-2]問題是機(jī)器人自主導(dǎo)航的關(guān)鍵環(huán)節(jié),根據(jù)不同的應(yīng)用環(huán)境,將路徑規(guī)劃算法分為全局靜態(tài)路徑規(guī)劃算法和局部動(dòng)態(tài)路徑規(guī)劃算法。全局靜態(tài)路徑規(guī)劃常用的算法有Dijkstra算法[3]、遺傳算法[4]、A*算法[5-6]、快速擴(kuò)展隨機(jī)樹算法[7]等;局部動(dòng)態(tài)路徑規(guī)劃常用的算法有強(qiáng)化學(xué)習(xí)[8]、動(dòng)態(tài)窗口法[9]、模糊控制法[10]、人工勢場法[11-12]等。

A*算法是目前普遍使用的全局靜態(tài)路徑規(guī)劃方法。為了解決傳統(tǒng)A*算法規(guī)劃的路徑存在的問題,國內(nèi)外學(xué)者對傳統(tǒng)A*算法從運(yùn)行時(shí)間、路徑平滑度、路徑安全性等進(jìn)行優(yōu)化,獲得了良好路徑規(guī)劃效果[13-15]。然而,當(dāng)路徑出現(xiàn)動(dòng)態(tài)障礙物時(shí),如果繼續(xù)按照預(yù)先規(guī)劃的全局路徑行走,則易發(fā)生碰撞,導(dǎo)致路徑規(guī)劃失敗,因此需要機(jī)器人有動(dòng)態(tài)避障的能力。而在動(dòng)態(tài)環(huán)境中,人工勢場法由于計(jì)算量小、反應(yīng)速度快,規(guī)劃路徑具有安全性、實(shí)時(shí)性[16-18],被廣泛應(yīng)用于動(dòng)態(tài)路徑規(guī)劃。ROSTAMI等[19]引入調(diào)節(jié)因子改進(jìn)勢場函數(shù),解決目標(biāo)不可達(dá)問題,但由于缺少全局路徑的指引導(dǎo)致遍歷空間過大,搜索時(shí)間過長;YAO等[20]引用強(qiáng)化學(xué)習(xí)來解決復(fù)雜環(huán)境問題,但需要大量訓(xùn)練且規(guī)劃的路徑存在振蕩;李二超等[21]提出預(yù)測碰撞思想,通過傳感器探測進(jìn)行提前預(yù)判,解決局部極小值問題,但未考慮動(dòng)態(tài)障礙物對路徑的影響;陳天德等[22]提出改進(jìn)型虛擬障礙物法,引入威脅區(qū)的概念確定虛擬障礙物位置,以解決局部極小值問題,但缺少全局信息,導(dǎo)致規(guī)劃路徑質(zhì)量不高。

針對以上兩種算法的不足,研究人員將上述兩種算法結(jié)合,王曉燕等[23]和陳繼清等[24]未充分考慮對臨時(shí)障礙物和動(dòng)態(tài)障礙物處理的情況,算法應(yīng)用受限;王洪斌等[25]研究指出當(dāng)目標(biāo)點(diǎn)附近存在障礙物時(shí),該算法將受到挑戰(zhàn)。因此,研究復(fù)雜動(dòng)態(tài)環(huán)境下的高效規(guī)劃路徑算法是目前該領(lǐng)域亟待解決的關(guān)鍵問題之一。本文提出一種具有啟發(fā)式動(dòng)態(tài)路徑規(guī)劃的安全點(diǎn)引導(dǎo)(Safe-point Guide,SPG)混合算法,將改進(jìn)的A*和人工勢場法結(jié)合,通過改進(jìn)A*算法對靜態(tài)環(huán)境預(yù)先規(guī)劃出全局靜態(tài)路徑來降低轉(zhuǎn)彎代價(jià)、減少轉(zhuǎn)折點(diǎn)數(shù)、平滑路徑,然后,采用全局路徑產(chǎn)生的安全點(diǎn)引導(dǎo)改進(jìn)人工勢場法完成局部路徑規(guī)劃,這里引入環(huán)境斥力模型來確定安全點(diǎn)位置。由此,規(guī)劃出一條具有動(dòng)態(tài)避障能力的全局實(shí)時(shí)最優(yōu)路徑。

1 改進(jìn)A*算法

傳統(tǒng)A*算法規(guī)劃出的路徑經(jīng)過多次轉(zhuǎn)折,其運(yùn)行時(shí)間長、路徑不平滑,不符合機(jī)器人的運(yùn)動(dòng)特性,對此本文提出終點(diǎn)逼近策略去除冗余節(jié)點(diǎn),引入三次B樣條曲線平滑路徑,并將改進(jìn)A*算法與其他算法進(jìn)行仿真對比。

1.1 終點(diǎn)逼近策略

本節(jié)采用終點(diǎn)逼近策略刪除冗余節(jié)點(diǎn)來優(yōu)化路徑,使機(jī)器人能夠更快到達(dá)目標(biāo)點(diǎn)。終點(diǎn)逼近策略的流程圖如圖1所示。

首先獲取A*算法初步規(guī)劃的所有全局路徑節(jié)點(diǎn)U{pi,1≤i≤n},p1為起始點(diǎn),pn為目標(biāo)點(diǎn),pa為當(dāng)前點(diǎn),pb為下一節(jié)點(diǎn)。令a=1,b=i,i為算法跨度值且i=20。從起始點(diǎn)p1依次連接p2,p3,…,pi,判斷p1到pi是否經(jīng)過障礙物,是則將pi加入集合L;如果不經(jīng)過障礙物,且i≠m,則說明pi不是目標(biāo)點(diǎn),繼續(xù)判斷pi到pm是否經(jīng)過障礙物,這里m為解算路徑點(diǎn)的個(gè)數(shù),m=5。以此類推直至目標(biāo)點(diǎn),最終輸出集合L,即為最優(yōu)路徑。

為驗(yàn)證終點(diǎn)逼近策略的有效性,與傳統(tǒng)A*算法進(jìn)行仿真對比實(shí)驗(yàn),得到的結(jié)果如圖2所示。

從圖2的規(guī)劃結(jié)果可知,采用終點(diǎn)逼近策略的A*算法規(guī)劃出的路徑長度和轉(zhuǎn)折次數(shù)明顯減少。表1所示為去除冗余節(jié)點(diǎn)前后性能對比表,可見去除冗余節(jié)點(diǎn)后,轉(zhuǎn)折次數(shù)減少50%,路徑長度縮短11%,運(yùn)行時(shí)間減小63%,改進(jìn)效果明顯。

表1 去冗余節(jié)點(diǎn)前后性能對比

1.2 路徑平滑策略

采用終點(diǎn)逼近策略的A*算法使機(jī)器人運(yùn)行時(shí)間減少,但在靜態(tài)障礙環(huán)境下,路徑的平滑性是機(jī)器人需要考慮的重要指標(biāo)。將折線型路徑優(yōu)化成弧線型更有利于機(jī)器人移動(dòng),而三次B樣條曲線具有連續(xù)性、幾何不變性、凸包性等優(yōu)點(diǎn),常被用來平滑曲線,因此本文引用三次B樣條曲線對路徑進(jìn)行平滑處理。

1.3 靜態(tài)環(huán)境下改進(jìn)A*算法的仿真對比

為檢驗(yàn)所提改進(jìn)A*算法的有效性,分別與傳統(tǒng)A*算法、Dijkstra算法、蟻群算法進(jìn)行仿真對比,得到的結(jié)果如圖3所示。

從圖3的規(guī)劃結(jié)果可見,改進(jìn)A*算法規(guī)劃出的路徑平滑、轉(zhuǎn)折點(diǎn)少。表2所示為4種算法性能指標(biāo)的對比,可見與傳統(tǒng)A*算法相比,改進(jìn)A*算法在轉(zhuǎn)折次數(shù)和運(yùn)行時(shí)間上分別減少了23%和63%,由于對路徑進(jìn)行了平滑處理,其在路徑長度方面僅減少了4%。與其他兩種算法相比,改進(jìn)A*算法在轉(zhuǎn)折次數(shù)、運(yùn)行時(shí)間、路徑長度都有明顯減少。綜合來看,改進(jìn)A*算法可以增加路徑平滑性,縮短運(yùn)行時(shí)間,減少轉(zhuǎn)折次數(shù)。

表2 4種算法性能指標(biāo)的對比

2 改進(jìn)人工勢場法

在動(dòng)態(tài)環(huán)境下,需要考慮突然加入的臨時(shí)靜態(tài)障礙物和動(dòng)態(tài)障礙物對路徑規(guī)劃的影響,要求機(jī)器人能夠局部實(shí)時(shí)避障,對此本文引入人工勢場法,針對傳統(tǒng)人工勢場法路徑振蕩問題設(shè)計(jì)平面向量積法過濾振蕩節(jié)點(diǎn),引入速度斥力場進(jìn)行實(shí)時(shí)避障。

2.1 平面向量積法過濾振蕩節(jié)點(diǎn)

傳統(tǒng)人工勢場法的路徑運(yùn)行速度、方向和步長與障礙物影響距離的設(shè)置有關(guān),當(dāng)與障礙物影響距離有關(guān)的參數(shù)值取值不當(dāng)時(shí),會(huì)使路徑點(diǎn)短暫逃離障礙物影響距離,導(dǎo)致路徑發(fā)生振蕩。為此,本文提出平面向量積法,以過濾振蕩點(diǎn),解決路徑振蕩問題。

pa,pd是振蕩的開始點(diǎn)和結(jié)束點(diǎn),設(shè)算法步長為l,k時(shí)刻的路徑點(diǎn)位置為pk,依次解算所有路徑節(jié)點(diǎn)。當(dāng)檢測到k時(shí)刻的路徑點(diǎn)與k-2時(shí)刻的路徑點(diǎn)距離ρ(pk,pk-2)

振蕩點(diǎn)有異側(cè)振蕩和同側(cè)振蕩兩種情況,判斷這兩種情況時(shí)首先需要判斷點(diǎn)的位置關(guān)系。圖4所示為振蕩點(diǎn)示意圖,圖中pa,pb,pc,pd分別為振蕩起始點(diǎn)、振蕩起始點(diǎn)相鄰節(jié)點(diǎn)、振蕩終止點(diǎn)相鄰節(jié)點(diǎn)、振蕩終止點(diǎn)。由坐標(biāo)原點(diǎn)連接pa,pb,pc,pd,分別構(gòu)成向量μ,α,β。

β與α的向量積為

(1)

(2)

(2)當(dāng)pb和pc位于β的同側(cè)時(shí),因?yàn)榭傮w路徑方向指向目標(biāo)點(diǎn),發(fā)生振蕩的點(diǎn)更靠近障礙物,所以只保留p1=pa和pn+1=pd,將其余振蕩點(diǎn)全部去除。

2.2 添加相對速度斥力勢場

第i個(gè)障礙物的斥力

(3)

式中:krepp為斥力場增益系數(shù);p為機(jī)器人當(dāng)前位置;pobi為第i障礙物位置;ρ(p,pobi)為機(jī)器人與障礙物的距離;ρo為障礙物的影響半徑。

為了完成機(jī)器人動(dòng)態(tài)避障,引入相對速度斥力場,如圖5所示,其中點(diǎn)Α、點(diǎn)O分別為機(jī)器人和障礙物的位置,x,y為機(jī)器人的橫、縱坐標(biāo)。

第i個(gè)障礙物速度斥力場函數(shù)為

Urepvi=krepvvor|sinθ|。

(4)

第i個(gè)障礙物速度斥力函數(shù)

(5)

相對速度斥力函數(shù)

(6)

機(jī)器人所受新的斥力

(7)

式中N為障礙物的個(gè)數(shù)。

3 安全點(diǎn)引導(dǎo)混合算法

3.1 安全點(diǎn)引導(dǎo)混合算法工作原理

改進(jìn)A*算法規(guī)劃出的全局路徑雖然平滑性增加,運(yùn)行時(shí)間減少,但是不能實(shí)時(shí)避開動(dòng)態(tài)障礙物,且規(guī)劃路徑與障礙物的距離過近,存在風(fēng)險(xiǎn)路徑;改進(jìn)人工勢場法雖然可以實(shí)時(shí)避障,但是容易陷入局部極小值陷阱。因此,結(jié)合兩種改進(jìn)算法的優(yōu)勢,提出具有啟發(fā)式特性的SPG混合算法。SPG混合算法分為兩步:①采用改進(jìn)A*算法規(guī)劃出全局靜態(tài)路徑;②采用安全點(diǎn)引導(dǎo)的啟發(fā)式策略使改進(jìn)人工勢場法進(jìn)行局部避障,同時(shí)使機(jī)器人保證安全距離,逃離局部極小值點(diǎn)。

3.2 安全點(diǎn)引導(dǎo)的啟發(fā)式策略

SPG的啟發(fā)式策略步驟如下:先判斷機(jī)器人是否陷入局部極小值點(diǎn),是則由改進(jìn)A*算法規(guī)劃路徑中的安全點(diǎn)產(chǎn)生引力使機(jī)器人逃離,安全點(diǎn)位置由環(huán)境斥力模型計(jì)算得出。

3.2.1 判斷是否陷入或逃出局部極小值陷阱

添加安全點(diǎn)流程圖如圖6所示,計(jì)算第k-1,k步節(jié)點(diǎn)的位置pk-1,pk,這兩點(diǎn)與目標(biāo)點(diǎn)的距離分別為ρ(pk-1,pg)和ρ(pk,pg)。當(dāng)ρ(pk,pg)>ρ(pk-1,pg)時(shí),令k=k+m,這里m為解算路徑點(diǎn)個(gè)數(shù),m=5,計(jì)算第k+m步與第k步兩點(diǎn)間的距離,若該距離ρ(pk+m,pk)≤d,d的取值很小,為步長的3倍,則判斷陷入局部極小值陷阱,添加安全目標(biāo)點(diǎn),令k=k+2m,計(jì)算第k+2m步與第k+m步兩點(diǎn)間的距離以及兩點(diǎn)到目標(biāo)點(diǎn)的距離;若ρ(pk+2m,pk+m)>d且ρ(pk+2m,pg)<ρ(pk+m,pg),則機(jī)器人已逃離局部極小值。

3.2.2 環(huán)境斥力模型描述

黑色柵格表示障礙物區(qū)域,不同顏色深度的藍(lán)色柵格表示障礙物產(chǎn)生的斥力場疊加形成的不同斥力區(qū)域,柵格顏色越深,其所受到的斥力越大。斥力疊加的原理為:對于幾個(gè)障礙物同時(shí)存在的情況,其斥力場互相疊加形成合斥力場,某個(gè)柵格的斥力大小等于各個(gè)障礙物單獨(dú)存在時(shí)在該柵格產(chǎn)生斥力的代數(shù)和。柵格附近的障礙物越密集,該柵格受到的合斥力越大,且障礙物向外輻射斥力依次遞減,最內(nèi)側(cè)柵格所受斥力大小為2,最外側(cè)為1,障礙物柵格斥力場如圖7所示。

3.2.3 添加安全點(diǎn)

(1)遍歷從局部極小點(diǎn)到目標(biāo)點(diǎn)的所有路徑點(diǎn),記為U,將該段路徑劃分為M段子路徑,一般M=U～1.5U。

(2)計(jì)算子路徑上路徑點(diǎn)的斥力。選取斥力值最小的點(diǎn)做為安全點(diǎn)來引導(dǎo)人工勢場法,以消除局部極小值的影響。圖8所示為SPG混合算法的路徑圖。

圖中黑色方塊為柵格地圖中的障礙物,平滑曲線表示改進(jìn)A*算法規(guī)劃出的全局路徑,虛線表示改進(jìn)人工勢場法的實(shí)際路線。當(dāng)周圍障礙物增多時(shí),斥力場變復(fù)雜,可能會(huì)使機(jī)器人陷入局部極小點(diǎn)陷阱,無法移動(dòng)。為逃離局部極小值點(diǎn),首先找到全局路徑中的路徑點(diǎn),根據(jù)環(huán)境斥力模型的斥力疊加原理計(jì)算斥力最小的路徑點(diǎn),并將其設(shè)為安全點(diǎn),然后由安全點(diǎn)產(chǎn)生的引力場引導(dǎo)機(jī)器人逃離局部極小點(diǎn)。由圖8可知,SPG混合算法規(guī)劃出的路徑既能保證安全距離,也能使機(jī)器人逃離局部極小值點(diǎn)。

4 安全點(diǎn)引導(dǎo)混合算法仿真與實(shí)驗(yàn)分析

4.1 仿真實(shí)驗(yàn)

為了說明本文混合算法在復(fù)雜環(huán)境下路徑規(guī)劃的質(zhì)量,在MATLAB2019a,CPU為Intel Core i7-8700K @3.70 GHz,內(nèi)存為16 G的環(huán)境下對所提算法進(jìn)行仿真驗(yàn)證。設(shè)改進(jìn)勢場法初始參數(shù)引力場常量katt=100、斥力場常量krep=1、速度斥力常量krepv=1、斥力影響范圍ρ0=0.3 m、步長l=0.1 m。

4.1.1 靜態(tài)環(huán)境下混合算法的仿真對比

圖9所示為簡單環(huán)境下SPG混合算法與其他算法的路徑對比。

由圖9可見,傳統(tǒng)人工勢場法路徑更長,傳統(tǒng)混合算法偏離全局路徑(虛線)較遠(yuǎn),在保障移動(dòng)安全的情況下,SPG混合算法比傳統(tǒng)混合算法更加貼合全局路徑。表3所示為3種算法性能對比,可見與傳統(tǒng)人工勢場法和傳統(tǒng)混合算法相比,SPG混合算法的路徑長度分別縮短15%和10%,運(yùn)行時(shí)間分別縮短34.4%和25.6%,擴(kuò)展節(jié)點(diǎn)個(gè)數(shù)減少31.3%,說明本文所提算法在路徑長度和運(yùn)行時(shí)間上得到了較大提升。

表3 3種算法性能對比

4.1.2 動(dòng)態(tài)環(huán)境下混合算法的仿真結(jié)果

本文將機(jī)器人的移動(dòng)速度定義為Δt內(nèi)移動(dòng)的步長,Δt=0.05 s,速度為2 m/s。圖10所示為兩個(gè)動(dòng)態(tài)障礙物下的算法對比,圖中藍(lán)色柵格為動(dòng)態(tài)障礙,沿箭頭方向作速度為2 m/s的勻速運(yùn)動(dòng)。

(1)含有兩個(gè)動(dòng)態(tài)障礙物

由圖10可見,傳統(tǒng)混合算法和SPG混合算法均能躲避動(dòng)態(tài)障礙物,傳統(tǒng)混合算法偏離全局路徑(虛線)較遠(yuǎn),而在保障移動(dòng)安全的情況下,SPG混合算法比傳統(tǒng)混合算法更加貼合全局路徑。表4所示為兩種算法性能對比,可見與傳統(tǒng)混合算法相比,SPG混合算法的路徑長度縮短9.5%,運(yùn)行時(shí)間縮短30.9%,擴(kuò)展節(jié)點(diǎn)個(gè)數(shù)減少35.3%,說明SPG混合算法規(guī)劃的路徑更優(yōu)。

表4 兩種算法性能對比

為了對比兩種算法的速度實(shí)時(shí)性和平穩(wěn)性,計(jì)算兩種算法在各個(gè)時(shí)間點(diǎn)的速度。圖11所示為兩種算法的速度曲線,表5所示為兩種算法的速度隨時(shí)間變化的對比,可見傳統(tǒng)算法和本文算法分別在1.2 s和5.8 s遇到動(dòng)態(tài)障礙物,1.3 s～1.7 s和5.9 s～6.8 s時(shí)間段內(nèi)做出反應(yīng),規(guī)劃時(shí)間分別為0.4 s和0.9 s,反應(yīng)時(shí)間為0.1 s,比較滯后,在3 s和4.5 s通過過道,在3 s～3.3 s和4.5 s～4.8 s時(shí)間段做出反應(yīng),移動(dòng)機(jī)器人多次加速、減速,速度曲線振蕩劇烈,導(dǎo)致移動(dòng)機(jī)器人運(yùn)行不穩(wěn)定,且規(guī)劃時(shí)間較長,效率不高。本文算法分別在1.1 s和4.2 s遇到動(dòng)態(tài)障礙物,1.1 s～1.4 s和4.2 s～4.7 s時(shí)間段內(nèi)做出反應(yīng),規(guī)劃時(shí)間分別為0.3 s和0.5 s,反應(yīng)時(shí)間為0 s,反應(yīng)迅速,在2.4 s和3 s通過過道,在2.4 s～2.8 s和3 s～3.2 s時(shí)間段做出反應(yīng),規(guī)劃時(shí)間較傳統(tǒng)混合算法下降44%且速度曲線平滑,移動(dòng)機(jī)器人運(yùn)行平穩(wěn),規(guī)劃時(shí)間較短。因此,本文算法可解決移動(dòng)機(jī)器人減速導(dǎo)致的運(yùn)行不平穩(wěn)、速度振蕩等問題,提高移動(dòng)機(jī)器人的運(yùn)行效率。

表5 兩種算法速度變化時(shí)間對比

(2)加入臨時(shí)靜態(tài)障礙物

圖12所示為加入臨時(shí)靜態(tài)障礙物的路徑對比,紅色柵格為臨時(shí)靜態(tài)障礙物。

由圖12可見,傳統(tǒng)混合算法規(guī)劃路徑不平滑,極易陷入局部極小值且未能到達(dá)終點(diǎn)。SPG混合算法路徑平滑,既可順利躲避動(dòng)態(tài)障礙物和臨時(shí)障礙物,又可在躲避障礙物之后繼續(xù)貼合全局路徑(虛線)移動(dòng)。因此,在保障移動(dòng)安全的情況下,SPG混合算法更具有實(shí)時(shí)性和安全性。

上面仿真實(shí)驗(yàn)只驗(yàn)證了SPG混合算法對簡單動(dòng)態(tài)環(huán)境的避障能力,為了更充分地展現(xiàn)其良好的避障能力,在有4個(gè)動(dòng)態(tài)障礙物及加入臨時(shí)靜態(tài)障礙物的環(huán)境下檢驗(yàn)規(guī)劃效果,而且這些障礙物均設(shè)置在SPG混合算法經(jīng)過的路徑中,如圖13和圖14所示。圖中左側(cè)為到達(dá)終點(diǎn)的整體路徑規(guī)劃圖,右側(cè)分別為躲避4個(gè)障礙物的實(shí)時(shí)圖。黃色數(shù)字表示動(dòng)態(tài)障礙物的序號,黑色箭頭為動(dòng)態(tài)障礙物的運(yùn)動(dòng)方向。

由圖13和圖14的規(guī)劃結(jié)果可知,即使在加入多個(gè)動(dòng)態(tài)障礙物及臨時(shí)靜態(tài)障礙物的環(huán)境下,SPG混合算法也能順利躲避障礙物,然后繼續(xù)貼合全局路徑(虛線)移動(dòng),有良好的避障性能,同時(shí)也和障礙物留保持一定距離,避免真實(shí)環(huán)境中機(jī)器人和障礙物發(fā)生碰撞。因此,SPG混合算法規(guī)劃的路徑具有一定的平滑性、實(shí)時(shí)性和安全性。

4.2 實(shí)驗(yàn)驗(yàn)證及分析

為了在實(shí)際環(huán)境中驗(yàn)證本文SPG混合算法的有效性,實(shí)驗(yàn)選用搭載機(jī)器人操作系統(tǒng)(Robot Operating System,ROS)的移動(dòng)機(jī)器人,所搭建的實(shí)驗(yàn)環(huán)境是新疆大學(xué)博達(dá)校區(qū)電氣工程學(xué)院505實(shí)驗(yàn)室。實(shí)驗(yàn)環(huán)境如圖15左圖所示,右圖為使用激光雷達(dá)建立的在Rviz可視化界面內(nèi)顯示的環(huán)境模型。境中的試驗(yàn)臺(tái)和桌椅板凳均為靜態(tài)障礙物,實(shí)驗(yàn)環(huán)臺(tái)的順序從左往右分別記為1號、2號、3號、4號實(shí)驗(yàn)臺(tái),1號實(shí)驗(yàn)臺(tái)左側(cè)為起始位置,4號實(shí)驗(yàn)臺(tái)右側(cè)為終止位置。

圖16所示為動(dòng)態(tài)環(huán)境下的真實(shí)場景和Rviz界面。圖中黃色框?yàn)榻K點(diǎn)位置,藍(lán)色框?yàn)榕R時(shí)靜態(tài)障礙物位置,橙色框?yàn)閯?dòng)態(tài)障礙物位置。在實(shí)際環(huán)境中,動(dòng)態(tài)障礙物由TurtleBot2移動(dòng)機(jī)器人和紙箱組成,為將動(dòng)態(tài)障礙物信息直觀顯現(xiàn)在地圖上,加入局部地圖,如圖16中以機(jī)器人為中心的白色區(qū)域所示。

選取機(jī)器人在4個(gè)不同關(guān)鍵位置處的規(guī)劃路徑,如圖16a～圖16d所示。時(shí)刻1機(jī)器人所處的環(huán)境中并未檢測到動(dòng)態(tài)障礙物。時(shí)刻2機(jī)器人上的激光雷達(dá)刷新周圍環(huán)境中的信息,檢測到動(dòng)態(tài)障礙物或臨時(shí)靜態(tài)障礙物,為了躲避這些障礙物,機(jī)器人會(huì)修正原先規(guī)劃出的路徑。由時(shí)刻2的狀態(tài)可見,機(jī)器人重新規(guī)劃的新路線可以及時(shí)躲避障礙物,避免發(fā)生碰撞。在躲避動(dòng)態(tài)障礙物后,機(jī)器人在時(shí)刻3又針對臨時(shí)靜態(tài)障礙物規(guī)劃了路線。時(shí)刻4的狀態(tài)說明機(jī)器人在躲避障礙物后,仍會(huì)貼合之前規(guī)劃出的全局路徑。

5 結(jié)束語

針對傳統(tǒng)A*算法和人工勢場法規(guī)劃出的路徑存在冗余節(jié)點(diǎn)多、路徑不平滑、振蕩等問題,提出具有啟發(fā)特性的SPG混合算法。本文對傳統(tǒng)A*算法和人工勢場法進(jìn)行了改進(jìn)和優(yōu)化。通過設(shè)計(jì)SPG的啟發(fā)式策略,結(jié)合兩種算法的優(yōu)勢,彌補(bǔ)了改進(jìn)的A*算法和人工勢場法在實(shí)時(shí)規(guī)劃方面的不足,能在躲避臨時(shí)和移動(dòng)障礙物時(shí)規(guī)劃出安全路徑。通過仿真實(shí)驗(yàn)對比分析得出,與傳統(tǒng)混合算法相比,SPG混合算法在靜態(tài)環(huán)境下的路徑長度縮短了10%,運(yùn)行時(shí)間縮短了25.6%;在動(dòng)態(tài)環(huán)境下的路徑長度縮短了9.5%,運(yùn)行時(shí)間縮短了30.9%。最后,通過在真實(shí)環(huán)境進(jìn)行實(shí)驗(yàn),驗(yàn)證了所提SPG混合算法的有效性。