一種改進(jìn)的煤礦井下人員定位算法

李勝利，陳子春，劉賀揚(yáng)，王寶來(lái)，王玉福

（開(kāi)灤（集團(tuán)） 有限責(zé)任公司，河北 唐山 063018）

0 引 言

我國(guó)擁有豐富的煤炭資源，但目前面臨嚴(yán)重的地質(zhì)問(wèn)題，導(dǎo)致礦產(chǎn)資源開(kāi)采困難，不確定因素頻繁出現(xiàn)。如今，煤礦開(kāi)采已經(jīng)成為高危職業(yè)。為有效保護(hù)煤炭開(kāi)采作業(yè)人員安全，很多煤礦都引進(jìn)了地下無(wú)線定位技術(shù)。精準(zhǔn)的煤礦井下人員定位系統(tǒng)，為了解井下工作人員的實(shí)況信息，快速向地面提供救援信息，以及礦下緊急救援等工作的開(kāi)展，提供了保障。

定位技術(shù)在近年來(lái)高速發(fā)展，GPS 技術(shù)(Global Positioning System)是各類(lèi)定位技術(shù)中的代表，被普遍應(yīng)用于地面場(chǎng)景中的室外定位，但是對(duì)于井下，GPS 發(fā)出的信號(hào)不能被接收設(shè)備良好接收，因此并不適合應(yīng)用于較為復(fù)雜的井下環(huán)境。目前井下人員定位技術(shù)主要包括ZigBee、WiFi 等，但因?yàn)槊旱V的地下環(huán)境復(fù)雜，同樣存在定位精度較低和易受干擾問(wèn)題，而且一旦發(fā)生事故，干擾情況會(huì)更加嚴(yán)重。

為解決上述問(wèn)題，提出一種基于超寬帶(Ultra Wide Band，UWB)技術(shù)和到達(dá)時(shí)間測(cè)距(Time of Arrival，TOA)算法，同時(shí)融合了改進(jìn)的卡爾曼濾波(Kalman Filter，KF)算法，來(lái)解決信號(hào)在井下非視距傳播(NonLineOfSight，NLOS)造成的測(cè)距誤差問(wèn)題，以此來(lái)實(shí)現(xiàn)數(shù)據(jù)精準(zhǔn)處理，提高系統(tǒng)的測(cè)距精度和穩(wěn)定性。

1 UWB 無(wú)線定位技術(shù)特點(diǎn)及原理

1.1 UWB 技術(shù)特點(diǎn)

UWB 技術(shù)是一種短距離內(nèi)使用高頻率寬頻帶的無(wú)線載波通信技術(shù)，其超窄脈沖可達(dá)納秒級(jí)，通過(guò)在寬頻上發(fā)送一系列非常窄的低功率脈沖實(shí)現(xiàn)與基站的信息交互，具有不需解調(diào)、信號(hào)穿透能力強(qiáng)、抗干擾效果好、功耗低、定位精確度高、多徑分辨能力強(qiáng)、工程造價(jià)成本低等技術(shù)特點(diǎn)。

1.2 UWB 技術(shù)井下精確定位原理

UWB 發(fā)送窄低功率脈沖與基站進(jìn)行數(shù)據(jù)通訊，無(wú)線信號(hào)收發(fā)器接收到無(wú)線信號(hào)，利用無(wú)線定位算法與測(cè)距模型計(jì)算出兩個(gè)節(jié)點(diǎn)之間的距離，再利用預(yù)設(shè)的一個(gè)坐標(biāo)系設(shè)置基準(zhǔn)節(jié)點(diǎn)位置的坐標(biāo)值，最后通過(guò)基準(zhǔn)節(jié)點(diǎn)位置坐標(biāo)，利用位置估計(jì)算法計(jì)算出節(jié)點(diǎn)所在的位置坐標(biāo)，如圖1 所示。

圖1 UWB定位技術(shù)基本原理Fig.1 Basic principle of UWB positioning technology

2 無(wú)線定位算法

無(wú)線定位算法分為基于非測(cè)距的定位算法和基于測(cè)距的定位算法。由于基于非測(cè)距的定位算法在井下煤礦系統(tǒng)中的應(yīng)用受復(fù)雜環(huán)境影響較大，算法所產(chǎn)生的誤差較大，并且需要布置大量的基站用于信號(hào)傳輸，考慮到井下狹窄空間并不適合UWB 定位系統(tǒng)，以及工程成本等問(wèn)題，決定采用基于到達(dá)時(shí)間（TOA） 定位算法進(jìn)行雙邊雙程測(cè)距。

在TOA 方法中，距離由通過(guò)待測(cè)標(biāo)簽與基站之間的信號(hào)傳播時(shí)間決定。獲取基本數(shù)據(jù)后，利用相關(guān)算法實(shí)現(xiàn)目標(biāo)位置待測(cè)標(biāo)簽的檢測(cè)。在TOA方法中待測(cè)距離、信號(hào)到達(dá)時(shí)間、傳播速度之間的關(guān)系可以表示為：

式中：d 為待測(cè)節(jié)點(diǎn)和基站之間的距離；c 為無(wú)線信號(hào)在空氣中的傳播速度；tTOA為設(shè)備的無(wú)線信號(hào)發(fā)射和接收所需時(shí)間差。但由于無(wú)線信號(hào)的傳播速度約等于光速，會(huì)使實(shí)際距離與測(cè)算距離有很大的誤差值，現(xiàn)階段的工程通訊技術(shù)難以實(shí)現(xiàn)時(shí)間同步。為解決基站與標(biāo)簽的時(shí)間同步問(wèn)題，DecaWave 公司利用其發(fā)明的DW1000 芯片提出了SDS-TWR 雙邊雙程測(cè)距法。

SDS-TWR 雙邊雙程測(cè)距原理如圖2 所示，該方法的特點(diǎn)是不需兩節(jié)點(diǎn)時(shí)鐘同步，能夠有效降低時(shí)鐘誤差對(duì)其定位精度的影響，可通過(guò)硬件芯片設(shè)計(jì)來(lái)進(jìn)行時(shí)間延遲，配合UWB 高速信號(hào)的時(shí)間同步，用設(shè)定時(shí)間間隔對(duì)時(shí)鐘的誤差進(jìn)行補(bǔ)償。

如圖2 所示，無(wú)線信號(hào)經(jīng)過(guò)2 次雙程TOA 測(cè)距。TOF 為飛行時(shí)間，TA、TB表示接收信號(hào)處理后再發(fā)出信號(hào)所需的時(shí)間，TAB、TBA表示在同一節(jié)點(diǎn)上從發(fā)出信號(hào)到最終接收信號(hào)所需的時(shí)間，則2 次信號(hào)往返所需的整體時(shí)間為：

圖2 SDS- TWR測(cè)距原理Fig.2 SDS-TWR ranging principle

SDS-TWR 的測(cè)距公式為：

在井下環(huán)境下信號(hào)的傳輸距離多為中短程，所以TOF 飛行時(shí)間要遠(yuǎn)小于基站和待測(cè)標(biāo)簽的處理時(shí)間差，同時(shí)硬件電路中的時(shí)鐘會(huì)出現(xiàn)晶振偏頻現(xiàn)象，eA、eB為節(jié)點(diǎn)的時(shí)間偏移量，結(jié)合式（1） 測(cè)距公式可簡(jiǎn)化為：

無(wú)線信號(hào)在實(shí)際傳播過(guò)程中幾乎不可能避開(kāi)所有障礙物，必然存在干擾情況。這種情況下的信號(hào)傳播稱為非視距傳播NLOS，在使用TOA 測(cè)距模式時(shí)，NLOS 不可避免地會(huì)導(dǎo)致測(cè)距誤差，抑制NLOS 對(duì)于提高井下定位精度具有重要意義。目前主流定位技術(shù)中，卡爾曼濾波算法最為常見(jiàn)，該方法非常適合處理系統(tǒng)中隨機(jī)變量所構(gòu)成的動(dòng)態(tài)系統(tǒng)復(fù)雜問(wèn)題，利用遞推方法對(duì)系統(tǒng)中真實(shí)值進(jìn)行最優(yōu)估計(jì)，結(jié)合UWB 的技術(shù)特點(diǎn)，能夠有效抑制NLOS 對(duì)TOA 定位技術(shù)的影響。

3 人員定位算法

3.1 卡爾曼濾波算法

首先將原數(shù)據(jù)進(jìn)行Kalman 濾波運(yùn)算，并進(jìn)行定位實(shí)驗(yàn)。移動(dòng)結(jié)點(diǎn)MN 和AN 之間的距離，rk可以看作是在很短的預(yù)設(shè)時(shí)間內(nèi)的均勻變化：

離散Kalman 濾波方程如下式:

式中：xk為狀態(tài)的真實(shí)值；ωk-1為服從高斯分布的噪聲；t 為MN 從第k 個(gè)采樣點(diǎn)運(yùn)動(dòng)到第k+1 個(gè)采樣點(diǎn)所用的時(shí)間；A 為觀測(cè)向量的轉(zhuǎn)移陣。

離散卡爾曼濾波方程表示為：

式中：zk為采樣點(diǎn)k 對(duì)應(yīng)的距離測(cè)量值；vk和xk分別為觀測(cè)噪聲和進(jìn)程噪聲；H=[1,0]。

通過(guò)Kalman Filter 估計(jì)出當(dāng)前傳感器的測(cè)量值和前一點(diǎn)采樣點(diǎn)的值后，判斷是否超過(guò)閾值Tr，從而確定式（12） 中的是否將被替換為式（9） 卡爾曼先驗(yàn)估計(jì)。因此，公式（12） 可以修改為：

3.2 改進(jìn)卡爾曼技術(shù)的跟蹤與定位

圖3 為T(mén)OA 定位方式，根據(jù)圖示建立空間直角坐標(biāo)系，其中P101、P102和P103是位于同一平面的3 個(gè)基站。

圖3 TOA定位原理Fig.3 TOA positioning principle

設(shè)P100的 坐 標(biāo) 為(x, y, z)，則AN 的P101、P102、P103的坐標(biāo)為(xi, yi, zi)，i=1,2,3。則P100與P101之間的距離為：

由于式（16） 所示的P100坐標(biāo)(x,y,z)和觀測(cè)距離ri之間的關(guān)系是非線性的，即測(cè)量方程是非線性的，而KF 的狀態(tài)估計(jì)和預(yù)測(cè)是線性的。因此，為提高M(jìn)N 動(dòng)態(tài)定位精度，采用改進(jìn)卡爾曼技術(shù)來(lái)估計(jì)P100坐標(biāo)，即先將預(yù)測(cè)和狀態(tài)估計(jì)線性化后，再應(yīng)用于卡爾曼濾波。公式如下：

非線性測(cè)量模型可以表示為：

式中：Xk為P100的速度矢量坐標(biāo)和相應(yīng)坐標(biāo)軸運(yùn)動(dòng)，Xk=[x(k),y(k),z(k),vx(k),vy(k),vz(k)]T,vi(k)=(i(k)-i(k-1))/T,i = x,y,z；Wk是預(yù)測(cè)噪聲向量Wk的協(xié)方差矩陣；Zk為P100到P101之間的觀測(cè)距離向量，Zk=(r1(k),r2(k),r3(k))T；Rk為觀測(cè)噪聲向量Vk的協(xié)方差矩陣；T 為采樣時(shí)間間隔。因此，Kalman 濾波方程為：

式中：Pk為誤差協(xié)方差矩陣；Kk是卡爾曼增益矩陣；為轉(zhuǎn)移陣Xk-1=φk-1Xk-1；I3為3×3 單位陣，Hk可以表示為hk(xk)在k 時(shí)刻時(shí)Xk的雅可比矩陣。

4 仿真驗(yàn)證

為了驗(yàn)證算法的有效性，在Matlab 中對(duì)TOA定位方法和擴(kuò)展卡爾曼濾波(Extended Kalman Filtering, EKF)定位方法進(jìn)行了比較。圖4 為T(mén)OA定位模式的初始誤差與EKF 的TOA 定位模式的定位誤差對(duì)比。

圖4 初始定位誤差比較Fig.4 Comparison of initial positioning error

系統(tǒng)獲得1 000 個(gè)隨機(jī)采樣點(diǎn)，并在理想條件下進(jìn)行仿真，驗(yàn)證了算法的有效性。在TOA 定位模式下的隨機(jī)誤差最大0.1 m，而EKF 的TOA定位模式下的隨機(jī)誤差明顯改善，最大0.036 m。

在此次仿真實(shí)驗(yàn)中，將真實(shí)軌跡、TOA 定位軌跡和EKF 濾波定位軌跡進(jìn)行對(duì)比，如圖5 所示。X 方向和Y 方向距離誤差如圖6、圖7 所示。仿真結(jié)果表明，TOA 定位模式下的軌跡與真實(shí)軌跡誤差較大，加入EKF 濾波后的TOA 定位軌 跡誤差較小。

圖5 定位軌跡的比較Fig.5 Comparison of positioning trajectories

圖6 X方向距離誤差Fig.6 Distance error in X direction

圖7 Y方向距離誤差Fig.7 Distance error in Y direction

5 系統(tǒng)實(shí)施與分析

定位基站部署在煤礦井下環(huán)境中，采集待測(cè)標(biāo)簽數(shù)據(jù)。定位基站硬件設(shè)計(jì)主要包括：①UWB 定位模塊dw1000，是射頻信號(hào)收發(fā)器核心部分，負(fù)責(zé)待測(cè)基站、標(biāo)簽定位；②主控模塊STM32F407Z ET6，負(fù)責(zé)數(shù)據(jù)采集、處理和轉(zhuǎn)發(fā)，是整個(gè)定位系統(tǒng)的控制核心；③電源管理模塊，負(fù)責(zé)系統(tǒng)供電，內(nèi)置鋰電池應(yīng)急模式供電；④外部設(shè)備，包括OLED 顯示屏、指示燈、撥盤(pán)開(kāi)關(guān)、按鍵等部件。定位基站結(jié)構(gòu)如圖8 所示。

圖8 定位基站結(jié)構(gòu)框圖Fig.8 Structure diagram of positioning base station

6 結(jié) 語(yǔ)

針對(duì)煤礦井下定位過(guò)程中的非視距問(wèn)題，提出了一種改進(jìn)的卡爾曼濾波超寬帶室內(nèi)定位算法。對(duì)實(shí)際測(cè)量誤差進(jìn)行分析，通過(guò)一階函數(shù)構(gòu)建誤差修正模型，減小系統(tǒng)誤差對(duì)定位精度的影響，并采用KF 算法減小多徑效應(yīng)和NLOS 誤差。最后，通過(guò)EKF 提高了系統(tǒng)的定位精度。本文提出的EKF 算法有效地提高了超寬帶定位系統(tǒng)在視距和非視距條件下的定位精度，滿足煤礦井下人員和設(shè)備的定位應(yīng)用要求。