磁導(dǎo)式溫室協(xié)同作業(yè)移動平臺控制系統(tǒng)設(shè)計與實現(xiàn)

管澤峰，王新忠

(江蘇大學(xué) 農(nóng)業(yè)裝備工程學(xué)院，江蘇 鎮(zhèn)江 212001)

0 引言

隨著溫室向大型化、現(xiàn)代化、智能化方向迅速發(fā)展，如何提高溫室作業(yè)生產(chǎn)效率，使我國由設(shè)施大國轉(zhuǎn)變?yōu)樵O(shè)施強國成為目前設(shè)施領(lǐng)域發(fā)展的關(guān)鍵問題[1]。目前，我國設(shè)施農(nóng)業(yè)生產(chǎn)由于作業(yè)機具和配套設(shè)備尚不完善，缺乏專用的小型機械化作業(yè)機具，導(dǎo)致設(shè)施內(nèi)的勞動生產(chǎn)仍然依靠從業(yè)人員人工完成，勞動強度大、生產(chǎn)率低。因此，研制溫室省力作業(yè)裝置及相關(guān)技術(shù)有較大的市場需求。

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

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

1 系統(tǒng)組成

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

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

圖1 系統(tǒng)總體設(shè)計框圖

2 移動平臺運動建模

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

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

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

Fig.2 Kinematics model of mobile platform

由圖2中關(guān)系可得車體中心O1的坐標為

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

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

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

3 系統(tǒng)控制算法

3.1 主程序

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

圖3 主程序流程圖

3.2 基于模糊PID算法的路徑跟蹤系統(tǒng)

3.2.1 路徑獲取方法

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

3.2.2 模糊PID控制器的設(shè)計

本文以移動平臺的導(dǎo)航路徑橫向偏差和偏差變化率作為系統(tǒng)的狀態(tài)輸入變量，采用模糊控制對PID控制器參數(shù)進行在線調(diào)整， 以此滿足實際控制環(huán)境對被

控對象的要求。模糊PID控制器結(jié)構(gòu)如圖4所示。

圖4 模糊PID控制器結(jié)構(gòu)圖

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

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

3.2.3 MatLab仿真

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

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

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

圖5 模糊PID控制器仿真圖

圖6 階躍信號和尖峰信號下的輸出響應(yīng)

3.3 基于加權(quán)融合算法的目標距離信息融合

超聲波傳感器和紅外傳感器分別對目標距離信息測量3次，根據(jù)超聲波傳感器和紅外傳感器的測距信息計算相應(yīng)的權(quán)值，通過加權(quán)融合算法計算得到關(guān)于被測目標距離信息的融合值。

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

4.1 移動平臺行走導(dǎo)航系統(tǒng)直線跟蹤試驗

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

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

4.2 移動平臺行走導(dǎo)航系統(tǒng)人機跟隨試驗

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

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

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

5 結(jié)論

針對我國溫室環(huán)境條件下物料、機具等在溫室壟間頻繁運輸?shù)男枨?，通過磁導(dǎo)航傳感器和測距傳感器構(gòu)建溫室移動平臺的協(xié)同作業(yè)行走控制系統(tǒng)，采用模糊PID控制算法實現(xiàn)平臺預(yù)設(shè)路徑的跟蹤導(dǎo)航功能，在路徑導(dǎo)航的基礎(chǔ)上通過測距模塊實現(xiàn)協(xié)同作業(yè)的功能。試驗結(jié)果表明：本文設(shè)計的人機跟隨作業(yè)導(dǎo)航系統(tǒng)穩(wěn)定可靠。當平臺以1m/s的速度沿預(yù)設(shè)路徑行駛時，路徑橫向偏差最大為2.6cm，平均跟隨誤差不超過5.4cm，可以實現(xiàn)移動平臺在溫室壟間沿著預(yù)設(shè)路徑協(xié)同跟隨作業(yè)。