張國(guó)勝，李彩虹，張耀玉，李永迪，周瑞紅

(山東理工大學(xué) 計(jì)算機(jī)科學(xué)與技術(shù)學(xué)院，山東 淄博 255049)

路徑規(guī)劃技術(shù)作為實(shí)現(xiàn)移動(dòng)機(jī)器人自主導(dǎo)航的核心技術(shù)受到廣泛關(guān)注,其目的是使移動(dòng)機(jī)器人按照某些特定的評(píng)價(jià)標(biāo)準(zhǔn)規(guī)劃出一條從起點(diǎn)到目標(biāo)點(diǎn)的最優(yōu)或次優(yōu)的避障路徑[1]。根據(jù)對(duì)工作環(huán)境信息的理解層次,路徑規(guī)劃可分為全局路徑規(guī)劃和局部路徑規(guī)劃[2]。全局路徑規(guī)劃應(yīng)用在工作環(huán)境已知的場(chǎng)景下,是一種離線規(guī)劃方法,實(shí)時(shí)性和靈活性較差,可以規(guī)劃出全局最優(yōu)或次優(yōu)的路徑。局部路徑規(guī)劃應(yīng)用在工作環(huán)境未知或部分已知的場(chǎng)景下,移動(dòng)機(jī)器人根據(jù)局部環(huán)境信息進(jìn)行在線路徑規(guī)劃,具有較高的實(shí)時(shí)性和靈活性;由于缺少全局環(huán)境信息,傳感器的探測(cè)范圍有限,所以規(guī)劃路徑一般是局部最優(yōu)。常見(jiàn)的路徑規(guī)劃方法有A*算法[3]、蟻群算法[4]、動(dòng)態(tài)窗口法[5-6]、人工勢(shì)場(chǎng)法[7-8]和模糊控制法[9]等。其中,人工勢(shì)場(chǎng)法由于結(jié)構(gòu)簡(jiǎn)單、計(jì)算量小、實(shí)時(shí)性好且路徑較為平滑等優(yōu)點(diǎn)得到了廣泛應(yīng)用,但是存在局部極小值以及目標(biāo)不可達(dá)等問(wèn)題。

針對(duì)傳統(tǒng)人工勢(shì)場(chǎng)法存在的局部極小值和目標(biāo)不可達(dá)等問(wèn)題,國(guó)內(nèi)外學(xué)者做了大量的研究工作。林潔等[10]提出沿邊走的詳細(xì)策略來(lái)解決局部極小點(diǎn),但在陷入型障礙物以及復(fù)雜障礙物環(huán)境下會(huì)增加路徑長(zhǎng)度;黃開(kāi)啟等[11]設(shè)置模糊控制模塊為移動(dòng)機(jī)器人提供輔助作用力以擺脫局部極小值點(diǎn),但在虛擬力引導(dǎo)過(guò)程中路徑容易出現(xiàn)連續(xù)振蕩;Fan等[12]將人工勢(shì)場(chǎng)法與正六邊形引導(dǎo)方法相結(jié)合,通過(guò)繞行障礙物克服局部極小值,但在復(fù)雜障礙物環(huán)境中無(wú)疑加大了計(jì)算時(shí)間和耗能;Zhou等[13]提出在人工勢(shì)場(chǎng)法中構(gòu)建基于障礙物的切向量作為避障過(guò)程的虛擬力,使移動(dòng)機(jī)器人逃脫局部極小值點(diǎn),但未有效解決引力過(guò)大和障礙物環(huán)境誤判問(wèn)題;趙炳巍等[14]通過(guò)模擬退火算法,在移動(dòng)機(jī)器人關(guān)于障礙物右對(duì)稱(chēng)點(diǎn)區(qū)域設(shè)置隨機(jī)目標(biāo)以擺脫局部極小值陷阱,但未充分考慮當(dāng)前障礙物信息,算法效率無(wú)法保證;羅強(qiáng)等[15]采用切線法以及搜索法得到最優(yōu)逃逸力,移動(dòng)機(jī)器人在新的合力作用下擺脫局部極小值點(diǎn),但應(yīng)用場(chǎng)景簡(jiǎn)單,面對(duì)復(fù)雜障礙物環(huán)境時(shí)無(wú)法有效逃出陷阱區(qū)域。

針對(duì)上述算法的不足,本文在已改進(jìn)勢(shì)場(chǎng)函數(shù)的基礎(chǔ)上,設(shè)計(jì)自適應(yīng)速度調(diào)節(jié)機(jī)制,提高路徑規(guī)劃效率。針對(duì)局部極小值問(wèn)題,分別提出了APF-v1算法和APF-v2算法。通過(guò)對(duì)當(dāng)前障礙物環(huán)境判斷,兩種算法采取不同的虛擬目標(biāo)點(diǎn)設(shè)置策略打破受力平衡狀態(tài),引導(dǎo)移動(dòng)機(jī)器人擺脫局部極小值陷阱。最后利用ROS機(jī)器人操作系統(tǒng)對(duì)APF-v1和APF-v2算法進(jìn)行可行性的驗(yàn)證。

1 傳統(tǒng)人工勢(shì)場(chǎng)法

人工勢(shì)場(chǎng)法是由Khatib于1986年提出的一種虛擬力法[16],其基本思想是在移動(dòng)機(jī)器人的工作環(huán)境中構(gòu)造一個(gè)虛擬力場(chǎng),目標(biāo)點(diǎn)產(chǎn)生引力勢(shì)場(chǎng),障礙物產(chǎn)生斥力勢(shì)場(chǎng),移動(dòng)機(jī)器人在引力場(chǎng)和斥力場(chǎng)的共同作用下向目標(biāo)點(diǎn)移動(dòng)。

人工勢(shì)場(chǎng)的勢(shì)場(chǎng)函數(shù)U(X)定義:

U(X)=Uatt(X)+Urep(X),

(1)

式中:Uatt(X)為目標(biāo)點(diǎn)產(chǎn)生的引力勢(shì)場(chǎng);Urep(X)為障礙物產(chǎn)生的斥力勢(shì)場(chǎng);X為移動(dòng)機(jī)器人的當(dāng)前位置。

人工勢(shì)場(chǎng)中機(jī)器人所受的力F(X):

F(X)=Fatt(X)+Frep(X),

(2)

式中:Fatt(X)為目標(biāo)點(diǎn)對(duì)移動(dòng)機(jī)器人的引力;Frep(X)為障礙物對(duì)移動(dòng)機(jī)器人的斥力。

1.1 引力場(chǎng)

引力場(chǎng)是全局勢(shì)場(chǎng),引力勢(shì)能與移動(dòng)機(jī)器人和目標(biāo)點(diǎn)之間距離成正比,距離越大,移動(dòng)機(jī)器人所受的引力勢(shì)能越大。引力勢(shì)場(chǎng)函數(shù)定義:

(3)

式中:ε為引力增益系數(shù);Xg為目標(biāo)點(diǎn)的位置;ρ(X,Xg)為移動(dòng)機(jī)器人到目標(biāo)點(diǎn)的距離。

目標(biāo)點(diǎn)對(duì)移動(dòng)機(jī)器人的引力為引力勢(shì)場(chǎng)函數(shù)的負(fù)梯度:

Fatt(X)=-?Uatt(X)=-ε(X-Xg)。

(4)

1.2 斥力場(chǎng)

斥力場(chǎng)是局部勢(shì)場(chǎng),在障礙物影響范圍內(nèi),斥力勢(shì)能與移動(dòng)機(jī)器人和障礙物之間的距離成反比,距離越大,所受的斥力勢(shì)能越小,在障礙物影響范圍外,移動(dòng)機(jī)器人所受的斥力勢(shì)能為零。斥力勢(shì)場(chǎng)函數(shù)定義:

0,ρ(X,Xobs)>ρ0,

(5)

式中:η為斥力增益系數(shù);Xobs為障礙物的位置;ρ(X,Xobs)為移動(dòng)機(jī)器人到障礙物的距離;ρ0為障礙物的影響范圍。

障礙物對(duì)移動(dòng)機(jī)器人的斥力為斥力勢(shì)場(chǎng)函數(shù)的負(fù)梯度:

Frep(X)=

0,ρ(X,Xobs)>ρ0。

(6)

1.3 合力

人工勢(shì)場(chǎng)為引力場(chǎng)和斥力場(chǎng)的疊加,移動(dòng)機(jī)器人所受的合力為引力與斥力的矢量和,合力的方向即為移動(dòng)機(jī)器人運(yùn)動(dòng)的方向。移動(dòng)機(jī)器人受力分析如圖1所示。

圖1 移動(dòng)機(jī)器人受力分析

移動(dòng)機(jī)器人在合力的作用下向目標(biāo)點(diǎn)移動(dòng),但是存在以下問(wèn)題:

1) 引力過(guò)大問(wèn)題。當(dāng)移動(dòng)機(jī)器人與目標(biāo)點(diǎn)之間的距離過(guò)大時(shí),其所受的引力將遠(yuǎn)大于斥力,在移動(dòng)過(guò)程中可能會(huì)出現(xiàn)碰撞障礙物的情況。

2) 目標(biāo)不可達(dá)問(wèn)題。當(dāng)目標(biāo)點(diǎn)位于障礙物附近時(shí),隨著移動(dòng)機(jī)器人不斷向目標(biāo)點(diǎn)靠近,其所受的引力逐漸減小,斥力逐漸增大,斥力過(guò)大,引力相對(duì)較小,導(dǎo)致移動(dòng)機(jī)器在某一區(qū)域徘徊或靜止。

3) 局部極小值問(wèn)題。當(dāng)移動(dòng)機(jī)器人運(yùn)動(dòng)到某一非目標(biāo)點(diǎn)位置時(shí),其所受合力為零或引力與斥力方向相反而陷入局部極小值陷阱,移動(dòng)機(jī)器人無(wú)法確定下一步的運(yùn)動(dòng)方向,導(dǎo)致其在局部極小值點(diǎn)附近反復(fù)震蕩或靜止。

2 改進(jìn)的人工勢(shì)場(chǎng)法

2.1 改進(jìn)的引力勢(shì)場(chǎng)

針對(duì)移動(dòng)機(jī)器人距離目標(biāo)點(diǎn)過(guò)遠(yuǎn)導(dǎo)致的引力過(guò)大問(wèn)題,本文對(duì)引力勢(shì)場(chǎng)函數(shù)進(jìn)行修正,設(shè)置引力作用閾值將其分段,當(dāng)距離大于此閾值時(shí),采用修正的引力勢(shì)場(chǎng)函數(shù),避免引力過(guò)大問(wèn)題。修正的引力勢(shì)場(chǎng)函數(shù):

Uatt(X)=

(7)

式中dg為引力作用閾值。

目標(biāo)點(diǎn)對(duì)移動(dòng)機(jī)器人的引力為引力勢(shì)場(chǎng)函數(shù)的負(fù)梯度:

Fatt(X)=

(8)

2.2 改進(jìn)的斥力勢(shì)場(chǎng)

針對(duì)目標(biāo)不可達(dá)問(wèn)題,本文引入一種改進(jìn)的斥力勢(shì)場(chǎng)函數(shù),相較于傳統(tǒng)的斥力勢(shì)場(chǎng)函數(shù),改進(jìn)的斥力勢(shì)場(chǎng)函數(shù)加入了目標(biāo)點(diǎn)和移動(dòng)機(jī)器人之間的距離,保證僅目標(biāo)點(diǎn)處的勢(shì)場(chǎng)為全局最小。改進(jìn)的斥力勢(shì)場(chǎng)函數(shù):

Urep(X)=

(9)

斥力為改進(jìn)的斥力勢(shì)場(chǎng)函數(shù)的負(fù)梯度:

Frep(X)=

(10)

式中Frep1(X)和Frep2(X)分別為斥力Frep(X)在兩個(gè)不同方向上的分力。矢量Frep1(X)的方向由障礙物指向移動(dòng)機(jī)器人,為斥力分量;矢量Frep2(X)的方向由移動(dòng)機(jī)器人指向目標(biāo)點(diǎn),為引力分量。

(11)

(12)

改進(jìn)勢(shì)場(chǎng)函數(shù)后移動(dòng)機(jī)器人受力分析如圖2所示。

圖2 改進(jìn)勢(shì)場(chǎng)函數(shù)后移動(dòng)機(jī)器人受力分析

2.3 自適應(yīng)速度調(diào)節(jié)

傳統(tǒng)人工勢(shì)場(chǎng)法進(jìn)行路徑規(guī)劃任務(wù)時(shí)線速度采用固定值。結(jié)合實(shí)際工作經(jīng)驗(yàn),根據(jù)移動(dòng)機(jī)器人工作環(huán)境的復(fù)雜程度,提出了一種自適應(yīng)速度調(diào)節(jié)機(jī)制,以降低發(fā)生碰撞的概率。

移動(dòng)機(jī)器人線速度v的取值取決于當(dāng)前所處的環(huán)境。移動(dòng)機(jī)器人通過(guò)自身搭載的傳感器獲取周?chē)h(huán)境信息,通過(guò)傳感器返回的數(shù)據(jù)得到周?chē)系K物的數(shù)量obs_count以及移動(dòng)機(jī)器人與障礙物之間的距離dis_r_o,由此判斷當(dāng)前工作環(huán)境的復(fù)雜程度。當(dāng)移動(dòng)機(jī)器人處于簡(jiǎn)單環(huán)境時(shí),可適當(dāng)增大線速度,提高路徑規(guī)劃效率;當(dāng)移動(dòng)機(jī)器人處于復(fù)雜環(huán)境時(shí),適當(dāng)減小線速度,降低發(fā)生碰撞的概率;當(dāng)移動(dòng)機(jī)器人陷入局部極小值陷阱時(shí),則需進(jìn)一步降低移動(dòng)機(jī)器人運(yùn)動(dòng)速度,防止在虛擬目標(biāo)點(diǎn)引導(dǎo)過(guò)程中出現(xiàn)震蕩。線速度v取值如下:

(13)

式中:Nmax和Nmin分別為障礙物數(shù)量最大預(yù)設(shè)值和最小預(yù)設(shè)值;Dmax和Dmin分別為移動(dòng)機(jī)器人與障礙物之間距離最大預(yù)設(shè)值和最小預(yù)設(shè)值;vmax、vmid、vmin和vmmin分別表示線速度取得的較大、中等、較小和最小值。

2.4 虛擬目標(biāo)點(diǎn)法

移動(dòng)機(jī)器人在未知環(huán)境中依靠局部地圖信息進(jìn)行路徑規(guī)劃,當(dāng)遇到局部極小值問(wèn)題時(shí),僅靠對(duì)勢(shì)場(chǎng)函數(shù)進(jìn)行改進(jìn)無(wú)法有效解決。典型的局部極小值情況如圖3所示。當(dāng)移動(dòng)機(jī)器人處于障礙物影響范圍邊緣,其所受到的斥力較小,合力方向由移動(dòng)機(jī)器人指向目標(biāo)點(diǎn);當(dāng)靠近障礙物時(shí),其所受到的斥力大于引力,合力方向反轉(zhuǎn),移動(dòng)機(jī)器人陷入局部極小值陷阱,表現(xiàn)為在該區(qū)域反復(fù)震蕩或靜止。

圖3 局部極小值情況

局部極小值問(wèn)題是由移動(dòng)機(jī)器人所受合力為零達(dá)到平衡狀態(tài)所致,本文分別提出APF-v1和APF-v2兩種設(shè)置虛擬目標(biāo)點(diǎn)的方法打破該平衡狀態(tài),引導(dǎo)移動(dòng)機(jī)器人擺脫局部極小值陷阱。在設(shè)置虛擬目標(biāo)點(diǎn)時(shí),兩種算法都需要對(duì)周邊的障礙物環(huán)境進(jìn)行檢測(cè),選擇障礙物數(shù)量少的區(qū)域設(shè)置虛擬目標(biāo)點(diǎn)。其中,APF-v1算法在斥力分量偏轉(zhuǎn)90°方向的合適位置設(shè)置虛擬目標(biāo)點(diǎn),APF-v2算法則將虛擬目標(biāo)點(diǎn)設(shè)置在目標(biāo)障礙物外側(cè)的合適位置。

2.4.1APF-v1

移動(dòng)機(jī)器人的工作環(huán)境復(fù)雜多樣,虛擬目標(biāo)點(diǎn)的設(shè)置位置也直接影響規(guī)劃路徑的優(yōu)劣,在設(shè)置虛擬目標(biāo)點(diǎn)時(shí)應(yīng)充分利用傳感器感知到的環(huán)境信息。當(dāng)系統(tǒng)檢測(cè)到移動(dòng)機(jī)器人陷入局部極小值陷阱時(shí),通過(guò)直線連接移動(dòng)機(jī)器人與目標(biāo)點(diǎn),該直線將移動(dòng)機(jī)器人的工作環(huán)境分為兩部分,分別計(jì)算兩區(qū)域障礙物的數(shù)量,虛擬目標(biāo)點(diǎn)設(shè)置在障礙物數(shù)量少的區(qū)域。

移動(dòng)機(jī)器人通過(guò)傳感器反饋的數(shù)據(jù)計(jì)算得到各個(gè)力的大小和方向。以移動(dòng)機(jī)器人為圓心沿斥力分量Frep1向虛擬目標(biāo)點(diǎn)設(shè)置區(qū)域偏轉(zhuǎn)90°,沿此方向選取合適位置設(shè)置虛擬目標(biāo)點(diǎn),期間忽略原目標(biāo)點(diǎn)的引力。為避免忽略障礙物斥力所導(dǎo)致移動(dòng)機(jī)器人與障礙物發(fā)生碰撞的問(wèn)題,需保留障礙物產(chǎn)生的斥力。移動(dòng)機(jī)器人在虛擬目標(biāo)點(diǎn)產(chǎn)生的引力和障礙物產(chǎn)生的斥力共同作用下逃離局部極小值點(diǎn)。生成虛擬目標(biāo)點(diǎn)的位置(xvir,yvir):

(14)

式中:(xcur,ycur)為移動(dòng)機(jī)器人的當(dāng)前位置;ρvir為設(shè)置虛擬目標(biāo)點(diǎn)的距離;θ(Frep1)為斥力分量Frep1相對(duì)地圖坐標(biāo)系的角度;Ovir為生成虛擬目標(biāo)點(diǎn)的方向,Ovir為1表示虛擬目標(biāo)點(diǎn)生成在移動(dòng)機(jī)器人的左側(cè)區(qū)域,Ovir為-1表示虛擬目標(biāo)點(diǎn)生成在移動(dòng)機(jī)器人的右側(cè)區(qū)域。APF-v1算法設(shè)置虛擬目標(biāo)點(diǎn)后的移動(dòng)機(jī)器人受力分析如圖4所示。

圖4 APF-v1移動(dòng)機(jī)器人受力分析

2.4.2 APF-v2

APF-v1算法分析當(dāng)前受力情況,通過(guò)在斥力分量Frep1偏轉(zhuǎn)90°方向上選擇合適距離設(shè)置虛擬目標(biāo)點(diǎn),APF-v2算法則根據(jù)目標(biāo)障礙物分布情況設(shè)置虛擬目標(biāo)點(diǎn)。

當(dāng)系統(tǒng)檢測(cè)到移動(dòng)機(jī)器人陷入局部極小值陷阱時(shí),設(shè)置障礙物存儲(chǔ)模塊,記錄使其陷入局部極小值陷阱的障礙物位置信息,形成局部極小值障礙物組,在系統(tǒng)判斷的虛擬目標(biāo)點(diǎn)設(shè)置區(qū)域選擇障礙物組中最外側(cè)的為目標(biāo)障礙物,虛擬目標(biāo)點(diǎn)設(shè)置在移動(dòng)機(jī)器人當(dāng)前位置相對(duì)于目標(biāo)障礙物的對(duì)稱(chēng)點(diǎn)附近范圍內(nèi)。忽略原目標(biāo)點(diǎn)引力,移動(dòng)機(jī)器人在虛擬目標(biāo)點(diǎn)產(chǎn)生的引力和障礙物產(chǎn)生的斥力共同作用下擺脫局部極小值點(diǎn)。生成的虛擬目標(biāo)點(diǎn)位置(xvir,yvir):

(15)

式中:(xobs,yobs)為目標(biāo)障礙物的位置;β1和β2為距離調(diào)節(jié)參數(shù),取值為正。APF-v2算法設(shè)置虛擬目標(biāo)點(diǎn)后的移動(dòng)機(jī)器人受力分析如圖5所示。

圖5 APF-v2移動(dòng)機(jī)器人受力分析

虛擬目標(biāo)點(diǎn)采用一步一設(shè)的原則。移動(dòng)機(jī)器人在擺脫局部極小值陷阱的過(guò)程中可能會(huì)設(shè)置多個(gè)虛擬目標(biāo)點(diǎn),直至脫離局部極小值點(diǎn)的影響范圍。移動(dòng)機(jī)器人擺脫局部極小值陷阱后撤銷(xiāo)虛擬目標(biāo)點(diǎn),在原目標(biāo)點(diǎn)和障礙物的共同作用下繼續(xù)移動(dòng)。

3 移動(dòng)機(jī)器人模型

本文使用的兩輪差速移動(dòng)機(jī)器人模型是TurtleBot3系列的Burger版本,運(yùn)動(dòng)學(xué)模型如圖6所示。其中左右兩側(cè)是驅(qū)動(dòng)輪,分別由兩個(gè)電機(jī)驅(qū)動(dòng);前后的萬(wàn)向輪是從動(dòng)輪;ICR表示瞬時(shí)旋轉(zhuǎn)中心;rrob表示移動(dòng)機(jī)器人轉(zhuǎn)向半徑;drob表示移動(dòng)機(jī)器人直徑;vl和vr分別表示移動(dòng)機(jī)器人左輪和右輪的線速度。

圖6 移動(dòng)機(jī)器人運(yùn)動(dòng)學(xué)模型

運(yùn)動(dòng)參數(shù)包括線速度v和角速度w,其中線速度v∈[0, 0.22],單位為m/s;角速度w∈[-2.84, 2.84],單位為rad/s。通過(guò)調(diào)節(jié)左右驅(qū)動(dòng)輪速度來(lái)控制移動(dòng)機(jī)器人運(yùn)動(dòng)。移動(dòng)機(jī)器人的線速度與角速度和左右驅(qū)動(dòng)輪速度之間的關(guān)系:

(16)

TurtleBot3移動(dòng)機(jī)器人通過(guò)自身搭載的激光雷達(dá)獲取周?chē)h(huán)境信息。傳感器探測(cè)范圍為360°,探測(cè)距離為0.12～3.5 m,每15°返回一條激光雷達(dá)數(shù)據(jù)di(i=0～23),di表示該方向上最近障礙物的距離信息。移動(dòng)機(jī)器人激光雷達(dá)模型如圖7所示。

圖7 移動(dòng)機(jī)器人激光雷達(dá)模型

4 算法仿真與結(jié)果分析

為驗(yàn)證本文所提方法的有效性,在ROS機(jī)器人操作系統(tǒng)中利用Gazebo物理仿真平臺(tái)進(jìn)行仿真環(huán)境搭建,在離散型障礙物、一型障礙物、U型障礙物及混合障礙物環(huán)境中分別對(duì)傳統(tǒng)的人工勢(shì)場(chǎng)法和改進(jìn)的人工勢(shì)場(chǎng)法進(jìn)行對(duì)比實(shí)驗(yàn)。實(shí)驗(yàn)所采用的計(jì)算機(jī)配置是:Ubuntu 16.04操作系統(tǒng),ROS Kinetic機(jī)器人操作系統(tǒng),處理器為Intel i3-4160,運(yùn)行內(nèi)存4 GB,主頻3.6 GHz。

為更直觀地觀察仿真結(jié)果,將仿真環(huán)境以及移動(dòng)機(jī)器人運(yùn)行軌跡實(shí)時(shí)顯示在Rviz可視化工具中。其中,移動(dòng)機(jī)器人初始位置為起點(diǎn),方框?yàn)槟繕?biāo)點(diǎn),圓柱體為障礙物,實(shí)線為移動(dòng)機(jī)器人運(yùn)行的軌跡,方格地板邊長(zhǎng)為1 m。仿真過(guò)程算法主要參數(shù)設(shè)定見(jiàn)表1。

4.1 離散型障礙物環(huán)境下的仿真

Gazebo中離散稀疏障礙物環(huán)境和移動(dòng)機(jī)器人如圖8(a)所示,在Rviz中的投影如圖8(b)所示。其中,移動(dòng)機(jī)器人初始位置為起點(diǎn),起點(diǎn)坐標(biāo)為(-2, -1),目標(biāo)點(diǎn)坐標(biāo)為(4.5, 2.0)。

表1 仿真過(guò)程算法參數(shù)設(shè)定

(a)Gazebo中的仿真環(huán)境 (b) Rviz中的投影

APF-v1和APF-v2算法僅在解決局部極小值時(shí)采取不同的虛擬目標(biāo)點(diǎn)策略。當(dāng)未遇到局部極小值問(wèn)題時(shí),兩種算法規(guī)劃的路徑相同且花費(fèi)步數(shù)和耗時(shí)一致,故離散稀疏和密集障礙物環(huán)境中,將APF-v1和APF-v2算法統(tǒng)稱(chēng)為改進(jìn)的人工勢(shì)場(chǎng)法。傳統(tǒng)人工勢(shì)場(chǎng)法與改進(jìn)的人工勢(shì)場(chǎng)法規(guī)劃的路徑如圖9(a)和圖9(b)所示。改進(jìn)的人工勢(shì)場(chǎng)法采用自適應(yīng)速度調(diào)節(jié)機(jī)制,根據(jù)當(dāng)前工作環(huán)境的復(fù)雜程度實(shí)時(shí)對(duì)速度進(jìn)行調(diào)整。在離散稀疏障礙物環(huán)境中,傳統(tǒng)人工勢(shì)場(chǎng)法完成路徑規(guī)劃任務(wù)所用步數(shù)為253;采用自適應(yīng)速度調(diào)節(jié)機(jī)制時(shí),完成路徑規(guī)劃所用步數(shù)為238,且運(yùn)行軌跡相似,說(shuō)明自適應(yīng)調(diào)節(jié)機(jī)制能夠提高路徑規(guī)劃的效率。

(a)傳統(tǒng)人工勢(shì)場(chǎng)法路徑 (b) 改進(jìn)人工勢(shì)場(chǎng)法路徑

在離散密集障礙物環(huán)境中調(diào)整起始位置、目標(biāo)點(diǎn)位置以及障礙物布局如圖10(a) 所示,在Rviz中的投影如圖10(b)所示。其中,起點(diǎn)坐標(biāo)為 (-2, -2),目標(biāo)點(diǎn)坐標(biāo)為(4.2, -2.8)。

(a)Gazebo中的仿真環(huán)境 (b) Rviz中的投影

傳統(tǒng)人工勢(shì)場(chǎng)法與改進(jìn)的人工勢(shì)場(chǎng)法規(guī)劃的路徑如圖11(a)和圖11(b)所示。當(dāng)目標(biāo)點(diǎn)距離障礙物較近時(shí),傳統(tǒng)人工勢(shì)場(chǎng)法出現(xiàn)斥力過(guò)大問(wèn)題,移動(dòng)機(jī)器人在目標(biāo)點(diǎn)附近反復(fù)震蕩直至步數(shù)耗盡;改進(jìn)的人工勢(shì)場(chǎng)法在斥力勢(shì)場(chǎng)函數(shù)中引入目標(biāo)點(diǎn)與移動(dòng)機(jī)器人之間的距離,保證僅目標(biāo)點(diǎn)處的勢(shì)能為全局最小,完成路徑規(guī)劃任務(wù)所用步數(shù)為288。

(a)傳統(tǒng)人工勢(shì)場(chǎng)法路徑 (b) 改進(jìn)人工勢(shì)場(chǎng)法路徑

4.2 一型障礙物環(huán)境下的仿真

Gazebo中的一型障礙物環(huán)境和移動(dòng)機(jī)器人如圖12(a)所示,在Rviz中的投影如圖12(b)所示。其中,起點(diǎn)坐標(biāo)為(-2.5, 0),目標(biāo)點(diǎn)坐標(biāo)為(2.5, 0)。

(a)Gazebo中的仿真環(huán)境 (b) Rviz中的投影

傳統(tǒng)人工勢(shì)場(chǎng)法、APF-v1和APF-v2算法規(guī)劃的路徑如圖13所示。僅使用傳統(tǒng)人工勢(shì)場(chǎng)法時(shí),移動(dòng)機(jī)器人陷入局部極小值陷阱,在一型障礙物附近反復(fù)震蕩,導(dǎo)致路徑規(guī)劃任務(wù)失敗;本文提出的APF-v1算法在斥力分量Frep1偏轉(zhuǎn)90°方向上選擇合適距離設(shè)置虛擬目標(biāo)點(diǎn);APF-v2算法通過(guò)對(duì)當(dāng)前環(huán)境分析選擇在目標(biāo)障礙物外側(cè)設(shè)置虛擬目標(biāo)點(diǎn),兩種算法均能使移動(dòng)機(jī)器人在虛擬目標(biāo)點(diǎn)的引導(dǎo)下擺脫局部極小值陷阱。

(a)傳統(tǒng)人工勢(shì)場(chǎng)法路徑 (b) APF-v1算法路徑

4.3 U型障礙物環(huán)境下的仿真

Gazebo中的U型障礙物環(huán)境和移動(dòng)機(jī)器人如圖14(a)所示,在Rviz中的投影如圖14(b)所示。其中,起點(diǎn)坐標(biāo)為(-3.5, 0),目標(biāo)點(diǎn)坐標(biāo)為(3, 0)。

(a)Gazebo中的仿真環(huán)境 (b) Rviz中的投影

傳統(tǒng)人工勢(shì)場(chǎng)法、APF-v1和APF-v2算法在U型障礙物環(huán)境中規(guī)劃的路徑如圖15所示。傳統(tǒng)人工勢(shì)場(chǎng)法使移動(dòng)機(jī)器人陷入U(xiǎn)型障礙物陷阱,在局部極小值點(diǎn)附近反復(fù)震蕩直至步數(shù)耗盡;APF-v1算法在擺脫U型障礙物陷阱時(shí),由于移動(dòng)機(jī)器人在U型障礙物邊緣所受斥力分量Frep1角度變化較大,虛擬目標(biāo)點(diǎn)設(shè)置位置變化較大,導(dǎo)致移動(dòng)機(jī)器人擺脫U型障礙物陷阱所需步數(shù)較多;而APF-v2算法通過(guò)在目標(biāo)障礙物外側(cè)設(shè)置虛擬目標(biāo)點(diǎn)的方法,能規(guī)劃出相對(duì)更優(yōu)的路徑。

4.4 混合障礙物環(huán)境下的仿真

將離散型、一型和U型障礙物組合形成混合障礙物環(huán)境。Gazebo中的混合障礙物環(huán)境和移動(dòng)機(jī)器人如圖16(a)所示,在Rviz中的投影如圖16(b)所示。其中,起點(diǎn)坐標(biāo)為(-4, -2),目標(biāo)點(diǎn)坐標(biāo)為(4, 4)。

(a)傳統(tǒng)人工勢(shì)場(chǎng)法路徑 (b) APF-v1算法路徑

(a)Gazebo中的仿真環(huán)境 (b) Rviz中的投影

傳統(tǒng)人工勢(shì)場(chǎng)法、APF-v1和APF-v2算法規(guī)劃的路徑如圖17所示。由于傳感器探測(cè)范圍有限,移動(dòng)機(jī)器人沿勢(shì)場(chǎng)的負(fù)梯度方向移動(dòng)進(jìn)入U(xiǎn)型障礙物陷阱,傳統(tǒng)人工勢(shì)場(chǎng)法使移動(dòng)機(jī)器人在U型障礙物陷阱區(qū)域反復(fù)震蕩,導(dǎo)致路徑規(guī)劃任務(wù)失敗;而APF-v1算法和APF-v2算法使移動(dòng)機(jī)器人進(jìn)入U(xiǎn)型和一型障礙物陷阱時(shí),對(duì)當(dāng)前障礙物環(huán)境進(jìn)行判斷,通過(guò)設(shè)置虛擬目標(biāo)點(diǎn)的方法引導(dǎo)機(jī)器人擺脫局部極小值陷阱。其中,APF-v1算法在繞行障礙物邊緣時(shí)步數(shù)相對(duì)較多,APF-v2算法能規(guī)劃出相對(duì)更優(yōu)的路徑。

對(duì)傳統(tǒng)人工勢(shì)場(chǎng)法、APF-v1算法和APF-v2算法在上述五種仿真環(huán)境中從起點(diǎn)到目標(biāo)點(diǎn)的路徑規(guī)劃步數(shù)和時(shí)間進(jìn)行匯總,見(jiàn)表2。

從表2數(shù)據(jù)對(duì)比可以看出,在各種仿真環(huán)境中,APF-v1算法和APF-v2算法均能規(guī)劃出從起點(diǎn)到目標(biāo)點(diǎn)的路徑, 且在一型、U型和混合障礙物環(huán)境中,APF-v2算法規(guī)劃的路徑效果較好,所用的步數(shù)較少、耗時(shí)較短。

(a)傳統(tǒng)人工勢(shì)場(chǎng)法路徑 (b) APF-v1算法路徑

表2 仿真環(huán)境中算法對(duì)比數(shù)據(jù)

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

針對(duì)未知環(huán)境下的移動(dòng)機(jī)器人局部路徑規(guī)劃問(wèn)題,本文提出了APF-v1算法和APF-v2算法,并在ROS系統(tǒng)中設(shè)計(jì)不同的仿真環(huán)境,將本文所提算法與傳統(tǒng)人工勢(shì)場(chǎng)法進(jìn)行了對(duì)比實(shí)驗(yàn),驗(yàn)證了改進(jìn)算法的有效性。APF-v1算法和APF-v2算法具有以下特點(diǎn):

1)通過(guò)改進(jìn)引力和斥力勢(shì)場(chǎng)函數(shù),有效解決了引力過(guò)大和目標(biāo)不可達(dá)問(wèn)題;根據(jù)當(dāng)前環(huán)境復(fù)雜程度設(shè)計(jì)自適應(yīng)速度調(diào)節(jié)機(jī)制,提高路徑規(guī)劃的效率。

2)對(duì)存在局部極小值陷阱的復(fù)雜障礙物區(qū)域,算法對(duì)當(dāng)前障礙物環(huán)境判斷,分別在斥力分量偏轉(zhuǎn)90°方向上與目標(biāo)障礙物外側(cè)合適位置設(shè)置虛擬目標(biāo),引導(dǎo)移動(dòng)機(jī)器人擺脫局部極小值陷阱。

3)當(dāng)障礙物環(huán)境較為復(fù)雜時(shí),APF-v2算法完成路徑規(guī)劃任務(wù)花費(fèi)步數(shù)較少,用時(shí)較短,效率較高。

考慮到移動(dòng)機(jī)器人的實(shí)際工作環(huán)境可能更為復(fù)雜,下一步工作考慮將人工勢(shì)場(chǎng)法與模糊控制法以及強(qiáng)化學(xué)習(xí)方法結(jié)合,解決移動(dòng)機(jī)器人在更為復(fù)雜場(chǎng)景下的路徑規(guī)劃問(wèn)題,并進(jìn)一步縮短路徑長(zhǎng)度,使移動(dòng)機(jī)器人在各種環(huán)境下規(guī)劃出較優(yōu)的路徑。