羅 蕊，師五喜，李寶全

(天津工業(yè)大學(xué)電氣工程與自動(dòng)化學(xué)院，天津300387)

(*通信作者電子郵箱luor0312@126．com)

0 引言

輪式移動(dòng)機(jī)器人是一種典型的非完整性系統(tǒng)，具有廣泛的應(yīng)用前景。目前大量學(xué)者對(duì)于該系統(tǒng)的軌跡跟蹤問題進(jìn)行了研究:文獻(xiàn)［1］采用后退時(shí)變方法實(shí)現(xiàn)了三輪驅(qū)動(dòng)機(jī)器人軌跡跟蹤;文獻(xiàn)［2］基于滑模變結(jié)構(gòu)控制方法設(shè)計(jì)了移動(dòng)機(jī)器人軌跡跟蹤控制器;文獻(xiàn)［3］采用自適應(yīng)技術(shù)設(shè)計(jì)了控制器，引入坐標(biāo)變換解決了參數(shù)不確定輪式移動(dòng)機(jī)器人的任意軌跡跟蹤統(tǒng)一控制問題。以上方法雖然實(shí)現(xiàn)了移動(dòng)機(jī)器人的軌跡跟蹤，但都忽略了機(jī)器人運(yùn)行過程中的車輪側(cè)滑和滑移干擾問題。

移動(dòng)機(jī)器人系統(tǒng)是一個(gè)多變量耦合的復(fù)雜非線性學(xué)系統(tǒng)，為方便研究，一般假設(shè)機(jī)器人在“純滾動(dòng)無滑動(dòng)”的理想約束下運(yùn)動(dòng)。然而在實(shí)際運(yùn)行時(shí)，這種約束很難保證，如機(jī)器人在潮濕的路面運(yùn)行或急轉(zhuǎn)彎時(shí)，容易出現(xiàn)輪子滑移或側(cè)滑的情況，此時(shí)會(huì)破壞系統(tǒng)的非完整性。文獻(xiàn)［4］針對(duì)輪子產(chǎn)生縱向滑動(dòng)的機(jī)器人設(shè)計(jì)了全局收斂的自適應(yīng)跟蹤控制器;文獻(xiàn)［5］利用極坐標(biāo)變換來補(bǔ)償機(jī)器人的未知滑移和側(cè)滑擾動(dòng);文獻(xiàn)［6］將機(jī)器人滑移、側(cè)滑、內(nèi)部擾動(dòng)和參數(shù)不確定性描述成一種擾動(dòng)形式，提出了一種反饋線性化控制律;文獻(xiàn)［7］采用魯棒動(dòng)態(tài)面控制(Dynamic Surface Control，DSC)方法，利用非線性阻尼方法來抵消側(cè)滑滑移干擾。以上方法均能實(shí)現(xiàn)機(jī)器人軌跡跟蹤，但控制方法相對(duì)復(fù)雜，對(duì)于側(cè)滑滑移干擾不能實(shí)時(shí)補(bǔ)償，且只是對(duì)其控制算法進(jìn)行仿真并沒有應(yīng)用到實(shí)際平臺(tái)中。本文采用自抗擾控制(Active Disturbance Rejection Control，ADRC)方法，對(duì)存在滑移和側(cè)滑干擾的移動(dòng)機(jī)器人進(jìn)行軌跡跟蹤控制。與文獻(xiàn)［5－7］控制算法相比，ADRC不依賴于系統(tǒng)模型，能夠利用擴(kuò)張狀態(tài)觀測(cè)器(Extended State Observer，ESO)實(shí)時(shí)估計(jì)和補(bǔ)償系統(tǒng)的內(nèi)部和外部擾動(dòng)［8］，且控制簡(jiǎn)單、觀測(cè)器參數(shù)的整定有一套成熟的經(jīng)驗(yàn)公式可供參考［9］、系統(tǒng)響應(yīng)快、適應(yīng)范圍廣，所以該方法已經(jīng)大量應(yīng)用到四旋翼飛行器［10］、電力系統(tǒng)［11］以及船舶［12］等領(lǐng)域。

本文通過設(shè)計(jì)輔助運(yùn)動(dòng)學(xué)控制器，使得機(jī)器人的輔助速度漸近收斂到給定期望速度;然后利用反步法思想，通過自抗擾控制使得機(jī)器人實(shí)際運(yùn)動(dòng)軌跡跟蹤到期望軌跡，最后通過仿真和實(shí)驗(yàn)分別驗(yàn)證了所設(shè)計(jì)控制器的有效性。

1 移動(dòng)機(jī)器人模型

非完整性移動(dòng)機(jī)器人運(yùn)動(dòng)模型如圖1所示。

圖1 輪式移動(dòng)機(jī)器人模型Fig．1 Model of wheeled mobile robot

圖1中2b為機(jī)器人兩車輪之間的距離，2r為車輪直徑，v為機(jī)器人線速度，θ為機(jī)器人運(yùn)動(dòng)方向與X軸夾角，oc為機(jī)器人中心，p為機(jī)器人質(zhì)心位置，oc與p之間的距離記為d。

1．1 移動(dòng)機(jī)器人動(dòng)力學(xué)模型

完整性輪式移動(dòng)機(jī)器人動(dòng)力學(xué)方程如式(1)［6］:

其中:M(q)為系統(tǒng)慣性矩陣，V(q，q)為與位置和速度有關(guān)的哥特力和離心力矩陣，G(q)為重心力矢量，G(q)=0，τd為未知擾動(dòng)，τ=［τ1τ2］T為移動(dòng)機(jī)器人兩驅(qū)動(dòng)輪電機(jī)力矩，E(q)為輸入變換矩陣，AT(q)為非完整約束矩陣，λ為約束力項(xiàng)。M(q)，V(q，q)，E(q)表示如下:

其中:m為機(jī)器人的質(zhì)量，I為機(jī)器人的總轉(zhuǎn)動(dòng)慣量，Iw為車輪轉(zhuǎn)動(dòng)慣量。

1．2 受側(cè)滑和滑移影響的機(jī)器人模型

輪式移動(dòng)機(jī)器人在“純滾動(dòng)無滑動(dòng)”的理想約束下運(yùn)動(dòng)時(shí)，其運(yùn)動(dòng)學(xué)模型如式(2):

其中z=［vw］T，v=r(φr+φl(shuí))/2為機(jī)器人線速度，w=r(φr－φl(shuí))/(2b)為機(jī)器人角速度，［φr，φl(shuí)］T為機(jī)器人兩驅(qū)動(dòng)輪轉(zhuǎn)速。

受側(cè)滑和滑移影響時(shí)，移動(dòng)機(jī)器人運(yùn)動(dòng)學(xué)方程如式(5):

2 移動(dòng)機(jī)器人軌跡跟蹤控制器設(shè)計(jì)

本文首先基于移動(dòng)機(jī)器人運(yùn)動(dòng)學(xué)模型，設(shè)計(jì)輔助運(yùn)動(dòng)控制律，然后將移動(dòng)機(jī)器人的線速度和角速度的輔助速度作為輸入，利用反步法思想基于機(jī)器人動(dòng)力學(xué)模型設(shè)計(jì)一階線性自抗擾控制算法，使機(jī)器人的實(shí)際運(yùn)動(dòng)速度漸近收斂到輔助速度，最終實(shí)現(xiàn)機(jī)器人軌跡跟蹤。

2．1 移動(dòng)機(jī)器人運(yùn)動(dòng)控制律設(shè)計(jì)

假設(shè)機(jī)器人參考軌跡的運(yùn)動(dòng)學(xué)模型如式(8):

其中:vr為機(jī)器人期望線速度，wr為期望角速度，(xr，yr，θr)為機(jī)器人期望位姿。

定義軌跡誤差為:

對(duì)式(9)求導(dǎo)可得:

設(shè)計(jì)如下的輔助運(yùn)動(dòng)學(xué)控制律［7］:

其中:vc為機(jī)器人輔助線速度，wc為輔助角速度，k1、k2、k3為輔助運(yùn)動(dòng)控制律參數(shù)，且k1、k2、k3＞ 0。由文獻(xiàn)［7］知，在輔助運(yùn)動(dòng)控制律(11)作用下，機(jī)器人的期望輔助速度［vcwc］T漸近收斂到期望速度［vrwr］T。

2．2 自抗擾控制器設(shè)計(jì)

由上可知，式(11)設(shè)計(jì)的控制律可使輔助速度［vcwc］T漸近收斂到期望速度［vrwr］T，以下利用反步法思想基于機(jī)器人動(dòng)力學(xué)模型(7)設(shè)計(jì)自抗擾控制器，使機(jī)器人實(shí)際速度［v w］T漸近收斂到輔助速度［vcwc］T。為此定義速度跟蹤誤差ze:

由于機(jī)器人動(dòng)力學(xué)系統(tǒng)(7)是一階系統(tǒng)，則本文所設(shè)自抗擾控制器為一階線性自抗擾控制器。

由S(q)、M(q)、V(q，q)、E(q)可求得H(q)如式(13):

同時(shí)可計(jì)算得到式(15):

則機(jī)器人動(dòng)力學(xué)模型(7)可以簡(jiǎn)化為式(16):

注2 文獻(xiàn)［5－7］中，考慮參數(shù)不確定和擾動(dòng)的模型都用式(7)，通過本文研究可知，F(xiàn)1(q，q)=0，因此模型(7)可簡(jiǎn)化為式(16)。

由式(12)、式(16)重建系統(tǒng)模型可得式(17):

則系統(tǒng)控制量τ表示如式(18):

其中:K=diag(kp，kp')，珚D為D的觀測(cè)器估計(jì)值。

本文基于動(dòng)力學(xué)模型，利用自抗擾控制方法，使得機(jī)器人實(shí)際速度z收斂到輔助速度zc，使得ze收斂到0。

其中:z1、z2是兩個(gè)擴(kuò)張狀態(tài)，z2估計(jì)總擾動(dòng)項(xiàng)信號(hào) x2，β1、β2為觀測(cè)器增益。

假定控制器形式如式(22):

其中u0v為虛擬控制量。

由文獻(xiàn)［9］知，當(dāng)［β1，β2］= ［2α1，α12］，其中α1＞ 0決定觀測(cè)器的收斂速度，z2趨近于總擾動(dòng)項(xiàng)信號(hào)x2，由式(18)和式(20)得:

定義虛擬控制量u0v如下:

其中kp為控制系數(shù)，kp＞0。

同理對(duì)于如下角速度系統(tǒng)

其中:x1'=w，x2'=D2(zdist)為角速度控制系統(tǒng)總擾動(dòng)項(xiàng)，δw(t)為角速度控制通道擾動(dòng)量的導(dǎo)數(shù)。

建立如下擴(kuò)張狀態(tài)觀測(cè)器:

其中:z1'、z2'是兩個(gè)擴(kuò)張狀態(tài);β1'、β2'為觀測(cè)器增益，且［β1'，β2'］ = ［2α1'，α1'2］，α1' ＞ 0。

假定控制器形式如式(27):

其中u0w為虛擬輸出量。

由式(23)和式(25)得

定義虛擬控制量u0w如式(29):

綜上，由線速度和角速度自抗擾控制律可得移動(dòng)機(jī)器人兩驅(qū)動(dòng)輪輸出力矩為:

綜上，由輔助運(yùn)動(dòng)學(xué)控制器和基于動(dòng)力學(xué)模型的自抗擾控制器，可得系統(tǒng)控制器總框圖如圖2所示。

3 穩(wěn)定性證明

為了對(duì)系統(tǒng)進(jìn)行穩(wěn)定性分析，作以下假設(shè):

假設(shè)1［14］假定擴(kuò)張狀態(tài)觀測(cè)器估計(jì)誤差:

構(gòu)造系統(tǒng)Lyapunov函數(shù)V如式(31):

由此可知，只有(xe，ye，θe)=(0，0，0)時(shí)=0，而對(duì)于其他任意的(xe，ye，θe)≠0，由Lyapunov穩(wěn)定性理論可知系統(tǒng)的平衡狀態(tài) (xe，ye，θe)=(0，0，0) 漸近穩(wěn)定。

把式(18)代入式(17)可得:

由于 －K是Hurwitz的，則由假設(shè)1和文獻(xiàn)［14］定理3可知，式(33)的平衡狀態(tài)ze=0是漸近穩(wěn)定的。

綜上可得閉環(huán)控制系統(tǒng)漸近穩(wěn)定。

4 仿真

本文的實(shí)驗(yàn)對(duì)象是加拿大Quanser公司研發(fā)的輪式移動(dòng)機(jī)器人 Qbot2，其主要參數(shù)如下:m=3．79 kg，I=2．4 kg·m2，b=0．118 m，d=0，r=0．045 m，Iω=1．05 kg·m2。

為驗(yàn)證本文方法的有效性，在仿真過程中在仿真時(shí)間12～15 s內(nèi)加入模擬側(cè)滑和滑移干擾值，取μ=4 m/s，ξ=［10 sin t －16 sin t］T作為側(cè)滑和滑移模擬擾動(dòng)量，輔助運(yùn)動(dòng)學(xué)控制器參數(shù)為:k1=0．5，k2=25，k3=60;線速度自抗擾控制器參數(shù)為:kp=45，b1=0．025，α1=85;角速度自抗擾控制器參數(shù)為:kp'=15，b2=0．24，α1'=65。

為進(jìn)一步說明自抗擾控制的有效性，在仿真中與傳統(tǒng)PID(Proportion-Integral-Derivative)控制進(jìn)行了對(duì)比，其中線速度 PID 控制參數(shù)為:Pv=500，Iv=15，Dv=85，Pw=80，Iw=10，Dw=48。

本文的研究目的是使移動(dòng)機(jī)器人跟蹤給定的參考軌跡xr=sin t，yr= － cos t，θr=t，參考速度為 vr=1 m/s，wr=1 rad/s，機(jī)器人的初始位姿設(shè)為(0，0，0)。

為驗(yàn)證本文所設(shè)計(jì)控制方法的有效性，將本文方法與傳統(tǒng)PID控制方法進(jìn)行了對(duì)比，仿真結(jié)果如圖3所示。

圖3 ADRC控制與PID控制移動(dòng)機(jī)器人仿真結(jié)果曲線Fig．3 Simulation results of mobile robot by ADRC and PID control

從圖3的仿真結(jié)果看出，系統(tǒng)大約在5 s內(nèi)進(jìn)入穩(wěn)態(tài)，圖(b)、(c)顯示在一階線性自抗擾控制器控制下，ESO觀測(cè)到側(cè)滑和滑移等干擾值;從圖(d)、(e)可以看出，自抗擾控制快速克服了擾動(dòng)干擾，且超調(diào)量小、震蕩小，使得線速度和角速度快速收斂到輔助線速度和角速度，控制性能遠(yuǎn)優(yōu)于傳統(tǒng)PID控制;從圖(f)、(g)可以看出，自抗擾控制跟蹤誤差趨于零，最終滿足了機(jī)器人克服干擾以及軌跡跟蹤要求;從圖(h)、(i)ADRC控制和傳統(tǒng)PID控制輸出力矩曲線可知，傳統(tǒng)PID控制在受到干擾時(shí)不能及時(shí)克服擾動(dòng)，會(huì)產(chǎn)生很大的顫動(dòng)，系統(tǒng)穩(wěn)定性受到了很大影響，而自抗擾控制可以實(shí)時(shí)補(bǔ)償和克服擾動(dòng)，其控制性能遠(yuǎn)優(yōu)于傳統(tǒng)PID控制。

5 實(shí)驗(yàn)

為了進(jìn)一步驗(yàn)證本文方法的有效性，將該算法應(yīng)用到實(shí)驗(yàn)平臺(tái) Qbot2。實(shí)驗(yàn)環(huán)境包括:1臺(tái) Qbot2機(jī)器人，6個(gè)OptiTrack攝像頭，1臺(tái)PC主機(jī)。PC機(jī)是系統(tǒng)的控制中心，安裝有實(shí)驗(yàn)所需耍的控制軟件與攝像機(jī)跟蹤處理軟件(OptiTrack Tacking Tools)，PC機(jī)控制軟件集成了 Matlab/Simulink模塊和無線通信模塊。PC機(jī)與Qbot2通過無線模塊進(jìn)行通信，PC機(jī)和OptiTrack定位系統(tǒng)通過 USB2．0連接。Qbot2安裝有數(shù)據(jù)信息采集卡(HiQ)和QUARC實(shí)時(shí)控制軟件Gumstix嵌入式單片機(jī)，HiQ采集加速度計(jì)、陀螺儀和磁力計(jì)慣性測(cè)量元件的傳感器信號(hào)，Gumstix可以運(yùn)行QUARC，實(shí)現(xiàn)對(duì)Qbot2的實(shí)時(shí)控制，并通過無線模塊向PC機(jī)反饋Qbot2的狀態(tài)信息。OptiTrack定位系統(tǒng)實(shí)時(shí)跟蹤Qbot2位置信息并在PC機(jī)上顯示。系統(tǒng)配置如圖4所示。

圖4 移動(dòng)機(jī)器人實(shí)驗(yàn)平臺(tái)Fig．4 Experimental platform of mobile robot

在實(shí)驗(yàn)中設(shè)定參考軌跡是半徑為1 m的圓，在機(jī)器人運(yùn)行過程中，在路面設(shè)定了干擾，使機(jī)器人在運(yùn)行時(shí)間100 s時(shí)產(chǎn)生側(cè)滑和滑移。實(shí)驗(yàn)過程如圖5所示，在實(shí)驗(yàn)環(huán)境中放置一木板，在機(jī)器人運(yùn)行100 s左右時(shí)，人為用木板水平向圓內(nèi)方向快速推動(dòng)一下機(jī)器人，來模擬機(jī)器人運(yùn)動(dòng)過程中車輪產(chǎn)生的側(cè)滑和滑移干擾。

圖5 移動(dòng)機(jī)器人實(shí)驗(yàn)過程Fig．5 Experimental procedure of mobile robot

實(shí)驗(yàn)結(jié)果如圖6所示。

圖6 Qbot2實(shí)驗(yàn)結(jié)果Fig．6 Experimental results of Qbot2

從圖6可以看出，機(jī)器人在運(yùn)行到100 s左右時(shí)，由于車輪產(chǎn)生側(cè)滑和滑移，使得機(jī)器人運(yùn)動(dòng)軌跡出現(xiàn)明顯的偏移，如圖(a)所示，機(jī)器人運(yùn)動(dòng)線速度角速度誤差明顯變大，如圖(b)(c)所示;在機(jī)器人車輪產(chǎn)生側(cè)滑和滑移時(shí)，ESO快速檢測(cè)到擾動(dòng)信息，擾動(dòng)量輸出如圖(d)所示;系統(tǒng)控制量輸出如圖(e)所示，在100 s時(shí)力矩明顯產(chǎn)生振蕩，但在自抗擾控制作用下快速消除震動(dòng)。通過實(shí)驗(yàn)表明，在機(jī)器人運(yùn)動(dòng)過程中出現(xiàn)側(cè)滑滑移干擾時(shí)，加入自抗擾控制算法后機(jī)器人可以快速跟蹤和克服未知擾動(dòng)，且振蕩小，滿足了移動(dòng)機(jī)器人軌跡跟蹤要求。

6 結(jié)語(yǔ)

本文考慮到移動(dòng)機(jī)器人在運(yùn)動(dòng)過程中的側(cè)滑和滑移干擾問題，設(shè)計(jì)了自抗擾控制器，通過ESO實(shí)時(shí)觀測(cè)和補(bǔ)償擾動(dòng)，克服了機(jī)器人運(yùn)行過程中的內(nèi)外部擾動(dòng)，最終使得機(jī)器人運(yùn)動(dòng)軌跡逐漸收斂到期望軌跡。由仿真和實(shí)驗(yàn)結(jié)果可知自抗擾控制可以實(shí)時(shí)估計(jì)和補(bǔ)償擾動(dòng)，超調(diào)量小、振蕩小、具有較強(qiáng)的魯棒性，滿足了移動(dòng)機(jī)器人軌跡跟蹤以及抗干擾的要求。