磁導式溫室協同作業移動平臺控制系統設計與實現

管澤峰，王新忠

(江蘇大學 農業裝備工程學院，江蘇 鎮江 212001)

0 引言

隨著溫室向大型化、現代化、智能化方向迅速發展，如何提高溫室作業生產效率，使我國由設施大國轉變為設施強國成為目前設施領域發展的關鍵問題[1]。目前，我國設施農業生產由于作業機具和配套設備尚不完善，缺乏專用的小型機械化作業機具，導致設施內的勞動生產仍然依靠從業人員人工完成，勞動強度大、生產率低。因此，研制溫室省力作業裝置及相關技術有較大的市場需求。

當前我國設施農業具有內部結構不統一、壟間距狹窄、種植物密集等特點，因此國外的軌道式移動平臺難以應用到我國的設施農業中。所以，采用能夠靈活調整運動狀態的自主移動平臺是解決這一問題的有效方法。國外針對農業機械導航技術的相關研究較多[2-3]：Palamas采用機器視覺引導溫室移動機器人壟間行走[4]；Gonzalez提出了基于地圖匹配的慎思規劃和基于傳感器反饋算法的反應式行為技術兩種方法實現對溫室移動機器人的自主導航[5]；Subramanian采用基于視覺和雷達的傳感器系統，通過模糊邏輯算法實現小型溫室移動機器人的自主導航[6]。國內的相關研究起步較晚，張鐵民等提出了基于CCD圖像傳感器、電子羅盤、超聲波和加速度計等多傳感器信息融合導航控制方法，利用模糊算法對導航路徑進行信息獲取，在行駛速度為45cm/s下的路徑跟蹤偏差為7.47mm[7]。周俊等基于卡爾曼濾波的思想融合各傳感器的觀測值給出輪式拖拉機跟蹤控制方法，避免了視覺系統耗時引起的狀態反饋滯后，改善導航魯棒性[8]。張智剛等利用Kalman濾波器對GPS定位數據進行平滑處理，并采用純追蹤模型實現農業機械的直線跟蹤控制，行駛速度為0.6m/s時的平均誤差小于0.02m[9]。

本文設計了一種設施環境條件下的協同跟隨作業移動平臺行走控制系統。利用磁導航傳感器采集溫室壟間的預設路徑信息，使用模糊自整定PID控制算法對導航信息進行優化，實現溫室移動平臺對預設路徑的精準跟蹤；在此基礎上，通過測距模塊實現移動平臺的人機跟隨功能。本移動平臺通過溫室壟間的協同跟隨作業，可減輕作業人員的勞動強度，提高生產效率。

1 系統組成

移動平臺行走控制系統主要由主控制器、磁導航傳感器模塊、測距傳感器模塊、直流無刷電機及其驅動模塊、電池組及其固定裝置等系統和部件組成，如圖1所示。

本文以STC15W4K56S4單片機為主控制器采集傳感器信息。將XGS-19016N型數字式磁導航傳感器安裝在平臺前端正下方，用以獲取預設磁導引路徑上方的磁信號。測距傳感器模塊由HC-SR04超聲波傳感器和GP2Y0A02YK0F型紅外傳感器組成。超聲波傳感器和紅外傳感器上下并排安裝于移動平臺正前方，通過超聲波傳感器和紅外傳感器檢測平移前方作業人員或者障礙物的距離。

圖1 系統總體設計框圖

2 移動平臺運動建模

針對上述移動平臺，首先對其進行運動狀態分析，建立圖2所示的履帶式移動平臺運動模型。以預設路徑為基準建立坐標系，其中平臺的磁導航傳感器中心距離預設磁導引路徑的橫向位置偏差為p，平臺行駛方向與預設磁路徑方向之間夾角的角度偏差為θ。

O1.移動平臺的幾何中心k.車體幾何中心到磁導航傳感器中心的距離(mm)v.平臺的行駛速度(mm/s)ω.平臺的轉動角速度(rad/s)vl、vr.左右履帶輪的線速度(mm/s)

圖2 移動平臺底盤運動模型

Fig.2 Kinematics model of mobile platform

由圖2中關系可得車體中心O1的坐標為

其中，x0、y0、θ0為平臺初始時刻的位置坐標和方位角。

移動平臺路徑跟蹤的設計任務是給定平臺一個初始速度，使得平臺沿著預設磁導引路徑行駛。本文設計的履帶式移動平臺通過左右履帶輪的差速行駛調整導航位姿，因此輸入狀態變量為行駛路徑橫向偏差和橫向偏差變化率，輸出控制量ul和ur分別是左右驅動輪的電樞電壓，對平臺左右驅動輪進行速度控制，對車輛的運動狀態進行微分得

并且由于角度偏θ差很小，所以可以近似認為sinθ=θ，對上式進行拉氏變化，并將得到車輛連續的運行過程，即

3 系統控制算法

3.1 主程序

主程序根據各個模塊的傳感器信息判斷是否執行路徑跟蹤、人機跟隨及手動遙控等子程序，流程如圖3所示。

圖3 主程序流程圖

3.2 基于模糊PID算法的路徑跟蹤系統

3.2.1 路徑獲取方法

采用磁導航傳感器獲取預設路徑的磁導引信息，通過磁導航傳感器輸出點的位置提供移動平臺與導引路徑之間偏差距離信息。在平臺行駛過程中，磁導航傳感器內部垂直于磁條上方的探測點檢測到磁信號后會有連續35個檢測點輸出信號，求出兩個邊界輸出點的中間值作為平臺的中心點，主控制器根據輸出點的位置判斷導航路徑的偏差信息。

3.2.2 模糊PID控制器的設計

本文以移動平臺的導航路徑橫向偏差和偏差變化率作為系統的狀態輸入變量，采用模糊控制對PID控制器參數進行在線調整， 以此滿足實際控制環境對被

控對象的要求。模糊PID控制器結構如圖4所示。

圖4 模糊PID控制器結構圖

ECEPBPMPSZONSNMNBPBNB PB PBNB PB PBNM PB ZONM PM ZONS PS ZONS ZO ZOZO ZO PSPMNB PB PSNM PB PSNM PM ZONM PM NSNS PS NSZO ZO NSZO ZO NMPSNM PM PSNM PM PSNS PS ZONS PS NSZO ZO NSPS NS NMPS NS NBZONM PM PMNM PS PSNS PS ZOZO ZO NSPS NS NMPS NS NMPM NM NBNSNM PS PMNS PS PSZO ZO ZOPS NS NSPM NS NMPM NM NBPM NM NBNMZO ZO PMZO ZO NSPS NS ZOPM NM NSPM NM NSPB NB NSPB NB NSNBZO ZO PBPS ZO PBPS NM ZOPM NM ZOPM NB ZOPB NB PSPB NB PS

根據表1，考慮到控制器的精度與復雜程度，將路徑橫向偏差和橫向偏差變化率的模糊論域都取為{-3，-2，-1，0，1，2，3}，將其模糊子集定義為{NB,NM,NS,ZO,PS,PM,PB}，對應值代表負大，負中，負小，零，正小，正中，正大。

3.2.3 MatLab仿真

在MatLab數字分析軟件命令窗口運行Fuzzy函數進入模糊邏輯編輯器，選擇控制類型為mamdani型系統，建立fuzzy logic controller模塊，再利用Simulink工具箱搭建自適應模糊PID控制系統模型，如圖5所示。

仿真時計算機的采樣時間為0.01s，平臺的速度為1m/s。首先，比較兩種控制器在單位階躍信號作用下的輸出；其次，在時間軸的2s處設置一個尖峰干擾信號比較兩種控制器在隨機干擾下的輸出情況。輸出響應如圖6所示。

通過圖6中的對比可以看出：在開始階段，系統在模糊PID控制器作用下的上升速度更快，系統能夠很好地跟蹤輸入信號，且模糊PID控制器在擾動輸入的情況下，能在更短的時間內調整到穩定狀態。因此，以上仿真結果表明模糊PID既能減少振蕩，使系統較好地達到穩態，又能提高系統的魯棒性。

圖5 模糊PID控制器仿真圖

圖6 階躍信號和尖峰信號下的輸出響應

3.3 基于加權融合算法的目標距離信息融合

超聲波傳感器和紅外傳感器分別對目標距離信息測量3次，根據超聲波傳感器和紅外傳感器的測距信息計算相應的權值，通過加權融合算法計算得到關于被測目標距離信息的融合值。

4 移動平臺的運動控制試驗分析

4.1 移動平臺行走導航系統直線跟蹤試驗

移動平臺在江蘇大學溫室進行實地性能測試。將移動平臺縱向對稱安置在導引路徑上，并通過軟件設定平臺的路徑跟蹤速度為1m/s；將自制的簡易漏斗裝置固定在平臺后側正中間，通過注射器針頭滴落在地面的液滴記錄平臺的實際行駛軌跡，以預設路徑和平臺的行駛方向為參照物，液滴在導引路徑左側為負值，在右側為正值。試驗現場如圖7所示；平臺在路徑跟蹤過程中的實時橫向偏差如圖8所示。

從圖7中可以看出：平臺在5m行駛距離內的最大橫向偏差為2.6cm，表明平臺在溫室壟間行走穩定，且在路徑跟蹤過程中能夠沿著預設路徑行駛。

4.2 移動平臺行走導航系統人機跟隨試驗

將溫室移動平臺的行駛速度設置為1m/s，并將長1.5m、寬1m的泡沫板安置在距離作業平臺1m處。在距離平臺1～2m處設置10個標記點，每個標記點間隔10cm。試驗過程中，以1.5～2m/s左右的速度將泡沫板從距離作業平臺1m處分別移至各標記點處，通過測量作業平臺前端中點與泡沫板的距離判斷平臺的安全跟隨性能。實際測得平臺與障礙物之間的平均跟隨距離為94.6cm，平均跟隨偏差為5.4cm。試驗表明：本課題設計的溫室移動作業平臺能以較高的精度實現協同作業功能，滿足溫室作業要求。

圖7 移動平臺行駛軌跡路線圖

圖8 移動平臺行駛路徑橫向偏差

5 結論

針對我國溫室環境條件下物料、機具等在溫室壟間頻繁運輸的需求，通過磁導航傳感器和測距傳感器構建溫室移動平臺的協同作業行走控制系統，采用模糊PID控制算法實現平臺預設路徑的跟蹤導航功能，在路徑導航的基礎上通過測距模塊實現協同作業的功能。試驗結果表明：本文設計的人機跟隨作業導航系統穩定可靠。當平臺以1m/s的速度沿預設路徑行駛時，路徑橫向偏差最大為2.6cm，平均跟隨誤差不超過5.4cm，可以實現移動平臺在溫室壟間沿著預設路徑協同跟隨作業。