黃輝先，王 暉，湯紅忠

礦井組合導(dǎo)航系統(tǒng)的設(shè)計與應(yīng)用

黃輝先，王 暉，湯紅忠

針對礦井捷聯(lián)式慣性導(dǎo)航系統(tǒng)（SINS）誤差累計的問題，提出了一種基于射頻位置修正技術(shù)的礦井組合導(dǎo)航系統(tǒng)。系統(tǒng)將射頻標(biāo)簽存儲的實際位置和SINS解算位置的差值作為量測量，利用Kalman濾波器估計并補償SINS存在的陀螺漂移和加速度計零偏。該方法在修正點對誤差估計精度高，收斂速度快，能夠?qū)INS累計誤差進(jìn)行一次性修正。

組合導(dǎo)航；卡爾曼濾波器；捷聯(lián)式慣性導(dǎo)航系統(tǒng)（SINS）；射頻識別

1 引言

我國煤礦井下地質(zhì)條件非常復(fù)雜，部分礦井巷道長達(dá)數(shù)十千米，作業(yè)地點分散，作業(yè)人員和設(shè)備流動性大。尤其是發(fā)生礦災(zāi)時，井下工作人員很難確定所處的具體位置[1]。鑒于此種現(xiàn)狀，研究適用于礦井下的導(dǎo)航技術(shù)將對礦產(chǎn)安全生產(chǎn)起到極大的促進(jìn)作用。目前井下使用較多的是射頻識別（Radio Frequency Identification，RFID）導(dǎo)航和慣性導(dǎo)航，但是兩者單獨使用時均存在一些固有缺陷。

RFID系統(tǒng)由射頻標(biāo)簽和閱讀器組成，是一種無線射頻方式進(jìn)行非接觸的雙向通信技術(shù)。根據(jù)導(dǎo)航機(jī)制的不同，可將無線導(dǎo)航算法分為兩類：基于距離無關(guān)的（range-free）導(dǎo)航算法和基于測距的（range-based）導(dǎo)航算法[2]。后者主要技術(shù)方案有AOA（Angle of Arrival）[3]、RSSI（Received Signal Strength Indicator）[4]和TOA（Time of Arrival）[5-6]。但這些方案的共同問題是定位精度比較低，尤其是工作時需全程接收外部信息，自主性差，這給實際應(yīng)用留下了巨大隱患[7]。

SINS利用慣性導(dǎo)航算法將載體三維加速度和角速率解算為載體的姿態(tài)、速度和位置信息。該系統(tǒng)具備自主、實時，不受地域、時間和氣候條件限制，可靠性高等諸多優(yōu)點，廣泛應(yīng)用于宇航、導(dǎo)彈制導(dǎo)以及特殊環(huán)境導(dǎo)航等領(lǐng)域。但是SINS存在的缺陷是定位誤差隨工作時間累計，無法長時間工作[8-9]。

針對上述兩種礦井導(dǎo)航系統(tǒng)所存在的缺陷，本文提出了一種基于RFID位置修正技術(shù)的礦井組合導(dǎo)航系統(tǒng)。該系統(tǒng)以SINS為基礎(chǔ)，當(dāng)載體通過修正點時，將RFID標(biāo)簽存儲的修正點實際位置與SINS的解算位置的差值作為系統(tǒng)誤差模型的量測量，利用Kalman濾波器對陀螺漂移和加速度計零偏進(jìn)行估計并補償，從而達(dá)到修正SINS累計誤差的目的。

2 捷聯(lián)式慣性導(dǎo)航系統(tǒng)

2.1 SINS解算算法

姿態(tài)更新[10]：

式中，rn、rb分別表示矢量在n系和b系中零標(biāo)量四元素，其中坐標(biāo)變換矩陣為n系到b系的姿態(tài)矩陣：

式中的q0、q1、q2、q3為坐標(biāo)變換的四元素參數(shù)，通過姿態(tài)矩陣可以推導(dǎo)出姿態(tài)角，從而求出載體相對于參考坐標(biāo)系的姿態(tài)角。

速度更新[10]：

式中，fb是固聯(lián)在載體上的加速度計測量值；分別為n系下e系到i系的角速率和n系到e系的角速率；gn為n系下的重力加速度；Vn即為n系下的載體速度。

位置更新[10]：

2.2 SINS缺陷及產(chǎn)生機(jī)理

對于工作在非極地地區(qū)的SINS，為了簡化計算，導(dǎo)航坐標(biāo)系一般選取地理坐標(biāo)系，這樣，捷聯(lián)式慣性導(dǎo)航完全等效于指北方位系統(tǒng)。在實際工作中，陀螺漂移引起的數(shù)學(xué)平臺漂移率與陀螺漂移的方向相反，刻度系數(shù)誤差引起對運載體角速度的測量誤差，經(jīng)姿態(tài)更新計算引入系統(tǒng)。隨著姿態(tài)更新次數(shù)的增加，誤差累積。經(jīng)過推算，誤差方程如下：

其中，δL、δλ、δh分別為被測物體的緯度、經(jīng)度和高度誤差；δVE、δVN、δVU為被測物體東向、北向、天向的速度誤差，δφE、δφN、δφU分別為東向、北向、天向的姿態(tài)誤差角；RM、RN分別為地球某點對應(yīng)的子午圈和卯酉圈半徑；?為加速度計零偏；ε為陀螺漂移；f為三軸加速度計測量的比力。

3 組合導(dǎo)航系統(tǒng)設(shè)計

圖1是礦井組合導(dǎo)航系統(tǒng)原理框架圖。慣性測量單元將載體的三軸加速度與三軸角速率信息送至SINS解算平臺進(jìn)行導(dǎo)航解算，得到導(dǎo)航參數(shù)。當(dāng)導(dǎo)航系統(tǒng)接近修正點時，RFID閱讀器讀取預(yù)存在射頻標(biāo)簽中修正點的實際位置信息，進(jìn)行Kalman數(shù)據(jù)融合處理，從而達(dá)到修正SINS累積誤差的目的。

圖1 礦井組合導(dǎo)航系統(tǒng)原理框架圖

3.1 系統(tǒng)硬件設(shè)計

3.1.1 系統(tǒng)框架

組合導(dǎo)航系統(tǒng)采用雙DSP架構(gòu)的低功耗設(shè)計方案，雙DSP架構(gòu)優(yōu)點在于將數(shù)據(jù)采集與導(dǎo)航解算分開進(jìn)行，提高了系統(tǒng)的實時性及有效精度。其中DSP1負(fù)責(zé)陀螺儀、加速度計、溫度及射頻數(shù)據(jù)的采集，并且該DSP還負(fù)責(zé)慣性傳感器的溫度補償及射頻數(shù)據(jù)的解碼工作。DSP2負(fù)責(zé)捷聯(lián)解算，數(shù)據(jù)融合算法的實施，人機(jī)接口交互及解算數(shù)據(jù)存儲等功能。人機(jī)交互主要通過3.5寸觸摸屏和USB2.0接口來實現(xiàn)。DSP之間的數(shù)據(jù)交互采用芯片自帶的XINT并行總線。

3.1.2 主要器件選型

（1）DSP1選用控制能力出眾的TMS320F28335，該處理器為32位浮點DSP，主頻可達(dá)150 MHz，適合數(shù)據(jù)采集與預(yù)處理工作。DSP2選用高性能、低功耗、32位浮點型的TMS320C6748，主頻300 MHz，峰值運算速度達(dá)2 400 MIPS/ 1 800 MFLOPS，適用于復(fù)雜的算法處理。

（2）慣性傳感器選用了星網(wǎng)宇達(dá)公司的XW-GS1830型陀螺儀和江門市利德電子公司的3038加速度計。其中陀螺儀測量范圍±200（°）/s，零偏穩(wěn)定性0.5（°）/h，具備低功耗、小體積、高精度等特點。加速度計測量范圍±100g，核心部分采用了第二代壓阻MEMS敏感元件，沖擊保護(hù)達(dá)10 000g。

（3）射頻標(biāo)簽采用ISO/IEC 14443近藕合IC卡，最大的讀取距離為l5 cm。

3.2 數(shù)據(jù)融合算法推導(dǎo)

由系統(tǒng)原理知，組合導(dǎo)航算法實現(xiàn)的關(guān)鍵是快速準(zhǔn)確地將RFID存儲的數(shù)據(jù)與SINS解算的數(shù)據(jù)進(jìn)行融合。算法建立以位置為融合量的系統(tǒng)數(shù)學(xué)模型，在此基礎(chǔ)上設(shè)計Kalman濾波器估計并補償誤差。其中組合導(dǎo)航算法中的SINS解算部分依據(jù)第2章進(jìn)行。

3.2.1 建立系統(tǒng)誤差模型狀態(tài)方程

該組合導(dǎo)航系統(tǒng)基于兩種導(dǎo)航系統(tǒng)的位置信息進(jìn)行Kalman濾波，選擇經(jīng)度、緯度以及高度作為量測量。

取狀態(tài)變量：

依據(jù)式（4）～（12）列寫捷聯(lián)式慣性導(dǎo)航系統(tǒng)誤差模型狀態(tài)方程，表達(dá)式如下：

其中，XSINS是系統(tǒng)的狀態(tài)向量；ASINS為系統(tǒng)的狀態(tài)矩陣；GSINS為系統(tǒng)噪聲系數(shù)矩陣；wSINS為系統(tǒng)的噪聲向量。狀態(tài)矩陣ASINS為15×15的矩陣，系統(tǒng)噪聲矩陣為：

噪聲系數(shù)矩陣為：

系統(tǒng)噪聲的方差強度陣按下式求?。?/p>

其中q即為系統(tǒng)噪聲的方差強度陣。

對狀態(tài)方程進(jìn)行離散化處理，處理后的狀態(tài)方程為：

其中一步轉(zhuǎn)移矩陣為：

3.2.2 建立系統(tǒng)量測方程

由前述組合系統(tǒng)原理知，在RFID修正點，系統(tǒng)選取RFID提供的實際位置信息與SINS導(dǎo)航解算的位置信息的差值作為量測量。量測方程如下：

其中，LRFID、λRFID、hRFID分別為RFID存儲的修正點緯度、經(jīng)度和高度；LSINS、λSINS、hSINS分別為SINS解算的緯度、經(jīng)度及高度。

由式（16）可推出系統(tǒng)量測方程的矢量形式：

式中，H是卡爾曼濾波器的觀測矩陣：

I為單位矩陣，v是觀測噪聲矢量，濾波過程中近似看做白噪聲序列，并假設(shè)其方差陣為Rk。

根據(jù)誤差的數(shù)學(xué)模型，建立Kalman濾波方程：

至此，完成了組合導(dǎo)航算法的數(shù)據(jù)融合部分的算法推導(dǎo)。

4 組合導(dǎo)航系統(tǒng)驗證

4.1 系統(tǒng)原理仿真

為了分析和驗證，首先對系統(tǒng)進(jìn)行仿真研究。仿真工具選用MATLAB，陀螺漂移與加速度計零偏均包含兩部分，即隨機(jī)常值部分和具有指數(shù)型相關(guān)部分。短時間內(nèi)，僅考慮隨機(jī)常值部分。

在仿真過程中，通過預(yù)設(shè)軌跡得到慣性測量單元的基本數(shù)據(jù)，其中在預(yù)設(shè)軌跡中選取兩處RFID修正點，修正點間隔距離為1 200 m。軌跡初始經(jīng)、緯度為115.0°、38.9°，高度為-500 m。載體速度為2 m/s，運行高度不變。姿態(tài)誤差初始值δφE(0)=δφN(0)=0.02',δφU(0)=10',速度誤差初始值δVE=δVN=δVU=0,位置誤差初始值δL=δλ=δh=0，陀螺漂移為εx=εy=εz=0.001()°/s,加速度計零偏?x=?y= ?z=10-4g，算法更新時間T=0.1 s。

圖3為載體經(jīng)過修正點時，在UKF和Kalman濾波條件下的姿態(tài)誤差變化曲線。UKF為Kalman濾波的改進(jìn)型，適用于SINS這種非線性系統(tǒng)，但是通過對比分析發(fā)現(xiàn)兩種濾波方法效果基本一致。出現(xiàn)這種結(jié)果的原因是一定精度的導(dǎo)航系統(tǒng)的誤差量都可看做小量，非線性方程中關(guān)于誤差量的高階項都可看做高階小量而略去不計，誤差方程可描述為線性方程，所以兩者效果基本一致。而對于工程應(yīng)用來說，UKF需要對方程進(jìn)行UT變換且算法相對復(fù)雜，因此系統(tǒng)選擇Kalman濾波作為數(shù)據(jù)融合算法基礎(chǔ)。另外由圖知載體經(jīng)過修正點時，姿態(tài)誤差逐漸收斂至零值附近，即系統(tǒng)在修正點準(zhǔn)確地估計出了陀螺的常值漂移和加速度計的常值零偏，并且得到補償。為了驗證姿態(tài)誤差收斂速度與位置信息時間間隔的關(guān)系，圖3（a）中修正點位置信息時間間隔為1 s，圖3（b）間隔為0.5 s。對比兩圖可得，提供修正位置信息間隔時間越短，姿態(tài)誤差收斂得越快。但是，在實際使用時，時間間隔越短意味著射頻標(biāo)簽使用越多，同時，算法計算量越大。因此，在實際使用時應(yīng)按照具體情況選取時間間隔。

圖3 修正過程中姿態(tài)誤差曲線圖

圖4 SINS算法位置誤差曲線圖

載體按照預(yù)設(shè)軌跡以2 m/s行進(jìn)，初始條件及參數(shù)均不變，對純SINS進(jìn)行仿真，仿真時間為1 800 s，觀測仿真過程中位置誤差的變化曲線。為了便于分析，位置誤差換算為地理距離誤差δP,仿真結(jié)果如圖4所示。分析驗證了2.5節(jié)結(jié)論，即SINS由于自身陀螺的常值漂移和加速度計的常值零偏，導(dǎo)致誤差隨時間逐步增大，如不進(jìn)行修正，不適合礦井環(huán)境下的長時間導(dǎo)航。圖中，1 800 s的仿真實驗的位置誤差最大偏差達(dá)到6 m。

在上述SINS仿真條件下，對組合導(dǎo)航系統(tǒng)進(jìn)行仿真研究。與前者相區(qū)別的是載體行進(jìn)過程中，在600 s和1 200 s兩處分別以0.5 s為間隔提供4次真實位置信息，觀測仿真過程中位置誤差，結(jié)果如圖5所示。對圖5進(jìn)行分析，載體位置誤差在600 s和1 200 s兩處得到一次性的修正。結(jié)合圖3的信息，系統(tǒng)在修正點準(zhǔn)確估計出陀螺與加速度計的誤差，并且修正了由上述誤差引起姿態(tài)、速度及位置誤差。在0～600 s、600～1 200 s以及1 200 s～1 800 s只有SINS在工作，誤差依舊在累計，但是從整體來評估系統(tǒng)，其精度明顯提高，整個實驗過程中最大偏差為1.94 m。因此，該組合導(dǎo)航系統(tǒng)在長時間的礦井導(dǎo)航中有很好的適用性。

4.2 實物數(shù)據(jù)分析

為了對實際系統(tǒng)進(jìn)行驗證，系統(tǒng)自帶了數(shù)據(jù)采集與回放裝置，將修正點前后的導(dǎo)航參數(shù)變化量存儲至FLASH中，實驗結(jié)束后系統(tǒng)通過USB2.0接口將存儲數(shù)據(jù)發(fā)送至PC機(jī)，最后MATLAB對比修正點前后導(dǎo)航參數(shù)修正量（導(dǎo)航系統(tǒng)在修正前后輸出值與真實值之差），經(jīng)過10次擬合與統(tǒng)計分析，得出表1。

表1 修正點導(dǎo)航參數(shù)修正量

通過對表1數(shù)據(jù)分析，系統(tǒng)經(jīng)過修正點時導(dǎo)航參數(shù)得到了修正，并且修正精度達(dá)到了預(yù)期效果。

5 結(jié)論

由于SINS誤差存在累計，無法長時間使用，陸地上可以依據(jù)GPS信息對其修正，但是在礦井下該方法無法使用。鑒于此，提出了一種基于RFID位置修正的礦井組合導(dǎo)航系統(tǒng)。本文系統(tǒng)將兩種導(dǎo)航系統(tǒng)的位置差值作為系

圖5 RFID位置修正條件下的位置誤差曲線圖

統(tǒng)量測量，利用Kalman濾波技術(shù)估計出誤差，從而解決了SINS誤差累積的問題。實驗結(jié)果表明該系統(tǒng)相比于純GPS導(dǎo)航（誤差為60 m）、基于CELLID的蜂窩無線網(wǎng)絡(luò)（誤差200～300 m）而言，可靠性和導(dǎo)航精度得到大幅度提高。下一步，將在礦井環(huán)境下進(jìn)行實驗，并且依據(jù)實驗結(jié)果來解決修正點、修正間隔時間和修正次數(shù)的選取問題。

[1]孫澤宇，李蒙.改進(jìn)無線傳感器網(wǎng)絡(luò)礦井監(jiān)測系統(tǒng)定位算法[J].計算機(jī)測量與控制，2010，18（9）：2008-2013.

[2]Guo Zhongwen，Guo Ying，Hong Feng，et al.Perpendicular intersection：locating wireless sensors with mobile beacon[C]// Proceedings of the 2008 Real-Time Systems Symposium.[S.l.]：IEEE，2008：93-102.

[3]Dakkak M，Nakib A，Daachi B，et al.Indoor localization method based on RTT and AOA using coordinatesclustering[J]. Computer Networks，2011，55（8）：1794-1803.

[4]周艷，李海成.基于RSSI無線傳感器網(wǎng)絡(luò)空間定位算法[J].通信學(xué)報，2009，30（6）：75-79.

[5]Tang H，Park Y W，Qiu T.NLOS mitigation for TOA location based on amodified deterministicmodel[J].Research Letters in Signal Processing，2008，8（1）：1-4.

[6]賀遠(yuǎn)華，黎洪生.距離幾何TOA無線定位算法[J].計算機(jī)工程與應(yīng)用，2010，46（12）：112-114.

[7]崔遜學(xué)，趙湛，王成，等.無線傳感器網(wǎng)絡(luò)的領(lǐng)域應(yīng)用與設(shè)計技術(shù)[M].北京：國防工業(yè)出版社，2009.

[8]呂振，劉丹，李春光.基于捷聯(lián)慣性導(dǎo)航的井下人員精確定位系統(tǒng)[J].煤炭學(xué)報，2009，32（8）：1149-1152.

[9]秦永元.慣性導(dǎo)航[M].北京：科學(xué)出版社.2009.

[10]嚴(yán)恭敏.車載自主定位定向系統(tǒng)[D].西安：西北工業(yè)大學(xué)，2006.

HUANG Huixian,WANG Hui,TANG Hongzhong

湘潭大學(xué) 信息工程學(xué)院，湖南 湘潭 411105

Information Engineering College,Xiangtan University,Xiangtan,Hunan 411105,China

For the error accumulated problems of mine Strapdown Inertial Navigation System（SINS）,a mine integrated navigation system based on Radio Frequency Identification（RFID）position correction technology is presented.The difference of the SINS solver location and the actual location stored in RFID tags is selected as the system observation,at the same time,Kalman filter is used to estimate and compensate the gyro drifts and accelerometer biases existed in SINS.Through new methods,cumulative error correction of SINS can be corrected at one-time,moreover,the estimation precision is high and the estimators are rapidly convergence by this method at the correction points.

integrated navigation;Kalman filter;Strapdown Inertial Navigation System（SINS）;Radio Frequency Identification（RFID）

A

TP212

10.3778/j.issn.1002-8331.1108-0343

HUANG Huixian,WANG Hui,TANG Hongzhong.Design and application of integrated navigation system in the mine. Computer Engineering and Applications,2013,49（7）：248-251.

湖南省自然科學(xué)基金委員會與湘潭市政府自然科學(xué)聯(lián)合基金（No.10JJ9008）；湖南省教育廳基金（No.10C1266）；湖南省科技計劃項目（No.2011FJ3183）。

黃輝先（1955—），男，博士，教授，研究領(lǐng)域：先進(jìn)控制理論及其在工業(yè)自動化控制中的應(yīng)用等；王暉（1986—），男，碩士研究生，研究領(lǐng)域：先進(jìn)控制與應(yīng)用；湯紅忠（1979—），女，講師，研究領(lǐng)域：圖像處理與導(dǎo)航研究等。E-mail：happyxtu@163.com

2011-08-31

2011-11-29

1002-8331（2013）07-0248-04