水田環境下水稻直播機自動駕駛控制方法

張 雁 李彥明 劉翔鵬 陶建峰 劉成良 李瑞川

(1.上海交通大學機械與動力工程學院， 上海 200240； 2.山東五征集團， 日照 262399)

0 引言

隨著農業機械化和自動化的不斷進步，精準農業也得到了顯著的發展。無人農機的發展降低了農業生產成本、提高了質量并且減少了人工投入[1]。自主駕駛是無人農機核心技術之一。自主駕駛系統使得農機無需駕駛員控制方向盤，并且可以長時間工作，不會因為疲憊而降低作業質量[2]。因此，相對駕駛員，自主駕駛系統在長時間工作時可以更高精度地完成作業任務，提高作業質量，并且可以降低人力成本。隨著計算機和傳感器技術的進步，農機領域的自主駕駛系統獲得了長足的發展且成為研究熱點之一[3]。對于工作在水田等非結構化環境下的水稻直播機等農機裝備而言，在非平坦且泥濘易滑的水田環境里實現高精度的路徑跟蹤和自動駕駛具有重要意義。

在農業自主駕駛系統上，常用的路徑跟蹤方法有PID控制[4-5]、模糊控制[6-8]、純追蹤算法[9-12]、最優控制[13-16]等。通常以轉向角作為控制變量[17-18]。在無法獲得準確的轉向數學模型時，PID控制或模糊控制是農機轉向控制最簡單的控制方法[19]。文獻[20-22]實現了基于PID控制方法的轉向控制器，但是實驗結果顯示其算法具有較大的超調，且誤差較大。WU等[23]設計了一種串聯閉環PID控制器，實驗結果顯示這種控制器可以提高跟蹤精度，但是控制器所需的轉向角速度較難測量。文獻[24-25]基于模糊控制算法實現了應用于農機的自適應自動駕駛算法。李逃昌[15]采用模糊控制調整前視距離的純追蹤模型方法在插秧機平臺上進行了實驗，但實驗結果顯示路徑跟蹤時存在大于0.1 m的超調，影響了路徑跟蹤的精度。ZHANG等[26]提出了一種綜合拖拉機動態信息和跟蹤誤差的動態路徑跟蹤算法，具有較好的跟蹤精度。HAN等[27]基于航向角偏差計算轉向角實現了一種自動駕駛算法。JAVAD等[28]實現了一種高精度的非線性跟蹤算法，仿真結果顯示橫向偏差均方根僅1.36 cm。李培新等[29]基于跟蹤誤差模型提出一種無人駕駛預測控制方法，仿真結果表明平均橫向偏差小于0.1 m，航向角小于1°。但這些算法均未在水田類似的泥濘和非平坦路面測試，無法直接用于水稻直播機的自主駕駛。李革等[30]用基于速度、路徑彎度等線性調整前視距離的改進純追蹤方法在插秧機平臺上做了水田實驗，實驗結果顯示作業段平均誤差為0.058 m，最大跟蹤誤差達0.135 m，但實驗直線段長度過短，不足20 m。

為實現水稻直播機在水田環境下較高精度的自動駕駛，提出一種在不平坦泥濘水田環境中的水稻直播機自動駕駛控制方法，以實現直線段的高精度路徑跟蹤和地頭自動轉向。

1 運動學模型

1.1 模型

在不考慮車胎與地面的相互作用，假設車輛不會發生側滑俯仰等情況下，水稻直播機可簡化為兩輪車模型[31]，如圖1所示。模型中所涉及的參數和說明如表1所示。

圖1 車輛運動學模型Fig.1 Kinematic model of vehicle

參數含意δ前輪轉角,也是控制變量ω轉向角速度θ航向角θp期望路徑P在點T處切線的航向角L水稻直播機軸距(x,y)車輛后軸中心點坐標,y軸正向為正北方向v車輛速度P車輛跟蹤的期望路徑d橫向偏差φ航向角偏差R車輛轉彎半徑M(xm, ym)車輛后軸中點坐標T(xt, yt)路徑P上距M最近的點坐標

1.2 農機運動學模型

(1)

(2)

由式(1)和式(2)可得

(3)

速度v在x軸方向的分量為

(4)

速度v在y軸方向的分量為

(5)

因此，水稻直播機的運動學模型為

(6)

水稻直播機的運動狀態可以用四維向量描述為

s=(x,y,θ,δ)

(7)

2 自動駕駛控制方法

水稻直播機的自動駕駛控制方法包括在作業段的直線路徑跟蹤算法和地頭處的自動轉彎算法。在直線段應盡量提高跟蹤精度，在地頭轉彎時應盡量提高速度。因為水田坑坑洼洼，不平坦的路面會導致車身傾斜，給農機定位帶來一定的誤差。為消除這種影響，需要根據車身的傾角對定位數據作出修正。

2.1 不平坦路面的定位修正

在水田環境等不平整路面上行駛時，車身往往會發生傾斜。當定位系統安裝在有一定高度的車身上時，會給定位帶來誤差,如圖2所示。定位系統測量車輛后輪軸中點M的位置作為車輛的位置。在車身橫滾角和俯仰角為零時，定位天線測量點在車軸上的投影與M重合。在車身傾斜時，定位天線測量點在車輛后軸上的投影與M點并不重合，從而無法獲得準確的M點位置信息。

圖2 車身傾斜造成的誤差Fig.2 Position errors caused by incline

車身傾斜造成的誤差有2個，一個為橫滾角導致的誤差elat，一個為俯仰角導致的誤差elon。設定位天線的安裝高度為H，橫滾角為α，俯仰角為ψ，則

elat=Hsinα

(8)

elon=Hsinψ

(9)

因此修正后的車輛位置M的坐標為

(10)

式中 (xm,ym)——修正后的M點坐標

2.2 直線段非線性路徑跟蹤控制算法

橫向偏差d的計算方法為

(11)

設θp為期望路徑P在點T處切線的航向角，則航向角偏差φ為

φ=θp-θ

(12)

橫向偏差代表當前的跟蹤偏差，而航向角偏差影響隨后的跟蹤偏差。因此車輛轉向時既要考慮橫向偏差也要考慮航向角偏差，可將兩者組合起來計算轉向角。其中消除航向角偏差φ的前輪轉角控制量δφ等于航向角偏差φ，即

δφ=φ

(13)

消除橫向偏差d的前輪轉角控制量δd為一個非線性反饋控制，為

(14)

其中

dfront=d-Lsinθ

(15)

式中k2——可調參數

dfront——前輪的橫向偏差

通過對控制算法添加對橫向偏差和前進速度的非線性反饋環節，通過反正切變換對反饋輸出限幅，同時在速度較快時減小前輪轉角輸出，可以有效減小超調量，提高穩定性。

結合式(13)～(15)，添加積分環節消除穩態誤差，基于航向角偏差、橫向偏差和車輛前進反饋的非線性控制路徑跟蹤算法為

(16)

其中Iint為減小靜態誤差的積分項，為橫向偏差d在過去一段時間的積分；k1、k2和ki為轉向算法中根據實際情況可調的參數，代表各項對轉向角影響的比重。

2.3 地頭自動調頭算法

地頭自動調頭的關鍵在于識別是否到達地頭。可以預先通過手持式定位儀標定作業地塊的邊界關鍵點，建立作業地塊的數據庫，從數據庫獲得的邊界關鍵點計算出作業地塊的邊界位置信息。實時計算水稻直播機位置與地邊界的距離，在小于閾值D時認為車輛到達地頭，自動執行調頭動作。

對于長江中下游平原地區的水田，地邊界大部分可看作直線段，因此可通過計算點到線段的距離來獲得車輛與地邊的距離，判斷是否到達地頭，如圖3所示。M為車輛位置，dis為車輛從M到地邊AB的距離，C為M到AB所在直線的垂足。點到線段的距離可以通過計算向量點積獲得。

圖3 計算水稻直播機與地邊距離的3種情況Fig.3 Three conditions for calculating distance between rice seeder and border of field

在控制算法識別車輛到達地頭后，車輛自動執行調頭動作。調頭時的控制算法根據作業要求的作業線間距dgap得到轉向半徑R為

(17)

結合式(2)得前輪轉角δ為

(18)

根據式(18)計算的車輛轉角執行轉向。轉向過程中，當車輛相較于下一條作業線的橫向偏差絕對值和航向角偏差絕對值小于預設的閾值時，認為車輛已完成調頭，結束調頭轉向過程，重新執行式(16)的直線路徑跟蹤算法。

3 算法仿真

通過1.2節建立的農機運動學模型，建立如圖4所示的算法仿真模型。

圖4 算法仿真模型Fig.4 Algorithm simulation model

圖中的控制算法模塊內即為預設的路徑跟蹤算法，d為橫向偏差，fi為航向角偏差，y為輸出的前輪轉角。分別對本文的路徑跟蹤算法、純追蹤算法和PD控制算法進行仿真。仿真時，初始橫向偏差均設為0.5 m,車輛速度設為1.5 m/s。文中k1取1，k2取3，ki取0.05；純追蹤算法預瞄距離Ld取2 m；PD控制Kp取1.2，Kd取0.8。仿真結果如圖5所示。

圖5 仿真結果Fig.5 Simulation results

由仿真結果可見，本文算法能迅速響應消除橫向偏差，超調小，穩定性好。相較于傳統的純追蹤算法和PD控制算法，本文算法超調量減小30%，穩定性更好。

4 實驗驗證

4.1 實驗平臺

實驗平臺為上海世達爾公司的洋馬VP6型水稻直播機，如圖6所示。該平臺搭載了由北斗GNSS定位系統、自動轉向系統、油門控制系統、導航主控制器、慣性姿態檢測模塊、轉向角檢測模塊等組成的自主駕駛控制系統。主控制器為一塊800 MHz的ARM Cortex-A8控制器(AM335X型，美國德州儀器公司)，通過CAN總線和串口總線與其他傳感器模塊、執行模塊通信；油門控制系統和轉向控制系統分別采用STM8作為次級控制器，響應主控命令；用戶通過基于Android的用戶終端遠程操控和監測水稻直播機。系統架構如圖7所示。

圖6 實驗平臺Fig.6 Experiment platform

圖7 自動駕駛系統架構Fig.7 Framework of autonomous system

為測試本文提出的自動駕駛控制方法的性能，分別在水泥地面以及水田環境下對本文算法進行實驗驗證。

4.2 實驗參數

車輛軸距L為1.05 m，天線安裝高度H為2 m。經過參數整定，文中Iint取車輛過去20 s內的橫向偏差的積分，k1取1，k2取2.26，ki取0.05。本文實驗地頭自動調頭閾值D取2.9 m，重新上線橫向偏差絕對值閾值取0.3 m,航向角偏差絕對值閾值取30°。

4.3 水泥地面實驗

將水稻直播機的速度設定為0.8 m/s，在水泥路面上進行直線行走實驗，跟蹤直線的橫向偏差如圖8所示。水稻直播機在水泥路面上的直線跟蹤橫向偏差絕對值最大為0.026 3 m，平均絕對偏差為0.006 9 m，跟蹤精度較高。

圖8 水泥路面實驗的橫向偏差曲線Fig.8 Deviation curve of path tracking on cement road

4.4 水田實驗

盡管水泥地實驗表明本文的控制方法具有較高的精度，但是仍然需要水田實驗來測試控制算法的有效性。

在實驗前先用手持式定位儀將作業水田邊界點的位置信息保存在數據庫中，并加載到用戶終端里。啟動自動駕駛前，用戶終端根據作業要求將作業路徑規劃成間隔為2.5 m的平行直線段。在水田實驗中，車輛速度設為0.8 m/s，初始橫向偏差和航向角偏差為零。實驗水田并不平整，車身常有傾斜。經過實驗，車輛實際行走軌跡如圖9所示。圖9中點劃線為地邊界，實線為車輛實際行走軌跡。右上角為車輛自動駕駛的起點，右下角為終點。圖10為用戶終端界面實時顯示的行走軌跡。

圖9 水田實驗車輛行駛軌跡Fig.9 Path of rice seeder in paddy field

從圖9和圖10中車輛的行走軌跡可以看到，車輛可在地頭自動調頭進入下一條作業直線段。

圖10 用戶終端界面Fig.10 Interface of user terminal

水田實驗的所有直線段橫向偏差數據如圖11所示。實際路徑跟蹤軌跡的效果如圖12所示。各直線段定義為啟動自主駕駛后或調頭后第1次偏差小于0.05 m后到下一次調頭前的這段路徑。水田實驗中所有直線段路徑跟蹤的偏差數據如表2所示。從表2可知，每條直線段的橫向偏差均方根不超過0.041 m，總體橫向偏差均方根為0.035 m。每條直線段的平均橫向絕對偏差不超過0.032 m，總體平均橫向絕對偏差為0.027 m。航向角平均絕對偏差為0.027°。

圖11 所有直線段的橫向偏差Fig.11 Lateral deviations of all straight paths

圖12 車輛水田跟蹤效果Fig.12 Paths tracking of rice seeder in paddy field

表2 水田實驗中所有直線段路徑跟蹤偏差Tab.2 Summary of deviation of path tracking in paddy field

表3為橫向偏差數據中小于0.05 m和0.10 m的橫向偏差占所有數據的比例。從表3可知，每段直線的橫向偏差絕對值都有不小于76.8%的比例在0.05 m以內，不小于98.4%的橫向偏差絕對值在0.10 m內。在整個實驗中，有85.8%的橫向偏差絕對值在0.05 m以內，99.5%的橫向偏差絕對值在0.10 m以內。其中，第1條、第5條、第6條和第8條直線段，路徑跟蹤的所有橫向偏差絕對值小于0.10 m。圖13為各段作業線橫向偏差分布圖，縱坐標為這段路徑橫向偏差所在各偏差范圍的頻率。水田實驗結果表明，本文的自動駕駛方法在水田的直線跟蹤上具有較好的跟蹤精度，可以完成自動駕駛作業的路徑跟蹤要求。

表3 橫向偏差小于0.05 m和0.1 m的數據比例Tab.3 Ratio of lateral deviation less than 0.05 m and 0.1 m %

圖13 各段直線的橫向偏差分布Fig.13 Distribution of lateral deviation of each path

5 結束語

將橫向偏差和航向角偏差非線性組合作為反饋，結合積分環節控制方向盤轉角實現自動駕駛轉向。通過實時計算水稻直播機位置到地邊界的距離實現地頭自動調頭切換作業線。為了提高定位精度，通過姿態傳感器獲取車身傾角對定位數據做了修正。水泥路面實驗結果表明，本文算法在直線跟蹤時橫向偏差絕對值最大為0.026 3 m，平均絕對偏差僅0.006 9 m。在不平整水田的實驗結果表明，本文算法跟蹤路徑的平均橫向絕對偏差總體僅0.027 m，85.8%的橫向偏差絕對值小于0.05 m，99.5%的橫向偏差絕對值小于0.10 m。本文提出的方法在不平整泥濘水田環境中具有較好的魯棒性，能實現對路徑精準的跟蹤和地頭自動調頭切換作業線，有效地改善了水稻直播機在不平整泥濘水田等惡劣環境下自動化作業的精度和效率。