李宗偉，王翀，王剛，徐志明，崔朋志，姜孟馮

(應(yīng)急管理部信息研究院， 北京 100029)

0 引言

現(xiàn)有煤礦井下人員定位系統(tǒng)大多基于無線技術(shù)實現(xiàn)人員定位[1-3]?；灸Ｊ绞窃诘V井巷道鋪設(shè)傳輸總線和搭建定位基站，井下人員攜帶定位終端，當(dāng)經(jīng)過定位基站工作范圍內(nèi)時與定位基站建立通信并進行電磁波測距，以獲得相對準(zhǔn)確的定位終端自身位置，將定位終端自身位置與編號等信息發(fā)送至定位基站，定位基站通過傳輸總線將定位終端信息發(fā)送至地面監(jiān)控中心，從而完成實時定位監(jiān)控。飛行時間(Time of Flight，TOF)測距定位方法具有設(shè)備布設(shè)簡單、能耗低、定位精度不受信號強度及發(fā)射功率等因素影響的特點[4-6]，被廣泛應(yīng)用于煤礦井下人員定位系統(tǒng)中，然而該定位方法存在以下缺點：① 在基站無法覆蓋的區(qū)域容易造成定位盲區(qū)[7]，而實現(xiàn)全覆蓋需要鋪設(shè)大量基站，導(dǎo)致成本高。② 基于無線通信的測距技術(shù)在視距通信時效果較好，在非視距環(huán)境效果較差，而在煤礦井下非視距環(huán)境普遍存在[8]。③ 在地形復(fù)雜區(qū)域及經(jīng)過大型機電設(shè)備附近時，無線通信受噪聲干擾嚴(yán)重，導(dǎo)致定位精度受限。

捷聯(lián)慣性導(dǎo)航定位是一種既不需要與外部交互信息、也不向外部輻射能量的自主定位方法[9]。其工作原理是通過對慣性傳感器元件(陀螺儀和加速度計)的測量結(jié)果進行積分運算，獲得速度和位置等導(dǎo)航信息。捷聯(lián)慣性導(dǎo)航定位具有在短時間內(nèi)獲得較高的定位精度、不容易受外界干擾及設(shè)備攜帶方便等優(yōu)點，但獨立工作時定位誤差會隨著工作時間的增加逐漸增大[10-12]。

結(jié)合TOF測距定位方法和捷聯(lián)慣性導(dǎo)航定位方法的特點，本文提出了一種煤礦井下人員融合定位方法。當(dāng)定位終端位于定位基站通信覆蓋范圍內(nèi)時，采用TOF測距定位方法，當(dāng)定位終端位于定位基站通信覆蓋范圍之外時，采用捷聯(lián)慣性導(dǎo)航定位方法，并分別通過近距離和遠距離無線通信方式向定位基站傳輸定位數(shù)據(jù)，實現(xiàn)人員軌跡和位置坐標(biāo)的實時監(jiān)控。

1 相關(guān)原理

1.1 TOF測距定位

TOF測距定位原理是通過測量信號在收發(fā)節(jié)點間的往返時間來計算兩節(jié)點之間的距離，從而完成定位估計。為減小測距過程中因時鐘偏移而導(dǎo)致的測距誤差，采用TOF對稱雙向雙邊測距方法，其原理如圖1所示。定位終端向定位基站發(fā)送一個測距請求的數(shù)據(jù)包，定位基站接收數(shù)據(jù)包后對其進行處理，處理完成后向定位終端發(fā)送一個確認(rèn)響應(yīng)的數(shù)據(jù)包，此時定位終端記錄從發(fā)送數(shù)據(jù)包到接收數(shù)據(jù)包的時間T1，定位基站記錄從接收數(shù)據(jù)包到回復(fù)數(shù)據(jù)包的時間T2。再由定位基站向定位終端發(fā)送一個測距請求的數(shù)據(jù)包，此時請求端與響應(yīng)端互換，重復(fù)請求響應(yīng)過程，定位基站記錄從發(fā)送數(shù)據(jù)包到接收數(shù)據(jù)包的時間T3，定位終端記錄從接收數(shù)據(jù)包到回復(fù)數(shù)據(jù)包的時間T4。則定位終端和定位基站之間的距離為

(1)

式中C為信號傳播速度。

圖1 TOF對稱雙向雙邊測距原理

定位終端將包含自身信息的數(shù)據(jù)包發(fā)送給就近3個定位基站，基于TOF對稱雙向雙邊測距原理計算定位終端分別與3個定位基站之間的距離(式(2))，并利用三邊測距法確定定位終端位置，從而獲得礦井人員位置信息。

(2)

式中：(x,y)為定位終端坐標(biāo)；(x1,y1)，(x2,y2)，(x3,y3)分別為3個定位基站坐標(biāo)；d1，d2，d3分別為定位終端到3個定位基站的距離。

1.2 捷聯(lián)慣性導(dǎo)航定位

捷聯(lián)慣性導(dǎo)航定位過程如圖2所示(b表示載體坐標(biāo)系，i表示慣性坐標(biāo)系，t表示導(dǎo)航坐標(biāo)系)。

圖2 捷聯(lián)慣性導(dǎo)航定位過程

(3)

(4)

經(jīng)過有害加速度補償后，得到載體相對導(dǎo)航坐標(biāo)系的加速度a(t)，由各參數(shù)進行定位計算：對加速度進行一次積分運算可得t時刻載體移動速度v(t)，再對移動速度進行一次積分運算可得t時刻載體位置s(t)，同時通過姿態(tài)矩陣獲取載體姿態(tài)角。

(5)

(6)

式中：v(t0)為t0時刻載體初始速度；s(t0)為t0時刻載體初始位置。

捷聯(lián)慣性導(dǎo)航定位方法在短時間內(nèi)有較高定位精度，但定位誤差隨時間不斷累計[15-17]，因此本文采用卡爾曼濾波算法得到最優(yōu)誤差估計進行反饋校正，實現(xiàn)精確定位。

選取狀態(tài)誤差參數(shù)：

X=[Δve,Δvn,Δvu,Δse,Δsn,Δsu,Δφe,Δφn,Δφu,

Δεe,Δεn,Δεu,ΔNe,ΔNn,ΔNu]T

(7)

式中：Δve，Δvn，Δvu分別為東、北、天方向的速度誤差；Δse，Δsn，Δsu分別為東、北、天方向的位置誤差；Δφe，Δφn，Δφu分別為東、北、天方向的姿態(tài)角誤差；Δεe,Δεn,Δεu分別為東、北、天方向的陀螺儀零偏誤差；ΔNe，ΔNn，ΔNu分別為東、北、天方向的加速度計零偏誤差。

狀態(tài)方程和觀測方程[18-20]為

(8)

式中：Φ為狀態(tài)轉(zhuǎn)移矩陣；W為系統(tǒng)噪聲矩陣；Z為觀測矩陣；H為量測矩陣；V為觀測噪聲矩陣。

卡爾曼濾波的最優(yōu)誤差估計包括以下步驟。

① 狀態(tài)一步預(yù)測：

(9)

② 均方誤差一步預(yù)測：

P(t|t-1)=Φ(t|t-1)P(t-1)ΦT(t|t-1)+

W(t-1)Q(t-1)WT(t-1)

(10)

③ 濾波增益：

K(t)=P(t|t-1)HT(t)(H(t)P(t|t-1)HT(t)+

P(t))-1

(11)

④ 狀態(tài)估計：

(12)

⑤ 均方誤差估計：

P(t)=(I-K(t)H(t))P(t|t-1)

(13)

(14)

(15)

2 融合定位

TOF測距定位方法具有能耗低、設(shè)備部署方便、定位精度不受信號強度和發(fā)射功率影響等優(yōu)點，但受非視距影響較大，所以在定位精度要求不高且空間較寬闊的巷道(非視距誤差小)應(yīng)用效果好。捷聯(lián)慣性導(dǎo)航定位方法具有抗電磁干擾能力強、不依賴外界信息及定位精度高等優(yōu)點，適用于定位基站無線定位范圍無法覆蓋的巷道。

基于TOF測距定位方法和捷聯(lián)慣性導(dǎo)航定位方法的特點，提出了一種煤礦井下人員融合定位方法，如圖3所示。在主要巷道布設(shè)支持近距離和遠距離2種無線通信方式的多通信融合定位基站。在近距離無線通信覆蓋范圍的邊緣位置布設(shè)藍牙信標(biāo)，廣播其位置信息；在近距離無線通信覆蓋范圍之外，每隔1 km布設(shè)1個藍牙信標(biāo)，為慣性導(dǎo)航單元提供校準(zhǔn)信息(包括初始位置信息和方向信息)。定位終端由通過藍牙無線連接的設(shè)備1和設(shè)備2組成，設(shè)備1由礦井人員隨身攜帶，設(shè)備2固定在礦井人員的足部。當(dāng)定位終端處于定位基站近距離無線通信覆蓋范圍內(nèi)，采用TOF測距定位方式；當(dāng)定位終端處于定位基站近距離無線通信覆蓋范圍之外，定位終端自動切換工作模式，由TOF測距定位轉(zhuǎn)變?yōu)榻萋?lián)慣性導(dǎo)航定位。

圖3 煤礦井下人員融合定位

多通信融合定位基站電路組成如圖4所示。近距離無線通信單元與定位終端進行TOF測距定位并接收定位數(shù)據(jù)；遠距離無線通信單元在定位終端切換到捷聯(lián)慣性導(dǎo)航定位模式時接收定位數(shù)據(jù)；定位基站通過總線傳輸單元向地面監(jiān)控中心傳輸定位數(shù)據(jù)。

圖4 多通信融合定位基站電路組成

定位終端電路組成如圖5所示。設(shè)備1的近距離無線通信單元用于與多通信融合定位基站進行TOF測距定位和傳輸定位數(shù)據(jù)，遠距離無線通信單元用于傳輸捷聯(lián)慣性導(dǎo)航定位數(shù)據(jù)至定位基站；設(shè)備2的捷聯(lián)慣性導(dǎo)航單元由MEMS慣性器件組成，藍牙通信單元用于與設(shè)備1通信和接收巷道內(nèi)藍牙信標(biāo)的位置校準(zhǔn)信息。

煤礦井下人員融合定位流程如圖6所示。定位終端首先發(fā)射近距離通信廣播，定位基站接收到廣播包，返回近距離通信應(yīng)答。若定位終端接收到應(yīng)答，說明定位終端在定位基站近距離無線通信覆蓋范圍內(nèi)，則采用TOF測距定位方法，得到定位終端與定位基站之間的距離，進而獲得定位終端相對定位基站的相對坐標(biāo)，通過近距離無線通信方式將定位數(shù)據(jù)(相對坐標(biāo)和時間戳)發(fā)送給附近的定位基站；若定位終端不能接收到應(yīng)答，說明定位終端處于定位基站近距離通信覆蓋范圍之外，則采用捷聯(lián)慣性導(dǎo)航定位方法，通過藍牙通信啟動捷聯(lián)慣性導(dǎo)航單元，同時接收周圍藍牙信標(biāo)提供的校準(zhǔn)信息，得到定位終端在切換成捷聯(lián)慣性導(dǎo)航定位模式后的位置和方向，進而獲得定位終端相對定位基站的相對坐標(biāo)，通過遠距離無線通信方式將定位數(shù)據(jù)(相對坐標(biāo)和時間戳)發(fā)送給附近的定位基站。如果定位終端處于定位基站遠距離通信覆蓋范圍之外，則對慣性導(dǎo)航定位數(shù)據(jù)進行本地存儲，在恢復(fù)通信后傳輸至定位基站。定位基站將定位數(shù)據(jù)傳輸至地面監(jiān)控中心，地面監(jiān)控中心的定位服務(wù)器根據(jù)定位終端的相對坐標(biāo)、時間戳及定位基站的絕對坐標(biāo)，獲得定位終端在井下的絕對坐標(biāo)，從而得到人員軌跡和位置。

(a) 設(shè)備1

(b) 設(shè)備2

圖6 煤礦井下人員融合定位流程

3 實驗驗證

為驗證本文方法的有效性，開發(fā)實驗系統(tǒng)在井下進行實驗。采用ZigBee技術(shù)進行近距離無線傳輸，使用CC2530芯片和Z-Stack協(xié)議棧設(shè)計定位終端組成設(shè)備1的近距離無線通信單元；采用LoRa技術(shù)進行遠距離無線通信，使用遠距離、低功耗的LoRa無線收發(fā)模塊SX1278設(shè)計定位終端組成設(shè)備1的遠距離無線通信單元；采用加速度傳感器ADXL345、陀螺儀ITG-3205和數(shù)字羅盤HMC5883L設(shè)計定位終端組成設(shè)備2的捷聯(lián)慣性導(dǎo)航單元；采用NRF51822設(shè)計藍牙通信單元。

實驗地點在北京京煤集團有限責(zé)任公司大臺煤礦水平運輸巷道，該巷道長1 200 m、寬5 m、高3.5 m，如圖7所示。在A點布設(shè)定位基站，定位基站通過網(wǎng)口與計算機連接，通過計算機測試，設(shè)置AB段在定位基站ZigBee無線通信覆蓋范圍內(nèi)，BC段在定位基站ZigBee無線通信覆蓋范圍之外、LoRa通信覆蓋范圍內(nèi)，CD段在LoRa通信覆蓋范圍之外。在B點布設(shè)藍牙信標(biāo)，參與實驗人員攜帶定位終端由A點出發(fā)行走至D點，再原路返回。

圖7 定位實驗路線

經(jīng)過多次重復(fù)實驗發(fā)現(xiàn)，人員由A點行走至B點期間，定位終端處于TOF測距定位模式，定位精度為3～5 m；在B點處，定位終端切換至捷聯(lián)慣性導(dǎo)航定位模式，能接收藍牙信標(biāo)的信號并通過LoRa通信將定位數(shù)據(jù)發(fā)送至定位基站，定位精度在1 m以內(nèi)；當(dāng)人員由C點行走至D點期間，定位終端自動將慣性導(dǎo)航定位數(shù)據(jù)進行本地存儲，當(dāng)人員返回由C點向B點行走時，定位終端能將存儲的定位數(shù)據(jù)通過LoRa通信發(fā)送至定位基站。

計算機將慣性導(dǎo)航定位數(shù)據(jù)通過卡爾曼濾波處理后，與未經(jīng)過卡爾曼濾波處理的定位數(shù)據(jù)進行對比，如圖8所示(以B點位置為坐標(biāo)原點,(x,y)為人員位置)。從圖8可看出，行走距離達500 m后，無卡爾曼濾波的捷聯(lián)慣性導(dǎo)航定位軌跡方向和位置偏移較大，這是由于陀螺儀和加速度計的累計誤差逐漸增大；加入卡爾曼濾波后，方向和位置偏差得到有效糾正。

圖8 定位實驗結(jié)果

4 結(jié)語

煤礦井下人員融合定位方法結(jié)合TOF測距定位和捷聯(lián)慣性導(dǎo)航定位的優(yōu)點，分區(qū)域定位。在定位基站近距離無線通信覆蓋區(qū)域，采用TOF測距定位，僅需定位基站和定位終端，硬件設(shè)備少、結(jié)構(gòu)簡單，能有效快速定位，節(jié)約了通信成本和時間；在定位基站近距離無線通信無法覆蓋區(qū)域，及時切換工作模式，轉(zhuǎn)換為捷聯(lián)慣性導(dǎo)航定位方式，并通過卡爾曼濾波算法進一步提高定位精度，具有很強的抗干擾能力；在定位基站遠距離無線通信無法覆蓋區(qū)域，將慣性導(dǎo)航定位數(shù)據(jù)進行本地存儲，減少了定位盲區(qū)。