基于參考軌跡的移動機器人避碰決策及軌跡跟蹤方法

聶振邦,于海斌+

(1.中國科學(xué)院 沈陽自動化研究所機器人學(xué)國家重點實驗室,遼寧 沈陽 110016;2.中國科學(xué)院 網(wǎng)絡(luò)化控制系統(tǒng)重點實驗室,遼寧 沈陽 110016;3.中國科學(xué)院 機器人與智能制造創(chuàng)新研究院,遼寧 沈陽 110169;4.中國科學(xué)院大學(xué),北京 100049)

0 引言

移動機器人具有自主、靈活的特點,在車間自動化物流系統(tǒng)中得到了廣泛的應(yīng)用。但為了讓移動機器人安全高效地完成物流運輸任務(wù),需要進行運動規(guī)劃。運動規(guī)劃一般分為環(huán)境地圖建模、路徑規(guī)劃、軌跡規(guī)劃、軌跡跟蹤4個步驟。

通常,車間移動機器人軌跡跟蹤是指在軌跡規(guī)劃基礎(chǔ)上對機器人進行反饋控制,以減小機器人實時位置與具有時間特征的參考軌跡之間的偏差。除了減小軌跡跟蹤誤差以外,近年來,根據(jù)實際應(yīng)用的需要,基于實時信息動態(tài)地避免由時間和位置擾動帶來的碰撞問題開始得到關(guān)注,也成為研究熱點之一。

具有避碰能力的軌跡跟蹤方法通常包括避碰觸發(fā)條件和軌跡跟蹤控制兩個部分。已有的避碰觸發(fā)條件主要基于當(dāng)前相對位置和相對速度信息。文獻[1]提出一種軌跡跟蹤控制器,當(dāng)障礙物進入沖突檢測范圍就切換到避碰模式;文獻[2]～文獻[4]提出基于相對位置和速度信息來切換避碰模式的反饋控制器,只有當(dāng)機器人相對障礙物的運動速度可能導(dǎo)致碰撞時才會進行避碰。經(jīng)典的軌跡跟蹤控制,主要有基于滑?？刂频姆椒╗5]、基于反步法的方法[6]、基于動態(tài)反饋線性化的方法[7]等。近年來,隨著機器人計算能力的增強,以及數(shù)學(xué)優(yōu)化方法和工具的進步,研究者提出了基于模型預(yù)測控制(Model Predictive Control,MPC)的軌跡跟蹤方法[8-9],通過在線求解包含機器人非線性約束的優(yōu)化問題來計算最優(yōu)的控制輸入。文獻[10]和文獻[11]給出了幾種基于MPC的實時避碰軌跡跟蹤控制算法。

以上研究能夠保證機器人運動的安全性,但這些方法在進行避碰決策時只使用了當(dāng)前信息,并沒有充分利用參考軌跡的全局信息,使得避碰算法過于保守,導(dǎo)致機器人的實際運動軌跡與參考軌跡偏差較大。為此,本文提出基于參考軌跡全局信息的避碰決策和軌跡跟蹤方法。該方法包括基于參考軌跡全局信息的避碰觸發(fā)條件構(gòu)建,以及最小化跟蹤誤差的避碰軌跡跟蹤控制算法。

1 基于當(dāng)前信息的避碰及軌跡跟蹤方法

現(xiàn)有基于當(dāng)前信息進行避碰的移動機器人軌跡跟蹤方法可分為兩類：一類根據(jù)機器人與障礙物的相對距離進行避碰[1],另一類根據(jù)機器人與障礙物的相對位置和相對速度進行避碰[2-4]。

1.1 基于相對距離信息的方法

圖1 機器人感知范圍

(1)

(2)

1.2 基于相對位置和相對速度的方法

在基于相對距離信息的避碰與軌跡跟蹤研究的基礎(chǔ)上,人們進一步開展了根據(jù)當(dāng)前的相對位置和相對速度信息進行避碰與軌跡跟蹤的研究[2-4]。

如圖2所示,Ri的感知區(qū)域被劃分為沖突區(qū)、過渡區(qū)、安全區(qū)。

圖2 感知區(qū)域劃分

據(jù)此定義沖突區(qū)Cij、過渡區(qū)Tij、安全區(qū)Sij如下：

Cij={zij∈Rij||γij|≤π/2};

Tij={zij∈Rij|π/2<|γij|≤π/2+δ};

Sij={zij∈Rij||γij|>π/2+δ}。

(3)

其中δ∈(0,π/2)為可調(diào)參數(shù)。

基于上述分區(qū),將基于相對距離信息的碰撞風(fēng)險度量函數(shù)式(1)進行擴展,定義考慮相對速度信息的碰撞風(fēng)險度量函數(shù)Wij：

(4)

考慮相對速度信息的避碰梯度向量為

(5)

其中：kij>0為可調(diào)參數(shù),σij為分區(qū)權(quán)重系數(shù)。

2 基于參考軌跡全局信息的避碰決策與軌跡跟蹤

本章在綜合考慮參考軌跡全局信息的前提下,提出避碰決策與軌跡跟蹤方法。

(6)

2.1 基于參考軌跡信息的避碰觸發(fā)條件

本節(jié)在如圖2所示的感知區(qū)域劃分的基礎(chǔ)上,對zij∈Cij的情況作進一步的篩選,提出一組基于參考軌跡信息的避碰觸發(fā)條件,以減少不必要的避碰動作。

圖3 存在碰撞風(fēng)險的軌跡跟蹤場景

(7)

(3)避碰觸發(fā)條件 當(dāng)存在Rj或Oj,使得zij∈Cij,且滿足以下條件之一時,Ri需要進行避碰：

其中：τp>0,θth>0為可調(diào)參數(shù),符號×為叉積算子,sign為符號函數(shù)。

2.2 最小化跟蹤誤差的避碰軌跡跟蹤控制

2.2.1 常規(guī)MPC軌跡跟蹤控制算法

考慮如圖4所示的車式移動機器人的軌跡跟蹤控制。將時間按控制周期進行離散化,k∈對應(yīng)時刻tk=kT。

圖4 車式移動機器人等效模型

機器人的速度上限vm,加速度上限am,轉(zhuǎn)向角φ∈[-φm,φm],φ的變化率上限wm,軸距為l,后輪軸中心pa=[xa,ya],pa=p-0.5l,其中l(wèi)=[lcosθ,lsinθ]T。

在時刻tk,參照文獻[12],以最小化跟蹤誤差為優(yōu)化目標(biāo),構(gòu)造MPC軌跡跟蹤問題如下：

(8)

(9)

|f1[n]|≤b1。

(10)

(11)

其中：

(12)

(13)

f1[n]=[u[n]T,u[n+1]T-u[n]T]T;

(14)

b1=[vm,φm,am,wm]T。

(15)

以上MPC軌跡跟蹤控制算法未考慮避碰的要求,不能保證軌跡跟蹤過程中對障礙物的避碰。為此,還需要研究軌跡跟蹤誤差優(yōu)化的避碰策略,以構(gòu)建機器人避碰速度的約束空間Vs,作為MPC軌跡跟蹤優(yōu)化的約束條件。

2.2.2 面向軌跡跟蹤誤差優(yōu)化的避碰策略

為了減小避碰對軌跡跟蹤精度的影響,針對機器人之間的避碰、機器人與障礙物間的避碰,本文提出一組基于速度向量調(diào)節(jié)的避碰策略。

為有效描述基于速度調(diào)節(jié)的避碰機制,利用文獻[13]提出的碰撞錐(Collision Cone,CC)概念。具體如圖5所示,l1,l2為從pi出發(fā),與圓o(pj,rij)相切的直線,l1,l2所夾的區(qū)域為碰撞錐,以下記為CCij。記碰撞錐的邊界l1,l2的方向為θl1,θl2。

圖5 碰撞錐示意

不難看出,在不考慮跟蹤誤差的情況下,只需調(diào)節(jié)vi,vj,使相對速度vij脫離碰撞錐,即vij?CCij,就可以避免Ri與Rj之間的碰撞。但是,如果對相對速度調(diào)節(jié)方向不作必要的約束,有可能對軌跡跟蹤性能產(chǎn)生不利的影響。

GF Machining Solutions（又名“GF加工方案”）是一家為工模具和精密零件加工行業(yè)提供機床和自動化解決方案的世界領(lǐng)先的系統(tǒng)供應(yīng)商。其產(chǎn)品范圍從放電加工機床、高速銑床和高性能銑床、夾具和托盤系統(tǒng)、三維激光紋理機，到提供客戶服務(wù)、備品備件、易損件、耗品以及自動化的解決方案。作為一個全球化的集團，GF Machining Solutions隸屬于Georg Fischer集團（瑞士），管理著全球范圍內(nèi)50個自己的組織架構(gòu)。

基于以上認(rèn)識,本文提出基于參考軌跡信息確定相對速度vij調(diào)節(jié)方向的原則,以保證機器人按照參考軌跡規(guī)定的方式來協(xié)調(diào)運動。

以如圖6所示情況為例,為保持按參考軌跡規(guī)定的Rj先通過交叉口,此時,vij應(yīng)向碰撞錐CCij的逆時針方向調(diào)節(jié)。

圖6 存在碰撞風(fēng)險的場景

根據(jù)上述分析,就移動障礙物與固定障礙物的避碰,分4種情況給出構(gòu)造Ri在速度平面vx-vy中避碰速度約束空間Vs的方法。

情況1避免與移動機器人Rj碰撞,vij(tk)在CCij(tk)內(nèi)。

(16)

(17)

情況3避免與固定障礙物Oj碰撞,vi(tk)在CCij(tk)內(nèi)。

(18)

其中ξc∈(0,π/2)為可調(diào)參數(shù)。

情況4避免與固定障礙物Oj碰撞,vi(tk)不在CCij(tk)內(nèi)。

(19)

其中ξs∈(0,π/2)為可調(diào)參數(shù)。

不難驗證,以上得到的避碰速度約束空間Vs是凸的多邊形空間。因此,基于Vs可以使用線性代數(shù)的方法構(gòu)造出速度v=[vx,vy]對應(yīng)的一組半平面(Half Planes,HP)線性不等式約束,其中的每個約束Avx+Bvy+C≤0都對應(yīng)Vs的一條邊界Avx+Bvy+C=0。

對于HP中的每個約束Avx+Bvy+C≤0,構(gòu)造MPC控制變量u[k]的避碰約束：

Av[k]cosθp[k]+Bv[k]sinθp[k]+C≤0。

(20)

顯然,將式(20)并入問題(8)進行求解,就可以在優(yōu)化跟蹤誤差的同時避免碰撞。

3 實驗驗證與分析

本章設(shè)計了4個移動機器人跟蹤預(yù)規(guī)劃安全參考軌跡的實驗場景。在軌跡跟蹤過程中,對機器人施加位置擾動和時間擾動,引入碰撞風(fēng)險,以驗證所提方法的避碰和軌跡跟蹤性能。

以下記本文所提基于參考軌跡全局信息的避碰軌跡跟蹤方法為CABR(collision avoidance based on reference trajectory)。選取當(dāng)前最具代表性的基于相對位置和相對速度信息的軌跡跟蹤方法(Collision Avoidance Velocity-based Gradient,CAVG)[2]與本文所提的CABR進行比較。

同時,為了深度說明2.1節(jié)提出的避碰觸發(fā)條件和2.2.2節(jié)提出的避碰策略的作用,也與基于當(dāng)前信息的避碰軌跡跟蹤(Collision Avoidance Not Based on Reference trajectory,CANBR)算法和未考慮避碰的軌跡跟蹤(Trajectory tracKing No Collision Avoidance,TKNCA)算法進行了對比。

各方法的控制對象為車式移動機器人,安全半徑r=0.5 m,軸距l(xiāng)=0.85 m,最大速度vm=1.5 m/s,最大加速度am=1.5m/s2,最大轉(zhuǎn)向角φm=π/9 rad,轉(zhuǎn)向角最大變化率wm=π rad/s。所有場景中的參考軌跡都滿足式(6)。

各方法在仿真實驗中的參數(shù)設(shè)定如下。TKNCA、CANBR、CABR求解MPC軌跡跟蹤問題(8)時,控制周期T=0.1 s,預(yù)測時域NH=5。CANBR和CABR構(gòu)造與固定障礙物避碰的速度方向θi的避碰區(qū)域SCij時,使用ξc=ξs=π/9rad。CABR避碰觸發(fā)條件中的參數(shù)τp=1 s,θth=0.1 rad,CABR構(gòu)造Ri的避碰速度約束空間Vs時的參數(shù)tl=6 s。CAVG的控制器參數(shù)γ=0 rad,其余與文獻[2]的實驗部分一致。CANBR、CABR、CAVG的感知范圍半徑Di相同。

3.1 參考軌跡不相交的場景

位置擾動的情況是：當(dāng)R1、R2的間距降到5 m以下時,分別對R1、R2施加擾動,使機器人往路徑左側(cè)平移距離pe=0.5 m,為避免感知范圍半徑Di影響避碰效果,取Di=6 m。

各方法得到的運動軌跡如圖7所示,圖中顯示了每0.2 s機器人的位置?？梢钥闯?使用TKNCA時,R1、R2的安全區(qū)域存在交錯重疊,說明R1、R2發(fā)生了碰撞。使用其余3種方法時,R1、R2沒有發(fā)生碰撞,體現(xiàn)了避碰策略的作用。

圖7 4種方法得到的運動軌跡(pe=0.5 m)

各方法得到的機器人跟蹤誤差如圖8所示。在t=7.5 s擾動發(fā)生后,TKNCA的跟蹤誤差減小最快,隨后產(chǎn)生大幅回升,說明機器人運動發(fā)生振蕩,致使碰撞發(fā)生。使用CANBR與CABR時,跟蹤誤差一開始較大,在t=9 s后快速減小,之后出現(xiàn)小幅回升,說明機器人優(yōu)先進行避碰,等到?jīng)]有碰撞風(fēng)險后迅速縮小跟蹤誤差。使用基于相對位置和相對速度信息的CAVG時,機器人的跟蹤誤差下降速度最慢。

圖8 4種方法得到的機器人跟蹤誤差

3.2 參考軌跡相交的場景

感知半徑Di決定了沖突區(qū)域Cij(如圖2)的大小,增加Di可以讓機器人提前發(fā)現(xiàn)危險,提高安全性,但也容易導(dǎo)致不必要的避碰動作。該場景驗證所提方法在不同的感知半徑Di下的有效性,以及在機器人受到不同幅度位置擾動時的避碰和軌跡跟蹤效果。

圖9 機器人的參考軌跡

圖與障礙物的間距變化

為了模擬不同的碰撞風(fēng)險,選擇ct=4.5 m、3.5 m、2.5 m、1.5 m和pe=0.125 m、0.25 m、0.5 m的不同組合構(gòu)造擾動。

實驗發(fā)現(xiàn),TKNCA在ct=1.5 m,pe=0.5 m的場景下發(fā)生了碰撞。分別在感知半徑Di=2 m、3.5 m、6 m、8.5 m、11 m的情況下,使用CABR、CANBR、CAVG對機器人進行軌跡跟蹤控制。加上ct、pe的不同取值,每種方法得到60組軌跡跟蹤數(shù)據(jù)。

對每一組數(shù)據(jù),計算平均跟蹤誤差(ATE)：

(19)

圖11顯示了各方法的相對誤差指標(biāo)RTE。在不同的ct下,CAVG和CANBR的RTE都隨著感知半徑的增加而增加。CABR得到的RTE受感知半徑增加的影響不大。CABR的RTE比另兩種方法都要小,并且在ct=4.5 m和3.5 m時幾乎為0,說明CABR避免了不必要的避碰動作。

圖11 不同ct下各方法的RTE

3.3 四向交叉路口場景

本場景驗證了所提方法在機器人受到不同幅度的時間擾動時的避碰和軌跡跟蹤效果。

圖12 4個機器人的參考軌跡

在t=4 s時,對R1施加擾動,將其實際運動速度降為max{0,vc-vd},其中vc為各方法計算得到的速度控制量,vd>0為擾動幅度。擾動的持續(xù)時間記為Ddur,選擇Ddur=1 s、2 s、3 s,以及vd=0.5 m/s,0.7 m/s進行測試。通過實驗發(fā)現(xiàn),在Ddur=3 s,vd=0.7 m/s時,使用TKNCA的機器人發(fā)生了碰撞。

分別在感知半徑Di=2 m、4 m、6 m、8 m、10 m的情況下,對CABR、CANBR、CAVG進行軌跡跟蹤實驗。對每組軌跡跟蹤數(shù)據(jù),統(tǒng)計各方法從擾動開始的時刻t=4 s到機器人離開中心區(qū)域的時刻t=8.8 s這一時段內(nèi)的ATE。計算CATE=ATE-ATEb,其中ATEb為TKNCA得到的ATE。在不同擾動參數(shù)下,各方法得到的CATE隨Di的變化如圖13所示。

圖13 不同方法得到的CATE

對所有的6種擾動,CABR得到的CATE都是最小的,隨Di的增加的變化不明顯;并且,除了Ddur=3 s,vd=0.7 m/s的情況以外,CABR的CATE幾乎為0。說明CABR能夠避免不必要的避碰動作,而其余兩種方法的CATE都隨Di的增加而增加。

在Di=8 m和10 m時,CANBR的CATE有明顯的上升。這是因為在這兩種情況中,機器人較早地感知到了其他機器人,CANBR根據(jù)當(dāng)前狀態(tài)信息acw2構(gòu)造的Vs使得R1減速讓R3先通過交叉口,而不是按參考軌跡規(guī)定那樣讓R1先通過,由此產(chǎn)生了很大的跟蹤誤差。CABR則使用基于參考軌跡信息得到的acw1來構(gòu)造Vs,保持了R1、R3通過交叉口的先后順序,產(chǎn)生的跟蹤誤差較小。

3.4 多機器人交換位置場景

本場景驗證所提方法在機器人數(shù)量較多時的效果,工作區(qū)域和機器人參考軌跡如圖14所示。機器人的起點位于正方形工作區(qū)域的邊界x=±25 m,y=±25 m上,終點為另一側(cè)邊界上其他機器人的起點。

圖14 機器人的參考軌跡

選擇ct=4.5 m、3.5 m、2.5 m、1.5 m和pe=0.125 m、0.25 m、0.5 m共12組擾動參數(shù)的組合進行測試。通過實驗發(fā)現(xiàn),TKNCA在其中8組擾動下發(fā)生了碰撞。分別在感知半徑Di=2 m、3.5 m、6 m、8.5 m、11 m的情況下,對CABR、CANBR、CAVG進行軌跡跟蹤實驗。和前面的實驗場景一樣,以CATE作為性能指標(biāo)。在計算ATE時,統(tǒng)計時段為整個軌跡跟蹤過程。

若實驗過程中,機器人與參考軌跡的偏差超過10 m,則認(rèn)為跟蹤失敗,不統(tǒng)計該次的數(shù)據(jù)。CABR成功完成了所有60次實驗。CANBR有2次跟蹤失敗,CAVG有20次跟蹤失敗。

在不同擾動下各方法CATE隨Di的變化如圖15所示。CABR得到的CATE比CANBR和CAVG都小,并且隨Di的變化不明顯。在大多數(shù)擾動場景中,CABR的CATE都接近于0。在ct=1.5 m,pe=0.5 m時,機器人的碰撞風(fēng)險最高,CABR的CATE有所增加,但仍然小于CANBR。

通過以上4個場景的實驗,可以得出以下結(jié)論：

(1)利用基于參考軌跡全局信息的避碰觸發(fā)條件進行避碰決策,可以有效減少移動機器人非必要的避碰動作。

(2)基于參考軌跡信息推導(dǎo)避碰速度約束空間Vs,將其作為避碰約束條件,構(gòu)造最小化跟蹤誤差的MPC避碰軌跡跟蹤控制方法,可以實現(xiàn)位置擾動和時間擾動環(huán)境下對參考軌跡的高精度跟蹤。

4 結(jié)束語

針對車間物流移動機器人軌跡跟蹤的魯棒控制問題,本文提出一種基于參考軌跡全局信息的避碰決策及軌跡跟蹤控制方法。該方法在基于當(dāng)前相對位置和相對速度劃分機器人感知區(qū)域的基礎(chǔ)上,構(gòu)建了基于參考軌跡信息的避碰觸發(fā)條件,對是否需要碰撞作進一步的判斷,避免不必要的避碰動作;將機器人的軌跡跟蹤控制建模成以最小化跟蹤誤差為目標(biāo)的MPC問題,根據(jù)參考軌跡信息推導(dǎo)面向軌跡跟蹤誤差優(yōu)化的避碰速度約束空間,將其作為MPC的避碰約束條件,可以實現(xiàn)機器人在位置擾動和時間擾動下的高精度軌跡跟蹤。

下一步工作將致力于將所提方法應(yīng)用到實際移動機器人系統(tǒng)。