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

?

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

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

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

為實現(xiàn)畜牧養(yǎng)殖環(huán)境全方位監(jiān)測，開發(fā)了可實現(xiàn)軌跡跟蹤的自主移動監(jiān)測平臺。以具有非完整約束特性的自主移動平臺為研究對象，研究其軌跡跟蹤問題。在平臺的結構基礎上，通過建立其運動學模型及誤差模型，提出基于Lyapunov函數(shù)和反推（Backstepping）時變狀態(tài)反饋控制方法的軌跡跟蹤算法，實現(xià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)測；算法；模型；自主移動平臺；軌跡跟蹤；運動學模型；Lyapunov函數(shù)；Beckstepping

0 引 言

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

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

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

1 系統(tǒng)結構與模型

1.1 平臺裝置結構

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

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

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.驅動輪 2.狀態(tài)指示燈 3.轉向輪 4.工控機 5.激光避障傳感器 6.激光定位掃描儀NAV350 7.急停開關

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. 自主移動平臺結構簡圖

b. Structure diagram of autonomous mobile platform

圖1 自主移動平臺系統(tǒng)結構圖

Fig.1 Autonomous mobile platform system structure diagram

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

1.2 平臺運動學模型分析

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

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

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

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

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

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幾何關系得

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

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

結合式（6）對式（5）進行時間求導并化簡可得如下微分方程

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

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

1.4 軌跡參考點的選擇

參考點P的選擇是建立誤差模型的前提，也是平臺能夠快速收斂于期望軌跡的關鍵，如圖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 控制器的設計

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

平臺軌跡跟蹤控制采用分層控制的思想，分為上層控制器設計和底層執(zhí)行器控制。本文研究上層控制器的設計，底層控制采用PID控制，平臺軌跡跟蹤控制系統(tǒng)結構如圖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）中的轉移矩陣和誤差矩陣。q取值是根據(jù)前視點q所在曲線的曲率半徑和平臺的旋轉半徑進行設定。線速度和角速度數(shù)據(jù)q既是速度輸出反饋量，也是軌跡跟蹤控制器輸入量。由結構圖可知，只要設計準確的平臺軌跡跟蹤控制器，就能控制平臺轉向輪轉角和速度，實現(xiàn)平臺位姿的不斷調整，最終使得系統(tǒng)誤差向量P趨向于0。

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

2.2 平臺軌跡跟蹤控制器設計

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

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

3 軌跡跟蹤算法仿真

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

仿真是在Matlab2015a環(huán)境下，應用控制律式（14）對平臺的直線和圓周軌跡進行跟蹤驗證，式（14）中參數(shù)1,2,3,4為正的指數(shù)衰減時間常數(shù)，參數(shù)的取值影響平臺軌跡跟蹤收斂速度和穩(wěn)定性。其中參數(shù)1,3調節(jié)橫向誤差x，2,4調節(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)，初始線速度和轉向輪轉角為(0,0)=(0 m/s, 0 rad)，初始參考點的位姿為(x(0),y(0),θ(0))=(1 m, ?1 m,π/3 rad)，參考點線速度和轉向輪轉角為(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 直線軌跡跟蹤仿真結果

圖6a表示平臺在控制律式（14）的作用下，平臺能夠平穩(wěn)地跟蹤給定的直線軌跡；圖6b表示隨著平臺不斷收斂于期望直線軌跡過程中，其誤差向量P=(x,y,)與時間的關系；圖6c表示平臺線速度、轉向角與時間的關系。由圖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)，初始線速度和轉向角為(0,0)= (0 m/s, 0 rad)，初始參考點位姿為(x(0),y(0),θ(0))=(1 m, 0 m, π/2 rad)，參考點線速度和轉向輪轉角為(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的仿真結果可以看出，在初始誤差分別為x=10 mm，y=2 016 mm，=?19.9°和x=?80 mm，y=1 020 mm，=28.8°的情況下，根據(jù)控制律式（14）設計的控制器，能使平臺平穩(wěn)的收斂于期望軌跡，取得良好的跟蹤效果。

圖7 圓弧軌跡跟蹤仿真結果

4 軌跡跟蹤算法試驗

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

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

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

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

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

圖9 試驗場地圖

4.2 平臺軌跡跟蹤試驗

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

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

圖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 直線軌跡跟蹤試驗結果

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

圖11 圓弧軌跡跟蹤試驗結果

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

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

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

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

5 結論與討論

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

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

4）當控制器的系統(tǒng)參數(shù)為(1,2,3,4)= (0.1,0.2,0.15,0.3)時，仿真結果表明，當平臺軌跡跟蹤穩(wěn)定后其直線跟蹤誤差為x=±13.1 mm，y=±7.4 mm，=±0.75°，圓弧軌跡跟蹤誤差為x=±7.6 mm，y=±9.1 mm，=±0.52°，能夠較好的跟蹤期望軌跡。驗證試驗和應用性試驗結果表明，當系統(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輪機構，轉向方式為前輪轉向模型的前提下，采用基于Lyapunov函數(shù)和反推法（Backstepping）時變狀態(tài)反饋控制方法的軌跡跟蹤算法，解決畜牧養(yǎng)殖環(huán)境監(jiān)測平臺軌跡跟蹤問題，該平臺與傳統(tǒng)的差速轉向平臺或機器人不同的是，它是通過控制方向輪的方向來改變平臺的運動方向，具有非完整約束的特性，在實際工程應用中，該控制方式容易使平臺運動出現(xiàn)偏移和車輪打滑現(xiàn)象，因此需要定位精度和實時性較高的定位裝置，才能保證平臺的運動精度。在平臺運動過程中，轉向輪存在小角度快速左右轉動情況，經分析，這與控制器輸出的控制轉向輪轉角數(shù)據(jù)快速波動有關。該控制算法較為復雜，想要平臺獲得較快的系統(tǒng)響應，需要性能較高的硬件設備，才能滿足系統(tǒng)的實時性，成本較高。因此，下一步可通過改變方向控制方式和簡化算法，降低算法的復雜性對系統(tǒng)硬件的需求，并采用濾波的方式減少輸入和輸出數(shù)據(jù)的波動，解決轉向輪轉角快速左右轉動問題，提高平臺的綜合性能，降低成本。

[1] 陸明洲，沈明霞，丁永前，等. 畜牧信息智能監(jiān)測研究進展[J]. 中國農業(yè)科學，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] 宋立博，李勁松. 輪式移動機器人嵌入式自適應控制器設計與仿真[J]. 控制理論與應用，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]. 控制理論與應用，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] 呂學勤，張軻，吳毅雄. 輪式移動焊接機器人輸出反饋線性化控制[J]. 機械工程學報，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]. 太原理工大學學報，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]. 控制理論與應用，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]. 自動化學報，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è)機械路徑跟蹤方法[J]. 農業(yè)機械學報，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 濾波和純追蹤模型的農業(yè)機械導航控制[J]. 農業(yè)機械學報，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]. 中國科學技術大學學報，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]. 清華大學學報，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)定性分析中的應用[J]. 山東大學學報：工學版，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]. 東南大學學報：自然科學報，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]. 儀器儀表學報，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]. 農業(yè)工程學報，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)代農業(yè)產業(yè)技術體系建設專項資金（CARS-33-13）；國家自然科學基金項目（51108194）

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

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

10.11975/j.issn.1002-6819.2018.13.011

TP23

A

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