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

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

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

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

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

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

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

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

R=L/tan[α(t)]

(1)

w=v(t)/R

(2)

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

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

(3)

(4)

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

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

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

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

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

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

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

(5)

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

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

(6)

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

T:=(C,Δτ)

(7)

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

2.2.1 避開(kāi)障礙物

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

d

(8)

2.1.2 速度和加速度約束

機(jī)器人速度和加速度的動(dòng)態(tài)約束通過(guò)類(lèi)似的懲罰函數(shù)來(lái)描述幾何約束。平均平移和旋轉(zhuǎn)速度分別根據(jù)歐式距離和方向角的改變量計(jì)算，在兩個(gè)連續(xù)的位姿xi、xi+1和兩個(gè)姿勢(shì)之間的過(guò)渡的時(shí)間間隔Δ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)動(dòng)學(xué)約束

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

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

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

βi ,i=βi ,i + 1

(13)

(14)

(15)

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

(16)

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

(17)

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

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

(18)

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

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

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

圖4 超圖

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

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

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

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

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

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

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

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

s0、s1、s2、s3、s4是機(jī)器人連續(xù)的5個(gè)位姿狀態(tài)，v0、v1、v2、v3是連接上述5個(gè)位姿的速度狀態(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)過(guò)程中需要重新給g2o庫(kù)定義加加速度邊作為約束條件和添加加加速度約束。在局部規(guī)劃起始狀態(tài)和中間狀態(tài)以及終點(diǎn)狀態(tài)中分別應(yīng)用。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

真車(chē)測(cè)試結(jié)果表明：阿克曼機(jī)器人能避開(kāi)障礙物，能夠基本到達(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算法后，添加加加速度約束后，整個(gè)導(dǎo)航過(guò)程中機(jī)器人沒(méi)有受到任何沖擊發(fā)生震蕩，運(yùn)動(dòng)過(guò)程非常平順，基本到達(dá)預(yù)期要求。

5 結(jié)論與展望

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

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

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

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

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