張國勝，李彩虹，張耀玉，李永迪，周瑞紅

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

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

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

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

1 傳統(tǒng)人工勢場法

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

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

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

(1)

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

人工勢場中機(jī)器人所受的力F(X):

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

(2)

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

1.1 引力場

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

(3)

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

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

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

(4)

1.2 斥力場

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

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

(5)

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

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

Frep(X)=

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

(6)

1.3 合力

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

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

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

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

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

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

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

2.1 改進(jìn)的引力勢場

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

Uatt(X)=

(7)

式中dg為引力作用閾值。

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

Fatt(X)=

(8)

2.2 改進(jìn)的斥力勢場

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

Urep(X)=

(9)

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

Frep(X)=

(10)

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

(11)

(12)

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

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

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

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

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

(13)

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

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

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

圖3 局部極小值情況

局部極小值問題是由移動機(jī)器人所受合力為零達(dá)到平衡狀態(tài)所致,本文分別提出APF-v1和APF-v2兩種設(shè)置虛擬目標(biāo)點(diǎn)的方法打破該平衡狀態(tài),引導(dǎo)移動機(jī)器人擺脫局部極小值陷阱。在設(shè)置虛擬目標(biāo)點(diǎn)時,兩種算法都需要對周邊的障礙物環(huán)境進(jìn)行檢測,選擇障礙物數(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

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

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

(14)

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

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

2.4.2 APF-v2

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

當(dāng)系統(tǒng)檢測到移動機(jī)器人陷入局部極小值陷阱時,設(shè)置障礙物存儲模塊,記錄使其陷入局部極小值陷阱的障礙物位置信息,形成局部極小值障礙物組,在系統(tǒng)判斷的虛擬目標(biāo)點(diǎn)設(shè)置區(qū)域選擇障礙物組中最外側(cè)的為目標(biāo)障礙物,虛擬目標(biāo)點(diǎn)設(shè)置在移動機(jī)器人當(dāng)前位置相對于目標(biāo)障礙物的對稱點(diǎn)附近范圍內(nèi)。忽略原目標(biāo)點(diǎn)引力,移動機(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)后的移動機(jī)器人受力分析如圖5所示。

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

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

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

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

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

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

(16)

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

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

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

為驗(yàn)證本文所提方法的有效性,在ROS機(jī)器人操作系統(tǒng)中利用Gazebo物理仿真平臺進(jìn)行仿真環(huán)境搭建,在離散型障礙物、一型障礙物、U型障礙物及混合障礙物環(huán)境中分別對傳統(tǒng)的人工勢場法和改進(jìn)的人工勢場法進(jìn)行對比實(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)境以及移動機(jī)器人運(yùn)行軌跡實(shí)時顯示在Rviz可視化工具中。其中,移動機(jī)器人初始位置為起點(diǎn),方框?yàn)槟繕?biāo)點(diǎn),圓柱體為障礙物,實(shí)線為移動機(jī)器人運(yùn)行的軌跡,方格地板邊長為1 m。仿真過程算法主要參數(shù)設(shè)定見表1。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

5 結(jié)束語

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

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

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

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

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