基于LESO的輪式移動(dòng)機(jī)器人滑模軌跡跟蹤控制

羅 淇，陳昌忠,2，劉 鑫

（1.四川輕化工大學(xué)自動(dòng)化與信息工程學(xué)院，四川 宜賓 644000；2.人工智能四川省重點(diǎn)實(shí)驗(yàn)室，四川 宜賓 644000）

引言

輪式移動(dòng)機(jī)器人(Wheeled Mobile Robot，WMR)因具有占地面積小、控制簡(jiǎn)單、機(jī)動(dòng)靈活等優(yōu)點(diǎn)被廣泛應(yīng)用于醫(yī)療、民用工程等領(lǐng)域[1-3]。為使WMR經(jīng)預(yù)設(shè)路線到達(dá)指定地點(diǎn)，國(guó)內(nèi)外學(xué)者利用了多種控制方法來(lái)設(shè)計(jì)WMR 軌跡跟蹤控制器，如自抗擾控制[4]、滑模控制[5]、模型預(yù)測(cè)控制[6]、反步法[7]。然而，WMR 的工作路段可能存在路面濕滑、凹凸不平等情況而發(fā)生車輪打滑的情況，使機(jī)器人偏離預(yù)設(shè)軌跡[8-14]。因此，研究車輪打滑情況下的WMR 軌跡跟蹤具有重要實(shí)際意義。

文獻(xiàn)[8]引入滑動(dòng)率表示左右輪的打滑程度，設(shè)計(jì)滑模觀測(cè)器對(duì)打滑擾動(dòng)進(jìn)行估計(jì)。文獻(xiàn)[9]研究Mecanum 輪打滑干擾的全向移動(dòng)機(jī)器人軌跡跟蹤，結(jié)合反步法運(yùn)動(dòng)學(xué)控制器和非線性誤差反饋控制律設(shè)計(jì)了自抗擾反步控制器。文獻(xiàn)[10]基于積分終端滑模控制設(shè)計(jì)了運(yùn)動(dòng)學(xué)模型下的WMR 側(cè)滑擾動(dòng)的軌跡跟蹤控制。文獻(xiàn)[11]利用非線性干擾觀測(cè)器對(duì)機(jī)器人自身以及打滑集總干擾進(jìn)行估計(jì)，設(shè)計(jì)了魯棒跟蹤控制器。文獻(xiàn)[12]設(shè)計(jì)運(yùn)動(dòng)學(xué)控制器和自抗擾動(dòng)力學(xué)控制器并證明了WMR 車輪打滑動(dòng)力學(xué)模型是有界輸入有界輸出且穩(wěn)定的。文獻(xiàn)[13]設(shè)計(jì)WMR 的輔助運(yùn)動(dòng)學(xué)控制器，通過(guò)觀測(cè)器估計(jì)和補(bǔ)償干擾，使機(jī)器人實(shí)際速度收斂到輔助速度。文獻(xiàn)[14]利用神經(jīng)網(wǎng)絡(luò)的方法對(duì)打滑干擾進(jìn)行估計(jì)，設(shè)計(jì)了集合運(yùn)動(dòng)學(xué)模型的動(dòng)力學(xué)控制器。然而，文獻(xiàn)[8-10]僅分析機(jī)器人運(yùn)動(dòng)學(xué)模型，未考慮動(dòng)力學(xué)模型，可能會(huì)使運(yùn)動(dòng)學(xué)控制器提供的虛擬速度與動(dòng)力學(xué)控制器輸出的實(shí)際速度存在偏差；文獻(xiàn)[11]跟蹤誤差位于規(guī)定的范圍，速度跟蹤波動(dòng)較大；文獻(xiàn)[12-14]未進(jìn)行擾動(dòng)誤差分析。

鑒于此，本文利用線性擴(kuò)張狀態(tài)觀測(cè)器和積分滑模面設(shè)計(jì)輪式移動(dòng)機(jī)器人車輪打滑情況下的軌跡跟蹤控制方案。該方案由兩部分組成，運(yùn)動(dòng)學(xué)控制器輸出虛擬速度，動(dòng)力學(xué)控制器輸出力矩。在移動(dòng)機(jī)器人打滑動(dòng)力學(xué)模型下，將總擾動(dòng)作為一個(gè)新的狀態(tài)變量，利用線性擴(kuò)張狀態(tài)觀測(cè)器對(duì)總擾動(dòng)進(jìn)行估計(jì)。基于實(shí)際速度與虛擬速度之間的跟蹤誤差設(shè)計(jì)帶有積分項(xiàng)的滑模面，將總擾動(dòng)的估計(jì)值前饋并結(jié)合積分滑模面設(shè)計(jì)動(dòng)力學(xué)控制器。最后與基于超螺旋干擾觀測(cè)器設(shè)計(jì)的軌跡跟蹤控制方案進(jìn)行仿真對(duì)比，驗(yàn)證所提出方案的有效性。

1 模型描述

WMR 系統(tǒng)模型是非完整約束的[1]，通常由運(yùn)動(dòng)學(xué)模型和動(dòng)力學(xué)模型描述。

1.1 運(yùn)動(dòng)學(xué)模型

輪式移動(dòng)機(jī)器人的運(yùn)動(dòng)模型如圖1 所示，其中圖1(a)表示未打滑情況，圖1(b)表示打滑情況。圖1(a)中，兩后輪為驅(qū)動(dòng)輪，前輪為隨動(dòng)輪，點(diǎn)p為兩驅(qū)動(dòng)輪軸線的中點(diǎn)，點(diǎn)po為移動(dòng)機(jī)器人的質(zhì)心點(diǎn)，定義兩個(gè)坐標(biāo)系用來(lái)描述WMR 的位姿和運(yùn)動(dòng)狀態(tài)，其中XOY是以地面為參考系的全局坐標(biāo)系，xo pyo是以機(jī)器人兩驅(qū)動(dòng)輪軸線的中點(diǎn)為原點(diǎn)的局部坐標(biāo)系。2r為驅(qū)動(dòng)輪直徑，2b為兩驅(qū)動(dòng)輪之間的距離，d為機(jī)器人質(zhì)心與驅(qū)動(dòng)輪軸線中點(diǎn)的距離，υ為WMR行進(jìn)的線速度，ω為機(jī)器人車體轉(zhuǎn)動(dòng)的角速度，θ為方向角即機(jī)器人運(yùn)動(dòng)方向與X軸方向的夾角，φR、φL分別表示右、左驅(qū)動(dòng)輪旋轉(zhuǎn)的角度。定義輪式移動(dòng)機(jī)器人在全局坐標(biāo)系中的位姿向量為q=[x,y,θ,φR,φL]T，其中(x,y)表示兩驅(qū)動(dòng)輪軸線中點(diǎn)p在全局坐標(biāo)系的位置，(xc,yc)表示移動(dòng)機(jī)器人質(zhì)心點(diǎn)po在全局坐標(biāo)系的位置。

圖1 輪式移動(dòng)機(jī)器人運(yùn)動(dòng)示意圖

圖1(b)中，μ為車輪打滑時(shí)對(duì)線速度的擾動(dòng)量；ηL與ηR分別為車輪打滑時(shí)左右驅(qū)動(dòng)輪旋轉(zhuǎn)角速度的擾動(dòng)量。當(dāng)車輪受到打滑影響時(shí)，理想非完整約束條件會(huì)被破壞，因此新約束條件[11]的矩陣表達(dá)式為：

定義矩陣S(q) 為滿足A(q) ·S(q)=0 解空間的一組基，則WMR打滑時(shí)的運(yùn)動(dòng)學(xué)模型[11]為：

1.2 動(dòng)力學(xué)模型

由式(2)可知，當(dāng)車輪在地面上發(fā)生打滑現(xiàn)象時(shí)，其速度會(huì)受到影響，因此引入動(dòng)力學(xué)模型對(duì)車輪速度進(jìn)行調(diào)整，WMR動(dòng)能函數(shù)[7]表示為：

式中，mc為車體質(zhì)量；Ic為WMR 車體繞經(jīng)其質(zhì)心點(diǎn)po點(diǎn)垂直軸的轉(zhuǎn)動(dòng)慣量；Iw為驅(qū)動(dòng)輪繞輪軸的轉(zhuǎn)動(dòng)慣量。

由式(3)對(duì)位姿向量q=[x,y,θ,φR,φL]T中5 個(gè)元素分別求偏導(dǎo)并代入WMR 系統(tǒng)拉格朗日方程得到WMR動(dòng)力學(xué)模型[7]為：

其中，M(q)∈?5×5為WMR 系統(tǒng)的慣性矩陣；V(q,)∈?5×5為科氏矩陣；B(q)∈?5×2為輸入變換矩陣；λ∈?3×1為拉格朗日乘子向量；τ=[τR,τL]T為驅(qū)動(dòng)輪力矩控制的輸入向量。

對(duì)式(2)求導(dǎo)得¨，將式(2)和代入式(4)，并左乘ST(q)，可進(jìn)一步得打滑動(dòng)力學(xué)模型[12]為：

2 控制方案設(shè)計(jì)

WMR 軌跡跟蹤控制方案結(jié)構(gòu)框圖如圖2 所示。運(yùn)動(dòng)學(xué)控制器以WMR 產(chǎn)生的實(shí)際位姿與期望位姿之差即位姿誤差和期望速度作為輸入，輸出為虛擬控制速度zc[14]。LESO 通過(guò)控制量和輸出量估計(jì)系統(tǒng)狀態(tài)，并對(duì)系統(tǒng)受到的總擾動(dòng)進(jìn)行估計(jì)。動(dòng)力學(xué)控制器以實(shí)際速度和虛擬速度之差即速度跟蹤誤差ze和總擾動(dòng)的估計(jì)值作為輸入，輸出為力矩。

圖2 WMR軌跡跟蹤控制方案結(jié)構(gòu)框圖

2.1 運(yùn)動(dòng)學(xué)控制器設(shè)計(jì)

在WMR 軌跡跟蹤控制問(wèn)題中，只需知道位姿向量q中的位置坐標(biāo)(x,y) 和方向角θ便可知機(jī)器人的位置[2,7,12]。因此在運(yùn)動(dòng)學(xué)控制器設(shè)計(jì)中，新定義WMR 在全局坐標(biāo)系下期望位姿狀態(tài)為qr=[xr,yr,θr]T，WMR 從初始位姿狀態(tài)q移動(dòng)到期望位姿qr，位姿誤差示意圖如圖3所示。

圖3 WMR位姿誤差示意圖

由圖3 定義位姿跟蹤誤差為qe=qr-q，其中qe=[ex,ey,eθ]T，WMR位姿誤差模型[1]為：

針對(duì)WMR 位姿誤差模型式(6)，設(shè)計(jì)如下運(yùn)動(dòng)學(xué)虛擬速度控制器[3]：

其中，υr和ωr分別為期望線速度和期望角速度；k1和k2均為大于零的常數(shù)。

2.2 LESO設(shè)計(jì)

LESO 將WMR 在運(yùn)動(dòng)中受到的擾動(dòng)擴(kuò)張成一個(gè)新的狀態(tài)變量x2=h，其中x2=[h1,h2]T，定義狀態(tài)變量x1=[υ,ω]T，則WMR打滑動(dòng)力學(xué)模型式(5)擴(kuò)張為：

定義(i=1,2) 為狀態(tài)變量xi(i=1,2) 的估計(jì)值，根據(jù)文獻(xiàn)[11]對(duì)擴(kuò)張系統(tǒng)式(8)構(gòu)造LESO為：

2.3 動(dòng)力學(xué)控制器設(shè)計(jì)

基于實(shí)際速度與虛擬控制速度之差設(shè)計(jì)積分滑模面，將LESO 估計(jì)的擾動(dòng)值前饋，設(shè)計(jì)于動(dòng)力學(xué)控制器中。定義WMR速度跟蹤誤差為：

根據(jù)滑模控制理論，針對(duì)速度跟蹤誤差（式(10)），帶有積分項(xiàng)的滑模面設(shè)計(jì)為：

其中，c1、c2、a1、a2均為滑模面參數(shù)且a1＞0，a2＜1。定義s=[s1,s2]T，c=diag{c1,c2}，a=diag{a1,a2}，則積分滑模面為：

針對(duì)WMR 速度跟蹤誤差式(10)，結(jié)合積分滑模面式(12)設(shè)計(jì)以下動(dòng)力學(xué)控制器：

定理1WMR 在動(dòng)力學(xué)控制器式(13)中參數(shù)選擇合適情況下，速度跟蹤誤差式(10)將在有限時(shí)間內(nèi)收斂至零。

由式(5) 和 式(8) 得=Hτ+x2,將其代入式(14)得：

將式(13)代入式(15)可得：

定義ε=x2-為L(zhǎng)ESO 的擾動(dòng)估計(jì)誤差，則式(16)改寫(xiě)為：

根據(jù)文獻(xiàn)[15]定理2，LESO 的估計(jì)誤差ε在總擾動(dòng)h有界情況下是有界的，則存在正數(shù)M1，使得‖ε‖≤M1，式(17)由范數(shù)的相容性得：

當(dāng)t＞T時(shí)，對(duì)式(19)求導(dǎo)可得：

參考文獻(xiàn)[17]中定理4.2，求解式(20)可得：

其中，ci∈c，ai∈a，則速度跟蹤誤差式(10)將在有限時(shí)間內(nèi)收斂至零，由此定理1得證。

3 仿真實(shí)驗(yàn)分析

3.1 軌跡跟蹤仿真分析

為驗(yàn)證本文基于LESO 觀測(cè)擾動(dòng)的優(yōu)勢(shì)，給出了其與基于超螺旋干擾觀測(cè)器(Super-Twisting Disturbance Observer，STDOB)設(shè)計(jì)的軌跡跟蹤控制方案的仿真對(duì)比結(jié)果。根據(jù)文獻(xiàn)[18]，基于打滑動(dòng)力學(xué)模型式(5)，超螺旋干擾觀測(cè)器設(shè)計(jì)為：

在Matlab/Simulink 仿真中，差速版智能小車（型號(hào)L150，輪趣科技（東莞）有限公司）的物理參數(shù)見(jiàn)表1。輪式移動(dòng)機(jī)器人的初始參考位姿qr=[0,1,π]T，期望速度υr=1 m/s，ω=1 rad/s，在本次仿真對(duì)比實(shí)驗(yàn)中選擇圓形參考軌跡xr=cost，yr=sint進(jìn)行仿真實(shí)驗(yàn)，仿真時(shí)間設(shè)定為20 s。仿真中假定的打滑擾動(dòng)參數(shù)見(jiàn)表2。仿真中控制方案參數(shù)見(jiàn)表3。

表1 WMR物理參數(shù)

表2 打滑擾動(dòng)參數(shù)

表3 控制方案參數(shù)

對(duì)LESO 和STDOB 觀測(cè)下的WMR 圓形參考軌跡跟蹤、位置、方向角、線速度和角速度跟蹤誤差進(jìn)行仿真對(duì)比，對(duì)比結(jié)果如圖4所示。從圖4(a)中可以看出，兩種觀測(cè)器下均能跟蹤參考軌跡，但LESO 在x軸[-1.0,0.5]及y軸[-1.0,0.5]的14 圓弧段的軌跡跟蹤誤差比STDOB更小。從圖4(b)可以看出兩種觀測(cè)器下的位置跟蹤誤差均能在一定時(shí)間內(nèi)收斂至零，但在3.0～6.0 s 時(shí)STDOB 的位置誤差收斂較差，約8.0 s 左右逐漸收斂誤差逐漸與LESO 誤差一致。從圖4(c)可以看出兩種觀測(cè)器下的方向角跟蹤誤差均能在一定時(shí)間內(nèi)收斂至零，但在約3.5～6.0 s 左右STDOB 方向角誤差較LESO 波動(dòng)更大。從圖4(d)可以看出兩種觀測(cè)器下的線速度跟蹤誤差差別不大。從圖4(e)可以看出在1.0～4.0 s 時(shí)，STDOB 觀測(cè)下角速度誤差波動(dòng)更大。

圖4 LESO和STDOB下WMR圓形參考軌跡跟蹤及其誤差仿真對(duì)比

3.2 軌跡跟蹤仿真收斂性與擾動(dòng)分析

對(duì)LESO和STDOB觀測(cè)下WMR擾動(dòng)誤差、滑模面和驅(qū)動(dòng)輪力矩進(jìn)行仿真對(duì)比，對(duì)比結(jié)果如圖5 所示。從圖5(a)和圖5(b)可以看出在LESO觀測(cè)下擾動(dòng)誤差波動(dòng)較小，然而在STDOB 觀測(cè)下擾動(dòng)誤差明顯出現(xiàn)鋸齒形抖動(dòng)，這意味著LESO 抗干擾能力強(qiáng)，觀測(cè)精度較高，收斂誤差小。從圖5(c)可以看出在LESO 觀測(cè)下滑模面較快趨于零，且無(wú)明顯抖動(dòng)。從圖5(d)和圖5(e)可以看出在LESO觀測(cè)下左右驅(qū)動(dòng)輪力矩較STDOB更穩(wěn)定，無(wú)明顯抖動(dòng)。

圖5 LESO和STDOB下擾動(dòng)誤差、滑模面和驅(qū)動(dòng)輪力矩仿真對(duì)比

由圖4 和圖5 可以看出在LESO 觀測(cè)下，對(duì)擾動(dòng)誤差和滑模面的觀測(cè)值均能穩(wěn)定收斂至零，不會(huì)發(fā)生過(guò)多抖動(dòng)；力矩控制效果也穩(wěn)定。這可能是因?yàn)長(zhǎng)ESO 對(duì)擾動(dòng)進(jìn)行估計(jì)并對(duì)動(dòng)力學(xué)控制器進(jìn)行補(bǔ)償時(shí)，不依賴于擾動(dòng)的具體模型，因此本文提出的輪式移動(dòng)機(jī)器人軌跡跟蹤控制方案具有良好的抗干擾能力。而STDOB 是通過(guò)對(duì)系統(tǒng)模型的測(cè)量值與其估計(jì)值之間的誤差進(jìn)行估計(jì)的，在滑模控制設(shè)計(jì)中邊界層切換不光滑而產(chǎn)生抖振現(xiàn)象，使其魯棒性較差。

本文方法與自抗擾控制不同誤差跟蹤對(duì)比如圖6 所示。圖6(a)所示為輪式移動(dòng)機(jī)器人在本文方法與文獻(xiàn)[12]中自抗擾控制方法下的角速度跟蹤誤差仿真曲線對(duì)比結(jié)果，圖6(b)所示為兩種方法下的X軸跟蹤誤差仿真曲線對(duì)比結(jié)果，圖6(c)所示為兩種方法下的Y軸跟蹤誤差仿真曲線對(duì)比結(jié)果。從圖6(a)可以看出兩種方法下角速度跟蹤誤差均能夠趨近于零，但是自抗擾控制方法要在4.0 s 左右才到達(dá)零曲線，而本文方法在2.5 s 左右已達(dá)到零曲線，這表明本文方法對(duì)角速度的跟蹤性能較好。從圖6(b)與圖6(c)可以看出兩種方法下，位置誤差均能趨近于零；圖6(b)中，X軸跟蹤誤差在2.0 s 和5.0 s時(shí)，本文方法較自抗擾控制方法波動(dòng)值小；圖6(c)中，Y軸跟蹤誤差在3.5～4.5 s 時(shí)，本文方法較自抗擾控制方法曲線波動(dòng)值小。圖6(b)和圖6(c)對(duì)比結(jié)果表明本文方法在4.0 s 左右已經(jīng)跟蹤上圓形軌跡，而自抗擾控制方法需在4.5 s 左右跟蹤上圓形軌跡。這可能是因?yàn)椋疚姆椒ㄔ谠O(shè)計(jì)動(dòng)力學(xué)控制器時(shí)增加了滑模面控制，而滑模控制響應(yīng)速度快的優(yōu)點(diǎn)在輪式移動(dòng)機(jī)器人軌跡跟蹤中發(fā)揮了作用。

圖6 本文方法與自抗擾控制不同跟蹤誤差對(duì)比

4 結(jié)束語(yǔ)

為解決車輪發(fā)生打滑情況的輪式移動(dòng)機(jī)器人軌跡跟蹤問(wèn)題，提出了一種基于線性擴(kuò)張狀態(tài)觀測(cè)器的滑模控制方案。該方案利用線性擴(kuò)張狀態(tài)觀測(cè)器對(duì)系統(tǒng)模型受到的總擾動(dòng)進(jìn)行估計(jì)，將總擾動(dòng)的估計(jì)值前饋并結(jié)合速度跟蹤誤差的積分滑模面設(shè)計(jì)了動(dòng)力學(xué)控制器。仿真結(jié)果表明該控制方案能夠使移動(dòng)機(jī)器人跟蹤圓形參考軌跡，位置、方向角、速度跟蹤誤差能較快收斂至零。與基于超螺旋干擾觀測(cè)器設(shè)計(jì)的軌跡跟蹤控制方案相比，所提出方案的擾動(dòng)誤差收斂誤差小，擾動(dòng)觀測(cè)精度更高，滑模面能無(wú)抖動(dòng)收斂至零，左右驅(qū)動(dòng)輪力矩穩(wěn)定，無(wú)鋸齒狀抖動(dòng)。