冀 杰 賀 慶 趙立軍,2 李 平 劉 洋 王小康

(1.西南大學工程技術學院, 重慶 404100; 2.重慶文理學院智能制造工程學院, 重慶 402160;3.重慶市農業(yè)科學院農業(yè)機械研究所, 重慶 404100)

0 引言

由于電動智能除草機器人在節(jié)省勞動力和環(huán)境保護方面優(yōu)勢明顯,它已成為綠色智能農業(yè)機械發(fā)展的重要體現(xiàn),受到越來越多研究機構和科研人員的重視[1]。路徑跟蹤控制是智能農機的關鍵技術之一,對于提高除草機器人作業(yè)效率和作業(yè)質量起到?jīng)Q定性的作用。而除草機器人在作業(yè)時往往受到系統(tǒng)內部參數(shù)攝動、未建模誤差以及未知外部干擾等不確定因素的影響,難以快速準確地跟蹤期望路徑。因此,有必要為除草機器人設計一種響應快速、精度高、魯棒性強并具有自適應能力的路徑跟蹤控制器。

近年來,各國學者設計了不同的控制器用于解決路徑跟蹤問題,例如自適應控制[2-4]、滑?？刂芠5-7]、神經(jīng)網(wǎng)絡控制[8-9]、H∞控制[10]等。其中,滑模控制具有快速響應、魯棒性強等優(yōu)點,被越來越多地應用到智能農機、智能車輛的路徑跟蹤中。針對非完整系統(tǒng)的動力學模型,文獻[11]提出了基于反演技術的自適應滑?？刂?該控制器能夠使系統(tǒng)具有全局穩(wěn)定性和較強的魯棒性, 但需要準確獲得集中擾動的上界信息,這在實際應用中難以實現(xiàn)。為此,文獻[12]在跟蹤控制器中增加了干擾觀測器和自適應補償器,用于處理系統(tǒng)不確定性和外界干擾。而在文獻[13]設計的自適應滑模動態(tài)控制器中,引入了補償外部擾動和動態(tài)不確定性的估計項,以及改善閉環(huán)穩(wěn)定性、補償外部擾動估計誤差的反饋項。近年來,為了獲得更快的響應速度,終端滑?？刂浦饾u被用于機器人系統(tǒng)中,它借助非線性超平面使跟蹤誤差在有限時間內收斂到零。文獻[14]提出一種非奇異終端滑模(NTSM)控制,解決了普通終端滑模的奇異性問題。但控制器中的不連續(xù)到達控制會導致嚴重抖振。為克服控制信號的抖動,二階滑模[15]、分數(shù)階滑模[16]、遞歸滑模[17]等策略也被不斷探索和應用。

由于除草機器人的動力學模型具有強非線性、使用環(huán)境具有強干擾的特點,為了有針對性地解決軌跡跟蹤中的響應性、跟蹤性及魯棒性問題,基于上述分析,本文設計一種自適應快速積分終端滑?？刂破?。首先,考慮驅動輪動力學特性以及未建模誤差、外界干擾、動靜摩擦等不確定性因素,建立除草機器人的動力學模型。然后,基于所建立的動力學模型,設計帶積分元素的快速終端滑?？刂破?對滑模函數(shù)的控制參數(shù)進行自適應調整,從而適度減弱抖振,并且,通過嵌入自適應估計律,在不明確系統(tǒng)不確定性和干擾的情況下實現(xiàn)前饋補償。最后,通過仿真對比和實地試驗來驗證所設計控制器在快速響應、高精度和強魯棒性方面的性能。

1 除草機器人動力學建模

圖1是自主設計的除草機器人模型俯視圖,運動系統(tǒng)由兩個驅動后輪和兩個萬向前輪組成,利用兩后輪差速驅動實現(xiàn)除草機器人的運動控制。圖中OXY表示全局坐標系,Gxy表示除草機器人局部坐標系。點G表示兩后輪軸中心點位置,點Z為除草機器人質心位置。質心點Z的位姿q(xZ,yZ,θ)表示除草機器人在全局坐標系下的位姿。2a為兩后輪中心之間的距離,b為點Z與點G之間的距離,兩后輪半徑為r。

圖1 除草機器人模型俯視圖Fig.1 Top view of weeding robot model

除草機器人的運動學模型為

(1)

式中v——除草機器人點G線速度,m/s

w——除草機器人點G角速度,rad/s

B——運動學變換矩陣

u——速度向量

除草機器人為典型的非完整約束、非線性動力學系統(tǒng),它在運動過程中始終滿足約束方程

(2)

本文所設計的除草機器人采用機械鏈條傳動,并假定驅動輪始終做無滑動的純滾動運動,通過對兩驅動輪進行動力學分析,得到

(3)

式中Iw——左、右后輪的轉動慣量,kg·m2

τr、τl——左、右電機的驅動力矩,N·m

Fr、Fl——左、右輪的驅動力,N

μw——電機與傳動機構之間的粘性摩擦因數(shù)

針對質心點Z,建立除草機器人本體的牛頓歐拉方程,得到動力學方程

(4)

IZ——割草機質心的轉動慣量,kg·m2

λ——拉格朗日乘子

m——除草機器人質量

對運動學方程(1)、約束方程(2)、驅動輪動力學方程(3)以及除草機器人動力學方程(4)進行聯(lián)立求解,得到除草機器人的完整動力學模型

(5)

其中

(6)

(7)

(8)

(9)

(10)

式中M(q)——慣性力矩陣

A(q)——非完整約束矩陣

S(q)——輸入變換矩陣

τ——驅動力矩向量

式(5)兩邊同時乘以BT(q),消除其中的約束項A(q)λ,并代入式(1),則受非完整約束的除草機器人動力學方程可簡化為

(11)

其中

2 除草機器人控制系統(tǒng)設計

2.1 控制系統(tǒng)結構

除草機器人的整體控制系統(tǒng)如圖2所示,由基于運動學模型的速度控制器和基于動力學模型的軌跡跟蹤滑?？刂破鞴餐M成。根據(jù)除草機器人實際位姿q=(xZ,yZ,θZ)T與參考位姿qr=(xr,yr,θr)T產生虛擬的期望速度ud=(vd,wd)T,其中,參數(shù)觀測器用于估計除草機器人質心點Z與兩后輪中心點G的距離b。然后,利用設計的滑?？刂破鳟a生控制轉矩,從而使除草機器人的實際速度u能夠跟蹤虛擬速度ud,同時,利用自適應估計律補償外界干擾和系統(tǒng)不確定性。

圖2 控制系統(tǒng)結構框圖Fig.2 Block diagram of control system

2.2 速度控制器設計

2.2.1速度控制律推導

如前所述,速度控制器的目標是生成虛擬期望速度ud,從而使除草機器人的實際位姿與參考位姿的跟蹤誤差收斂到零。根據(jù)文獻[18]定義除草機器人的軌跡跟蹤誤差qe為

(12)

對式(12)進行求導得到

(13)

為了有效消除機器人的軌跡跟蹤誤差,定義除草機器人軌跡跟蹤誤差qe的李雅普諾夫函數(shù)為

(14)

kθ——正常數(shù)

對式(14)進行求導得出

(15)

(16)

式中kx(vr,wr)、ky(vr,wr)——與角速度和線速度參考值相關的有界非線性連續(xù)函數(shù)

運動學模型參數(shù)b的自適應律表示為

(17)

2.2.2穩(wěn)定性分析

速度控制器是基于李雅普諾夫函數(shù)而設計的,需要引入以下假設和Barbalat引理以及推論來作為證明控制器穩(wěn)定性的前提。

假設:參考線速度和參考角速度以及它們的導數(shù)都是有界的。在任意時間內滿足

(18)

將式(16)、(17)代入式(15)得到

(19)

(20)

(21)

對式(21)求導可得

(22)

2.3 滑?？刂破髟O計

2.3.1滑?？刂坡赏茖?/p>

在不確定性或外部干擾的情況下,滑?？刂葡到y(tǒng)的輸出u=(v,w)T要能夠有效跟蹤虛擬速度ud=(vd,wd)T。為了實現(xiàn)這一目標,需要進行以下合理假設:不確定性和外界干擾是有界的,并且滿足關系[20]

(23)

式中η1、η2、η3——未知有界的正數(shù)

在不考慮干擾和不確定性的情況下,式(11)變?yōu)?/p>

(24)

速度跟蹤誤差表示為

(25)

為了在系統(tǒng)不確定性和外界干擾的影響下,消除穩(wěn)態(tài)誤差,并在有限時間內迅速收斂到平衡狀態(tài),滑模面引入積分元素和非線性函數(shù)sigφ(x),即

(26)

式中α1、α2——滑模函數(shù)參數(shù),α1>0、α2>0

φ——非線性函數(shù)參數(shù),0<φ<1

對于β>0,?x∈R,sigφ(x)是一個單調遞增的平滑函數(shù)[21],定義為

sigφ(x)=|x|φsgn(x)

(27)

式中 sgn(x)——符號函數(shù)

(28)

所以等效控制律為

(29)

在考慮干擾和不確定性的情況下,為保證系統(tǒng)的狀態(tài)不離開滑模面,設計切換控制律

(30)

式中ξ1、ξ2——控制參數(shù),ξ1>0、ξ2>0

δ——控制參數(shù),δ>1

(31)

式中σ1、σ2、σ3——自適應律增益參數(shù),σ1>0、σ2>0、σ3>0

將式(29)與式(30)相加即可得到滑?？刂坡?即

(32)

2.3.2穩(wěn)定性分析

為分析滑模控制器的穩(wěn)定性,定義李雅普諾夫函數(shù)

(33)

對式(33)微分得

(34)

由式(13)、(28)、(32)得到

(35)

假設s(t=0)≠0,由式(31)可知1、2、3是單調遞增的,所以存在一個有限的時間T1使得

(36)

(37)

其中

Ψ=min(Γs,Γ1,Γ2,Γ3)

(38)

假設V2(0)≠0到V2(tr)=0的時間為tr。由式(37)可得

(39)

所以,在任意初始條件V2(0)下,存在有限的時間tr使得V2→0,即s→0。當系統(tǒng)處于滑動模態(tài)時(s=0)有

(40)

根據(jù)式(40)可得

(41)

設初始時刻的誤差為eu(0),求解微分方程式(41)得到

(42)

設t=ts時,eu=0,則由式(42)可求解出ts為

(43)

因此,當式(26)中的s為0時,即系統(tǒng)處于滑動模態(tài)時,在任何初始條件eu(0)下,誤差eu將在有限的時間ts內收斂到0。

2.4 控制器參數(shù)優(yōu)化

在實際應用中,由于噪聲和切換延遲,滑模變量s可能存在抖振的情況,i會一直增加并造成控制輸入過大。因此,本研究通過使用死區(qū)的方法來緩解這個問題[22]。式(31)變?yōu)?/p>

(44)

式中ρ——較小的正常數(shù)

(45)

式中ι——邊界層,ι>0

減小式(26)中的參數(shù)φ,可提高系統(tǒng)響應速度,但會增大系統(tǒng)抖振,甚至使系統(tǒng)發(fā)散。所以,理想的φ應該是時變的,即隨著誤差的減少而增大。因此,設計φ與eu的經(jīng)驗函數(shù)

φ=e-(λ|eu|+0.5)γ+0.2

(46)

式中λ、γ——正常數(shù)

3 仿真分析

為測試控制算法的有效性,在Matlab軟件中對其進行仿真驗證,并與文獻[13]所提出的自適應滑模動態(tài)控制器(Adaptive sliding mode dynamic controller, ASMDC)、文獻[24]提出的雙環(huán)姿態(tài)跟蹤魯棒控制器(Double ring attitude tracking robust controller, DRATRC)、文獻[11]提出的自適應滑模控制器(Adaptive sliding mode controller, ASMC)進行對比。仿真分析采用的除草機器人物理參數(shù)以及控制器仿真參數(shù)如表1、2所示,除草機器人的初始位姿為:xZ=-1 m,yZ=-1 m,θZ=0 rad。

表1 除草機器人參數(shù)Tab.1 Parameters of weeding robot

表2 控制器仿真參數(shù)Tab.2 Simulation parameters of controller

除草機器人在縱向、橫向和航向上受到系統(tǒng)不確定性和外界干擾造成的合力設置為

(47)

仿真結果如圖3所示。從圖3a可以看出,當除草機器人的初始位置與期望路徑存在一定的差距時,4種算法均能使機器人較為快速地逼近期望路徑,并在隨后的運動中實現(xiàn)較好的軌跡跟蹤效果。圖3b顯示了除草機器人在橫向、縱向和航向角上的跟蹤誤差,比較本文所設計的自適應快速積分終端滑?？刂破?AFITSMC)與另外3種控制器的跟蹤誤差可以看出,4種控制器的控制效果存在一定的差異。4種控制器在3個方向上的初始誤差分別是1 m、1 m、0 rad,采用ASMDC、DRATRC和ASMC控制器時,除草機器人的3種誤差約13 s之后會漸近逼近零,振蕩收斂。位置穩(wěn)態(tài)誤差在-0.05～0.05 m之間波動,而航向角的穩(wěn)態(tài)誤差在-0.1～0.1 rad之間波動。由于AFITSMC算法在滑模函數(shù)中引入了積分元素和非線性函數(shù),所以,橫向、縱向和航向角上的誤差在有限時間約3 s內快速收斂到零,隨后,穩(wěn)態(tài)誤差也收斂于零。可見,AFITSMC控制器的快速性和精確性優(yōu)于另外3種控制器。并且,由圖3c可以看出,自適應估計增益能夠在有限時間內收斂到穩(wěn)定狀態(tài)。圖3d顯示了4種控制器的線速度與角速度的跟蹤曲線,與另外3種控制器相比,本文提出的AFITSMC控制器能夠使機器人迅速達到期望的線速度和角速度,并最終穩(wěn)定收斂到期望值。但是,由于AFITSMC控制器的快速跟蹤特性,線速度與角速度會在起始階段存在短時間的速度突變。從圖3e可以看出,在模型不確定性和外界干擾的影響下,采用ASMDC控制器和ASMC控制器時,兩驅動電機的輸入力矩在短時間內會有較大的波動,容易造成除草機器人抖振,不利于除草機器人作業(yè)。而采用DRATRC控制器和AFITSMC控制器時,輸入力矩曲線較為平滑,有效降低了系統(tǒng)的抖振。另外,除草機器人的輸入力矩在初始時有比較明顯跳變,這是由于除草機器人的起始位置與參考軌跡的起始位置有比較大的誤差,需要較大的控制力矩使得系統(tǒng)迅速達到平衡狀態(tài)。因此,在實際軌跡跟蹤時,可以適當減小起始誤差和相應的控制增益,以減小在啟動階段的力矩跳變。

圖3 除草機器人仿真結果Fig.3 Simulation results of weeding robot

綜上可知,在系統(tǒng)不確定性和外界干擾的影響下,本文所設計的AFITSMC控制器與ASMDC控制器、DRATRC控制器、ASMC控制器相比,具備一定的抗抖振能力和魯棒性,并在有限時間內能使跟蹤誤差快速收斂到零,具有更好的跟蹤性能。

4 試驗驗證

4.1 試驗平臺

為進一步驗證提出的控制算法的有效性,在如圖4所示的自主設計的除草機器人平臺上進行試驗驗證,具體結構參數(shù)見表1。除草機器人平臺上配備了英偉達Jetson Agx Xavier控制器、Velodyne 16線激光雷達、慣性測量單元(IMU)、額定功率2 000 W和額定轉速100 r/min的科亞直流伺服電機及其驅動器、48 V蓄電池等硬件。主機的操作環(huán)境為Ubuntu 18.04系統(tǒng),機器人操作系統(tǒng)為ROS 1,通過C++語言在ROS 1上編寫本文控制算法節(jié)點、路徑規(guī)劃節(jié)點和SLAM節(jié)點來實現(xiàn)除草機器人的跟蹤控制?；诩す饫走_和IMU傳感器的融合數(shù)據(jù),SLAM節(jié)點實時發(fā)布除草機器人的位姿信息、線速度及角速度信息到控制算法節(jié)點,本文采用文獻[25]提出的 LeGO-LOAM算法作為機器人的SLAM算法?？刂扑惴ü?jié)點通過從路徑規(guī)劃節(jié)點獲取參考路徑信息。最后,通過科亞直流伺服電機驅動器的串口協(xié)議,向左、右后輪驅動電機發(fā)送控制力矩指令,最終實現(xiàn)除草機器人的軌跡跟蹤控制。

圖4 除草機器人平臺Fig.4 Weeding robot platform1.慣性測量單元 2.割刀電機驅動器 3.后輪電機驅動器 4.電源 5.Velodyne 16激光雷達 6.Jetson Agx Xavier控制器 7.左右后輪驅動電機

4.2 除草機器人作業(yè)試驗

試驗場地選擇重慶市農業(yè)科學院的閑置地塊,該地塊起伏不平且雜草的平均高度約為32 cm,除草機器人在作業(yè)行駛時會受到隨機干擾,試驗現(xiàn)場如圖5所示。除草機器人在草坪上的作業(yè)路徑主要包括直線行駛以及掉頭轉向,因此,以S形路徑作為目標軌跡進行跟蹤測試。根據(jù)除草機器人結構尺寸和試驗場地環(huán)境,實際目標路徑如圖6a中黑色曲線所示。由于使用激光雷達和IMU傳感器獲取除草機器人的里程計信息,因此,除草機器人在初始時刻的位姿為(0 m,0 m,0 rad);而規(guī)劃路徑的起始點位姿為(1 m,1 m,0 rad),初始時刻的橫向誤差、縱向誤差、航向角誤差分別為1 m、1 m、0 rad。

圖5 試驗場景Fig.5 Scenario for experiment

圖6 試驗結果Fig.6 Experimental results

從圖6a、6b可以看出:在初始時刻,除草機器人的橫向偏差和縱向偏差大約在5 s內都能迅速收斂到零附近,證明提出的控制算法能夠保證除草機器人的跟蹤誤差在有限時間內迅速收斂。而在穩(wěn)態(tài)軌跡跟蹤過程中,除草機器人的橫向誤差、縱向誤差和航向角誤差的統(tǒng)計如表3所示,橫向、縱向、航向角誤差總體最大絕對值分別為0.097 9 m、0.102 6 m、0.057 8 rad,而最小絕對值為0.000 1 m、0.000 1 m、0.000 1 rad,平均絕對值分別為0.015 6 m、0.015 5 m、0.008 6 rad,均方差分別為0.019 5 m、0.019 7 m、0.011 1 rad。數(shù)據(jù)表明:即使存在模型不確定和外界干擾的情況下,本文提出的控制算法能夠使除草機器人較為快速且平滑地跟蹤目標軌跡,將誤差控制在較小范圍內,有效保證了跟蹤效果。從圖6c所示的橫向、縱向、航向角誤差的概率分布圖也能看出,3個方向上的誤差近似服從以零為均值的正態(tài)分布,誤差大概率集中在-0.05～0.05區(qū)間內,波動幅度較小。對除草機器人的感知及執(zhí)行機構進行分析,得出造成機器人在3個方向上產生誤差波動的2個主要原因:激光雷達和IMU傳感器存在一定噪聲且精度不足;執(zhí)行電機控制存在時間滯后,導致除草機器人在獲取定位時的位姿信息和速度信息不夠精確,且力矩執(zhí)行不夠及時。因此,與主機發(fā)出的規(guī)劃、定位、控制信號相比,執(zhí)行機構存在不可避免的延遲和誤差。從圖6d可以看出,線速度和角速度也能實現(xiàn)較好的跟蹤效果。在初始時刻,由于除草機器人位姿與參考位姿有較大的誤差,為了迅速使誤差得到收斂,機器人需要產生較大的瞬時線速度和角速度,從而很快收斂到期望速度。在11、19、29、36 s時刻,由于曲率的突變,跟蹤的線速度與角速度會產生一個較小的突變,這是由于路徑規(guī)劃時速度不平滑導致的。在其他時間內,除草機器人的線速度和角速度無明顯抖動,表明本文提出的控制算法能夠有效抑制除草機器人在軌跡跟蹤過程中的抖振。

表3 跟蹤誤差統(tǒng)計結果Tab.3 Statistical results of tracking errors

從軌跡跟蹤試驗結果可知,設計的控制器在控制精度和穩(wěn)定性方面都能滿足除草機器人的作業(yè)要求,并且,系統(tǒng)在受到參數(shù)攝動和外界干擾時,控制器能有效抑制干擾造成的抖振,具有較強的魯棒性和快速收斂性。

5 結論

(1)針對除草機器人易受到外部干擾和自身不確定性的特點,建立了考慮驅動輪動力學特性以及未建模誤差、外界干擾、摩擦變化等不確定性因素的除草機器人動力學模型。

(2)基于自適應控制和終端滑?？刂圃O計了除草機器人的軌跡跟蹤控制方法,能夠保證除草機器人在有限時間內迅速跟蹤期望軌跡。此外,所提出的自適應律可以在線估計未知擾動,從而提高了擾動抑制性能,顯著緩和了控制抖振。

(3)提出的方法能夠使姿態(tài)跟蹤誤差在短時間內收斂到零,并且控制器參數(shù)不需要明確的擾動上界,實際應用中具有普適性。

(4)仿真與試驗結果表明,所設計的跟蹤控制算法能夠使除草機器人在真實作業(yè)環(huán)境中具有高跟蹤精度、快速響應和很強的抗干擾性能。