基于Decawave DWM1001的井下精確定位系統(tǒng)設(shè)計

孫澤明

(山西省電子工業(yè)科學(xué)研究所有限公司，山西 太原 030006)

0 引言

我國煤炭資源非常豐富，并且煤炭在我國能源結(jié)構(gòu)中占有絕對主導(dǎo)地位。據(jù)統(tǒng)計，我國每年的煤炭發(fā)電量仍占所有發(fā)電量的65%以上?！吨袊沙掷m(xù)能源發(fā)展戰(zhàn)略》報告指出，到21世紀(jì)中期，煤炭仍是我國主要使用的能源。由此看來，在相當(dāng)長時間段內(nèi)，煤炭資源的開發(fā)使用仍占有非常重要的地位。由于礦井中環(huán)境復(fù)雜，監(jiān)管難度大，工作人員對煤礦生產(chǎn)中存在的安全隱患如瓦斯、透水等情況難以及時發(fā)現(xiàn)，從而經(jīng)常導(dǎo)致事故發(fā)生；事故發(fā)生以后，又由于井下定位技術(shù)相對落后，被困在礦井下的人員很難進(jìn)行精確定位，給事故救援工作造成很大的困難，因此實現(xiàn)井下人員實時精確定位顯得非常重要，也越來越受到國家監(jiān)管部門的高度重視?，F(xiàn)有的煤礦人員定位系統(tǒng)只實現(xiàn)區(qū)域定位，對事故后的救援、安全生產(chǎn)提供的支持有限。UWB是一種超寬帶技術(shù)的無線通信技術(shù)，其定位精度可達(dá)厘米級，可綜合應(yīng)用于煤礦井下人員、車輛的定位管理。

1 系統(tǒng)介紹

DWM1001是Decawave公司提供的專注于實時定位系統(tǒng)(RTLS)的一體化解決方案。該方案集成了DW1000 UWB收發(fā)器、nRF52832片上系統(tǒng)、一個3-axis加速度計和UWB天線和藍(lán)牙天線。nRF52832片上系統(tǒng)除了包含藍(lán)牙模塊，還內(nèi)嵌32位ARMCortex-M4F CPU，該CPU具有512kBFlash及64kRAM。DW1000UWB 收發(fā)器符合IEEE802.15.4-2011標(biāo)準(zhǔn)，3-axis加速度計可以獲取運動狀態(tài)下各設(shè)備的速度、位移數(shù)據(jù)，所以該方案就是一個完整的UWB實時定位模塊，既可以作為定位標(biāo)簽(定位卡)，也可以作為定位分站，也可以作為網(wǎng)關(guān)實現(xiàn)與外網(wǎng)(internet、wifi)的連接，將數(shù)據(jù)傳輸?shù)饺魏尉W(wǎng)絡(luò)。

2 測距算法

2.1 UWB基本測距原理

生活中，為了獲取一個物體在二維或三維空間的絕對位置，需要確定它與一些已知的物體之間的距離。UWB測距就是利用已知錨點(分站)的坐標(biāo)，通過計算各個錨點與未知站點(人員或設(shè)備，定位卡)的距離而計算出未知站點的坐標(biāo)。計算單個分站與定位卡的距離主要有幾類算法:信號強度算法、時間算法、相位算法等，而基于時間的算法由于對設(shè)備要求相對簡單，精度相對較高，成本更低，所以應(yīng)用更廣泛。

基于時間的算法主要有飛行時間(TOF)算法、到達(dá)時間差算法(TDOA)等，由于時間算法對分站之間、分站與定位卡之間的時間同步要求非常高，每1μs的差距在測距上就會產(chǎn)生30 cm的誤差，所以大都采用雙向測距(TWR:TWO WAY RANGING)，通過高速處理器，消耗相對多的信號傳輸次數(shù)和時間來回避設(shè)備之間由于時間不同步而產(chǎn)生的誤差。

2.2 基于時間的測距原理

TOF(Time Of Flight飛行時間測距法)主要利用信號在兩個設(shè)備(分站與定位卡)之間飛行時間來測量設(shè)備間的距離。雙向飛行時間法(TW-TOF,two way-time of flight)每個模塊從啟動開始即會生成一條攜帶ID和時間戳的信息，設(shè)備A在S1時刻發(fā)射請求定位性質(zhì)的脈沖信號，設(shè)備B在S2收到設(shè)備A的請求信號，并在S3時刻發(fā)射一個應(yīng)答信號，被設(shè)備A在自己的S4時刻接收。由此可以計算出脈沖信號在兩個設(shè)備之間的飛行時間，從而確定飛行距離L。

圖1 TOF算法原理

距離：L=C*[(S2-S1)+(S4-S3)]/2(C為光速)[2]。

單獨的TOF算法有一個非常嚴(yán)格的約束，即發(fā)送設(shè)備和接收設(shè)備必須時間上完全同步，每1μs的誤差就會造成30cm的誤差。這是一個目前很難解決的問題。為了去除時間同步問題，產(chǎn)生了一種新型算法就是Double-sided Two-way Ranging的算法巧妙地避開了這個問題，它既利用了TOF測距的優(yōu)點，又去除了TOF的同步問題，從而為TOF的實用化提供了支持。

2.3 TWR雙向測距原理(如圖2)

圖2 TWR測距原理

如圖2所示，設(shè)備A即定位卡，設(shè)備B即定位分站，分站具有處理信息、計算能力，定位卡只發(fā)送數(shù)據(jù)而不計算。

●<分站>向<定位卡>發(fā)送第一次測距請求，發(fā)送信息包括設(shè)備ID和時間戳1

●<分站>收到<定位卡>請求定位信息后再向定位卡發(fā)送確認(rèn)信息，發(fā)送信息包括設(shè)備ID和時間戳2

●第一次交互之間的時間差Tprop就是兩者之間的傳輸時間

●<分站>收到<定位卡>第一次信息后到向<定位卡>發(fā)送確認(rèn)信息是<分站>處理信息的時間，即Treply1，這在軟件上是可以獲取的

●同理，<定位卡>在收到<分站>的確認(rèn)信息后再次向分站發(fā)送ID和時間戳

●從圖2可以得出Tprop=(Tround1+Tround2-Treply1-Treply2)/2，即分站與定位卡之間信號的傳輸時間，由此可以算出兩者之間的距離

●Treply1、Treply2、Tround1、Tround2只與自身時鐘有關(guān)，而與其他設(shè)備無關(guān)，所以避開了設(shè)備之間時鐘的誤差。

2.4 系統(tǒng)設(shè)計

本系統(tǒng)主要對分站與定位卡之間的距離進(jìn)行計算，不對多個分站之間協(xié)同計算定位卡的2維或者3維坐標(biāo)進(jìn)行計算，所以軟件分為定位卡和分站兩種。

2.4.1 硬件初始化(如圖3)

圖3 硬件初始化流程

2.4.2 定位卡軟件流程

定位卡只負(fù)責(zé)搜索定位分站，搜索到后發(fā)送定位請求，收到定位分站的應(yīng)答后再發(fā)送一次ID和時間戳即可，軟件流程如圖4。

圖4 定位卡軟件流程

2.4.3 定位分站軟件流程

定位分站負(fù)責(zé)對收到的定位請求進(jìn)行應(yīng)答，在收到第二次請求后將兩次的數(shù)據(jù)進(jìn)行計算，發(fā)給上位機或者進(jìn)行2維、3維位置計算的主定位分站，軟件流程如圖5。

圖5 定位分站軟件流程

3 實際應(yīng)用

以上程序在室內(nèi)進(jìn)行試驗時精度能達(dá)到30 cm以下，但是在井下進(jìn)行實驗時測距數(shù)據(jù)就很不穩(wěn)定，數(shù)據(jù)精度比較差，分析原因是由于煤礦井下環(huán)境相對復(fù)雜，巷道內(nèi)設(shè)備、金屬構(gòu)件比較多，UWB波的吸收較多，多徑效應(yīng)比較大，所以為了提高測量精度必須解決多徑效應(yīng)問題，而卡爾曼濾波算法是解決多徑效應(yīng)的不錯的方法。

3.1 卡爾曼濾波介紹

卡爾曼濾波(Kalman filtering)是一種最優(yōu)化自回歸數(shù)據(jù)處理算法，不要求信號和噪聲都是平穩(wěn)過程,對于每個時刻的系統(tǒng)擾動和觀測誤差(即噪聲)，只要對它們的統(tǒng)計性質(zhì)作某些適當(dāng)?shù)募俣?，通過對含有噪聲的觀測信號進(jìn)行處理，就能在平均意義上求得誤差為最小的真實信號的估計值。觀測數(shù)據(jù)中包括系統(tǒng)擾動和外界噪聲，所以這種估值算法也可看作是濾波過程。由于它便于計算機編程實現(xiàn)，并能夠?qū)ΜF(xiàn)場采集的數(shù)據(jù)進(jìn)行實時的更新和處理，所以Kalman濾波是目前應(yīng)用最為廣泛的濾波方法，在通信和控制等很多領(lǐng)域得到了較好的應(yīng)用。

3.2 建立卡爾曼濾波模型

現(xiàn)以井下人員做勻速直線運動且速度不超過1m/s為前提，以最小方差估計為算法進(jìn)行建模。

Lm+1=Lm+Vmt

.

(1)

Lm=Lm-1+Vm-1t.

(2)

其中，Lm為m時刻井下人員移動，t為測量間隔，V為移動速度

在TWR系統(tǒng)模型中，假設(shè)井下人員作勻速直線移動，在時間小于1 s的前提下，井下人員當(dāng)前的位置是跟前一時刻的位置相同的。由于井下人員的位置是標(biāo)量時間函數(shù)，而且沒有外部因素控制，所以采用標(biāo)量差分方程表示系統(tǒng)狀態(tài)：

狀態(tài)方程：

L(m|m-1)=L(m-1|m-1)+λ(m).

(3)

觀測方程：

d(m)=L(m)+ε(m).

(4)

過程噪聲λk)可以認(rèn)為主要是由井下人員具有一定速度引起的，觀測噪聲ε(m)主要是定位過程中由于晶振的頻率漂移所引入的量化噪聲，λ(m)和ε(m)都是均值為零的正態(tài)白噪聲。在建立該狀態(tài)方程和觀測方程情況下，卡爾曼濾波基本能得到“最佳”估計。

Kalman濾波的狀態(tài)更新方程為:

L(m|m-1)=L(m-1|m-1).

(5)

P(m|m-1)=P(m-1|m-1)+Q

.

(6)

Kalman濾波的測量更新方程為:

γ(m)=d(m)-L(m|m-1)

.

(7)

Kg(m)=P(m|m-1)/[P(m|m-1)+R]

.

(8)

L(m|m)=L(m|m-1)+Kg(m)*γ(m)

.

(9)

P(m|m)=[1-Kg(m)]*P(m|m-1).

(10)

其中L(m|m-1)和L(m|m)為m時刻狀態(tài)變量的預(yù)測值和估計值;P(m|m-1)和P(m|m)為m時刻預(yù)測值和估計值誤差的協(xié)方差矩陣;Kg(m)為m時刻的卡爾曼增益;γ(m)為測量向量d(m)所對應(yīng)的信息過程。給定初始值L(0|0)和P(0|0)，根據(jù)k時刻的觀測值d(k)，由式(6))～式(10)就可以遞推計算得到k時刻TWR的狀態(tài)估計值L(k|k)。

從式(9)可以看出，m時刻的狀態(tài)估計值是由m時刻的測量值和上一時刻的估計值計算得到的。

當(dāng)TWR測量值存在較大誤差時，后續(xù)的TWR最優(yōu)估計值也受到較大影響，當(dāng)TWR測量值存在較大偏差時，不僅使當(dāng)前的TWR濾波值發(fā)生較大偏差，而且由于卡爾曼濾波器的記憶性，后續(xù)的TWR濾波值也會受到影響，造成濾波估計值與真實值發(fā)生偏離。對于這些偏差較大的TWR測量值，會使TWR估計值嚴(yán)重失真，故需要對卡爾曼濾波中出現(xiàn)的異常測量值進(jìn)行修正。

因此本文引入閾值δ來判斷當(dāng)前TWR測量值誤差是否偏大。將計算所得的信息γ(m)與閾值δ進(jìn)行比較，如果信息值γ(m)>閾值δ，表明當(dāng)前TWR測量值嚴(yán)重失真，不應(yīng)該帶入卡爾曼濾波進(jìn)行迭代計算，并用狀態(tài)預(yù)測值來代替TWR估計值。如果信息值γ(m)不大于閾值δ，則表明當(dāng)前的測量值在誤差容忍范圍之內(nèi)，可帶入卡爾曼濾波迭代過程進(jìn)行計算。針對該點，將卡爾曼濾波算法迭代過程中的式(8)修改為當(dāng)γ(m)<δ時Kg(m)=P(m|m-1)/[P(m|m-1)+R]，當(dāng)γ(m)>=δ時Kg(m)=0

閾值的獲取必須通過在井下實際使用環(huán)境內(nèi)通過實驗獲得。在實際應(yīng)用中，通過大量的現(xiàn)場實驗數(shù)據(jù)來設(shè)置不同的閾值，考察TWR估計值的誤差來確定井下特殊環(huán)境中的閾值[1]。

3.3 應(yīng)用濾波算法

建立好卡爾曼濾波算法模型后，只需在定位分站計算與定位卡之間的距離之后將算法嵌入，將前一時刻的測量結(jié)果和本時刻的測量結(jié)果按模型算法進(jìn)行計算估值即可獲取精度相對較高的測量距離。

4 結(jié)束語

采用Decawave DWM1001與卡爾曼濾波是一種對井下人員精確定位行之有效的方法，特別是對行進(jìn)中的人和物的跟蹤定位，能極大地改善多徑效應(yīng)帶來的誤差，精度能達(dá)到50 cm以內(nèi)。