基于UKF的深井監(jiān)測(cè)移動(dòng)節(jié)點(diǎn)定位算法*

余修武，劉　琴，張　楓，周利興，胡沐芳，張　可

(1.南華大學(xué) 環(huán)境保護(hù)與安全工程學(xué)院，湖南 衡陽 421001；2.金屬礦山安全與健康國(guó)家重點(diǎn)實(shí)驗(yàn)室，安徽 馬鞍山 243000； 3.湖南省鈾尾礦庫(kù)退役治理技術(shù)工程技術(shù)研究中心，湖南 衡陽 421001)

0　引言

深井下環(huán)境復(fù)雜多變，具有高溫、高濕、通風(fēng)性差等特點(diǎn)，使得礦井開采及人員設(shè)備管理愈加困難，為保障井下工作的安全有序進(jìn)行，引入一項(xiàng)重要的現(xiàn)代監(jiān)測(cè)技術(shù)，即定位監(jiān)測(cè)技術(shù)[1-5]。在正常情況下，可以對(duì)人員、礦車等進(jìn)行實(shí)時(shí)動(dòng)態(tài)定位監(jiān)測(cè)；一旦發(fā)生礦難，可及時(shí)定位救援，減少傷亡和損失。礦井巷道信號(hào)，尤其在深井中，衰減速度較快，單純采用接收信號(hào)強(qiáng)度(RSSI)測(cè)距算法來進(jìn)行定位[6]，其定位精度較低，為提高其定位精度，文獻(xiàn)[7]提出了一種錨節(jié)點(diǎn)鏈?zhǔn)讲渴鸬膭?dòng)態(tài) RSSI測(cè)距定位算法，利用錨節(jié)點(diǎn)間距和RSSI來計(jì)算巷道實(shí)際環(huán)境中的路徑損耗指數(shù)，以此提高RSSI定位精度及對(duì)環(huán)境的適應(yīng)性；文獻(xiàn)[8]提出了一種動(dòng)態(tài)識(shí)別礦井人員的無線全局定位算法，按照巷道的地形特點(diǎn)建立相應(yīng)的地形模型，結(jié)合局部坐標(biāo)系變換，實(shí)現(xiàn)人員無線全局定位，該算法具有一定的定位精度；文獻(xiàn)[9]引入卡爾曼濾波，對(duì)礦井的移動(dòng)節(jié)點(diǎn)進(jìn)行定位，通過與彈性粒子型相結(jié)合的方式，提高對(duì)移動(dòng)節(jié)點(diǎn)的實(shí)時(shí)定位精度；文獻(xiàn)[10]針對(duì)井下人員的移動(dòng)特性與定位精度，提出一種結(jié)合運(yùn)動(dòng)方程與卡爾曼濾波的移動(dòng)節(jié)點(diǎn)預(yù)測(cè)算法，定位精度有一定的提高并且能對(duì)未知數(shù)據(jù)進(jìn)行預(yù)測(cè)。上述文獻(xiàn)均基于線性系統(tǒng)理論，而實(shí)際深井定位是非線性系統(tǒng)，本文提出一種基于非線性函數(shù)不敏卡爾曼濾波( UKF)[11-13]移動(dòng)節(jié)點(diǎn)定位算法(U-MPA)，以進(jìn)一步減少定位誤差。

1　U-MPA監(jiān)測(cè)系統(tǒng)構(gòu)建

井下環(huán)境中，無線傳感器網(wǎng)絡(luò)的錨節(jié)點(diǎn)是沿巷道確定性間隔部署的，而移動(dòng)節(jié)點(diǎn)是沿巷道隨機(jī)游動(dòng)分布的礦車或人員。U-MPA監(jiān)測(cè)系統(tǒng)拓?fù)淙鐖D1所示，其中礦井外部設(shè)置集控中心，與現(xiàn)有工業(yè)以太網(wǎng)相連接，傳輸速度快，安全性高；巷道內(nèi)，每隔一定間距，在巷道壁上布設(shè)1個(gè)固定傳感器節(jié)點(diǎn)(即錨節(jié)點(diǎn))，用于實(shí)時(shí)采集巷道內(nèi)環(huán)境信息，包括：溫度、濕度、風(fēng)速和有害氣體濃度等[14]；移動(dòng)傳感器節(jié)點(diǎn)(即移動(dòng)節(jié)點(diǎn))由人員佩戴(或安裝至礦車)，用于實(shí)時(shí)監(jiān)測(cè)人員(或礦車)周圍環(huán)境，可以有效地對(duì)錨節(jié)點(diǎn)間盲區(qū)進(jìn)行監(jiān)測(cè)；集控中心通過移動(dòng)節(jié)點(diǎn)與錨節(jié)點(diǎn)構(gòu)成的定位系統(tǒng)，可以清楚地知道人員(或礦車)的具體位置，一般情況下可用于人員考勤與監(jiān)控，危急情況下可用于應(yīng)急定位救援。

圖1　礦井U-MPA監(jiān)測(cè)系統(tǒng)拓?fù)銯ig.1　Monitoring system topology of U-MPA in mine

2　U-MPA定位算法

2.1　RSSI測(cè)距

RSSI測(cè)距的基本原理是：根據(jù)收發(fā)節(jié)點(diǎn)的信號(hào)強(qiáng)度，計(jì)算信號(hào)的傳輸損耗，再將傳輸損耗轉(zhuǎn)化為距離。對(duì)于節(jié)點(diǎn)間的距離選用對(duì)數(shù)-常態(tài)分布模型進(jìn)行計(jì)算，如式(1)所示：

(1)

式中：d表示接收端與發(fā)射端的距離，單位為m；d0表示參考距離，通常取d0為1 m；RSSI(d)表示距離發(fā)射源d處接收到的信號(hào)強(qiáng)度，單位為dBm；RSSI(d0)表示對(duì)應(yīng)d0處待定節(jié)點(diǎn)接收到的信號(hào)強(qiáng)度，單位為dBm；β表示路徑衰減指數(shù)，周圍環(huán)境變化會(huì)對(duì)其產(chǎn)生很大影響。

為了使得該模型能盡量滿足礦井的特殊環(huán)境，保證RSSI測(cè)距的精度，對(duì)RSSI(d0)和β進(jìn)行優(yōu)化，得到最佳值。由文獻(xiàn)[8]可知，最佳值RSSI(d0)=41，β=2.3。

2.2　UKF節(jié)點(diǎn)距離計(jì)算

移動(dòng)節(jié)點(diǎn)定位系統(tǒng)是一種非線性系統(tǒng)，為了提高對(duì)傳感器節(jié)點(diǎn)位置的估計(jì)精度，采用不敏卡爾曼濾方法對(duì)系統(tǒng)進(jìn)行預(yù)估。一般形式的離散非線性系統(tǒng)[15]方程如式(2)所示：

(2)

式中：xk表示k時(shí)刻的預(yù)估值；F表示系統(tǒng)參數(shù)，這里取為1；zk表示k時(shí)刻的測(cè)量值；H表示測(cè)量系統(tǒng)參數(shù)，取其為1；Wk和Vk分別表示均值為零的高斯過程噪聲和測(cè)量噪聲，協(xié)方差分別為Qk和Rk。

UKF是采用不敏變換來進(jìn)行遞推計(jì)算的，經(jīng)變換后的均值和協(xié)方差能夠精確地表示。設(shè)有均值為μ、協(xié)方差為Pk的n維隨機(jī)變量x，可以得到(2n+1)個(gè)采樣點(diǎn)χi和對(duì)應(yīng)的權(quán)值ωi，如式(3)所示：

(3)

k時(shí)刻，第i個(gè)采樣點(diǎn)χi的計(jì)算如式(4)所示：

(4)

(5)

設(shè)k時(shí)刻采樣點(diǎn)j的狀態(tài)估計(jì)向量為Xj(k)，協(xié)方差為Pj(k)，根據(jù)式(4)、式(5)計(jì)算出樣點(diǎn)χj,k-1|k-1和其對(duì)應(yīng)的權(quán)值ωj，由式(3)可得到相應(yīng)樣點(diǎn)的一步預(yù)測(cè)，如式(6)所示：

(6)

式中：χj,k|k-1表示k-1時(shí)刻的估計(jì)值；zj,k|k-1表示k-1時(shí)刻的測(cè)量值。

利用式(6)得到的一步預(yù)測(cè)χj,k|k-1，可得到狀態(tài)預(yù)測(cè)估計(jì)Xj,k|k-1和狀態(tài)預(yù)測(cè)協(xié)方差Pj,k|k-1，如式(7)所示：

(7)

則狀態(tài)測(cè)量與相應(yīng)的協(xié)方差如式(8)所示：

綜合狀態(tài)預(yù)測(cè)估計(jì)和狀態(tài)測(cè)量，得到現(xiàn)在狀態(tài)的最優(yōu)狀態(tài)估計(jì)Xj,k及其相應(yīng)的協(xié)方差Pj,k，如式(9)所示：

(9)

(10)

式中：Kk為k時(shí)刻的卡爾曼增益, 如式(11)所示：

(11)

根據(jù)RSSI測(cè)距模型和估算值RSSIk|k，由式(1)得錨節(jié)點(diǎn)與移動(dòng)節(jié)點(diǎn)的距離，如式(12)所示：

(12)

2.3　移動(dòng)節(jié)點(diǎn)定位

為了更準(zhǔn)確地對(duì)移動(dòng)節(jié)點(diǎn)進(jìn)行定位，根據(jù)巷道的地形特點(diǎn)建立巷道地形模型及局部坐標(biāo)系，最終將局部坐標(biāo)系轉(zhuǎn)化至全局坐標(biāo)系中，實(shí)現(xiàn)對(duì)移動(dòng)節(jié)點(diǎn)的定位監(jiān)測(cè)。

按照巷道的地形特點(diǎn)，可以將其分為以下3種模型：直線型巷道模型，如圖2(a)所示；凹型巷道模型，如圖2(b)所示；凸型巷道模型，如圖2(c)所示。

圖2　礦井巷道典型地形模型Fig.2　Typical terrain model of mine laneway

根據(jù)RSSI測(cè)距法，采用屏蔽模型，可以求得移動(dòng)節(jié)點(diǎn)的坐標(biāo)值，定位算法如式(13)所示：

(13)

式中：d(a)和d(b)表示移動(dòng)節(jié)點(diǎn)到錨節(jié)點(diǎn)的距離；d表示2相鄰錨節(jié)點(diǎn)的距離；φ表示2錨節(jié)點(diǎn)連線與移動(dòng)節(jié)點(diǎn)間的夾角。

在進(jìn)行礦體的開采時(shí)，并非所有的巷道均是直線型，可能隔幾十米會(huì)出現(xiàn)凹凸型的彎道，上述典型巷道模型能夠隨機(jī)組合成一個(gè)完整的巷道。為了得到移動(dòng)節(jié)點(diǎn)在巷道中的全局定位，需要將巷道局部坐標(biāo)系向全局坐標(biāo)系變換，采用該定位方法，會(huì)使得移動(dòng)節(jié)點(diǎn)定位精度得到提高。

3　U-MPA定位算法實(shí)現(xiàn)流程

U-MPA定位算法實(shí)現(xiàn)過程如圖3所示。

圖3　U-MPA定位算法流程Fig.3　Flow chart of U-MPA

1)錨節(jié)點(diǎn)周期性發(fā)送自身ID、信號(hào)接收強(qiáng)度RSSI(位置信息)。

2)設(shè)置移動(dòng)節(jié)點(diǎn)的信號(hào)接收強(qiáng)度RSSI是上一時(shí)刻的測(cè)量值。

3)經(jīng)過UKF預(yù)估后，得出錨節(jié)點(diǎn)與移動(dòng)節(jié)點(diǎn)的距離。

4)為了對(duì)移動(dòng)節(jié)點(diǎn)實(shí)現(xiàn)更準(zhǔn)確定位，引入巷道局部坐標(biāo)系，對(duì)典型巷道進(jìn)行坐標(biāo)分析，由此得到移動(dòng)節(jié)點(diǎn)的位置信息。

4　仿真實(shí)驗(yàn)

采用MATLAB對(duì)U-MPA定位算法進(jìn)行仿真試驗(yàn)，同時(shí)與RSSI定位算法進(jìn)行比較。在100 m×5 m的矩形區(qū)域內(nèi)進(jìn)行監(jiān)測(cè)，錨節(jié)點(diǎn)沿巷道兩側(cè)間隔部署，設(shè)錨節(jié)點(diǎn)的通信半徑為45 m。無線傳感器網(wǎng)絡(luò)在平面的通信半徑為100 m左右，而無線信號(hào)在深井下傳輸受環(huán)境影響較嚴(yán)重，為了達(dá)到較好數(shù)據(jù)傳輸，一般選擇取小于100 m的一半較好。

4.1　錨節(jié)點(diǎn)間隔誤差

如圖4所示，表示錨節(jié)點(diǎn)按不同間隔部署時(shí)的定位誤差，從中可以看出，隨著錨節(jié)點(diǎn)的間距增大，節(jié)點(diǎn)的定位誤差也隨之增大。錨節(jié)點(diǎn)間距越小，通信范圍內(nèi)移動(dòng)節(jié)點(diǎn)隨機(jī)分布的范圍就小，因此錨節(jié)點(diǎn)附近的移動(dòng)節(jié)點(diǎn)定位產(chǎn)生的誤差就更小。

圖4　定位誤差與錨節(jié)點(diǎn)間隔密度關(guān)系Fig.4　Relationship between positioning error and anchor node spacing density

從圖4中的仿真實(shí)驗(yàn)結(jié)果可以知，U-MPA算法對(duì)井下移動(dòng)節(jié)點(diǎn)定位有較好的定位精度，在錨節(jié)點(diǎn)間隔密度為35 m和75 m時(shí)，U-MPA定位誤差分別為2%和25%，RSSI算法定位誤差分別為15%和80%；在錨節(jié)點(diǎn)間隔45 m時(shí)，定位誤差為5%，因此可根據(jù)定位精度的要求，部署相應(yīng)數(shù)量的錨節(jié)點(diǎn)來提高定位精度。

4.2　U-MPA定位偏差

(14)

圖5　定位偏差與錨節(jié)點(diǎn)間隔密度關(guān)系Fig.5　Relationship between positioning deviation and anchor node spacing density

由圖5中的仿真實(shí)驗(yàn)結(jié)果可知，在錨節(jié)點(diǎn)間隔密度為0～100 m內(nèi)，RSSI算法的最大定位偏差14.8 m，U-MPA算法的最大定位偏差7.6 m，為RSSI算法的51%；RSSI算法的平均定位偏差4.8 m，U-MPA算法的平均定位偏差2.1 m，為RSSI算法的44%；U-MPA算法較RSSI算法的平均定位偏差有明顯降低。

5　結(jié)論

1)根據(jù)深井實(shí)際環(huán)境，以及人員或礦車設(shè)備的移動(dòng)特性，提出一種基于非線性函數(shù)不敏卡爾曼濾波移動(dòng)節(jié)點(diǎn)定位算法(U-MPA)，經(jīng)與RSSI定位算法對(duì)比仿真，U-MPA定位精度較高。

2)根據(jù)仿真計(jì)算，在錨節(jié)點(diǎn)間隔密度≤100 m時(shí)，U-MPA算法的平均偏差-均方根定位誤差(RMSE)是RSSI算法的44%；錨節(jié)點(diǎn)間隔密度為45 m時(shí)，通信性價(jià)比較高，U-MPA算法和RSSI算法的定位誤差分別為5%和20%，定位精度提高了4倍。

[1]余修武，范飛生，李睿，等．基于接收信號(hào)強(qiáng)度分區(qū)礦山無線定位算法[J]．中國(guó)安全生產(chǎn)科學(xué)技術(shù)，2015,11(9)：70-75．

YU Xiuwu,FAN Feisheng,LI Rui,et al.Study on wireless positioning algorithm in mine based on received signal strength partition[J]. Journal of Safety Science and Technology, 2015,11(9):70-75.

[2]冀汶莉，馬晴，賈東．井下移動(dòng)目標(biāo)定位跟蹤關(guān)鍵技術(shù)[J]．西安科技大學(xué)學(xué)報(bào)，2016，36(1)：132-138．

JI Wenli, MA Qing, JIA Dong. Key technology of locating and tracking mobile object in underground mines[J]. Journal of Xi’an University of Science and Technology, 2016,36(1):132-138.

[3]李寧，王李管，賈明濤，等．基于改進(jìn)遺傳算法的礦井人員定位新算法[J]．中南大學(xué)學(xué)報(bào)(自然科學(xué)版)，2016，47(3)：929-935．

LI Ning,WANG Liguan, JIA Mingtao,et al.New personnel localization algorithm in mine based on improved genetic algorithm[J]. Journal of Central South University, 2016,47(3):929-935.

[4]Sun J P, Li C X. Mine TOA geometric positioning method based on improved mean filtering and parameter fitting[J]. Meitan Xuebao/journal of the China Coal Society, 2015, 40(5):1206-1212.

[5]黃曉鵬. 無線傳感器網(wǎng)絡(luò)中移動(dòng)信標(biāo)節(jié)點(diǎn)的路徑規(guī)劃與定位算法研究[D].南京：南京大學(xué),2015.

[6]Ye M, Wang Y P. Location estimation in wireless sensor networks based on probabilistic model with variant variance and evolutionary algorithm[J]. Journal of Software, 2013, 24(4):859-872.

[7]喬鋼柱, 曾建潮. 信標(biāo)節(jié)點(diǎn)鏈?zhǔn)讲渴鸬木聼o線傳感器網(wǎng)絡(luò)定位算法[J]. 煤炭學(xué)報(bào), 2010, 35(7):1229-1233.

QIAO Gangzhu, ZENG Jianchao. Localization algorithm of beacon nodes chain deployment based on coal mine underground wireless sensor networks[J].Journal of China Coal Society,2010,35(7):1229-1233.

[8]王紅旗, 劉勇, 羅宇鋒. 段序號(hào)可動(dòng)態(tài)識(shí)別的礦井人員無線全局定位[J]. 計(jì)算機(jī)工程與應(yīng)用, 2014, 50(19):245-248.

WANG Hongqi, LIU Yong, LUO Yufeng. Wireless global positioning of mine personnel with dynamically identifiable segmental serials[J]. Computer Engineering and Applications,2014,50(19): 245-248.

[9]王建平，徐恒，李奇越．基于卡爾曼濾波的礦井移動(dòng)節(jié)點(diǎn)定位算法研究[J]．電子測(cè)量與儀器學(xué)報(bào)，2013，27(2)：120-126．

WANG Jianping,XU Heng,LI Qiyue.Research of algorithm for mobile node localization in mine based on kalman filtering[J]. Journal of Electronic Measurement and Instrument, 2013,27(2):120-126.

[10]王妍，鄧慶緒，劉賡浩，等．結(jié)合運(yùn)動(dòng)方程與卡爾曼濾波的動(dòng)態(tài)目標(biāo)追蹤預(yù)測(cè)算法[J]．計(jì)算機(jī)科學(xué)，2015，42(12)：76-81．

WANG Yan, DENG Qingxu,LIU Genghao,et al. Dynamic target tracking and predicting algorithm based on combination of motion equation and kalman filter[J]. Computer Science, 2015,42(12):76-81.

[11]蔣鵬，宋華華，王興民．基于動(dòng)態(tài)生成樹和改進(jìn)不敏卡爾曼濾波的傳感器網(wǎng)絡(luò)目標(biāo)跟蹤算法研究[J]．儀器儀表學(xué)報(bào)，2015，36(2)：415-421．

JIANG Peng, SONG Huahua, WANG Xingmin. Target tracking algorithm for sensor networks based on dynamic spanning tree and improved unscented kalman filter[J]. Chinese Journal of Scientific Instrument,2015,36(2):415-421.

[12]Zhen-Tao H U, Jin Z, Zhen G. Unscented kalman probability hypothesis density filter based on interacting multiple model[J]. Control & Decision, 2016.

[13]于洪波，王國(guó)宏，孫蕓，等．一種融合UKF和EKF的粒子濾波狀態(tài)估計(jì)算法[J]．系統(tǒng)工程與電子技術(shù)，2013，35(7)：1375-1379．

YU Hongbo,WANG Guohong, SUN Yun,et al. Particle filtering algorithm of state estimation on fusion of UKF and EKF[J]. System Engineering and Electronics,2013,35(7):1375-1379.

[14]李菲菲. WSN煤礦瓦斯?jié)舛缺O(jiān)測(cè)系統(tǒng)時(shí)間同步技術(shù)研究[D].徐州：中國(guó)礦業(yè)大學(xué),2015.

[15]李猛. 礦井安全監(jiān)控系統(tǒng)結(jié)構(gòu)及無線定位技術(shù)研究[D].重慶：重慶大學(xué),2012.