基于全局穩(wěn)定的爬壁機(jī)器人雙環(huán)軌跡跟蹤控制

滕 昊，莊 園，鄧世建+

(1.中國礦業(yè)大學(xué) 信息與控制工程學(xué)院，江蘇 徐州 221008；2.云南民族大學(xué) 電氣信息工程學(xué)院，云南 昆明 650540)

0 引 言

對期望軌跡跟蹤控制是爬壁機(jī)器人作業(yè)的基礎(chǔ)，本文依托磁吸附輪式移動爬壁機(jī)器人對軌跡跟蹤控制進(jìn)行分析研究，考慮到現(xiàn)爬壁機(jī)器人軌跡跟蹤控制中主要面臨3個難點[2-4]：跟蹤精度差、控制響應(yīng)速度慢以及系統(tǒng)狀態(tài)不穩(wěn)定問題。

考慮將航向角加入到系統(tǒng)的反饋控制中，即采用位置反饋加上姿態(tài)(航向角)反饋，在航向角為預(yù)期的前提下再到達(dá)指定位置，可有效保證控制精度；而對于響應(yīng)控制問題，目前所應(yīng)用的控制算法主要有如下所示：

(1)非線性狀態(tài)反步控制法[5-8]：通過建立數(shù)學(xué)模型，根據(jù)穩(wěn)定性條件，反推控制律使得實際軌跡和期望軌跡之間的誤差收斂，以準(zhǔn)確跟蹤期望軌跡，由于這種方法響應(yīng)速度較慢，跟蹤誤差收斂的時間較長，且對于不確定參數(shù)沒有較強(qiáng)的適應(yīng)性，所以，在實際應(yīng)用中，存在較大的局限性。

(2)自適應(yīng)反步法[9]：為解決反步法中的不確定參數(shù)無法估計的問題，自適應(yīng)反步通過濾波器反饋信號并對不確定參數(shù)進(jìn)行在線估計，將估計的值最后用反步法得到受約束的控制律，作用于控制系統(tǒng)作用，反復(fù)自調(diào)整、自適應(yīng)最終實現(xiàn)穩(wěn)定的軌跡跟蹤。但該方法的計算十分復(fù)雜龐大，工作效率較低。

(3)滑模跟蹤控制法[10-12]：對于模型已知的控制系統(tǒng)，通過設(shè)計趨近律使得控制誤差快速收斂，迫使系統(tǒng)按照預(yù)定“滑動模態(tài)”的狀態(tài)軌跡運動。由于滑動模態(tài)可以進(jìn)行設(shè)計且與對象參數(shù)及擾動無關(guān)，這對于軌跡跟蹤控制具有較強(qiáng)的魯棒性。

綜合以上分析，滑?？刂扑惴ň哂幸子诳刂贫音敯粜詮?qiáng)的特點對于爬壁機(jī)器人在現(xiàn)場不確定復(fù)雜環(huán)境下更加適用。但是滑?？刂扑惴m然具有較快的響應(yīng)速度與一定的魯棒性，但是系統(tǒng)內(nèi)部的不確定參數(shù)需要根據(jù)系統(tǒng)的特征做進(jìn)一步處理?；诖耍疚囊赃\動學(xué)模型為基礎(chǔ)，對爬壁機(jī)器人軌跡跟蹤控制器進(jìn)行設(shè)計，為整個爬壁機(jī)器人運動控制器搭建可靠的控制框架，以及后續(xù)的動力學(xué)建模提供協(xié)助速度參考，首先建立運動學(xué)模型。

1 爬壁機(jī)器人運動學(xué)模型

本文以4輪磁吸附爬壁機(jī)器人在石油罐體上作業(yè)為例[13]，對控制系統(tǒng)進(jìn)行設(shè)計分析，并驗證穩(wěn)定性。以壁面作為作業(yè)面，建立二維平面坐標(biāo)系如圖1所示。

圖1 爬壁機(jī)器人運動學(xué)模型

圖1中XOY為作業(yè)面坐標(biāo)系，根據(jù)懸吊法，爬壁機(jī)器人的質(zhì)心與幾何中心重合記為參考坐標(biāo)系原點o1， 建立隨爬壁機(jī)器人一起運動的參考坐標(biāo)系o1x1y1。 設(shè)爬壁機(jī)器人的狀態(tài)為p=[x,y,θ]T， 其也可以視為爬壁機(jī)器人本體坐標(biāo)系OXY在作業(yè)面坐標(biāo)系下的位姿量。

設(shè)爬壁機(jī)器人的期望跟蹤狀態(tài)為pd=[xd,yd,θd]T， 爬壁機(jī)器人在作業(yè)面坐標(biāo)系下的運動速度為v， 角速度為ω， 于是可以得到在本體坐標(biāo)系中爬壁機(jī)器人的速度角速度與作業(yè)面坐標(biāo)系中速度角速度的關(guān)系如下

(1)

式中：vX,vY為在爬壁機(jī)器人本體下的速度量，進(jìn)一步可以得到

(2)

式中：可以看出，爬壁機(jī)器人控制系統(tǒng)為欠驅(qū)動系統(tǒng)[15]，可以爬壁機(jī)器人對于位置出動跟蹤，角度θ隨動跟蹤。以此思路，接下來設(shè)計爬壁機(jī)器人的跟蹤控制系統(tǒng)。

2 軌跡跟蹤控制系統(tǒng)

根據(jù)運動學(xué)欠驅(qū)動的特點，以位置量主動跟蹤，角度量隨動跟蹤，設(shè)計雙環(huán)反饋控制系統(tǒng)：位置環(huán)與角度環(huán)，如圖2所示。

圖2 雙閉環(huán)系統(tǒng)結(jié)構(gòu)

在位置環(huán)，通過設(shè)計控制律實現(xiàn)對xd,yd的跟蹤，并在位置環(huán)中產(chǎn)生的角度跟蹤量θd輸入到角度控制環(huán)，實現(xiàn)對偏航角的跟蹤，也就是實現(xiàn)θ的隨動，從而可以準(zhǔn)確跟蹤期望軌跡。下面設(shè)計軌跡跟蹤控制率。

3 軌跡跟蹤控制律設(shè)計

根據(jù)位姿誤差的特點，需要使誤差收斂，具有良好的滑動模態(tài)特性，所以將位姿誤差設(shè)為滑模切換面，與Lyapunov函數(shù)，根據(jù)趨近律約束，利用反步法設(shè)計控制律，以保證系統(tǒng)穩(wěn)定。目前工程上為保證簡單可行，采用直接求導(dǎo)滑模面代入穩(wěn)定函數(shù)中，設(shè)計控制率，這種方法雖然簡單有效，但需要依賴增益的調(diào)節(jié)使得角度快速跟蹤期望，來保證系統(tǒng)穩(wěn)定，這樣就犧牲了速度作為代價，所以本文對此進(jìn)行改進(jìn)，通過設(shè)計約束條件，系統(tǒng)不依賴控制增益調(diào)節(jié)就能夠準(zhǔn)確跟蹤與達(dá)到穩(wěn)定。

考慮到位置環(huán)控制器為主動跟蹤期望軌跡，所以在位置環(huán)控制率設(shè)計中引入動態(tài)全局漸進(jìn)穩(wěn)定性定理，在趨近律中采用雙曲函數(shù)代替一次函數(shù)，并需要外環(huán)位置控制閉環(huán)系統(tǒng)滿足Lipchitz條件，根據(jù)Lyapunov全局穩(wěn)定性，進(jìn)而判斷整個系統(tǒng)的收斂性。首先設(shè)計位置外環(huán)控制率。

3.1 控制律設(shè)計

首先引入動態(tài)全局漸進(jìn)穩(wěn)定雙曲函數(shù)趨近律狀態(tài)方程[16]

(3)

其中，α,k>0。

對于式(3)，若滿足Lipchitz條件，則系統(tǒng)在奇點η=0處是全局漸近穩(wěn)定的。

證明：爬壁機(jī)器人運動控制系統(tǒng)為連續(xù)時間非線性時變系統(tǒng)，則討論此條件下的Lyapunov意義下的穩(wěn)定性。

(1)V為正定；

則系統(tǒng)原點平衡狀態(tài)在原點漸近穩(wěn)定。

(4)

對式(4)求導(dǎo)得到

(5)

根據(jù)上述定理啟發(fā)，取上述狀態(tài)量“x”為位置誤差，設(shè)其滿足Lipchitz條件，定義滑模切換面，根據(jù)反步法設(shè)計控制律，使系統(tǒng)漸近穩(wěn)定，并實現(xiàn)所有位置狀態(tài)量有界并跟蹤期望軌跡。

定義滑模切換函數(shù)為軌跡的偏差量：ex=x-xd,ey=y-yd， 對其求導(dǎo)可得

(6)

(7)

建立好位置誤差微分方程之后需要設(shè)計合適的控制輸入v，作用于位置誤差微分方程(6)、方程(7)，最終使得使得ex,ey有界或者趨于0。

為了方便設(shè)計控制率，這里令

(8)

則位置誤差微分方程轉(zhuǎn)化為

(9)

設(shè)計控制率為

(10)

其中，α1>0,k1>0,α2>0,k2>0。

由式(9)、式(10)可得

(11)

(12)

對于式(11)，設(shè)存在ex1、ex2， 則由Lipchitz條件定義

(13)

由以上分析可見，雙曲函數(shù)趨近律可得位置控制系統(tǒng)漸近穩(wěn)定且ex,ey可以趨近于0。

(14)

將位置控制產(chǎn)生的θd傳遞給內(nèi)環(huán)控制器，通過設(shè)計姿態(tài)控制率ω來保證θ跟蹤θd。

至此，位置控制率設(shè)計完成，由式(8)、式(10)得

(15)

或

(16)

下面需要進(jìn)行內(nèi)環(huán)姿態(tài)控制率的設(shè)計來使得θ跟蹤θd。

3.2 內(nèi)環(huán)姿態(tài)控制率設(shè)計

(17)

由于角度跟蹤需要快速跟蹤期望航向角，以保證爬壁機(jī)器人在到達(dá)指定位置前首先調(diào)整好角度，所以本文根據(jù)高為炳院士提出的趨近律設(shè)計控制率[18]

(18)

證明：不是一般性，令f(s)=k3s3，s=s3，η=α3， 則

(19)

設(shè)Lyapunov函數(shù)為

(20)

滿足Lyapunov全局漸進(jìn)穩(wěn)定性定理條件(1)，對式(20)兩邊同時求導(dǎo)得

(21)

基于此，利用反步法將式(17)代入式(19)可得控內(nèi)環(huán)控制率為

(22)

式中：k3為常數(shù)且大于0；α3表示切換函數(shù)s3趨近于0的速率，反映著糾偏能力的強(qiáng)弱，其為常數(shù)且大于0。在設(shè)計時，不能將α3調(diào)的過大，這樣容易造成抖動。

至此，姿態(tài)控制率設(shè)計完成，綜上述各式，系統(tǒng)的控制律為

(23)

3.3 閉環(huán)系統(tǒng)穩(wěn)定性分析

根據(jù)式(13)的分析可得外環(huán)系統(tǒng)滿足Lipchitz條件，所以，對于有界控制率輸入v或者u1、u2，可保證xe、ye在有限時間內(nèi)有界并收斂?；诖藯l件下，可對雙閉環(huán)系統(tǒng)的全局漸進(jìn)穩(wěn)定性進(jìn)行驗證。

設(shè)雙閉環(huán)系統(tǒng)的Lyapunov函數(shù)為

(24)

式中：α1>0，α2>0，k1>0，k2>0。

由式(24)可知，Lyapunov函數(shù)V滿足全局漸進(jìn)穩(wěn)定判據(jù)(1)(2)，求V的一階導(dǎo)數(shù)得

(25)

考慮到航向角誤差的影響，不能將式(11)、式(12)直接代入。對式(2)中各狀態(tài)量做如下變換

(26)

在位置控制率設(shè)計時以θ隨動控制，所以替換上式中vcosθd與vsinθd為u1、u2并代入，于是可得到位置誤差狀態(tài)方程如下

(27)

為簡化計算，令α1tanh(k1xe)=t1，v(cosθ-cosθd)=t2，α2tanh(k2ye)=t3，v(sinθ-sinθd)=t4代入式(27)，再代入式(25)，得

(28)

(29)

(30)

根據(jù)三角函數(shù)不等式的性質(zhì) |sinx|≤x， 則式(29)、式(30)可變換為

(31)

(32)

因為θe指數(shù)收斂，所以 |cosθ-cosθd|、 |sinθ-sinθd| 均指數(shù)收斂，所以由式(31)、式(32)可得

(33)

(34)

所以，可以判斷

(35)

所以可得

(36)

所以可得

(37)

至此，位置控制器與姿態(tài)控制器設(shè)計完成并已證明運動學(xué)跟蹤控制器雙閉環(huán)系統(tǒng)的全局漸進(jìn)穩(wěn)定性。下面通過仿真來驗證所設(shè)計的算法的可靠性與高效性。

4 仿真驗證

將爬壁機(jī)器人的作業(yè)環(huán)境設(shè)為大型油罐體表面，其自身重量設(shè)為105 kg，輪部半徑較小設(shè)為0.05 m，以直線跟蹤為參考，調(diào)整路徑跟蹤控制器參數(shù)見表1。

表1 路徑跟蹤控制器參數(shù)

以直線路徑作為參照，取參考起始位置[xd(0)yd(0)θd(0)]=[0 0 0]， 取爬壁機(jī)器人的起始位置 [x(0)y(0)θ(0)]=[-6 5 0]， 令xd=t，yd=xd。 與未改進(jìn)的位置滑模趨近律作比較，如圖3～圖5所示。

圖3中圖(a)為一次線性函數(shù)趨近律跟蹤直線仿真圖；圖(b)為雙曲函數(shù)趨近律跟蹤直線仿真圖。從圖中可以得出，相比一次線性函數(shù)趨近律，改進(jìn)后的趨近律可以響應(yīng)速度較快，并用較短的時間內(nèi)跟蹤直線軌跡，且跟蹤過程平穩(wěn)，所以雙曲函數(shù)趨近律可以大幅提高爬壁機(jī)器人的響應(yīng)速度，提高控制精度。

圖3 直線跟蹤仿真

圖4中圖(a)為一次線性函數(shù)趨近律實現(xiàn)對x、y以及θ的跟蹤仿真圖；圖(b)中是雙曲函數(shù)趨近律對x、y以及θ的跟蹤仿真。圖4(a)中現(xiàn)實，xd在一次線性趨近律的收斂時間需要7 s，而改進(jìn)后的趨近律2 s收斂；yd在一次線性趨近律收斂時長同樣也需要7 s，但雙曲函數(shù)趨近律收斂時間僅需要3 s；偏航角的θ的跟蹤由于是隨動跟蹤，收斂時間相差較小。所以，改進(jìn)后的雙曲趨近律比目前使用的一次線性趨近律有著明顯的性能提升，對于現(xiàn)場需要緊急救援等工作起到了關(guān)鍵作用。

圖4 位置、角度的跟蹤

圖5 控制輸入信號v和ω

如圖5中所示，爬壁機(jī)器人剛開始的位置即初始狀態(tài)和初始期望位置存在誤差，所以仿真曲線波動明顯，然后逐漸趨于穩(wěn)定，相較于未改進(jìn)的滑?？刂扑惴▓D(a)，基于全局穩(wěn)定的雙環(huán)滑?？刂茍D(b)的v和ω能夠在更短時間內(nèi)達(dá)到期望狀態(tài)并且波動較小，系統(tǒng)控制輸入量ω穩(wěn)定速度相當(dāng)，但v在5 s后基本保持不變，而未改進(jìn)的滑?？刂扑惴▌t需要10 s左右，改進(jìn)后控制更加穩(wěn)定。

綜上仿真分析，本文利用改進(jìn)的雙曲趨近律設(shè)計的滑?？刂坡煽捎行嵘辣跈C(jī)器人的響應(yīng)，對于現(xiàn)場一些特殊緊集情況的使用具有較好的效果。

5 結(jié)束語

針對爬壁機(jī)器人路徑跟蹤控制，考慮到爬壁機(jī)器人快速跟蹤預(yù)定軌跡，本文改進(jìn)了以便趨近律提出雙曲函數(shù)趨近律，結(jié)合Lyapunov穩(wěn)定性利用反步法設(shè)計控制律，主要包含以下找工作：

(1)基于作業(yè)面坐標(biāo)系與爬壁機(jī)器人本體坐標(biāo)系，建立運動學(xué)方程；

(2)為保證對軌跡跟蹤的精度，設(shè)計了雙環(huán)軌跡跟蹤控制系統(tǒng)工作圖；

(3)根據(jù)控制系統(tǒng)結(jié)構(gòu)，為解決依賴控制增益保證系統(tǒng)內(nèi)部穩(wěn)定的難題，本文提出用雙曲函數(shù)趨近律代替一次函數(shù)的方法，驗證了雙曲函數(shù)趨近律穩(wěn)定性的基礎(chǔ)上，對位置內(nèi)環(huán)控制率進(jìn)行設(shè)計，提高了位置跟蹤的準(zhǔn)確性與快速性；為保證航向角能先于位置收斂以保證跟蹤精度，采用高為炳教授提出的趨近律設(shè)計角速度控制率。對整個雙環(huán)控制系統(tǒng)進(jìn)行穩(wěn)定性分析，結(jié)果滿足Lyapunov全局漸進(jìn)穩(wěn)定，并保證了誤差的收斂性；

(4)對改進(jìn)的基于全局漸近穩(wěn)定的雙環(huán)跟蹤滑?？刂扑惴ㄅc未改進(jìn)的雙環(huán)滑?？刂扑惴ㄟM(jìn)行了比較仿真，改進(jìn)后的控制律跟蹤效更好，能很快糾正系統(tǒng)誤差，響應(yīng)速度更快更準(zhǔn)確，有效提升爬壁機(jī)器人對路徑的準(zhǔn)確性與快速性。

綜上，基于運動學(xué)的控制律設(shè)計為爬壁機(jī)器人的動力學(xué)建模及設(shè)計控制器做好了準(zhǔn)備，搭好了整個運動控制系統(tǒng)的設(shè)計框架，下面只需要根據(jù)動力學(xué)模型將輸出力矩的控制參數(shù)追蹤本文設(shè)計的v,ω以實現(xiàn)追蹤。