基于協同定位和自適應卡爾曼濾波的定位算法研究

龐明瑞 高振航 劉璐 臧悅程

摘 ?要：針對煤礦井下復雜環境中非視距誤差對無線信號定位的干擾，文章設計了區域判定和校正策略減小標簽和基站之間的無效通信，提高了定位效率;提出基于UWB的煤礦井下協同定位算法，移動的標簽充當定位基站，減少定位盲區提高定位精度;提出改進的自適應卡爾曼濾波算法對協同定位結果進行優化，獲取信息向量檢驗非視距環境誤差，并對協方差矩陣進行修正。仿真驗證結果表明：改進后的自適應卡爾曼濾波算法結合協同定位算法相比卡爾曼濾波算法結合協同定位算法定位誤差更小，可以更好地應用于煤礦井下人員定位中。

關鍵詞：超寬帶;煤礦井下定位;區域判定和校正策略;協同定位算法;自適應卡爾曼濾波

中圖分類號：TP18 ? ? 文獻標識碼：A文章編號：2096-4706（2021）24-0074-07

Abstract： Aiming at the interference of non-line-of-sight errors on wireless signal positioning in complex environment of underground coal mines， this paper designs a regional determination and correction strategy to reduce the invalid communication between tags and base stations， and improves positioning efficiency; an underground coal mine cooperative positioning algorithm based on UWB is proposed， the mobile tag acts as a positioning base station， reducing the positioning blind area and improving the positioning accuracy; an improved adaptive Kalman filter algorithm is proposed to optimize the collaborative positioning result， obtain the information vector to check the non-line-of-sight environment error， and correct the covariance matrix. The simulation results show that the improved adaptive Kalman filter algorithm combined with cooperative positioning algorithm has less positioning error than Kalman filter algorithm combined with cooperative positioning algorithm， and can be better applied to personnel positioning in underground coal mine.

Keywords： ultra-wideband; underground coal mine positioning; area determination and correction strategy; cooperative positioning algorithm; adaptive Kalman filter

0 ?引 ?言

煤炭資源在我國能源中仍占主體地位，是我們國家經濟社會發展、社會生產生活之中最為關鍵和緊要的原始材料和能量來源。近年來我國高度重視煤炭行業的安全生產工作，大力建設并完善煤礦井下人員定位系統。煤礦井下人員定位領域逐步追求達到厘米級的定位精度，相關定位技術和產品已經成為許多行業的研究熱點。而UWB技術可以憑借較高的定位精度以及自身良好的工作性能，很好地適應于井下工作環境，逐漸成為近年來煤礦井下定位的首選方式。因此，本文基于UWB技術，對煤礦井下人員定位算法進行研究，實現井下人員實時跟蹤與精確定位。

本文采用TOA測距的定位方法進行定位基于到達時間TOA測距的定位方法，它根據UWB信號在空氣中的傳播時間來獲取定位標簽和定位基站之間的距離，從而求解出定位標簽的位置[1]。采用改進的非對稱雙邊雙程測距ADS-TWR算法在一定程度上進行測距上的優化，能夠有效地消除節點間時鐘未完全同步對測距精度的影響。之后采用協同定位及自適應卡爾曼濾波算法對定位結果進行優化，減小非視距誤差對定位結果的影響，提高定位精度。

1 ?區域判定與校正策略

在實際煤礦井下人員定位時，定位標簽的每一次定位都要向所有通信范圍內的井下定位基站發送請求幀，進行ADS-TWR測距來實現信息交互。而完成定位并不需要這么多定位基站來通信，這樣會造成大量無效通信，一方面消耗節點功率，另一方面增加獲取數據的時間，增加系統復雜程度，嚴重影響定位的實時性和準確性。因此需要進行標簽的區域判定與校正，自動判定所處的區域并且只與該區域內的基站進行通信[2]，來提高定位的效率。并且通過區域校正保證判定的標簽處在準確的區域內。

1.1 ?定位基站分布模型

首先根據煤礦井下巷道的特點進行定位基站的分布設計。井下巷道狹窄細長，為了實現人員的精準定位，將巷道進行二維定位模型，定位標簽由3個以上基站進行定位。本定位系統將定位基站部署在巷道墻壁兩端呈等間距分布狀，井下定位基站分布圖如圖1所示。A0，A1，…，A2n+4為對稱分布在巷道兩端的定位基站，每四個定位基站形成一個定位區域，將整個巷道分成n個區域。巷道同一側每兩個相鄰的定位基站之間的距離為l米，巷道寬即區域寬為m?？紤]基站測距的穩定性以及保證定位的精度，l選取3 m。

1.2 ?定位過程的回傳幀機制

在采用ADS-TWR測距算法之后，不需要定位基站和定位標簽的時鐘同步，減小了定位標簽和定位基站之間由時鐘漂移造成的測距誤差，提高了定位精度。而引入區域判定與區域校正機制后需要在測距后加入一個Resport幀，標簽向基站發送Final幀后，基站向標簽再發送一個回傳幀Resport，其中含有標簽基站測距得到的距離值，為標簽的區域判定提供測距值?；貍鲙瑱C制如圖2所示：

1.3 ?區域判定模型

在確定井下巷道內基站的分布以及引入定位過程的回傳幀機制后，根據定位基站的分布圖來進行區域判定。區域判定主要包括定位基站、測距值以及定位區域三個要素。對著三個要素分別建立集合，分別用I、D、R表示，其中I集合是所有定位基站的編號集合，D集合是測量距離，是在單次定位過程中標簽通信范圍之內所有的定位基站發送給定位標簽的Report回傳幀所包含的測距值，R是所有的定位區域，面積為m×l。他們之間建立的關系模型如圖3所示：

根據上圖關系模型，得到標簽的區域判定模型：首先，標簽根據接收到所有通信距離內基站發來的回傳幀所包含的距離值來判斷出5個與自身距離最短的定位基站，并尋找4個編號為基數并且連續的基站，假設這4個基站編號為i1、i2、i3、i4，其中i1=2n+1、i2=2n+2、i3=2n+3、i4=2n+4，則這四個基站Ai1、Ai2、Ai3、Ai4就在同一個定位區域內，他們所圍成的定位區域則為Rn。判定到定位區域后，標簽則與這幾個定位基站Ai1、Ai2、Ai3、Ai4進行通信，Ai1、Ai2、Ai3、Ai4與標簽通過ADS-TWR測距算法分別測得距離di1、di2、di3、di4。之后標簽根據測得的四個距離di1、di2、di3、di4計算出此時相對于區域Rn的坐標（x，y）。

其中，f（D）表示定位標簽根據接收到所有通信距離內基站發來的回傳幀所包含的距離值來判斷出4個與自身距離最短的定位基站，判別準則為編號是奇數開頭并且基站編號為4個連續的編號，編號即為f（D）所求值。

g（i1，i2，i3，i4）表示標簽所處的區域編號的求解公式，根據f（D）確定的定位基站編號來確定標簽所處的定位區域Rn，判別準則為找到4個連續編號基站中編號最小的基站，令其編號為2n+1，求得n的值即為區域的標號，Zn即為判定區域。

h（Rn）表示根據便簽的位置判定區域，來求解對該標簽進行定位的基站的編號。判斷準則為在獲得區域編號n后，求得2n+1、2n+2、2n+3、2n+4，即為基站的編號，并且標簽只與這幾個基站進行定位。

p（di1，di2，di3，di4，Rn）表示對標簽所處區域的判定，利用標簽與4個基站的回傳幀所包含的測距值來判定變遷是否仍處于當前區域，若不處于則進行區域的更新。判定準則是根據標簽在該定位區域的定位坐標來判斷，若橫坐標x<0，則在編號為n-1的定位區域內;若0l則在編號為n+1的下一區域內。

1.4 ?區域判定策略

根據區域判定模型來得到區域判定策略：

第一步：標簽上電并向所有基站發送定位幀，測距結果通過回傳幀Report發送給標簽，利用判定模型中f（D）計算出基站（i1，i2，i3，i4）。

第二步：利用式g（i1，i2，i3，i4）計算出定位區域Rn。

第三步：利用式h（Rn）根據判定的四個基站，進行測距通信。

第四步：得到距離值di1，di2，di3，di4，最小二乘法定位得到標簽在該定位區域內的坐標值（x，y），之后根據式p（di1，di2，di3，di4，Rn）判定區域，若x<0，則在編號為n-1的定位區域Rn-1內，返回第三步判斷新的定位基站;若x>l則在編號為n+1的下一區域Rn+1內，返回第三步判斷新的定位基站。

第五步：若標簽檢測異常，則返回第一步。

1.5 ?區域校正機制

煤礦井下環境復雜，電磁干擾、環境變化等都會影響標簽和基站測距過程，造成一些誤差，因此我們建立區域校正機制來針對這些數據異常進行處理校正。它的原理就是檢測區域判定過程中的異常情況，一旦檢測出數據異常，則進行初始化重新進行區域判定。異常情況包括以下3種：

（1）若第一步無法滿足得到四個編號連續且最小的編號為奇數的定位基站，如編號不連續或不滿足四個等情況，則無法進行區域的判定，需要進行區域校正。

（2）若沒有得到4個距離值或無法結算出定位坐標，說明測距有誤，定位基站或定位區域選擇有誤，則無法進行區域的判定，需要進行區域校正，重新判定。

（3）若基站接收不到通信幀或標簽接收不到返回的回傳幀，則無法進行區域的判定，說明基站或標簽設備存在問題，需要進行區域校正。

綜合以上定位基站分布模型、定位過程的回傳幀機制、區域判定模型、區域判定過程以及區域校正策略可得，區域判定和校正機制可以解決標簽無效通信的問題，降低時間損耗，提高定位精確性和準確度。采用區域判定與校正的系統測距定位總流程圖如圖4所示。

2 ?協同定位算法

為了減小煤礦井下非視距誤差對人員定位造成的誤差，本課題提出一種基于UWB的煤礦井下人員協同定位算法對定位結果進行優化。其基本原理是定位基站充當固定參考節點，定位標簽充當移動的參考節點[3]，原理圖如圖5所示，A1、A2、A3、A4、A5、A6為基站，T1、T2、T3為標簽。

目標節點的定位需要三個參考節點的位置信息。圖中T1、T3標簽節點均滿足定位范圍內3個定位基站的要求，而T2節點覆蓋的通信范圍中僅有A1、A5這兩個定位基站，按照傳統TOA定位方法，不滿足三個基站不能夠完成定位。但是如果采用文中提出的協同定位的定位方法，標簽節點可以充當定位基站與T2進行通信，則可以把T2通信范圍內的T1標簽節點視為移動的定位參考節點來對T2的進行定位。這種標簽當作基站的方式迭代，可以獲得所有目標節點的位置。

將參考節點以三個為單位進行分組，假設第k組中參考節點坐標為Ai（xi，yi）、Aj（xj，yj）、Am（xm，ym），目標定位節點坐標（x，y），之間所對應距離為di、dj、dm，列出以下距離方程：

其中，Xt-1是標簽節點在（t-1）時刻的坐標，Uth是目標節點的位移閾值上限。當某一時刻第k組參考節點存在異常的測距信息時，表示超出閾值，上式不成立，則該參考節點k組的定位結果不符，應當舍棄。統計所有參考節點組未超過閾值的定位結果，取其平均值作為應求得目標節點在t時刻的定位結果。

下文中將分視距環境和非視距環境兩種情況分別進行仿真實驗。設置四個參考節點作為定位基站，兩個目標節點作為定位標簽，驗證其中一個少于三個參考節點的目標節點的定位效果，用均方根誤差（Root Squared Error， RMSE）作為評價指標。RMSE的計算公式為：

其中定位標簽的真實坐標位置為（x，y），由定位算法估計得出的位置坐標估計值為（，）。均方根誤差值的大小和實驗數據誤差呈正相，值越小代表計算精度越高。

視距環境下定位得到x，y軸的誤差對比如圖6所示，計算出的均方根誤差RMSE值如表1所示：

由以上仿真結果可得，在視距環境中，TOA定位算法以及協同定位算法都有較高的定位精度，在此基礎上協同定位算法精度稍好于TOA定位算法，性能也有所提升。

在非視距環境中，遮擋住某些參考節點來增大其測距誤差。則非視距環境下目標節點x，y軸定位誤差對比圖如7所示。

可以得出在非視距環境中，協同定位算法目標節點定位誤差相對較小。因此，不論是視距環境還是非視距環境中，協同定位算法都比TOA定位算法有更好的定位精度，尤其在非視距環境中優勢更明顯。因此本文將利用協同定位算法來抑制非視距誤差，提高井下工作人員的定位精度。

3 ?自適應卡爾曼濾波算法

卡爾曼濾波在視距環境中能夠消除測量誤差，但是在非視距環境中，噪聲的分布概率會由于環境的影響出現變化，導致噪聲的協方差矩陣難以確定[4]。本文根據協同定位算法解算出的定位結果，獲取卡爾曼濾波器的新息向量[5]來檢驗非視距環境中的測距誤差，之后根據檢測結果修正協方差矩陣，同時構造觀測噪聲的協方差矩陣，來減少測距異常值，提高定位精度。算法的流程圖如圖8所示。

根據系統的狀態方程，設置狀態參數X=[x，y]，狀態轉移矩陣A=[1，0;0，1]，Wt為誤差向量。假定有一個目標節點MS，參考節點BS數量不少于3個。t時刻第i個BS和MS之間的測距信息表示為：

令α=diag（α1，α2，…，αn），調整系統的觀測噪聲協方差矩陣，對卡爾曼濾波算法進行自適應修正，在一定程度上可以實現對測量信息的修正，從而提高系統的抗干擾能力，提升定位精度。

4 ?仿真驗證

為了驗證協同定位算法以及自適應濾波算法對定位精確度的影響，采用四個參考節點和兩個目標節點進行仿真實驗，目標節點T1在完成自身定位后用作T2的參考節點，對目標節點T2進行定位。仿真實驗的仿真環境為：充當基站的參考節點A1（0，10），A2（10，10），A3（10，0），A4（15，7.5），以及充當標簽的目標節點T1坐標位置（5，5），T2坐標（1.5，0.8），單位為米（m）。之后生成多個參考，獲取目標節點T2在各參考節點定位下的測距信息，并且加入下如圖10所示的目標節點T2的測距隨機誤差。實際情況中非視距環境測距誤差約為50 mm～200 mm，在視為非視距情況。加入的隨機測距噪聲如圖9所示。

將TOA定位算法、協同定位算法配合標準卡爾曼濾波算法（KF）以及協同定位算法配合改進后的自適應卡爾曼濾波算法（AKF）三種定位情況進行仿真實驗驗證。設置采樣間隔1 s，得到目標節點T2的位置，和預設的200個坐標點進行定位坐標的比較。將改進后的自適應卡爾曼濾波算法（AKF）的誤警概率設置為0.01。得到的軌跡如圖10所示，TOA定位算法的誤差如圖11所示，KF和AKF定位算法的誤差如圖12所示：

則由數據可得三種定位算法RMSE值如表3所示。

通過如上仿真實驗的驗證可得，TOA定位算法的定位誤差最大，卡爾曼濾波算法結合協同定位算法很大程度上降低了非視距誤差，有很好的定位效果，改進后的自適應卡爾曼濾波算法結合協同定位算法調整協方差矩陣相比卡爾曼濾波算法結合協同定位算法定位誤差更小，可以更好地應用到煤礦井下人員定位中。

5 ?結 ?論

本文利用TOA的定位方法進行煤礦井下人員定位，在ADS-TWR測距算法的基礎上增加區域判定與校正機制減少無效通信，提高定位效率;之后采用協同定位算法將標簽作為定位基站進行定位，分別在視距和非視距環境下進行仿真實驗驗證其對定位精度的提升;最后采用自適應卡爾曼濾波片算法獲取信息向量檢驗非視距環境誤差，并對協方差矩陣進行修正，降低誤差，并進行仿真實驗驗證自適應卡爾曼濾波算法結合協同定位算法在非視距誤差環境下相比卡爾曼濾波算法結合協同定位算法定位誤差更小，可以更好地應用于煤礦井下人員定位中。

參考文獻：

[1] GAO S C，ZHANG S J，WANG G，et al. Robust Second-Order Cone Relaxation for TW-TOA-Based Location With Clock Imperfection [J].IEEE Signal Processing Letters，2016，23（8）：1047-1051.

[2] 方文浩，陸陽，衛星.基于區域判定的超寬帶井下高精度定位 [J].計算機應用，2018，38（7）：1989-1994.

[3] 蔣晗中.協同定位技術研究綜述 [J].通信電源技術，2017，34（6）：48-50.

[4] 鄧鍇.基于NLOS識別和誤差消除的無線定位算法研究 [D].成都：西南交通大學，2018.

[5] 劉韜，徐愛功，隋心.基于自適應抗差卡爾曼濾波的UWB室內定位 [J].傳感技術學報，2018，31（4）：567-572.

作者簡介：龐明瑞（1997—）女，漢族，山西大同人，碩士研究生在讀，研究方向：嵌入式技術;高振航（1998—）男，漢族，河南開封人，碩士研究生在讀，研究方向：嵌入式技術;劉璐（1997—）女，漢族，山東濱州人，碩士研究生在讀，研究方向：嵌入式技術;臧悅程（1998—）男，漢族，吉林白山人，碩士研究生在讀，研究方向：嵌入式技術。