一種改進的煤礦井下人員定位算法

李勝利，陳子春，劉賀揚，王寶來，王玉福

（開灤（集團） 有限責任公司，河北 唐山 063018）

0 引 言

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

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

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

1 UWB 無線定位技術特點及原理

1.1 UWB 技術特點

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

1.2 UWB 技術井下精確定位原理

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

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

2 無線定位算法

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

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

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

SDS-TWR 雙邊雙程測距原理如圖2 所示，該方法的特點是不需兩節(jié)點時鐘同步，能夠有效降低時鐘誤差對其定位精度的影響，可通過硬件芯片設計來進行時間延遲，配合UWB 高速信號的時間同步，用設定時間間隔對時鐘的誤差進行補償。

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

圖2 SDS- TWR測距原理Fig.2 SDS-TWR ranging principle

SDS-TWR 的測距公式為：

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

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

3 人員定位算法

3.1 卡爾曼濾波算法

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

離散Kalman 濾波方程如下式:

式中：xk為狀態(tài)的真實值；ωk-1為服從高斯分布的噪聲；t 為MN 從第k 個采樣點運動到第k+1 個采樣點所用的時間；A 為觀測向量的轉移陣。

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

式中：zk為采樣點k 對應的距離測量值；vk和xk分別為觀測噪聲和進程噪聲；H=[1,0]。

通過Kalman Filter 估計出當前傳感器的測量值和前一點采樣點的值后，判斷是否超過閾值Tr，從而確定式（12） 中的是否將被替換為式（9） 卡爾曼先驗估計。因此，公式（12） 可以修改為：

3.2 改進卡爾曼技術的跟蹤與定位

圖3 為TOA 定位方式，根據(jù)圖示建立空間直角坐標系，其中P101、P102和P103是位于同一平面的3 個基站。

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

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

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

非線性測量模型可以表示為：

式中：Xk為P100的速度矢量坐標和相應坐標軸運動，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是預測噪聲向量Wk的協(xié)方差矩陣；Zk為P100到P101之間的觀測距離向量，Zk=(r1(k),r2(k),r3(k))T；Rk為觀測噪聲向量Vk的協(xié)方差矩陣；T 為采樣時間間隔。因此，Kalman 濾波方程為：

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

4 仿真驗證

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

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

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

在此次仿真實驗中，將真實軌跡、TOA 定位軌跡和EKF 濾波定位軌跡進行對比，如圖5 所示。X 方向和Y 方向距離誤差如圖6、圖7 所示。仿真結果表明，TOA 定位模式下的軌跡與真實軌跡誤差較大，加入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)實施與分析

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

圖8 定位基站結構框圖Fig.8 Structure diagram of positioning base station

6 結 語

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