基于協(xié)同定位和自適應(yīng)卡爾曼濾波的定位算法研究

龐明瑞 高振航 劉璐 臧悅程

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

關(guān)鍵詞：超寬帶;煤礦井下定位;區(qū)域判定和校正策略;協(xié)同定位算法;自適應(yīng)卡爾曼濾波

中圖分類號：TP18 ? ? 文獻(xiàn)標(biāo)識碼：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 ?引 ?言

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

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

1 ?區(qū)域判定與校正策略

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

1.1 ?定位基站分布模型

首先根據(jù)煤礦井下巷道的特點(diǎn)進(jìn)行定位基站的分布設(shè)計(jì)。井下巷道狹窄細(xì)長，為了實(shí)現(xiàn)人員的精準(zhǔn)定位，將巷道進(jìn)行二維定位模型，定位標(biāo)簽由3個(gè)以上基站進(jìn)行定位。本定位系統(tǒng)將定位基站部署在巷道墻壁兩端呈等間距分布狀，井下定位基站分布圖如圖1所示。A0，A1，…，A2n+4為對稱分布在巷道兩端的定位基站，每四個(gè)定位基站形成一個(gè)定位區(qū)域，將整個(gè)巷道分成n個(gè)區(qū)域。巷道同一側(cè)每兩個(gè)相鄰的定位基站之間的距離為l米，巷道寬即區(qū)域?qū)挒閙?？紤]基站測距的穩(wěn)定性以及保證定位的精度，l選取3 m。

1.2 ?定位過程的回傳幀機(jī)制

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

1.3 ?區(qū)域判定模型

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

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

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

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

h（Rn）表示根據(jù)便簽的位置判定區(qū)域，來求解對該標(biāo)簽進(jìn)行定位的基站的編號。判斷準(zhǔn)則為在獲得區(qū)域編號n后，求得2n+1、2n+2、2n+3、2n+4，即為基站的編號，并且標(biāo)簽只與這幾個(gè)基站進(jìn)行定位。

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

1.4 ?區(qū)域判定策略

根據(jù)區(qū)域判定模型來得到區(qū)域判定策略：

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

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

第三步：利用式h（Rn）根據(jù)判定的四個(gè)基站，進(jìn)行測距通信。

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

第五步：若標(biāo)簽檢測異常，則返回第一步。

1.5 ?區(qū)域校正機(jī)制

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

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

（2）若沒有得到4個(gè)距離值或無法結(jié)算出定位坐標(biāo)，說明測距有誤，定位基站或定位區(qū)域選擇有誤，則無法進(jìn)行區(qū)域的判定，需要進(jìn)行區(qū)域校正，重新判定。

（3）若基站接收不到通信幀或標(biāo)簽接收不到返回的回傳幀，則無法進(jìn)行區(qū)域的判定，說明基站或標(biāo)簽設(shè)備存在問題，需要進(jìn)行區(qū)域校正。

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

2 ?協(xié)同定位算法

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

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

將參考節(jié)點(diǎn)以三個(gè)為單位進(jìn)行分組，假設(shè)第k組中參考節(jié)點(diǎn)坐標(biāo)為Ai（xi，yi）、Aj（xj，yj）、Am（xm，ym），目標(biāo)定位節(jié)點(diǎn)坐標(biāo)（x，y），之間所對應(yīng)距離為di、dj、dm，列出以下距離方程：

其中，Xt-1是標(biāo)簽節(jié)點(diǎn)在（t-1）時(shí)刻的坐標(biāo)，Uth是目標(biāo)節(jié)點(diǎn)的位移閾值上限。當(dāng)某一時(shí)刻第k組參考節(jié)點(diǎn)存在異常的測距信息時(shí)，表示超出閾值，上式不成立，則該參考節(jié)點(diǎn)k組的定位結(jié)果不符，應(yīng)當(dāng)舍棄。統(tǒng)計(jì)所有參考節(jié)點(diǎn)組未超過閾值的定位結(jié)果，取其平均值作為應(yīng)求得目標(biāo)節(jié)點(diǎn)在t時(shí)刻的定位結(jié)果。

下文中將分視距環(huán)境和非視距環(huán)境兩種情況分別進(jìn)行仿真實(shí)驗(yàn)。設(shè)置四個(gè)參考節(jié)點(diǎn)作為定位基站，兩個(gè)目標(biāo)節(jié)點(diǎn)作為定位標(biāo)簽，驗(yàn)證其中一個(gè)少于三個(gè)參考節(jié)點(diǎn)的目標(biāo)節(jié)點(diǎn)的定位效果，用均方根誤差（Root Squared Error， RMSE）作為評價(jià)指標(biāo)。RMSE的計(jì)算公式為：

其中定位標(biāo)簽的真實(shí)坐標(biāo)位置為（x，y），由定位算法估計(jì)得出的位置坐標(biāo)估計(jì)值為（，）。均方根誤差值的大小和實(shí)驗(yàn)數(shù)據(jù)誤差呈正相，值越小代表計(jì)算精度越高。

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

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

在非視距環(huán)境中，遮擋住某些參考節(jié)點(diǎn)來增大其測距誤差。則非視距環(huán)境下目標(biāo)節(jié)點(diǎn)x，y軸定位誤差對比圖如7所示。

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

3 ?自適應(yīng)卡爾曼濾波算法

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

根據(jù)系統(tǒng)的狀態(tài)方程，設(shè)置狀態(tài)參數(shù)X=[x，y]，狀態(tài)轉(zhuǎn)移矩陣A=[1，0;0，1]，Wt為誤差向量。假定有一個(gè)目標(biāo)節(jié)點(diǎn)MS，參考節(jié)點(diǎn)BS數(shù)量不少于3個(gè)。t時(shí)刻第i個(gè)BS和MS之間的測距信息表示為：

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

4 ?仿真驗(yàn)證

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

將TOA定位算法、協(xié)同定位算法配合標(biāo)準(zhǔn)卡爾曼濾波算法（KF）以及協(xié)同定位算法配合改進(jìn)后的自適應(yīng)卡爾曼濾波算法（AKF）三種定位情況進(jìn)行仿真實(shí)驗(yàn)驗(yàn)證。設(shè)置采樣間隔1 s，得到目標(biāo)節(jié)點(diǎn)T2的位置，和預(yù)設(shè)的200個(gè)坐標(biāo)點(diǎn)進(jìn)行定位坐標(biāo)的比較。將改進(jìn)后的自適應(yīng)卡爾曼濾波算法（AKF）的誤警概率設(shè)置為0.01。得到的軌跡如圖10所示，TOA定位算法的誤差如圖11所示，KF和AKF定位算法的誤差如圖12所示：

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

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

5 ?結(jié) ?論

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

參考文獻(xiàn)：

[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] 方文浩，陸陽，衛(wèi)星.基于區(qū)域判定的超寬帶井下高精度定位 [J].計(jì)算機(jī)應(yīng)用，2018，38（7）：1989-1994.

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

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

[5] 劉韜，徐愛功，隋心.基于自適應(yīng)抗差卡爾曼濾波的UWB室內(nèi)定位 [J].傳感技術(shù)學(xué)報(bào)，2018，31（4）：567-572.

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