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

(1.上海交通大學(xué)機(jī)械與動(dòng)力工程學(xué)院， 上海 200240； 2.山東五征集團(tuán)， 日照 262399)

0 引言

隨著農(nóng)業(yè)機(jī)械化和自動(dòng)化的不斷進(jìn)步，精準(zhǔn)農(nóng)業(yè)也得到了顯著的發(fā)展。無人農(nóng)機(jī)的發(fā)展降低了農(nóng)業(yè)生產(chǎn)成本、提高了質(zhì)量并且減少了人工投入[1]。自主駕駛是無人農(nóng)機(jī)核心技術(shù)之一。自主駕駛系統(tǒng)使得農(nóng)機(jī)無需駕駛員控制方向盤，并且可以長時(shí)間工作，不會(huì)因?yàn)槠v而降低作業(yè)質(zhì)量[2]。因此，相對(duì)駕駛員，自主駕駛系統(tǒng)在長時(shí)間工作時(shí)可以更高精度地完成作業(yè)任務(wù)，提高作業(yè)質(zhì)量，并且可以降低人力成本。隨著計(jì)算機(jī)和傳感器技術(shù)的進(jìn)步，農(nóng)機(jī)領(lǐng)域的自主駕駛系統(tǒng)獲得了長足的發(fā)展且成為研究熱點(diǎn)之一[3]。對(duì)于工作在水田等非結(jié)構(gòu)化環(huán)境下的水稻直播機(jī)等農(nóng)機(jī)裝備而言，在非平坦且泥濘易滑的水田環(huán)境里實(shí)現(xiàn)高精度的路徑跟蹤和自動(dòng)駕駛具有重要意義。

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

為實(shí)現(xiàn)水稻直播機(jī)在水田環(huán)境下較高精度的自動(dòng)駕駛，提出一種在不平坦泥濘水田環(huán)境中的水稻直播機(jī)自動(dòng)駕駛控制方法，以實(shí)現(xiàn)直線段的高精度路徑跟蹤和地頭自動(dòng)轉(zhuǎn)向。

1 運(yùn)動(dòng)學(xué)模型

1.1 模型

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

圖1 車輛運(yùn)動(dòng)學(xué)模型Fig.1 Kinematic model of vehicle

參數(shù)含意δ前輪轉(zhuǎn)角,也是控制變量ω轉(zhuǎn)向角速度θ航向角θp期望路徑P在點(diǎn)T處切線的航向角L水稻直播機(jī)軸距(x,y)車輛后軸中心點(diǎn)坐標(biāo),y軸正向?yàn)檎狈较騰車輛速度P車輛跟蹤的期望路徑d橫向偏差φ航向角偏差R車輛轉(zhuǎn)彎半徑M(xm, ym)車輛后軸中點(diǎn)坐標(biāo)T(xt, yt)路徑P上距M最近的點(diǎn)坐標(biāo)

1.2 農(nóng)機(jī)運(yùn)動(dòng)學(xué)模型

(1)

(2)

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

(3)

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

(4)

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

(5)

因此，水稻直播機(jī)的運(yùn)動(dòng)學(xué)模型為

(6)

水稻直播機(jī)的運(yùn)動(dòng)狀態(tài)可以用四維向量描述為

s=(x,y,θ,δ)

(7)

2 自動(dòng)駕駛控制方法

水稻直播機(jī)的自動(dòng)駕駛控制方法包括在作業(yè)段的直線路徑跟蹤算法和地頭處的自動(dòng)轉(zhuǎn)彎算法。在直線段應(yīng)盡量提高跟蹤精度，在地頭轉(zhuǎn)彎時(shí)應(yīng)盡量提高速度。因?yàn)樗锟涌油萃?，不平坦的路面?huì)導(dǎo)致車身傾斜，給農(nóng)機(jī)定位帶來一定的誤差。為消除這種影響，需要根據(jù)車身的傾角對(duì)定位數(shù)據(jù)作出修正。

2.1 不平坦路面的定位修正

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

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

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

elat=Hsinα

(8)

elon=Hsinψ

(9)

因此修正后的車輛位置M的坐標(biāo)為

(10)

式中 (xm,ym)——修正后的M點(diǎn)坐標(biāo)

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

橫向偏差d的計(jì)算方法為

(11)

設(shè)θp為期望路徑P在點(diǎn)T處切線的航向角，則航向角偏差φ為

φ=θp-θ

(12)

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

δφ=φ

(13)

消除橫向偏差d的前輪轉(zhuǎn)角控制量δd為一個(gè)非線性反饋控制，為

(14)

其中

dfront=d-Lsinθ

(15)

式中k2——可調(diào)參數(shù)

dfront——前輪的橫向偏差

通過對(duì)控制算法添加對(duì)橫向偏差和前進(jìn)速度的非線性反饋環(huán)節(jié)，通過反正切變換對(duì)反饋輸出限幅，同時(shí)在速度較快時(shí)減小前輪轉(zhuǎn)角輸出，可以有效減小超調(diào)量，提高穩(wěn)定性。

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

(16)

其中Iint為減小靜態(tài)誤差的積分項(xiàng)，為橫向偏差d在過去一段時(shí)間的積分；k1、k2和ki為轉(zhuǎn)向算法中根據(jù)實(shí)際情況可調(diào)的參數(shù)，代表各項(xiàng)對(duì)轉(zhuǎn)向角影響的比重。

2.3 地頭自動(dòng)調(diào)頭算法

地頭自動(dòng)調(diào)頭的關(guān)鍵在于識(shí)別是否到達(dá)地頭?？梢灶A(yù)先通過手持式定位儀標(biāo)定作業(yè)地塊的邊界關(guān)鍵點(diǎn)，建立作業(yè)地塊的數(shù)據(jù)庫，從數(shù)據(jù)庫獲得的邊界關(guān)鍵點(diǎn)計(jì)算出作業(yè)地塊的邊界位置信息。實(shí)時(shí)計(jì)算水稻直播機(jī)位置與地邊界的距離，在小于閾值D時(shí)認(rèn)為車輛到達(dá)地頭，自動(dòng)執(zhí)行調(diào)頭動(dòng)作。

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

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

在控制算法識(shí)別車輛到達(dá)地頭后，車輛自動(dòng)執(zhí)行調(diào)頭動(dòng)作。調(diào)頭時(shí)的控制算法根據(jù)作業(yè)要求的作業(yè)線間距dgap得到轉(zhuǎn)向半徑R為

(17)

結(jié)合式(2)得前輪轉(zhuǎn)角δ為

(18)

根據(jù)式(18)計(jì)算的車輛轉(zhuǎn)角執(zhí)行轉(zhuǎn)向。轉(zhuǎn)向過程中，當(dāng)車輛相較于下一條作業(yè)線的橫向偏差絕對(duì)值和航向角偏差絕對(duì)值小于預(yù)設(shè)的閾值時(shí)，認(rèn)為車輛已完成調(diào)頭，結(jié)束調(diào)頭轉(zhuǎn)向過程，重新執(zhí)行式(16)的直線路徑跟蹤算法。

3 算法仿真

通過1.2節(jié)建立的農(nóng)機(jī)運(yùn)動(dòng)學(xué)模型，建立如圖4所示的算法仿真模型。

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

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

圖5 仿真結(jié)果Fig.5 Simulation results

由仿真結(jié)果可見，本文算法能迅速響應(yīng)消除橫向偏差，超調(diào)小，穩(wěn)定性好。相較于傳統(tǒng)的純追蹤算法和PD控制算法，本文算法超調(diào)量減小30%，穩(wěn)定性更好。

4 實(shí)驗(yàn)驗(yàn)證

4.1 實(shí)驗(yàn)平臺(tái)

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

圖6 實(shí)驗(yàn)平臺(tái)Fig.6 Experiment platform

圖7 自動(dòng)駕駛系統(tǒng)架構(gòu)Fig.7 Framework of autonomous system

為測(cè)試本文提出的自動(dòng)駕駛控制方法的性能，分別在水泥地面以及水田環(huán)境下對(duì)本文算法進(jìn)行實(shí)驗(yàn)驗(yàn)證。

4.2 實(shí)驗(yàn)參數(shù)

車輛軸距L為1.05 m，天線安裝高度H為2 m。經(jīng)過參數(shù)整定，文中Iint取車輛過去20 s內(nèi)的橫向偏差的積分，k1取1，k2取2.26，ki取0.05。本文實(shí)驗(yàn)地頭自動(dòng)調(diào)頭閾值D取2.9 m，重新上線橫向偏差絕對(duì)值閾值取0.3 m,航向角偏差絕對(duì)值閾值取30°。

4.3 水泥地面實(shí)驗(yàn)

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

圖8 水泥路面實(shí)驗(yàn)的橫向偏差曲線Fig.8 Deviation curve of path tracking on cement road

4.4 水田實(shí)驗(yàn)

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

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

圖9 水田實(shí)驗(yàn)車輛行駛軌跡Fig.9 Path of rice seeder in paddy field

從圖9和圖10中車輛的行走軌跡可以看到，車輛可在地頭自動(dòng)調(diào)頭進(jìn)入下一條作業(yè)直線段。

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

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

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

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

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

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

表3 橫向偏差小于0.05 m和0.1 m的數(shù)據(jù)比例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 結(jié)束語

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