基于UWB超寬帶技術(shù)的無人駕駛農(nóng)機(jī)導(dǎo)航定位技術(shù)研究

黃騰揚，許 航，黎銘宇，樊 越，黃 龍，鄧嫄媛

(成都大學(xué) 機(jī)械工程學(xué)院，成都 610106)

0 引言

在各種不同的無線網(wǎng)絡(luò)環(huán)境中，不管是服務(wù)工業(yè)、農(nóng)業(yè)，還是日常生活，穩(wěn)定、快速、準(zhǔn)確地獲取移動位置信息的定位技術(shù)及與其匹配的定位系統(tǒng)已經(jīng)成為當(dāng)前的研究熱點。因為國家對UWB超寬帶技術(shù)的功率有所限制，很少有將超寬帶技術(shù)面向農(nóng)業(yè)方向的研究，隨著國家工信部于2008年12月6日以來對超寬帶技術(shù)使用頻率和功率的規(guī)定日漸完善，大多數(shù)對于超寬帶技術(shù)的研究仍然是圍繞室內(nèi)定位系統(tǒng)展開的開發(fā)與設(shè)計。考慮到未來農(nóng)業(yè)地區(qū)多樣性，農(nóng)作區(qū)域也不僅僅限于大面積定位的平原田間，需要不同的定位技術(shù)去針對不同的農(nóng)作環(huán)境。在丘陵地區(qū)果園農(nóng)業(yè)中，因為地形復(fù)雜，田地狹小且難以抵達(dá)，大型農(nóng)機(jī)應(yīng)用困難，只適合少數(shù)小型農(nóng)機(jī)作業(yè)，而GNSS等傳統(tǒng)的定位技術(shù)，在此類地區(qū)的性能表現(xiàn)遠(yuǎn)不及平原農(nóng)業(yè)地區(qū)。為探索適應(yīng)不同地區(qū)的不同環(huán)境定位技術(shù)，本文旨在將室內(nèi)定位性能優(yōu)秀的UWB技術(shù)融入到室外傳統(tǒng)農(nóng)業(yè)機(jī)械中，提出一種室外導(dǎo)航定位系統(tǒng)的開發(fā)思路，為室內(nèi)外一體化定位系統(tǒng)設(shè)計提供參考。

1 針對室外環(huán)境應(yīng)用的算法選擇與優(yōu)化

1.1 定位算法選擇

在應(yīng)用合適的測距算法得到相應(yīng)距離信息后，要使系統(tǒng)具備定位功能，就要固定基站以確定基準(zhǔn)點，通過附近作為基準(zhǔn)點的基站為農(nóng)機(jī)導(dǎo)航的移動節(jié)點實現(xiàn)定位。針對不同需求，如應(yīng)用定位系統(tǒng)規(guī)劃路線或判斷記錄行駛軌跡，UWB定位算法的分類也不同。目前，常用的無線定位測量方法有AOA(Angle of Arriva, 到達(dá)角度定位)、TOA(Time of Arriva, 到達(dá)時間定位)、TDOA(Time Difference of Arriva, 到達(dá)時間差定位)和RSS(Received Signal Strength, 接收信號強(qiáng)度定位)，上述算法在不同應(yīng)用環(huán)境下都具備各自的優(yōu)勢。而其中TOA技術(shù)指的是由基站向移動站發(fā)出特定的測距命令或指令信號，并且要求終端對該指令進(jìn)行響應(yīng)。基站會記錄下由發(fā)出測距指令到收到終端確認(rèn)信號所花費的時間，該時間主要由射頻信號在環(huán)路上的傳播時延、終端的響應(yīng)時延和處理時延以及基站的處理時延組成。只要能夠得到終端和基站的響應(yīng)和處理時延，就可以準(zhǔn)確算出射頻信號的環(huán)路傳播時延。

針對在室外難以處處精確化處理和無法避免時間飄移的實際情況，考慮利用TOA技術(shù)處理測距數(shù)值、確定位置信息，能夠最大程度地改善UWB技術(shù)在室外定位系統(tǒng)中的應(yīng)用。

結(jié)合Trilateration 三邊測量法，能夠快速確定位置信息。三邊測量法的原理如圖1所示，以三個節(jié)點A、B、C為圓心畫圓，坐標(biāo)分別是(XA,YA)、(XB,YB)、(XC,YC),這三個圓周相交于點D，交點D即為移動節(jié)點，A、B、C即為參考節(jié)點，A、B、C與交點D的距離分別為da、db、dc,假設(shè)交點D的坐標(biāo)為(X,Y)，則

圖1 三邊測量法原理圖

(1)

由式(1)變化可得式(2)

(X-XA)2+(Y-YA)2=da2
(X-XB)2+(Y-YB)2=db2
(X-XC)2+(Y-YC)2=dc2

(2)

由式(3)可以得到交點D的坐標(biāo)為

(3)

理想狀態(tài)下，已知平面內(nèi)三點坐標(biāo)，知道被測點D到A、B、C的距離，得出被測點D的坐標(biāo)。通過TWR測距方法，分別對A、B、C測距，即可得出自身坐標(biāo)。

1.2 定位算法優(yōu)化

因為定位誤差的不確定性，分以下兩種情況來考慮定位算法。

第一種情況：兩個及以上圓完全不相交，如圖2所示。

圖2 質(zhì)心算法優(yōu)化概念圖

在這種情況下必須采取一定方法選取點A、B、C，才能進(jìn)行真實位置點的計算，常用的方法是用半徑比例進(jìn)行計算

(4)

同理可以求出其余兩點(Xac,Yac)和(Xbc,Ybc)的坐標(biāo)，進(jìn)而應(yīng)用三角形質(zhì)心求出定位點坐標(biāo)

(5)

第二種情況：兩個圓彼此相交于一片區(qū)域，如圖3、圖4所示。此類情況應(yīng)用最小二乘法進(jìn)行優(yōu)化。

圖3 距離區(qū)域包含定位點

圖4 距離區(qū)域不包含定位點

已知n個基站的坐標(biāo)和到未知節(jié)點距離，建立方程組(6)

(6)

用方程組中前n-1個方程減去第n個方程后，得到線性化的方程式(7)

(7)

AX=b

(8)

其中A與b是系數(shù)矩陣，X是一系列離散點的集合。

用最小二乘法求解得X為

X=(ATA)-1ATb

(9)

2 UWB定位技術(shù)室內(nèi)外定位實驗

2.1 UWB定位系統(tǒng)實驗平臺

本實驗利用Robomaster EP小車來模擬農(nóng)機(jī)，通過官方給出的API接口使小車作為本文所搭建的UWB超寬帶定位系統(tǒng)的實驗平臺，分別對室內(nèi)外不同環(huán)境下導(dǎo)航定位控制系統(tǒng)的靜態(tài)定位精度和動態(tài)導(dǎo)航跟蹤精度進(jìn)行實驗測試。

標(biāo)簽主要搭載在Robomaster EP上，標(biāo)簽的位置信息在完成數(shù)據(jù)處理后被所有基站接收儲存，并由其中一個與上位機(jī)(PC)直連的基站將位置數(shù)據(jù)信息通過API通道對小車進(jìn)行運動控制。UWB定位系統(tǒng)實驗平臺結(jié)構(gòu)圖如圖5所示。實驗通過比較并統(tǒng)計小車于UWB定位環(huán)境中的參考位置和實驗環(huán)境下的實際位置，獲得系統(tǒng)誤差，通過誤差分析處理，對定位系統(tǒng)進(jìn)行相應(yīng)調(diào)整。

圖5 UWB定位系統(tǒng)實驗平臺結(jié)構(gòu)圖

2.2 實驗設(shè)計與環(huán)境布置

室內(nèi)外實驗空間布置分別如圖6和圖7所示，圖中水平向量為X軸正向，豎直向量為Y軸正向，垂直向量Z軸正向。室內(nèi)外基站位置確保完全相同，基站A1、A2、A3的坐標(biāo)分別為(0 cm,0 cm,180 cm), (650 cm,0 cm,180 cm),(0 cm,685 cm,180 cm)。

圖6 室內(nèi)實驗空間布置圖

圖7 室外實驗空間布置圖

以T0(0，0，0)為原點，自原點開始在選定實驗區(qū)域XY平面上等間距布置灰度值1～99的不同灰度標(biāo)簽，如圖8和圖9所示，用于輔助動態(tài)定位精度實驗。在不同灰度標(biāo)志點隨機(jī)選取一點，精確測量其在實驗空間中的坐標(biāo)，為靜態(tài)定位精度測試提供參考。

圖8 室內(nèi)標(biāo)簽布置圖

圖9 室外標(biāo)簽布置圖

2.3 定位系統(tǒng)測試實驗

為了減少影響定位系統(tǒng)實驗的變量，室內(nèi)外的定位跟蹤精度測試步驟完全相同。靜態(tài)定位精度測量方面，手動放置標(biāo)簽于實驗環(huán)境搭建時選取好的標(biāo)記上，如圖10所示。對電腦端定位樣點圖(圖11)上的100個定位樣點(Xi，Yi)位置坐標(biāo)進(jìn)行數(shù)據(jù)統(tǒng)計，如公式(10)和(11)，求得均值點P(Xp，Yp)。

圖10 靜態(tài)定位精度測試

圖11 靜態(tài)定位樣點

(10)

(11)

式中Xp—橫坐標(biāo)的系統(tǒng)誤差；

Yp—縱坐標(biāo)的系統(tǒng)誤差。

求得均值點與真值點的距離即可得到定位系統(tǒng)距離上的系統(tǒng)誤差Dp，帶入式(12)求得靜態(tài)定位精度

(12)

于Robomaster EP上外接灰度傳感器，將UWB定位標(biāo)簽置于灰度傳感器Robomaser EP車身平面對稱處，確保標(biāo)簽與灰度傳感器連線與地面垂直，如圖12所示。啟動小車使小車在指定區(qū)域內(nèi)隨機(jī)行駛，車輛到達(dá)0～99不同灰度點時，車載灰度傳感器發(fā)送信號給PC，上位機(jī)接受信號后，記錄當(dāng)前時刻的位置數(shù)據(jù)，于電腦端定位樣點圖(圖13)完整記錄小車到達(dá)所有灰度點時的系統(tǒng)數(shù)據(jù)。

圖12 灰度傳感器與車載標(biāo)簽布置圖

圖13 動態(tài)定位樣點

通過式(13)和(14)得到去除系統(tǒng)誤差后的動態(tài)定位點坐標(biāo)(Xj,Yj)

Xj=X-Xp

(13)

Yj=Y-Yp

(14)

得到優(yōu)化后的距離標(biāo)準(zhǔn)差Dj后，再利用公式(15)求得動態(tài)定位精度

(15)

3 實驗結(jié)果分析

3.1 靜態(tài)定位精度

用上位機(jī)記錄室內(nèi)外靜態(tài)定位精度測試數(shù)據(jù)分別如圖14和圖15所示。

圖14 室內(nèi)定位點分布

圖15 室外定位點分布

根據(jù)第2節(jié)所述方法分析計算得到靜態(tài)定位精度數(shù)據(jù)如表1所示。

表1 靜態(tài)定位精度數(shù)據(jù)表(cm)

從表1數(shù)據(jù)可以看出，本系統(tǒng)的靜態(tài)定位精度于室內(nèi)外表現(xiàn)穩(wěn)定且良好，能夠達(dá)到農(nóng)機(jī)導(dǎo)航過程中對定位系統(tǒng)精度的要求。

3.2 動態(tài)導(dǎo)航跟蹤精度

車輛到達(dá)室內(nèi)外0～99不同灰度點時，在定位系統(tǒng)當(dāng)中的位置信息分別如圖16和圖17所示。

圖16 室內(nèi)動態(tài)定位點

圖17 室外動態(tài)定位點

根據(jù)第2節(jié)所述方法分析計算得到動態(tài)導(dǎo)航跟蹤精度數(shù)據(jù)如表2所示。

表2 動態(tài)導(dǎo)航跟蹤精度數(shù)據(jù)表(cm)

從2表數(shù)據(jù)可以看出，本系統(tǒng)在室內(nèi)的動態(tài)導(dǎo)航跟蹤精度較高，能夠控制在5 cm以內(nèi)，室外情況下的動態(tài)導(dǎo)航跟蹤精度雖然低于室內(nèi)情況，仍可控制在10 cm左右，能夠滿足農(nóng)機(jī)自主行駛過程中對導(dǎo)航跟蹤精度的需求。

4 結(jié)論

本文將UWB定位技術(shù)在算法和硬件上進(jìn)行優(yōu)化，設(shè)計搭建了一種基于UWB超寬帶技術(shù)的導(dǎo)航定位控制系統(tǒng)，該系統(tǒng)適用于室內(nèi)和室外導(dǎo)航定位，并設(shè)計實驗對該導(dǎo)航定位控制系統(tǒng)在室內(nèi)外的靜態(tài)定位精度與動態(tài)導(dǎo)航跟蹤精度進(jìn)行了測試。實驗表明所設(shè)計的導(dǎo)航定位控制系統(tǒng)在室內(nèi)和室外環(huán)境下均具有較為穩(wěn)定的靜態(tài)定位精度與動態(tài)導(dǎo)航跟蹤精度，能夠滿足無人駕駛農(nóng)機(jī)導(dǎo)航定位需求。由此證明本UWB定位系統(tǒng)不僅在室內(nèi)環(huán)境下具備較高的可靠性和穩(wěn)定性，在室外局域范圍內(nèi)也有相當(dāng)可觀的性能，具有較為廣闊的開發(fā)應(yīng)用前景。

(05)