謝春麗,劉斐灝

(東北林業(yè)大學(xué) 交通學(xué)院,黑龍江 哈爾濱 150040)

0 引 言

移動(dòng)機(jī)器人的自主導(dǎo)航中,避障是最關(guān)鍵的功能之一[1]。避障的實(shí)時(shí)性與穩(wěn)定性是當(dāng)前移動(dòng)機(jī)器人路徑規(guī)劃的典型挑戰(zhàn)[2]。隨著場景的復(fù)雜性增加,動(dòng)態(tài)障礙物對于算法的實(shí)時(shí)性要求增加[3],為在動(dòng)態(tài)環(huán)境下生成安全平滑的軌跡,需要軌跡規(guī)劃算法能夠在動(dòng)態(tài)場景下對動(dòng)態(tài)障礙物實(shí)時(shí)進(jìn)行軌跡跟蹤與運(yùn)動(dòng)狀態(tài)預(yù)測[4]。

現(xiàn)有機(jī)器人動(dòng)態(tài)避障方法存在對復(fù)雜動(dòng)態(tài)環(huán)境處理能力不夠,易陷入局部最小值[5],且實(shí)時(shí)性不滿足實(shí)際應(yīng)用的需要,且在混合場景下無法準(zhǔn)確區(qū)分動(dòng)態(tài)與靜態(tài)障礙物類型[6]。因此動(dòng)態(tài)避障軌跡規(guī)劃系統(tǒng)需要對復(fù)雜環(huán)境有較優(yōu)的處理能力,并能實(shí)時(shí)求解出可行軌跡[7]。

TEB算法[8]是基于彈性帶(elastic bands, EB)[9]方法改進(jìn),EB算法使用兩個(gè)分量來產(chǎn)生避撞路徑,即排斥力與收縮力,障礙物施加排斥力在彈性帶上形成安全距離,收縮力在避障條件滿足的前提下讓路徑收縮。

TEB算法能夠產(chǎn)生一條無碰撞的時(shí)間最優(yōu)化運(yùn)動(dòng)路徑,對環(huán)境中的障礙物進(jìn)行實(shí)時(shí)的檢測并作出在線規(guī)劃。動(dòng)態(tài)障礙物環(huán)境下,軌跡規(guī)劃算法需要對環(huán)境信息有較優(yōu)且快速的處理能力,并且能對動(dòng)態(tài)障礙物進(jìn)行軌跡跟蹤與預(yù)測[10]。然而,TEB算法在動(dòng)態(tài)場景下會將動(dòng)態(tài)障礙物視為靜態(tài)去處理,導(dǎo)致機(jī)器人在導(dǎo)航過程中與動(dòng)態(tài)障礙物發(fā)生碰撞。

針對上述問題,筆者采用不同頻率的均值濾波器區(qū)分動(dòng)、靜態(tài)障礙物,增加動(dòng)態(tài)障礙物代價(jià)地圖,對障礙物進(jìn)行聚類處理[11],結(jié)合卡爾曼濾波[12]對障礙物的運(yùn)動(dòng)軌跡進(jìn)行跟蹤預(yù)測,通過預(yù)測障礙物軌跡,結(jié)合TEB算法規(guī)劃合理的軌跡進(jìn)行避障。

1 阿克曼機(jī)器人的運(yùn)動(dòng)學(xué)模型

采取阿克曼轉(zhuǎn)向結(jié)構(gòu)機(jī)器人進(jìn)行仿真與真實(shí)實(shí)驗(yàn),阿克曼轉(zhuǎn)向結(jié)構(gòu)機(jī)器人的運(yùn)動(dòng)學(xué)模型如圖1。

圖1 阿克曼轉(zhuǎn)向結(jié)構(gòu)機(jī)器人運(yùn)動(dòng)學(xué)模型

圖中(Xw,Yw)為世界坐標(biāo)系下坐標(biāo)軸,(X,Y)為機(jī)器人基坐標(biāo)系,坐標(biāo)原點(diǎn)為機(jī)器人后軸中心S點(diǎn),其中車輛前進(jìn)方向?yàn)閄軸正方向,垂直車身向左為Y軸正方向。I點(diǎn)為阿克曼轉(zhuǎn)向機(jī)器人瞬時(shí)旋轉(zhuǎn)中心(instantaneous center of rotation,ICR) ,φ代表車輛前輪轉(zhuǎn)向角,其中φ∈(-π/2, π/2),R為車輛最小轉(zhuǎn)彎半徑,β為車身與世界坐標(biāo)系橫軸夾角。

左、右后輪的平移速度分別為vl、vr,且滿足機(jī)器人線速度v=(vl+vr)/2。L為軸距,其前輪轉(zhuǎn)向角φ與轉(zhuǎn)彎半徑R、軸距L有關(guān),需滿足式(1):

(1)

式中:ω為機(jī)器人角速度。

設(shè)在t時(shí)刻,機(jī)器人姿態(tài)由位置狀態(tài)s(t)和運(yùn)動(dòng)狀態(tài)u(t)=[v(t),φ(t)]T表示,則阿克曼機(jī)器人運(yùn)動(dòng)學(xué)狀態(tài)方程表示為式(2):

(2)

2 TEB算法原理與求解優(yōu)化流程

2.1 TEB算法基本原理

TEB算法最初是為移動(dòng)基礎(chǔ)機(jī)器人導(dǎo)航規(guī)劃開發(fā)的。通過在彈性帶的基礎(chǔ)上引入時(shí)間因素,增加機(jī)器人相鄰位姿的時(shí)間間隔。因此,機(jī)器人的離散運(yùn)動(dòng)軌跡T可描述為N個(gè)有序的位姿與時(shí)間間隔的集合,表示為式(3):

T=[S1,ΔT1,…,Sk,ΔTk,…,ΔTN-1,SN]T

(3)

Sk=[Pk,βk]T,k∈[1,n],Pk={xk,yk}

式中:Sk表示機(jī)器人在世界坐標(biāo)系下的位姿,其中Pk為機(jī)器人坐標(biāo)。ΔTk為位姿Sk過渡到Sk+1的時(shí)間間隔。

由連續(xù)機(jī)器人位姿組成的軌跡示例如圖2。

圖2 軌跡連續(xù)位姿序列與時(shí)間間隔

2.2 TEB求解優(yōu)化流程

在TEB中引入的時(shí)間因素將機(jī)器人控制系統(tǒng)與速度,加速度等動(dòng)態(tài)約束進(jìn)行拓展,能夠?yàn)榫植恳?guī)劃器提供一個(gè)持續(xù)優(yōu)化的解決方案,在非完整約束移動(dòng)機(jī)器人上得以適用[13],如圖3。

通過在局部代價(jià)地圖中對全局路徑的子集進(jìn)行采樣,創(chuàng)建局部姿態(tài)序列,將全局路徑轉(zhuǎn)變?yōu)閺椥詭нM(jìn)行優(yōu)化,其生成的路徑不一定是最短路徑,但能夠滿足時(shí)間最優(yōu)。在障礙物環(huán)境較多情況下,為防止在路徑規(guī)劃過程陷入局部極小值,對具有不同拓?fù)浣Y(jié)構(gòu)的多條軌跡進(jìn)行維護(hù)和優(yōu)化,并提出兩階段優(yōu)化算法解決軌跡局部最優(yōu)問題[14],而后針對阿克曼轉(zhuǎn)向結(jié)構(gòu)機(jī)器人進(jìn)行拓展[15]。

圖3 TEB算法的優(yōu)化流程

最優(yōu)軌跡b*是通過最小化代價(jià)函數(shù)獲得,這些函數(shù)包括運(yùn)動(dòng)規(guī)劃中的部分沖突目標(biāo)與約束。這些目標(biāo)包括CPU資源消耗、路徑長度、總運(yùn)動(dòng)時(shí)間或以上目標(biāo)函數(shù)的加權(quán)組合。

通過對不符合移動(dòng)機(jī)器人運(yùn)動(dòng)學(xué)約束的軌跡加以懲罰函數(shù),將軌跡允許解限制在可行集內(nèi)。TEB的優(yōu)化問題被定義為一個(gè)加權(quán)非線性最小二乘代價(jià)函數(shù),該代價(jià)函數(shù)考慮目標(biāo)函數(shù)J和懲罰函數(shù)P的沖突集,表示為:

(4)

式中:b{s1,sN}表示軌跡中的起始位姿s1與目標(biāo)位姿sN不受優(yōu)化;σi為每個(gè)目標(biāo)函數(shù)的加權(quán)因子;fJ為目標(biāo)函數(shù)的綜合,目標(biāo)函數(shù)J中包括計(jì)算資源,軌跡,運(yùn)動(dòng)時(shí)間等因素;fP為懲罰函數(shù)綜合,懲罰函數(shù)P包括障礙物約束、非完整運(yùn)動(dòng)學(xué)約束、速度與加速度約束等。

將TEB優(yōu)化問題映射到一個(gè)超圖上,構(gòu)成解向量的機(jī)器人姿態(tài)sk和時(shí)間間隔ΔTk組成超圖的頂點(diǎn),超圖的邊由非線性規(guī)劃的代價(jià)項(xiàng)組成。此外,將不受優(yōu)化的機(jī)器人起始和目標(biāo)位姿、靜態(tài)障礙物等靜態(tài)參數(shù)作為超圖的固定頂點(diǎn)。得到的超圖通過g2o框架進(jìn)行有效求解,該圖優(yōu)化框架利用系統(tǒng)矩陣的稀疏結(jié)構(gòu),采用了魯棒性和效率較高的Levenberg-Marquardt(LM)算法求解,計(jì)算出各個(gè)目標(biāo)函數(shù)與懲罰函數(shù)的加權(quán)因子,通過最小化代價(jià)函數(shù)求解出最優(yōu)軌跡。

3 基于卡爾曼濾波的障礙物運(yùn)動(dòng)估計(jì)

在ROS的運(yùn)動(dòng)規(guī)劃框架中,代價(jià)地圖為全局及局部規(guī)劃器提供實(shí)時(shí)環(huán)境信息。代價(jià)地圖用一個(gè)網(wǎng)格表示,其中每個(gè)單元格存在3種狀態(tài)設(shè)定:即空閑(0)、占用(254)和未知(255),代價(jià)地圖以特定的速度進(jìn)行更新,但其只能表示靜態(tài)障礙物的信息。

筆者提出了一個(gè)實(shí)時(shí)的動(dòng)態(tài)障礙物避障策略,使用快速和慢速均值濾波器對二維激光雷達(dá)點(diǎn)云數(shù)據(jù)進(jìn)行預(yù)處理,將動(dòng)態(tài)障礙物從環(huán)境中分離出來,完成動(dòng)態(tài)障礙物檢測,建立動(dòng)態(tài)障礙物的代價(jià)地圖層對檢測到的動(dòng)態(tài)障礙物進(jìn)行聚類處理,利用卡爾曼濾波方法,根據(jù)當(dāng)前動(dòng)態(tài)障礙物的運(yùn)動(dòng)狀態(tài)進(jìn)行軌跡跟蹤與狀態(tài)預(yù)測。

3.1 動(dòng)態(tài)障礙物檢測

對于代價(jià)地圖的每一個(gè)單元格,使用一個(gè)快速運(yùn)行均值濾波器與一個(gè)慢速運(yùn)行均值濾波器,對機(jī)器人前方探測到的激光點(diǎn)云進(jìn)行預(yù)處理。其中單個(gè)單元格的濾波器表示為:

Pf(t+1)=(1-αf)Pf(t)+αfC(t)

(5)

Ps(t+1)=(1-αs)Ps(t)+αsC(t)

(6)

式中:Pf(t)、Ps(t)分別為快速運(yùn)行均值濾波器與慢速均值濾波器在t時(shí)刻的輸出值;αf、αs為當(dāng)前代價(jià)地圖C(t)對均值濾波器的影響程度,且滿足0≤αs<αf≤1。

通過兩個(gè)不同運(yùn)行速度的均值濾波器對激光點(diǎn)云數(shù)據(jù)進(jìn)行高頻噪聲與低頻噪聲濾除,實(shí)現(xiàn)對動(dòng)態(tài)障礙物占用單元格的識別?？焖龠^濾器激活閾值為c1,快速和慢速濾波器之間的差異必須超過閾值c2,以消除低頻噪聲的準(zhǔn)靜態(tài)障礙,表示為式(7)、式(8):

Pf(t)>c1

(7)

Pf(t)-Ps(t)>c2

(8)

通過設(shè)定閾值操作,障礙物將在代價(jià)地圖中生成一個(gè)二進(jìn)制映射,即設(shè)置動(dòng)態(tài)障礙物、靜態(tài)障礙物及邊界所在單元格標(biāo)記為1,在代價(jià)地圖采用不同的顏色表示障礙物類型及邊界;自由區(qū)域標(biāo)記為0,設(shè)置為空白單元格。

初始代價(jià)地圖設(shè)置如圖4,靜態(tài)層為所建立的環(huán)境地圖邊界,障礙物層包括環(huán)境中已有的障礙物及激光雷達(dá)實(shí)時(shí)檢測到的靜態(tài)障礙物,膨脹層是在障礙物外層增加膨脹范圍,使機(jī)器人在導(dǎo)航過程中能夠與障礙物保持更加安全的防碰撞距離。

為優(yōu)化后的代價(jià)地圖如圖5,通過對實(shí)時(shí)檢測到的動(dòng)態(tài)障礙物激光點(diǎn)云進(jìn)行分割與聚類,生成動(dòng)態(tài)障礙物層代價(jià)地圖,并在初始代價(jià)地圖對應(yīng)的膨脹層中增加設(shè)置動(dòng)態(tài)障礙物的膨脹范圍。

圖4 初始代價(jià)地圖

圖5 優(yōu)化后代價(jià)地圖

3.2 動(dòng)態(tài)障礙物軌跡跟蹤與預(yù)測

為實(shí)現(xiàn)動(dòng)態(tài)避障,機(jī)器人感知系統(tǒng)需要對每個(gè)動(dòng)態(tài)障礙物的運(yùn)動(dòng)參數(shù)進(jìn)行估計(jì)與預(yù)測。隨著動(dòng)態(tài)障礙物運(yùn)動(dòng),激光雷達(dá)將獲取到的障礙物點(diǎn)云數(shù)據(jù)上傳到可視化端,動(dòng)態(tài)障礙物代價(jià)地圖層也隨之更新動(dòng)態(tài)障礙物信息。將當(dāng)前代價(jià)地圖中所占用的單元格與歷史代價(jià)地圖上動(dòng)態(tài)障礙物層單元格進(jìn)行軌跡分析,構(gòu)成數(shù)據(jù)關(guān)聯(lián)問題。

在軌跡跟蹤的過程中,新檢測的障礙物狀態(tài)如果能夠與多個(gè)之前檢測的障礙物狀態(tài)關(guān)聯(lián),則將過去障礙物的平均狀態(tài)作為新障礙物平均狀態(tài),利用卡爾曼濾波跟蹤障礙物的目的就是為了獲取障礙物的實(shí)時(shí)狀態(tài),其狀態(tài)包括位置、速度、方向、障礙物的特征識別。隨著采樣時(shí)間的迭代,卡爾曼濾波器將對動(dòng)態(tài)障礙物的軌跡進(jìn)行更新預(yù)測并加以校正,跟蹤過程中動(dòng)態(tài)障礙物狀態(tài)的所有參數(shù)也隨著進(jìn)行同步更新。

動(dòng)態(tài)障礙物在t時(shí)刻的位置與運(yùn)動(dòng)狀態(tài)Ot表示為式(9):

Ot=[xt,yt,vx,vy,kt]

(9)

式中:xt、yt為障礙物在t時(shí)刻的坐標(biāo)位置;vx、vy為動(dòng)態(tài)障礙物t時(shí)刻投影到x、y軸上的速度分量;kt為障礙物的運(yùn)動(dòng)方向。

由此,可得到卡爾曼濾波對于動(dòng)態(tài)障礙物t+1時(shí)刻運(yùn)動(dòng)狀態(tài)Ot+1的預(yù)測方程如式(10):

Ot+1=AOt+But

(10)

式中:A為狀態(tài)轉(zhuǎn)移矩陣;B為噪聲增益矩陣;ut為過程噪聲。

根據(jù)傳感器獲得動(dòng)態(tài)障礙物t+1時(shí)刻運(yùn)動(dòng)狀態(tài)測量值與濾波器對系統(tǒng)t+1時(shí)刻預(yù)測狀態(tài),可得到測量值與預(yù)測值之間存在的誤差以及預(yù)測誤差協(xié)方差矩陣如式(11)和式(12):

o=Z-HOt+1

(11)

Pt+1=APtAT+Q

(12)

式中:o為t+1時(shí)刻系統(tǒng)狀態(tài)測量值與預(yù)測值之間存在的誤差;Z為傳感器傳遞的狀態(tài)測量向量;H為觀測矩陣,將預(yù)測的運(yùn)動(dòng)狀態(tài)向量轉(zhuǎn)換為測量向量相同數(shù)據(jù)類型;Q為過程噪聲協(xié)方差矩陣。

得到卡爾曼增益矩陣方程如式(13):

Kt+1=Pt+1HT(R+HPt+1HT)-1

(13)

式中:R為測量噪聲協(xié)方差矩陣,由于傳感器測量只有位置信息,所以此處R為位置的方差。

根據(jù)卡爾曼濾波可對動(dòng)態(tài)障礙物運(yùn)動(dòng)狀態(tài)矩陣式(10)以及預(yù)測誤差協(xié)方差矩陣式(12)分別進(jìn)行更新,表示為式(14)和式(15):

Ot+1=(I-Kt+1H)Qt+1+Kt+1Z

(14)

Pt+1=(I-Kt+1H)Pt+1

(15)

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

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

通過開源仿真平臺GAZEBO設(shè)置仿真環(huán)境(包括靜態(tài)場景以及動(dòng)態(tài)障礙物),用于驗(yàn)證該算法在動(dòng)態(tài)障礙物場景下避障的有效性。仿真對象為具備阿克曼轉(zhuǎn)向結(jié)構(gòu)的非完整運(yùn)動(dòng)學(xué)約束機(jī)器人,配置的傳感器為單線2維激光雷達(dá),IMU(慣性測量單元),里程計(jì)。仿真計(jì)算機(jī)配置:Intel Core i5-4210U,雙核四線程,Ubuntu18.04/ROS Melodic。

仿真環(huán)境設(shè)置:動(dòng)態(tài)障礙物運(yùn)動(dòng)設(shè)計(jì)包括與機(jī)器人運(yùn)動(dòng)方向垂直,與機(jī)器人運(yùn)動(dòng)方向同向的兩種運(yùn)動(dòng)狀態(tài)。通過仿真實(shí)驗(yàn),分析改進(jìn)后的算法在面對兩種運(yùn)動(dòng)方向的動(dòng)態(tài)障礙物時(shí)的避障路徑,并對機(jī)器人避障仿真結(jié)果進(jìn)行分析與討論。動(dòng)態(tài)仿真環(huán)境設(shè)置如圖6,圖中5個(gè)作勻速往復(fù)運(yùn)動(dòng)的動(dòng)態(tài)障礙物中,①②④⑤沿著Xw軸運(yùn)動(dòng),③沿著Yw軸運(yùn)動(dòng)。

圖6 動(dòng)態(tài)仿真環(huán)境設(shè)置

為了模擬復(fù)雜真實(shí)環(huán)境與機(jī)器人運(yùn)動(dòng),以動(dòng)態(tài)行人及真實(shí)轎車尺寸作為參考,其中行人的平均運(yùn)動(dòng)速度為3～5 m/s,復(fù)雜環(huán)境中車輛的前向速度不大于25 km/h。根據(jù)仿真場景中,動(dòng)態(tài)障礙物和智能小車的尺寸與運(yùn)動(dòng)性能和行人尺寸與真實(shí)車輛尺寸、運(yùn)動(dòng)性能比較,按照1∶5的比例設(shè)置小車與動(dòng)態(tài)障礙物運(yùn)動(dòng)參數(shù)如表1、表2。

表1 動(dòng)態(tài)障礙物運(yùn)動(dòng)參數(shù)

表2 機(jī)器人運(yùn)動(dòng)參數(shù)

4.2 全局路徑規(guī)劃算法選擇

將全局路徑規(guī)劃算法A*與Dijkstra算法分別和TEB算法組合進(jìn)行仿真實(shí)驗(yàn),兩種算法組合下機(jī)器人運(yùn)動(dòng)參數(shù)設(shè)置不變,每個(gè)算法組合各進(jìn)行20次實(shí)驗(yàn)。比較不同全局路徑規(guī)劃算法規(guī)劃的路徑長度,路徑的平滑度以及機(jī)器人運(yùn)動(dòng)時(shí)間,其中路徑平滑度定義如下:通過均勻選取全局路徑上的路徑點(diǎn)P′,共選取NP′個(gè)路徑點(diǎn),計(jì)算路徑點(diǎn)之間的角速度變化率α,設(shè)α> 20%的路徑點(diǎn)數(shù)量為Nα, 從而按式(16)求得路徑平滑度ηsmooth:

(16)

仿真結(jié)果平均值如表3。通過對全局路徑規(guī)劃出的路徑長度、路徑平滑度及機(jī)器人運(yùn)動(dòng)時(shí)間進(jìn)行比較,選擇Dijkstra+TEB的組合算法,其中Dijkstra算法作為全局路徑規(guī)劃算法,TEB算法作為局部軌跡規(guī)劃算法,該組合算法能夠根據(jù)起始點(diǎn)與終止點(diǎn)狀態(tài)以及環(huán)境地圖信息,求解出兩點(diǎn)間最短且更加平滑的路徑,實(shí)現(xiàn)運(yùn)動(dòng)點(diǎn)到點(diǎn)導(dǎo)航任務(wù)時(shí)間最小化。

表3 全局路徑規(guī)劃算法比較

4.3 仿真實(shí)驗(yàn)結(jié)果分析

利用Rviz對仿真結(jié)果進(jìn)行可視化分析。針對動(dòng)態(tài)障礙物與機(jī)器人運(yùn)動(dòng)方向垂直,以及動(dòng)態(tài)障礙物與機(jī)器人運(yùn)動(dòng)方向同向,分別進(jìn)行機(jī)器人避障軌跡分析(圖7)。2種障礙物運(yùn)動(dòng)方向均進(jìn)行20次仿真測試實(shí)驗(yàn)。

圖7 機(jī)器人避障軌跡

障礙物實(shí)際位置如圖7(a)中文字描述與箭頭指向所示,障礙物周邊存在一圈代價(jià)地圖膨脹層,其余空白區(qū)域部分代表環(huán)境中車輛可自由運(yùn)動(dòng)區(qū)域。機(jī)器人當(dāng)前位置、規(guī)劃的全局路徑與局部軌跡以及動(dòng)態(tài)障礙物的當(dāng)前位置與歷史代價(jià)地圖如圖7(a)中文字標(biāo)注。機(jī)器人在沿著全局路徑向終點(diǎn)運(yùn)動(dòng)時(shí),遭遇到②動(dòng)態(tài)障礙物,根據(jù)圖中②動(dòng)態(tài)障礙物歷史代價(jià)地圖與當(dāng)前時(shí)刻障礙物位置可知,②障礙沿著Xw軸向其負(fù)半軸作勻速運(yùn)動(dòng),此時(shí)障礙物運(yùn)動(dòng)方向與機(jī)器人運(yùn)動(dòng)方向垂直。

在當(dāng)前時(shí)刻,機(jī)器人沿著根據(jù)Dijkstra算法規(guī)劃出的全局路徑向終點(diǎn)運(yùn)動(dòng),局部規(guī)劃的軌跡如圖7(a)。根據(jù)障礙物歷史代價(jià)地圖結(jié)合卡爾曼濾波,對②動(dòng)態(tài)障礙物的運(yùn)動(dòng)軌跡進(jìn)行跟蹤并預(yù)測,判斷下一時(shí)刻②障礙物軌跡會與機(jī)器人軌跡存在位置沖突,潛在沖突點(diǎn)如圖7(b),故改進(jìn)的動(dòng)態(tài)避障軌跡規(guī)劃系統(tǒng)通過對機(jī)器人的局部軌跡進(jìn)行在線優(yōu)化,生成的優(yōu)化結(jié)果如圖7(b)中優(yōu)化后的局部軌跡,Dijkstra算法根據(jù)當(dāng)前車輛位置重新對全局路徑進(jìn)行最短路徑搜索。

機(jī)器人沿著根據(jù)Dijkstra算法規(guī)劃出的全局路徑向終點(diǎn)運(yùn)動(dòng),當(dāng)機(jī)器人運(yùn)行到如圖7(c)中機(jī)器人位置,此時(shí)檢測到③動(dòng)態(tài)障礙物,同樣根據(jù)③動(dòng)態(tài)障礙物歷史代價(jià)地圖與當(dāng)前障礙物的位置可得,③障礙物正沿著Yw向其負(fù)半軸作勻速往復(fù)運(yùn)動(dòng),且該經(jīng)過潛在沖突點(diǎn)后會調(diào)轉(zhuǎn)方向沿著Yw向其正半軸運(yùn)動(dòng)。當(dāng)前時(shí)刻動(dòng)態(tài)障礙物與機(jī)器人運(yùn)動(dòng)方向?yàn)橥颉?/p>

若按照圖7(c)所規(guī)劃的路徑行駛,機(jī)器人與③障礙物將在潛在沖突點(diǎn)處會發(fā)生碰撞,且由于③障礙物是作往返運(yùn)動(dòng),機(jī)器人可能因?yàn)檎系K物的返回而規(guī)劃出倒退避讓的軌跡,增加了行駛的危險(xiǎn)性以及導(dǎo)航運(yùn)動(dòng)時(shí)間。改進(jìn)的動(dòng)態(tài)避障軌跡規(guī)劃系統(tǒng)重新規(guī)劃處合理的路徑如圖7(d),優(yōu)化后的局部軌跡從障礙物上方繞行徑,避免在障礙物交匯處陷入局部最優(yōu)或發(fā)生碰撞,此時(shí)Dijkstra算法規(guī)劃的全局路徑雖是最短路徑,但會導(dǎo)致碰撞。

由以上仿真實(shí)驗(yàn)分析可知:改進(jìn)的動(dòng)態(tài)避障軌跡規(guī)劃系統(tǒng)能夠滿足實(shí)時(shí)動(dòng)態(tài)避障要求及運(yùn)動(dòng)時(shí)間最優(yōu)化。經(jīng)過上述各20次仿真實(shí)驗(yàn),機(jī)器人與動(dòng)態(tài)障礙物發(fā)生碰撞的次數(shù)為3次,其中避障失敗的實(shí)驗(yàn)中,碰撞發(fā)生均出現(xiàn)在動(dòng)態(tài)障礙物代價(jià)地圖以及周圍靜態(tài)障礙物代價(jià)地圖出現(xiàn)重合或者交叉時(shí),例如②、③動(dòng)態(tài)障礙物在運(yùn)動(dòng)過程中與環(huán)境地圖發(fā)生代價(jià)地圖的重合,此時(shí)機(jī)器人嘗試求解軌跡而可行域過小導(dǎo)致解陷入局部最優(yōu),機(jī)器人避讓不及導(dǎo)致與機(jī)器人發(fā)生碰撞。根據(jù)結(jié)論分析得改進(jìn)的動(dòng)態(tài)避障軌跡規(guī)劃系統(tǒng)動(dòng)態(tài)避障成功率為92.5%,該動(dòng)態(tài)避障規(guī)劃系統(tǒng)可有效對動(dòng)態(tài)障礙物進(jìn)行實(shí)時(shí)避障,機(jī)器人能成功完成導(dǎo)航并到達(dá)目標(biāo)點(diǎn)。

5 真實(shí)動(dòng)態(tài)環(huán)境避障實(shí)驗(yàn)

實(shí)驗(yàn)測試的移動(dòng)機(jī)器人如圖8,該移動(dòng)機(jī)器人Nanocar采用阿克曼轉(zhuǎn)向結(jié)構(gòu),主要傳感器配備思嵐二維激光雷達(dá),IMU,里程計(jì)。車載計(jì)算機(jī)系統(tǒng)采用微型計(jì)算機(jī)樹莓派4B,4核8線程CPU,頻率為1.5 GHz。

圖8 真實(shí)實(shí)驗(yàn)阿克曼轉(zhuǎn)向結(jié)構(gòu)機(jī)器人

在真實(shí)動(dòng)態(tài)環(huán)境中,同樣設(shè)置與機(jī)器人運(yùn)動(dòng)方向同向及垂向兩種運(yùn)動(dòng)狀態(tài)的障礙物。

圖9中動(dòng)態(tài)障礙物運(yùn)動(dòng)方向與機(jī)器人運(yùn)動(dòng)方向?yàn)橄嘞?機(jī)器人檢測障礙物并完成避障的全過程。機(jī)器人位置,全局路徑以及動(dòng)態(tài)障礙物位置的運(yùn)動(dòng)方向如圖中標(biāo)識,其中V0為動(dòng)態(tài)障礙物的運(yùn)動(dòng)速度與方向。機(jī)器人實(shí)時(shí)檢測到動(dòng)態(tài)障礙物并根據(jù)系統(tǒng)的預(yù)測作出軌跡規(guī)劃完成避障的過程。

圖9 機(jī)器人與動(dòng)態(tài)障礙物相向運(yùn)動(dòng)場景

圖9(a)中,實(shí)驗(yàn)員勻速推動(dòng)柜子作為動(dòng)態(tài)障礙物,但尚未進(jìn)入機(jī)器人軌跡規(guī)劃算法考慮的范圍,此時(shí)全局路徑與局部軌跡重合;圖9(b)中規(guī)劃系統(tǒng)通過動(dòng)態(tài)障礙物歷史代價(jià)地圖判斷出障礙物的運(yùn)動(dòng)方向并進(jìn)行軌跡跟蹤,通過預(yù)測出障礙物的下一時(shí)刻運(yùn)動(dòng)狀態(tài),實(shí)時(shí)規(guī)劃出避讓障礙物的運(yùn)動(dòng)軌跡;圖9(c)為機(jī)器人完成動(dòng)態(tài)避障后的運(yùn)動(dòng)狀態(tài),以及與動(dòng)態(tài)障礙物的相對位置關(guān)系。

圖10中動(dòng)態(tài)障礙物運(yùn)動(dòng)方向與機(jī)器人運(yùn)動(dòng)方向?yàn)榇怪薄?/p>

圖10(a)中為當(dāng)前機(jī)器人的位置,動(dòng)態(tài)障礙物的位置及其歷史代價(jià)地圖。圖10(b)中,改進(jìn)的動(dòng)態(tài)避障規(guī)劃系統(tǒng)對局部軌跡進(jìn)行優(yōu)化,選擇從動(dòng)態(tài)障礙物右側(cè)通過。圖10(c)中顯示了避障過程完成后機(jī)器人與動(dòng)態(tài)障礙物的相對位置關(guān)系。機(jī)器人改變路徑,避開障礙物成功通過。

在同一環(huán)境下進(jìn)行重復(fù)多次實(shí)驗(yàn),驗(yàn)證該動(dòng)態(tài)避障軌跡規(guī)劃系統(tǒng)能夠在實(shí)時(shí)檢測到動(dòng)態(tài)障礙物后,結(jié)合歷史代價(jià)地圖,通過卡爾曼濾波進(jìn)行障礙物軌跡跟蹤以及運(yùn)動(dòng)狀態(tài)預(yù)測,規(guī)劃出安全且滿足最小化運(yùn)動(dòng)時(shí)間的平滑避障路徑,順利完成導(dǎo)航任務(wù)到達(dá)目標(biāo)點(diǎn)。

6 結(jié) 語

針對阿克曼轉(zhuǎn)向結(jié)構(gòu)的移動(dòng)機(jī)器人在動(dòng)態(tài)場景下的實(shí)時(shí)避障問題,提出了基于改進(jìn)時(shí)間彈性帶并結(jié)合卡爾曼濾波的動(dòng)態(tài)避障軌跡規(guī)劃系統(tǒng)。首先對阿克曼轉(zhuǎn)向結(jié)構(gòu)機(jī)器人進(jìn)行運(yùn)動(dòng)學(xué)模型建立并分析。算法上,通過在時(shí)間彈性帶算法中增加動(dòng)態(tài)障礙物代價(jià)地圖層實(shí)現(xiàn)動(dòng)態(tài)障礙物的檢測,結(jié)合卡爾曼濾波對動(dòng)態(tài)障礙物進(jìn)行軌跡跟蹤以及障礙物運(yùn)動(dòng)狀態(tài)預(yù)測,從而實(shí)時(shí)規(guī)劃出安全的動(dòng)態(tài)障礙物避障軌跡,使機(jī)器人能夠在復(fù)雜動(dòng)態(tài)環(huán)境中完成同向及垂向的障礙物運(yùn)動(dòng)避障,實(shí)現(xiàn)無碰撞導(dǎo)航。

通過設(shè)置仿真動(dòng)態(tài)環(huán)境以及真實(shí)動(dòng)態(tài)場景對該系統(tǒng)進(jìn)行實(shí)驗(yàn)分析討論。結(jié)合可視化軟件對動(dòng)態(tài)障礙物檢測,軌跡跟蹤預(yù)測及優(yōu)化前后軌跡對比,通過實(shí)際場景與可視化動(dòng)態(tài)避障過程進(jìn)行對應(yīng),證明了該動(dòng)態(tài)避障軌跡規(guī)劃系統(tǒng)能夠在動(dòng)態(tài)場景環(huán)境下實(shí)現(xiàn)動(dòng)態(tài)避障,完成導(dǎo)航目標(biāo),驗(yàn)證了該系統(tǒng)的有效性。

該系統(tǒng)在TEB中增加動(dòng)態(tài)障礙物代價(jià)地圖,構(gòu)建多層代價(jià)地圖對場景環(huán)境進(jìn)行分解處理,同時(shí)僅通過對單線激光雷達(dá)采集的點(diǎn)云進(jìn)行濾波,結(jié)合卡爾曼濾波實(shí)現(xiàn)動(dòng)態(tài)障礙物的追蹤與預(yù)測,實(shí)現(xiàn)避障完成導(dǎo)航工作。