大型平移式噴灌機分布式級聯協同導航控制方法

孟令剛 范松濤 周 燕

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

0 引言

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

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

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

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

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

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

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

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

(1)

(2)

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

(3)

(4)

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

(5)

(6)

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

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

(7)

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

(8)

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

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

2 分布式級聯的協同導航控制方法

2.1 協同導航偏差求解

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

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

(9)

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

(10)

(11)

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

(12)

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

(13)

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

2.2 協同導航控制

2.2.1主塔車路徑導航控制算法

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

(14)

式中Kl——橫向偏差反饋系數

Kθ——航向偏差反饋系數

主塔車沿導航路徑的速度vs保持不變，則

(15)

(16)

2.2.2子塔車協同跟隨控制算法

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

如圖5所示，子塔車1的航向偏差為eθ,1,j。由多塔車運動學模型，子塔車1通過改變與主塔車的相對速度調節自身運動姿態。為實現子塔車1隨主塔車協同行走，消除子塔車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，聯立式(19)、(20)可得

(21)

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

(22)

2.3 速度頻率轉換算法

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

由于變頻電機的輸出轉速與工作頻率并不嚴格線性相關，本文基于分段擬合建立了驅動輪速度與頻率的函數表達式

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

(23)

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

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

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

3 協同導航控制系統設計

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

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

4 試驗驗證

4.1 試驗樣機研制

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

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

4.2 試驗結果分析

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

導航過程中，系統的導航路徑通過設定點A、B的GPS經緯度坐標獲得；初始狀態時，噴灌機桁架方向調整到近似與行駛方向垂直的位置，啟動導航控制系統，大型噴灌機各塔車沿預定路徑進行協同導航行走，各塔車的真實行走軌跡由安裝在各塔車節點控制器中的RTK移動站模塊采集。經過多次試驗調試，設定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進行多塔車協同導航試驗，試驗結果如圖11所示。

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

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

5 結論

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

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