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

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

0 引言

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

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

1 DS-TWR測距模型

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

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

由圖1可得

將式（1）和式（2）相乘后移項可得

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

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

2 位置解算算法

2.1 WLS算法

設(shè)4個基站的坐標(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)估計值（x,y）。

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

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

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

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

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

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

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

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

2.2 CHAN算法

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

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

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

將式（14）改寫為

當(dāng)矩陣Ga可逆時z的最大似然估計值za為

式中ψ=c2BQB，c為信號傳播速度。

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

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

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

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

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

3 實(shí)驗分析

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

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

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

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

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

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

圖4 CHAN與WLS算法誤差對比（靜態(tài)實(shí)驗）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時，CHAN算法的誤差在7 cm以內(nèi)的概率約為80%，WLS算法的誤差在7 cm以內(nèi)的概率約為25%。

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

3.2 動態(tài)實(shí)驗分析

利用移動小車搭載定位標(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é)果（動態(tài)實(shí)驗）Fig. 6 Positioning results of CHAN and WLS algorithms （dynamic experiment）

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

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

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

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

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

4 結(jié)語

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