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

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

拖拉機自動導航變曲度路徑跟蹤控制

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

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

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

拖拉機；自動導航；路徑跟蹤；變曲度；前視距離；模糊控制

0 引 言

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

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

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

1 材料與方法

1.1 拖拉機運動學建模

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

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

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

考慮前后輪幾何約束及運動學約束可得拖拉機的運動學模型為

1.2 純追蹤控制器設計

1.2.1 純追蹤控制算法

純追蹤控制算法是移動機器人典型的橫向控制方法。該方法以二輪車模型為基礎，以車輛后軸中心為切點、車輛縱向方向為切線，通過調(diào)整前輪轉角1，使車輛沿著一條圓弧軌跡線到達預瞄點。

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

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

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

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

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

1.2.2 前視區(qū)域及預瞄點的確定

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

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

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

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

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

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

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

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

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

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

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

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

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

1.3 模糊控制器設計

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

前輪轉角δ、前輪轉角前饋量1與前輪轉角補償量δ滿足以下關系：

1.3.1 輸入輸出變量模糊化

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

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

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

3）前輪轉角補償量δ。基本論域為[?3.6°，4.8°]，量化等級為{?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計算方法[22]如圖4所示。

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

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

1.3.2 模糊控制規(guī)則設計

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

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

表1 前輪轉角補償量模糊控制規(guī)則

注：NB為負大；NM為負中；NS為負??；ZO為零；PS為正??；PM為正中；PB為正大；模糊語言描述為：橫向誤差為正大（PB）或負大（NB），航向誤差為正大（PB）或者負大（NB），那么前輪轉角為負大（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 算法驗證

2.1 試驗平臺

為驗證變曲度路徑跟蹤算法，以DF2204無級變速拖拉機為試驗平臺開展導航試驗。試驗地點為北京市密云區(qū)河南寨。

1.導航天線 2.車載控制器 3.電動方向盤 4.華測組合導航 5.角度傳感器

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

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

2.2 試驗設置

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

圖6 采樣路徑

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

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

2.3 試驗結果與分析

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

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

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

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

圖10 前輪轉角前饋量與補償量在行駛過程中的變化

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

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

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

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

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

3 結 論

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

2）行駛速度為1.0、1.5、2.0和3.0 m/s時的最大跟蹤誤差分別為10.8、16.8、16.4和19.9 cm，平均絕對誤差分別為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高地隙噴霧機的GNSS自動導航作業(yè)系統(tǒng)設計[J]. 農(nóng)業(yè)工程學報，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] 徐廣飛，陳美舟，金誠謙，等. 拖拉機自動駕駛關鍵技術綜述[J]. 中國農(nóng)機化學報，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拖拉機自主行駛與作業(yè)控制方法[J]. 農(nóng)業(yè)工程學報，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] 黃沛琛，羅錫文，張智剛. 改進純追蹤模型的農(nóng)業(yè)機械地頭轉向控制方法[J]. 計算機工程與應用，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] 唐小濤，陶建峰，李志騰，等. 自動導航插秧機路徑跟蹤系統(tǒng)穩(wěn)定性模糊控制優(yōu)化方法[J]. 農(nóng)業(yè)機械學報，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] 張華強，王國棟，呂云飛，等. 基于改進純追蹤模型的農(nóng)機路徑跟蹤算法研究[J]. 農(nóng)業(yè)機械學報，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]. 機械工程學報，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]. 農(nóng)業(yè)機械學報，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] 楊陽陽，何志剛，汪若塵，等. 基于轉角補償?shù)闹悄苘囕v循跡控制系統(tǒng)[J]. 儀表技術與傳感器，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] 趙凱，朱愿，馮明月，等. 基于多點序列預瞄的自動駕駛汽車路徑跟蹤算法研究[J]. 汽車技術，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] 邵明璽，辛喆，江秋博，等. 拖拉機后懸掛橫向位姿調(diào)整的模糊PID控制[J]. 農(nóng)業(yè)工程學報，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] 中國國家標準化管理委員會.自走式農(nóng)業(yè)機械導航系統(tǒng)作業(yè)性能及評價方法（GB/T 37164—2018）[S]. 北京：中國標準出版社. 2018.

[23] 賈全. 拖拉機自動導航系統(tǒng)關鍵技術研究[D]. 北京：中國農(nóng)業(yè)機械化科學研究院，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]. 農(nóng)業(yè)工程學報，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

國家重點研發(fā)計劃項目（2021YFB3901302）

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