復(fù)雜環(huán)境下移動機器人全局路徑規(guī)劃與跟蹤

周 滔,趙 津,胡秋霞,席阿行,劉東杰

(貴州大學(xué) 機械工程學(xué)院,貴陽 550025)

0 概述

移動機器人在工業(yè)和服務(wù)業(yè)中應(yīng)用較為廣泛。其中,路徑規(guī)劃和跟蹤反映了移動機器人在路面行駛與環(huán)境的交互能力,在復(fù)雜環(huán)境中其對路徑規(guī)劃算法的要求較高。目前,國內(nèi)外學(xué)者提出許多路徑規(guī)劃算法及其改進算法,并對生成的運動軌跡進行有效跟蹤。文獻[1]針對障礙物密集的環(huán)境提出一種回報詳細分類Q(RDC-Q)學(xué)習(xí)算法,使機器人在運行空間達到一個安全等級較高的狀態(tài)。文獻[2]研究蟻群算法的主要參數(shù)對規(guī)劃路徑的長度和搜索效率的影響,驗證蟻群算法最佳匹配參數(shù)組的準(zhǔn)確性,在復(fù)雜環(huán)境下經(jīng)過多次迭代得到最短路徑。大量研究表明,機器人硬件系統(tǒng)的精度[3-5]在短期內(nèi)不能得到解決的情況下,對路徑規(guī)劃算法研究顯得尤為重要。文獻[6]將已知環(huán)境柵格化后,規(guī)劃出最優(yōu)路徑,在聯(lián)合虛擬仿真平臺上采用一種輸入輸出反饋線性化的運動控制器對路徑進行跟蹤。但使用的路徑規(guī)劃算法只能在單一環(huán)境下尋優(yōu)且路徑跟蹤誤差較大。文獻[7]根據(jù)農(nóng)業(yè)機器人的工作特點和運動特性,基于遺傳算法得到全局路徑,并沿運動軌跡行駛,提高農(nóng)業(yè)機器人自主控制能力。文獻[8]基于B樣條曲線將特定環(huán)境下的路徑進行優(yōu)化,提高了移動機器人的行駛平順性。但全局規(guī)劃算法的選擇研究較少,其需要針對應(yīng)用場景考慮算法的綜合性能,包括路徑長度、運行時間和穩(wěn)定性。

為了滿足復(fù)雜環(huán)境下全局路徑的實時性要求和最短路徑的需求,本文通過分析人工勢場(Artificial Potential Field,APF)法、A*算法、雙向快速擴展隨機樹(Bidirectional Rapidly Exploring Random Trees,BRRT)算法和概率路標(biāo)(Probabilistic Roadmap,PRM)算法的優(yōu)劣,將4種算法分別應(yīng)用到簡單與復(fù)雜環(huán)境下,然后選擇最優(yōu)的算法在建立的復(fù)雜環(huán)境下實時規(guī)劃出無碰撞、路徑較短的全局路徑,并采用一種改進的移動機器人自適應(yīng)軌跡跟蹤控制算法對其運動軌跡進行跟蹤。

1 路徑規(guī)劃算法

1.1 人工勢場法模型

人工勢場法將控制對象看作二維空間中的一個質(zhì)點,機器人在運動空間中任意位置X(X=[xy]T)的移動方向由機器人的勢場合力的方向決定[9]。 引力勢場函數(shù)為:

(1)

其中,k(k>0)是引力場的比例位置增益系數(shù),X是機器人在運動空間的位置,Xg是目標(biāo)位置,ρ(X,Xg)=‖Xg-X‖表示X與Xg之間的距離,則吸引力負梯度可以用勢場函數(shù)表示:

Fatt=-[Uatt(X)]=k(X-Xg)

(2)

斥力場函數(shù)為:

(3)

其中,η和ρ(X,X0)的定義同引力場,ρ0由障礙物和目標(biāo)的具體情況而定,是受障礙物影響的距離長度,其小于目標(biāo)與障礙物之間的最小距離,同時小于障礙物之間距離的一半。

相應(yīng)的斥力用斥力場函數(shù)表示為:

Frep(X)=-[Urep(X)]

(4)

Frep(XX=

(5)

機器人受到的合力為:

Ft=Fatt+Frep

(6)

機器人在運動空間的受力情況如圖1所示。

圖1 機器人受力情況

機器人的初始位置和目標(biāo)位置如圖2(a)所示,利用人工勢場法得到圖2(b)和圖2(c),分別是APF產(chǎn)生的引力場和排斥場。從機器人各個角度測量障礙物的距離,產(chǎn)生的力可以用來控制機器人的速度。

圖2 APF引力場和斥力場

圖2(a)依據(jù)人工勢場法得到總勢場和APF生成路徑結(jié)果如圖3所示,負坡度平面表示引力,錐形表示障礙物依據(jù)人工勢場法生成的斥力。 APF極易陷入極小值,針對此問題,研究人員提出多種方法來避免。例如,添加一個虛擬障礙,避免陷入局部最小[10];使用模擬退火算法或?qū)嵤└倪M的斥力場模型;通過改變障礙物自適應(yīng)勢場函數(shù)的權(quán)重來使機器人逃離局部最小。APF方法速度快,計算量小,因此,主要用于局部路徑規(guī)劃和避障[11]。

圖3 APF總勢場和路徑

1.2 A*算法

A*算法是一種啟發(fā)算法,由于其最優(yōu)性、完備性、高效性,是當(dāng)前被廣泛應(yīng)用的一種路徑規(guī)劃算法[12],對A*算法的改進也是路徑規(guī)劃領(lǐng)域的研究熱點。A*算法估價函數(shù)式可表述為:

f(n)=g(n)+h(n)

(7)

其中,f(n)表示總的啟發(fā)式代價函數(shù),為起點到當(dāng)前節(jié)點n的實際距離函數(shù),g(n)是節(jié)點到目標(biāo)的啟發(fā)式代價,h(n)小于或等于節(jié)點n到目標(biāo)點的實際距離,是得到最優(yōu)解的條件,h(n)表示當(dāng)前節(jié)點到目標(biāo)的歐氏距離[13],即:

(8)

A*算法的路徑規(guī)劃如圖4所示,其中黑色代表障礙物,白色是自由空間,灰色是搜索的空間,線條對應(yīng)從原點到目標(biāo)位置的最短路徑。

圖4 A*算法路徑規(guī)劃

A*算法概念簡單,易于實現(xiàn),但其對內(nèi)存要求較高。因為必須事先存儲整個地圖,所以不適合應(yīng)用在大場景或高維操作空間。

1.3 快速擴展隨機樹算法

1.3.1 RRT

文獻[14]提出一種快速擴展隨機樹(Rapidly-exploring Randorn Trees,RRT)搜索算法[14]。該算法的基本構(gòu)造如圖5所示,其為了達到期望的距離,將逐步、快速地降低樹與隨機采樣節(jié)點之間的距離。RRT包括一個碰撞檢測算法的擴展過程,由一個度量函數(shù)、隨機采樣算法和組態(tài)空間組成[15],最終找到從起點qinit到目標(biāo)qgoal的可行路徑。

圖5 RRT算法構(gòu)造示意圖

1.3.2 BRRT

BRRT用2個RRT加快路徑規(guī)劃速度:從起點開始,隨機樹朝著目標(biāo)擴展;從目標(biāo)開始,隨機樹向起點擴展,當(dāng)兩樹相遇時路徑便會產(chǎn)生[16]。本文采用雙向RRT算法,如圖6所示,圖6(a)中2棵不同節(jié)點的樹,即BRRT隨機采樣過程,五角星節(jié)點是兩棵樹的交點。該方法的優(yōu)點是對空間隨機抽樣,進而減少計算量,快速生成次優(yōu)路徑。

圖6 BRRT生成的路徑

1.4 概率路標(biāo)算法

文獻[17]提出一種基于圖搜索的概率路標(biāo)算法,具體步驟如下:

1)創(chuàng)建離線地圖。隨機選擇不在障礙物內(nèi)部的點,用直線連接所有頂點,并在整個工作區(qū)繪制一個圖表。

2)在線路徑規(guī)劃。根據(jù)已知圖(路線圖),使用基于圖搜索的路徑規(guī)劃算法與運用A*算法,定義連接點之間的歐幾里德距離為局部代價,與目標(biāo)點之間的歐幾里德距離作為啟發(fā)式代價[18]。

圖7是采用概率路標(biāo)算法得到的隨機采樣結(jié)果及機器人路徑規(guī)劃路線圖。PRM能用相對少的隨機采樣點找到一個解,當(dāng)采樣點太少,或者分布不合理時,PRM算法是不完備的,且重復(fù)性較差。如果增加隨機采樣點,則算法可以達到完備。因此,PRM是概率完備但不是最優(yōu)的算法。

2 路徑規(guī)劃算法仿真結(jié)果對比

本文從低分辨率圖像的地圖服務(wù)中導(dǎo)出道路地圖圖像。此處非道路區(qū)域歸為障礙區(qū),并定義這些區(qū)域為禁止區(qū)域,用像素為0的黑色表示。道路區(qū)域歸為移動機器人的自由區(qū)域,可以自由通行,用像素為1的白色表示。如圖8所示,原始地圖二值化,進行濾波膨脹處理(消除文本信息),圖像下載到設(shè)計的Matlab仿真平臺GUI地圖區(qū)域,應(yīng)用到實驗場景分析中的場景2。

圖8 復(fù)雜環(huán)境的建立

為了測試4種算法在線實時路徑規(guī)劃能力,本文選擇適合復(fù)雜環(huán)境下的全局路徑規(guī)劃算法,在同一起點和終點設(shè)置條件下對20次實驗結(jié)果進行比較分析,在簡單環(huán)境和復(fù)雜環(huán)境下討論4種算法的綜合性能,即實時性和最短路徑需求。

2.1 簡單環(huán)境

場景1是將2個形狀不一樣的圓形特征添加到地圖中,起點(22,21),終點(70,85)。4種算法在20次實驗中均找到了最優(yōu)路徑,如圖9所示,將路徑長度、平均運行時間進行統(tǒng)計分析,結(jié)果如圖10和表1所示。

圖9 場景1路徑規(guī)劃結(jié)果

圖10 4種算法的路徑長度

算法平均計算時間APF算法295A*算法3 402BRRT算法3 079PRM算法2 964

在圖10中,最短路徑是由確定性搜索算法APF得到,APF算法的路徑優(yōu)于A*算法,其次是PRM與BRRT。這是因為受勢場力的作用,APF在局部區(qū)域得到的路徑優(yōu)于其他3種算法得到的路徑。因此,大多數(shù)研究將APF用于局部規(guī)劃。

從運行時間上看,A*算法運行時間最長,依次是BRRT、PRM、APF。數(shù)據(jù)顯示的隨機性搜索算法BRRT和PRM隨機性較強,搜索路徑不能重復(fù),穩(wěn)定性較差。綜合考慮路徑長度與運行時間結(jié)果可知,在簡單環(huán)境下的路徑規(guī)劃應(yīng)選擇APF算法。

2.2 復(fù)雜環(huán)境

場景2是一個充滿狹窄通道的復(fù)雜地圖,起點(6,7),終點(97,79)。如圖11所示,APF在22次實驗中第20次成功找到最優(yōu)路徑,為了分析與其他3種算法的路徑長度與運行時間,對尋找到最優(yōu)路徑的20次實驗的路徑長度、平均運行時間進行統(tǒng)計分析。4種算法的路徑長度如圖12所示,確定性搜索算法A*與APF的路徑整體上優(yōu)于隨機性算法BRRT與PRM,同時BRRT與PRM的概率完備且不最優(yōu)。如圖11所示,BRRT的路徑不平滑,2種算法均不利于當(dāng)前環(huán)境下的路徑規(guī)劃。

圖11 場景2路徑規(guī)劃結(jié)果

圖12 4種算法的路徑長度

在表2中,復(fù)雜環(huán)境下搜索時間最長的是BRRT,然后依次是A*、PRM、APF。A*算法必須覆蓋幾乎整個操作空間,才能找到一個最佳的解決方案,所以其過程比APF方法慢很多。但在實驗中,實現(xiàn)APF算法時,出現(xiàn)了找不到路徑的現(xiàn)象,是因為APF在復(fù)雜環(huán)境下無法避開局部極小值。所以,該算法不完備,且非最優(yōu)化。

表2 4種算法在場景2中的路徑規(guī)劃時間

通過復(fù)雜環(huán)境與簡單環(huán)境的比較可以得出,全局路徑規(guī)劃算法有多種,它們不能同時滿足所有需求,即沒有通用的全局規(guī)劃程序,但每一種路徑規(guī)劃算法都具有獨特的特性,使算法本身適用于特定的問題。在復(fù)雜環(huán)境下,本文綜合路徑長度、搜索時間、穩(wěn)定性考慮,A*是最適合的路徑規(guī)劃算法,將得到的路徑作為后續(xù)移動機器人跟蹤的參考軌跡。

3 軌跡跟蹤

3.1 軌跡跟蹤過程

本文研究控制的是一種軸對稱非完整約束的2輪差分驅(qū)動移動機器人,輪式移動機器人的運動學(xué)模型如圖13所示。

圖13 移動機器人軌跡跟蹤模型

假設(shè)當(dāng)前位姿為pc=[xc,yc,θc]T,給定的參考位姿pr=[xr,yr,θr]T,當(dāng)前速度矢量qc=[vc,wc]T,給定的參考速度矢量qr=[vr,wr]T,根據(jù)非線性約束方程:

(9)

跟蹤的位姿誤差為:

(10)

控制目標(biāo)是為了實現(xiàn)機器人的行駛軌跡同參考軌跡相吻合。其位姿誤差的微分方程為:

(11)

將反步法引入基于運動學(xué)軌跡的跟蹤控制律,使其有更好的跟蹤效果[19-20]。

(12)

圖14 移動機器人跟蹤控制框圖

3.2 控制律設(shè)計

根據(jù)建立的移動機器人運動學(xué)模型,本文應(yīng)用改進的自適應(yīng)軌跡跟蹤控制算法[21]。該算法為了加速軌跡的誤差收斂速度,在機器人位姿誤差中引入縱坐標(biāo)誤差,并采用人工勢場與位姿誤差協(xié)同控制機器人導(dǎo)向。

3.2.1 人工勢場法

(13)

因此,可得:

(14)

圖15 人工場計算示意圖

3.2.2 人工勢場法的控制律設(shè)計

在控制律式(12)中導(dǎo)入以上人工場方法得到設(shè)計的控制律為:

(15)

其中,kx、ky、kθ、kβ、kd均為非負數(shù)的控制參數(shù)。

(16)

3.3 軌跡跟蹤結(jié)果

實驗跟蹤運動軌跡的實驗參數(shù)為:起始點(6,7)到終點(97,79),長度為150 m,機器人初始位姿(5.7,6.7,π/14)。2個跟蹤參考輸入為:vr=1.2 m/s,wr=0 rad/s。

如圖16～圖18所示,路徑平滑處理后,對相鄰節(jié)點分段進行跟蹤,移動機器人橫向和縱向上的跟蹤誤差在理想軌跡波動較大處明顯,最大誤差范圍max{|xe|,|ye|}=0.14 m,在可控范圍內(nèi)。結(jié)合位姿誤差和方向角跟蹤結(jié)果可知,該控制算法有效地跟蹤了設(shè)計的期望軌跡。

圖16 參考軌跡與跟蹤軌跡

圖17 移動機器人方向跟蹤

圖18 移動機器人位姿誤差

4 結(jié)束語

為解決在復(fù)雜環(huán)境下移動機器人的路徑規(guī)劃與跟蹤問題,本文利用可視化GUI設(shè)計4種路徑規(guī)劃算法仿真平臺。在簡單環(huán)境和復(fù)雜環(huán)境下對比隨機性搜索算法與確定性搜索算法的優(yōu)劣,綜合算法的路徑長度、運行時間、穩(wěn)定性,在簡單環(huán)境中選擇APF算法,在復(fù)雜地圖中選擇A*算法,并采用一種改進的基于運動學(xué)模型的移動機器人對最優(yōu)運動軌跡進行跟蹤。仿真結(jié)果驗證了本文算法的有效性。下一步將研究基于多傳感器定位的導(dǎo)航問題,實現(xiàn)移動機器人在室外復(fù)雜環(huán)境下的自主導(dǎo)航。