畜牧養(yǎng)殖環(huán)境監(jiān)測自主移動平臺軌跡跟蹤控制算法

呂恩利，韋鑒峰，王 昱，趙俊宏，王飛仁，劉妍華

?

畜牧養(yǎng)殖環(huán)境監(jiān)測自主移動平臺軌跡跟蹤控制算法

呂恩利1,2，韋鑒峰1，王 昱1,2，趙俊宏1，王飛仁1，劉妍華3※

（1. 華南農(nóng)業(yè)大學(xué)工程學(xué)院，廣州 510642；2. 華南農(nóng)業(yè)大學(xué)南方農(nóng)業(yè)機械與裝備關(guān)鍵技術(shù)教育部重點實驗室，廣州 510642； 3. 華南農(nóng)業(yè)大學(xué)工程基礎(chǔ)教學(xué)與訓(xùn)練中心，廣州 510642）

為實現(xiàn)畜牧養(yǎng)殖環(huán)境全方位監(jiān)測，開發(fā)了可實現(xiàn)軌跡跟蹤的自主移動監(jiān)測平臺。以具有非完整約束特性的自主移動平臺為研究對象，研究其軌跡跟蹤問題。在平臺的結(jié)構(gòu)基礎(chǔ)上，通過建立其運動學(xué)模型及誤差模型，提出基于Lyapunov函數(shù)和反推（Backstepping）時變狀態(tài)反饋控制方法的軌跡跟蹤算法，實現(xiàn)自主移動平臺轉(zhuǎn)向輪轉(zhuǎn)角和行駛速度的控制。仿真和試驗表明：所提出的控制算法能使該平臺較好的收斂于期望軌跡，當跟蹤期望軌跡穩(wěn)定后，控制參數(shù)為(1,2,3,4)=(0.1,0.2,0.15,0.3)時，直線軌跡跟蹤誤差為x=±7 mm，y=±9.8 mm，=±4.2°，圓弧軌跡跟蹤誤差為x=±6.2 mm，y=±8.3 mm，=±5.8°，取得良好的跟蹤精度。該研究可為畜牧養(yǎng)殖環(huán)境監(jiān)測自主移動平臺軌跡跟蹤控制提供參考。

環(huán)境監(jiān)測；算法；模型；自主移動平臺；軌跡跟蹤；運動學(xué)模型；Lyapunov函數(shù)；Beckstepping

0 引 言

準確高效地采集畜牧環(huán)境參數(shù)是反饋控制畜牧環(huán)境的基礎(chǔ)[1]，隨著產(chǎn)業(yè)自動化與智能化的發(fā)展，自動化與智能化裝備代替人工勞動成為產(chǎn)業(yè)重要組成部分。以自主移動平臺（以下簡稱平臺）為載體，集各種傳感器于一身的智能裝備可替代人工勞動，定時對畜牧環(huán)境進行巡檢和環(huán)境監(jiān)測是未來畜牧環(huán)境監(jiān)測的一個重要發(fā)展方向。而以平臺為主的類機器人軌跡跟蹤也成為重要的研究方向。

移動機器人和無人駕駛車輛的軌跡跟蹤問題，在過去數(shù)十年中得到廣泛研究[2]。在國內(nèi)，宋立博等[3-9]通過對移動機器人的運動學(xué)模型進行分析，并提出不同工作環(huán)境和場景下移動機器人的軌跡跟蹤算法，并通過仿真取得較好的效果，但所提出的算法只是理論分析和仿真，沒有通過試驗驗證其算法。李逃昌等[10-11]應(yīng)用純數(shù)學(xué)跟蹤方法對農(nóng)業(yè)移動機器人作業(yè)路徑進行跟蹤試驗，由于定位的難度高導(dǎo)致定位的精度較差，并且存在較大的控制遲滯性，因此跟蹤精度不高。在國外，Oriolo等[12]研究了無障礙環(huán)境下移動機器人的運動控制問題，通過線性化反饋控制雙輪差動移動機器人，相較于具有非完整約束特性的移動機器人而言，具有完整約束特性的雙輪差動移動機器人的可控能力較強，控制更為簡單。Velasco-Villa等[13]針對移動機器人的軌跡跟蹤問題，提出了一種基于室內(nèi)視覺絕對定位系統(tǒng)測量的局部狀態(tài)反饋算法，并在試驗平臺上證明了該算法的準確性，但是該定位方式具有局限性，不能很好的解決光線較暗環(huán)境下移動機器人軌跡跟蹤問題。Del Rio等[14-15]通過建立移動機器人的運動學(xué)和動力學(xué)模型，采用直接控制移動機器人執(zhí)行器輸出的力的大小方法，實現(xiàn)移動機器人速度和轉(zhuǎn)向控制，但在實際應(yīng)用中，難以對移動機器人的力學(xué)模型進行具體量化分析，因此，很難被運用到實際工程中。在應(yīng)用領(lǐng)域，近年來，隨著倉儲物流的快速發(fā)展，國內(nèi)外倉庫內(nèi)使用搬運移動機器人（automated guided vehicle）對物品或者貨架進行搬運成為了主流，但在畜牧養(yǎng)殖環(huán)境監(jiān)測領(lǐng)域暫未發(fā)現(xiàn)有移動機器人的研究和使用。

針對上述問題，本文采用平臺搭載環(huán)境監(jiān)測傳感器方法，定時巡檢和監(jiān)測畜牧環(huán)境信息。所研究的平臺可歸類為非完整約束移動機器人，對具有非完整約束的移動機器人軌跡跟蹤控制的研究，是解決畜牧環(huán)境監(jiān)測平臺軌跡跟蹤控制的重要途徑[16-24]。通過對平臺的運動學(xué)模型和誤差模型分析，提出基于現(xiàn)Lyapunov函數(shù)和反推（Backstepping）時變狀態(tài)反饋軌跡跟蹤控制算法，使平臺按照預(yù)定的軌跡進行運動和巡檢，此期為畜牧環(huán)境監(jiān)測自主移動平臺軌跡跟蹤提供參考。

1 系統(tǒng)結(jié)構(gòu)與模型

1.1 平臺裝置結(jié)構(gòu)

平臺由安全檢測系統(tǒng)、傳感器、電氣設(shè)備、I/O模塊、工控機、電源、上位機等組成。平臺長寬高尺寸分別為0.5 m×0.4 m×0.77 m，質(zhì)量30 kg。平臺由4個輪組成，分別為2個轉(zhuǎn)向輪和2個驅(qū)動輪。軸距為0.35 m。轉(zhuǎn)向輪通過舵機驅(qū)動實現(xiàn)轉(zhuǎn)向，轉(zhuǎn)向角范圍為?0.52～0.52 rad，在實際運動過程中，為保護其轉(zhuǎn)向機械結(jié)構(gòu)，取轉(zhuǎn)向角范圍為?0.49～0.49 rad。驅(qū)動輪采用直流伺服電機驅(qū)動，驅(qū)動速度范圍為0～0.3 m/s。平臺采用激光定位方式，頂部安裝激光定位傳感器NAV350。具體結(jié)構(gòu)如圖1所示。

1.急停開關(guān) 2.狀態(tài)指示燈 3.激光定位掃描儀NAV350 4.激光避障傳感器 5.I/O模塊 6.工控機 7.絕對值式角度編碼器 8.轉(zhuǎn)向輪 9. 24V鉛蓄電池 10.驅(qū)動輪

1.Quick stop switch 2.State indicator lamp 3.Laser positioning scanner NAV350 4.Laser obstacle avoidance sensor 5.I/O module 6.Industrial control machine 7.Absolute value angle encoder 8.Steering wheel 9. 24 V lead-acid battery 10.Drive wheel

a. 自主移動平臺系統(tǒng)實物圖

a. Physical figure of autonomous mobile platform system

1.驅(qū)動輪 2.狀態(tài)指示燈 3.轉(zhuǎn)向輪 4.工控機 5.激光避障傳感器 6.激光定位掃描儀NAV350 7.急停開關(guān)

1.Drive wheel 2.State indicator lamp 3.Steering wheel 4.Industrial control machine 5.Laser obstacle avoidance sensor 6.Laser positioning scanner NAV350 7.Quick stop switch

b. 自主移動平臺結(jié)構(gòu)簡圖

b. Structure diagram of autonomous mobile platform

圖1 自主移動平臺系統(tǒng)結(jié)構(gòu)圖

Fig.1 Autonomous mobile platform system structure diagram

當平臺運行時，狀態(tài)指示燈亮綠燈，NAV350掃描周圍環(huán)境檢測反光標貼，獲取平臺全局坐標實時位姿，角度編碼器和速度傳感器分別實時獲取轉(zhuǎn)向輪轉(zhuǎn)角大小與平臺運動速度大小。傳感器采集的數(shù)據(jù)通過I/O模塊輸入工控機，系統(tǒng)獲取各個傳感器數(shù)據(jù)。

1.2 平臺運動學(xué)模型分析

假設(shè)室內(nèi)路面平坦、運動速度可變，平臺為實心輪胎并無側(cè)偏、無縱側(cè)滑移，根據(jù)運動學(xué)原理，建立如圖2所示的運動學(xué)關(guān)系模型。

注：(X,O,Y)為笛卡爾全局坐標系；(XV,V,YV)為平臺局部坐標系；1,2為轉(zhuǎn)向輪；3,4為驅(qū)動輪；5為等效轉(zhuǎn)向輪；6為等效驅(qū)動輪；L為軸距，m；θ為平臺航向角，rad；ICR為平臺瞬時旋轉(zhuǎn)中心；v為平臺運動速度，m·s-1；γ為轉(zhuǎn)向輪轉(zhuǎn)角，rad；R2為平臺瞬時旋轉(zhuǎn)半徑，m。

根據(jù)建立的平臺運動學(xué)模型圖，可簡化為輪5和輪6組成的二自由度輪式自行車模型，其運動學(xué)方程式為：

式中()為平臺運動學(xué)模型圖2中點坐標，既驅(qū)動輪中心點坐標，，分別為平臺在全局地圖中的橫坐標和縱坐標，m；為平臺運動速度，m/s；為平臺轉(zhuǎn)動角速度，rad/s；為平臺航向角，rad；為平臺等效轉(zhuǎn)向輪轉(zhuǎn)角（簡稱轉(zhuǎn)向角），rad；為平臺驅(qū)動輪和轉(zhuǎn)向輪軸距，m；式（1）表示平臺的位姿=(,,)與運動速度和轉(zhuǎn)向角之間的關(guān)系，根據(jù)其數(shù)學(xué)轉(zhuǎn)化關(guān)系可有

式（2）表示轉(zhuǎn)向角與平臺軸距，平臺轉(zhuǎn)動速度和移動速度之間的關(guān)系，在控制模型中以和為控制輸出量，和的數(shù)值經(jīng)過式（2）轉(zhuǎn)化為平臺轉(zhuǎn)向輪轉(zhuǎn)角和為輸出控制量。對于本平臺，控制其轉(zhuǎn)向輪轉(zhuǎn)角是控制運動方向最直接最簡單的方式。

1.3 平臺誤差模型分析

式（3）為式（1）的矩陣表示形式。

在軌跡跟蹤過程中，平臺能夠沿著期望軌跡運動，需要用到全局坐標中2個點，分別是參考點P(reference point)和當前點P(current point)，其中參考點P=(x,y,θ)T為期望軌跡上的點即目標點位姿，參考點即是目標點，P=(x,y,θ)T是平臺當前位姿點坐標，定義誤差向量P=(x,y,θ)T，P為目標點位姿向量P與當前點位姿向量P的誤差值向量。根據(jù)圖3幾何關(guān)系得

式中x，y為平臺當前點的坐標，m；x，y為目標點的坐標，m；x，y為目標點和當前點的誤差值，m；θ，θ，θ分別為當前點的姿態(tài)角、目標點姿態(tài)角和誤差姿態(tài)角，rad。式（4）用矩陣形式表示可得

式中T為正交矩陣，設(shè)參考點的速度和轉(zhuǎn)速為q=(v,w)，當前平臺的速度和轉(zhuǎn)速為q=(v,w)，并設(shè)定q=(v,w)= (,)=。由式（3）可得

結(jié)合式（6）對式（5）進行時間求導(dǎo)并化簡可得如下微分方程

式中v和v分別為平臺當前點的速度和期望點的速度，m/s；w和w分別為平臺當前點的轉(zhuǎn)速和期望點的轉(zhuǎn)速，rad/s。

注：平臺當前位姿為C(xc,yc,θc)；平臺目標點位姿為r(xr,yr,θr)；誤差量為Pe(xe,ye,θe)。

1.4 軌跡參考點的選擇

參考點P的選擇是建立誤差模型的前提，也是平臺能夠快速收斂于期望軌跡的關(guān)鍵，如圖4所示為本研究平臺參考點的選擇方法。

注：交點r1，r2；半徑R1，R2，m；圓心C1，C2；平臺當前的位姿C1(xc,yc,θc)；航向角誤差值α，rad。

根據(jù)以上描述，可利用如下公式進行計算

式中1為圓1半徑，m；2為圓2半徑，m；x2，y2為切點2坐標，m；x，y是以點2為圓心半徑為2的圓與曲線的交點坐標，m；為平臺的航向角誤差值，rad。

根據(jù)式（8）求出唯一解2，式（9）求出各相交點1和2的坐標，式（10）求得航向角差值，最后判斷參考點，完成參考點P的選擇。

2 控制器的設(shè)計

2.1 平臺軌跡跟蹤控制系統(tǒng)設(shè)計

平臺軌跡跟蹤控制采用分層控制的思想，分為上層控制器設(shè)計和底層執(zhí)行器控制。本文研究上層控制器的設(shè)計，底層控制采用PID控制，平臺軌跡跟蹤控制系統(tǒng)結(jié)構(gòu)如圖5所示。

由圖5可知，系統(tǒng)的輸入為位姿參數(shù)變量P，P和運動參數(shù)變量q，q，輸出為下一刻的線速度v(t+1)和角速度w(t+1)即γ(t+1)。由于平臺在運動過程中不可避免的出現(xiàn)偏移情況，因此平臺的實時位姿不能用模型累積方法計算，模型累積計算得到的位姿會有累積誤差，隨著時間的推移，誤差會增大，最終出現(xiàn)位姿錯誤，所以系統(tǒng)平臺的位姿P是由激光定位傳感器NAV350實時采集。P為平臺參考點。T和P分別是式（5）中的轉(zhuǎn)移矩陣和誤差矩陣。q取值是根據(jù)前視點q所在曲線的曲率半徑和平臺的旋轉(zhuǎn)半徑進行設(shè)定。線速度和角速度數(shù)據(jù)q既是速度輸出反饋量，也是軌跡跟蹤控制器輸入量。由結(jié)構(gòu)圖可知，只要設(shè)計準確的平臺軌跡跟蹤控制器，就能控制平臺轉(zhuǎn)向輪轉(zhuǎn)角和速度，實現(xiàn)平臺位姿的不斷調(diào)整，最終使得系統(tǒng)誤差向量P趨向于0。

注：Pr，Pc為位姿參數(shù)變量，Te，Pe分別為轉(zhuǎn)移矩陣和誤差矩陣，qr，qc為運動參數(shù)變量，vc(t+1)，wc(t+1)分別為線速度和角速度即γc(t+1)。

2.2 平臺軌跡跟蹤控制器設(shè)計

式中3,4為常數(shù)，且v≠0，有如下中間過程式：

將控制律式（14）帶入式（13）中，整理得

3 軌跡跟蹤算法仿真

3.1 仿真環(huán)境和條件

仿真是在Matlab2015a環(huán)境下，應(yīng)用控制律式（14）對平臺的直線和圓周軌跡進行跟蹤驗證，式（14）中參數(shù)1,2,3,4為正的指數(shù)衰減時間常數(shù)，參數(shù)的取值影響平臺軌跡跟蹤收斂速度和穩(wěn)定性。其中參數(shù)1,3調(diào)節(jié)橫向誤差x，2,4調(diào)節(jié)縱向誤差y和位姿誤差θ。

任意一條連續(xù)平滑的路徑軌跡可以看成是由直線和圓弧軌跡擬合而成，用=f(x)+f(x)(,=0…)來表示，為起點到終點的期望軌跡函數(shù)，f(x)表示軌跡各直線函數(shù)段，f(x)表示軌跡各圓弧函數(shù)段。

3.2 直線軌跡跟蹤仿真

在Matlab2015a環(huán)境下，采用編程方式對直線軌跡f(x)進行仿真。所跟蹤的直線參考軌跡(期望軌跡)斜率為1.73(tan(π/3))，平臺在全局坐標初始位姿為(x(0),y(0),θ(0))=(1 m, ?1 m, 4π/9rad)，初始線速度和轉(zhuǎn)向輪轉(zhuǎn)角為(0,0)=(0 m/s, 0 rad)，初始參考點的位姿為(x(0),y(0),θ(0))=(1 m, ?1 m,π/3 rad)，參考點線速度和轉(zhuǎn)向輪轉(zhuǎn)角為(v,γ)=(0.18 m/s,0 rad)，移動機器人軸距=0.35 m，系統(tǒng)參數(shù)(1,2,3,4)=(0.1,0.2,0.15,0.3)，平臺直線軌跡跟蹤仿真效果如圖6所示。

圖6 直線軌跡跟蹤仿真結(jié)果

圖6a表示平臺在控制律式（14）的作用下，平臺能夠平穩(wěn)地跟蹤給定的直線軌跡；圖6b表示隨著平臺不斷收斂于期望直線軌跡過程中，其誤差向量P=(x,y,)與時間的關(guān)系；圖6c表示平臺線速度、轉(zhuǎn)向角與時間的關(guān)系。由圖6中可看，平臺直線軌跡仿真誤差隨時間增長而不斷收斂，從第40秒開始，平臺直線軌跡跟蹤誤差為x=±13.1 mm，y=±7.4 mm，=±0.75°，平臺直線軌跡仿真獲得較好的跟蹤精度。

3.3 圓弧曲線軌跡跟蹤仿真

同樣在Matlab2015a環(huán)境下，對圓弧軌跡f(x)進行仿真，圓弧參考軌跡（期望軌跡）是圓心為(0,0)，半徑為=1 m的圓。平臺全局坐標初始位姿為(x(0),y(0),θ(0))= (1.8 m, -1 m, 2π/3rad)，初始線速度和轉(zhuǎn)向角為(0,0)= (0 m/s, 0 rad)，初始參考點位姿為(x(0),y(0),θ(0))=(1 m, 0 m, π/2 rad)，參考點線速度和轉(zhuǎn)向輪轉(zhuǎn)角為(v,γ)=(0.18 m/s, 0.34 rad)，軸距=0.35 m，系統(tǒng)參數(shù)(1,2,3,4)= (0.1,0.2,0.15,0.3)，得到平臺圓弧軌跡跟蹤仿真效果如圖7所示。

由圖7中可看，平臺圓弧軌跡仿真誤差隨時間增長而不斷收斂，從第5秒開始，平臺圓弧軌跡跟蹤誤差為x=±7.6 mm，y=±9.1 mm，=±0.52°。從圖6和圖7的仿真結(jié)果可以看出，在初始誤差分別為x=10 mm，y=2 016 mm，=?19.9°和x=?80 mm，y=1 020 mm，=28.8°的情況下，根據(jù)控制律式（14）設(shè)計的控制器，能使平臺平穩(wěn)的收斂于期望軌跡，取得良好的跟蹤效果。

圖7 圓弧軌跡跟蹤仿真結(jié)果

4 軌跡跟蹤算法試驗

4.1 試驗材料和環(huán)境

試驗在圖1的平臺上進行，在本試驗中用到的相關(guān)儀器和材料：激光定位傳感器NAV350，是德國Sick公司研發(fā)的一款具有建全局地圖坐標系和定位功能的室內(nèi)傳感器，其定位精度達±4 mm，位姿角度測量范圍是0～360°，數(shù)據(jù)采集頻率為8 Hz；Beckhoff工控機，WIN7系統(tǒng)平臺；I/O模塊端子；24 V蓄電池；速度和角度編碼器，型號同為CAX60-R13/12-E10-LB，分辨率為12bit/4096，重復(fù)精度為±2bit，信號輸出范圍4～20 mA；24 V直流伺服驅(qū)動電機，型號為57B2C1230-SCO，額定功率120 W，額定轉(zhuǎn)速3 000 r/min；轉(zhuǎn)向舵機，型號為LF20MG，旋轉(zhuǎn)角度90°，扭矩2 N·m。

圖8 平臺執(zhí)行器響應(yīng)曲線圖

試驗軟件是在WIN7系統(tǒng)上搭載的Microsoft visual studio/Twincat上進行，Twincat是德國Beckhoff公司開發(fā)的一個集成C/C++，PLC，matlab/simulink 3種可編程、算法設(shè)計和人機交互界面制作平臺，且各編程語言之間數(shù)據(jù)可通過接口實現(xiàn)交換，是一個功能強大的嵌入式開發(fā)平臺。

試驗在圖9中以長和寬分別為11.4和5.6 m的長方形場地中進行，場地中布置有7個直徑均為90 mm的柱形反光標貼。激光定位傳感器NVA350使用的環(huán)境條件是室內(nèi)或者無強自然光影響的室外環(huán)境，因此試驗時要求環(huán)境無太陽光直接照射，圖9a的試驗環(huán)境中無太陽光照射，可模擬畜牧環(huán)境場地，滿足軌跡跟蹤試驗環(huán)境要求。

圖9 試驗場地圖

4.2 平臺軌跡跟蹤試驗

為驗證軌跡跟蹤算法和仿真的準確性，將采用上述設(shè)計的控制器在平臺上進行試驗，試驗分為直線軌跡跟蹤和圓弧軌跡跟蹤2種驗證性試驗，以及單函數(shù)軌跡跟蹤和分段函數(shù)軌跡跟蹤2種應(yīng)用性試驗，驗證性試驗收集其運動軌跡、位姿誤差變化和平臺速度與轉(zhuǎn)向輪轉(zhuǎn)角數(shù)據(jù)，應(yīng)用性試驗收集其運動軌跡跟蹤情況。

1）驗證性試驗。通過驗證性試驗得到的結(jié)果與仿真結(jié)果進行比較，進一步驗證仿真的準確性，先進行直線軌跡跟蹤試驗，試驗平臺初始點為(x(0),y(0),θ(0))= (?4.73 m, ?0.25 m,1.73 rad)，初始線速度和轉(zhuǎn)向輪轉(zhuǎn)角為(0,0)=(0 m/s, 0 rad)，初始參考點的位姿為(x(0),y(0),θ(0))= (2.44 m, 2.44 m, π/4 rad)，參考點線速度和轉(zhuǎn)向輪轉(zhuǎn)角為(v,γ)=(0.18 m/s, 0 rad)。跟蹤的直線軌跡函數(shù)為=。圖10為平臺跟蹤直線軌跡的試驗結(jié)果。

圖10中各圖曲線是數(shù)據(jù)在Matlab中以圖形的形式顯示，其數(shù)據(jù)均來源于Twincat中示波器scope模塊的記錄，數(shù)據(jù)采集頻率為100 Hz，本次試驗總共獲取5 358組數(shù)據(jù)。從圖10b中可以看出，當運行時間大約為25 s時，位姿誤差開始向0收斂，根據(jù)數(shù)據(jù)記錄，從第25秒開始平臺直線軌跡跟蹤誤差為x=±6.2 mm，y=±8.3 mm，=±4.2°，獲得良好的直線軌跡跟蹤精度。

圖10 直線軌跡跟蹤試驗結(jié)果

在進行圓弧軌跡跟蹤試驗時，設(shè)置跟蹤的圓弧軌跡半徑為=1.5 m，圓心為(0,0)。試驗平臺的初始點為(x(0),y(0),θ(0))=(-3.85 m, 0.04 m, -0.35 rad)，初始線速度和轉(zhuǎn)向輪轉(zhuǎn)角為(0,0)=(0.04 m/s, 0.49 rad)，初始參考點的位姿為(x(0),y(0),θ(0))=(-1.49 m, ?0.15 m, ?π/2 rad)，參考點線速度和轉(zhuǎn)向輪轉(zhuǎn)角為(v,γ)=(0.18 m/s,0.24 rad)，系統(tǒng)控制器保持參數(shù)不變。圖11為平臺跟蹤圓弧軌跡的試驗結(jié)果。

圖11 圓弧軌跡跟蹤試驗結(jié)果

圓弧軌跡跟蹤試驗中采集到4501組數(shù)據(jù)，由圖11b中可以看出，從第30秒開始，平臺位姿誤差向0收斂。從第30秒開始，平臺圓軌跡跟蹤誤差為x=±7 mm，y=±9.8 mm，θ=±5.8°，獲得較好的圓弧軌跡跟蹤精度。

2）應(yīng)用性試驗，通過對一般軌跡的跟蹤，觀察其在工程上的應(yīng)用效果。圖12分別是軌跡為正弦函數(shù)=sin()和直線與圓弧組成的分段函數(shù)軌跡，其他系統(tǒng)參數(shù)不變。

圖12 任意函數(shù)軌跡跟蹤試驗結(jié)果

從圖12a中可以看出，平臺在正弦函數(shù)的波峰和波谷時，并不能夠完全沿著期望軌跡進行運動，這是由于平臺本身機械結(jié)構(gòu)即前輪轉(zhuǎn)向轉(zhuǎn)角的極限值限制(?0.49～0.49 rad)。從圖12的軌跡跟蹤試驗得出，在初始誤差分別為x=15 mm，y=385 mm，=?23°和x=?15 mm，y=757 mm，=10°的情況下，平臺在控制器的調(diào)節(jié)下均能較快速地跟蹤期望軌跡，使平臺軌跡跟蹤誤差快速收斂，獲得較好的軌跡跟蹤精度和穩(wěn)定性。

5 結(jié)論與討論

1）開發(fā)了可實現(xiàn)畜牧環(huán)境監(jiān)測的自主移動平臺，提出了一種可實現(xiàn)自主移動平臺軌跡跟蹤的算法，并通過仿真驗證算法的可靠性，試驗驗證了算法的有效性和穩(wěn)定性，并使平臺較快收斂于期望軌跡。能夠滿足工程應(yīng)用的需求。

2）建立了平臺運動學(xué)模型及其誤差模型，提出平臺軌跡參考點的選擇方法，為平臺軌跡跟蹤算法的開發(fā)和期望點的選擇提供了參考。

4）當控制器的系統(tǒng)參數(shù)為(1,2,3,4)= (0.1,0.2,0.15,0.3)時，仿真結(jié)果表明，當平臺軌跡跟蹤穩(wěn)定后其直線跟蹤誤差為x=±13.1 mm，y=±7.4 mm，=±0.75°，圓弧軌跡跟蹤誤差為x=±7.6 mm，y=±9.1 mm，=±0.52°，能夠較好的跟蹤期望軌跡。驗證試驗和應(yīng)用性試驗結(jié)果表明，當系統(tǒng)參數(shù)不變，跟蹤直線軌跡時，跟蹤穩(wěn)定后，其誤差為x=±6.2 mm，y=±8.3 mm，=±4.2°，跟蹤圓弧軌跡時，跟蹤穩(wěn)定后，其誤差值x=±7 mm，y=±9.8 mm，=±5.8°，平臺具有較好的跟蹤穩(wěn)定性和跟蹤效果。該算法為平臺和移動機器人的軌跡跟蹤控制提供了理論依據(jù)，為工程實現(xiàn)提供了參考。

本文是基于平臺為4輪機構(gòu)，轉(zhuǎn)向方式為前輪轉(zhuǎn)向模型的前提下，采用基于Lyapunov函數(shù)和反推法（Backstepping）時變狀態(tài)反饋控制方法的軌跡跟蹤算法，解決畜牧養(yǎng)殖環(huán)境監(jiān)測平臺軌跡跟蹤問題，該平臺與傳統(tǒng)的差速轉(zhuǎn)向平臺或機器人不同的是，它是通過控制方向輪的方向來改變平臺的運動方向，具有非完整約束的特性，在實際工程應(yīng)用中，該控制方式容易使平臺運動出現(xiàn)偏移和車輪打滑現(xiàn)象，因此需要定位精度和實時性較高的定位裝置，才能保證平臺的運動精度。在平臺運動過程中，轉(zhuǎn)向輪存在小角度快速左右轉(zhuǎn)動情況，經(jīng)分析，這與控制器輸出的控制轉(zhuǎn)向輪轉(zhuǎn)角數(shù)據(jù)快速波動有關(guān)。該控制算法較為復(fù)雜，想要平臺獲得較快的系統(tǒng)響應(yīng)，需要性能較高的硬件設(shè)備，才能滿足系統(tǒng)的實時性，成本較高。因此，下一步可通過改變方向控制方式和簡化算法，降低算法的復(fù)雜性對系統(tǒng)硬件的需求，并采用濾波的方式減少輸入和輸出數(shù)據(jù)的波動，解決轉(zhuǎn)向輪轉(zhuǎn)角快速左右轉(zhuǎn)動問題，提高平臺的綜合性能，降低成本。

[1] 陸明洲，沈明霞，丁永前，等. 畜牧信息智能監(jiān)測研究進展[J]. 中國農(nóng)業(yè)科學(xué)，2012，45(14)：2939－2947.

Lu Mingzhou, Shen Mingxia, Ding Yongqian, et al. Review on the intelligent technology for animal husbandry information monitoring[J]. Scientia Agricultura Sinica, 2012, 45(14): 2939－2947. (in Chinese with English abstract)

[2] Tua Agustinus Tamba, Bonghee Hong, Keum-Shik Hong. A path following control of an unmanned autonomous forklift[J]. International Journal of Control, Automation, and Systems, 2009, 7(1): 113－122.

[3] 宋立博，李勁松. 輪式移動機器人嵌入式自適應(yīng)控制器設(shè)計與仿真[J]. 控制理論與應(yīng)用，2012，29(9)：1147－1151.

Song Libo, Li Jinsong. Design and simulation of embedded model-free adaptive control system of wheeled mobile robots[J]. Control Theory & Applications, 2012, 29(9): 1147－1151. (in Chinese with English abstract)

[4] 鄢立夏，馬保離. 輪式移動機器人的位置量測輸出反饋軌跡跟蹤控制[J]. 控制理論與應(yīng)用，2016，33(6)：763－771.

Yan Lixia, Ma Baoli. Out feedback trajectory tracking control of wheeled mobile robots with position measurements[J]. Control Theory & Applications, 2016, 33(6): 763–771. (in Chinese with English abstract)

[5] 呂學(xué)勤，張軻，吳毅雄. 輪式移動焊接機器人輸出反饋線性化控制[J]. 機械工程學(xué)報，2014，50(6)：48－54.

Lü Xueqing, Zhang Ke, Wu Yixiong. Control of thewheeled mobile welding robot based on output feedback linearization[J]. Chinese Journal of Mechanical Engineering, 2014, 50(6): 48－54. (in Chinese with English abstract)

[6] 和永智，劉偉軍，周船，等. 輪式移動機器人與地形交互運動仿真研究[J]. 機器人，2007，29(5)：498－504.

He Yongzhi, Liu Weijun, Zhou Chuan, et al. Simulation of mutual motion between wheeled mobile robotsand terrains[J]. Robot, 2007, 29(5): 498－504. (in Chinese with English abstract)

[7] 王仲民，岳宏，劉繼巖. 輪式移動機器人軌跡跟蹤控制[J]. 太原理工大學(xué)學(xué)報，2005，36(4)：474－476.

Wang Zhongmin, Yue Hong, Liu Jiyan. Trajectory tracking control on wheeled mobile robot[J]. J of Tai yuan Univ of Tech, 2005, 36(4): 474－476. (in Chinese with English abstract)

[8] 龐海龍，馬保離. 不確定輪式移動機器人的任意軌跡跟蹤[J]. 控制理論與應(yīng)用，2014，31(3)：285－292.

Pang Hailong, Ma Baoli. Adaptive unified controller of arbitrary trajectory tracking for wheeled mobilerobots with unknown parameters[J]. Control Theory & Applications, 2014, 31(3): 285－292. (in Chinese with English abstract)

[9] 吳衛(wèi)國，陳輝堂，王月娟. 移動機器人的全局軌跡跟蹤控制[J]. 自動化學(xué)報，2001，27(3)：326－331.

Wu Weiguo, Chen Huitang, Wang Yuejuan. Global trajectoty tracking control of mobile robots[J]. Acta Automatica Sinica, 2001, 27(3): 326－331. (in Chinese with English abstract)

[10] 李逃昌，胡靜濤，高雷，等. 基于模糊自適應(yīng)純追蹤模型的農(nóng)業(yè)機械路徑跟蹤方法[J]. 農(nóng)業(yè)機械學(xué)報，2013，44(1)：205－210.

Li Taochang, Hu Jingtao, Gao Lei. Agricultural machine path tracking method based on fuzzy adaptive pure pursuit model[J]. Transactions of the Chinese Society for Agricultural Machinery, 2013, 44(1): 205－210. (in Chinese with English abstract)

[11] 張智剛，羅錫文，趙祚喜. 基于Kalman 濾波和純追蹤模型的農(nóng)業(yè)機械導(dǎo)航控制[J]. 農(nóng)業(yè)機械學(xué)報，2009，40(增刊)：6－12.

Zhang Zhigang, Luo Xiwen, Zhao Zuoxi, et al. Trajectory tracking control method based on Kalman filter and pure pursuit model for agricultural vehicle[J]. Transactions of the Chinese Society for Agricultural Machinery, 2009, 40(Supp.): 6－12. (in Chinese with English abstract)

[12] Oriolo G, De Luca A, Vendittelli M. WMR control via dynamic feedback linearization: Design, implementation, and experimental validation[J]. IEEE Transactions on Control Systems Technology, 2002, 10(6): 835－852.

[13] Velasco-Villa M, Aranda-Bricaire E, Rodrigu-Ezcortesh, et al. Trajectory tracking for a wheeled mobile robot using a vision based positioning system and an attitude observer[J]. European Journal of Control, 2012, 18(4): 348－355.

[14] Del Rio F D, Jim Enez G, Sevillano J L, et al. Error adaptive tracking for mobile robots[C]//The 28th IEEE Annual Conference of the Industrial Electronics Society, Sevilla, Spain: IEEE, 2002, 3: 2415－2420.

[15] Jiang Z P, Henk N R. Tracking control of mobile robots: a case study in backstepping[J]. Automatica, 1997, 33(7): 1393－1399.

[16] Fierro R, Lewis F L. Control of a nonholonomic mobile robot: backstepping kinematics into dynamics[C]//IEEE Proceedings of 34th Conference on Decision and Control, 1995: 3805－3810.

[17] 徐俊艷，張培仁. 非完整輪式移動機器人軌跡跟蹤控制研究[J]. 中國科學(xué)技術(shù)大學(xué)學(xué)報，2004，34(3)：376－380.

Xu Junyan, Zhang Peiren. Research on trajectory tracking control of nonholonomic wheeled mobile robots[J]. J of Univ of Sci and Tech of China, 2004, 34(3): 376－380. (in Chinese with English abstract)

[18] 劉磊，向平，王永驥，等. 非完整約束下的輪式移動機器人軌跡跟蹤[J]. 清華大學(xué)學(xué)報，2007，47(S2)：1884－1889.

Liu Lei, Xiang Ping, Wang Yongji, et al. Trajectory tracking of a nonholonomic wheeled mobile robot[J]. Journal of Tsinghua University, 2007, 47(S2): 1884－1889. (in Chinese with English abstract)

[19] Spandan Roy, Sanbhunath Nandy, Ranjit Ray, et al. Robust path tracking control of nonholonomic wheeled mobile robot: experimental validation[J]. International Journal of Control, Automation and Systems, 2015, 13(4): 897－905.

[20] Morin P, Samson C. Control of nonholonomic mobilerobots based on the transverse function approach[J]. IEEE Transactions on Robotics, 2009, 25(5): 1058－1073.

[21] Ailon A, Zohar I. Control strategies for driving a group of nonholonomic kinematical mobile robots in formation along a time parameterized path[J]. IEEE/ASME Transaction on Mecha-tronics, 2012, 17(2): 326－336.

[22] Fierro R, Lewis F L. Control of a nonholonomic mobile robot: Backstepping kinematics into dynamics[C]// Procee-dings of the IEEE Conference on Decision and Control, New Orleans, LA, USA, 1995: 3805－3810.

[23] Azza El-sayed Bayoumo Ibrahim. Wheeled mobile robot trajectory tracking using sliding mode control[J]. Journal of Computer Sciences, 2016, 12(1): 48－55.

[24] Kanayama Yutaka. A stable tracking control method for a non-holonomic mobile robot[J]. IEEE/RSJ International Workshop on Intelligence Robots and Systems, 1991, 91: 3－5.

[25] 閔穎穎，劉允剛. Barbalat引理在系統(tǒng)穩(wěn)定性分析中的應(yīng)用[J]. 山東大學(xué)學(xué)報：工學(xué)版，2007，37(1)：51－56.

Min Yingying, Liu Yungang. Barbalat lamma and its application in analysis of system stability[J]. Journal of Shandong University : Engineering Science, 2007, 37(1): 51－56. (in Chinese with English abstract)

[26] Jiang Z P, Nijmeijer H. Tracking control of mobile robots: A case study in backstepping[J]. Automatica, 1997, 33(7): 1393－1399.

[27] 李世華，田玉平. 移動小車的有限時間軌跡跟蹤控制[J]. 東南大學(xué)學(xué)報：自然科學(xué)報，2004，34(1)：113－116.

Li Shihua, Tian Yuping. Trajectory tracking control of mobile robots in finite time[J]. Journal of Southeast University: Natural Science Edition, 2004, 34(1): 113－116. (in Chinese with English abstract)

[28] 朱奇光，王佳，張朋珍，等. 基于高斯矩改進SURF算法的移動機器人定位研究[J]. 儀器儀表學(xué)報，2015，36(11)：2451－2457.

Zhu Qiguang, Wang Jia, Zhang Pengzhen, et al. Research on mobile robot localization based on gaussian moment improved SURF algorithm[J]. Chinese Journal of Scientific Instrument, 2015, 36(11): 2451－2457. (in Chinese with English abstract)

Trajectory tracking algorithm of autonomous mobile platform for animal husbandry environment information monitoring

Lü Enli1,2, Wei Jianfeng1, Wang Yu1,2, Zhao Junhong1, Wang Feiren1, Liu Yanhua3※

(1.510642,; 2.510642,; 3.,510642,)

environmental monitoring; algorithms; models; autonomous mobile platform; trajectory tracking; kinematic model; lyapunov function; beckstepping

呂恩利，韋鑒峰，王 昱，趙俊宏，王飛仁，劉妍華. 畜牧養(yǎng)殖環(huán)境監(jiān)測自主移動平臺軌跡跟蹤控制算法[J]. 農(nóng)業(yè)工程學(xué)報，2018，34(13)：86－94. doi：10.11975/j.issn.1002-6819.2018.13.011 http://www.tcsae.org

Lü Enli, Wei Jianfeng, Wang Yu, Zhao Junhong, Wang Feiren, Liu Yanhua. Trajectory tracking algorithm of autonomous mobile platform for animal husbandry environment information monitoring[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2018, 34(13): 86－94. (in Chinese with English abstract) doi：10.11975/j.issn.1002-6819.2018.13.011 http://www.tcsae.org

2018-01-17

2018-05-10

國家科技支撐計劃課題（2015BAD18B0303）；現(xiàn)代農(nóng)業(yè)產(chǎn)業(yè)技術(shù)體系建設(shè)專項資金（CARS-33-13）；國家自然科學(xué)基金項目（51108194）

呂恩利，副教授，博士生導(dǎo)師，主要研究方向為農(nóng)業(yè)工程、農(nóng)產(chǎn)品冷鏈物流裝備、智能倉儲運輸裝備控制研究。 Email：enlilv@scau.edu.cn

劉妍華，博士，副教授，主要研究方向為農(nóng)產(chǎn)品冷鏈物流和農(nóng)業(yè)設(shè)施節(jié)能設(shè)計。Email：cynthial@scau.edu.cn

10.11975/j.issn.1002-6819.2018.13.011

TP23

A

1002-6819(2018)-13-0086-09