基于超寬帶無線定位的農(nóng)業(yè)設(shè)施內(nèi)移動平臺路徑跟蹤研究

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

?

基于超寬帶無線定位的農(nóng)業(yè)設(shè)施內(nèi)移動平臺路徑跟蹤研究

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

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

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

農(nóng)業(yè)機械；控制；模型；超寬帶；純追蹤；動態(tài)前視距離；路徑跟蹤

0 引 言

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

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

1 試驗樣機結(jié)構(gòu)

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

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

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

2 基于UWB的車體定位

2.1 定位原理

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

圖3 試驗場地布置

2.2 TOA定位算法

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

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

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

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

=(T)-1T（3）

根據(jù)上述定位算法，經(jīng)過反復(fù)調(diào)試，當(dāng)4個基站組成20 m×15 m的矩形時，此區(qū)域內(nèi)能獲得較好的定位精度，其中該矩形內(nèi)16 m×11 m小矩形試驗區(qū)域內(nèi)中心的定位誤差在7 cm以內(nèi)，矩形邊緣定位誤差在12 cm以內(nèi)，基本滿足試驗要求。

3 路徑跟蹤算法

3.1 傳統(tǒng)純追蹤模型及其改進

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

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

注：12為期望路徑；為后軸中心點；為前軸中心點；為軸距，m；為車體方向；為預(yù)瞄點；為前視直線；為車體轉(zhuǎn)向中心；為前視距離，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 傳統(tǒng)純追蹤模型

Fig.4 Traditional pure pursuit model

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

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

由圖5還可得

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

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

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

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

3.2 偏差程度分析

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

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

3.3 動態(tài)前視距離

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

本文的動態(tài)前視距離的思想本質(zhì)上是在最優(yōu)固定前視距離fixed的基礎(chǔ)上，根據(jù)當(dāng)前車體的偏差程度|Δ|再動態(tài)增減fixed，實現(xiàn)更精細的路徑跟蹤控制。

3.4 路徑跟蹤仿真

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

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

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

表1 仿真結(jié)果統(tǒng)計

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

4 試驗與結(jié)果分析

4.1 試驗過程

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

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

圖8 不同初始狀態(tài)下直線跟蹤軌跡

表2 直線跟蹤試驗結(jié)果統(tǒng)計

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

圖9 矩形跟蹤軌跡

表3 矩形跟蹤試驗結(jié)果統(tǒng)計

4.2 試驗結(jié)果分析

4.2.1 路徑跟蹤精度

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

4.2.2 路徑跟蹤穩(wěn)定性

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

4.2.3 收斂快速性

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

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

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

5 結(jié) 論

1）利用UWB技術(shù)組建的無線定位系統(tǒng)，在16 m×11 m矩形試驗區(qū)域內(nèi)中心的定位誤差在7 cm以內(nèi)，矩形邊緣定位誤差在12 cm以內(nèi)，基本滿足農(nóng)業(yè)設(shè)施內(nèi)路徑跟蹤試驗要求；

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

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

系統(tǒng)的硬件制造誤差、UWB定位誤差會造成跟蹤精度的下降。在后續(xù)研究中，還應(yīng)考慮遮擋物、現(xiàn)場溫度、路面?zhèn)然日鎸嵲O(shè)施環(huán)境對路徑跟蹤造成的影響。還可引進模糊控制等智能算法，以進一步減少跟蹤的橫向偏差，改善路徑跟蹤的質(zhì)量，提高其魯棒性。另外，關(guān)于車體偏差程度的數(shù)學(xué)界定還有細化提升的空間。

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

[2] 胡靜濤，高雷，白曉平，等. 農(nóng)業(yè)機械自動導(dǎo)航技術(shù)研究進展[J]. 農(nóng)業(yè)工程學(xué)報，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 自動導(dǎo)航作業(yè)系統(tǒng)設(shè)計[J]. 農(nóng)業(yè)工程學(xué)報，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逆向模型的拖拉機導(dǎo)航純追蹤控制方法[J]. 農(nóng)業(yè)機械學(xué)報，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] 熊中剛，葉振環(huán)，賀娟，等. 基于免疫模糊PID的小型農(nóng)業(yè)機械路徑智能跟蹤控制[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] 李革，王宇，郭劉粉，等. 插秧機導(dǎo)航路徑跟蹤改進純追蹤算法[J]. 農(nóng)業(yè)機械學(xué)報，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]. 北京工業(yè)大學(xué)學(xué)報，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] 黃沛琛，羅錫文，張智剛. 改進純追蹤模型的農(nóng)業(yè)機械地頭轉(zhuǎn)向控制方法[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)

[12] 汪小旵，魯偉，陳滿，等. 基于改進純追蹤模型的溫室采摘運輸自動跟隨系統(tǒng)[J]. 農(nóng)業(yè)機械學(xué)報，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] 李逃昌，胡靜濤，高雷. 基于級聯(lián)式控制策略的農(nóng)業(yè)機械魯棒自適應(yīng)路徑跟蹤控制[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] 翟志強，朱忠祥，杜岳峰，等. 基于虛擬現(xiàn)實的拖拉機雙目視覺導(dǎo)航試驗[J]. 農(nóng)業(yè)工程學(xué)報，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] 武星，沈偉良，樓佩煌，等. 視覺導(dǎo)引AGV魯棒特征識別與精確路徑跟蹤研究[J]. 農(nóng)業(yè)機械學(xué)報，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]. 儀器儀表學(xué)報，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] 劉書靜，羅海勇，吳彬，等. 基于最小二乘測距定位算法信標最優(yōu)部署模型[J]. 計算機學(xué)報，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] 周樹帆，劉芳，湯澤云，等. 基因表達式編程輔助的室內(nèi)無線定位方法[J]. 土木建筑與環(huán)境工程，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] 符世琛，李一鳴，楊健健，等. 基于超寬帶技術(shù)的掘進機自主定位定向方法研究[J]. 煤炭學(xué)報，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] 馬永濤，侯振寰，姜啟登，等. 基于超寬帶信號到達時間的室內(nèi)人員被動式定位算法及仿真[J]. 天津大學(xué)學(xué)報：自然科學(xué)與工程技術(shù)版，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] 戴波，呂昕，劉學(xué)君，等. 基于改進的超寬帶?；穫}儲堆垛定位技術(shù)[J]. 上海交通大學(xué)學(xué)報，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] 孫小文，張小超，趙博，等. 農(nóng)資倉儲脈沖超寬帶室內(nèi)定位系統(tǒng)設(shè)計與試驗及誤差分析[J]. 農(nóng)業(yè)工程學(xué)報，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

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

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

中國農(nóng)業(yè)工程學(xué)會高級會員：姚立?。‥041200722S）

姚立健，Santosh K Pitla，楊自棟，夏坡坡，趙辰彥. 基于超寬帶無線定位的農(nóng)業(yè)設(shè)施內(nèi)移動平臺路徑跟蹤研究[J]. 農(nóng)業(yè)工程學(xué)報，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