改進時間彈性帶的動態避障軌跡規劃系統研究

謝春麗,劉斐灝

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

0 引 言

移動機器人的自主導航中,避障是最關鍵的功能之一[1]。避障的實時性與穩定性是當前移動機器人路徑規劃的典型挑戰[2]。隨著場景的復雜性增加,動態障礙物對于算法的實時性要求增加[3],為在動態環境下生成安全平滑的軌跡,需要軌跡規劃算法能夠在動態場景下對動態障礙物實時進行軌跡跟蹤與運動狀態預測[4]。

現有機器人動態避障方法存在對復雜動態環境處理能力不夠,易陷入局部最小值[5],且實時性不滿足實際應用的需要,且在混合場景下無法準確區分動態與靜態障礙物類型[6]。因此動態避障軌跡規劃系統需要對復雜環境有較優的處理能力,并能實時求解出可行軌跡[7]。

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

TEB算法能夠產生一條無碰撞的時間最優化運動路徑,對環境中的障礙物進行實時的檢測并作出在線規劃。動態障礙物環境下,軌跡規劃算法需要對環境信息有較優且快速的處理能力,并且能對動態障礙物進行軌跡跟蹤與預測[10]。然而,TEB算法在動態場景下會將動態障礙物視為靜態去處理,導致機器人在導航過程中與動態障礙物發生碰撞。

針對上述問題,筆者采用不同頻率的均值濾波器區分動、靜態障礙物,增加動態障礙物代價地圖,對障礙物進行聚類處理[11],結合卡爾曼濾波[12]對障礙物的運動軌跡進行跟蹤預測,通過預測障礙物軌跡,結合TEB算法規劃合理的軌跡進行避障。

1 阿克曼機器人的運動學模型

采取阿克曼轉向結構機器人進行仿真與真實實驗,阿克曼轉向結構機器人的運動學模型如圖1。

圖1 阿克曼轉向結構機器人運動學模型

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

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

(1)

式中:ω為機器人角速度。

設在t時刻,機器人姿態由位置狀態s(t)和運動狀態u(t)=[v(t),φ(t)]T表示,則阿克曼機器人運動學狀態方程表示為式(2):

(2)

2 TEB算法原理與求解優化流程

2.1 TEB算法基本原理

TEB算法最初是為移動基礎機器人導航規劃開發的。通過在彈性帶的基礎上引入時間因素,增加機器人相鄰位姿的時間間隔。因此,機器人的離散運動軌跡T可描述為N個有序的位姿與時間間隔的集合,表示為式(3):

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

(3)

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

式中:Sk表示機器人在世界坐標系下的位姿,其中Pk為機器人坐標。ΔTk為位姿Sk過渡到Sk+1的時間間隔。

由連續機器人位姿組成的軌跡示例如圖2。

圖2 軌跡連續位姿序列與時間間隔

2.2 TEB求解優化流程

在TEB中引入的時間因素將機器人控制系統與速度,加速度等動態約束進行拓展,能夠為局部規劃器提供一個持續優化的解決方案,在非完整約束移動機器人上得以適用[13],如圖3。

通過在局部代價地圖中對全局路徑的子集進行采樣,創建局部姿態序列,將全局路徑轉變為彈性帶進行優化,其生成的路徑不一定是最短路徑,但能夠滿足時間最優。在障礙物環境較多情況下,為防止在路徑規劃過程陷入局部極小值,對具有不同拓撲結構的多條軌跡進行維護和優化,并提出兩階段優化算法解決軌跡局部最優問題[14],而后針對阿克曼轉向結構機器人進行拓展[15]。

圖3 TEB算法的優化流程

最優軌跡b*是通過最小化代價函數獲得,這些函數包括運動規劃中的部分沖突目標與約束。這些目標包括CPU資源消耗、路徑長度、總運動時間或以上目標函數的加權組合。

通過對不符合移動機器人運動學約束的軌跡加以懲罰函數,將軌跡允許解限制在可行集內。TEB的優化問題被定義為一個加權非線性最小二乘代價函數,該代價函數考慮目標函數J和懲罰函數P的沖突集,表示為:

(4)

式中:b{s1,sN}表示軌跡中的起始位姿s1與目標位姿sN不受優化;σi為每個目標函數的加權因子;fJ為目標函數的綜合,目標函數J中包括計算資源,軌跡,運動時間等因素;fP為懲罰函數綜合,懲罰函數P包括障礙物約束、非完整運動學約束、速度與加速度約束等。

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

3 基于卡爾曼濾波的障礙物運動估計

在ROS的運動規劃框架中,代價地圖為全局及局部規劃器提供實時環境信息。代價地圖用一個網格表示,其中每個單元格存在3種狀態設定:即空閑(0)、占用(254)和未知(255),代價地圖以特定的速度進行更新,但其只能表示靜態障礙物的信息。

筆者提出了一個實時的動態障礙物避障策略,使用快速和慢速均值濾波器對二維激光雷達點云數據進行預處理,將動態障礙物從環境中分離出來,完成動態障礙物檢測,建立動態障礙物的代價地圖層對檢測到的動態障礙物進行聚類處理,利用卡爾曼濾波方法,根據當前動態障礙物的運動狀態進行軌跡跟蹤與狀態預測。

3.1 動態障礙物檢測

對于代價地圖的每一個單元格,使用一個快速運行均值濾波器與一個慢速運行均值濾波器,對機器人前方探測到的激光點云進行預處理。其中單個單元格的濾波器表示為:

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

(5)

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

(6)

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

通過兩個不同運行速度的均值濾波器對激光點云數據進行高頻噪聲與低頻噪聲濾除,實現對動態障礙物占用單元格的識別?？焖龠^濾器激活閾值為c1,快速和慢速濾波器之間的差異必須超過閾值c2,以消除低頻噪聲的準靜態障礙,表示為式(7)、式(8):

Pf(t)>c1

(7)

Pf(t)-Ps(t)>c2

(8)

通過設定閾值操作,障礙物將在代價地圖中生成一個二進制映射,即設置動態障礙物、靜態障礙物及邊界所在單元格標記為1,在代價地圖采用不同的顏色表示障礙物類型及邊界;自由區域標記為0,設置為空白單元格。

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

為優化后的代價地圖如圖5,通過對實時檢測到的動態障礙物激光點云進行分割與聚類,生成動態障礙物層代價地圖,并在初始代價地圖對應的膨脹層中增加設置動態障礙物的膨脹范圍。

圖4 初始代價地圖

圖5 優化后代價地圖

3.2 動態障礙物軌跡跟蹤與預測

為實現動態避障,機器人感知系統需要對每個動態障礙物的運動參數進行估計與預測。隨著動態障礙物運動,激光雷達將獲取到的障礙物點云數據上傳到可視化端,動態障礙物代價地圖層也隨之更新動態障礙物信息。將當前代價地圖中所占用的單元格與歷史代價地圖上動態障礙物層單元格進行軌跡分析,構成數據關聯問題。

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

動態障礙物在t時刻的位置與運動狀態Ot表示為式(9):

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

(9)

式中:xt、yt為障礙物在t時刻的坐標位置;vx、vy為動態障礙物t時刻投影到x、y軸上的速度分量;kt為障礙物的運動方向。

由此,可得到卡爾曼濾波對于動態障礙物t+1時刻運動狀態Ot+1的預測方程如式(10):

Ot+1=AOt+But

(10)

式中:A為狀態轉移矩陣;B為噪聲增益矩陣;ut為過程噪聲。

根據傳感器獲得動態障礙物t+1時刻運動狀態測量值與濾波器對系統t+1時刻預測狀態,可得到測量值與預測值之間存在的誤差以及預測誤差協方差矩陣如式(11)和式(12):

o=Z-HOt+1

(11)

Pt+1=APtAT+Q

(12)

式中:o為t+1時刻系統狀態測量值與預測值之間存在的誤差;Z為傳感器傳遞的狀態測量向量;H為觀測矩陣,將預測的運動狀態向量轉換為測量向量相同數據類型;Q為過程噪聲協方差矩陣。

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

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

(13)

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

根據卡爾曼濾波可對動態障礙物運動狀態矩陣式(10)以及預測誤差協方差矩陣式(12)分別進行更新,表示為式(14)和式(15):

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

(14)

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

(15)

4 實驗分析

4.1 仿真實驗設置

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

仿真環境設置:動態障礙物運動設計包括與機器人運動方向垂直,與機器人運動方向同向的兩種運動狀態。通過仿真實驗,分析改進后的算法在面對兩種運動方向的動態障礙物時的避障路徑,并對機器人避障仿真結果進行分析與討論。動態仿真環境設置如圖6,圖中5個作勻速往復運動的動態障礙物中,①②④⑤沿著Xw軸運動,③沿著Yw軸運動。

圖6 動態仿真環境設置

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

表1 動態障礙物運動參數

表2 機器人運動參數

4.2 全局路徑規劃算法選擇

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

(16)

仿真結果平均值如表3。通過對全局路徑規劃出的路徑長度、路徑平滑度及機器人運動時間進行比較,選擇Dijkstra+TEB的組合算法,其中Dijkstra算法作為全局路徑規劃算法,TEB算法作為局部軌跡規劃算法,該組合算法能夠根據起始點與終止點狀態以及環境地圖信息,求解出兩點間最短且更加平滑的路徑,實現運動點到點導航任務時間最小化。

表3 全局路徑規劃算法比較

4.3 仿真實驗結果分析

利用Rviz對仿真結果進行可視化分析。針對動態障礙物與機器人運動方向垂直,以及動態障礙物與機器人運動方向同向,分別進行機器人避障軌跡分析(圖7)。2種障礙物運動方向均進行20次仿真測試實驗。

圖7 機器人避障軌跡

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

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

機器人沿著根據Dijkstra算法規劃出的全局路徑向終點運動,當機器人運行到如圖7(c)中機器人位置,此時檢測到③動態障礙物,同樣根據③動態障礙物歷史代價地圖與當前障礙物的位置可得,③障礙物正沿著Yw向其負半軸作勻速往復運動,且該經過潛在沖突點后會調轉方向沿著Yw向其正半軸運動。當前時刻動態障礙物與機器人運動方向為同向。

若按照圖7(c)所規劃的路徑行駛,機器人與③障礙物將在潛在沖突點處會發生碰撞,且由于③障礙物是作往返運動,機器人可能因為障礙物的返回而規劃出倒退避讓的軌跡,增加了行駛的危險性以及導航運動時間。改進的動態避障軌跡規劃系統重新規劃處合理的路徑如圖7(d),優化后的局部軌跡從障礙物上方繞行徑,避免在障礙物交匯處陷入局部最優或發生碰撞,此時Dijkstra算法規劃的全局路徑雖是最短路徑,但會導致碰撞。

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

5 真實動態環境避障實驗

實驗測試的移動機器人如圖8,該移動機器人Nanocar采用阿克曼轉向結構,主要傳感器配備思嵐二維激光雷達,IMU,里程計。車載計算機系統采用微型計算機樹莓派4B,4核8線程CPU,頻率為1.5 GHz。

圖8 真實實驗阿克曼轉向結構機器人

在真實動態環境中,同樣設置與機器人運動方向同向及垂向兩種運動狀態的障礙物。

圖9中動態障礙物運動方向與機器人運動方向為相向,機器人檢測障礙物并完成避障的全過程。機器人位置,全局路徑以及動態障礙物位置的運動方向如圖中標識,其中V0為動態障礙物的運動速度與方向。機器人實時檢測到動態障礙物并根據系統的預測作出軌跡規劃完成避障的過程。

圖9 機器人與動態障礙物相向運動場景

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

圖10中動態障礙物運動方向與機器人運動方向為垂直。

圖10(a)中為當前機器人的位置,動態障礙物的位置及其歷史代價地圖。圖10(b)中,改進的動態避障規劃系統對局部軌跡進行優化,選擇從動態障礙物右側通過。圖10(c)中顯示了避障過程完成后機器人與動態障礙物的相對位置關系。機器人改變路徑,避開障礙物成功通過。

在同一環境下進行重復多次實驗,驗證該動態避障軌跡規劃系統能夠在實時檢測到動態障礙物后,結合歷史代價地圖,通過卡爾曼濾波進行障礙物軌跡跟蹤以及運動狀態預測,規劃出安全且滿足最小化運動時間的平滑避障路徑,順利完成導航任務到達目標點。

6 結 語

針對阿克曼轉向結構的移動機器人在動態場景下的實時避障問題,提出了基于改進時間彈性帶并結合卡爾曼濾波的動態避障軌跡規劃系統。首先對阿克曼轉向結構機器人進行運動學模型建立并分析。算法上,通過在時間彈性帶算法中增加動態障礙物代價地圖層實現動態障礙物的檢測,結合卡爾曼濾波對動態障礙物進行軌跡跟蹤以及障礙物運動狀態預測,從而實時規劃出安全的動態障礙物避障軌跡,使機器人能夠在復雜動態環境中完成同向及垂向的障礙物運動避障,實現無碰撞導航。

通過設置仿真動態環境以及真實動態場景對該系統進行實驗分析討論。結合可視化軟件對動態障礙物檢測,軌跡跟蹤預測及優化前后軌跡對比,通過實際場景與可視化動態避障過程進行對應,證明了該動態避障軌跡規劃系統能夠在動態場景環境下實現動態避障,完成導航目標,驗證了該系統的有效性。

該系統在TEB中增加動態障礙物代價地圖,構建多層代價地圖對場景環境進行分解處理,同時僅通過對單線激光雷達采集的點云進行濾波,結合卡爾曼濾波實現動態障礙物的追蹤與預測,實現避障完成導航工作。