黃騰揚(yáng)，許 航，黎銘宇，樊 越，黃 龍，鄧嫄媛

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

0 引言

在各種不同的無線網(wǎng)絡(luò)環(huán)境中，不管是服務(wù)工業(yè)、農(nóng)業(yè)，還是日常生活，穩(wěn)定、快速、準(zhǔn)確地獲取移動(dòng)位置信息的定位技術(shù)及與其匹配的定位系統(tǒng)已經(jīng)成為當(dāng)前的研究熱點(diǎn)。因?yàn)閲覍?duì)UWB超寬帶技術(shù)的功率有所限制，很少有將超寬帶技術(shù)面向農(nóng)業(yè)方向的研究，隨著國家工信部于2008年12月6日以來對(duì)超寬帶技術(shù)使用頻率和功率的規(guī)定日漸完善，大多數(shù)對(duì)于超寬帶技術(shù)的研究仍然是圍繞室內(nèi)定位系統(tǒng)展開的開發(fā)與設(shè)計(jì)?？紤]到未來農(nóng)業(yè)地區(qū)多樣性，農(nóng)作區(qū)域也不僅僅限于大面積定位的平原田間，需要不同的定位技術(shù)去針對(duì)不同的農(nóng)作環(huán)境。在丘陵地區(qū)果園農(nóng)業(yè)中，因?yàn)榈匦螐?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è)計(jì)提供參考。

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

1.1 定位算法選擇

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

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

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

圖1 三邊測(cè)量法原理圖

(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)可以得到交點(diǎn)D的坐標(biāo)為

(3)

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

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

因?yàn)槎ㄎ徽`差的不確定性，分以下兩種情況來考慮定位算法。

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

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

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

(4)

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

(5)

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

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

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

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

(6)

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

(7)

AX=b

(8)

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

用最小二乘法求解得X為

X=(ATA)-1ATb

(9)

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

2.1 UWB定位系統(tǒng)實(shí)驗(yàn)平臺(tái)

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

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

圖5 UWB定位系統(tǒng)實(shí)驗(yàn)平臺(tái)結(jié)構(gòu)圖

2.2 實(shí)驗(yàn)設(shè)計(jì)與環(huán)境布置

室內(nèi)外實(shí)驗(yàn)空間布置分別如圖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)實(shí)驗(yàn)空間布置圖

圖7 室外實(shí)驗(yàn)空間布置圖

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

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

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

2.3 定位系統(tǒng)測(cè)試實(shí)驗(yàn)

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

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

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

(10)

(11)

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

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

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

(12)

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

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

圖13 動(dòng)態(tài)定位樣點(diǎn)

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

Xj=X-Xp

(13)

Yj=Y-Yp

(14)

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

(15)

3 實(shí)驗(yàn)結(jié)果分析

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

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

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

圖15 室外定位點(diǎn)分布

根據(jù)第2節(jié)所述方法分析計(jì)算得到靜態(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)航過程中對(duì)定位系統(tǒng)精度的要求。

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

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

圖16 室內(nèi)動(dòng)態(tài)定位點(diǎn)

圖17 室外動(dòng)態(tài)定位點(diǎn)

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

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

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

4 結(jié)論

本文將UWB定位技術(shù)在算法和硬件上進(jìn)行優(yōu)化，設(shè)計(jì)搭建了一種基于UWB超寬帶技術(shù)的導(dǎo)航定位控制系統(tǒng)，該系統(tǒng)適用于室內(nèi)和室外導(dǎo)航定位，并設(shè)計(jì)實(shí)驗(yàn)對(duì)該導(dǎo)航定位控制系統(tǒng)在室內(nèi)外的靜態(tài)定位精度與動(dòng)態(tài)導(dǎo)航跟蹤精度進(jìn)行了測(cè)試。實(shí)驗(yàn)表明所設(shè)計(jì)的導(dǎo)航定位控制系統(tǒng)在室內(nèi)和室外環(huán)境下均具有較為穩(wěn)定的靜態(tài)定位精度與動(dòng)態(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)