許正榮， 王文周， 辜麗川， 喬 焰， 褚剛秀， 焦 俊

(安徽農(nóng)業(yè)大學信息與計算機學院，安徽 合肥 230036)

由于農(nóng)用履帶機器人(Agricultural tracked robot, ATR)具有體積小、活動范圍大等優(yōu)點，已經(jīng)在農(nóng)田環(huán)境監(jiān)測、農(nóng)產(chǎn)品采摘等領(lǐng)域獲得高度重視。對ATR進行穩(wěn)定、精確的控制是ATR的關(guān)鍵技術(shù)之一[1-2]。但是ATR運動方程的非線性、耦合性、系統(tǒng)參數(shù)的時變性，以及農(nóng)田地形復雜性、環(huán)境多變性都給ATR的控制增加了難度，常規(guī)的線性控制法難以滿足農(nóng)業(yè)實際應用需求和高精度的控制性能要求。一般的控制方法中，反饋線性化方法在系統(tǒng)存在不確定性，受到干擾時其魯棒性沒有保障[3]；Backstepping法需要系統(tǒng)的模型滿足狀態(tài)線性化；神經(jīng)網(wǎng)絡控制法控制算法復雜[4]；PID控制法對于非線性和結(jié)構(gòu)不確定系統(tǒng)的控制效果較差[5]；焦俊等[6]針對移動機器人動力學的高度非線性和運動環(huán)境的不確定性，提出基于模糊邏輯的移動機器人路徑跟蹤控制方法，該方法對于差動轉(zhuǎn)向移動機器人的運動控制具有普適性，但其對較大曲率的軌跡跟蹤效果表現(xiàn)欠佳。

滑模變結(jié)構(gòu)控制不依賴被控對象精確的數(shù)學模型，具有對參數(shù)和環(huán)境變化不敏感和工程實現(xiàn)簡單的特性，然而，“抖振”問題是滑?？刂票旧砉逃械娜毕?，它會使機器人的執(zhí)行機構(gòu)在頻繁切換中出現(xiàn)故障，降低系統(tǒng)穩(wěn)定性。對此，有研究者試圖通過在切換函數(shù)中增加積分項來解決這一問題，雖然積分項在一定程度上降低了“抖振”，但是隨著積分項的引入，又會導致在初始誤差較大的情況下，出現(xiàn)積分飽和現(xiàn)象，使系統(tǒng)出現(xiàn)大幅度超調(diào)，甚至不穩(wěn)定[7-10]，阻礙了滑?？刂频膽?。

為了解決該問題，本研究從控制策略角度出發(fā)，構(gòu)建變傾斜參數(shù)的自適應積分滑模切換函數(shù)，基于該函數(shù)提出由等效控制和切換控制組成的自適應滑模跟蹤控制，將機器人的速度和位姿誤差反饋至變結(jié)構(gòu)控制中，然后根據(jù)運動學關(guān)系，分解驅(qū)動輪的期望速度，同時從理論上分析證明該控制方法的可達性和穩(wěn)定性，從仿真和試驗上驗證該控制方法的有效性。

1 軌跡跟蹤的位姿誤差模型

ATR的幾何結(jié)構(gòu)俯視圖以及坐標系定義的簡化圖如圖1所示, 其中XOY、xboayb分別為慣性坐標系和跟蹤坐標系，ATR狀態(tài)由質(zhì)心Oa在XOY坐標系中的位置及航向θ來表示，其中(x，y)為機器人質(zhì)心在XOY坐標系中的坐標,θ為ATR運動方向和X軸的夾角,ν和ω分別表示ATR的線速度和角速度[11-12]，p為機器人的幾何中心，d為質(zhì)心p與幾何中心Oa之間的距離，驅(qū)動輪半徑為r，兩輪間軸向距離為2A。根據(jù)圖1得到履帶機器人運動學模型為式(1)，ATR的實際位姿為p=(X,Y,θ)，期望位姿為pr=(Xr,Yr,θr)T，ATR的位姿誤差如圖2所示。

圖1 機器人模型Fig.1 Sketch of agricultural tracked robot(ATR) model

圖2 位姿誤差示意圖Fig.2 Sketch map of position and orientation error

(1)

軌跡跟蹤誤差為式(2)：

(2)

式中，xe和ye分別為xb軸和yb軸向誤差，θe為航向角誤差，將式(2)微分，結(jié)合式(1)得到如式(3)的位姿誤差微分方程。

(3)

2 自適應滑模軌跡跟蹤控制設(shè)計

為了實現(xiàn)ATR的自適應滑模軌跡跟蹤控制(Adaptive sliding mode trajectory tracking control, ASMTTC)，在分析了位姿誤差模型和滑?！岸墩瘛睂ο到y(tǒng)穩(wěn)定性影響的基礎(chǔ)上，構(gòu)建基于變sigmoid的積分切換函數(shù)的ASMTTC，將位姿誤差和可調(diào)參數(shù)反饋至ASMTTC中，ASMTTC再輸出左右驅(qū)動輪角速度，使得ATR的位姿滿足式(4)，ASMTTC控制系統(tǒng)結(jié)構(gòu)如圖3所示。

(4)

式中p是機器人位姿，pr是期望位姿。

圖3 自適應滑模控制結(jié)構(gòu)圖Fig.3 Structure diagram of adaptive sliding mode trajectory tracking control (ASMTTC)

2.1 切換函數(shù)的設(shè)計

針對位姿誤差微分方程式(3)的特點，設(shè)計基于變傾斜參數(shù)的Sigmoid滑模切換函數(shù)式(5)。由于使用了具有飽和特性的非線性函數(shù)，這樣在出現(xiàn)較大位姿誤差時，就抑制了積分項的作用，避免出現(xiàn)較大的超調(diào)；在誤差較小時具有一定的放大作用，在不引起“抖振”的情況下使控制精度得到提高[13-14]。

(5)

式中kk1、kk2均為正常數(shù)，α1、α2為傾斜參數(shù)，其自適應調(diào)節(jié)公式為式(6)。

(6)

式中c1、c2、c3、c4均為正常數(shù)，從式(6)可知，α1、α2隨著位姿誤差的減小而增大。

2.2 滑模控制律設(shè)計

對式(5)求導可得式(7)。

(7)

將式(3)代入式(7)，得式(8)。

(8)

令式(8)等于0，得到等效控制式(9)。

(9)

式中Ueq是等效控制。令切換控制為:

(10)

式中β1、β2為切換增益，均大于0；sat( )為飽和函數(shù)；△1、△2為邊界層厚度。若邊界層厚度值較大，抗“抖振”能力較強，會降低控制精度；若厚度值較小，能夠提高控制精度，但易引起“抖振”。

綜合式(9)和式(10)，得到總的滑?？刂坡蒛=Ueq+Usw。

(11)

2.3 穩(wěn)定性分析

從邊界層的可達性和邊界層內(nèi)系統(tǒng)平衡點的穩(wěn)定性方面分析證明，ASMTTC可以控制機器人跟蹤軌跡收斂于平衡點[15-18]，即跟蹤位姿誤差趨于0。

定理1：考慮系統(tǒng)式(3)，按式(5)構(gòu)造切換函數(shù)，若采用式(11)所示的滑?？刂坡?，則式(3)所描述的系統(tǒng)將在有限時間內(nèi)到達滑模邊界。

證明：以s1為切換函數(shù)為證明實例，當系統(tǒng)到達邊界層時，存在式(12)

(12)

(13)

(1)當s1>0時，當系統(tǒng)到達滑模邊界層時，t=t0，s1(t0)=△1。由式(13)，可得式(14)，

(14)

(2)當s1<0時，若系統(tǒng)到達滑模邊界層，s1(t0)=-△1，由式(13)，可得式(15)。

(15)

(3)當s1=0時，t=t0=0，系統(tǒng)到達滑模邊界層內(nèi)。

因為△1,λ是有界常實數(shù)，所以從上述3種情況可知，在切換函數(shù)初值s1(0)有界的條件下，可以在有限時間內(nèi)到達滑模邊界層，到達時間t0<[s1(0)-△1]λ-1。

證明：以切換函數(shù)s1為證明實例，當系統(tǒng)到達滑模邊界層后，存在

(16)

記ρ為拉普拉斯變換算子，對式(16)進行拉普拉斯變換，s1,d(t)拉普拉斯變換后分別為s1(ρ),d(ρ)，得到

(17)

結(jié)合拉普拉斯變換終值定理，可得

(18)

(19)

運用Lyapunov函數(shù)

(20)

顯然，V>0，并且滿足|xe|→時，|V|→，是徑向無界，存在

(21)

當t→時，依據(jù)式(19)可得

因此，存在

(22)

2.4 驅(qū)動控制

根據(jù)式(11)，當運動學模型輸入一個速度[vdωd]T時，相應的左、右驅(qū)動輪輸出的角速度為ωld、ωrd：

ωld=(vd(t)-ωdA)r-1

(24)

ωrd=(vd(t)+ωdA)r-1

(25)

3 仿真及結(jié)果分析

為了檢驗所設(shè)計控制算法的有效性，運用MATLAB搭建仿真模型，對ATR控制系統(tǒng)進行仿真分析，機器人長度L=0.55 m，寬度W=0.4 m，驅(qū)動輪半徑r=0.1 m，兩驅(qū)動輪軸間距2A=0.36 m?？刂破鲄?shù)：kk1=kk2=2，c1=c3=2，c3=c4=4，kωl=kωr=1 000，仿真采樣時間為50 ms。

3.1 基于ASMTTC與SMC的階躍信號響應特性分析

分別采用ASMTTC和指數(shù)趨近律的滑模控制(Sliding mode control，SMC)對初始值為0、最大值為20 rad/s的階躍信號進行仿真控制研究。由于左、右驅(qū)動輪的仿真條件、參數(shù)和模型一樣，因此本仿真只列出左驅(qū)動輪的階躍響應特性曲線。由圖4可知，在ASMTTC調(diào)節(jié)下，驅(qū)動輪角速度從0開始，響應曲線平滑，系統(tǒng)在0.375 s達到穩(wěn)定狀態(tài)，輸出角速度的終止值為20 rad/s，角速度跟蹤誤差從-20 rad/s收斂趨近于0。這是由于ASMTTC中滑模切換函數(shù)中采用了變傾斜參數(shù)的積分項，在系統(tǒng)出現(xiàn)較大誤差情況時，能夠限制積分項作用，使系統(tǒng)不出現(xiàn)過大的超調(diào)；在誤差較小時有放大作用，在不引起抖振的情況下改善控制精度。而在SMC控制時，系統(tǒng)需要0.75 s才能達到穩(wěn)定狀態(tài)，且有抖振現(xiàn)象。

圖4 ASMTTC和SMC的階躍響應曲線Fig.4 Step response curves of ASMTTC and SMC

3.2 基于ASMTTC折線軌跡的跟蹤控制

以折線為參考軌跡，其初始位姿為[0,0,pi/4]T，ATR初始位姿為[-2,-2,pi/4]T，ATR速度為2 m/s，仿真時間0

3.3 基于ASMTTC圓軌跡的跟蹤控制

選取半徑為10 m的圓形軌跡：x=10 cosθ、y=10 sinθ為參考路徑，軌跡初始位姿為[10,0,pi/2]T，ATR的初始位姿為[7,0,pi/2]T，左右驅(qū)動輪角速度都從0開始，分別達到10 rad/s、30 rad/s后，ATR以2 m/s速度逆時針方向運行，仿真時間0

圖5 折線軌跡跟蹤結(jié)果(a)及誤差曲線(b)Fig.5 Tracking result (a)and error curve (b)of fold line trajectory

圖6 圓軌跡跟蹤結(jié)果(a)及誤差曲線(b)Fig.6 Tracking result (a)and error curve (b)of circular trajectory

4 驗證與分析

為了驗證ASMTTC的正確性和有效性，構(gòu)建基于S3C2440的控制系統(tǒng)，以自主研發(fā)的ATR為控制對象，進行野外控制試驗。

4.1 控制系統(tǒng)

控制器系統(tǒng)主要完成對慣導信息采集和履帶機器人的控制。根據(jù)控制策略與控制算法構(gòu)建由硬件和軟件兩部分組成的控制系統(tǒng)。硬件部分包括S3C2440微處理器、存儲模塊、慣導系統(tǒng)、傳感器模塊、電機驅(qū)動單元、數(shù)據(jù)通信模塊和電源等，其中微控制器模塊負責整個數(shù)據(jù)的處理和運算，存儲模塊負責信息存儲，電機驅(qū)動單元驅(qū)動機器人運行，傳感器模塊負責采集現(xiàn)場信息，電源模塊為電路提供工作電壓，數(shù)據(jù)通信模塊負責與遠端服務器進行信息通信?？刂破鹘Y(jié)構(gòu)見圖7，實物見圖8。

圖7 控制器硬件結(jié)構(gòu)Fig.7 The hardware structure of controller

圖8 控制器硬件實物圖Fig.8 The hardware object of controller

4.2 控制軟件

控制軟件運行在Linux操作系統(tǒng)環(huán)境下，主要由驅(qū)動和應用層程序組成。驅(qū)動程序與底層硬件緊密相連。應用層程序是根據(jù)編程接口(API)函數(shù)，針對控制需求而編寫的應用程序?？刂栖浖軜?gòu)和軟件流程分別如圖9和圖10所示。

圖9 控制軟件架構(gòu)Fig.9 Structure diagram of control software

圖10 控制軟件流程圖Fig.10 Flowchart of control software

在采集慣導數(shù)據(jù)之前，微處理器先初始化慣導的數(shù)據(jù)格式和更新率。慣導經(jīng)過內(nèi)部差分解算處理后，輸出格式和含義如表1所示的ASCII碼數(shù)據(jù)，數(shù)據(jù)間以逗號隔開，從串口傳給微處理器。微處理器對接收的慣導數(shù)據(jù)進行坐標轉(zhuǎn)換，存儲位姿信息，計算控制量。程序流程見圖11。

表1 SPAN-CPT的數(shù)據(jù)格式(標準國際單位)

圖11 慣導數(shù)據(jù)采集與處理流程Fig.11 Data collection and processing of the inertial navigation system

為了計算ATR位姿誤差，需要實現(xiàn)大地坐標與笛卡爾坐標的轉(zhuǎn)換，轉(zhuǎn)換程序如下：

void GdToCt (PCRDCARTESIANdkp, PCRDGEODETICddp,doublecbz, doublebl)

// dkp：笛卡爾坐標指針；ddp：大地坐標指針；cbz：參考橢球的長半軸；bl：參考橢球的扁率。

天津濱海新區(qū)是國家發(fā)展戰(zhàn)略的重要一極，是中國的，更是世界的。一個面向世界的國際化城市不能沒有自己的城市品牌，特別是在西方發(fā)達城市普遍重視建設(shè)自己的城市品牌的形式下，天津濱海新區(qū)城市品牌的建設(shè)顯得尤為重要，也尤為迫切。

{ doublef; //偏心率平方

doubler; //卯酉圈半徑

f=2*bl-bl*bl;

r=cbz/sqrt(1-f*sin(ddp→latitude)*sin(ddp→latitude));

dkp→x=(r+ddp→height)*cos(ddp→latitude)*cos(ddp→longitude);

dkp→y=(r+ddp→height)*cos(ddp→latitude)*sin(ddp→longitude);

dkp→z=(r*(1-f)+ddp→height)*sin(ddp→latitude); }

static unsigned int fuzh( unsigned int *mmap_addr , unsigned int zh ,unsigned int clsf ,unsigned char m)

// mmap_addr:映射寄存器的地址；zh:要設(shè)置的值；clsf:清除標志；m:清除的位置

{ unsigned int tmp1; //暫存寄存器值

unsigned int tmp2;

tmp2 = ioread32( mmap_addr );//讀取寄存器的值

tmp1 = tmp2;

tmp2 = tmp2 & (～(clsf << m)); //清除tmp2對應位

tmp2 = tmp2 | zh << m;// 設(shè)置tmp2對應位

iowrite32( tmp2 , mmap_addr );//將tmp2值寫入mmap_addr寄存器

return tmp1 } ; //返回原來寄存器的值

野外控制試驗在長有雜草的沙壤土農(nóng)田中進行(圖12)。以ATR上安裝的慣導SPAN-CPT作為狀態(tài)信息獲取設(shè)備，狀態(tài)信息更新率是10 Hz，速度精度是0.01 m/s，角度精度是0.02 rad，位置測量精度是0.01 m。

圖12 試驗中履帶機器人Fig.12 ATR used in actual experiment

ATR參考路徑為

(26)

ATR長度L=0.55 m，寬度W=0.4 m，驅(qū)動輪半徑r=0.1 m，兩驅(qū)動輪軸間距2A=0.36 m，ATR運行速度為2 m/s，履帶機器人的初始位姿為

[x(0)y(0)θ(0)]T=[0 20 π/12]T

期望位姿為

[x(0)y(0)θ(0)]T=[10 40 π/4]T

初始位姿誤差為

[xe(0)ye(0)θe(0)]T=[10 20 π/6]T

ASMTTC對ATR控制后的軌跡結(jié)果顯示，除初始位置和參考軌跡曲率變化較大的區(qū)域外，其他區(qū)域的跟蹤軌跡較為平滑(圖13)。從ASMTTC控制下產(chǎn)生的誤差曲線圖中可以發(fā)現(xiàn)，在ATR運動的初始階段，由于ATR初始位姿與初始命令位姿不一致，使得初始位姿誤差較大，在39～50 s和79～90 s期間，由于路徑曲率變化較大，機械轉(zhuǎn)向幅度較大，ATR受到的側(cè)滑和離心力影響也較為嚴重，引起較大的位姿誤差，所產(chǎn)生的xe、ye、θe誤差范圍分別為-0.03～0.04 m、-0.08～0.06 m、-0.03～0.05 rad。ATR運行在曲率變化較小的區(qū)域時，跟蹤軌跡十分平滑，實際運行軌跡與參考軌跡之間的誤差較小，趨近于0。

a：軌跡跟蹤曲線；b：軌跡跟蹤誤差曲線。圖13 ATR曲線軌跡跟蹤結(jié)果Fig.13 Tracking result of curve trajectory for ATR

在相同試驗條件、不同運行速度下，ATR對同一軌跡跟蹤時產(chǎn)生的誤差見表2。由表2可知，當ATR分別以1 m/s、3 m/s和4 m/s速度運行時，xb軸向的最小距離誤差是-0.04 m，最大距離誤差是0.04 m；yb軸向的最小距離誤差是-0.09 m， 最大距離誤差是0.07 m；航向角θe最小誤差是-0.03 rad，最大誤差是0.05 rad?？梢?，當以曲線和直線組合的路徑為參考軌跡時，運用ASMTTC控制ATR，具有較好的穩(wěn)定性和控制精度。

由于試驗設(shè)備條件限制，目前ATR試驗速度都比較低，高速和惡劣環(huán)境下的ASMTTC性能試驗將是下一階段的主要研究工作。

表2 低速條件下的軌跡跟蹤位姿誤差

5 結(jié) 論

針對ATR的軌跡跟蹤問題，在運動學模型的基礎(chǔ)上，構(gòu)建了變傾斜參數(shù)的自適應積分滑模切換函數(shù)，基于這個函數(shù)提出了由等效控制和切換控制組成的自適應滑模跟蹤控制。在出現(xiàn)較大位姿誤差的情況下，切換函數(shù)能夠限制積分項作用，使系統(tǒng)不出現(xiàn)過大的超調(diào)；在誤差較小的情況下，有一定的放大作用，提高控制精度。在不引起抖振的條件下，消除靜態(tài)誤差，使控制精度得到提高。在1～4 m/s速度條件下，位姿參數(shù)誤差范圍分別為：-0.04≤xe≤0.04 m、-0.09≤ye≤0.07 m、-0.03≤θe≤0.05 rad。本研究結(jié)果表明ASMTTC能夠滿足ATR田間作業(yè)的實際需求。

參考文獻：

[1] 焦 俊,陳無畏,王繼先,等. 基于GA和LS-SVM的AGV變結(jié)構(gòu)控制[J].系統(tǒng)仿真學報，2008,20(14)：3777-3781.

[2] TEMEL T, ASHRAFIUON H. Sliding mode speed controller for tracking of under actuated surface vessels with extended kalman filter[J]. Electronics Letters, 2015, 51(6): 467-269.

[3] BLAZIC S. A noval trajectory tracking control law for wheeled mobile robots[J]. Robotics and Autonomous Systems, 2011, 59(11): 1001-1007.

[4] 焦 俊,江朝暉,金瑞春,等.農(nóng)用機器人轉(zhuǎn)向系統(tǒng)自適應內(nèi)?？刂芠J].農(nóng)業(yè)機械學報，2011,42(10):186-191,234.

[5] ASIF M, KHAN M J, CAI N. Adaptive sliding mode dynamic controller with integrator in the loop for nonholonomic wheeled mobile robot trajectory tracking[J]. International Journal of Control, 2014, 87(5): 964-975.

[6] 焦 俊，孔 文，王 強，等. 基于輸入模糊化的農(nóng)用履帶機器人自適應滑?？刂芠J]. 農(nóng)業(yè)機械學報，2015，46(6)：14-19,13.

[7] 熊中剛，葉振環(huán)，賀 娟，等. 基于免疫模糊 PID 的小型農(nóng)業(yè)機械路徑智能跟蹤控制[J]. 機器人，2015，37(2)：212-223.

[8] 鄭澤偉，霍 偉，諸 兵. 非完整移動機器人全局路徑跟蹤控制[J]. 控制理論與應用，2012，29(6)：741-746.

[9] CHEN C Y, LI T H S, YEH Y C, et al. Design and implementation of an adaptive sliding-mode dynamic controller for wheeled mobile robots[J]. Mechatronics, 2009, 19(2): 156-166.

[10] 白曉平，胡靜濤，高 雷，等. 農(nóng)機導航自校正模型控制方法研究[J]. 農(nóng)業(yè)機械學報，2015，46(2)：1-7.

[11] GHASEMI M, NERSESOV S G, CLAYTON G. Sliding mode coordination control for multiagent systems with under actuated agent dynamics[J]. International Jouranl of Control, 2014, 97(12): 2615-2633.

[12] 焦 俊，汪宏喜，陳祎瓊，等. 基于UKF的農(nóng)用履帶機器人滑動參數(shù)的估計[J]. 農(nóng)業(yè)機械學報，2014，45(4)：55-60.

[13] CHEN X Q, LI J T. Sliding mode control for uncertain unified chaotic systems with nonlinear inputs[J]. Journal of Control Engineering, 2017, 24(1): 239-242.

[14] WANG Z Y, LI Y D, ZHU L. Dual adaptive neural sliding mode control of non-holonomic mobile robot[J]. Journal of Mechanical Engineerin, 2010, 46(23): 16-22.

[15] ZUO Z Y. Non-singular fixed-time terminal sliding model control of nonlinear systems[J]. Control Theory and Applications, 2015, 9(4): 545-552.

[16] XU S D, CHEN C C, WU Z L. Study of nonsingular fast terminal sliding mode fault tolerant control[J]. IEEE Trans on Industrial Electronics, 2015, 62(6): 3906-3913.

[17] MOSTASHFI A, FAKHARI A, MOHAMMAD A B. A novel design of inspection robot for high-voltage power lines[J].Industrial Robot An International Journal,2014,41(2):166-175.

[18] 高為炳. 變結(jié)構(gòu)控制的理論及設(shè)計方法[M]. 北京：科學出版社，1996：34-36.