吳才聰，吳思賢，文 龍，陳智博，楊衛(wèi)中，翟衛(wèi)欣

·農(nóng)業(yè)裝備工程與機(jī)械化·

拖拉機(jī)自動(dòng)導(dǎo)航變曲度路徑跟蹤控制

吳才聰，吳思賢，文 龍，陳智博，楊衛(wèi)中，翟衛(wèi)欣

（1. 中國(guó)農(nóng)業(yè)大學(xué)信息與電氣工程學(xué)院，北京 100083； 2. 農(nóng)業(yè)農(nóng)村部農(nóng)機(jī)作業(yè)監(jiān)測(cè)與大數(shù)據(jù)應(yīng)用重點(diǎn)實(shí)驗(yàn)室，北京 100083）

針對(duì)當(dāng)前拖拉機(jī)自動(dòng)導(dǎo)航曲線跟蹤控制精度不能滿足生產(chǎn)需要的問題，該研究提出一種基于前輪轉(zhuǎn)角前饋補(bǔ)償策略的變曲度路徑跟蹤控制方法。綜合考慮農(nóng)機(jī)作業(yè)速度和目標(biāo)路徑曲度對(duì)前視距離的影響，通過調(diào)整前視區(qū)域和計(jì)算預(yù)瞄點(diǎn)，動(dòng)態(tài)調(diào)整前視距離和前輪轉(zhuǎn)角前饋量，在追蹤預(yù)瞄點(diǎn)的過程中，利用農(nóng)機(jī)與目標(biāo)路徑偏差設(shè)計(jì)變曲度路徑跟蹤模糊控制器，通過實(shí)時(shí)調(diào)整拖拉機(jī)前輪轉(zhuǎn)角補(bǔ)償量減小穩(wěn)態(tài)誤差。以DF2204無級(jí)變速拖拉機(jī)為試驗(yàn)平臺(tái)，設(shè)計(jì)并研發(fā)了自動(dòng)導(dǎo)航系統(tǒng)，開展21組變曲度路徑跟蹤控制試驗(yàn)。試驗(yàn)結(jié)果表明，拖拉機(jī)以1.0、1.5、2.0和3.0 m/s速度行駛時(shí)的平均絕對(duì)誤差的平均值分別為2.7、2.7、3.3和4.0 cm，均方根誤差的平均值分別為3.4、3.7、4.6和5.0 cm，滿足農(nóng)業(yè)生產(chǎn)需求。所提方法可有效提高農(nóng)機(jī)曲線路徑跟蹤精度，減少漏耕，提高農(nóng)田利用率。

拖拉機(jī)；自動(dòng)導(dǎo)航；路徑跟蹤；變曲度；前視距離；模糊控制

0 引 言

農(nóng)機(jī)自動(dòng)導(dǎo)航技術(shù)可有效提高農(nóng)機(jī)作業(yè)精度與作業(yè)效率[1-3]。目前，農(nóng)機(jī)自動(dòng)導(dǎo)航的直線跟蹤精度可達(dá)到±2.5 cm，滿足農(nóng)機(jī)直線路徑作業(yè)要求，但在曲線路徑特別是變曲度曲線路徑作業(yè)方面，由于路徑曲率變化導(dǎo)致跟蹤效果較差[4]，會(huì)造成明顯的漏耕，導(dǎo)致土地浪費(fèi)。不規(guī)則農(nóng)田和坡耕地分布較廣[5]，亟需提高農(nóng)機(jī)變曲度路徑跟蹤控制精度以適應(yīng)機(jī)械無人化生產(chǎn)。

路徑跟蹤控制是實(shí)現(xiàn)農(nóng)機(jī)自動(dòng)導(dǎo)航技術(shù)的關(guān)鍵[6]，國(guó)內(nèi)外已有眾多學(xué)者開展了大量研究，其中純追蹤控制算法（Pure Pursuit，PP）是一種模擬人工駕駛過程的幾何路徑跟蹤控制方法，是農(nóng)業(yè)機(jī)械自動(dòng)導(dǎo)航技術(shù)中常用的控制算法。前視距離作為純追蹤算法的唯一輸入?yún)?shù)，對(duì)農(nóng)機(jī)的路徑跟蹤效果起到至關(guān)重要的作用。Netto等[7]分析了前視距離與車速的關(guān)系，提出了前視距離與車速的二次函數(shù)方程，但在農(nóng)業(yè)場(chǎng)景下農(nóng)機(jī)作業(yè)速度較低，車速與前視距離的耦合性較弱，對(duì)不同速度的農(nóng)機(jī)作業(yè)任務(wù)適應(yīng)性較差。黃沛琛等[8]使用BP神經(jīng)網(wǎng)絡(luò)實(shí)現(xiàn)動(dòng)態(tài)調(diào)整前視距離改進(jìn)純追蹤算法，仿真驗(yàn)證方法可行，但該方法需要大量的訓(xùn)練樣本，需要對(duì)不同種類農(nóng)機(jī)的起始位姿樣本進(jìn)行訓(xùn)練，難以在田內(nèi)實(shí)際應(yīng)用，具有一定局限性。唐小濤等[9]提出了一種基于模糊自適應(yīng)純追蹤模型的農(nóng)業(yè)機(jī)械直線路徑跟蹤方法，以橫向誤差和速度作為模糊控制器的輸入，前視距離作為輸出，但該方法忽略了前方目標(biāo)路徑曲度對(duì)車輛行駛狀態(tài)的影響，跟蹤誤差大，很難快速修正。Nagasaka等[10]在固定轉(zhuǎn)彎半徑的前提下，利用航向誤差實(shí)現(xiàn)了農(nóng)機(jī)的地頭轉(zhuǎn)彎路徑跟蹤，但該方法由于決策量較少，在直線與掉頭曲線銜接處出現(xiàn)超調(diào)現(xiàn)象，不適用于曲度多變的曲線路徑跟蹤。張華強(qiáng)等[11]通過粒子群算法（Particle Swarm Algorithm，PSO）根據(jù)實(shí)時(shí)橫向跟蹤誤差動(dòng)態(tài)確定前視距離，提高了拖拉機(jī)自動(dòng)導(dǎo)航的直線跟蹤精度，但不適合曲線作業(yè)。陳無畏等[12]構(gòu)建了速度和前視距離與路徑彎曲度之間的二次函數(shù)，但忽略了速度與前視距離之間的耦合關(guān)系。李革等[13]針對(duì)曲線路徑跟蹤對(duì)純追蹤算法進(jìn)行改進(jìn)，提出了前視距離與速度等變量的線性關(guān)系，插秧機(jī)以1 m/s速度行駛時(shí)，曲線平均跟蹤誤差為9.6 cm；Yang等[14]提出一種基于最優(yōu)預(yù)瞄點(diǎn)的路徑跟蹤控制方法，拖拉機(jī)以1 m/s行駛時(shí)，曲線平均跟蹤誤差為6.7 cm，均不滿足精準(zhǔn)作業(yè)要求[15]。

為了提高拖拉機(jī)自動(dòng)導(dǎo)航技術(shù)在不規(guī)則田塊或坡耕地行駛時(shí)的變曲度路徑跟蹤精度，本文提出一種基于前輪轉(zhuǎn)角前饋補(bǔ)償策略的拖拉機(jī)自動(dòng)導(dǎo)航變曲度路徑跟蹤控制方法，通過改進(jìn)純追蹤算法，綜合考慮拖拉機(jī)行駛速度與目標(biāo)路徑彎曲度動(dòng)態(tài)調(diào)整前視距離，利用拖拉機(jī)與目標(biāo)路徑的實(shí)時(shí)位置誤差設(shè)計(jì)模糊控制器，通過模糊控制算法對(duì)前輪轉(zhuǎn)角進(jìn)行補(bǔ)償，并通過田間試驗(yàn)驗(yàn)證該方法的有效性。

1 材料與方法

1.1 拖拉機(jī)運(yùn)動(dòng)學(xué)建模

本研究采用的東風(fēng)DF2204拖拉機(jī)為前輪轉(zhuǎn)向、四輪驅(qū)動(dòng)。假設(shè)拖拉機(jī)在田內(nèi)或機(jī)耕道的路徑跟蹤過程中行駛速度較低，行駛路面起伏程度較小，轉(zhuǎn)向車輪轉(zhuǎn)角一致，忽略輪胎的側(cè)偏特性，車輛航向角速度即為車輛橫擺角速度，則可將拖拉機(jī)簡(jiǎn)化為二輪車模型。以正北方向?yàn)檩S正方向，正東方向?yàn)檩S正方向建立慣性坐標(biāo)系，得到圖1所示拖拉機(jī)在慣性坐標(biāo)系下的運(yùn)動(dòng)學(xué)模型。

注：δ1為前輪轉(zhuǎn)角，(°)；θ為車輛航向角，(°)；A為車輛前輪中心；B為車輛后輪中心；v為車輛速度，m·s-1；L為車輛軸距，m；R為轉(zhuǎn)向半徑，m；α為車輛前視角，(°)；Ld為車輛后軸中心與預(yù)瞄點(diǎn)的距離，m。

在慣性坐標(biāo)系中，(x，y)為車輛二輪車模型前輪坐標(biāo)，(x，y)為車輛二輪車模型后輪坐標(biāo)。由圖1幾何關(guān)系可得二輪車模型前輪轉(zhuǎn)角為

考慮前后輪幾何約束及運(yùn)動(dòng)學(xué)約束可得拖拉機(jī)的運(yùn)動(dòng)學(xué)模型為

1.2 純追蹤控制器設(shè)計(jì)

1.2.1 純追蹤控制算法

純追蹤控制算法是移動(dòng)機(jī)器人典型的橫向控制方法。該方法以二輪車模型為基礎(chǔ)，以車輛后軸中心為切點(diǎn)、車輛縱向方向?yàn)榍芯€，通過調(diào)整前輪轉(zhuǎn)角1，使車輛沿著一條圓弧軌跡線到達(dá)預(yù)瞄點(diǎn)。

根據(jù)圖1幾何關(guān)系有：

通過前視距離L確定前視角，進(jìn)而由公式（3）計(jì)算出前輪轉(zhuǎn)角1。前視距離是純追蹤控制算法的一個(gè)重要參數(shù)，前視距離設(shè)置是否合理直接影響路徑跟蹤水平。前視距離設(shè)置過長(zhǎng)，跟蹤軌跡會(huì)更平滑，但在曲線轉(zhuǎn)向處會(huì)存在轉(zhuǎn)向不足或轉(zhuǎn)向過度的情況；前視距離設(shè)置過小，車輛會(huì)在短時(shí)間內(nèi)根據(jù)預(yù)瞄點(diǎn)頻繁調(diào)整前輪轉(zhuǎn)角，影響車輛行駛的穩(wěn)定性。前視距離L可表示為與車速相關(guān)的線性函數(shù)：

式中Lmin為最小前視距離，m；min為最小車速，m/s；為前視距離系數(shù)。

公式（4）將速度作為調(diào)整前視距離的直接因素，而拖拉機(jī)在田間機(jī)耕道或不規(guī)則田塊作業(yè)時(shí)，目標(biāo)路徑崎嶇多彎，車輛常以較低速度行駛，僅依賴車速而忽略行駛道路的曲度對(duì)前視距離的影響，對(duì)目標(biāo)路徑的跟蹤不夠精確。

基于已有研究[16-19]，以公式（4）為基礎(chǔ)，本文提出綜合考慮目標(biāo)路徑彎曲度和車輛自身行駛位姿等因素確定前視距離：先根據(jù)農(nóng)機(jī)的參數(shù)、實(shí)時(shí)位置、車速等確定前視區(qū)域，然后對(duì)前視區(qū)域內(nèi)所有路徑點(diǎn)的曲度求和表征目標(biāo)路徑的彎曲程度，根據(jù)彎曲程度計(jì)算預(yù)瞄點(diǎn)，從而確定前視距離L和前視角，最后基于純追蹤控制算法計(jì)算出前輪轉(zhuǎn)角前饋量。

1.2.2 前視區(qū)域及預(yù)瞄點(diǎn)的確定

駕駛員在駕車行駛時(shí)，會(huì)注視目標(biāo)路徑的一段范圍，這段范圍被稱為前視區(qū)域（圖2）。

注：S(xs,ys)為前視區(qū)域起點(diǎn)；E(xe,ye)為前視區(qū)域終點(diǎn)；Lsum為前視區(qū)域積分長(zhǎng)，m；Ldmin為最小前視距離，m。

前視區(qū)域起點(diǎn)(x，y)由最短前視距離Lmin確定，為拖拉機(jī)前進(jìn)方向上與農(nóng)機(jī)后輪中心距離大于Lmin的最近路徑點(diǎn)。前視區(qū)域的長(zhǎng)度L將根據(jù)當(dāng)前車速進(jìn)行動(dòng)態(tài)調(diào)整。車速越快、道路越平緩，前視區(qū)域越廣，相反前視區(qū)域越小，結(jié)合上述分析，并參考文獻(xiàn)[20]，提出前視區(qū)域積分長(zhǎng)度的計(jì)算公式為

式中min_sum為前視區(qū)域的最小積分長(zhǎng)，m；max_sum為前視區(qū)域的最大積分長(zhǎng)，m；，為預(yù)設(shè)常數(shù)，根據(jù)實(shí)車試驗(yàn)確定。

預(yù)瞄點(diǎn)是駕駛員在前視區(qū)域內(nèi)選取的要追蹤的路徑點(diǎn)。結(jié)合上文分析可知，純追蹤控制算法跟蹤性能的關(guān)鍵在于預(yù)瞄點(diǎn)的選擇。研究表明[21]，目標(biāo)路徑彎曲程度越大，前視距離越小，由此可得預(yù)瞄點(diǎn)應(yīng)更靠近前進(jìn)方向前視區(qū)域起點(diǎn)的位置。根據(jù)這一思想，確定預(yù)瞄點(diǎn)首先要確定目標(biāo)路徑彎曲度。目標(biāo)路徑彎曲度的計(jì)算原理圖見圖3。

注：M(xm,ym)、Q(xq,yq)和P(xp,yp)為前視區(qū)域內(nèi)相鄰的3個(gè)路徑點(diǎn)；βi是MQ相對(duì)于QP的夾角，(°)。

將前視區(qū)域的目標(biāo)路徑離散成若干個(gè)路徑點(diǎn)，用折線將相鄰路徑點(diǎn)連接，相鄰折線的夾角β為相鄰路徑點(diǎn)方向角的改變量，前視區(qū)域的路徑彎曲度定義為

式中為前視區(qū)域內(nèi)的折線數(shù)量，數(shù)值由前視區(qū)域內(nèi)離散路徑點(diǎn)的數(shù)量確定。

相鄰折線間的夾角β由公式（7）計(jì)算：

前視距離L隨路徑彎曲度變化而變化，即預(yù)瞄點(diǎn)在前視區(qū)域內(nèi)距前視區(qū)域起點(diǎn)的曲線長(zhǎng)度L隨目標(biāo)路徑彎曲度變化而變化：

式中min、max為的最小和最大值，為預(yù)設(shè)常數(shù)。

為了確保預(yù)瞄點(diǎn)隨路徑彎曲度變化而連續(xù)變化，的取值應(yīng)與L的最大值相對(duì)應(yīng)，則有

L確定后便可在前視區(qū)域內(nèi)確定預(yù)瞄點(diǎn)，進(jìn)而確定前視距離L與車輛前視角，再根據(jù)公式（3），即可得出車輛前輪轉(zhuǎn)角前饋量1。

1.3 模糊控制器設(shè)計(jì)

由于純追蹤控制算法在追蹤預(yù)瞄點(diǎn)的過程中沒有考慮農(nóng)機(jī)當(dāng)前位姿與參考路徑的誤差，可能會(huì)導(dǎo)致農(nóng)機(jī)行駛過程中存在穩(wěn)態(tài)誤差，因此需要進(jìn)一步改進(jìn)跟蹤控制算法。本研究設(shè)計(jì)一種基于模糊控制算法的模糊控制器實(shí)現(xiàn)對(duì)農(nóng)機(jī)前輪轉(zhuǎn)角的補(bǔ)償，既考慮車輛的當(dāng)前位置誤差，又考慮前方目標(biāo)路徑彎曲度對(duì)轉(zhuǎn)向的影響，進(jìn)而提升跟蹤精度。本文選用二維模糊控制器確定前輪轉(zhuǎn)角補(bǔ)償量δ：先計(jì)算農(nóng)機(jī)實(shí)時(shí)位置與目標(biāo)路徑的橫向誤差與航向誤差作為模糊控制器的輸入量，模糊控制器對(duì)輸入量進(jìn)行模糊化處理，利用模糊規(guī)則進(jìn)行模糊推理，最后進(jìn)行反模糊化輸出前輪轉(zhuǎn)角補(bǔ)償量δ。

前輪轉(zhuǎn)角δ、前輪轉(zhuǎn)角前饋量1與前輪轉(zhuǎn)角補(bǔ)償量δ滿足以下關(guān)系：

1.3.1 輸入輸出變量模糊化

以橫向誤差e和航向誤差e作為模糊控制器的輸入，前輪轉(zhuǎn)角補(bǔ)償量δ作為輸出。首先對(duì)輸入、輸出變量進(jìn)行模糊化處理。其中e、e、δ的論域由試驗(yàn)確定。

1）橫向誤差e?；菊撚?yàn)閇?12 cm，12 cm]，量化等級(jí)為{?12 cm，?8 cm，?4 cm，0 cm，4 cm，8 cm，12 cm}={NB，NM，NS，ZO，PS，PM，PB}，量化因子取1。

2）航向誤差e。基本論域?yàn)閇?6°，6°]，量化等級(jí)為{?6°，?4°，?2°，0°，2°，4°，6°}={NB，NM，NS，ZO，PS，PM，PB}，量化因子取1。

3）前輪轉(zhuǎn)角補(bǔ)償量δ?；菊撚?yàn)閇?3.6°，4.8°]，量化等級(jí)為{?3.6°，?2.4°，?1.2°，0°，1.2°，2.4°，3.6°}={NB，NM，NS，ZO，PS，PM，PB}，量化因子取1。

本研究中輸入輸出量的隸屬函數(shù)均選擇三角隸屬度函數(shù)。橫向誤差e與航向誤差e計(jì)算方法[22]如圖4所示。

注：R(xr,yr)為目標(biāo)路徑點(diǎn)中距離車輛后輪中心的最近點(diǎn)；T(xt,yt)為目標(biāo)路徑點(diǎn)中距離車輛后輪中心的次近點(diǎn)；φ為RT與正北方向的順時(shí)針夾角，(°)；ec為橫向誤差，cm；eh為航向誤差，(°)；

車輛后輪中心到線的距離即為農(nóng)機(jī)行駛過程中的橫向誤差e，根據(jù)式（11）計(jì)算。

1.3.2 模糊控制規(guī)則設(shè)計(jì)

模糊控制規(guī)則是將人工駕駛策略以及專家知識(shí)經(jīng)驗(yàn)，利用模糊集合理論和語言變量轉(zhuǎn)化為數(shù)值運(yùn)算，進(jìn)而利用計(jì)算機(jī)實(shí)現(xiàn)模糊自動(dòng)控制[23]。

前輪轉(zhuǎn)角自動(dòng)補(bǔ)償模糊控制規(guī)則的基本原則為：當(dāng)農(nóng)機(jī)與目標(biāo)路徑的橫向誤差為負(fù)值較大、航向誤差為負(fù)值較大時(shí)，前輪轉(zhuǎn)角補(bǔ)償量應(yīng)輸出為正值較大。當(dāng)農(nóng)機(jī)與目標(biāo)路徑的橫向誤差為正值較大、航向誤差為正值較大時(shí)，前輪轉(zhuǎn)角補(bǔ)償量應(yīng)輸出為負(fù)值較大。根據(jù)農(nóng)機(jī)不同的位置狀態(tài)共得出49條控制規(guī)則，見表1。

表1 前輪轉(zhuǎn)角補(bǔ)償量模糊控制規(guī)則

注：NB為負(fù)大；NM為負(fù)中；NS為負(fù)小；ZO為零；PS為正小；PM為正中；PB為正大；模糊語言描述為：橫向誤差為正大（PB）或負(fù)大（NB），航向誤差為正大（PB）或者負(fù)大（NB），那么前輪轉(zhuǎn)角為負(fù)大（NB）或者正大（PB）。

Note: NB is negative big; NM is negative middle; NS is negative small; ZO is zero; PS is positive small; PM is positive middle; PB is positive big; The fuzzy language is described as: if the lateral error is positive big (PB) or negative big (NB), and the heading error is positive big (PB) or negative big (NB), then the front wheel rotation angle is negative big (NB) or positive big (PB).

2 算法驗(yàn)證

2.1 試驗(yàn)平臺(tái)

為驗(yàn)證變曲度路徑跟蹤算法，以DF2204無級(jí)變速拖拉機(jī)為試驗(yàn)平臺(tái)開展導(dǎo)航試驗(yàn)。試驗(yàn)地點(diǎn)為北京市密云區(qū)河南寨。

1.導(dǎo)航天線 2.車載控制器 3.電動(dòng)方向盤 4.華測(cè)組合導(dǎo)航 5.角度傳感器

拖拉機(jī)為后輪驅(qū)動(dòng)，阿克曼轉(zhuǎn)向，軸距3.042 m，前輪輪距1.92 m，后輪輪距1.90 m，單邊制動(dòng)轉(zhuǎn)向半徑5 m，前輪內(nèi)側(cè)最大轉(zhuǎn)角50°。工控機(jī)等車載控制器通過控制局域網(wǎng)（Controller Area Network，CAN）控制發(fā)動(dòng)機(jī)、變速箱及液壓提升系統(tǒng)。

GNSS導(dǎo)航系統(tǒng)為華測(cè)CGI-610，獲取拖拉機(jī)的實(shí)時(shí)位姿信息。華測(cè)CGI-610內(nèi)置高精度MEMS陀螺儀與加速度計(jì)，可提供準(zhǔn)確的姿態(tài)和厘米級(jí)位置信息，定位精度10 mm，航向精度0.2°/（為雙天線基線長(zhǎng)），數(shù)據(jù)傳輸頻率為25 Hz。GNSS天線安裝在駕駛室頂部?jī)蓚?cè)，雙天線相位中心連線與農(nóng)機(jī)中軸線垂直。設(shè)備主機(jī)與農(nóng)機(jī)固連，安裝底面與農(nóng)機(jī)駕駛室底面平行，主機(jī)坐標(biāo)系三軸指向與農(nóng)機(jī)車體坐標(biāo)系三軸指向一致。

2.2 試驗(yàn)設(shè)置

試驗(yàn)在顛簸不平的旱地田塊開展，通過人工駕駛拖拉機(jī)在田塊邊緣采集帶有GNSS軌跡點(diǎn)序列的目標(biāo)路徑，通過數(shù)據(jù)可視化分析，將目標(biāo)路徑段分為變道曲線段、掉頭曲線段、直線段及S曲線段。采樣間隔設(shè)置為0.1 m。路徑全長(zhǎng)約為250 m。圖6為采集軌跡點(diǎn)的可視化結(jié)果。

圖6 采樣路徑

在相同曲度路徑下，車速設(shè)定為1.0 m/s，將本研究提出的控制算法與固定前視距離的純追蹤算法進(jìn)行對(duì)比試驗(yàn)。文獻(xiàn)[24]提出前視距離略大于軸距時(shí)有較好的跟蹤效果，結(jié)合試驗(yàn)農(nóng)機(jī)機(jī)身參數(shù)，對(duì)比試驗(yàn)中固定前視距離L依次設(shè)定為2.0、3.5和5.0 m。

Lmin參照農(nóng)機(jī)前輪輪距設(shè)定為2 m，min設(shè)定為目標(biāo)路徑前視區(qū)域彎曲度之和的最小值，max設(shè)定為目標(biāo)路徑前視區(qū)域彎曲度之和的最大值。經(jīng)試驗(yàn)調(diào)參min設(shè)定為0.5 m/s，max設(shè)定為4.0 m/s，min_sum與max_sum分別設(shè)定為0.8、2.2 m。根據(jù)公式（7）計(jì)算參數(shù)為0.4，為0.6。

2.3 試驗(yàn)結(jié)果與分析

圖7為農(nóng)機(jī)以1.0 m/s的速度行駛時(shí)本文控制方法與3種固定前視距離的純追蹤控制方法的跟蹤軌跡及局部放大圖。圖8為1.0 m/s速度下各控制方法的橫向誤差變化。圖9、圖10分別為農(nóng)機(jī)在1.0 m/s速度下使用本文控制方法的前視距離隨目標(biāo)路徑彎曲度的變化曲線和前輪轉(zhuǎn)角前饋量與補(bǔ)償量的變化曲線。表2、表3分別為固定前視距離的純追蹤算法和本文控制算法在1.0 m/s速度下的橫向誤差統(tǒng)計(jì)結(jié)果。

圖7 跟蹤軌跡及局部放大圖

圖8 不同控制算法的橫向誤差

圖9 前視距離隨前視區(qū)域曲度的變化

圖10 前輪轉(zhuǎn)角前饋量與補(bǔ)償量在行駛過程中的變化

由圖7、圖8可以看出，本文考慮目標(biāo)路徑彎曲度的跟蹤控制方法均優(yōu)于固定前視距離的純追蹤算法。在行駛速度為1.0 m/s的變曲度路徑跟蹤過程中，考慮目標(biāo)路徑彎曲度控制算法的平均絕對(duì)誤差為2.7 cm，最大橫向誤差為10.8 cm，均方根誤差為3.4 cm。固定前視距離為2.0、3.5和5.0 m的平均絕對(duì)誤差平均值分別為7.1、5.6和11.8 cm，最大橫向誤差平均值分別為25.2、18.9和55.7 cm，均方根誤差平均值分別為5.5、4.9和19.4 cm。試驗(yàn)結(jié)果表明，本文算法各項(xiàng)指標(biāo)均優(yōu)于固定前視距離的純追蹤控制算法，其中均方根誤差平均值較3種固定前視距離的純追蹤控制算法分別降低了38.18%、30.61%和82.47%，均降低30%以上，有效提高了路徑跟蹤精度。由圖9看出，本文算法能根據(jù)目標(biāo)路徑道路彎曲度自主調(diào)整前視距離，前視距離在1.8～3.2 m之間自適應(yīng)調(diào)整，對(duì)目標(biāo)路徑具有一定范圍內(nèi)的預(yù)測(cè)性。由圖10可以看出，在農(nóng)機(jī)行駛的全過程中，本文算法能根據(jù)實(shí)時(shí)位置誤差對(duì)前輪轉(zhuǎn)角進(jìn)行補(bǔ)償，實(shí)時(shí)調(diào)整農(nóng)機(jī)位姿，減少行駛誤差。

表2 固定前視距離的純追蹤算法跟隨變曲度路徑的橫向誤差

考慮到拖拉機(jī)在田內(nèi)作業(yè)的實(shí)際情況，不同的作業(yè)任務(wù)行駛速度不同，在1.5、2.0和3.0 m/s的速度下開展3組試驗(yàn)，橫向誤差統(tǒng)計(jì)結(jié)果如表3。

表3 不同行駛速度的橫向誤差統(tǒng)計(jì)結(jié)果

試驗(yàn)結(jié)果表明，本文提出的考慮目標(biāo)道路彎曲度的跟蹤控制算法能夠適應(yīng)多種作業(yè)速度場(chǎng)景，其中3.0 m/s速度下的最大絕對(duì)值誤差均值控制在19.9 cm，平均絕對(duì)誤差平均值為4.0 cm，均方根誤差均值為5.0 cm，表明該導(dǎo)航控制算法具有較好的導(dǎo)航效果[15]，對(duì)不同農(nóng)機(jī)作業(yè)速度有著較好的適應(yīng)性。

3 結(jié) 論

1）針對(duì)拖拉機(jī)傳統(tǒng)跟蹤控制算法在曲線路徑下作業(yè)精度低、速度適應(yīng)性較差的問題，本研究針對(duì)前輪轉(zhuǎn)角提出一種前饋-補(bǔ)償策略的路徑跟蹤控制方法，以傳統(tǒng)純追蹤控制方法為基礎(chǔ)，通過農(nóng)機(jī)行駛速度動(dòng)態(tài)調(diào)整前視區(qū)域，根據(jù)前視區(qū)域的道路彎曲度計(jì)算預(yù)瞄點(diǎn)，從而動(dòng)態(tài)確定前視距離，通過純追蹤模型計(jì)算前輪轉(zhuǎn)角前饋量，再利用模糊控制器通過農(nóng)機(jī)當(dāng)前位置的橫向和航向誤差實(shí)現(xiàn)對(duì)前輪轉(zhuǎn)角的補(bǔ)償。

2）行駛速度為1.0、1.5、2.0和3.0 m/s時(shí)的最大跟蹤誤差分別為10.8、16.8、16.4和19.9 cm，平均絕對(duì)誤差分別為2.7、2.7、3.3和4.0 cm，均方跟誤差平均值分別為3.4、3.7、4.6和5.0 cm。

[1] Lowenberg-DeBoer J, Huang I Y, Grigoriadis V, et al. Economics of robots and automation in field crop production[J]. Precision Agriculture, 2020, 21(2): 278-299.

[2] Kelc D, Stajnko D, Berk P, et al. Reduction of environmental pollution by using RTK-navigation in soil cultivation[J]. International Journal of Agricultural and Biological Engineering, 2019, 12(5): 173-178.

[3] 劉兆朋，張智剛，羅錫文，等. 雷沃ZP9500高地隙噴霧機(jī)的GNSS自動(dòng)導(dǎo)航作業(yè)系統(tǒng)設(shè)計(jì)[J]. 農(nóng)業(yè)工程學(xué)報(bào)，2018，34(1)：15-21.

Liu Zhaopeng, Zhang Zhigang, Luo Xiwen, et al. Design of automatic navigation operation system for Lovol ZP9500 high clearance boom sprayer based on GNSS[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2018, 34(1): 15-21. (in Chinese with English abstract)

[4] 徐廣飛，陳美舟，金誠(chéng)謙，等. 拖拉機(jī)自動(dòng)駕駛關(guān)鍵技術(shù)綜述[J]. 中國(guó)農(nóng)機(jī)化學(xué)報(bào)，2022，43(6)：126-134.

Xu Guangfei, Chen Meizhou, Jin Chengqian, et al. A review of key technology tractor automatic driving[J]. Journal of Chinese Agricultural Mechanization, 2022, 43(6): 126-134. (in Chinese with English abstract)

[5] Hameed I A, la Cour-Harbo A, Osen O L. Side-to-side 3D coverage path planning approach for agricultural robots to minimize skip/overlap areas between swaths[J]. Robotics and Autonomous Systems, 2016, 76: 36-45.

[6] 吳才聰，王東旭，陳智博，等. SF2104拖拉機(jī)自主行駛與作業(yè)控制方法[J]. 農(nóng)業(yè)工程學(xué)報(bào)，2020，36(18)：42-48.

Wu Caicong, Wang Dongxu, Chen Zhibo, et al. Autonomous driving and operation control method for SF2104 tractors[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2020, 36(18): 42-48. (in Chinese with English abstract)

[7] Netto M, Blosseville J M, Lusetti B, et al. A new robust control system with optimized use of the lane detection data for vehicle full lateral control under strong curvatures[C]//2006 IEEE Intelligent Transportation Systems Conference. Toronto, 2006: 1382-1387.

[8] 黃沛琛，羅錫文，張智剛. 改進(jìn)純追蹤模型的農(nóng)業(yè)機(jī)械地頭轉(zhuǎn)向控制方法[J]. 計(jì)算機(jī)工程與應(yīng)用，2010，46(21)：216-219.

Huang Peichen, Luo Xiwen, Zhang Zhigang. Control method of headland turning based on improved pure pursuit model for agricultural machine[J]. Computer Engineering and Applications, 2010, 46(21): 216-219. (in Chinese with English abstract)

[9] 唐小濤，陶建峰，李志騰，等. 自動(dòng)導(dǎo)航插秧機(jī)路徑跟蹤系統(tǒng)穩(wěn)定性模糊控制優(yōu)化方法[J]. 農(nóng)業(yè)機(jī)械學(xué)報(bào)，2018，49(1)：29-34.

Tang Xiaotao, Tao Jianfeng, Li Zhiteng, et al. Fuzzy control optimization method for stability of path tracking system of automatic transplanter[J]. Transactions of the Chinese Society for Agricultural Machinery, 2018, 49(1): 29-34. (in Chinese with English abstract)

[10] Nagasaka Y, Saito H, Tamaki K, et al. An autonomous rice transplanter guided by global positioning system and inertial measurement unit[J]. Journal of Field Robotics, 2009, 26(6/7): 537-548.

[11] 張華強(qiáng)，王國(guó)棟，呂云飛，等. 基于改進(jìn)純追蹤模型的農(nóng)機(jī)路徑跟蹤算法研究[J]. 農(nóng)業(yè)機(jī)械學(xué)報(bào)，2020，51(9)：18-25.

Zhang Huaqiang, Wang Guodong, Lü Yunfei, et al. Agricultural machinery automatic navigation control system based on improved pure tracking model[J]. Transactions of the Chinese Society for Agricultural Machinery, 2020, 51(9): 18-25. (in Chinese with English abstract)

[12] 陳無畏，李進(jìn)，王檀彬，等. 視覺導(dǎo)航智能車輛的路徑跟蹤預(yù)瞄控制[J]. 機(jī)械工程學(xué)報(bào)，2008，44(10)：277-282.

Chen Wuwei, Li Jin, Wang Tanbin, et al. Preview control for road following of vision guided intelligent vehicle[J]. Chinese Journal of Mechanical Engineering, 2008, 44(10): 277-282. (in Chinese with English abstract)

[13] 李革，王宇，郭劉粉，等. 插秧機(jī)導(dǎo)航路徑跟蹤改進(jìn)純追蹤算法[J]. 農(nóng)業(yè)機(jī)械學(xué)報(bào)，2018，49(5)：21-26.

Li Ge, Wang Yu, Guo Liufen, et al. Improved pure pursuit algorithm for rice transplanter path tracking[J]. Transactions of the Chinese Society for Agricultural Machinery, 2018, 49(5): 21-26. (in Chinese with English abstract)

[14] Yang Y, Li Y, Wen X, et al. An optimal goal point determination algorithm for automatic navigation of agricultural machinery: Improving the tracking accuracy of the pure pursuit algorithm[J]. Computers and Electronics in Agriculture, 2022, 194: 106760.

[15] Bell T. Automatic tractor guidance using carrier-phase differential GPS[J]. Computers and Electronics in Agriculture, 2000, 25(1/2): 53-66.

[16] 楊陽陽，何志剛，汪若塵，等. 基于轉(zhuǎn)角補(bǔ)償?shù)闹悄苘囕v循跡控制系統(tǒng)[J]. 儀表技術(shù)與傳感器，2019(5)：73-77.

Yang Yangyang, He Zhigang, Wang Ruochen, et al. Path tracking control system of intelligent vehicle based on steering angle compensation[J]. Instrument Technique and Sensor, 2019(5): 73-77. (in Chinese with English abstract)

[17] Wang J, Steiber J, Surampudi B. Autonomous ground vehicle control system for high-speed and safe operation[J]. International Journal of Vehicle Autonomous Systems, 2009, 7(1/2): 18-35.

[18] Shi B, Meng W, Liu H, et al. A normalized approach for evaluating driving styles based on personalized driver modeling[C]//Proceedings of SAE-China Congress 2014: Selected Papers. Berlin, Heidelberg, Springer, 2015: 433-444.

[19] Li G, Zhu F, Qu X, et al. Driving style classification based on driving operational pictures[J]. IEEE Access, 2019, 7: 90180-90189.

[20] 趙凱，朱愿，馮明月，等. 基于多點(diǎn)序列預(yù)瞄的自動(dòng)駕駛汽車路徑跟蹤算法研究[J]. 汽車技術(shù)，2018(11)：1-5.

Zhao Kai, Zhu Yuan, Feng Mingyue, et al. Research on path tracking algorithm of autonomous vehicles based on multi-point sequence preview[J]. Automobile Technology, 2018(11): 1-5. (in Chinese with English abstract)

[21] 邵明璽，辛喆，江秋博，等. 拖拉機(jī)后懸掛橫向位姿調(diào)整的模糊PID控制[J]. 農(nóng)業(yè)工程學(xué)報(bào)，2019，35(21)：34-42.

Shao Mingxi, Xin Zhe, Jiang Qiubo, et al. Fuzzy PID control for lateral pose adjustment of tractor rear suspension[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2019, 35(21): 34-42. (in Chinese with English abstract)

[22] 中國(guó)國(guó)家標(biāo)準(zhǔn)化管理委員會(huì).自走式農(nóng)業(yè)機(jī)械導(dǎo)航系統(tǒng)作業(yè)性能及評(píng)價(jià)方法（GB/T 37164—2018）[S]. 北京：中國(guó)標(biāo)準(zhǔn)出版社. 2018.

[23] 賈全. 拖拉機(jī)自動(dòng)導(dǎo)航系統(tǒng)關(guān)鍵技術(shù)研究[D]. 北京：中國(guó)農(nóng)業(yè)機(jī)械化科學(xué)研究院，2013.

Jia Quan. Study on Key Technology of Tractor Auto-navigation System[D]. Beijing: Chinese Academy of Agricultural Mechanization Sciences, 2013. (in Chinese with English abstract)

[24] Petrinec K, Kovacic Z, Marozin A. Simulator of multi-AGV robotic industrial environments[C]//IEEE International Conference on Industrial Technology. Maribor, Slovenia: IEEE, 2003, 2: 979-983.

Variable curvature path tracking control for the automatic navigation of tractors

Wu Caicong, Wu Sixian, Wen Long, Chen Zhibo, Yang Weizhong, Zhai Weixin

(1.,,100083,;2.,,100083,)

The current control accuracy of automatic navigation curve tracking in tractors cannot fully meet the production needs in modern agriculture.In this study, a tracking control system of variable curvature path was presented as the front wheel angle feedforward compensation. Firstly, a comprehensive evaluation was made on the influence of agricultural machinery operating speed and reference path curvature on the foresight distance. The lookahead area was then altered to compute the preview point, in order to realize the dynamic adjustment of the lookahead distance and the feedforward amount of the front wheel angle. Furthermore, the lookahead distance posed a significant effect on the speed of the vehicle. Specifically, the vehicle often traveled at a lower speed on the rugged and curved reference path, when the tractor was working on the field mechanized road or irregular fields. However, the lookahead distance was typically expressed as a linear function that related to the speed of the vehicle. The inaccurate tracking of the reference path was normally obtained due to the weak coupling, if only the speed of the vehicle without considering the curvature of the driving road on the lookahead distance. Therefore, the varying lookahead distance was proposed to depend on the curvature of the path. Meanwhile, the varying curve length of the preview point in the lookahead area was relative to the starting point of the lookahead area depending on the curvature of the reference path. Secondly, the steady-state error was often observed in the driving process of the agricultural machinery, because the pure pursuit algorithm failed to consider the deviation between the current pose and the reference path of the agricultural machinery, when tracking the preview point. An improved controller with the fuzzy control algorithm was designed to realize the compensation of the front wheel angle of agricultural machinery. Specifically, the lateral error and heading error of the agricultural machinery were used as the inputs of the fuzzy controller, and the compensation amount of the front wheel angle was used as the output. The sum of the feedforward and compensation amount of the front wheel angle were used as the control amount of the front wheel angle of the vehicle. This control amount considered the lateral error and heading error of the current position of the vehicle, together with the influence of the forward reference path on the steering of the agricultural machinery. The tracking accuracy was then greatly improved in the agricultural machinery. Taking the DF2204 continuously variable transmission tractor as the test platform, an automatic navigation system was developed to verify the practicability of the path tracking algorithm. 21 sets of variable curvature path tracking experiments were carried out in Beijing Miyun Experimental Field. The results indicate that the average absolute errors were 2.7, 2.7, 3.3, and 4.0 cm, and the average root errors were 3.4, 3.7, 4.6, and 5.0 cm, respectively, when the tractor moved at 1.0, 1.5, 2.0, and 3.0 m/s. The tracking control approach of variable curvature path in the autonomous navigation can be expected to effectively increase the tracking accuracy of agricultural machinery curve paths and the utilization rate of farmland.

tractors; automatic navigation; path tracking; variable curvature; lookahead distance; fuzzy control

10.11975/j.issn.1002-6819.2022.21.001

S147.2

A

1002-6819(2022)-21-0001-07

吳才聰，吳思賢，文龍，等. 拖拉機(jī)自動(dòng)導(dǎo)航變曲度路徑跟蹤控制[J]. 農(nóng)業(yè)工程學(xué)報(bào)，2022，38(21)：1-7.doi：10.11975/j.issn.1002-6819.2022.21.001 http://www.tcsae.org

Wu Caicong, Wu Sixian, Wen Long, et al. Variable curvature path tracking control for the automatic navigation of tractors[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2022, 38(21): 1-7. (in Chinese with English abstract) doi：10.11975/j.issn.1002-6819.2022.21.001 http://www.tcsae.org

2022-07-18

2022-09-09

國(guó)家重點(diǎn)研發(fā)計(jì)劃項(xiàng)目（2021YFB3901302）

吳才聰，教授，博士生導(dǎo)師，研究方向?yàn)檗r(nóng)機(jī)大數(shù)據(jù)、農(nóng)機(jī)導(dǎo)航與位置服務(wù)。Email：wucc@cau.edu.cn