孫 偉,李婉秋,初 婧,李瑞豹

(遼寧工程技術(shù)大學(xué)測繪與地理科學(xué)學(xué)院,遼寧 阜新 123000)

?

基于誤差修正技術(shù)的井下人員MEMS定位方法*

孫 偉*,李婉秋,初 婧,李瑞豹

(遼寧工程技術(shù)大學(xué)測繪與地理科學(xué)學(xué)院,遼寧 阜新 123000)

針對礦井安全對井下人員位置信息精度和連續(xù)性提出的更高要求,提出一種基于誤差修正方案的井下人員MEMS定位技術(shù)。依據(jù)加速度計輸出比力均值和方差判定人員步態(tài)識別并完成腳部靜止時間段的檢測,結(jié)合零速誤差修正工作原理,改進傳統(tǒng)卡爾曼濾波方案,完成人員靜止狀態(tài)下運動信息計算誤差和慣性器件偏差的濾波估計與補償。搭建MEMS慣導(dǎo)系統(tǒng)實驗平臺,設(shè)計加速度方差閾值獲取實驗以及人員水平與三維運動實驗。實驗結(jié)果表明,基于誤差修正技術(shù)的人員定位方法具有較高定位精度和可靠性。

MEMS;井下定位;零速誤差修正;慣性導(dǎo)航

井下人員位置信息的實時準確獲取對于掌握井下人員運動軌跡、確定事故發(fā)生時遇險礦工的準確位置,可為救援方案的制定提供依據(jù)[1-3]?；赯igbee技術(shù)和射頻識別技術(shù)的定位方法可對井下人員進行粗略范圍內(nèi)定位,Zigbee技術(shù)定位原理是依據(jù)接收到信號的強弱判定人員與信號發(fā)生器之間的相對距離,對于煤巷這種密閉的工作環(huán)境中,受電磁干擾和多徑效應(yīng)的影響,基于Zigbee技術(shù)的定位精度則大大降低;基于射頻識別技術(shù)能實現(xiàn)區(qū)域定位,但定位精度低,只能滿足人員管理的需求;另外,由于煤礦井下環(huán)境的特殊性,精確的衛(wèi)星定位系統(tǒng)無法應(yīng)用其中[4-7]。MEMS慣導(dǎo)系統(tǒng)以其具有低成本、低功耗、提供信息連續(xù)、不受外界信息干擾等優(yōu)勢成為井下人員定位技術(shù)的發(fā)展方向[8-10],國內(nèi)外已有學(xué)者和研究機構(gòu)在室內(nèi)定位過程中圍繞慣導(dǎo)系統(tǒng)的航位推算、組合定位方面開展預(yù)研工作,但是對于井下人員的特殊工作環(huán)境而言,巷道的不平整、沒有類似WIFI、UWB等可輔助慣導(dǎo)進行組合的外界參考信息等,因此其適用性和可靠性受到影響[11-13]。

通過對阜新市部分煤礦井下工作環(huán)境的實際調(diào)研,現(xiàn)有巷道轉(zhuǎn)角處設(shè)有無線裝置,用于對礦工人數(shù)的統(tǒng)計,但是人員在任意兩個相鄰無線裝置之間部分為定位盲區(qū),巷道短則數(shù)十米,長則上百米,假若出現(xiàn)事故,因無法實現(xiàn)人員的亞米級精確的有效定位而進一步增加了營救難度。本文以慣性定位技術(shù)為依托,通過結(jié)合人體運動特點,提出采用改進零速修正閉環(huán)卡爾曼濾波的純慣性定位方法,旨在實現(xiàn)任意兩個無線裝置之間的亞米級精確定位。

1 井下人員定位系統(tǒng)機械編排與常用坐標系定義

慣導(dǎo)系統(tǒng)的完整解算過程主要包括系統(tǒng)初始化和導(dǎo)航解算兩部分,依據(jù)人體行走特有的腳部運動特點,通過設(shè)計步態(tài)識別算法,將加速度計敏感的腳部加速度變化信息與設(shè)定閾值進行對比,完成腳部運動狀態(tài)的判定,通過設(shè)定卡爾曼濾波過程中的更新內(nèi)容,實現(xiàn)慣導(dǎo)系統(tǒng)解算信息的閉環(huán)修正(如圖1)。

圖1 MEMS定位系統(tǒng)算法流程圖

為滿足井下工作人員位置信息解算需要,將MEMS慣性測量單元IMU(Inertial Measurement Unit)固定在工作人員腳部(如圖2)。

圖2 導(dǎo)航坐標系與IMU坐標系的相對位置

定義IMU坐標系(s系)的坐標原點o位于IMU質(zhì)心,設(shè)定ox軸水平向下為正、oy軸水平向上為正、oz軸垂直于ox和oy軸構(gòu)成的平面且滿足右手螺旋定則,IMU坐標系隨著腳部運動而變化。將初始時刻的人體靜止狀態(tài)下的當?shù)厮阶鴺讼刀x為導(dǎo)航坐標系(oFUR),并定義oF軸沿人體正前向、oU軸沿天向、oR軸垂直于oF與oU構(gòu)成的平面且滿足右手螺旋定則。

2 MEMS慣性系統(tǒng)初始對準

慣導(dǎo)系統(tǒng)進入導(dǎo)航工作狀態(tài)之前進行的初始對準是為慣導(dǎo)解算提供初始姿態(tài)矩陣,而且初始對準精度直接制約著導(dǎo)航系統(tǒng)的定位精度[14-16]?；贛EMS器件的慣導(dǎo)系統(tǒng)由于器件本身精度低,陀螺儀無法有效敏感地球自轉(zhuǎn)角速度導(dǎo)致傳統(tǒng)的方位自對準方法存在適用性問題,為此將MEMS慣導(dǎo)系統(tǒng)的初始方位角置零。利用水平加速度計輸出中包含當?shù)刂亓Ψ至拷馑愠龀跏紩r刻的俯仰角和橫滾角,如式(1):

(1)

3 人員步態(tài)識別

人員行進過程中步態(tài)檢測的依據(jù)是人體運動學(xué),將人員的步態(tài)周期定義由站立、腳部提起、腿部擺動、腳部落地等幾個基本動作構(gòu)成,由于上述動作是以一定的時序發(fā)生,因此也被稱為人員行走中的基本步態(tài),其他復(fù)雜的腳部運動也通常包含上述基本過程。按照人員步行運動過程中腳的運動狀態(tài),將一個步行周期分為靜止和擺動兩種狀態(tài),其中靜止狀態(tài)是指腳完全與地面接觸并不存在任何加速度和角速度。據(jù)統(tǒng)計分析,靜止狀態(tài)占一個步行周期中的62%左右。在零速修正算法中,靜止狀態(tài)又稱為零速狀態(tài),而人員步態(tài)識別的判斷依據(jù)來自于MEMS慣導(dǎo)系統(tǒng)的加速度計測量輸出,選取某次實驗過程中加速度計的輸出數(shù)據(jù)進行分析(如圖3)。

從圖3可看出靜止時間段內(nèi)加速度計輸出比力幅值穩(wěn)定在重力加速度gn附近,運動時間段內(nèi)比力幅值變化明顯,因此可通過比力輸出的方差對人體步態(tài)進行檢測,具體檢測過程如下:

①計算比力幅值:依據(jù)MEMS慣導(dǎo)系統(tǒng)在每個離散時刻t1,t2,…tm,tm+1,…的輸出,計算當前任意時刻tm處的加速度計輸出比力幅值,即:

(2)

其中,fi(tm)(i=x,y,z)為tm時刻的加速度計輸出比力。

圖3 比力幅值

(3)

③運動狀態(tài)判定:根據(jù)加速度計輸出頻率,設(shè)定區(qū)間長度m1;根據(jù)加速度計輸出噪聲的方差特性,設(shè)定方差閾值GateV。當Varm

4 基于零速修正的卡爾曼濾波器設(shè)計

零速修正算法己成為解決慣導(dǎo)系統(tǒng)誤差的最有效方法之一。本文第三節(jié)已經(jīng)研究了零速修正算法中的人員步態(tài)識別,即利用加速度計測量值判斷人體是否處于零速狀態(tài);零速修正算法的第二步為更新過程,通過將檢測的零速狀態(tài)作為一個測量值傳給卡爾曼濾波器并實現(xiàn)對MEMS慣導(dǎo)系統(tǒng)的誤差修正?？柭鼮V波是基于系統(tǒng)模型和噪聲測量來估算人員定位系統(tǒng)的姿態(tài)、速度和位置誤差。一次完整的零速修正算法需要從系統(tǒng)狀態(tài)值中減去估算的狀態(tài)值,以此實現(xiàn)對誤差的限制,保證算法計算結(jié)果的準確性。

傳統(tǒng)慣導(dǎo)系統(tǒng)速度更新需要從加速度計輸出中去除有害加速度的影響。MEMS慣導(dǎo)系統(tǒng)的加速度計零偏遠遠大于有害加速度的影響,因此將MEMS慣導(dǎo)系統(tǒng)速度更新公式簡化為:

(4)

考慮到井下工作環(huán)境及MEMS導(dǎo)航系統(tǒng)特性,采用直角坐標系位置更新算法如下

(5)

式中:F(k+1)、U(k+1)、R(k+1)表示k+1時刻導(dǎo)航系3個方向的位移計算值。導(dǎo)航解算過程中一直進行零速修正,并且在天向速度更新中直接采用重力常值g0,避免了天向通道的發(fā)散現(xiàn)象。

速度誤差會引起位置誤差的積累,當檢測到人員處于零速狀態(tài),但估算速度不為零時將引起位置存在誤差。因此,當檢測到人員處于靜止狀態(tài)時,不僅要對速度誤差進行修正,同時也要對位置誤差進行修正。慣導(dǎo)系統(tǒng)的零速修正由靜止檢測觸發(fā),即通過在檢測為腳部靜止時間區(qū)間內(nèi)將速度計算結(jié)果重置為零,達到修正速度誤差的目的。為充分利用靜止檢測的檢測結(jié)果估計更多的誤差參數(shù),設(shè)計零速修正卡爾曼濾波器,結(jié)合零速修正工作原理,對傳統(tǒng)的卡爾曼濾波器進行如下修改:

在卡爾曼更新時刻tk,若靜態(tài)檢測結(jié)果為運動狀態(tài),則濾波器只進行時間更新;若靜態(tài)檢測結(jié)果為靜止狀態(tài),則卡爾曼濾波器進行完整更新(即時間更新+量測更新),并閉環(huán)修正井下人員定位系統(tǒng)的速度誤差、位置誤差、姿態(tài)誤差及慣性器件誤差。井下人員定位系統(tǒng)卡爾曼濾波流程如圖4所示。

其中,Xk、Pk分別表示k時刻狀態(tài)估計、估計誤差協(xié)方差矩陣。根據(jù)MEMS慣導(dǎo)系統(tǒng)速度、位置和姿態(tài)更新方程建立卡爾曼濾波狀態(tài)方程為:

(6)

其中,卡爾曼濾波狀態(tài)量:

X=[δVnδPnΦnsεs]T

式中:δVn=[δVFδVUδVR]T為導(dǎo)航系的速度誤差;δPn=[δFδUδR]T為位移誤差;Φn=[φFφUφR]T為失準角;s=[xyz]T為加速度計零偏;εs=[εxεyεz]T為陀螺常值漂移。

圖4 定位系統(tǒng)卡爾曼濾波流程

狀態(tài)轉(zhuǎn)移矩陣F的表達式為:

W為狀態(tài)噪聲向量:

定義卡爾曼濾波量測方程:

Z=HX+V

(7)

人員處于靜態(tài)條件下的系統(tǒng)觀測量Z即為MEMS導(dǎo)航系統(tǒng)的解算速度,即

量測矩陣H=[I3×303×12]。

為實現(xiàn)計算機環(huán)境下的數(shù)據(jù)處理,將卡爾曼狀態(tài)方程和量測方程進行離散化:

(8)

式中,Φk,k-1為一步轉(zhuǎn)移矩陣;Wk為系統(tǒng)激勵白噪聲序列;Vk為量測白噪聲序列。同時Wk、Vk滿足:

(9)

給出離散卡爾曼濾波方程:

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

狀態(tài)估計:

濾波增益:

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

估計均方誤差:

圖5 個人定位實驗平臺

5 實驗結(jié)果及分析

5.1 實驗條件

為驗證論文提出研究方法的可行性,基于實驗室現(xiàn)有設(shè)備搭建個人定位實驗平臺(如圖5)。其中,慣性測量單元采用XSENS公司的MTI-G(內(nèi)含陀螺儀和加速度計,參數(shù)指標如表1),采用實驗室自研6000系列DSP導(dǎo)航計算機實現(xiàn)慣性測量單元數(shù)據(jù)采集與導(dǎo)航解算,通過RS422串口完成導(dǎo)航計算機解算后位置數(shù)據(jù)向上位機的發(fā)送;在上位機開發(fā)基于C語言的數(shù)據(jù)接收端程序,實現(xiàn)步行者運動狀態(tài)信息的存儲與實時顯示。

表1 慣性器件參數(shù)

為實現(xiàn)人員定位MEMS慣導(dǎo)系統(tǒng)初始化需求,將實驗過程中的時間參數(shù)設(shè)定如表2所示。

表2 系統(tǒng)參數(shù)

表3 卡爾曼濾波器參數(shù)

5.2 方差閾值GateV選取實驗

比力方差閾值是實驗過程開始之前需要確定的另一個重要系統(tǒng)參數(shù),該參數(shù)的設(shè)定對于判定工作人員腳部運動狀態(tài)有著重要作用?？紤]到不同慣性器件對運動狀態(tài)感應(yīng)程度的不同,進行人員定位實驗之前需要將MEMS系統(tǒng)安裝完成后并開展一次預(yù)行走實驗,通過對預(yù)行走實驗數(shù)據(jù)分析完成比例方差閾值的選取,并將該參數(shù)設(shè)定在系統(tǒng)解算中。

根據(jù)實驗過程中采用的IMU采樣頻率為200Hz,選取計算區(qū)間長度m1=20。以某次試驗為例,給出該次試驗中人體行走過程的比力方差及其放大圖。

通過對圖6(b)中靜態(tài)檢測比力方差數(shù)據(jù)的分析,以去除方差變化過程的數(shù)據(jù)值為判斷節(jié)點,得到本次實驗過程的方差閾值選定值為GateV=0.0075。

圖6 靜態(tài)檢測比力方差

5.3 行走軌跡實驗

由于巷道走向存在水平和高度上的變化,因此井下人員在巷道行走過程存在水平和高度方向上位置的改變,為有效驗證本文提出的誤差修正技術(shù)對于提高MEMS慣導(dǎo)系統(tǒng)定位精度的可行性及其對不同空間環(huán)境的適應(yīng)性,擬在學(xué)校測繪樓開展二維平面行走實驗和借助樓梯完成存在高度變化的三維立體行走實驗。

平面路徑,即試驗人員所行走的路徑為測繪樓三樓的走廊區(qū)域,該區(qū)域形狀如圖7所示,具有典型的代表性,可直觀地檢測出井下人員導(dǎo)航系統(tǒng)所給出的定位結(jié)果與實際路徑的符合程度。

圖7 二維平面路徑示意圖

二維平面行走試驗包括:直角折線路徑和“6”字形路徑。

圖7為測繪樓走廊示意圖,總長為50m,寬度為13m;圖8和圖9展示了二維預(yù)訂行走路線及實際運行軌跡的對比曲線。其中,圖8所示的MEMS解算位移與預(yù)訂行走路線具有較高的重合度,北向位移與東向位移的最大偏差小于0.4m。為進一步驗證MEMS零速誤差修正技術(shù)在復(fù)雜行走環(huán)境下的可行性,開展了如圖9所示的“6”字形二維平面實驗,整個實驗過程人員行走114m,北向最大位移偏差0.9m,產(chǎn)生該誤差的原因在于慣性器件偏差的積累所致。

圖8 直角折線二維平面實驗

圖9 “6”字形二維平面實驗

立體路徑實驗,即實驗人員所行走的路徑為測繪樓西側(cè)的樓梯區(qū)域,測繪樓為五層,其樓梯為四段,該區(qū)域的形狀如圖10所示,同樣具有典型的代表性,不僅能直觀的檢測出MEMS導(dǎo)航系統(tǒng)所給出的定位結(jié)果在平面中的符合程度,更能檢測出其在高度方向上的符合程度。

圖10 三維立體路徑示意圖

三維立體試驗從測繪樓五樓的樓梯口出發(fā),完成樓梯行走,最終走到一樓樓梯口停止,最后時刻的前位移誤差為:0.35m。右位移誤差為:0.47m。通過高度計測量值得該樓層距離一層高度約為11.6m,而個人室內(nèi)導(dǎo)航結(jié)果為12.5m。高度誤差為0.9m。

圖8、圖9和圖11的試驗曲線直觀地顯示了試驗人員在試驗過程中的行走路徑,能夠與實際路線較好地匹配,說明井下人員導(dǎo)航技術(shù)無論對二維平面環(huán)境還是三維立體環(huán)境均能進行準確地描述,具有良好的空間構(gòu)筑性能,進一步驗證了人體運動狀態(tài)檢測方法及高精度定位技術(shù)在井下人員定位方法研究中的可行性。

圖11 三維行走試驗結(jié)果

然而,結(jié)合實驗數(shù)據(jù)分析可看出,井下人員導(dǎo)航系統(tǒng)較長時間工作過程中,存在橫向漂移與高度測量不精確問題,這是由于原理驗證試驗中采用的微慣性導(dǎo)航系統(tǒng)未進行系統(tǒng)誤差的標定與補償,陀螺與加速度計零位誤差與標度因數(shù)誤差等因素對導(dǎo)航精度的影響較大。同時在試驗人員行走過程中,當行走速度加快或者跑步前進的過程中,對人體腳部靜止狀態(tài)的檢測會有誤判或漏判的情況發(fā)生,導(dǎo)致橫向位移的測量誤差變大。這是由于原理驗證試驗中采用的微慣性導(dǎo)航系統(tǒng)體積過大,在人體腳部觸地靜止時,捆綁在腳部的系統(tǒng)仍存在晃動,這將導(dǎo)致系統(tǒng)未處于零速狀態(tài),容易導(dǎo)致系統(tǒng)對靜止狀態(tài)出現(xiàn)誤判或漏判。

6 結(jié)論

提出一種基于MEMS誤差修正技術(shù)的井下人員定位方法,分析了井下人員定位技術(shù)原理及實驗驗證的基本過程,論證了人體運動狀態(tài)檢測及零速誤差修正這兩項關(guān)鍵技術(shù)在人員高精度、實時定位研究中的可行性,并初步掌握了井下人員定位系統(tǒng)的應(yīng)用性能。然而,試驗結(jié)果也顯示出當前研究成果存在的不足之處,如:靜止狀態(tài)檢測的誤判或漏判、橫向位移測量存在位置偏移。針對上述問題擬進一步開展相應(yīng)研究將是后續(xù)工作的重點。

[1] 胡平,鞠捷.井下救援人員定位中的信號抗干擾技術(shù)研究[J].計算機仿真,2013,30(4):208-212.

[2]孔軼艷.礦難中井下救援通信系統(tǒng)設(shè)計與實現(xiàn)[J].煤炭技術(shù),2013,32(2):97-98.

[3]張玉,楊維,韓東升,等.井下應(yīng)急救援通信網(wǎng)絡(luò)拓撲規(guī)劃與資源配置[J].華中科技大學(xué)學(xué)報(自然科學(xué)版),2013,41(7):61-66.

[4]田豐,郭巍,王傳云,等.基于Zigbee技術(shù)的煤礦井下GIS輔助定位算法[J].煤炭學(xué)報,2008,33(12):1442-1446.

[5]張治斌,徐小玲,閻連龍.基于Zigbee井下無線傳感器網(wǎng)絡(luò)的定位方法[J].煤炭學(xué)報,2009,34(1):125-128.

[6]王經(jīng)緯,何涇沙.基于RFID技術(shù)的無源定位算法[J].計算機工程與設(shè)計,2013,34(10):3486-3491.

[7]陳灣灣,陳智剛,馬林,等.MEMS微機械陀螺溫度特性分析與建模[J].傳感技術(shù)學(xué)報,2014,27(2):194-197.

[8]宋海賓,楊平,徐立波.MEMS傳感器隨機誤差分析及處理[J].傳感技術(shù)學(xué)報,2013,26(12):1719-1723.

[9]Sang Kyeong Park,Young Soo Suh.A Zero Velocity Detection Algorithm Using Inertial Sensors for Pedestrian Navigation Systems[J].Sensors,2010,(10):9163-9178.

[10]周啟帆,張海,常艷紅,等.基于MIMU的行人導(dǎo)航算法研究[J].系統(tǒng)仿真學(xué)報,2010,6(4):281-286.

[11]張國鵬,王艷芬,丁恩杰.礦井無線多媒體傳感器網(wǎng)絡(luò)UWB信號收發(fā)策略研究[J].煤炭科學(xué)技術(shù),2013,41(12):71-75.

[12]劉勇,羅宇鋒,王紅旗,等.一種新型井下人員組合定位系統(tǒng)設(shè)計[J].工礦自動化,2014,40(2):11-15.

[13]Antonio Ramón Jiménez Ruiz,Fernando Seco Granja,José Carlos Prieto Honorato,et al.Accurate Pedestrian Indoor Navigation by Tightly Coupling Foot-Mounted IMU and RFID Measurements[J].IEEE Transactions on Instrumentation and Measurement,2012,61(1):178-189.

[14]紀志農(nóng),李海軍,李新純,等.捷聯(lián)慣導(dǎo)系統(tǒng)傳遞對準誤差模型降階分析[J].中國慣性技術(shù)學(xué)報,2009,17(4):403-407.

[15]Sun Wei,Xu Aigong,Gao Yang.Strapdown Gyrocompass Algorithm for AUV Attitude Determination Using a Digital Filter[J].Measure-ment,2013,46(1):815-822.

[16]許睿,孫永榮,陳武,等.一種基于抗差濾波的行人導(dǎo)航算法研究[J].系統(tǒng)工程與電子技術(shù),2010,32(7):1506-1509.

孫偉(1984-),男,副教授,博士生導(dǎo)師,黑龍江蘿北縣人。中國宇航學(xué)會會員,中國慣性技術(shù)協(xié)會會員;2007年于哈爾濱工程大學(xué)獲得學(xué)士學(xué)位,2011年于哈爾濱工程大學(xué)獲得博士學(xué)位。主持國家級、省部級科研項目7項;發(fā)表學(xué)術(shù)論文四十余篇,其中SCI論文6篇,EI論文20篇。長期從事慣性導(dǎo)航技術(shù)研究,sunwei-3775235@163.com。

MEMSPositionMethodforMbasedonErrorCorrection*

SUNWei*,LIWanqiu,CHUJing,LIRuibao

(School of Geomatics,Liaoning Technical University,Fuxin Liaoning 123000,China;)

According to the requirement of position accuracy and continuity for underground staff by mine safety,MEMS position method for mining personnel based on error correction was proposed in this paper.Based on the changes of acceleration of personnel walking characteristics changes,the gait recognition method was designed by mean and variance of the force for the purpose of detecting the time period in a stationary state.Combined with the principle of zero error update,the traditional Kalman filter was designed and the calculation errors for personal moving and inertial errors were estimated in stationary.The evidence for gait recognition could be calculated by the experiment of acceleration variance threshold.The horizontal and three-dimensional motion experiments were carried out by personnel MEMS inertial navigation system.Experiment results show that,the higher accuracy and reliability based on error correction technology could be reached by personnel positioning method.

MEMS;mining position;zero error update;inertial navigation

項目來源:國家自然科學(xué)基金項目(41304032);高等學(xué)校博士學(xué)科點專項科研基金項目(新教師類)(20132121120005);遼寧省科技廳博士啟動基金項目(20121084)

2014-02-24修改日期:2014-06-02

10.3969/j.issn.1004-1699.2014.07.009

TP212

:A

:1004-1699(2014)07-0898-07