基于UWB的礦井巷道精準(zhǔn)三維定位系統(tǒng)設(shè)計(jì)

沈國(guó)杰，周德勝，鄭雪娜，李 敏

(1.重慶水利電力職業(yè)技術(shù)學(xué)院，重慶 402160； 2.中煤科工集團(tuán)重慶研究院有限公司，重慶 400039；3.重慶大學(xué) 城市科技學(xué)院，重慶 402167)

煤礦井下人員和設(shè)備的精確定位是當(dāng)今煤炭領(lǐng)域的熱門(mén)研究課題，是礦井工人生命安全和設(shè)備資產(chǎn)安全的重要保障[1]。近年來(lái)，物聯(lián)網(wǎng)、人工智能技術(shù)在煤礦安全生產(chǎn)中迅速展開(kāi)應(yīng)用，尤其是智能開(kāi)采、機(jī)車無(wú)人駕駛、機(jī)器人自動(dòng)巡檢等技術(shù)的突飛猛進(jìn)，對(duì)井下定位的精度和實(shí)時(shí)性都提出了更高的要求，傳統(tǒng)的井下定位系統(tǒng)已經(jīng)不能滿足技術(shù)革新的需求[2]。

煤礦井下巷道彎曲、傾斜、表面粗糙，圍巖介質(zhì)復(fù)雜，金屬鉚釘、鋼網(wǎng)眾多，電纜管道布局繁雜，無(wú)線信號(hào)傳輸時(shí)存在嚴(yán)重畸變和衰減，常見(jiàn)的基于信號(hào)強(qiáng)度和基于角度測(cè)量的定位算法不適用于井下人員和設(shè)備定位[3-4]，而基于時(shí)間測(cè)量的定位算法測(cè)量精度只與信號(hào)傳輸時(shí)間有關(guān)，非常適合應(yīng)用于煤礦巷道等封閉空間的定位。常見(jiàn)的室內(nèi)無(wú)線定位技術(shù)，如紅外、藍(lán)牙、WIFI、超聲波、4G等技術(shù)，功耗大、信號(hào)穿透能力不強(qiáng)、受非視距影響嚴(yán)重，在煤礦巷道的特殊工況環(huán)境下應(yīng)用嚴(yán)重受限。超寬帶定位技術(shù)(UWB)采用納秒級(jí)的超窄脈沖傳輸數(shù)據(jù)，時(shí)間分辨率高，超窄脈沖占空比低、功耗低、帶寬高、穿透能力和抗干擾能力強(qiáng)，對(duì)其他無(wú)線通信系統(tǒng)干擾弱，在110 kbit/s低速模式下通信范圍可達(dá)數(shù)百米，非常適合在工況環(huán)境惡劣的煤礦井下使用[5-9]。

筆者擬采用低速率UWB技術(shù)設(shè)計(jì)礦井巷道人員與設(shè)備精確定位系統(tǒng)，采用基于到達(dá)時(shí)間差的TDOA算法計(jì)算標(biāo)簽到基站的距離，利用卡爾曼濾波對(duì)非視距等干擾信號(hào)進(jìn)行濾波，最后采用Chan多基站算法計(jì)算標(biāo)簽的三維坐標(biāo)。

1 UWB定位系統(tǒng)設(shè)計(jì)

煤礦井下巷道是封閉式的不規(guī)則空間，為實(shí)現(xiàn)定位信號(hào)全覆蓋，需要布置多基站，以多基站協(xié)同計(jì)算實(shí)現(xiàn)高精度定位。設(shè)計(jì)的UWB定位系統(tǒng)結(jié)構(gòu)如圖1 所示。

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

定位標(biāo)簽根據(jù)自身對(duì)位置信息的實(shí)時(shí)性需求定期向基站發(fā)送定位請(qǐng)求，基站收到定位請(qǐng)求后將其發(fā)送給服務(wù)器，若服務(wù)器允許則向收到定位請(qǐng)求的基站及其鄰近基站發(fā)出對(duì)該標(biāo)簽進(jìn)行定位掃描的命令，基站對(duì)標(biāo)簽進(jìn)行定位掃描后將數(shù)據(jù)返回至服務(wù)器進(jìn)行計(jì)算，得到標(biāo)簽的具體位置[10-11]。

為實(shí)現(xiàn)三維定位，標(biāo)簽至少需要處于4個(gè)基站的信號(hào)覆蓋范圍之內(nèi)，且4個(gè)基站不在同一平面。巷道基站布置如圖2所示。在巷道中每間隔50 m的橫截面上布置基站，每個(gè)截面上布置2個(gè)，且相鄰 2個(gè)截面的基站一個(gè)布置在頂部，另一個(gè)布置在對(duì)角線上，為減小來(lái)自巷道壁的干擾，基站與巷道壁保持0.5 m以上距離。

圖2 巷道基站布置示意圖

2 UWB硬件模塊設(shè)計(jì)

為簡(jiǎn)化設(shè)計(jì)，定位基站和定位標(biāo)簽采用相同的UWB模塊電路，模塊框圖如圖3所示。

圖3 UWB核心模塊框圖

模塊采用Decawave公司的DW1000芯片作為UWB信號(hào)處理芯片，其符合IEEE 802.15.4—2011無(wú)線標(biāo)準(zhǔn)，采用相干接收技術(shù)，并對(duì)室內(nèi)精確定位和通信進(jìn)行了特別優(yōu)化，在110 kbit/s傳輸速率下最遠(yuǎn)傳輸距離可達(dá)450 m(視距)。為增強(qiáng)抗干擾能力，模塊對(duì)無(wú)線射頻信號(hào)進(jìn)行了放大，采用TPS61240芯片為射頻功放電路供電，利用三態(tài)電源緩沖器SN74LV1T125增強(qiáng)電源電路的抗振能力，提高魯棒性；為減小整機(jī)功耗，只有在定位時(shí)，才將TPS61240的EN引腳設(shè)為功放電路供電。模塊采用全向天線，射頻放大電路原理如圖4所示。

圖4 射頻放大電路原理圖

3 算法設(shè)計(jì)

定位通常是先采用恰當(dāng)?shù)姆绞将@得目標(biāo)到基準(zhǔn)的距離，再結(jié)合基準(zhǔn)的已知坐標(biāo)計(jì)算待測(cè)坐標(biāo)。通常利用信號(hào)飛行時(shí)間、信號(hào)到達(dá)角度或者到達(dá)信號(hào)強(qiáng)度等方法獲取距離。鑒于礦井巷道特殊的工況環(huán)境，基站布置很難保持統(tǒng)一標(biāo)準(zhǔn)，設(shè)備和人員流動(dòng)性、隨意性較強(qiáng)，標(biāo)簽時(shí)鐘同步困難，筆者采用基站接收信號(hào)到達(dá)時(shí)間差(TDOA)算法計(jì)算距離，并對(duì)基于到達(dá)時(shí)間差計(jì)算的距離進(jìn)行卡爾曼濾波處理以減小非視距等干擾，最后結(jié)合Chan算法，合理利用多個(gè)基站的數(shù)據(jù)進(jìn)行三維坐標(biāo)計(jì)算[12-14]。

TDOA三維定位算法可等效為雙曲面方程，設(shè)標(biāo)簽坐標(biāo)為L(zhǎng)B(x,y,z)，已知基站i的坐標(biāo)為BS(xi,yi,zi)，則標(biāo)簽到基站i的距離ri為：

(1)

標(biāo)簽到第i個(gè)基站與第一個(gè)基站的距離差ri1為：

ri1=ri-r1

(2)

ri1=c(ti-t1)=cΔti1

(3)

式中：c為光速；ti為第i個(gè)基站接收到信號(hào)的時(shí)間。

當(dāng)基站數(shù)等于4個(gè)的時(shí)候，由公式(1)、(2)、(3)便可求得標(biāo)簽坐標(biāo)。

3.1 卡爾曼濾波

由于礦井巷道空間不規(guī)則，人員和設(shè)備流動(dòng)性大，存在嚴(yán)重的非視距誤差(NLOS)，實(shí)際距離差值可由公式(4)表示:

rn1(tk)=Dn1(tk)+NLOSn1(tk)+w(tk)

(4)

式中：Dn1(tk)為視距下的理論距離差；NLOSn1(tk)是tk時(shí)刻的非視距誤差，是均值非負(fù)的隨機(jī)變量；w(tk)為系統(tǒng)誤差，可以視為高斯白噪聲。

為減小NLOSn1(tk)和w(tk)對(duì)基站測(cè)距帶來(lái)的誤差，對(duì)ri1實(shí)施卡爾曼濾波處理。將測(cè)量值rn1k(k=1，2，…，m)作為觀察值，構(gòu)建系統(tǒng)狀態(tài)空間模型如下：

(5)

(6)

卡爾曼濾波迭代器由公式(7)～(11)表示：

x(k|k-1)=Ax(k-1|k-1)

(7)

P(k|k-1)=AP(k-1|k-1)AT+Q

(8)

Kg(k)=P(k|k-1)GT(GP(k|k-1)GT+R)-1

(9)

x(k|k)=x(k|k-1)+Kg(k)[Z(k)-Gx(k|k-1)]

(10)

P(k|k)=(I-Kg(k)G)P(k|k-1)

(11)

式中：x(k|k-1)是k-1時(shí)刻對(duì)k時(shí)刻狀態(tài)的估計(jì)；x(k|k)為k時(shí)刻的最優(yōu)狀態(tài)估計(jì)；P(k|k)和P(k|k-1)分別為x(k|k)和x(k|k-1)對(duì)應(yīng)的協(xié)方差矩陣；Q為狀態(tài)協(xié)方差矩陣；R為觀測(cè)值協(xié)方差矩陣；Kg(k)為卡爾曼增益；I為單位矩陣。

3.2 Chan算法

(12)

式(12)中的ei為對(duì)應(yīng)的誤差估計(jì)，假設(shè)ei～N(0,δ2),由式(1)、(2)可得：

(13)

(14)

(15)

令：

(16)

(17)

公式(15)可簡(jiǎn)化為：

φ=H-GP

(18)

假設(shè)r1，x，y，z相互獨(dú)立，使用加權(quán)最小二乘法可得待測(cè)向量P的估計(jì)：

(19)

實(shí)際上，r1與待測(cè)坐標(biāo)(x,y,z)相關(guān)，需要對(duì)待測(cè)坐標(biāo)進(jìn)行進(jìn)一步估計(jì)。在干擾條件下：

(20)

誤差矢量可表示為：

φ=ΔH-ΔGP

(21)

cov(P′)=(GTφ-1G)-1

(22)

(23)

其中:

(24)

φ′的誤差協(xié)方差矩陣為：

(25)

(26)

最終定位坐標(biāo)為：

(27)

4 實(shí)驗(yàn)及分析

在長(zhǎng)280 m、寬5.2 m、高3.8 m的水平走向巷道內(nèi)按圖1、圖2所示搭建定位系統(tǒng)。以巷道入口處左下角為原點(diǎn)布置基站，建立坐標(biāo)系統(tǒng)，采用相位式激光測(cè)距儀(精度2 mm)測(cè)量定位目標(biāo)的實(shí)際距離。基站通過(guò)網(wǎng)口將采集到的標(biāo)簽到達(dá)時(shí)間上傳到模擬服務(wù)器進(jìn)行坐標(biāo)計(jì)算。受限于PCB加工精度與天線安裝工藝等，所設(shè)計(jì)的UWB定位模塊在實(shí)驗(yàn)巷道內(nèi)可靠定位距離在160 m以內(nèi)(丟包率3%以內(nèi)認(rèn)為可靠)。實(shí)驗(yàn)定位系統(tǒng)內(nèi)同一個(gè)標(biāo)簽同時(shí)至少被8個(gè)基站搜索到，選取其中6個(gè)信號(hào)最強(qiáng)的基站數(shù)據(jù)計(jì)算坐標(biāo)。當(dāng)標(biāo)簽移動(dòng)的時(shí)候，按照速度方向進(jìn)行基站切換。分別對(duì)固定標(biāo)簽(M1,M2)和沿x軸方向速度不大于1 m/s的手推車移動(dòng)標(biāo)簽(M3)進(jìn)行定位，結(jié)果如圖5～8所示，其中圖5和圖6分別為安裝在不同位置的2個(gè)固定標(biāo)簽M1、M2的10次定位結(jié)果，圖7和圖8為移動(dòng)標(biāo)簽M3在移動(dòng)過(guò)程中每500 ms定位一次的坐標(biāo)分布圖。

圖5 M1位置坐標(biāo)

圖6 M2位置坐標(biāo)

圖7 M3在x、y軸坐標(biāo)上的分布

圖8 M3 在x、z軸坐標(biāo)上的分布

對(duì)比有無(wú)卡爾曼濾波的定位效果，從圖5～8可以看出，定位算法中加入卡爾曼濾波得到的位置數(shù)據(jù)更加收斂，定位精度更高。記第i次測(cè)量坐標(biāo)誤差ei為：

(28)

式中：exi、eyi、ezi為第i次測(cè)量x、y、z軸方向的誤差。用一段時(shí)間內(nèi)的最大誤差(EMAX)和均方根誤差(ERSME)衡量定位效果，結(jié)果如表1所示，其中均方根誤差為：

(29)

表1 定位誤差

由表1可知，無(wú)卡爾曼濾波時(shí)，固定標(biāo)簽定位誤差最大值達(dá)到0.11 m，均方根誤差為0.09 m；而加入卡爾曼濾波后，最大誤差為0.08 m，均方根誤差為0.07 m。移動(dòng)標(biāo)簽在卡爾曼濾波下最大誤差和均方根誤差分別為0.09 m和0.08 m?？柭鼮V波與Chan算法結(jié)合與單純Chan算法相比，固定標(biāo)簽定位精度提高11%，慢速移動(dòng)標(biāo)簽精度提高27%。

5 結(jié)語(yǔ)

針對(duì)工況環(huán)境復(fù)雜的煤礦巷道中人員和設(shè)備定位系統(tǒng)精度不高的問(wèn)題，設(shè)計(jì)了一套三維精確定位系統(tǒng)，給出了系統(tǒng)整體設(shè)計(jì)方案，以及定位基站的空間布局和關(guān)鍵算法。實(shí)驗(yàn)結(jié)果顯示，卡爾曼濾波算法能夠有效抑制巷道內(nèi)非視距等干擾對(duì)定位精度的影響，卡爾曼濾波與Chan算法結(jié)合可以提高定位精度。