基于超寬帶無線定位的農業設施內移動平臺路徑跟蹤研究

姚立健，Santosh K Pitla，楊自棟，夏坡坡，趙辰彥

?

基于超寬帶無線定位的農業設施內移動平臺路徑跟蹤研究

姚立健1，Santosh K Pitla2，楊自棟1，夏坡坡1，趙辰彥1

（1. 浙江農林大學工程學院，杭州 311300； 2. Department of Biological Systems Engineering,University of Nebraska-Lincoln, Lincoln, 68583）

為實現農業設施內車輛自動導航，提出了一種基于超寬帶（ultra wideband, UWB）無線定位的路徑跟蹤方法。運用4個基站組建UWB無線定位系統，采用加權最小二乘法（weighted least squares, WLS）法解超靜定方程組，提高了移動標簽的定位精度。重新定義前視距離，根據車體航向與前視直線的夾角界定車體偏差程度，并提出基于動態前視距離的改進型純追蹤模型。在MATLAB 2016a軟件環境下的仿真說明該文算法優于采用固定視距的傳統純追蹤算法，并進行實車試驗。結果顯示，在UWB定位系統的引導下，車體在不同初始狀態下均能很好地收斂到期望直線，當速度為0.5 m/s時，在4種初始狀態下進行直線跟蹤，穩態偏差為5.4～8.4 cm，穩態偏差均值為6.3 cm。在矩形路徑跟蹤時，當橫向偏差和航向偏差均為0的初始狀態下，全程平均偏差為20.6 cm，跟蹤偏差主要出現在90°轉向處，最大偏差為85.5 cm，說明改進后的純追蹤算法的路徑追蹤質量均優于采用固定視距的傳統純追蹤模型，能滿足農業設施內移動平臺自動導航的需求。該方法可為農業設施內車輛導航提供新思路。

農業機械；控制；模型；超寬帶；純追蹤；動態前視距離；路徑跟蹤

0 引 言

農業設施內自動導航的作業平臺能承擔自主移動作業及農資自動運輸等功能，是智能農業裝備研究的熱點之一。設施環境空間狹小，因此移動平臺需要有精準的路徑跟蹤能力。純追蹤（pure pursuit）[1]是一種純幾何路徑跟蹤控制方法，能模擬人駕駛車輛的行為，具有控制參數少、預見性強等優點，在車輛導航中得到廣泛運用[2-5]。其中動態前視距離（lookahead distance）被認為是提升傳統純追蹤模型路徑跟蹤精度的有效手段，Park等[6-7]采用一組分段線性比例函數來描述不同速度狀態下前視距離的取值。熊中剛等[8-9]根據路徑曲率和車輛行駛速度動態確定前視距離，速度越大前視距離越大，彎度越大則前視距離越小。段建民等[10]認為前視距離與車速為二次函數關系，該二次函數的3個系數分別為制動距離、反應距離和最小轉彎半徑。還有學者將神經網絡[11]、模糊控制[12-15]等智能算法引入到動態視距的確定中，以便獲得更好的跟蹤精度。上述關于動態前視距離的研究主要集中在與車體速度、路徑曲率等相關性上，未充分考慮車體偏差程度對前視距離取值的影響，研究成果也主要應用在田間直線跟蹤，對于設施溫室內需要進行頻繁直角轉向的矩形路徑跟蹤研究較少。

實現車輛自動導航的另一個關鍵技術是獲取定位信息，GPS（global position system）[3-11,13]常被用來獲取移動目標的位置坐標，但主要應用于室外農田環境。農業設施內無法接收清晰的GPS信號，因此必須尋找一種可靠、精準的室內定位技術取代GPS。機器視覺[16-17]、超聲波[18]、激光[19]和無線傳感技術[20-22]常被用于室內定位。機器視覺能獲取場地豐富的信息，但定位的實時性不高，而超聲波和激光測距一般適用于結構化的工廠環境，且信息采集速度慢，導航精度不高。與上述室內定位技術相比，超寬帶（ultra wideband, UWB）是利用納秒級或亞納秒級脈寬進行短距離內高速傳輸數據的無線通信技術，具有很高的時間分辨率，理論上能獲得厘米級的定位精度。同時UWB具有發送功率小、抗干擾性能強、消耗電能小等諸多優勢，因此越來越多地被運用到室內高精度定位當中[23-26]。本研究采用UWB無線定位技術獲取移動平臺高精度的位置信息，并對傳統純追蹤算法進行改進，提出一種基于偏差程度的動態前視距離確定方法，以適應空間狹小、移動平臺需要頻繁改變行駛方向的矩形路徑追蹤工況，最終得到一種適合農業設施環境內的作業平臺自動導航方法，并利用自制的試驗樣機在室內場地中進行導航試驗來分析驗證。

1 試驗樣機結構

本文試驗樣機采用純電動2SW-2DW四輪機構，由鋰電池（48 V、20 Ah）供電，如圖1所示。樣機軸距為0.84 m，輪距為0.55 m，前后輪徑均為32 cm。為滿足Ackermann轉向定理，采用電動推桿（BXTL150）推動梯形四桿機構實現前輪轉向，最大轉向角為32°，后輪的最小轉向半徑為min為1.34 m。推桿由L298N驅動器驅動。樣機后輪由直流電機（AQMD6020BLS）驅動，通過減速器和差速器來實現左、右輪差速行駛，并通過光電增量式編碼器（E40S8-200-3-T-24）將位移量轉換成脈沖數，據此來檢測后輪的轉速。由若干個UWB無線傳感器（I-UWB LPS PA）組建的定位系統為樣機提供相對位置坐標，該系統的數據刷新頻率為1~50 Hz，測距精度可達10 cm以內。樣機的姿態信息是由電子陀螺儀（WT901C）提供，測量范圍為0～±180°，精度為0.1°。主控制器（MC9S12XS128MAL）接收UWB定位系統和電子陀螺儀傳來的位姿信息，換算成車體相對于期望路徑的偏差信息并輸入路徑跟蹤模型，計算樣機轉向所需的脈寬，再由驅動器驅動電動推桿執行轉向動作，完成路徑跟蹤過程。2個編碼器與控制器、電動機等組成閉環系統，保證控制精度。控制系統框圖如圖2所示。

1.UWB基站 2. 車體 3.電動推桿 4.前輪 5.電子陀螺儀 6.移動標簽 7.差速器 8.直流電機 9.后輪 10.鋰電池

注：(x,y)為移動標簽坐標；θ為航向偏差，(°)；PWM為脈寬調制，Uf為電動推桿控制電壓，V；Ur為直流電機控制電壓，V；PCM為脈沖編碼調制；α為前輪轉向角，(°)；v為后輪速度，m·s-1。下同。

2 基于UWB的車體定位

2.1 定位原理

UWB定位系統由若干固定基站和移動標簽通過一定的通信方式組成[24]。獲取3維空間中某點坐標（,,）一般至少需要4個基站，增加基站數量不但可以提高定位精度，同時也可以作為通信質量欠佳時的冗余備份。本文默認移動樣機行駛的路面呈水平狀態，因此忽略高程方向的參數計算。UWB定位系統的定位精度與基站的分布、標簽的位置、障礙物等密切相關，因此需通過反復調整才能布設最優的定位環境。根據文獻[25]定位試驗方法，由4個基站組成一個平面定位坐標系如圖3所示，小矩形為模擬溫室環境的矩形路徑跟蹤的試驗區域。

圖3 試驗場地布置

2.2 TOA定位算法

考慮到UWB是在極窄脈沖下進行通信，具有很高的時間分辨率，因此本文采用基于測量信號到達時間（TOA，time of arrival）的定位方法來獲取移動標簽的實時位置信息[27]。如圖3，在平面直角坐標系中，移動標簽坐標為（,），4個基站的坐標分別為（1,1）、（2,2）、（3,3）和（4,4）。根據TOA定位原理，移動標簽到各基站的距離r為

式中為光速，3×108m/s，τ為信號在移動標簽到第個基站之間的傳輸時間，s，=1,2…4。

將式（1）中方程2、3、4分別與方程1相減可得

因為在坐標系中，各基站的坐標（x,y）及、中的數據均可由UWB定位系統獲得，將這些數據帶入式（2），即可求得移動標簽的坐標，即（,）。式（2）中的方程數多于變量數，屬于超靜定方程組。本文采用加權最小二乘法[21]（WLS，weighted least squares）解式（2），得到移動標簽坐標的最優解見式（3）。

=(T)-1T（3）

根據上述定位算法，經過反復調試，當4個基站組成20 m×15 m的矩形時，此區域內能獲得較好的定位精度，其中該矩形內16 m×11 m小矩形試驗區域內中心的定位誤差在7 cm以內，矩形邊緣定位誤差在12 cm以內，基本滿足試驗要求。

3 路徑跟蹤算法

3.1 傳統純追蹤模型及其改進

傳統純追蹤模型是一種直觀、簡單的純幾何算法，忽略了輪胎與地面的側向滑動，通過合適的固定前視距離來確定位于期望路徑上的預瞄點[12]，規劃出從車體到預瞄點所經過的圓弧線并確定前輪的轉角。如圖4所示，12為車體當前需跟蹤的路線，即期望路徑，以車體后軸中心點為基準點、車體中心線為基準線，定義車體當前的橫向偏差和航向偏差分別如圖所示，在12上找到一點C，使得等于固定前視距離，則點即為預瞄點，為前視直線。的垂直平分線與的垂線交于點，根據傳統純追蹤模型思想，可規劃一段以為圓心從點到預瞄點的圓弧，該圓弧半徑為車體的轉向半徑，車體將在當前狀態下沿著這段圓弧行駛直至位姿信息被刷新。根據幾何運算[12]可得當前前輪轉向角為

式中為前視距離，m；為橫向偏差，m；為航向偏差，（°）；為軸距，m。

注：12為期望路徑；為后軸中心點；為前軸中心點；為軸距，m；為車體方向；為預瞄點；為前視直線；為車體轉向中心；為前視距離，m；為橫向偏差，m。下同。

Note:12is the desired path;is the center of the rear axle;is the center of the front axle;is the wheelbase, m;is vehicle direction;is the preview point;is the lookahead straight line;is the steering center of the vehicle;is the lookahead distance, m;is the lateral deviation, m. The same as below.

圖4 傳統純追蹤模型

Fig.4 Traditional pure pursuit model

由式（4）可以看出，傳統純追蹤模型要求≥，由于有這個約束，因此該模型僅適合于橫向偏差不太大的工況。在設施溫室內，移動作業平臺行駛的路線多為矩形，需頻繁更改當前跟蹤路線，時常會出現較大橫向偏差，因此需對該模型進行改進。如圖5所示，從點向12作垂線，垂足為，沿著12方向找到一點，使得=（前視距離），則點為改進后純追蹤模型的預瞄點，為前視直線，車體方向與前視直線間的夾角為Δ。同樣可規劃一段以為圓心從點到點的圓弧，方法同上。此時前輪的轉向角調整為

式（5）中各變量定義與式（4）相同。

由圖5還可得

|Δ|=|+arctan(/)| （6）

式中和正負號分別表示車體在跟蹤路徑左側橫向偏差為正、右側為負，航向偏差逆時針時為正、順時針為負，下同。

注：AF為改進后模型的前視直線；ΔΘ為車體方向AB與前視直線間AF的夾角，(°)；O′為改進后模型的車體轉向中心。

因為在計算過程中不考慮車輛的動力學模型，車體當前的轉向角僅與、和這3個參數有關。和可由儀器硬件客觀獲取，因此前視距離則成為決定轉向角的關鍵參數，一般通過計算機仿真可獲得最優的固定前視距離fixed。

3.2 偏差程度分析

由式（5）可知，前視距離與轉向角為負相關，根據駕車者經驗，當車體偏離期望路線較大時，駕駛人會采用較大的前輪轉向角（較小的前視距離）來快速修正車體位姿，反之則會采用較小的轉向角（較大的前視距離）以保證行駛平穩，因此實際駕車時前視距離會隨車體偏差程度動態調整。但車體偏差程度的界定主要依賴個人經驗，缺少定量分析方法。因此本文介紹一種定量分析偏差程度的方法。由圖6可知，當車體方向1與前視直線重合（即Δ=0°）時，車體的轉向角0°，即無需通過轉向來調整車體姿態，可認為該狀態為最小偏差狀態。當|Δ|逐步增大，車體偏差狀態也在逐步增大，當|Δ|=180°（即3狀態）時，可認為此時車體的狀態為最大偏差狀態。因此可用|Δ|的大小來定量描述偏差程度，|Δ|的值可由3.1節式（6）求得，其取值范圍在[0，180°]。|Δ|越接近0°，偏差程度越小，反之越大。

注：AH為前視直線；AB1表示車體最小偏差狀態；AB3表示車體最大偏差狀態；AB2表示車體瞬時狀態；KH為最佳固定前視距離，m；KM為最小前視距離，m；KN為最大前視距離，m。

3.3 動態前視距離

根據上節討論的車體偏差程度可動態確定前視距離。首先通過最優的固定前視距離fixed初選預瞄點，如圖6所示。當車體處于3狀態時，此時偏差最大，可適當縮小前視距離（即圖6中預瞄點由向左移動）以增大前輪轉向角度，從而提高修正車體位姿的速度[1]。同理當車體處于1狀態時，適度將預瞄點右移可提高車體收斂的平順性[1]。經反復仿真，確定最大和最小偏差狀態時的前視距離分別為0.5fixed和1.5fixed，即預瞄點分別在和點。通過歸一化線性關系可求出|Δ|在[0, 180°]任意值時動態前視距離dynamic。

本文的動態前視距離的思想本質上是在最優固定前視距離fixed的基礎上，根據當前車體的偏差程度|Δ|再動態增減fixed，實現更精細的路徑跟蹤控制。

3.4 路徑跟蹤仿真

為驗證本文改進型純追蹤算法的有效性，選擇車體在較大偏差的初始狀態（,）為(1 m,120°)時進行直線跟蹤仿真試驗，正負號定義與式（6）一致。仿真試驗在MATLAB R2016b環境下完成，仿真時車體軸距與真車一致。仿真速度為0.5 m/s，采樣周期為0.5 s。圖7顯示了傳統的固定視距fixed純追蹤模型和本文改進后的動態視距dynamic純追蹤模型的仿真對比。

以控制工程理論中的平均偏差、穩態偏差、穩定距離和調整時間等指標綜合衡量路徑跟蹤的質量。平均偏差是從仿真開始到結束所有橫向偏差的均值；穩定距離是指移動車體從初始狀態第1次收斂到橫向偏差為0的穩定點所走過的水平距離；調整時間是指從初始狀態行駛到穩定點所需的時間；穩態偏差是指從穩定點向后的橫向偏差均值；最大偏差是指整個仿真過程中最大的橫向偏差。平均偏差與穩態偏差是反映路徑跟蹤精度的重要指標。穩定距離和調整時間則反映路徑跟蹤的收斂速度。對圖7數據進行統計可得表1。

注：傳統純追蹤模型最佳固定視距Lfixed為2.2 m，是經過計算機反復仿真而得；車體在跟蹤路徑左側橫向偏差d為正、右側為負；航向偏差θ逆時針時為正、順時針為負。下同。

表1 仿真結果統計

由表1可以看出，采用動態視距dynamic模型的穩態偏差為2.1 cm，而固定視距fixed模型為4.6 cm，前者的跟蹤精度遠高于后者。在收斂速度方面，采用動態視距模型的穩定距離和調整時間分別為803.6 cm和24.5 s，而固定視距模型的這2個指標分別為960.5 cm和27.5 s，說明采用動態視距模型收斂速度更快。圖7b為偏差收斂效果（-）圖，該圖能清晰反映車體偏差逐步收斂的細節：先是航向偏差快速減小而橫向偏差在增大；接著是橫向偏差減小而航向偏差增大；最后階段是橫向偏差和航向偏差同時減小直至收斂到原點附近。在這收斂過程中，受車體機械結構和算法所限，橫向和航向偏差不可能同時減少，但整體偏差狀態的趨勢就像“螺線”一樣逐步向原點收斂。綜合圖7和表1可知，采用動態視距dynamic模型性能指標也優于固定視距fixed模型的仿真效果。

4 試驗與結果分析

4.1 試驗過程

為檢驗本文算法有效性，筆者在浙江農林大學體育館內選定一塊試驗場地（圖1），場地布置如圖3所示。試驗時間為2018年5－7月，場地氣溫20～32 ℃。試驗樣機前進速度與仿真時速度同為0.5 m/s，采樣頻率10 Hz。試驗分為直線跟蹤和矩形路線跟蹤2類。同一初始狀態的試驗重復5次，各指標求平均值。

試驗1：為檢驗本文算法對于不同偏差程度的適應性，設計4種偏差初始狀態來測試樣機直線跟蹤效果。分別按傳統的固定視距fixed純追蹤模型和改進后動態視距dynamic純追蹤模型進行對比試驗。各初始狀態下的固定視距fixed值分別由計算機反復仿真獲取。跟蹤軌跡如圖8所示，對圖8數據進行統計可得表2。

圖8 不同初始狀態下直線跟蹤軌跡

表2 直線跟蹤試驗結果統計

試驗2：為檢驗試驗樣機在農業設施內常見的矩形路徑跟蹤的能力，設計了16 m×11 m的矩形路線。如圖9，樣機出發的初始位置為（10,13），初始航向與當前跟蹤路徑方向一致，因此初始狀態為（0,0°）。經計算機反復仿真，本試驗的固定視距fixed取1.8 m。綜合考慮車體的路徑跟蹤精度和最小轉向半徑為min，程序中設定在距離下一條跟蹤直線不足1 m時更換當前跟蹤直線。矩形路徑順時針跟蹤1周的軌跡見圖9，跟蹤數據統計結果見表3所示。

圖9 矩形跟蹤軌跡

表3 矩形跟蹤試驗結果統計

4.2 試驗結果分析

4.2.1 路徑跟蹤精度

在試驗1中，4種初始狀態下動態可變視距純追蹤模型路徑追蹤平均偏差分別為31.6、35.0、23.3、30.5 cm，平均偏差均值為30.1 cm，而傳統固定視距純追蹤模型平均偏差為49.9、54.8、31.1、34.9 cm，均值為42.7 cm。當車體達到穩定狀態后，動態可變視距模型的穩態偏差分別為5.6、5.4、8.4、5.9 cm，穩態偏差均值為6.3 cm，而傳統的固定視距模型則為14.2、8.1、4.4、4.5 cm，均值為7.8 cm。在試驗2中，動態可變視距和傳統固定視距這2種模型的平均偏差分別為20.6和30.5 cm。上述分析表明采用動態可變視距純追蹤模型能提升路徑跟蹤的精度。

4.2.2 路徑跟蹤穩定性

由統計學知識得，標準差能反映樣本數據集的離散程度，標準差越大則數據集離散程度越高，數據集的穩定性越差，因此動態可變視距和傳統固定視距這2種模型平均偏差與穩態偏差的標準差能反映路徑跟蹤的穩定性。動態可變視距模型在4種初始狀態下，其平均偏差值與穩態偏差值的標準差分別為4.3與1.2 cm，而采用固定視距模型的標準差分別為9.9和4.0 cm。前者的標準差明顯小于后者，說明在各初始狀態下動態可變視距模型中的平均偏差值與穩態偏差值的離散程度較低，即路徑跟蹤的穩定性較固定視距模型高。

4.2.3 收斂快速性

在試驗1中，4種初始狀態下動態可變視距模型調整時間分別為13.5、6.6、10.1、7.7 s，其對應的穩定距離分別為229.4、80.3、283.0、221.7 cm，而固定視距模型的調整時間為21.2、12.3、12.0、8.7 s，其對應的穩定距離分別為410.9、86.0、504.0、426.1 cm。前者的各項數據值均優于后者，說明采用動態可變視距模型跟蹤收斂的快速性優于固定視距模型。

4.2.4 初始偏差程度對最大偏差的影響

由式（6）可得，在試驗1中4種初始狀態下的偏差程度|Δ|值分別為105.1°、36.5°、39.0°、17.7°。其中偏差程度最大為105.1°，此狀態下（圖8a）的初始橫向偏差雖然只有0.4 m，但初始航向偏差高達?120°，車體要經過類似“掉頭”的較大幅度轉向，才能收斂到期望跟蹤的直線，此時的轉向半徑不低于min（134 cm），因此在該狀態下，出現了最大偏差124.6 cm，此狀態下的調整時間也最長（13.5 s）。在圖8b～8d中，車體初始偏差程度相對較小，因此最大偏差與調整時間也明顯變小。在試驗2中，動態可變視距和傳統固定視距這2種模型下車體的初始狀態一致，最大偏差分別為85.5和117.7 cm，最大偏差出現在90°轉向處，這是因為在該處需更換當前追蹤路線所致。2種模型對應的最大偏差均在1 m左右，相差并不明顯。綜上可得車體最大偏差與初始偏差程度相關，初始偏差程度越大，各指標值也相應越大。

5 結 論

1）利用UWB技術組建的無線定位系統，在16 m×11 m矩形試驗區域內中心的定位誤差在7 cm以內，矩形邊緣定位誤差在12 cm以內，基本滿足農業設施內路徑跟蹤試驗要求；

2）基于動態前視距離的改進型純追蹤模型，在仿真試驗中的穩態偏差為2.1 cm、穩定距離為803.6 cm、調整時間為24.5 s，而固定視距fixed模型的穩態偏差、穩定距離、調整時間分別為4.6 cm、960.5 cm和27.5 s，說明前者的跟蹤精度和收斂速度這2個指標要優于后者；

3）利用實車進行了直線和矩形2類路徑跟蹤試驗，試驗表明，在UWB定位系統引導下，采用改進型純追蹤算法，使試驗樣機的直線跟蹤穩態偏差在5.4～8.4 cm之間，穩態偏差均值為6.3 cm，矩形跟蹤平均偏差為20.6 cm，能滿足農業設施內移動平臺的作業需求；

系統的硬件制造誤差、UWB定位誤差會造成跟蹤精度的下降。在后續研究中，還應考慮遮擋物、現場溫度、路面側滑等真實設施環境對路徑跟蹤造成的影響。還可引進模糊控制等智能算法，以進一步減少跟蹤的橫向偏差，改善路徑跟蹤的質量，提高其魯棒性。另外，關于車體偏差程度的數學界定還有細化提升的空間。

[1] Coulter R C. Implementation of the pure pursuit path tracking algorithm[R]. Carnegie-Mellon UNIV Pittsburgh PA Robotics INST, 1992.

[2] 胡靜濤，高雷，白曉平，等. 農業機械自動導航技術研究進展[J]. 農業工程學報，2015，31(10)：1－10.

Hu Jingtao, Gao Lei, Bai Xiaoping, et al. Review of research on automatic guidance of agricultural vehicles[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2015, 31(10): 1－10. (in Chinese with English abstract)

[3] Rains G C, Faircloth A G, Thai C, et al. Technical note: evaluation of a simple pure pursuit path-following algorithm for an autonomous, articulated-steer vehicle[J]. Applied Engineering in Agriculture, 2014, 30(3): 367－374.

[4] Ye Y, He L, Zhang Q. A robotic platform “bin-dog” for bin management in orchard environment[C/OL]//2016 ASABE Annual International Meeting. American Society of Agricultural and Biological Engineers, 2016:1 [2018-08-10] doi:10.13031/aim.20162462088

[5] 劉兆朋，張智剛，羅錫文，等. 雷沃ZP9500 高地隙噴霧機的GNSS 自動導航作業系統設計[J]. 農業工程學報，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)

[6] Park M W, Lee S W, Han W Y. Development of lateral control system for autonomous vehicle based on adaptive pure pursuit algorithm[C]//International Conference on Control, Automation and Systems. IEEE, 2014: 1443－1447.

[7] 張聞宇，丁幼春，王雪玲，等. 基于SVR逆向模型的拖拉機導航純追蹤控制方法[J]. 農業機械學報，2016，47(1)：29－36.

Zhang Wenyu, Ding Youchun, Wang Xueling, et al. Pure pursuit control method based on SVR inverse-model for tractor navigation[J]. Transactions of the Chinese Society for Agricultural Machinery, 2016, 47(1): 29－36. (in Chinese with English abstract)

[8] 熊中剛，葉振環，賀娟，等. 基于免疫模糊PID的小型農業機械路徑智能跟蹤控制[J]. 機器人，2015，37(2)：212－223.

Xiong Zhonggang，Ye Zhenhuan，He Juan, et al. Small agricultural machinery path intelligent tracking control based on fuzzy immune PID[J]. Robot, 2015, 37(2): 212－223. (in Chinese with English abstract)

[9] 李革，王宇，郭劉粉，等. 插秧機導航路徑跟蹤改進純追蹤算法[J]. 農業機械學報，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)

[10] 段建民，楊晨，石慧. 基于Pure Pursuit算法的智能車路徑跟蹤[J]. 北京工業大學學報，2016，42(9)：1301－1306.

Duan Jianmin, Yang Chen, Shi Hui. Path tracking based on pure pursuit algorithm for intelligent vehicles[J]. Journal of Beijing University of Technology, 2016, 42(9): 1301－1306. (in Chinese with English abstract)

[11] 黃沛琛，羅錫文，張智剛. 改進純追蹤模型的農業機械地頭轉向控制方法[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)

[12] 汪小旵，魯偉，陳滿，等. 基于改進純追蹤模型的溫室采摘運輸自動跟隨系統[J]. 農業機械學報，2016，47(12)：8－13.

Wang Xiaochan, Lu Wei, Chen Man, et al. Automatic following system for greenhouse harvesting transportation based on adaptive pure pursuit model[J]. Transactions of the Chinese Society for Agricultural Machinery, 2016, 47(12): 8－13. (in Chinese with English abstract)

[13] 李逃昌，胡靜濤，高雷. 基于級聯式控制策略的農業機械魯棒自適應路徑跟蹤控制[J]. 機器人，2014，36(2)：241－249.

Li Taochang, Hu Jingtao, Gao Lei. Robust adaptive path tracking control of agricultural machines based on cascaded control strategy[J]. Robot, 2014, 36(2): 241－249. (in Chinese with English abstract)

[14] Urrea C, Mu?oz J. Path Tracking of mobile robot in crops[J]. Journal of Intelligent & Robotic Systems, 2015, 80(2): 193－205.

[15] Lacevic B, Velagic J. Evolutionary design of fuzzy logic based position controller for mobile robot[J]. Journal of Intelligent & Robotic Systems, 2011, 63(3/4): 595－614.

[16] 翟志強，朱忠祥，杜岳峰，等. 基于虛擬現實的拖拉機雙目視覺導航試驗[J]. 農業工程學報，2017，33(23)：56－65.

Zhai Zhiqiang, Zhu Zhongxiang, Du Yuefeng, et al. Test of binocular vision-based guidance for tractor based on virtual reality[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2017, 33(23): 56－65. (in Chinese with English abstract)

[17] 武星，沈偉良，樓佩煌，等. 視覺導引AGV魯棒特征識別與精確路徑跟蹤研究[J]. 農業機械學報，2016，47(7)：48－56.

Wu Xing, Shen Weiliang, Lou Peihuang, et al. Robust feature recognition and precise path tracking for vision-guided AGV[J]. Transactions of the Chinese Society for Agricultural Machinery, 2016, 47(7): 48－56. (in Chinese with English abstract)

[18] González R, Rodríguez F, Sánchezhermosilla J, et al. Navigation techniques for mobile robots in greenhouses[J]. Applied Engineering in Agriculture, 2009, 25(2): 153－165.

[19] Lee I N, Lee K H, Lee J H, et al. Autonomous greenhouse sprayer navigation using automatic tracking algorithm[J]. Applied Engineering in Agriculture, 2015, 31(1): 17－21.

[20] 魏振春，呂增威，楊成，等. 基于RSS正確性判定的礦井機車無線定位方法[J]. 儀器儀表學報，2014，35(1)：178－184.

Wei Zhenchun, Lv Zengwei, Yang Cheng, et al. Wireless positioning method based on correctness judgment of received signal strength for mine locomotive[J]. Chinese Journal of Scientific Instrument, 2014, 35(1): 178－184. (in Chinese with English abstract)

[21] 劉書靜，羅海勇，吳彬，等. 基于最小二乘測距定位算法信標最優部署模型[J]. 計算機學報，2013，36(3)：546－556.

Liu Shujing, Luo Haiyong, Wu Bin, et al. Optimal landmark deployment patterns for range-based least squares localization[J]. Chinese Journal of Computers, 2013, 36(3): 546－556. (in Chinese with English abstract)

[22] 周樹帆，劉芳，湯澤云，等. 基因表達式編程輔助的室內無線定位方法[J]. 土木建筑與環境工程，2016，38(增刊1)：198－203.

Zhou Shufan, Liu Fang, Tang Zeyun, et al. Wireless indoor position method assisted with gene expression programming[J]. Journal of Civil Architectural & Environmental Engineering, 2016, 38(Supp.1): 198－203. (in Chinese with English abstract)

[23] 符世琛，李一鳴，楊健健，等. 基于超寬帶技術的掘進機自主定位定向方法研究[J]. 煤炭學報，2015，40(11)：2603－2610.

Fu Shichen, Li Yiming, Yang Jianjian, et al. Research on autonomous positioning and orientation method of roadheader based on ultra wide-band technology[J]. Journal of China Coal Society, 2015, 40(11): 2603－2610. (in Chinese with English abstract)

[24] 馬永濤，侯振寰，姜啟登，等. 基于超寬帶信號到達時間的室內人員被動式定位算法及仿真[J]. 天津大學學報：自然科學與工程技術版，2017，50(8)：843－849.

Ma Yongtao, Hou Zhenhuan, Jiang Qideng, et al. Device-free indoor person localization algorithm and simulation based on time of arrival for UWB signal[J]. Journal of Tianjin University: Science and Technology, 2017, 50(8): 843－849. (in Chinese with English abstract)

[25] 戴波，呂昕，劉學君，等. 基于改進的超寬帶危化品倉儲堆垛定位技術[J]. 上海交通大學學報，2016，50(6)：963－967.

Dai Bo, Lv Xin, Liu Xuejun, et al. Improved UWB-based location technology applied to hazardous chemicals five-distances stacking storage[J]. Journal of Shanghai Jiao Tong University, 2016, 50(6): 963－967. in Chinese with English abstract)

[26] 孫小文，張小超，趙博，等. 農資倉儲脈沖超寬帶室內定位系統設計與試驗及誤差分析[J]. 農業工程學報，2017，33(增刊1)：23－29.

Sun Xiaowen, Zhang Xiaochao, Zhao Bo, et al. Design, experiment and error analysis of impulse radio-ultra wide band indoor positioning system used in agricultural warehousing[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2017, 33(Supp.1): 23－29. (in Chinese with English abstract)

[27] Irahhauten Z, Nikookar H, Klepper M. 2D UWB localization in indoor multipath environment using a joint ToA/DoA technique[C]//IEEE Wireless Communications and NETWORKING Conference. IEEE, 2012: 2253－2257.

Path tracking of mobile platform in agricultural facilities based on ultra wideband wireless positioning

Yao Lijian1, Santosh K Pitla2, Yang Zidong1, Xia Popo1, Zhao Chenyan1

(1.,, 311300,; 2.,,,, 68583,)

In order to realize automatic vehicle navigation in agricultural facilities without GPS (global position system) signal, a path tracking method based on UWB (ultra wideband) positioning was proposed in this study. The test prototype uses a pure electric and four wheels mechanism with 2SW-2DW (2 steering wheels and 2 drive wheels) structure. The closed-loop system with the controller, driver, motor and encoder ensures the accuracy of the control. The indoor UWB wirelesspositioning system based on the TOA (time of arrival) principle was built by using 4 anchor nodes, and a WLS (weighted least squares) method was used to solve the statically indeterminate equations, which improved the positioning accuracy of mobile tags. The positioning error of the UWB positioning system in the 16 m×11 m rectangular center area is within 7 cm, and the positioning error of the rectangular edge is less than 12 cm, which meets the positioning requirements of path tracking test in agricultural facilities. In view of the limitation that the lookahead distance must be greater than the lateral deviation in the traditional pure pursuit model, the lookahead distance was redefined in this paper. So the scope of application of the pure pursuit model had been widened. The deviation degree of vehicle was defined quantitatively according to the angle between the heading of the vehicle and the lookahead line. The traditional pure pursuit model was improved, and a new pure pursuit model algorithm based on dynamic lookahead distance was proposed to further improve the quality of path tracking. The algorithm of path tracking was simulated and verified using MATLAB 2016a. The results showed that the average error, the maximum deviation and the stability distance of the improved algorithm with dynamic lookahead distance were better than those of the traditional pure pursuit algorithm with fixed lookahead distance, indicating that the proposed improved algorithm is effective theoretically. The real vehicle test results showed that in the guidance of UWB positioning system, the vehicle could converge to the desired path in different initial states. Linear tracking were carried out in 4 initial states. When the test prototype speed is 0.5 m/s and the signal sampling period is 10 Hz, the average deviation, maximum deviation, stable distance and adjustment time are 23.3-35.0 cm, 39.0-124.6 cm,80.3-283.0 cm, and 6.6-13.5 s, respectively. It was also observed that the above corresponding index increased with the initial deviation of the prototype. When the prototype reaches a stable state, the steady-state deviation is 5.4-8.4 cm, and its average steady-state deviation is 6.3 cm. In the rectangular path tracking, the overall mean deviation is 20.6 cm when the initial lateral deviation and heading deviation are 0. The lateral deviation mainly occurs at 90° turning, and the maximum deviation is 85.5 cm. The path tracking quality of the improved pure tracking algorithm with dynamic lookahead distance is better than the traditional pure tracking model with fixed lookahead distance, and it can meet the requirements of mobile platforms in frequent steering for automatic navigation in the agricultural facilities. This method could provide a new idea for vehicle navigation in agricultural facilities.

agricultural machinery; control; models; ultra wideband; pure pursuit; dynamic lookahead distance; path tracking

10.11975/j.issn.1002-6819.2019.02.003

S224.3; TP242.3

A

1002-6819(2019)-02-0017-08

2018-08-10

2018-12-26

浙江省基礎公益研究計劃項目（LGN18F030001）；浙江省科技廳重大專項（2016C02G2100540）；浙江省教育廳項目（Y201636110）

姚立健，副教授，博士，主要從事智能農業裝備與農業機器人等方向研究。Email：ljyao@zafu.edu.cn

中國農業工程學會高級會員：姚立健（E041200722S）

姚立健，Santosh K Pitla，楊自棟，夏坡坡，趙辰彥. 基于超寬帶無線定位的農業設施內移動平臺路徑跟蹤研究[J]. 農業工程學報，2019，35(2)：17－24. doi：10.11975/j.issn.1002-6819.2019.02.003 http://www.tcsae.org

Yao Lijian, Santosh K Pitla, Yang Zidong, Xia Popo, Zhao Chenyan. Path tracking of mobile platform in agricultural facilities based on ultra wideband wireless positioning[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2019, 35(2): 17－24. (in Chinese with English abstract) doi：10.11975/j.issn.1002-6819.2019.02.003 http://www.tcsae.org