姚立健，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）

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

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

0 引 言

農(nóng)業(yè)設(shè)施內(nèi)自動導(dǎo)航的作業(yè)平臺能承擔(dān)自主移動作業(yè)及農(nóng)資自動運(yùn)輸?shù)裙δ?，是智能農(nóng)業(yè)裝備研究的熱點(diǎn)之一。設(shè)施環(huán)境空間狹小，因此移動平臺需要有精準(zhǔn)的路徑跟蹤能力。純追蹤（pure pursuit）[1]是一種純幾何路徑跟蹤控制方法，能模擬人駕駛車輛的行為，具有控制參數(shù)少、預(yù)見性強(qiáng)等優(yōu)點(diǎn)，在車輛導(dǎo)航中得到廣泛運(yùn)用[2-5]。其中動態(tài)前視距離（lookahead distance）被認(rèn)為是提升傳統(tǒng)純追蹤模型路徑跟蹤精度的有效手段，Park等[6-7]采用一組分段線性比例函數(shù)來描述不同速度狀態(tài)下前視距離的取值。熊中剛等[8-9]根據(jù)路徑曲率和車輛行駛速度動態(tài)確定前視距離，速度越大前視距離越大，彎度越大則前視距離越小。段建民等[10]認(rèn)為前視距離與車速為二次函數(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)需要進(jìn)行頻繁直角轉(zhuǎn)向的矩形路徑跟蹤研究較少。

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

1 試驗(yàn)樣機(jī)結(jié)構(gòu)

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

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

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

2 基于UWB的車體定位

2.1 定位原理

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

圖3 試驗(yàn)場地布置

2.2 TOA定位算法

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

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

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

因?yàn)樵谧鴺?biāo)系中，各基站的坐標(biāo)（x,y）及、中的數(shù)據(jù)均可由UWB定位系統(tǒng)獲得，將這些數(shù)據(jù)帶入式（2），即可求得移動標(biāo)簽的坐標(biāo)，即（,）。式（2）中的方程數(shù)多于變量數(shù)，屬于超靜定方程組。本文采用加權(quán)最小二乘法[21]（WLS，weighted least squares）解式（2），得到移動標(biāo)簽坐標(biāo)的最優(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小矩形試驗(yàn)區(qū)域內(nèi)中心的定位誤差在7 cm以內(nèi)，矩形邊緣定位誤差在12 cm以內(nèi)，基本滿足試驗(yàn)要求。

3 路徑跟蹤算法

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

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

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

注：12為期望路徑；為后軸中心點(diǎn)；為前軸中心點(diǎn)；為軸距，m；為車體方向；為預(yù)瞄點(diǎn)；為前視直線；為車體轉(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)較大橫向偏差，因此需對該模型進(jìn)行改進(jìn)。如圖5所示，從點(diǎn)向12作垂線，垂足為，沿著12方向找到一點(diǎn)，使得=（前視距離），則點(diǎn)為改進(jìn)后純追蹤模型的預(yù)瞄點(diǎn)，為前視直線，車體方向與前視直線間的夾角為Δ。同樣可規(guī)劃一段以為圓心從點(diǎn)到點(diǎn)的圓弧，方法同上。此時前輪的轉(zhuǎn)向角調(diào)整為

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

由圖5還可得

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

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

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

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

3.2 偏差程度分析

由式（5）可知，前視距離與轉(zhuǎn)向角為負(fù)相關(guān)，根據(jù)駕車者經(jīng)驗(yàn)，當(dāng)車體偏離期望路線較大時，駕駛?cè)藭捎幂^大的前輪轉(zhuǎn)向角（較小的前視距離）來快速修正車體位姿，反之則會采用較小的轉(zhuǎn)向角（較大的前視距離）以保證行駛平穩(wěn)，因此實(shí)際駕車時前視距離會隨車體偏差程度動態(tài)調(diào)整。但車體偏差程度的界定主要依賴個人經(jīng)驗(yàn)，缺少定量分析方法。因此本文介紹一種定量分析偏差程度的方法。由圖6可知，當(dāng)車體方向1與前視直線重合（即Δ=0°）時，車體的轉(zhuǎn)向角0°，即無需通過轉(zhuǎn)向來調(diào)整車體姿態(tài)，可認(rèn)為該狀態(tài)為最小偏差狀態(tài)。當(dāng)|Δ|逐步增大，車體偏差狀態(tài)也在逐步增大，當(dāng)|Δ|=180°（即3狀態(tài)）時，可認(rèn)為此時車體的狀態(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ù)瞄點(diǎn)，如圖6所示。當(dāng)車體處于3狀態(tài)時，此時偏差最大，可適當(dāng)縮小前視距離（即圖6中預(yù)瞄點(diǎn)由向左移動）以增大前輪轉(zhuǎn)向角度，從而提高修正車體位姿的速度[1]。同理當(dāng)車體處于1狀態(tài)時，適度將預(yù)瞄點(diǎn)右移可提高車體收斂的平順性[1]。經(jīng)反復(fù)仿真，確定最大和最小偏差狀態(tài)時的前視距離分別為0.5fixed和1.5fixed，即預(yù)瞄點(diǎn)分別在和點(diǎn)。通過歸一化線性關(guān)系可求出|Δ|在[0, 180°]任意值時動態(tài)前視距離dynamic。

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

3.4 路徑跟蹤仿真

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

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

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

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

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

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

4.1 試驗(yàn)過程

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

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

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

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

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

圖9 矩形跟蹤軌跡

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

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

4.2.1 路徑跟蹤精度

在試驗(yàn)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)車體達(dá)到穩(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。在試驗(yàn)2中，動態(tài)可變視距和傳統(tǒng)固定視距這2種模型的平均偏差分別為20.6和30.5 cm。上述分析表明采用動態(tài)可變視距純追蹤模型能提升路徑跟蹤的精度。

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

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

4.2.3 收斂快速性

在試驗(yàn)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。前者的各項(xiàng)數(shù)據(jù)值均優(yōu)于后者，說明采用動態(tài)可變視距模型跟蹤收斂的快速性優(yōu)于固定視距模型。

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

由式（6）可得，在試驗(yàn)1中4種初始狀態(tài)下的偏差程度|Δ|值分別為105.1°、36.5°、39.0°、17.7°。其中偏差程度最大為105.1°，此狀態(tài)下（圖8a）的初始橫向偏差雖然只有0.4 m，但初始航向偏差高達(dá)?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)整時間也明顯變小。在試驗(yàn)2中，動態(tài)可變視距和傳統(tǒng)固定視距這2種模型下車體的初始狀態(tài)一致，最大偏差分別為85.5和117.7 cm，最大偏差出現(xiàn)在90°轉(zhuǎn)向處，這是因?yàn)樵谠撎幮韪鼡Q當(dāng)前追蹤路線所致。2種模型對應(yīng)的最大偏差均在1 m左右，相差并不明顯。綜上可得車體最大偏差與初始偏差程度相關(guān)，初始偏差程度越大，各指標(biāo)值也相應(yīng)越大。

5 結(jié) 論

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

2）基于動態(tài)前視距離的改進(jìn)型純追蹤模型，在仿真試驗(yàn)中的穩(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個指標(biāo)要優(yōu)于后者；

3）利用實(shí)車進(jìn)行了直線和矩形2類路徑跟蹤試驗(yàn)，試驗(yàn)表明，在UWB定位系統(tǒng)引導(dǎo)下，采用改進(jìn)型純追蹤算法，使試驗(yàn)樣機(jī)的直線跟蹤穩(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)然日鎸?shí)設(shè)施環(huán)境對路徑跟蹤造成的影響。還可引進(jìn)模糊控制等智能算法，以進(jìn)一步減少跟蹤的橫向偏差，改善路徑跟蹤的質(zhì)量，提高其魯棒性。另外，關(guān)于車體偏差程度的數(shù)學(xué)界定還有細(xì)化提升的空間。

[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è)機(jī)械自動導(dǎo)航技術(shù)研究進(jìn)展[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 高地隙噴霧機(jī)的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逆向模型的拖拉機(jī)導(dǎo)航純追蹤控制方法[J]. 農(nóng)業(yè)機(jī)械學(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ī)械路徑智能跟蹤控制[J]. 機(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ī)導(dǎo)航路徑跟蹤改進(jìn)純追蹤算法[J]. 農(nóng)業(yè)機(jī)械學(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] 黃沛琛，羅錫文，張智剛. 改進(jìn)純追蹤模型的農(nóng)業(yè)機(jī)械地頭轉(zhuǎn)向控制方法[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)

[12] 汪小旵，魯偉，陳滿，等. 基于改進(jìn)純追蹤模型的溫室采摘運(yùn)輸自動跟隨系統(tǒng)[J]. 農(nóng)業(yè)機(jī)械學(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è)機(jī)械魯棒自適應(yīng)路徑跟蹤控制[J]. 機(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] 翟志強(qiáng)，朱忠祥，杜岳峰，等. 基于虛擬現(xiàn)實(shí)的拖拉機(jī)雙目視覺導(dǎo)航試驗(yàn)[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è)機(jī)械學(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ī)車無線定位方法[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] 劉書靜，羅海勇，吳彬，等. 基于最小二乘測距定位算法信標(biāo)最優(yōu)部署模型[J]. 計算機(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] 周樹帆，劉芳，湯澤云，等. 基因表達(dá)式編程輔助的室內(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ìn)機(jī)自主定位定向方法研究[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] 馬永濤，侯振寰，姜啟登，等. 基于超寬帶信號到達(dá)時間的室內(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é)君，等. 基于改進(jìn)的超寬帶?；穫}儲堆垛定位技術(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è)計與試驗(yàn)及誤差分析[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ǔ)公益研究計劃項(xiàng)目（LGN18F030001）；浙江省科技廳重大專項(xiàng)（2016C02G2100540）；浙江省教育廳項(xiàng)目（Y201636110）

姚立健，副教授，博士，主要從事智能農(nóng)業(yè)裝備與農(nóng)業(yè)機(jī)器人等方向研究。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