基于井下巷道網絡與定位設備數據聯合拓撲的井下精確定位系統實時定位糾偏算法

周 翔，韓燕南，周嘉欣

（1.中煤科工集團常州研究院有限公司，江蘇 常州 213000；2.天地（常州）自動化股份有限公司，江蘇 常州 213000）

人員精確定位對于實時監控井下人員數量、人員動態分布、生產指揮調度等方面具有重要意義[1-2]。在煤礦井下，由于定位接收器本身的系統誤差，以及復雜的井下巷道對于定位信號的反射、遮擋等因素的影響，導致定位測量數據與真實值存在偏差，這就造成在定位系統中出現人員偏離巷道的情況，無法準確顯示人員當前所處巷道位置，因此需要對出現定位偏移問題的人員進行糾偏處理。地圖匹配是目前常用的定位糾偏方法[3-4]，常見的地圖匹配算法包括最短距離[5]、投影[6-8]、基于網絡拓撲結構[9-12]、基于隱馬爾可夫模型[13]以及基于D-S 證據推理[14-16]等地圖匹配算法。最短距離地圖匹配是一種基礎的匹配方法，邏輯簡單、速度快、實時性好，但匹配準確率較低且穩定性差[3,5]；投影地圖匹配計算速度快，實時性好，在一定程度上可以提高目標的定位精度，但在道路網絡復雜的情況下，匹配精度和穩定性均會降低[7-8]；基于網絡拓撲關系的地圖匹配算法準確率和可靠性高，實用性強，適用于復雜的道路網絡環境，但缺點在于計算耗時較大[9-10]；基于隱馬爾可夫模型的地圖匹配算法將車輛GPS 軌跡點位置作為HMM中的觀察變量，將車輛實際所在位置作為HMM 中的隱藏狀態變量，通過實際數據訓練模型再進行真實位置的預測，適用于相對簡單的道路網絡場景中[13]；基于D-S 證據推理的地圖匹配算法不使用歷史軌跡點信息，單個點匹配錯誤不會對下一個定位點的匹配產生太大影響，但隨著道路越來越密集會降低匹配結果的準確率[14-16]。

提出了一種基于井下巷道網絡與定位設備數據聯合拓撲的實時定位糾偏算法，首先構建出巷道網絡與定位設備之間的拓撲關系，并基于拓撲關系，根據人員定位信息快速準確定位其所在巷道，再利用糾偏計算模型對實現人員實時定位坐標進行糾偏解算，實現具有實時性的人員定位糾偏效果。

1 井下定位糾偏算法

1.1 算法基本思路

算法思路示意圖如圖1。

圖1 算法思路示意圖Fig.1 The design idea of algorithm

假設人員1 根據定位設備的測量結果被定位在4 條巷道中間，距離巷道BE 最近，如果根據距離最近原則來定位，容易將人員1 定位在巷道BE 上，但如果人員1 實際由定位器1 定位，而定位器1 與巷道AB 存在唯一確定關系，所以人員1 的真實位置應該在巷道AB 或者相鄰巷道BC 上。如果人員1 由定位器2 定位，根據定位器2 和巷道的對應關系可知，人員1 的真實位置應該在巷道DE 上。因此實現井下人員實時位置糾偏的關鍵點在于如何確定巷道網絡與定位設備之間的唯一映射關聯，以及高效的糾偏計算模型。

1.2 算法設計描述

根據算法基本思路的分析，設計的實時定位糾偏算法主要包括井下數據拓撲關系構建、巷道定位和糾偏計算3 個部分。

1）井下數據拓撲關系構建。井下數據包括巷道中線點數據、巷道中線數據和定位設備數據。設計的井下數據拓撲關系有2 類：①巷道網絡拓撲結構：利用中線點數據和中線數據計算出巷道網絡；②巷道-設備拓撲關系：利用中線數據和定位設備數據進行拓撲計算，確定定位設備與巷道的唯一映射關系。通過構建出的拓撲關系將離散的定位設備數據和巷道數據重新組織關聯，這樣既可利用巷道ID 唯一確定定位設備，又可利用設備ID 唯一確定巷道，便于加速后續的巷道定位計算。井下數據拓撲結構如圖2。

圖2 井下數據拓撲結構Fig.2 Topology of mine and equipment data

2）巷道定位。準確定位人員所在巷道是保證糾偏計算準確率的前提。實現巷道定位需要構建人員的潛在定位巷道，再根據人員的定位信息從潛在定位巷道集合篩選最佳定位巷道。巷道定位流程如圖3。

圖3 巷道定位流程Fig.3 The flow of mine positioning

3）糾偏計算。糾偏計算的核心在于建立空間坐標糾偏解算模型，該解算模型是將定位設備對人員的測距信息、井下數據拓撲關系和巷道定位結果作為輸入參數，由模型自動計算出人員位置糾偏坐標，以代替誤差較大的原始定位坐標，由此實現對偏離巷道人員的糾偏。

2 算法設計及實現

算法的設計由井下數據拓撲關系構建和坐標糾偏解算2 部分組成。算法流程設計如圖4。

圖4 算法流程設計Fig.4 The design flow of algorithm

具體設計思路為：在構建井下數據拓撲結構階段，分別計算出巷道拓撲網絡和巷道-設備拓撲關系，利用這2 個拓撲結構將離散的數據組織關聯；在坐標糾偏解算階段，先利用巷道拓撲網絡和巷道-設備拓撲關系確定人員的最佳定位巷道，再將人員測距信息、測距信號偏離量和定位巷道的兩中線點作為參數輸入糾偏解算模型中計算出糾偏坐標，最終完成糾偏過程。

2.1 井下數據拓撲關系構建

1）巷道網絡拓撲關系。每條巷道由相互離散的中線點構成，因此可利用巷道中線點之間的鄰接關系構建巷道網絡拓撲結構。構建中線點之間鄰接關系首先需要計算出井下巷道之間的相交點，合并交點，并且為每個巷道中線點賦值唯一標識，再根據巷道網絡之間的相交關系，將每個中線點與其所有鄰接點的唯一標識一一關聯，完成巷道網絡拓撲結構的構建。巷道網絡拓撲關系模型如圖5，使用字典存儲這類拓撲關系模型，通過中線點的坐標或者ID 可快速查找與當前中線點存在鄰接關系的所有中線點。

圖5 巷道網絡拓撲結構Fig.5 Topological structure of mine network

2）巷道-設備拓撲關系。利用定位設備數據與巷道數據構建出巷道-設備拓撲關系，確定設備與巷道之間的唯一映射。通過在巷道-設備拓撲關系網絡中匹配人員定位數據中的定位設備信息可快速定位出人員位置糾偏的最佳巷道。巷道網絡-設備拓撲結構如圖6。

圖6 巷道網絡-設備拓撲結構Fig.6 Topological structure of joint mine network and equipment

2.2 坐標糾偏計算流程

實時定位糾偏算法的實現流程如下：

第1 步：人員初始坐標偏移差判斷。人員定位偏移差如圖7。

圖7 人員定位偏移差Fig.7 Calculation of personnel positioning deviation

因為每條巷道都是由2 個相互離散的巷道中線點構成的，所以選擇定位巷道中的其中1 個中線點作為計算基準點S，另1 個關聯中線點為P，則通過式（1）計算出人員T 初始坐標與巷道中線的偏移角度θ，然后利用偏移角度θ 計算出人員與巷道中線的偏離值Dc，如果Dc小于巷道半徑距離r，則將當前的人員實時位置直接投影至巷道中線上，否則進入第2 步。

第2 步：測距偏離計算。測距偏離計算如圖8。

圖8 測距偏離計算Fig.8 Calculation of distance measurement deviation

由于糾偏的結果是將人員定位至巷道的中線上，而在實際中，定位設備被安裝在巷道墻壁一側，因此測距信號相對于巷道中線存在一定夾角，因此在糾偏計算前需要分別計算出定位設備在巷道中線上的投影坐標，以及人員測距相對于巷道中線的偏移量。設D 為測距，S、P 分別為兩測距方向所對應的兩中線點，R 為定位設備，R 在由點S 和P 構成的巷道上，θ 為測距信號與巷道的偏離角（人工配置），式（3）可計算出定位設備R 的投影坐標Rp，式（4）計算得到測距偏離量Dp。

第3 步：巷道定位判斷。巷道定位判斷如圖9。

圖9 巷道定位判斷Fig.9 Judgment of mine positioning

判斷人員測距偏離量Dp是否超出定位設備與測距方向中線點距離的范圍。圖9 中黑色虛線框內為與測距方向S 相關聯的巷道中線點集合。式（5）中的S 是測距方向的中線點，Rp為定位設備在巷道中線上的投影坐標，L 為定位設備的投影坐標Rp與測距方向點S 的間距，C 為計算結果值。

第4 步：完成糾偏計算。

3 試驗測試

西北某煤礦的井下巷道情況復雜，井下人員初始定位偏離巷道的現象較為嚴重，對于實時定位糾偏的需求較為迫切，因此被用作試驗對象。分別從實時性、巷道定位精度和糾偏坐標精度3 個方面對所提出的實時定位糾偏算法進行試驗。

將實時定位糾偏算法以后端服務的方式嵌入公司開發的井下人員精確定位系統中。試驗環境如下：①操作系統：Windows Server；②硬件配置：Core i7-10700F 2.9GHz、16GB RAM；③開發環境：C#、VS2019；④開發平臺：Windows10。

3.1 實時性試驗

對實時定位糾偏算法的實時效果進行測試，實時性試驗分成拓撲計算耗時和糾偏計算耗時2 部分。試驗對象包括543 個中線點、127 個定位設備和120 個隨機定位數據。將算法部署至現場持續運行一段時間得到的實時性試驗結果見表1。

表1 實時性試驗結果Table 1 Results of real-time tests

試驗結果顯示，提出的單人實時糾偏算法平均耗時在0.002 5 s 左右，構建數據拓撲關系的耗時在0.188 s 左右，具有較好的實時效果，能夠滿足井下人員精確定位對于糾偏的實時計算需求。

3.2 巷道定位精度試驗

巷道定位精度試驗是對算法將人員定位到實際所在巷道上的準確率進行測試。試驗的樣本數據與實時性實驗保持一致。巷道定位精度試驗結果見表2。

表2 巷道定位精度試驗結果Table 2 Results of mine positioning accuracy experiment

其中，一次定位是指人員被定位在定位設備所在的巷道上，在試驗中，這些人員的巷道定位準確率為100%；多次定位為人員被定位在了定位設備所在巷道的周圍巷道上，定位準確率約為94%，試驗結果顯示，每次迭代計算出的潛在定位巷道數量為1 時，定位精度為100%，當潛在定位巷道數量大于1 時，設定位巷道與其余潛在巷道的方位角度差為β，如果β 大于30°，巷道定位全部準確，如果β 小于15°，巷道定位準確率約為86.7%，如果β 介于15°～30°，巷道定位準確率約為94.1%。多次巷道定位情況的試驗結果統計如圖10。

圖10 多次巷道定位情況的試驗結果統計Fig.10 Statistics of experimental results of multiple mine positioning

根據試驗結果的分析，當人員測距在定位設備范圍內或者潛在定位巷道數量為1 時，算法的巷道定位準確率最高，當潛在定位巷道數量大于1 時，且存在于最佳定位巷道的方位夾角過小的巷道時，定位精度有所降低，但定位精度能夠保持在85%以上，總體的巷道定位精度較好，該算法在面對多巷道關聯的復雜情況下具有一定的魯棒性。

3.3 定位糾偏精度試驗

糾偏前后效果對比如圖11。圖11（a）為試驗煤礦現場的井下人員精確定位系統中人員原始定位效果，圖11（b）為人員的原始定位經過算法糾偏后的效果，所有發生定位偏離的人員均已被準確糾偏。根據現場對于實時定位糾偏算法的實驗反饋，表明該算法具有較好的穩定性，糾偏效果明顯，能夠保持較高的準確率和實時性效果。

圖11 糾偏前后效果對比Fig.11 Comparison of the effects of personnel positioning correction

4 結 語

針對煤礦井下人員定位易受硬件系統測算誤差以及復雜巷道條件的影響，從而產生定位偏移的問題，研究并實現了一種基于井下數據拓撲結構的實時定位糾偏算法。試驗結果表明：該算法的拓撲構建耗時低于0.2 s，平均糾偏計算耗時約為0.002 5 s，對坐標偏離的人員巷道定位準確率超過90%，坐標糾偏計算平均誤差較小，具有較高的糾偏準確率和實時性，同時在復雜井下巷道條件下表現出良好的魯棒性，能夠滿足井下人員實時精確定位的需求。