基于UWB的井下人員定位算法研究

賀磊， 魏明生， 仇欣宇， 唐守鋒， 李文帥， 張旭

（1. 江蘇師范大學(xué) 物理與電子工程學(xué)院，江蘇 徐州 221116；2. 中國礦業(yè)大學(xué) 信息與控制工程學(xué)院，江蘇 徐州 221008）

0 引言

瓦斯爆炸、塌方等災(zāi)難嚴(yán)重威脅井下人員的生命安全，因此有必要建立有效的井下人員精確定位方案，當(dāng)發(fā)生礦難時(shí)，施救人員可對(duì)礦工進(jìn)行精準(zhǔn)定位并展開營救[1]。當(dāng)前井下定位技術(shù)種類較多，如RFID、ZigBee、WiFi、藍(lán)牙等[2]。這幾種定位技術(shù)基本可滿足井下定位要求，但仍存在信號(hào)傳輸距離短、定位精度較低、功耗較高、易受外界干擾等缺陷[3-5]。超寬帶（Ultra Wide Band，UWB）定位技術(shù)采用納秒級(jí)別的脈沖信號(hào)進(jìn)行通信，具有較高的時(shí)間分辨率，適用于非視距環(huán)境下的定位，且定位精度較高，與其他幾種定位技術(shù)相比具有明顯優(yōu)勢。

UWB定位基于測距原理，常用的測距算法包括單邊雙向測距（Single-Sided Two-Way Ranging，SS-TWR）和雙邊雙向測距（Double-Sided Two-Way Ranging，DS-TWR）。其中SS-TWR因受時(shí)鐘偏移的影響，誤差較大[6-7]。DS-TWR在測距雙方之間發(fā)送多條信息，獲得多個(gè)時(shí)間戳，通過加長測量周期來提高測量精度[8]。井下人員定位對(duì)精度要求較高，因此適宜采用DS-TWR測距算法[9]。根據(jù)測距信息，通過位置解算算法可得到定位標(biāo)簽的坐標(biāo)。本文通過靜態(tài)實(shí)驗(yàn)和動(dòng)態(tài)實(shí)驗(yàn)對(duì)加權(quán)最小二乘（Weighted Least Squares，WLS）算法[10]和CHAN[11]兩種位置解算算法進(jìn)行對(duì)比分析，并通過均方根誤差和誤差累計(jì)分布函數(shù)（Cumulative Distribution Function，CDF）綜合評(píng)估定位精度。

1 DS-TWR測距模型

DS-TWR測距模型如圖1所示，其中Tprop為UWB信號(hào)在定位基站與定位標(biāo)簽之間的傳播時(shí)間，可利用定位基站和標(biāo)簽的時(shí)鐘系統(tǒng)記錄的時(shí)間戳T1-T3，-來計(jì)算；Tround1為基站開始發(fā)送信號(hào)到基站接收到標(biāo)簽信號(hào)的時(shí)間；Treply1為標(biāo)簽接收到基站信號(hào)到標(biāo)簽開始回復(fù)基站的時(shí)間；Tround2為標(biāo)簽開始回復(fù)基站到標(biāo)簽再次接收到基站信號(hào)的時(shí)間；Treply2為基站接收到標(biāo)簽的回復(fù)信號(hào)到基站再次發(fā)送信號(hào)的時(shí)間；Tm1，Tm2分別為定位基站和標(biāo)簽的系統(tǒng)時(shí)鐘。

圖1 礦井DS-TWR測距模型Fig. 1 Mine DS-TWR ranging model

由圖1可得

將式（1）和式（2）相乘后移項(xiàng)可得

將式（1）和式（2）代入式（3）可得

由式（5）可計(jì)算出電磁波在定位標(biāo)簽與定位基站之間的傳播時(shí)間，結(jié)合信號(hào)傳播速度可計(jì)算出傳播距離。獲得定位基站與定位標(biāo)簽之間的距離后，再通過位置解算算法計(jì)算定位標(biāo)簽的坐標(biāo)[12]。

2 位置解算算法

2.1 WLS算法

設(shè)4個(gè)基站的坐標(biāo)為（xi，yi），i=1，2，3，4，待測定位標(biāo)簽的坐標(biāo)為（x，y），定位標(biāo)簽到基站的真實(shí)距離為ri，測量距離為，根據(jù)最小二乘算法可得

求解式（6）即可得到定位標(biāo)簽坐標(biāo)的最優(yōu)估計(jì)值（x,y）。

普通最小二乘算法適用于各個(gè)誤差項(xiàng)相等的情況，由于井下UWB定位環(huán)境復(fù)雜，為了提高定位標(biāo)簽位置解算精度，在普通最小二乘算法中引入加權(quán)矩陣W。設(shè)定位標(biāo)簽到第i個(gè) 基站的測距誤差為βi，則有

用測量距離與真實(shí)距離的平方差表示誤差矢量：

在實(shí)際應(yīng)用中，測量誤差遠(yuǎn)小于測量的距離，因此式（8）中的項(xiàng)可以忽略不計(jì)；由于不方便測量定位標(biāo)簽到基站的真實(shí)距離，通常利用代替真實(shí)距離ri進(jìn)行近似計(jì)算。誤差矢量可以進(jìn)一步表示為

誤差矢量φ的協(xié)方差矩陣為

式中E為期望函數(shù)。

最后對(duì)Ψ求逆矩陣，得到加權(quán)矩陣W：

結(jié)合式（6）得到WLS算法計(jì)算公式：

上述推導(dǎo)是針對(duì)4個(gè)定位基站的情況，當(dāng)基站數(shù)量為4個(gè)以上時(shí)可在此基礎(chǔ)上類推。

2.2 CHAN算法

在求解二維平面中的位置坐標(biāo)時(shí)，若定位基站的數(shù)量大于3，則會(huì)有冗余測距信息，可得到2個(gè)以上非線性方程。在CHAN算法求解過程中，首先使用WLS算法估計(jì)定位標(biāo)簽的初始位置坐標(biāo)，然后結(jié)合已知約束條件再次應(yīng)用WLS算法，得到最終位置坐標(biāo)估計(jì)值。

仍以4個(gè)基站為例對(duì)CHAN算法進(jìn)行推導(dǎo)，設(shè)z=[x,y,r1]T，根據(jù)文獻(xiàn)[13]可得

式中：ri1為定位標(biāo)簽到第i個(gè)基站與到第1個(gè)基站之間的距離差；；xi1，yi1分別為第i個(gè)基站與第1個(gè)基站的橫坐標(biāo)之差和縱坐標(biāo)之差。

將式（14）改寫為

當(dāng)矩陣Ga可逆時(shí)z的最大似然估計(jì)值z(mì)a為

式中ψ=c2BQB，c為信號(hào)傳播速度。

在實(shí)際應(yīng)用中通常用Q代替ψ，則式（16）可改寫為

根據(jù)定位基站與定位標(biāo)簽之間的距離約束關(guān)系得到誤差矢量：

φ1的協(xié)方差矩陣為

采用WLS算法對(duì)式（18）進(jìn)行求解，得

最后根據(jù)先驗(yàn)條件可以求得定位標(biāo)簽坐標(biāo)估計(jì)值。

3 實(shí)驗(yàn)分析

為了對(duì)比分析WLS算法和CHAN算法的效果，在實(shí)驗(yàn)室模擬井下環(huán)境，采用4個(gè)定位基站進(jìn)行實(shí)驗(yàn)，如圖2所示。設(shè)4個(gè)定位基站BS1-BS4的坐標(biāo)分別為（0，0），（560 cm，0），（560 cm，800 cm），（0，720 cm），實(shí)驗(yàn)設(shè)備數(shù)據(jù)更新頻率為1 Hz。

圖2 實(shí)驗(yàn)環(huán)境與自制定位設(shè)備Fig. 2 Experimental environment and self-made positioning equipment

3.1 靜態(tài)實(shí)驗(yàn)分析

實(shí)驗(yàn)開始時(shí)，將定位標(biāo)簽放置在坐標(biāo)（320 cm，400 cm）處，采集測距信息，并利用2種算法進(jìn)行定位，結(jié)果如圖3所示。

圖3 CHAN與WLS算法定位結(jié)果（靜態(tài)實(shí)驗(yàn)）Fig. 3 Positioning results of CHAN and WLS algorithms （static experiment）

計(jì)算定位標(biāo)簽估算位置與真實(shí)位置的歐氏距離，并繪制誤差曲線，如圖4所示?？煽闯?，定位標(biāo)簽為靜態(tài)時(shí)，CHAN算法的定位誤差小于WLS算法。經(jīng)計(jì)算，CHAN算法和WLS算法的均方根誤差分別為5.878 6，8.007 4 cm，CHAN算法的均方根誤差比WLS算法低26.59%。

圖4 CHAN與WLS算法誤差對(duì)比（靜態(tài)實(shí)驗(yàn)）Fig. 4 Comparison of errors between CHAN and WLS algorithms（static experiment）

CHAN算法和WLS算法的誤差CDF曲線如圖5所示?？煽闯?，CHAN算法的誤差CDF曲線在WLS算法的左上方，在設(shè)置同一誤差門限值的條件下，CHAN算法的誤差在門限值以內(nèi)的概率較高。例如，當(dāng)誤差門限值取7 cm時(shí)，CHAN算法的誤差在7 cm以內(nèi)的概率約為80%，WLS算法的誤差在7 cm以內(nèi)的概率約為25%。

圖5 CHAN與WLS算法的誤差CDF曲線（靜態(tài)實(shí)驗(yàn)）Fig. 5 Error CDF curves of CHAN and WLS algorithms（static experiment）

3.2 動(dòng)態(tài)實(shí)驗(yàn)分析

利用移動(dòng)小車搭載定位標(biāo)簽，模擬井下攜帶定位標(biāo)簽的作業(yè)人員。定位標(biāo)簽起始點(diǎn)坐標(biāo)為（160 cm，400 cm），結(jié)束點(diǎn)坐標(biāo)為（480 cm，400 cm），小車勻速行駛。利用CHAN和WLS算法得到的定位結(jié)果如圖6所示?？煽闯?，用CHAN算法得到的軌跡更加接近真實(shí)軌跡。

圖6 CHAN與WLS算法定位結(jié)果（動(dòng)態(tài)實(shí)驗(yàn)）Fig. 6 Positioning results of CHAN and WLS algorithms （dynamic experiment）

計(jì)算不同時(shí)刻定位標(biāo)簽估算位置與真實(shí)位置的歐氏距離，并繪制誤差曲線，如圖7所示?？煽闯?，定位標(biāo)簽為動(dòng)態(tài)時(shí)，CHAN算法的定位誤差小于WLS算法。經(jīng)計(jì)算，CHAN算法和WLS算法的均方根誤差分別為12.292 3，21.180 9 cm，CHAN算法的均方根誤差比WLS算法低41.97%。

圖7 CHAN與WLS算法誤差對(duì)比（動(dòng)態(tài)實(shí)驗(yàn)）Fig. 7 Comparison of errors between CHAN and WLS algorithms（dynamic experiment）

CHAN算法和WLS算法的誤差CDF曲線如圖8所示。

圖8 CHAN與WLS算法的誤差CDF曲線（動(dòng)態(tài)實(shí)驗(yàn)）Fig. 8 Error CDF curves of CHAN and WLS algorithms（dynamic experiment）

從圖8可看出，CHAN算法的誤差CDF曲線基本在WLS算法的上方，說明在動(dòng)態(tài)實(shí)驗(yàn)中CHAN算法的定位精度高于WLS算法。

4 結(jié)語

研究了基于UWB的井下人員定位算法。采用DS-TWR方式測量定位基站與定位標(biāo)簽的距離，利用WLS算法和CHAN算法對(duì)靜態(tài)測距信息和動(dòng)態(tài)測距信息進(jìn)行解算，最后通過均方根誤差和誤差CDF綜合評(píng)估2種算法的定位性能。實(shí)驗(yàn)結(jié)果表明：靜態(tài)實(shí)驗(yàn)時(shí)，CHAN算法和WLS算法的均方根誤差分別為5.878 6，8.007 4 cm，CHAN算法的均方根誤差比WLS算法低26.59%；動(dòng)態(tài)實(shí)驗(yàn)時(shí)，CHAN算法和WLS算法的均方根誤差分別為12.292 3，21.180 9 cm，CHAN算法的均方根誤差比WLS算法低41.97%；CHAN算法的定位精度高于WLS算法，更加適用于煤礦井下人員定位。