大型平移式噴灌機分布式級聯(lián)協(xié)同導(dǎo)航控制方法

孟令剛 范松濤 周 燕

(1.中國科學院半導(dǎo)體研究所光電系統(tǒng)實驗室， 北京 100083;2.中國科學院大學材料科學與光電技術(shù)學院， 北京 100049;3.中國科學院大學電子電器與通信工程學院， 北京 100049)

0 引言

中國農(nóng)業(yè)存在水資源短缺和利用效率不高的雙重問題，變量灌溉技術(shù)是現(xiàn)代農(nóng)業(yè)發(fā)展的必然選擇。大型平移式噴灌機(以下簡稱噴灌機)具有單機控制面積大、自動化程度高、灌溉效率高等優(yōu)點，是實現(xiàn)變量灌溉的有效載體[1-3]。目前噴灌機主要通過在行走方向調(diào)節(jié)運動速度以及在桁架方向改變噴頭流量實現(xiàn)變量灌溉[4-5]，為了實現(xiàn)更精準的變量灌溉，協(xié)同導(dǎo)航控制系統(tǒng)不僅要控制噴灌機按導(dǎo)航路徑行走，還要保證各塔車協(xié)同運動的同步性。

傳統(tǒng)大型平移式噴灌機主要通過在導(dǎo)航路徑上預(yù)設(shè)犁溝、鋼絲繩、地磁線等裝置，控制主塔車的航向?qū)崿F(xiàn)噴灌機路徑導(dǎo)航；通過跨角傳感器或同步控制桿等采用擺角歸零的“走-停-走”間歇控制方法，實現(xiàn)各子塔車隨主塔車協(xié)同同步行走。在該導(dǎo)航控制方式下，噴灌機只能沿固定的導(dǎo)航路徑行走、各塔車運動不連續(xù)、同步性差，且不能實時獲取各塔車的位置，無法滿足更精準變量灌溉下噴灌機自動導(dǎo)航協(xié)同行走的作業(yè)要求[6]。

近些年，衛(wèi)星定位技術(shù)快速發(fā)展，尤其是采用載波相位差分的RTK定位已達到厘米級定位精度，被廣泛應(yīng)用于包括噴灌機在內(nèi)的農(nóng)機導(dǎo)航控制領(lǐng)域[7-13]。國外關(guān)于噴灌機導(dǎo)航控制方面的研究較早[14-17]，目前美國的Reinke、Valmont、T-L等公司已有成熟的基于GPS的導(dǎo)航控制產(chǎn)品，這些產(chǎn)品在噴灌機的兩側(cè)/單側(cè)節(jié)點放置GPS定位模塊，通過調(diào)節(jié)驅(qū)動電機的占空比或變速電機(如X-Tec 直流電機、可調(diào)液壓驅(qū)動電機、變頻調(diào)速電機等)轉(zhuǎn)速調(diào)節(jié)驅(qū)動塔車的運動姿態(tài)，實現(xiàn)噴灌機主塔車或末端塔車路徑導(dǎo)航。在國內(nèi)，張小超等[18]基于GPS技術(shù)對大型噴灌機變量控制進行了研究，驗證了利用高精度GPS定位信號作為大型噴灌機控制反饋信號的可行性；張銀橋等[19]進一步提出了基于GPS變頻調(diào)速的噴灌機自動作業(yè)控制技術(shù)，根據(jù)處方圖實現(xiàn)了自動作業(yè)；吳普特團隊[20-21]研制了一種基于GPS導(dǎo)航的太陽能驅(qū)動平移式噴灌機，并設(shè)計了基于PID和模糊控制的單塔車導(dǎo)航控制系統(tǒng)，其在速度0.8 m/min下最大橫向誤差為18.4 cm。以上研究大都基于噴灌機單塔車導(dǎo)航控制，有關(guān)噴灌機多塔車導(dǎo)航控制的研究鮮有報道。

此外，為解決噴灌機同步行走問題，MOELLER等[16]提出了一種塔車逐級對準的噴灌機同步行走控制方法，即從最外側(cè)塔車到最內(nèi)側(cè)塔車依次對準校正。嚴海軍[3]提出了基于變頻調(diào)速的平移式噴灌機連續(xù)運行策略。YUAN等[22]在開發(fā)的大型平移式噴灌機虛擬仿真系統(tǒng)上，通過對相關(guān)控制參數(shù)的優(yōu)化以及田間試驗，驗證了基于角度傳感器的連續(xù)變頻調(diào)速控制系統(tǒng)能實現(xiàn)大型噴灌機的同步行走，在控制速度7.5 Hz下其最大跨間夾角誤差為0.6°，最大累積同步誤差為0.12°。以上研究都基于塔間夾角進行同步行走控制，然而在實際田間作業(yè)時，由于塔架變形和晃動等外界因素容易改變塔間夾角，且塔間夾角誤差存在傳遞累積，這極大影響了噴灌機同步行走的控制精度。

基于以上研究，為了實現(xiàn)大型平移式噴灌機多塔車協(xié)同導(dǎo)航，本文基于RTK定位及變頻調(diào)速技術(shù)，提出分布式級聯(lián)協(xié)同導(dǎo)航控制方法。并以自研大型噴灌機為試驗平臺，設(shè)計大型平移式噴灌機協(xié)同導(dǎo)航控制系統(tǒng)，通過試驗驗證該控制系統(tǒng)的有效性，以期實現(xiàn)復(fù)雜田間環(huán)境下，大型平移式噴灌機多塔車的路徑導(dǎo)航和同步行走。

1 大型平移式噴灌機多塔車運動學模型

以自研大型噴灌機為研究對象，其三維結(jié)構(gòu)如圖1所示，該噴灌機整機由1個主塔車、4個子塔車(子塔車1～4)以及桁架等組成。其中，子塔車與靠近主塔車一側(cè)的桁架組成一組跨體，且由子塔車驅(qū)動；主塔車與跨體間以帶集電環(huán)的轉(zhuǎn)環(huán)連接，以滿足跨體繞主塔車旋轉(zhuǎn)的作業(yè)需求；跨體間以球鉸及撓性軟管連接，使噴灌機跨體與跨體間有一定的旋轉(zhuǎn)運動空間。

噴灌機主塔車行走部分采用四輪驅(qū)動，子塔車行走部分采用二輪驅(qū)動，其單側(cè)前后兩個車輪由同一電機提供動力。為便于對噴灌機運動學模型進行分析，噴灌機行走過程中，主塔車模型被簡化為二輪差速模型，其通過控制兩側(cè)驅(qū)動輪速度實現(xiàn)轉(zhuǎn)向；子塔車模型被簡化為DOF(動態(tài)節(jié)點)，桁架結(jié)構(gòu)被簡化為剛性連接桿，跨體通過繞相對旋轉(zhuǎn)中心旋轉(zhuǎn)實現(xiàn)轉(zhuǎn)向，其相對旋轉(zhuǎn)中心位于主塔車轉(zhuǎn)環(huán)及子塔車球鉸處。假設(shè)不考慮噴灌機行走時各塔車驅(qū)動輪側(cè)向滑動、次級塔車的拖拽作用等因素的影響，噴灌機多塔車運動學模型可用主塔車運動學模型和子塔車級聯(lián)運動學模型共同表示，如圖2所示，圖中θs為噴灌機航向角，Ws為主塔車左右兩側(cè)輪距，L為塔車間距。

(1)

(2)

在子塔車級聯(lián)運動學模型中，子塔車1以主塔車中心(轉(zhuǎn)環(huán)結(jié)構(gòu)處)為相對旋轉(zhuǎn)中心做相對旋轉(zhuǎn)運動。由于主塔車與子塔車1存在航向夾角γ0,1,j，主塔車沿桁架垂向方向的速度分量為

(3)

(4)

式中v1,j——子塔車1在j時刻的速度

(5)

(6)

式中vi-1,j、vi,j——子塔車i-1和子塔車i在j時刻的速度

噴灌機塔車i的運動學方程滿足[24]

(7)

聯(lián)立式(2)、(6)、(7)，噴灌機多塔車運動學模型可表示為

(8)

式中v2,j、v3,j、v4,j——子塔車2、3、4在j時刻的速度

在多塔車運動學模型中，主塔車位姿變化主要受作業(yè)兩輪的驅(qū)動速度影響；子塔車位姿變化除受自身驅(qū)動速度影響外，還受上級塔車(主塔車一側(cè))驅(qū)動速度的影響。

2 分布式級聯(lián)的協(xié)同導(dǎo)航控制方法

2.1 協(xié)同導(dǎo)航偏差求解

在協(xié)同導(dǎo)航控制器中，噴灌機導(dǎo)航偏差和同步偏差的精度直接影響協(xié)同導(dǎo)航的控制精度，而協(xié)同導(dǎo)航偏差和同步偏差的精度與噴灌機各塔車的定位精度及航向精度相關(guān)。為獲得高精度定位信號和航向信號，選用基于雙天線RTK定位模塊實時獲取各塔車的位姿(位置和航向)。由于常見的RTK定位模塊輸出的噴灌機各塔車的位置為經(jīng)緯度和高度數(shù)據(jù)，為了便于導(dǎo)航控制，在導(dǎo)航控制過程中通常將經(jīng)緯高坐標轉(zhuǎn)換為本地局部東北天坐標。

如圖4所示，ENU坐標系下，噴灌機導(dǎo)航路徑由主塔車起始點A(xA,yA)和終點B(xB,yB)確定，導(dǎo)航航向角θs為直線AB的方向角，為

(9)

以點A為坐標原點，導(dǎo)航航向角所在的方向為X軸，與導(dǎo)航航向垂直的噴灌機桁架方向為Y軸，建立噴灌機導(dǎo)航坐標系(XNONYN)，從導(dǎo)航坐標系變換到ENU坐標系的矩陣T滿足

(10)

(11)

理想情況下，噴灌機在導(dǎo)航控制過程中各塔車沿導(dǎo)航路徑平移同步行走，各塔車Y軸坐標保持不變，X軸坐標與主塔車保持一致，則任意時刻j，塔車i的理想導(dǎo)航位姿P′i,j為

(12)

此時，主塔車沿導(dǎo)航路徑的橫向偏差el,0,j、航向偏差eθ,0,j以及各子塔車i的同步偏差es,i,j為

(13)

由此，基于上述協(xié)同導(dǎo)航偏差求解運算獲得了噴灌機主塔車導(dǎo)航偏差以及各塔車的同步偏差。

2.2 協(xié)同導(dǎo)航控制

2.2.1主塔車路徑導(dǎo)航控制算法

主塔車在田間行走作業(yè)時，通常用導(dǎo)航偏差(橫向偏差el,0,j和航向偏差eθ,0,j)表示主塔車偏離導(dǎo)航路徑的程度。當主塔車偏離導(dǎo)航路徑時，主塔車只能通過改變左右兩輪驅(qū)動速度來改變航向角速度，進而改變航向角進行路徑跟蹤。

(14)

式中Kl——橫向偏差反饋系數(shù)

Kθ——航向偏差反饋系數(shù)

主塔車沿導(dǎo)航路徑的速度vs保持不變，則

(15)

(16)

2.2.2子塔車協(xié)同跟隨控制算法

理想情況下，多塔車協(xié)同行走時，噴灌機各塔車沿桁架方向呈一條直線，且各塔車航向與噴灌機設(shè)定航向保持一致。然而，由于多種因素的干擾，在多塔車實際田間協(xié)同行走作業(yè)時，各子塔車實際行走速度與理論速度并不一致，各子塔車沿桁架垂向方向出現(xiàn)超前或者滯后現(xiàn)象，即子塔車i存在與設(shè)定航向之間的航向偏差eθ,i,j，或子塔車i所在的跨體與噴灌機整機桁架方向存在跨間夾角，兩者大小相等。

如圖5所示，子塔車1的航向偏差為eθ,1,j。由多塔車運動學模型，子塔車1通過改變與主塔車的相對速度調(diào)節(jié)自身運動姿態(tài)。為實現(xiàn)子塔車1隨主塔車協(xié)同行走，消除子塔車1的航向偏差，本文利用PI控制理論的線性負反饋比例控制策略獲取期望的子塔車1相對主塔車的速度Δv1

(17)

式中K′p——航向偏差角度比例增益

K′i——航向偏差角度積分增益

(18)

式中Kp——同步偏差比例增益

Ki——同步偏差積分增益

es,1,j-m——子塔車1在j-m時刻同步偏差

此時，子塔車1的控制速度由主塔車控制速度和相對速度Δv1共同組成，即

(19)

(20)

子塔車2的控制速度滿足vc,2=vc,1-Δv2，聯(lián)立式(19)、(20)可得

(21)

依次，子塔車i隨上級塔車協(xié)同行走時，其控制速度滿足

(22)

2.3 速度頻率轉(zhuǎn)換算法

由于噴灌機采用380V 1.1kW的UMC變頻電機為驅(qū)動輪提供動力，變頻電機包含減速比i=60的行星齒輪減速器，其輸出扭矩經(jīng)傳動比irat為1∶50的蝸輪蝸桿車輪減速器傳送給車輪。輪胎為普通的人字形農(nóng)用輪胎，輪胎直徑D為1.2 m。為實現(xiàn)每組驅(qū)動輪速度的獨立調(diào)節(jié)，每組變頻電機均配有控制電機工作頻率的變頻器，通過調(diào)節(jié)變頻電機的工作頻率控制電機的轉(zhuǎn)速，表1為UMC變頻電機的性能參數(shù)。

由于變頻電機的輸出轉(zhuǎn)速與工作頻率并不嚴格線性相關(guān)，本文基于分段擬合建立了驅(qū)動輪速度與頻率的函數(shù)表達式

f=g(f,i,irat,D)

(23)

表1 UMC變頻電機的性能參數(shù)Tab.1 Performance parameters of UMC variable frequency motor

式中f——變頻電機工作頻率

根據(jù)式(23)，理想情況下驅(qū)動輪速度與頻率的關(guān)系如圖6所示，在實際田間控制過程中，受行進阻力及車輪滑動等影響，變頻電機與實際速度并不完全呈現(xiàn)這種關(guān)系，而是存在波動。

3 協(xié)同導(dǎo)航控制系統(tǒng)設(shè)計

協(xié)同導(dǎo)航控制系統(tǒng)由控制主機、塔車節(jié)點控制器和塔車驅(qū)動器等組成，如圖7所示?？刂浦鳈C是協(xié)同導(dǎo)航控制系統(tǒng)的核心，由協(xié)同導(dǎo)航控制軟件、RTK基站模塊、RTCM廣播電臺、4G數(shù)傳電臺服務(wù)器等組成；塔車節(jié)點控制器是協(xié)同導(dǎo)航控制系統(tǒng)的數(shù)據(jù)采集及控制終端，由STM32微控制器、RTK移動站模塊、RTCM接收電臺、4G數(shù)傳電臺終端等組成；塔車驅(qū)動是塔車控制系統(tǒng)的運動執(zhí)行單元，由變頻器、變頻電機組成；控制主機及塔車節(jié)點控制器之間通過4G數(shù)傳電臺進行通信，其中，RTK基站模塊和RTK移動站模塊均采用和芯星通科技北京有限公司的雙天線RTK定位模塊(UB482型)，定位精度1 cm，定向精度0.2(°)/m，速度精度0.03 m/s。協(xié)同導(dǎo)航控制過程中，RTK基站將采集的載波相位傳輸至移動站，移動站求解相位差獲得厘米級精度位姿(位置和航向)數(shù)據(jù)。基站衛(wèi)星天線就近安裝在實驗基地，移動站衛(wèi)星天線安裝在各塔車頂部，基站載波相位數(shù)據(jù)傳輸通過RTCM廣播電臺發(fā)送。4G無線數(shù)傳采用成都億佰特電子科技有限公司E840-DTU型 4G數(shù)傳終端，用來實現(xiàn)主機與分布式塔車節(jié)點控制器雙向數(shù)據(jù)傳輸。由于RTK定位模塊的田間實際工作中定位精度為2 cm，噴灌機最大行走速度一般不大于1.5 m/min(2.5 cm/s)，根據(jù)奈奎斯特采樣定理，RTK定位的采樣頻率不大于2.25 Hz，對網(wǎng)絡(luò)的延時應(yīng)小于440 ms。經(jīng)實際測試，該4G數(shù)傳終端平均網(wǎng)絡(luò)延遲小于400 ms，4G數(shù)傳終端產(chǎn)生的網(wǎng)絡(luò)延時對協(xié)同導(dǎo)航控制系統(tǒng)的實時性影響在允許范圍內(nèi)。變頻器采用杭州三科頻技術(shù)有限公司的SKI600型矢量變頻器，該變頻器可通過485通信接口控制變頻電機的工作頻率。STM32微控制器選用STM32F103芯片作為下位機主控芯片，且集成232、485等通信接口，滿足導(dǎo)航控制終端數(shù)據(jù)采集及變頻器控制的需求。

如圖8所示，噴灌機協(xié)同導(dǎo)航作業(yè)前，首先設(shè)定導(dǎo)航路徑，并發(fā)送給協(xié)同導(dǎo)航控制器。作業(yè)過程中，塔車節(jié)點控制器通過RTK移動站模塊實時采集塔車的位姿(位置和航向)信息，并將采集的位姿信息發(fā)送給控制主機內(nèi)的協(xié)同導(dǎo)航控制器；協(xié)同導(dǎo)航控制器對各塔車位姿信息經(jīng)坐標轉(zhuǎn)換處理后，通過與導(dǎo)航路徑對比獲得主塔車導(dǎo)航偏差和子塔車同步偏差；經(jīng)內(nèi)部主塔車路徑導(dǎo)航控制器和子塔車協(xié)同行走控制器處理后得到各塔車驅(qū)動輪的控制速度；經(jīng)速度頻率轉(zhuǎn)換器轉(zhuǎn)為變頻電機的控制工作頻率，并以控制指令形式發(fā)送給塔車節(jié)點控制器。塔車節(jié)點控制器通過控制變頻器的輸出頻率調(diào)節(jié)電機的轉(zhuǎn)速，進而調(diào)節(jié)驅(qū)動輪的運動速度，實現(xiàn)多塔車協(xié)同導(dǎo)航控制。

4 試驗驗證

4.1 試驗樣機研制

本文研制了多塔車協(xié)同導(dǎo)航控制大型平移式噴灌機試驗樣機，如圖9所示，主要包含噴灌機本體及分布式級聯(lián)協(xié)同導(dǎo)航控制系統(tǒng)(控制主機、塔車節(jié)點控制器和塔車驅(qū)動器等)。噴灌機樣機參數(shù)見表2。與市場上銷售的DPP系列噴灌機不同，試驗樣機由變頻電機驅(qū)動，每個塔車均安裝有RTK定位模塊、含變頻驅(qū)動的分布式節(jié)點控制器，可實時獲得塔車位姿數(shù)據(jù)并對變頻電機工作頻率進行調(diào)節(jié)。

表2 大型平移式噴灌機樣機參數(shù)Tab.2 Prototype parameters of large translational sprinkler irrigation machine

4.2 試驗結(jié)果分析

協(xié)同導(dǎo)航控制試驗于2020年10月19—24日在河南省許昌市試驗田中進行，試驗地塊為正常的旱地地塊，未對機行道進行平整和硬化處理，如圖10所示。

導(dǎo)航過程中，系統(tǒng)的導(dǎo)航路徑通過設(shè)定點A、B的GPS經(jīng)緯度坐標獲得；初始狀態(tài)時，噴灌機桁架方向調(diào)整到近似與行駛方向垂直的位置，啟動導(dǎo)航控制系統(tǒng)，大型噴灌機各塔車沿預(yù)定路徑進行協(xié)同導(dǎo)航行走，各塔車的真實行走軌跡由安裝在各塔車節(jié)點控制器中的RTK移動站模塊采集。經(jīng)過多次試驗調(diào)試，設(shè)定Kl=0.48，Kθ=0.86，Kp=15，Ki=2.5。

噴灌機以低速(0.5 m/min)、中速(1.0 m/min)、高速(1.5 m/min)在AB之間定速行駛40 m進行多塔車協(xié)同導(dǎo)航試驗，試驗結(jié)果如圖11所示。

對試驗結(jié)果進行統(tǒng)計分析，主塔車橫向偏差、航向偏差以及各子塔車的同步偏差的平均絕對偏差、最大偏差、標準差表示協(xié)同導(dǎo)航控制效果，統(tǒng)計分析結(jié)果如表3所示。

由圖11和表3可知，在不同行走速度條件下，主塔車最大橫向偏差為3.26 cm，平均絕對偏差不超過1.40 cm；主塔車最大航向偏差為1.65°，平均絕對偏差不超過0.74°；各子塔車最大同步偏差為13.07 cm，平均絕對偏差不超過3.47 cm。由于變頻器輸出頻率隨時間線性改變，隨著導(dǎo)航速度的增加，各塔車進行速度調(diào)節(jié)達到設(shè)定速度的時間增加；并且控制系統(tǒng)中，協(xié)同導(dǎo)航控制算法無與速度相適應(yīng)的變量，因此噴灌機的導(dǎo)航偏差和同步偏差呈增大趨勢。但總體上看，協(xié)同導(dǎo)航控制系統(tǒng)能夠有效地消除導(dǎo)航偏差和同步偏差，保證噴灌機按導(dǎo)航路徑協(xié)同同步行走，這表明導(dǎo)航控制系統(tǒng)能夠滿足大型噴灌機多塔車協(xié)同作業(yè)的要求。

5 結(jié)論

(1)在大型平移式噴灌機運動學模型的基礎(chǔ)上，提出了一種分布式級聯(lián)協(xié)同導(dǎo)航控制方法，該方法解決了大型平移式噴灌機路徑導(dǎo)航以及多塔車同步行走問題。

(2)在不同導(dǎo)航速度下，通過田間試驗驗證了協(xié)同導(dǎo)航控制系統(tǒng)及控制方法的控制效果及穩(wěn)定性。不同速度下，主塔車最大橫向偏差為3.26 cm，平均絕對偏差不超過1.40 cm；主塔車最大航向偏差為1.65°，平均絕對偏差不超過0.74°；各子塔車最大同步偏差為13.07 cm，平均絕對偏差不超過3.47 cm。協(xié)同導(dǎo)航控制系統(tǒng)的控制精度和穩(wěn)定性滿足田間噴灌作業(yè)要求。