基于改進(jìn)TEB算法的阿克曼機(jī)器人運(yùn)動規(guī)劃系統(tǒng)

鄭凱林，韓寶玲，王新達(dá)

(北京理工大學(xué)機(jī)械與車輛學(xué)院，北京 100081)

移動機(jī)器人的路徑規(guī)劃分為全局路徑規(guī)劃和局部路徑規(guī)劃。全局路徑規(guī)劃負(fù)責(zé)在全局地圖尋找一條從起點(diǎn)到終點(diǎn)無碰撞的路徑，但一般不具有實(shí)時更新路徑動態(tài)避障而且滿足運(yùn)動學(xué)動力學(xué)約束的特點(diǎn)。局部路徑規(guī)劃起到修飾全局路徑，動態(tài)更新調(diào)整路徑避開動態(tài)障礙物的作用，機(jī)器人局部路徑規(guī)劃中常見的算法有動態(tài)窗口法[1]，在可行的速度采樣空間[v,w],模擬在未來時間段機(jī)器人可以達(dá)到的軌跡集合。設(shè)置評價函數(shù)評估軌跡的代價,選擇代價最小的軌跡對應(yīng)的速度發(fā)送給機(jī)器人底盤。該方法適用于全向移動和差速機(jī)器人，不適用阿克曼轉(zhuǎn)向幾何約束的機(jī)器人。廣東工業(yè)大學(xué)徐寶來等提出了適用阿克曼機(jī)器人的改進(jìn)動態(tài)窗口法，將線速度角速度采樣空間[v,w]替換成線速度虛擬轉(zhuǎn)向角空間[v,α][2],軌跡的優(yōu)劣受采樣的分辨率和評價函數(shù)的參數(shù)有關(guān)，而且控制動作在預(yù)測范圍內(nèi)保持不變。人工勢場法[3]也是廣泛應(yīng)用在機(jī)器人局部路徑規(guī)劃上[4]，但是機(jī)器人容易陷入障礙物斥力函數(shù)和目標(biāo)點(diǎn)引力函數(shù)平衡而造成目標(biāo)點(diǎn)不可達(dá)。燕山大學(xué)王洪斌等提出增設(shè)虛擬子目標(biāo)的方法解決局部極小值問題,利用自適應(yīng)步長調(diào)節(jié)算法對移動機(jī)器人的步長進(jìn)行了動態(tài)優(yōu)化[5]。改進(jìn)的人工勢場法都是為了機(jī)器人避開障礙物脫困，并無對具有非完整約束的阿克曼機(jī)器人運(yùn)動學(xué)和動力學(xué)的考慮。Rosmann等提出了TEB算法[6]，該算法將傳統(tǒng)的運(yùn)動規(guī)劃問題表述為一個圖優(yōu)化問題，可以適用于完整約束和非完整約束機(jī)器人[7]，后又針對阿克曼機(jī)器人進(jìn)行了擴(kuò)充支持[8]，由牛頓第二定律可知，加速度反映在加速度運(yùn)動時受到的力，加加速度則反映這作用力的變化快慢。較大的加加速度將會產(chǎn)生沖擊感，例如在電梯升降，汽車、飛機(jī)等加速和轉(zhuǎn)彎的過程中。

對于機(jī)器人而言，缺少對加加速度的約束，電機(jī)輸出力矩短時間產(chǎn)生較大范圍突變會對阿克曼機(jī)器人產(chǎn)生沖擊震蕩。在此研究基礎(chǔ)上，現(xiàn)在多目標(biāo)優(yōu)化問題約束上構(gòu)建加加速度約束，使動力學(xué)約束更加完善，并將該算法應(yīng)用在阿克曼機(jī)器人運(yùn)動規(guī)劃上，進(jìn)行了仿真和真實(shí)機(jī)器人實(shí)驗(yàn)。將從阿克曼機(jī)器人運(yùn)動學(xué)模型構(gòu)建、TEB算法模型構(gòu)建和求解優(yōu)化改進(jìn)、導(dǎo)航框架設(shè)計、仿真和真實(shí)機(jī)器人測試等方面進(jìn)行論述。

1 阿克曼機(jī)器人運(yùn)動模型構(gòu)建

機(jī)器人運(yùn)動規(guī)劃需要建立機(jī)器人的運(yùn)動學(xué)模型，在世界坐標(biāo)下對阿克曼機(jī)器人構(gòu)型描述如圖1所示。

圖1 阿克曼機(jī)器人在世界坐標(biāo)系的構(gòu)型

世界坐標(biāo)系為OWXWYW,機(jī)器人的基坐標(biāo)系設(shè)置在機(jī)器人后軸中心Or上，ICR是阿克曼機(jī)器人的瞬時旋轉(zhuǎn)中心，后軸速度為v,機(jī)器人的前輪虛擬轉(zhuǎn)向角α,方向角為θ,機(jī)器人的轉(zhuǎn)彎半徑為R，機(jī)器人前后軸軸距為L, 后輪兩輪距為2l。其中轉(zhuǎn)彎半徑R與軸距和前輪虛擬轉(zhuǎn)向角α有關(guān)，不能無限小，需滿足：

R=L/tan[α(t)]

(1)

w=v(t)/R

(2)

式中：w為機(jī)器人后軸角速度。

阿克曼機(jī)器人運(yùn)動學(xué)狀態(tài)方程建立如下：

(3)

(4)

阿克曼機(jī)器人的控制輸入量u(t)=[v(t),α(t)]T,虛擬轉(zhuǎn)向角α(t)因?yàn)榘⒖寺D(zhuǎn)向幾何機(jī)構(gòu)[9]存在一個范圍。

2 TEB算法模型構(gòu)建以及改進(jìn)約束

2.1 TEB軌跡模型構(gòu)建

TEB將機(jī)器人的一系列位姿軌跡模型抽象成帶有時間信息的彈性帶模型。機(jī)器人的第i個位姿狀態(tài)可以表示為si=[xi,yi,θi]，位姿包含位置信息xi、yi和方向角θi。ΔTi是機(jī)器人位姿si和si+1的過渡時間間隔。TEB的軌跡序列可以表示為如圖2所示。

圖2 機(jī)器人在世界坐標(biāo)系下的軌跡序列

機(jī)器人在世界坐標(biāo)系XOY下的位姿序列可以表示為

C={si}，i=0，1，…，n；n∈N

(5)

TEB描述的軌跡模型的位姿si到si+1的時間間隔為ΔTi,n個位姿間隔的時間序列為

Δτ={ΔTi}，i=0,1,…,n-1

(6)

每個時間間隔表示機(jī)器人從一個位姿運(yùn)動到另一個位姿的時間。TEB軌跡模型包含位姿序列信息和時間間隔序列信息，軌跡信息可以表示為

T:=(C,Δτ)

(7)

TEB算法存在的約束目標(biāo)如下：

2.2.1 避開障礙物

機(jī)器人的軌跡是必須滿足避開障礙物[10]。機(jī)器人的距離與障礙物的距離為d,機(jī)器人與障礙物的允許距離為dmin,障礙物根據(jù)環(huán)境動態(tài)更新，障礙物的位置可以通過激光雷達(dá)獲取，在代價地圖上讀取障礙物的位置。對于每個位姿，可能受n個障礙物影響，需要考慮對某個位姿的n個障礙物影響。當(dāng)障礙物超過機(jī)器人與障礙物允許的最小距離時，約束函數(shù)的值為0。當(dāng)機(jī)器人與障礙物的距離小于限制的距離，障礙物約束函數(shù)才起作用。約束函數(shù)如下：

d

(8)

2.1.2 速度和加速度約束

機(jī)器人速度和加速度的動態(tài)約束通過類似的懲罰函數(shù)來描述幾何約束。平均平移和旋轉(zhuǎn)速度分別根據(jù)歐式距離和方向角的改變量計算，在兩個連續(xù)的位姿xi、xi+1和兩個姿勢之間的過渡的時間間隔ΔTi[11]。

線速度：

(9)

角速度：

wi?(θi+1-θi)/ΔTi

(10)

線加速度：

ai?2(vi+1-vi)/(ΔTi+ΔTi+1)

(11)

角加速度：

αi=2(wi+1-wi)/(ΔTi+ΔTi+1)

(12)

2.1.3 非完整運(yùn)動學(xué)約束

非完整約束機(jī)器人相鄰的兩個位姿移動可近似為曲率不變的弧線運(yùn)動[12]。阿克曼機(jī)器人以恒定速率轉(zhuǎn)彎如圖3所示。

圖3 阿克曼機(jī)器人轉(zhuǎn)彎示意圖

機(jī)器人的位姿狀態(tài)由si運(yùn)動到si + 1，機(jī)器人做圓周運(yùn)動，si和si+1都落在曲率不變的圓弧上，其中滿足：

βi ,i=βi ,i + 1

(13)

(14)

(15)

式(15)中：θi和θi+1機(jī)器人在世界坐標(biāo)系的方向角，非完整約束的代價函數(shù)表示為

(16)

阿克曼機(jī)器人的轉(zhuǎn)彎半徑必須大于等于最小轉(zhuǎn)彎半徑

(17)

2.1.4 最短時間的目標(biāo)函數(shù)約束

最快時間是考慮TEB全局性，因?yàn)槟繕?biāo)函數(shù)需要依賴于所有的參數(shù)。是通過最小化所有時間間隔之和的平方要求機(jī)器人盡可能快地到達(dá)規(guī)劃的終點(diǎn)。最短時間約束函數(shù)可表示為

(18)

2.2 TEB算法求解模型的構(gòu)建

TEB的核心思想是求解出一系列這樣的最優(yōu)的帶有時間間隔的機(jī)器人位姿序列。目標(biāo)函數(shù)的大部分位姿是局部的，因?yàn)樗鼈儍H取決于少數(shù)幾個連續(xù)位姿的數(shù)量，而不是整條全局軌跡上的位姿。TEB的局部性導(dǎo)致稀疏系統(tǒng)矩陣的產(chǎn)生，可以使用專用的快速有效的大規(guī)模數(shù)值優(yōu)化方法[13]求解。關(guān)鍵思想是通過調(diào)整位姿和時間間隔來優(yōu)化TEB，使用權(quán)重求和模型求解多目標(biāo)優(yōu)化問題。通過構(gòu)建多目標(biāo)優(yōu)化得到最優(yōu)的位姿和時間間隔。

(19)

T*=argTminf(T)

(20)

式中：T*表示優(yōu)化的TEB結(jié)果；γk是函數(shù)k的權(quán)重系數(shù)；f(T)表示各種約束目標(biāo)函數(shù)之和，它是各目標(biāo)函數(shù)乘以權(quán)重系數(shù)的和。TEB優(yōu)化問題[式(18)]可以轉(zhuǎn)化為hyper-graph(超圖)問題，所謂 的超圖是其中一條邊的連接節(jié)點(diǎn)不受限制。因此，一條邊可以連接兩個以上的節(jié)點(diǎn)。TEB求解問題能夠轉(zhuǎn)換成以位姿和時間間隔為節(jié)點(diǎn)的超圖。節(jié)點(diǎn)之間通過給定的目標(biāo)函數(shù) 相連。圖4顯示了位姿、時間間隔、速度、加速度之間的連接關(guān)系。速度約束目標(biāo)函數(shù)需要知道兩個位姿歐式距離之差和時間間隔，因此，它形成了一條連接機(jī)器人位姿和時間間隔的邊。同理，加速度約束函數(shù)也是這樣。障礙物需要一條邊連接到能影響機(jī)器人的幾個位姿。代表障礙物的橢圓節(jié)點(diǎn)是無法優(yōu)化的。根據(jù)位姿，時間間隔、速度、加速度以及障礙物構(gòu)建出的超圖如4所示。

圖4 超圖

解決具有硬約束的非線性問題在計算代價非常大。因此，提高效率快速在線求解一直是非線性研究的重點(diǎn)。非線性問題轉(zhuǎn)化為近似非線性平方優(yōu)化問題，當(dāng)求解器利用問題的稀疏模式時，它通過一階導(dǎo)數(shù)逼近Hessian矩陣，從而有效地解決了該問題。約束作為懲罰項(xiàng)納入目標(biāo)函數(shù)[8]。

為了求解方程利用LevenbergMarquardt(LM)方法，因?yàn)樗骖欞敯粜院托省D優(yōu)化框架g2o[14]實(shí)現(xiàn)LM的高效稀疏變量求解方程[式(20)]。

TEB以一定的頻率進(jìn)行重復(fù)求解，這個頻率高于機(jī)器人的響應(yīng)控制周期。在每個軌跡修正步驟中，TEB算法都會動態(tài)添加新的位姿和刪除以前的位姿，為了將時空分辨率調(diào)整剩余的軌跡長度或規(guī)劃范圍。對于實(shí)時更新的障礙物，TEB在優(yōu)化的那一時刻當(dāng)作靜態(tài)障礙物來處理。g2o框架批量優(yōu)化了TEB軌跡，因此每次迭代都會生成一個新圖。在單個機(jī)器人控制周期內(nèi)，每個循環(huán)進(jìn)行多次循環(huán)迭代求解。在每個采樣周期中，控制輸入量u(t)從優(yōu)化的軌跡中得到。TEB算法流程如下:

1輸入全局路徑,給定機(jī)器人的起始位姿和終點(diǎn)位姿;2截取一段全局路徑,獲得初始化軌跡(包含機(jī)器人位姿和默認(rèn)時間間隔),構(gòu)建多目標(biāo)優(yōu)化問題;3進(jìn)入迭代求解循環(huán)4 調(diào)節(jié)修剪局部軌跡長度,維持局部優(yōu)化的軌跡長度不變,刪除機(jī)器人走過的固定長度的軌跡;5 更新代價地圖中的障礙物信息;6 建立hyper-graph(超圖)進(jìn)行多次迭代求解出最優(yōu)軌跡列;7 檢查軌跡是否可行;8 將式(19)B?映射到u(t),根據(jù)相鄰的軌跡點(diǎn)計算出控制量信息包括后軸速度和前輪虛擬轉(zhuǎn)向角;9 到達(dá)終點(diǎn),結(jié)束算法;

得到優(yōu)化的TEB軌跡之后，控制變量速度v和阿克曼機(jī)器人的虛擬轉(zhuǎn)角α可以計算出來下發(fā)給機(jī)器人驅(qū)動器。在每次新迭代之前，重新初始化階段都會檢查新的和更改的位姿點(diǎn)。

2.3 改進(jìn)TEB約束條件

在機(jī)器人的運(yùn)動過程中，加速度約束能保證速度變化率不會太大。加速度的變化率過大會對機(jī)器人造成沖擊。增加加加速度約束，將加加速度的變化率限定在一個較小的范圍，提高機(jī)器人運(yùn)動的平順性以及提高電機(jī)的壽命，現(xiàn)將加加速度約束添加到超圖中，構(gòu)建新的超圖。j0這條邊連接到這三個ΔT0、ΔT1、ΔT2這三個時間間隔頂點(diǎn)和s0、s1、s2、s3這四個位姿頂點(diǎn)，j1連接的時間間隔和位姿數(shù)量不變，分別往后順延一個序號，如圖4所示：

圖5 改進(jìn)的超圖

s0、s1、s2、s3、s4是機(jī)器人連續(xù)的5個位姿狀態(tài)，v0、v1、v2、v3是連接上述5個位姿的速度狀態(tài)(包含線速度和角速度)，a0、a1、a2是加速度狀態(tài)(包含線加速度和角加速度)，j0、j1滿足的加加速度約束關(guān)系如下：

線加加速度可表示為

j0lin=(a1lin-a0lin)/(0.25ΔT0+0.5ΔT1+

0.25ΔT2)

(21)

角加加速度可表示為

j0rot=(a1rot-a0rot)/(0.25ΔT0+0.5ΔT1+

0.25ΔT2)

(22)

在程序?qū)崿F(xiàn)過程中需要重新給g2o庫定義加加速度邊作為約束條件和添加加加速度約束。在局部規(guī)劃起始狀態(tài)和中間狀態(tài)以及終點(diǎn)狀態(tài)中分別應(yīng)用。

3 阿克曼機(jī)器人運(yùn)動規(guī)劃框架設(shè)計

整個運(yùn)動規(guī)劃框架的設(shè)計基于機(jī)器人ROS系統(tǒng)，利用ROS的通信機(jī)制進(jìn)行通信[15]。利用開源的move_base充當(dāng)決策層提供action通信機(jī)制，給定一個全局導(dǎo)航目標(biāo)點(diǎn)，調(diào)用Astar全局路徑規(guī)劃器生成全局路徑，全局規(guī)劃器發(fā)布全局路徑給局部路徑進(jìn)行路徑規(guī)劃，計算控制指令下發(fā)給機(jī)器人底盤。move_base節(jié)點(diǎn)將全局和局部路徑規(guī)劃程序通過狀態(tài)機(jī)連接在一起，以完成導(dǎo)航任務(wù)，還維護(hù)兩個用于完成導(dǎo)航任務(wù)的代價地圖，一個用于全局規(guī)劃程序，一個用于局部路徑規(guī)劃程序。整個框架設(shè)計如圖6所示。

move-base狀態(tài)機(jī)分為planning，controlling，clearing三種狀態(tài)。planning是接受新目標(biāo)與進(jìn)行全局規(guī)劃的整個過程，controlling是進(jìn)行局部路徑規(guī)劃發(fā)出速度指令的行為，clearing是全局規(guī)劃和局部規(guī)劃失敗或困住觸發(fā)recovery行為的用來處理上述情況的異常。當(dāng)move_base接收到目標(biāo)終點(diǎn)請求，move_base調(diào)用makePlan函數(shù)進(jìn)行全局規(guī)劃，便啟動Astar全局路徑規(guī)劃器，進(jìn)行全局路徑規(guī)劃，規(guī)劃完成將全局路徑以話題通信的方式發(fā)布(publishPlan)出來，move_base狀態(tài)機(jī)切換到controlling狀態(tài)，改進(jìn)的TEB路徑規(guī)劃器通過相關(guān)函數(shù)(setPlan)接收全局路徑，里程計負(fù)責(zé)定位，激光雷達(dá)獲得障礙物信息，以一定的頻率進(jìn)行局部路徑規(guī)劃，計算出控制指令，發(fā)布給機(jī)器人下位機(jī)控制器。

4 仿真實(shí)驗(yàn)和真車測試實(shí)驗(yàn)

仿真和實(shí)際實(shí)驗(yàn)的對象是一個具備阿克曼轉(zhuǎn)向機(jī)構(gòu)的非完整約束機(jī)器人。機(jī)器人的運(yùn)動學(xué)參數(shù)：機(jī)器人前軸虛擬轉(zhuǎn)角限定范圍為[-30°,30°]，軸距L=0.9 m，最小轉(zhuǎn)彎半徑1.56 m。

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

仿真平臺：stage。仿真計算機(jī)配置：CPU I7 主頻2.6 GHz，6核12線程，局部路徑規(guī)劃頻率為20 Hz。機(jī)器人參數(shù)如表1所示。

對權(quán)重參數(shù)調(diào)優(yōu)后，搭建仿真環(huán)境進(jìn)行測試。

仿真場景一：進(jìn)狹窄巷道和機(jī)器人脫困

仿真分析：由圖7(a)可知能規(guī)劃出符合阿克曼機(jī)器人運(yùn)動學(xué)的路徑，軌跡曲率符合要求。由圖7(b)可知該算法能規(guī)劃出調(diào)頭的路徑而且滿足阿克曼機(jī)器人的非完整約束和最小轉(zhuǎn)彎半徑約束。規(guī)劃的路徑符合仿真地圖的實(shí)際情形。

圖7 阿克曼機(jī)器人規(guī)劃局部路徑

仿真場景二：復(fù)雜迷宮地圖場景

仿真分析：由圖8、9可知，機(jī)器人能得到一條滿足阿克曼運(yùn)動學(xué)動力學(xué)從起點(diǎn)到終點(diǎn)的一條無碰撞路徑。

圖8 機(jī)器人路徑規(guī)劃過程展示

圖9 路徑規(guī)劃結(jié)果展示

4.1 真實(shí)機(jī)器人實(shí)驗(yàn)

對配有16線激光雷達(dá)的真實(shí)機(jī)器人在封閉多墻體場景進(jìn)行測試，測試的真實(shí)機(jī)器人如圖10、圖11所示。

圖10 機(jī)器人規(guī)劃過程

圖11 規(guī)劃結(jié)果精度測量

真車測試結(jié)果表明：阿克曼機(jī)器人能避開障礙物，能夠基本到達(dá)設(shè)定的目標(biāo)點(diǎn)，位置平均偏差約6 cm，航向角平均偏差約8°。實(shí)驗(yàn)次數(shù)設(shè)置為50次，在改進(jìn)TEB算法之前，機(jī)器人受到明顯沖擊的次數(shù)為5次，概率為1/10，改進(jìn)TEB算法后，添加加加速度約束后，整個導(dǎo)航過程中機(jī)器人沒有受到任何沖擊發(fā)生震蕩，運(yùn)動過程非常平順，基本到達(dá)預(yù)期要求。

5 結(jié)論與展望

針對阿克曼機(jī)器人運(yùn)動規(guī)劃問題，提出了一種改進(jìn)的TEB算法，并得到如下結(jié)論。

(1)建立了阿克曼機(jī)器人運(yùn)動控制模型。

(2)提出了一種改進(jìn)TEB算法，將加加速度約束添加到TEB算法中，減少加速度突變帶來的沖擊，提高運(yùn)動的平順性。

(3)將該改進(jìn)算法在阿克曼機(jī)器人應(yīng)用，并在stage仿真環(huán)境中實(shí)現(xiàn)，最終在真實(shí)機(jī)器人上進(jìn)行了測試和應(yīng)用。不僅驗(yàn)證了改進(jìn)的TEB算法的有效性，而且在阿克曼機(jī)器人上進(jìn)行了有效應(yīng)用。

該算法可廣泛應(yīng)用于阿克曼機(jī)器人的局部路徑規(guī)劃中，也可廣泛用于自動駕駛汽車自動泊車領(lǐng)域。