基于區(qū)域判定的超寬帶井下高精度定位

方文浩，陸 陽,2，衛(wèi) 星,2

(1.合肥工業(yè)大學(xué) 計算機(jī)與信息學(xué)院，合肥 230009； 2.安全關(guān)鍵工業(yè)測控技術(shù)教育部工程研究中心(合肥工業(yè)大學(xué))，合肥 230009)(*通信作者電子郵箱luyang.hf@126.com)

0 引言

隨著工業(yè)物聯(lián)網(wǎng)與人工智能技術(shù)在煤礦領(lǐng)域的不斷發(fā)展，煤礦井下人員與設(shè)備的位置信息已經(jīng)成為十分重要的基礎(chǔ)信息。目前，多采用射頻識別(Radio Frequency Identification, RFID)、WiFi、ZigBee、藍(lán)牙等無線技術(shù)實現(xiàn)礦井巷道下的物體定位，但由于煤礦井下環(huán)境復(fù)雜，導(dǎo)致以上無線定位技術(shù)普遍存在抗多徑效果差、易受環(huán)境干擾、定位精度差等問題，不能滿足井下日益發(fā)展的各類信息系統(tǒng)對高精度定位的要求。而超寬帶(Ultra WideBand, UWB)定位技術(shù)[1-2]采用帶寬大于500 MHz的納秒級超窄脈沖信號，穿透力強(qiáng)，時間分辨率高，信號在傳輸過程中抗多徑衰弱能力和抗干擾能力強(qiáng)，在井下巷道惡劣環(huán)境中不易受到影響，所以UWB定位可以有效克服傳統(tǒng)井下無線定位技術(shù)的缺陷。

基于測距的節(jié)點定位方法是目前高精度定位采用的主要技術(shù)。為實現(xiàn)高精度測距，Oh等[3]采用了傳播時間測距(Time Of Flight, TOF)算法，并提出了一種有效的預(yù)濾波方法，將接收到的信號轉(zhuǎn)化成正弦曲線，通過對正弦波應(yīng)用預(yù)濾波以消除正弦波的帶外噪聲，完成精確的TOF測距，該方法只是提高TOF估計精度，并未考慮到測距節(jié)點時鐘難以同步給測距精度帶來的巨大影響；Gao等[4]采用了雙向測距(Two Way Ranging, TWR)算法，并提出了一個魯棒最小二乘公式，將周轉(zhuǎn)時間與時鐘偏移作為干擾參數(shù)，采用二階錐松弛技術(shù)近似解決；卞佳興等[5]采用非對稱雙邊雙向測距(Asymmetric Double Sided Two-Way Ranging, ADS-TWR)算法，消除測距節(jié)點間時鐘未同步對精度的影響，同時抑制時鐘偏移引起的誤差，具有較高的精度。在提升定位精度方面，Daely等[6]提出一種基于蜻蜓算法的節(jié)點定位算法，利用蜻蜓算法估算節(jié)點位置；Sinfh等[7]提出了一種基于粒子群優(yōu)化(Particle Swarm Optimization, PSO)算法的節(jié)點定位算法，利用PSO對定位目標(biāo)進(jìn)行坐標(biāo)解析；張會清等[8]提出了基于BP神經(jīng)網(wǎng)絡(luò)和泰勒級數(shù)的定位算法，采用泰勒級數(shù)展開法確定目標(biāo)節(jié)點的坐標(biāo)；Wang等[9]提出了利用歐幾里得和三邊定位算法對目標(biāo)節(jié)點進(jìn)行定位。這些算法雖可提高定位精度，最高可達(dá)20 cm；但復(fù)雜度較高，對定位的實時性有較大影響。本文綜合考慮測距和定位的性能，采用ADS-TWR測距算法和三角形質(zhì)心坐標(biāo)解析算法[10]，在測距和定位的精度滿足要求的基礎(chǔ)上，通過定位算法的低復(fù)雜度減少定位時間。

考慮到井下巷道是封閉狹長空間，長達(dá)幾千米，需進(jìn)行井下多基站布置以完成井下定位。在多基站定位中，Han等[11]提出了一種多基站協(xié)同定位算法，提出用通信機(jī)制和投票機(jī)制來確定目標(biāo)節(jié)點的臨時坐標(biāo)，再使用兩跳基站節(jié)點來協(xié)助定位目標(biāo)節(jié)點；溫培博等[12]利用基站之間的幾何位置進(jìn)行篩選，根據(jù)每個定位目標(biāo)信息完成自適應(yīng)的最少基站的選擇及定位；鮑培明等[13]提出了無線傳感器網(wǎng)絡(luò)中多基站定位的多目標(biāo)蟻群算法，用多螞蟻位置的組合表示多基站的定位，用一組螞蟻并行搜索來獲得定位解。而本文針對狹長巷道的多基站定位，利用UWB通信技術(shù)，通過多基站均勻分布進(jìn)行井下區(qū)域劃分，并在高精度測距的基礎(chǔ)上，進(jìn)一步提出了一種基于ADS-TWR測距的標(biāo)簽區(qū)域判定(Region Determination)策略。標(biāo)簽在首次定位廣播請求幀后，根據(jù)各基站返回的距離，比較獲得距離自身最近的兩個基站，判斷所在區(qū)域，并在下一次定位時，只需向區(qū)域基站發(fā)送請求幀即可，解決了每一次定位標(biāo)簽因廣播請求幀產(chǎn)生大量無效通信的問題，大幅減少通信開銷；并在檢測到區(qū)域發(fā)生異常時，引入?yún)^(qū)域校正機(jī)制，提升定位的準(zhǔn)確性和穩(wěn)定性。

1 系統(tǒng)方案

1.1 系統(tǒng)結(jié)構(gòu)

煤礦井下巷道是由多條狹長隧道組成的封閉空間，在多數(shù)情況下通常只需要完成一維定位，即確定定位目標(biāo)延巷道伸展方向的坐標(biāo)，再結(jié)合巷道的地理信息便可確定其具體位置。一般情況下，可將多個UWB定位基站等間距地分布在巷道壁的一側(cè)[14]。系統(tǒng)結(jié)構(gòu)如圖1所示。

UWB定位系統(tǒng)主要由UWB測距基站、UWB移動標(biāo)簽、以太網(wǎng)交換機(jī)和定位管理顯示平臺四部分構(gòu)成。移動標(biāo)簽周期性地向測距基站發(fā)送UWB信號，測距基站遵循ADS-TWR測距原理與移動標(biāo)簽進(jìn)行信息交互，并測得相對于標(biāo)簽的準(zhǔn)確距離。定位管理顯示平臺通過以太網(wǎng)獲取到測距基站測得的一組距離信息，根據(jù)基站已知坐標(biāo)，進(jìn)行移動標(biāo)簽坐標(biāo)定位解析，并實時顯示。

1.2 節(jié)點設(shè)計

在硬件電路設(shè)計上，UWB測距基站與UWB移動標(biāo)簽基本相同，主要由微控制器模塊、UWB無線收發(fā)模塊、電源模塊構(gòu)成。微控制器是整個節(jié)點的核心，控制節(jié)點各模塊的協(xié)調(diào)運(yùn)作。節(jié)點采用基于ARM Cortex-M3的低功耗主控芯片STM32F107，該芯片含有以太網(wǎng)模塊；UWB無線收發(fā)模塊采用DecaWave公司的DW1000芯片，支持110 kb/s、850 kb/s、6.8 Mb/s三種高數(shù)據(jù)率通信，具有16 MHz和64 MHz兩種脈沖重復(fù)頻率(Pulse Repetition Frequency, PRF)、皮秒級時鐘分辨率、高達(dá)1 331.2 MHz的帶寬、可調(diào)控的發(fā)射功率等特點，為高精度測距和定位提供多種方案?；谛酒倪x擇，設(shè)計節(jié)點原理如圖2。

圖1 系統(tǒng)結(jié)構(gòu)

圖2 定位節(jié)點原理

主控芯片通過串行外設(shè)接口(Serial Peripheral Interface, SPI)與射頻芯片相連，并利用SPI總線協(xié)議對DW1000內(nèi)部寄存器進(jìn)行控制，最終實現(xiàn)通信、測距、定位。

2 ADS-TWR測距

基于測距的實時定位系統(tǒng)中，精確地獲取標(biāo)簽位置坐標(biāo)的必要條件是基站測量出相對于標(biāo)簽的精確距離。ADS-TWR測距算法通過在基站與標(biāo)簽之間完成兩次雙向測距，求得UWB信號在兩節(jié)點間的平均傳播時間ttof，再根據(jù)UWB信號傳播速率計算出節(jié)點間距離。該算法克服了基站與標(biāo)簽時鐘不同步對精度產(chǎn)生影響的缺陷，且能夠有效抑制節(jié)點因時鐘偏移引起的測量誤差[15-16]，為高精度定位提供了高精度的距離信息。ADS-TWR測距原理如圖3。

圖3 ADS-TWR測距原理

按照圖3基站與標(biāo)簽信息的交互方式，ADS-TWR測距的過程如下：

步驟1 由UWB移動標(biāo)簽發(fā)起，向UWB測距基站發(fā)送請求幀Poll，記發(fā)送時刻為t1，基站在a1時刻接收到Poll幀，并經(jīng)過treplyB時間，在a2時刻向標(biāo)簽發(fā)送應(yīng)答幀Resp，則treplyB=a2-a1。

步驟2 記標(biāo)簽在t2時刻接收到Resp幀，則troundA=t2-t1，并經(jīng)過treplyA時間，在t3向基站發(fā)送終止幀F(xiàn)inal，便有treplyA=t3-t2。

步驟3 記基站在a3時刻接收到Final幀，則troundB=a3-a2。

移動標(biāo)簽通過Final幀將troundA、treplyA值發(fā)送給測距基站，測距基站根據(jù)treplyB、troundA、treplyA、troundB值計算出標(biāo)簽到基站的UWB信號單向平均傳播時間ttof為：

(1)

精確的ttof確定后，兩節(jié)點間距離計算公式則為：

d=c×ttof

(2)

其中：c為電磁波傳播速度，為3×108m/s。最終，基站由式(2)計算出相對標(biāo)簽的精確距離d。

3 基于區(qū)域判定的定位機(jī)制

3.1 區(qū)域判定原理

3.1.1 問題描述

對于每一個移動標(biāo)簽，所處位置在不斷變換。通常標(biāo)簽的每一次定位都需要向井下所有基站廣播Poll幀，并與通信范圍內(nèi)的基站按ADS-TWR測距原理實現(xiàn)信息交互并測距，而完成井下移動標(biāo)簽一維定位只需要兩個基站參與測距即可，所以對于這種定位方式，除了與參與定位的兩個基站建立的通信是有效的，其余通信都是無效的，這樣既大量消耗節(jié)點功率，又大幅增加獲取一組距離的時間，嚴(yán)重影響定位的實時性和定位精度。針對此問題，提出了一種基于ADS-TWR測距的標(biāo)簽區(qū)域判定策略，如圖4，在ADS-TWR測距的基礎(chǔ)上，增加發(fā)送一個回傳幀Report。

圖4 基于ADS-TWR的回傳幀工作機(jī)制

標(biāo)簽初始化后，首先向所有基站廣播Poll幀，節(jié)點間通過ADS-TWR測距原理完成通信后，基站計算出相對標(biāo)簽的精確距離，并通過Report幀回傳給標(biāo)簽，標(biāo)簽根據(jù)所有距離信息判斷其所在區(qū)域。區(qū)域確定后，在下一次定位時，只選擇向區(qū)域內(nèi)的基站廣播Poll幀，按圖4方式進(jìn)行信息交互來實現(xiàn)測距和定位。

3.1.2 區(qū)域判定模型

在一維定位中，針對井下多基站情況，將所有基站等間距依次布置在巷道壁一側(cè)，兩個基站便形成一個區(qū)域。若基站標(biāo)簽節(jié)點的通信距離最高可達(dá)m，基站間距l(xiāng)一般可設(shè)置為m/2～m，本文考慮到基站測距的穩(wěn)定性，選取l=m/2，井下基站分布如圖5。

對于標(biāo)簽區(qū)域判定策略，可建立三元組模型：

RD→ {I,D,R}

其中：I表示基站編號的集合；D表示測距基站回傳給同一標(biāo)簽的距離集合；R表示劃分區(qū)域的集合。標(biāo)簽初始化后，根據(jù)距離集D，判斷最近的兩個基站，假設(shè)編號為i、j，滿足j=i+1，基站Ai、Aj便構(gòu)成區(qū)域Rj，則判定標(biāo)簽所處區(qū)域Rj。區(qū)域Rj判定后，標(biāo)簽選擇與基站Ai、Aj通信，Ai、Aj通過ADS-TWR算法測得距離di、dj，因此，區(qū)域判定策略中三元組I、D、R的關(guān)系如圖6所示。

圖5 井下基站分布

圖6 三元組關(guān)系

根據(jù)關(guān)系圖6，得到各元組映射關(guān)系：

f(D)={i,j};i,j∈I

(3)

其中i、j滿足條件di=min(D),dj=min(D-di)或dj=min(D),di=min(D-dj)。

f(i,j)=Rj;i=j-1

(4)

g(Rj)={i,j};i=j-1

(5)

(6)

3.1.3 基本策略

已知區(qū)域判定的三元組模型及其映射關(guān)系，則區(qū)域判定的基本策略如下：

步驟1 移動標(biāo)簽初始化后，廣播請求幀Poll，所有測距基站通過ADS-TWR算法測得距離并通過Report幀回傳給標(biāo)簽，所有距離構(gòu)成距離集D，標(biāo)簽依據(jù)距離最短原則，根據(jù)式(3)判斷兩個最短的距離為di、dj，則選擇基站Ai、Aj作為最近的基站，再根據(jù)式(4)判定所在區(qū)域Rj。

步驟2 區(qū)域Rj確定后，移動標(biāo)簽根據(jù)式(5)確定區(qū)域基站Ai、Aj，便只與Ai、Aj按圖4方式建立UWB通信完成測距，同時標(biāo)簽根據(jù)式(6)判斷標(biāo)簽是否移動到Rj的相鄰區(qū)域。若di、dj有一值大于l，則移動標(biāo)簽所在區(qū)域發(fā)生變化：當(dāng)di>l，區(qū)域Rj=Rj+1；dj>l時，則Rj=Rj-1。

步驟3 若di、dj都小于l，則接下來繼續(xù)進(jìn)行步驟2。

步驟4 若標(biāo)簽檢測出區(qū)域異常，復(fù)位，跳回步驟1，重新進(jìn)行區(qū)域判定。

綜合上述思想，結(jié)合圖5基站分布和標(biāo)簽位置，便得到測距基站與移動標(biāo)簽的工作時序，如圖7。

移動標(biāo)簽T的區(qū)域判定策略的工作機(jī)制為：在測距階段，標(biāo)簽T向基站A0至A4廣播Poll幀，由于基站A4所在位置超過標(biāo)簽T的通信距離，只有基站A0～A3與標(biāo)簽T建立連接，并通過ADS-TWR算法測得相對于T的距離d0～d3；在區(qū)域判定階段，標(biāo)簽T通過基站A0～A3的Report幀獲取到距離信息，并根據(jù)距離集D{d0,d1,d2,d3}選擇A1、A2為最近相鄰基站，判定T所在區(qū)域為R2；此后的定位，標(biāo)簽T只與區(qū)域基站A1、A2按圖4方式通信，并時刻檢測回傳距離d1、d2，判斷區(qū)域的變化和異常。

圖7 區(qū)域判定時序

3.2 區(qū)域校正

井下巷道環(huán)境因素復(fù)雜，如空氣濕度大、空氣中浮游雜塵含量高、電磁環(huán)境多變等，極易影響標(biāo)簽區(qū)域判定的準(zhǔn)確性。針對此問題，本文采用區(qū)域校正方法，在定位過程中，分析圖4信息交互中出現(xiàn)的異常情況，并根據(jù)圖6中三元組映射關(guān)系，檢測區(qū)域判定出現(xiàn)錯誤的信號，若標(biāo)簽實際區(qū)域已不是Rj，立即復(fù)位，進(jìn)行區(qū)域重判。通常，移動標(biāo)簽檢測到以下異常時，便實行區(qū)域校正方法確保區(qū)域Rj正確，維護(hù)定位的準(zhǔn)確性和系統(tǒng)的魯棒性。

1)j≠i+1，即最近基站不相鄰。移動標(biāo)簽上電或復(fù)位后，根據(jù)距離集D，由式(3)返回最近基站編號i、j，假設(shè)j>i，而j≠i+1，則Ai、Aj基站無法判定區(qū)域標(biāo)簽，需進(jìn)行區(qū)域校正。

2)di、dj同時大于l。定位過程中，標(biāo)簽檢測區(qū)域基站Ai、Aj返回的距離di、dj，若di>l以及dj>l，則區(qū)域Rj已不是標(biāo)簽實際所在區(qū)域，需進(jìn)行區(qū)域校正。

3)基站無響應(yīng)。定位過程中，標(biāo)簽多次嘗試向區(qū)域基站Ai或Aj發(fā)送Poll幀時，未接收到Ai或Aj的應(yīng)答幀Resp，表明基站Ai、Aj至少有一個基站已不在標(biāo)簽通信范圍內(nèi)，需進(jìn)行區(qū)域校正。

4)距離回傳超時。定位過程中，區(qū)域基站Ai、Aj向標(biāo)簽回傳距離后，標(biāo)簽接收回傳幀Report超時，即標(biāo)簽無法確定距離di、dj，因此便無法根據(jù)式(6)判斷區(qū)域Rj的變化以及異常的產(chǎn)生，影響定位的實現(xiàn)，需進(jìn)行區(qū)域校正。

3.3 基于區(qū)域判定的定位機(jī)制

引入“區(qū)域判定”和“區(qū)域校正”思想后，移動標(biāo)簽在定位過程中能夠準(zhǔn)確判斷其所在區(qū)域，只需與區(qū)域基站通信便能完成定位，解決了傳統(tǒng)多基站定位系統(tǒng)中所有基站參與同一標(biāo)簽定位的問題，降低了系統(tǒng)復(fù)雜度。

圖8為基于區(qū)域判定的定位流程。標(biāo)簽區(qū)域Rj確定后，其后每一次定位都向區(qū)域基站Ai、Aj發(fā)送請求幀Poll進(jìn)行信息交互，基站Ai、Aj根據(jù)ADS-TWR測距算法計算出精確距離di、dj，由定位管理顯示平臺獲取到di、dj，采用定位算法實現(xiàn)移動標(biāo)簽的一維定位。

本系統(tǒng)采用三角形質(zhì)心算法求解移動標(biāo)簽的坐標(biāo)，該算法模型如圖9?；続i、Aj與標(biāo)簽T的測距值為di、dj，以Ai、Aj為圓心，對應(yīng)距離di、dj為半徑畫圓，Ai、Aj的連接線將畫出的圓截取成兩個半圓，得到標(biāo)簽的3個近似位置特征點B、C、D，形成三角形△BCD，則所求解的標(biāo)簽延巷道伸展方向的坐標(biāo)xT為：

xT=(xb+xc+xd)/3

(7)

其中：xb表示特征點B的橫坐標(biāo)；xc表示特征點C的橫坐標(biāo)；xd表示特征點D的橫坐標(biāo)。定位管理顯示平臺根據(jù)Ai、Aj的固定坐標(biāo)及di、dj計算出xb、xc、xd，再利用式(7)估算出移動標(biāo)簽標(biāo)延巷道伸展方向的橫坐標(biāo)xT，完成一維定位。

圖8 基于區(qū)域判定的定位流程

圖9 三角形質(zhì)心算法模型

三角形質(zhì)心算法可削弱節(jié)點測距誤差對定位精度的影響，滿足定位系統(tǒng)的高精度需求；同時，該算法的計算量小，易于實現(xiàn)，處理時間短，有利于定位實時性處理。

4 實驗結(jié)果及分析

應(yīng)用本文設(shè)計的基站與標(biāo)簽硬件平臺，系統(tǒng)的性能還將受DW1000芯片參數(shù)設(shè)置的影響。通過測距實驗驗證，越大的PRF和帶寬具有越高精度的測距，且越低的數(shù)據(jù)速率和越大的發(fā)射功率具有越長的通信距離，因此設(shè)置基站標(biāo)簽節(jié)點參數(shù)為110 kb/s的傳輸數(shù)據(jù)率，16 MHz低功耗PRF，1 331.2 MHz帶寬，最大發(fā)射功率，以實現(xiàn)高精度的測距及最大距離的通信，同時兼顧節(jié)點功耗，并按照圖1系統(tǒng)結(jié)構(gòu)在長100 m、寬2 m、高3 m的封閉模擬環(huán)境下搭建實驗平臺，設(shè)基站間距l(xiāng)=50 m，布置3個基站，構(gòu)成兩個區(qū)域R1、R2，然后運(yùn)行本文設(shè)計的多基站定位系統(tǒng)，獲取測試數(shù)據(jù)，分析系統(tǒng)性能。

4.1 測距誤差

測距誤差是影響定位精度的重要因素之一，利用基站標(biāo)簽在R1、R2區(qū)域?qū)Χ鄠€實際距離進(jìn)行分組測距實驗，每組測量多個數(shù)據(jù)，并人工計算測距誤差。定義節(jié)點測距誤差為：

de=dr-ds

(8)

表1測距結(jié)果表明，基站與標(biāo)簽的測距誤差控制在20 cm以內(nèi)，某些時刻可達(dá)10 cm，總平均誤差為11.26 cm，實現(xiàn)了高精度測距，且相對于每個實際距離值，測距值都是正偏，所以采用三角形質(zhì)心算法作為坐標(biāo)求解算法非常合適。同時，測距的穩(wěn)定性較好，測距值波動幅度不大。

表1 ADS-TWR測距誤差統(tǒng)計

4.2 定位誤差

(9)

定位系統(tǒng)運(yùn)行時，標(biāo)簽T在區(qū)域R1、R2內(nèi)移動，每10 m內(nèi)標(biāo)簽被靜態(tài)地統(tǒng)計定位5次，將50個定位結(jié)果通過Matlab擬合成標(biāo)簽橫坐標(biāo)測量值曲線，結(jié)果如圖10，并根據(jù)式(9)計算每一次的定位誤差，得到結(jié)果如圖11。同時，在每個區(qū)域內(nèi)選取10個基準(zhǔn)點，給出具體的定位數(shù)據(jù)及誤差EX，如表2。

圖10 標(biāo)簽橫坐標(biāo)測量值與實際值對比

圖10結(jié)果顯示，仿真后的測量值曲線與實際值曲線近乎重合，且隨機(jī)放大區(qū)域的測量值偏移也很小，表明定位誤差很小。圖11結(jié)果表明，最差情況下，定位誤差可達(dá)到15 cm，某些時刻可實現(xiàn)厘米級定位，進(jìn)一步說明本系統(tǒng)具有高精度定位的特點。

表2 定位誤差EX統(tǒng)計

結(jié)合表1和表2，實驗數(shù)據(jù)表明，在高精度測距的基礎(chǔ)上，三角形質(zhì)心算法有效削弱了測距誤差對定位精度的影響，使得移動標(biāo)簽定位誤差EX的平均值達(dá)到5.925 cm，相對于總平均測距誤差，降低約47.4%。結(jié)合表2和圖11可得，系統(tǒng)的定位精度穩(wěn)定在15 cm以內(nèi)。而且，移動標(biāo)簽所在區(qū)域發(fā)生變換后，系統(tǒng)依舊能夠準(zhǔn)確地進(jìn)行定位，定位精度也保持在一個較高的指標(biāo)范圍之內(nèi)。

圖11 定位誤差

5 結(jié)語

本文針對井下狹長巷道的多基站定位問題，設(shè)計實現(xiàn)了一套基于UWB的高精度定位系統(tǒng)。該系統(tǒng)采用ADS-TWR測距技術(shù)，有效抑制了節(jié)點時鐘偏移引起的誤差，實現(xiàn)了高精度測距，為井下高精度定位提供了保障；采用三角形質(zhì)心算法，削弱測距誤差對定位精度的影響，進(jìn)一步提高定位精度。針對井下多基站定位中，標(biāo)簽每次定位都需向所有基站廣播請求幀的問題，該系統(tǒng)采用一種基于ADS-TWR測距的區(qū)域判定策略，標(biāo)簽通過判定所在區(qū)域，定位時只與區(qū)域基站通信，避免重復(fù)地與未參與定位的基站建立連接，大幅減少了節(jié)點間的無效通信，同時引入?yún)^(qū)域校正機(jī)制，提升了區(qū)域判定的準(zhǔn)確性，增強(qiáng)了定位的可靠性與穩(wěn)定性。