煤礦井下UWB人員定位系統(tǒng)研究

張海軍， 孫學(xué)成， 趙小虎， 閆坤程

(1.山煤國際能源集團(tuán)股份有限公司 煤業(yè)管理公司晉北分公司， 山西 大同 037101； 2.礦山物聯(lián)網(wǎng)應(yīng)用技術(shù)國家地方聯(lián)合工程實(shí)驗(yàn)室， 江蘇 徐州 221008； 3.中國礦業(yè)大學(xué) 信息與控制工程學(xué)院， 江蘇 徐州 221008)

0 引言

煤礦智能化建設(shè)對井下人員定位系統(tǒng)的精度、實(shí)時性提出了更高要求。與基于紅外探測[1]、超聲波[2]等技術(shù)相比，采用納秒級脈沖信號進(jìn)行通信的UWB(Ultra Wide Band，超寬帶)技術(shù)因其極高的帶寬和時間分辨率[3-6]，可很好地降低NLOS (Non Line of Sight，非視距)干擾，更好地適應(yīng)井下復(fù)雜環(huán)境，實(shí)現(xiàn)厘米級精度定位[7-8]。

為提高UWB定位系統(tǒng)精度，何永平等[9]采用2次粒子濾波方法處理原始數(shù)據(jù)；羅豪龍等[10]針對基于TDOA(Time Delay of Arrival，到達(dá)時間差)的UWB定位系統(tǒng)，采用Chan算法和改進(jìn)的卡爾曼濾波算法，進(jìn)一步消除了NLOS干擾；王浩等[11]針對UWB定位系統(tǒng)中的測距值進(jìn)行卡爾曼濾波后，采用Taylor級數(shù)展開方法得到定位坐標(biāo)，并采用中位值平均濾波方法對定位坐標(biāo)進(jìn)行優(yōu)化，使動態(tài)目標(biāo)定位精度更高。

針對井下無線信號傳輸中的NLOS干擾問題，本文設(shè)計了一種煤礦井下UWB人員定位系統(tǒng)。該系統(tǒng)通過二維區(qū)域判別機(jī)制確定定位區(qū)域，采用帶有回傳幀機(jī)制的ADS-TWR(Asymmetric Double-Sided Two-Way Ranging，非對稱雙邊雙向測距)算法實(shí)現(xiàn)測距，采用WLS(Weighted Least Squares，加權(quán)最小二乘法)與UKF(Unscented Kalman Filter，無損卡爾曼濾波)聯(lián)合定位算法解算位置坐標(biāo)，提高定位精度，同時通過備用鋰電池、LoRa通信技術(shù)等，保證系統(tǒng)應(yīng)急情況下正常工作。

1 系統(tǒng)整體設(shè)計

1.1 系統(tǒng)架構(gòu)

煤礦井下UWB人員定位系統(tǒng)主要由標(biāo)簽、基站(集成LoRa無線節(jié)點(diǎn))、LoRa集中器、工業(yè)以太網(wǎng)、交換機(jī)、上位機(jī)等組成，如圖1所示。

標(biāo)簽參與實(shí)時測距，并運(yùn)行二維區(qū)域判別機(jī)制。基站運(yùn)行帶有回傳幀機(jī)制的ADS-TWR算法，與標(biāo)簽完成實(shí)時測距。系統(tǒng)具有以太網(wǎng)和LoRa2種通信方式。正常情況下，基站將測距數(shù)據(jù)發(fā)送至以太網(wǎng)，經(jīng)交換機(jī)上傳至上位機(jī)進(jìn)行位置解算；在應(yīng)急情況下，基站將測距數(shù)據(jù)發(fā)送至LoRa無線節(jié)點(diǎn)，再通過LoRa集中器將數(shù)據(jù)傳輸至交換機(jī)。LoRa無線節(jié)點(diǎn)可組成Mesh網(wǎng)絡(luò)，根據(jù)井下情況自組網(wǎng)，保證測距數(shù)據(jù)在應(yīng)急情況下正常傳輸。上位機(jī)接收到測距數(shù)據(jù)后，實(shí)時運(yùn)行WLS與UKF聯(lián)合定位算法解算標(biāo)簽位置坐標(biāo)。

圖1 煤礦井下UWB人員定位系統(tǒng)結(jié)構(gòu)Fig.1 Structure of UWB personnel positioning system in coal mine

1.2 系統(tǒng)硬件

標(biāo)簽和基站是系統(tǒng)主要設(shè)備。 標(biāo)簽硬件組成如圖2所示。其主控芯片采用STM32F103，負(fù)責(zé)與UWB信號收發(fā)模塊DW1000通信[12]，并對接收數(shù)據(jù)進(jìn)行處理。DW1000用于收發(fā)UWB信號，并將測距過程中產(chǎn)生的時間戳數(shù)據(jù)發(fā)送至STM32F103。DW1000與STM32F103之間通過SPI(Serial Peripheral Interface，串行外設(shè)接口)通信。 標(biāo)簽采用鋰電池供電方式，通過充電管理模塊實(shí)現(xiàn)充放電管理，通過穩(wěn)壓模塊將鋰電池輸出的3.7 V電壓轉(zhuǎn)換為3.3 V，為STM32F103與DW1000供電。

圖2 標(biāo)簽硬件組成Fig.2 Label hardware construction

基站硬件組成如圖3所示。其UWB信號收發(fā)模塊與標(biāo)簽相同，且具有以太網(wǎng)收發(fā)模塊、交換機(jī)模塊、LoRa通信模塊[13]。主控芯片采用STM32F429，其通過SPI總線與UWB信號收發(fā)模塊通信，通過RMII接口與以太網(wǎng)收發(fā)模塊連接，實(shí)現(xiàn)以太網(wǎng)數(shù)據(jù)傳輸。交換機(jī)模塊可連接多路以太網(wǎng)收發(fā)模塊，實(shí)現(xiàn)基站之間的級聯(lián)。為保證系統(tǒng)應(yīng)急通信功能，設(shè)計了LoRa通信模塊，當(dāng)以太網(wǎng)通信中斷時，啟用LoRa通信方式?；疽凿囯姵刈鳛閭溆秒娫矗?dāng)電源被切斷后啟用鋰電池供電。

圖3 基站硬件組成Fig.3 Base station hardware construction

2 二維區(qū)域判別機(jī)制

在實(shí)際定位場景中， 標(biāo)簽每次定位都會向其周圍的所有基站廣播Poll幀，接收到該數(shù)據(jù)幀的基站向標(biāo)簽發(fā)送Resp幀，而UWB定位系統(tǒng)中一般只需3個基站即可實(shí)現(xiàn)二維定位，因此造成定位過程存在大量無效幀，加大了系統(tǒng)計算量，且影響定位精度。在多基站定位場景中，標(biāo)簽只與其距離最近的基站通信，可同時保證系統(tǒng)定位效率和精度。因此，引入二維區(qū)域判別機(jī)制，以解決定位過程中大量無效幀問題。該機(jī)制對定位區(qū)域進(jìn)行劃分，標(biāo)簽可自動判別自身所處區(qū)域并只與該區(qū)域內(nèi)的基站通信。

二維區(qū)域判別機(jī)制包括帶有回傳幀機(jī)制的ADS-TWR算法、區(qū)域判定策略及區(qū)域校正策略。

2.1 帶有回傳幀機(jī)制的ADS-TWR算法

采用帶有回傳幀機(jī)制的ADS-TWR算法進(jìn)行標(biāo)簽與基站之間的距離測量。ADS-TWR算法利用基站與標(biāo)簽之間的3次通信(收發(fā)Poll，Resp，F(xiàn)inal幀)得到測距過程中信號飛行時間，從而計算標(biāo)簽與基站之間距離。該算法無需標(biāo)簽與基站之間進(jìn)行時鐘同步，可減小由標(biāo)簽與基站之間時鐘偏移造成的測距誤差[14]。為使標(biāo)簽接收到單次測距數(shù)據(jù)后進(jìn)行二維區(qū)域判別，在ADS-TWR算法完成3次通信后加入回傳幀Report，如圖4所示。Report幀中含有標(biāo)簽和基站進(jìn)行1次ADS-TWR后的數(shù)據(jù)?；鞠驑?biāo)簽發(fā)送Report幀后，標(biāo)簽即可知本次測距數(shù)據(jù)，為二維區(qū)域判別提供依據(jù)。

2.2 區(qū)域判定策略

考慮到UWB定位系統(tǒng)中只需3個基站即可實(shí)現(xiàn)定位，為方便基站部署，將基站沿兩側(cè)巷道壁等間距分布，如圖5所示。設(shè)井下巷道內(nèi)共有2n+4個基站A1，A2，…，A2n+4(編號分別為1，2，…，2n+4)，其將巷道劃分為n+1個定位區(qū)域z0，z1，…，zn，每次定位過程中標(biāo)簽只需與其所在區(qū)域內(nèi)4個基站通信即可。沿巷道壁每2個基站之間距離為l，巷道寬度為w。為了保證定位精度，l一般為w的3～4倍，本文取l=4w。

圖4 帶有回傳幀機(jī)制的ADS-TWR算法原理Fig.4 Principle of ADS-TWR algorithm with return frame mechanism

圖5 井下基站部署Fig.5 Underground base station distribution

基于井下基站部署進(jìn)行二維區(qū)域判別。針對基站、測距數(shù)據(jù)及定位區(qū)域，分別建立集合I，D，Z。I中包含所有基站的編號；D中包含單次定位過程中標(biāo)簽通信范圍內(nèi)所有基站通過Report幀發(fā)送給該標(biāo)簽的測距數(shù)據(jù)；Z中包含所有劃分的定位區(qū)域。3個集合的關(guān)系如圖6所示。

圖6 基站、測距數(shù)據(jù)、定位區(qū)域集合關(guān)系Fig.6 Relationship among base station set, ranging data set and positioning area set

標(biāo)簽上電后，根據(jù)D中數(shù)據(jù)判斷5個距離自身最近的基站，從中選出連續(xù)且以奇數(shù)開頭的4個基站編號s1,s2,s3,s4。令s1=2i+1，s2=2i+2，s3=2i+3，s4=2i+4(i=0,1，…，n)，則判定標(biāo)簽處于由基站As1，As2，As3，As4圍成的定位區(qū)域zi中。確定zi后，標(biāo)簽與基站As1，As2，As3，As4通信，根據(jù)ADS-TWR算法分別得出標(biāo)簽與基站As1，As2，As3，As4的距離ds1，ds2，ds3，ds4。標(biāo)簽根據(jù)回傳的測距數(shù)據(jù)，采用WLS算法計算出自身相對于定位區(qū)域zi的坐標(biāo)(x，y)[15]。根據(jù)該坐標(biāo)可判斷標(biāo)簽是否移動到相鄰區(qū)域。

由此得到二維區(qū)域判別模型：

f(D)={s1,s2,s3,s4}s1,s2,s3,s4∈I

(1)

g(s1,s2,s3,s4)=zis1=2i+1

(2)

h(zi)={s1,s2,s3,s4}s1=2i+1,s2=2i+2,

s3=2i+3,s4=2i+4

(3)

(4)

式中f(·)，g(·)，h(·)，p(·)分別為集合D與I、I與Z、Z與I、D與Z的映射函數(shù)。

式(1)表示標(biāo)簽可根據(jù)廣播發(fā)送測距請求后測得的多個測距數(shù)據(jù)判別構(gòu)成該標(biāo)簽所處定位區(qū)域的4個基站。判別準(zhǔn)則為從最小的5個測距數(shù)據(jù)中選出4個連續(xù)的基站編號，且第1個編號為奇數(shù)。

式(2)表示標(biāo)簽可根據(jù)自身所處定位區(qū)域內(nèi)的4個連續(xù)基站編號，判別自身所處定位區(qū)域編號。判別準(zhǔn)則為令該4個連續(xù)基站編號中最小的編號為2i+1，求取i，則定位區(qū)域?yàn)閦i。

式(3)表示定位標(biāo)簽根據(jù)自身所處定位區(qū)域zi，判別該區(qū)域內(nèi)的4個基站編號，之后僅與該4個基站通信。判別準(zhǔn)則為利用已知定位區(qū)域編號計算2i+1，2i+2，2i+3，2i+4。

式(4)表示標(biāo)簽根據(jù)自身所處定位區(qū)域及該區(qū)域內(nèi)4個基站回傳的測距數(shù)據(jù)，判別當(dāng)前定位區(qū)域是否已切換為相鄰區(qū)域，并對標(biāo)簽所處的定位區(qū)域進(jìn)行更新。判別準(zhǔn)則為計算標(biāo)簽相對于該定位區(qū)域的坐標(biāo)，根據(jù)坐標(biāo)判斷是否發(fā)生區(qū)域變換。

根據(jù)二維區(qū)域判別模型，得出區(qū)域判定策略：

(1) 標(biāo)簽上電后，向所有基站廣播Poll幀，標(biāo)簽通信范圍內(nèi)的基站利用帶有回傳幀機(jī)制的ADS-TWR算法進(jìn)行測距，并將測距數(shù)據(jù)發(fā)送至標(biāo)簽，利用式(1)判別基站編號s1—s4。

(2) 標(biāo)簽根據(jù)式(2)及已知的s1—s4判別自身所處定位區(qū)域zi。

(3) 標(biāo)簽根據(jù)式(3)及已知的zi判別該區(qū)域內(nèi)的4個基站編號，只與這4個基站通信。

(4) 標(biāo)簽根據(jù)測距數(shù)據(jù)ds1，ds2，ds3，ds4，采用WLS算法計算自身相對于zi的坐標(biāo)(x，y)。根據(jù)式(4)，當(dāng)x<0且0l且0

(5) 若標(biāo)簽檢測出其他異常情況，則返回步驟(1)。

利用上述區(qū)域判定策略可得標(biāo)簽與基站之間的通信時序，如圖7所示。

圖7 標(biāo)簽與基站通信時序Fig.7 Communication time sequence between tag and base station

2.3 區(qū)域校正策略

井下環(huán)境中存在的電磁干擾、溫濕度變化等使得測距過程中出現(xiàn)異常[16]，由此導(dǎo)致定位誤差甚至失敗。針對該問題，提出了區(qū)域校正策略，實(shí)時校正定位過程中的錯誤位置。其思想為先檢測區(qū)域判別過程中的異常情況，若存在異常則初始化標(biāo)簽，重新進(jìn)行區(qū)域判別。

區(qū)域判別過程中的異常情況主要包括：

(1) 無滿足條件的4個連續(xù)且以奇數(shù)開頭的基站編號。在實(shí)時定位過程中，若基站返回給標(biāo)簽的測距數(shù)據(jù)集合D中最小值對應(yīng)的以奇數(shù)開頭的4個基站編號不連續(xù)或連續(xù)基站編號不足4個，則無法進(jìn)行區(qū)域判別，需進(jìn)行區(qū)域校正。

(2) 根據(jù)ds1，ds2，ds3，ds4無法解算標(biāo)簽相對位置。將ds1，ds2，ds3，ds4代入WLS算法中出現(xiàn)無解情況，說明定位過程中出現(xiàn)測距錯誤，無法解算標(biāo)簽相對位置，需進(jìn)行區(qū)域校正。

(3) 基站無應(yīng)答。標(biāo)簽多次向基站發(fā)送Poll幀后接收不到基站返回的Resp幀，或完成測距后標(biāo)簽接收不到基站返回的Report幀，說明定位過程中有基站脫離標(biāo)簽通信范圍，需進(jìn)行區(qū)域校正。

煤礦井下UWB人員定位系統(tǒng)采用區(qū)域判定和區(qū)域校正策略可準(zhǔn)確判別標(biāo)簽當(dāng)前所在定位區(qū)域，標(biāo)簽僅與該區(qū)域內(nèi)4個基站通信，無需再與其通信范圍內(nèi)的所有基站通信，解決了標(biāo)簽與多基站定位過程中的大量無效幀問題，減小了定位耗時及系統(tǒng)復(fù)雜度。

3 系統(tǒng)工作流程

煤礦井下UWB人員定位系統(tǒng)工作流程如圖8所示。 標(biāo)簽與基站根據(jù)二維區(qū)域判別機(jī)制，在相應(yīng)定位區(qū)域內(nèi)進(jìn)行測距?；緦y距數(shù)據(jù)通過以太網(wǎng)或Mesh網(wǎng)絡(luò)發(fā)送至上位機(jī)，上位機(jī)對測距數(shù)據(jù)進(jìn)行實(shí)時處理并解算標(biāo)簽位置。在位置解算過程中采用WLS與UKF聯(lián)合定位算法[16]，結(jié)合已知的基站坐標(biāo)，先采用WLS求解粗略的標(biāo)簽位置坐標(biāo)，再采用UKF對粗略位置坐標(biāo)進(jìn)行濾波、預(yù)測，得到精確坐標(biāo)，從而降低井下復(fù)雜環(huán)境中的NLOS干擾影響，提高定位精度。

圖8 系統(tǒng)工作流程Fig.8 System working flow

4 系統(tǒng)測試

在長20 m、寬2.5 m、高3 m的區(qū)域內(nèi)進(jìn)行系統(tǒng)測試。該區(qū)域內(nèi)布置6個基站A1—A6，將測試區(qū)域劃分為z0，z12個定位區(qū)域，如圖9所示。

圖9 系統(tǒng)測試區(qū)域Fig.9 System test area

4.1 定位測試

表1 系統(tǒng)靜態(tài)定位測試結(jié)果Table 1 Static positioning test results of the system

在動態(tài)定位測試中，采用滑動支架固定標(biāo)簽，在測試區(qū)域內(nèi)標(biāo)記路線，測試人員來回走動模擬井下NLOS環(huán)境，推動支架按標(biāo)記路線滑行，記錄系統(tǒng)定位坐標(biāo)，并計算定位誤差，結(jié)果如圖10所示。

圖10 系統(tǒng)動態(tài)定位測試結(jié)果Fig.10 Dynamic positioning test results of the system

測試結(jié)果表明：系統(tǒng)動靜態(tài)定位精度較高；在動態(tài)定位測試過程中，標(biāo)簽所屬區(qū)域發(fā)生變化時系統(tǒng)仍能穩(wěn)定輸出定位數(shù)據(jù)，且定位誤差較小；在NLOS環(huán)境下定位誤差較穩(wěn)定，表明系統(tǒng)具有較強(qiáng)的抗NLOS干擾能力。

4.2 應(yīng)急測試

在基站正常供電且工作一段時間后，同時切斷所有基站電源，觀察系統(tǒng)定位數(shù)據(jù)輸出狀況及基站鋰電池供電時間。測試結(jié)果見表2?？煽闯龌驹跀嚯娗闆r下，由鋰電池供電可維持3 h左右正常工作時間。

表2 基站斷電情況下系統(tǒng)應(yīng)急測試結(jié)果Table 2 Emergency test results of the system in case of base station power failure

將基站1作為Mesh網(wǎng)絡(luò)的匯聚節(jié)點(diǎn)。所有基站通過以太網(wǎng)方式傳輸定位數(shù)據(jù)，一段時間后斷開基站5，6用于級聯(lián)的網(wǎng)線，觀察定位數(shù)據(jù)傳輸情況，并計算定位數(shù)據(jù)丟包率與延時，結(jié)果見表3。可看出基站5，6在以太網(wǎng)斷線后，基站內(nèi)LoRa通信模塊經(jīng)歷2 s路由發(fā)現(xiàn)過程，此時定位數(shù)據(jù)傳輸中斷，之后通過LoRa方式傳輸定位數(shù)據(jù)，通信過程中存在一定程度的數(shù)據(jù)包丟失、延時增大等現(xiàn)象，但在無法進(jìn)行以太網(wǎng)通信的應(yīng)急情況下提供了定位數(shù)據(jù)，具有一定實(shí)用價值。

表3 以太網(wǎng)斷線情況下系統(tǒng)應(yīng)急測試結(jié)果Table 3 Emergency test results of the system in case of Ethernet disconnection

5 結(jié)論

(1) 煤礦井下UWB人員定位系統(tǒng)在測距過程中采用帶有回傳幀機(jī)制的ADS-TWR算法，無需時鐘同步，保證了測距精度；采用二維區(qū)域判別機(jī)制，減少了定位過程中的大量無效幀，提高了定位效率；采用WLS與UKF算法對定位坐標(biāo)進(jìn)行求解、濾波、預(yù)測，提高了定位精度。

(2) 系統(tǒng)在硬件設(shè)計上采用備用鋰電池技術(shù)，保證了基站斷電情況下的應(yīng)急供電；采用以太網(wǎng)與LoRa 2種通信方式，在以太網(wǎng)斷線情況下，采用Mesh網(wǎng)絡(luò)傳輸定位數(shù)據(jù)，保證系統(tǒng)應(yīng)急通信。

(3) 測試結(jié)果表明：系統(tǒng)具有較高的動靜態(tài)定位精度，抗NLOS干擾能力較強(qiáng)；在基站斷電或以太網(wǎng)斷線情況下，系統(tǒng)能夠?qū)崿F(xiàn)一段時間內(nèi)的應(yīng)急通信。