趙曉東，曹夢穎，宿景芳

(河北科技大學(xué) 信息科學(xué)與工程學(xué)院，河北 石家莊 050018)

0 引言

近年來,服務(wù)型移動機(jī)器人在自動化、計算機(jī)和人工智能等領(lǐng)域已經(jīng)成為研究熱點(diǎn)[1],在各類服務(wù)行業(yè)中的需求也比較迫切。能否對這些移動機(jī)器人開發(fā)普遍適用且高效安全的路徑規(guī)劃系統(tǒng)及能否具有自主感知能力等是移動機(jī)器人完成整個行程任務(wù)的熱點(diǎn)和難點(diǎn)[2-3]。在簡單已知環(huán)境下的路徑導(dǎo)航一般使用全局路徑規(guī)劃算法,傳統(tǒng)的全局路徑規(guī)劃算法有一般的啟發(fā)式算法[4],如Dijkstra算法、A*算法和RRT算法等。而當(dāng)移動機(jī)器人處于未知或部分信息的環(huán)境中時,則需要結(jié)合當(dāng)前所遇到的局部環(huán)境信息進(jìn)行實時路徑調(diào)整[5],即需要用到局部路徑規(guī)劃算法。局部路徑規(guī)劃算法可以使移動機(jī)器人根據(jù)障礙物的位置和特征進(jìn)行位置調(diào)整,從而在全局導(dǎo)航算法的指導(dǎo)下根據(jù)本地周圍實時環(huán)境規(guī)劃更優(yōu)的行走路徑。

動態(tài)社會環(huán)境中的局部移動機(jī)器人路徑規(guī)劃方法主要有無機(jī)器人動力學(xué)的方法和基于機(jī)器人動力學(xué)的方法,無機(jī)器人動力學(xué)的導(dǎo)航方法是指未考慮機(jī)器人的動力學(xué)和運(yùn)動學(xué),只根據(jù)周圍環(huán)境和當(dāng)前狀態(tài)確定接下來的運(yùn)動方向,比如人工勢場法[6]、矢量場直方圖法[7]、彈性帶[8]和速度障礙法[9]等已被證明能夠使機(jī)器人自主安全避開障礙物,并導(dǎo)航到給定目標(biāo),但在現(xiàn)實環(huán)境中直接利用輸出控制命令來控制移動機(jī)器人比較困難。要想在實際社會環(huán)境中使機(jī)器人自主避障和尋路,需要考慮機(jī)器人動力學(xué)約束,即基于機(jī)器人動力學(xué)的方法。時間彈性帶(Time Elastic Band,TEB)算法[10-11]是一種典型的基于機(jī)器人動力學(xué)的局部路徑規(guī)劃算法,國內(nèi)外很多研究者對其進(jìn)行了研究,是一種實用的機(jī)器人局部路徑規(guī)劃算法。TEB算法來源于彈性帶(Elastic Band, EB)算法,EB算法將要規(guī)劃的路徑看作一根可以隨意變形的彈性帶,通過施加內(nèi)部收縮力使路徑最短,并通過從障礙物向外輻射斥力使路徑變形避開障礙物,其目標(biāo)是使移動機(jī)器人在盡量避開障礙物的情況下按最短路徑行進(jìn),但EB算法沒有考慮動力學(xué)約束,因此需要一個專用的路徑跟蹤控制器。TEB算法在其基礎(chǔ)上進(jìn)行了改進(jìn),該方法通過最小化軌跡執(zhí)行時間、避開障礙物和局部優(yōu)化機(jī)器人軌跡如滿足速度和加速度的限制使其符合運(yùn)動學(xué)約束,可以進(jìn)行在線避障和路徑規(guī)劃。很多研究者對其進(jìn)行了運(yùn)動控制方面的改進(jìn),如文獻(xiàn)[12]將TEB算法與混合速度倒數(shù)障礙模型相結(jié)合,使其考慮了機(jī)器人和周圍移動障礙物之間的潛在碰撞,實現(xiàn)主動避障。文獻(xiàn)[13-14]在TEB算法中引入了社會學(xué)約束,將人類的社會時空特征(包括人類的位置、與機(jī)器人的相對運(yùn)動以及社會規(guī)則)整合到傳統(tǒng)的TEB在線軌跡規(guī)劃算法中,使其可以在動態(tài)的社會環(huán)境中安全地避開人類。鄭凱林等[15]在TEB算法中引入了加速度約束,通過使加速度在恰當(dāng)?shù)姆秶鷥?nèi)變化,避免了阿克曼機(jī)器人在運(yùn)動中產(chǎn)生的軌跡突變和速度震蕩。文郁等[16]在TEB算法中加入了危險懲罰因子約束、加速度跳變抑制約束和末端平滑約束來使機(jī)器人在障礙物密集處的運(yùn)動軌跡更加安全。而陳奕梅等[17]對于多機(jī)器人在動態(tài)障礙物環(huán)境中規(guī)劃路徑不佳且速度不穩(wěn)問題,融合了TEB算法和速度障礙(VO)法,提出了TEB-VO算法,同時設(shè)計了自適應(yīng)參數(shù)動態(tài)調(diào)節(jié)模塊從而使得機(jī)器人在靠近動態(tài)障礙物時輸出速度更加平緩。

由于在使用當(dāng)前的TEB算法進(jìn)行路徑規(guī)劃的實驗時,總會發(fā)現(xiàn)遇到靜態(tài)障礙物或者附近存在動態(tài)障礙物時機(jī)器人會出現(xiàn)驟停和突然加速的速度震蕩情況,缺乏魯棒性。為了克服這個問題,本文在TEB算法基礎(chǔ)上提出了基于曲率速度控制的時間彈性帶算法(Curve Speed Control-Time Elastic Band, CSC-TEB),該算法的中心思想是能夠使機(jī)器人根據(jù)軌跡彎曲程度來控制其線速度,在遇到障礙物時適當(dāng)降低速度大小,從而減少速度震蕩的頻率,提升避障平穩(wěn)性,具有計算簡單、執(zhí)行效率較高的優(yōu)點(diǎn)。

1 TEB算法

1.1 TEB算法的基本原理

早期的EB算法使用世界坐標(biāo)系,定義了一系列的機(jī)器人位姿Si={xi,yi,θi},其中xi和yi表示機(jī)器人的方向,θi表示機(jī)器人的方向角,因此EB算法的機(jī)器人軌跡可以表示為n個位姿序列的集合,如式(1)所示:

Q={Si}i=0,1,…,n。

(1)

TEB算法在EB算法基礎(chǔ)上加入了時間T,由位姿Si運(yùn)動到Si+1之間的時間間隔為ΔTi,n個位姿序列生成了n-1個時間間隔,由此可得到整個運(yùn)動過程所有時間間隔的集合如式(2)所示:

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

(2)

機(jī)器人在運(yùn)動過程中,都是按照順序由前一個位姿運(yùn)動到隨后的一個位姿,故機(jī)器人在局部路徑規(guī)劃算法內(nèi)的軌跡是由位姿序列和時間間隔序列2個集合共同組成的,如式(3)所示:

B:=(Q,τ)。

(3)

機(jī)器人運(yùn)動軌跡如圖1所示。

圖1 TEB 路徑位姿和時間間隔關(guān)系Fig.1 The relationship between the mobile robot’s postureand the time interval in the TEB algorithm

整體算法是通過多目標(biāo)優(yōu)化來獲取最優(yōu)路徑點(diǎn)集合,最優(yōu)軌跡B*通過最小化一個加權(quán)非線性最小二乘函數(shù)來獲得,如式(4)和式(5)所示:

(4)

(5)

式中:f(B)為考慮各種約束的目標(biāo)函數(shù),γk為目標(biāo)函數(shù)的權(quán)值,B*為最優(yōu)軌跡結(jié)果。

TEB算法已經(jīng)存在的約束有4種,跟隨路徑以及避障、速度和加速度約束、非完整運(yùn)動學(xué)約束以及最快路徑約束,具體如下:

① 跟隨已經(jīng)確定的全局路徑,所跟隨的路徑施加外力將整條“橡皮筋”拉向全局路徑,而避障可以理解為障礙物施加外力使其遠(yuǎn)離障礙物;

② 因為其速度/加速度只與移動機(jī)器人的連續(xù)狀態(tài)有關(guān),所以可以通過差分計算來進(jìn)一步得到,并且根據(jù)原有的性能約束,速度/加速度具有最大最小值限制;

③ 對于移動機(jī)器人的走向軌跡應(yīng)該是若干段弧組成的平滑軌跡,不應(yīng)有漂移到軌跡之外的點(diǎn);

④ 要盡可能減少所要花費(fèi)的時間,從而用最短時間目標(biāo)代替其他算法中的最短路徑目標(biāo)。

1.2 TEB超圖的構(gòu)建

TEB算法具有局部規(guī)劃性,每一次的迭代只需要插入一個新的位姿狀態(tài)或刪除一個舊的位姿狀態(tài),從而保證了整個優(yōu)化過程的求解速度,并且也保證了機(jī)器人的實時避障能力。TEB算法中規(guī)劃的路徑點(diǎn)與障礙物之間及相關(guān)條件的約束關(guān)系可以用超圖[18]表示,超圖的邊表示目標(biāo)函數(shù)和約束代價,頂點(diǎn)包括移動機(jī)器人的位姿、相鄰位姿的時間間隔以及障礙物。

傳統(tǒng)TEB算法的超圖如圖2所示(其中展示出了機(jī)器人在4種相鄰位姿間轉(zhuǎn)換時的約束關(guān)系)。

圖2 TEB超圖Fig.2 TEB hyper-graph

超圖作為一個開源框架,對于具有特殊結(jié)構(gòu)的非線性優(yōu)化問題是可以解決的。這個框架將軌跡優(yōu)化問題構(gòu)建成了一個g2o圖優(yōu)化問題,需要定義節(jié)點(diǎn)和邊,對于每個節(jié)點(diǎn)會定義相應(yīng)的增量,并通過g2o中關(guān)于大規(guī)模稀疏矩陣的優(yōu)化算法解決。即如圖2所示,將機(jī)器人的位姿描述為節(jié)點(diǎn)(vertex),而目標(biāo)函數(shù)以及設(shè)定的約束函數(shù)作為邊(edge)。

2 CSC-TEB算法

2.1 CSC-TEB算法設(shè)計

引入速度控制因子αv,如式(6)所示:

αv=1-Δθ/π。

(6)

移動機(jī)器人從第i個姿態(tài)Si運(yùn)動到第i+1個姿態(tài)Si+1的軌跡示意如圖3所示。di,i+1為Si和Si+1這2個姿態(tài)之間的直線連線,也稱為機(jī)器人運(yùn)動的目標(biāo)方向,θi與θi+1分別為機(jī)器人在姿態(tài)Si和Si+1時速度方向與下一目標(biāo)方向的夾角,也稱為速度偏角,Δθ為移動機(jī)器人的第i+1個姿態(tài)Si+1的速度偏角θi+1與第i個姿態(tài)Si的速度偏角θi之差。

圖3 速度偏移角夾角示意Fig.3 Diagram of velocity offset angle

根據(jù)實際情況,移動機(jī)器人的最大速度偏角只能達(dá)到π,即在朝下一目標(biāo)點(diǎn)完全相反方向運(yùn)動時取得,因此式(6)中的Δθ∈(0,π),即αv的取值(0,1)。

TEB算法中的速度約束只由速度的上限值和下限值來決定;而CSC-TEB算法進(jìn)行了改進(jìn),即機(jī)器人避障轉(zhuǎn)彎時的最大瞬時速度可由速度控制因子αv進(jìn)行調(diào)節(jié),如式(7)所示:

vi=vimax×αv(i=1,2,…,n),

(7)

由式(7)可以看出,機(jī)器人的最大瞬時速度vi在原來最大速度vimax的基礎(chǔ)上由αv加權(quán),而αv與Δθ成反比關(guān)系,當(dāng)移動機(jī)器人行進(jìn)的軌跡曲率變大時,即它的行進(jìn)方向偏離目標(biāo)點(diǎn)直線距離的程度變大時,速度控制因子就可以發(fā)揮降低移動機(jī)器人速度的作用。

2.2 CSC-TEB算法實現(xiàn)

為了實現(xiàn)上述速度控制因子αv的功能,添加了軌跡曲率與速度之間的約束,在TEB超圖中加入速度控制因子αv,改進(jìn)后的TEB超圖如圖4所示。

[46] East Asia Forum, Chinese security and freedom of navigation in the South China Sea, June 5, 2017, http://www.eastasiaforum.org/2017/06/05/chinese-security-and-freedom-of-navigation-in-the-south-china-sea/.

圖4 改進(jìn)后的TEB超圖Fig.4 Improved TEB hyper-graph

在上圖中,速度控制因子αv作為軌跡曲率與移動機(jī)器人的速度之間的約束,該約束在超圖中轉(zhuǎn)換成代價函數(shù),從而在局部路徑規(guī)劃過程中起到在避障時實時調(diào)整機(jī)器人運(yùn)動速度的作用。

CSC-TEB算法參與的整個路徑規(guī)劃算法實現(xiàn)遵循如下步驟:

① 在全局規(guī)劃和局部規(guī)劃配置文件中設(shè)置好相應(yīng)參數(shù),制定目標(biāo)路徑的起點(diǎn)和終點(diǎn),由move_base調(diào)用navfn包中的全局路徑規(guī)劃算法進(jìn)行全局路徑規(guī)劃;

② 全局路徑規(guī)劃完成后,反饋給move_base,move_base進(jìn)一步調(diào)用base_local_planner包中的局部路徑規(guī)劃TEB算法進(jìn)入局部路徑規(guī)劃;

③ 選取全局路徑規(guī)劃上的部分路徑點(diǎn)作為本次避障過程局部路徑規(guī)劃的路徑點(diǎn),記錄移動機(jī)器人的初始化軌跡以及位姿,得到每次迭代的時間間隔;

④ 將規(guī)劃路徑點(diǎn)所形成的航向角與移動機(jī)器人的速度之間用速度控制因子αv進(jìn)行關(guān)聯(lián),根據(jù)相鄰路徑點(diǎn)計算機(jī)器人的行進(jìn)速度和速度偏角等控制變量,最大速度由速度控制因子進(jìn)行約束,根據(jù)上述步驟構(gòu)建完整超圖,找出最優(yōu)軌跡;

⑤ 更新環(huán)境地圖中機(jī)器人與障礙物位置信息;

⑥ 判斷是否到達(dá)終點(diǎn);

⑦ 到達(dá)終點(diǎn)算法結(jié)束;沒有到達(dá)終點(diǎn)則返回③。

3 仿真實驗

為了分析并驗證CSC-TEB算法的優(yōu)越性,將CSC-TEB算法與TEB算法分別在靜態(tài)與動態(tài)場景中進(jìn)行了仿真。計算機(jī)配置為:CPU i7-7700 主頻 2.80 GHz,4核8線程,操作系統(tǒng)為Ubuntu 18.04,采用ROS機(jī)器人仿真平臺[19],2輪差速機(jī)器人進(jìn)行算法的仿真。仿真的初始參數(shù)配置如表1所示。

表1 機(jī)器人運(yùn)動學(xué)參數(shù)

3.1 靜態(tài)環(huán)境下的仿真實驗

在同一靜態(tài)場景下對TEB算法和CSC-TEB算法用于2輪差速機(jī)器人的導(dǎo)航。Rviz可視化平臺中的仿真場景如圖5所示,圖中存在2個圓形靜態(tài)障礙物。在該場景下應(yīng)用TEB算法和CSC-TEB算法都選擇了相同的最優(yōu)路徑,即中間帶紅色箭頭的路徑,但速度曲線呈現(xiàn)出區(qū)別如圖6所示。

圖5 仿真環(huán)境Fig.5 Simulation environment

(a)TEB算法速度曲線

(b)CSC-TEB算法速度曲線

圖6(a)和圖6(b)分別展示了通過圓形障礙物時TEB算法和CSC-TEB算法的線速度曲線,通過這2幅圖在相同場景下的對比,TEB算法一直以最大速度行進(jìn),而CSC-TEB算法在軌跡曲率較大的情況下,可以使移動機(jī)器人根據(jù)曲率來適當(dāng)降低速度,從而達(dá)到安全平穩(wěn)地度過彎道的目的。

為了使可視化效果更加明顯,本文亦在Stage平臺下對TEB算法和CSC-TEB算法進(jìn)行了對比仿真實驗。Stage平臺中TEB算法和CSC-TEB算法規(guī)劃的路徑如圖7所示,其中圖7(a)和圖7(b)中的紅色部分分別為2種算法在轉(zhuǎn)彎時所規(guī)劃軌跡的效果,可以發(fā)現(xiàn)TEB算法所規(guī)劃的運(yùn)行軌跡在轉(zhuǎn)彎時會貼著轉(zhuǎn)角前進(jìn),不能完全保證機(jī)器人的運(yùn)行安全;而CSC-TEB算法所規(guī)劃的運(yùn)行軌跡是在一個安全區(qū)域內(nèi)行進(jìn)的,軌跡更加平滑。

(b)CSC-TEB算法

綜上可以得出,CSC-TEB算法可以保證機(jī)器人在有效避障的前提下,對最大瞬時速度進(jìn)行適時調(diào)整,軌跡更加安全平滑。

3.2 動態(tài)環(huán)境下的仿真實驗

在完成靜態(tài)環(huán)境下的仿真實驗后,為了更好地展現(xiàn)出在動態(tài)環(huán)境下CSC-TEB算法的平穩(wěn)性,在仿真環(huán)境下加入了做勻速運(yùn)動的具有規(guī)則形狀的動態(tài)障礙物,該障礙物做著反復(fù)的周期運(yùn)動。而面對具有不規(guī)則形狀的障礙物,可以使用已經(jīng)設(shè)置好的膨脹半徑對其做膨脹化處理,有效保證移動機(jī)器人的安全性。

結(jié)合該局部路徑規(guī)劃算法所針對的實時躲避障礙物的特性以及本文的CSC-TEB算法進(jìn)行綜合考慮,需要對具有不同表面形貌的動態(tài)障礙物進(jìn)行實驗。本文設(shè)置了多個不同大小、且形狀不規(guī)則的障礙物進(jìn)行實驗,其中約50%的障礙物使得機(jī)器人產(chǎn)生較小的轉(zhuǎn)彎曲率,約50%的障礙物使得機(jī)器人產(chǎn)生較大的轉(zhuǎn)彎曲率,具體實驗環(huán)境如圖8所示。

(a)移動機(jī)器人具有較大轉(zhuǎn)彎曲率

(b)移動機(jī)器人具有較小轉(zhuǎn)彎曲率

在保證起點(diǎn)、終點(diǎn)和移動障礙物的運(yùn)動狀態(tài)一致的前提下,用TEB算法和CSC-TEB算法在該動態(tài)環(huán)境下進(jìn)行多次仿真實驗。對移動機(jī)器人從起點(diǎn)到終點(diǎn)的全部轉(zhuǎn)向角取絕對值后進(jìn)行求和運(yùn)算。實驗結(jié)果如表2所示。

表2 轉(zhuǎn)向角總和

實驗數(shù)據(jù)表明,當(dāng)移動機(jī)器人躲避移動障礙物而轉(zhuǎn)彎較小時,TEB算法和CSC-TEB算法體現(xiàn)出的性能相差不大。但是,當(dāng)轉(zhuǎn)彎較大時,在TEB算法下的轉(zhuǎn)向角之和遠(yuǎn)大于CSC-TEB算法的轉(zhuǎn)向角之和。

上述實驗結(jié)果表明,移動機(jī)器人轉(zhuǎn)彎幅度較大時,在TEB算法下迫使移動機(jī)器人產(chǎn)生更多曲折的路徑才能安全地躲避障礙物,移動機(jī)器人會因為需要重新規(guī)劃速度而花費(fèi)更多的時間,降低完成整體路程的效率。而在CSC-TEB算法中,移動機(jī)器人軌跡更加平滑,使用CSC-TEB算法的移動機(jī)器人更加適合社交導(dǎo)航。

在圖8(b)的實驗場景下,在保證起點(diǎn)、終點(diǎn)和移動障礙物的運(yùn)動狀態(tài)一致的前提下,截取了整體路徑規(guī)劃的速度變化曲線,如圖9所示。可以看到當(dāng)采用CSC-TEB算法時機(jī)器人對于動態(tài)障礙物的躲避是相對平穩(wěn)的,速度的跳變次數(shù)較少,整個避障過程更加穩(wěn)定,而TEB算法產(chǎn)生了多次的速度震蕩。

(a)TEB算法速度震蕩情況

(b)CSC-TEB算法速度震蕩情況

在該次實驗中也對二者的速度震蕩次數(shù)、避障時延和路徑耗時進(jìn)行了統(tǒng)計,數(shù)據(jù)如表3所示。

表3 速度震蕩次數(shù)和路徑耗時

表中數(shù)據(jù)是經(jīng)過100次實驗后得出的數(shù)據(jù)。使用TEB算法的移動機(jī)器人在轉(zhuǎn)彎時仍然是以最高線速度行駛,隨著實驗次數(shù)的增多,可以看到它在遇到動態(tài)障礙物時,速度震蕩次數(shù)較多,并且突變范圍較大。而在CSC-TEB算法下轉(zhuǎn)彎時線速度變小,運(yùn)動情況相對平穩(wěn),避障時延相對較短,并且路徑總耗時也較TEB算法降低了15.02%,基本達(dá)到預(yù)期要求,證明了本實驗的合理性。

通過上述靜態(tài)與動態(tài)場景下對TEB算法的仿真可知,CSC-TEB算法能夠使機(jī)器人在遇到障礙物時根據(jù)轉(zhuǎn)彎程度降低速度,減少了路徑重新規(guī)劃次數(shù),減少了速度震蕩和軌跡抖動現(xiàn)象,能夠?qū)崿F(xiàn)平穩(wěn)避障,節(jié)省了避障時延和整體路徑耗時。

4 結(jié)束語

本文對移動機(jī)器人在實時環(huán)境中的局部路徑規(guī)劃算法——TEB算法進(jìn)行了研究,并在其基礎(chǔ)上提出了CSC-TEB算法。所提出的算法對機(jī)器人在避障過程中遇到具有彎曲程度較大障礙物時的速度進(jìn)行了考慮,使其速度受軌跡彎曲程度的約束,最大瞬時速度可以適時調(diào)整。經(jīng)過靜態(tài)環(huán)境和動態(tài)環(huán)境仿真實驗的對比,證明了CSC-TEB算法能夠在成功避開障礙物的前提下,保證移動機(jī)器人行進(jìn)過程中的安全性,提升了機(jī)器人的軌跡平穩(wěn)性,同時在一定程度上節(jié)約了完成整個路徑規(guī)劃的總體時間延遲。