礦井組合導航系統的設計與應用

黃輝先，王 暉，湯紅忠

礦井組合導航系統的設計與應用

黃輝先，王 暉，湯紅忠

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

組合導航；卡爾曼濾波器；捷聯式慣性導航系統（SINS）；射頻識別

1 引言

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

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

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

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

2 捷聯式慣性導航系統

2.1 SINS解算算法

姿態更新[10]：

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

式中的q0、q1、q2、q3為坐標變換的四元素參數，通過姿態矩陣可以推導出姿態角，從而求出載體相對于參考坐標系的姿態角。

速度更新[10]：

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

位置更新[10]：

2.2 SINS缺陷及產生機理

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

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

3 組合導航系統設計

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

圖1 礦井組合導航系統原理框架圖

3.1 系統硬件設計

3.1.1 系統框架

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

3.1.2 主要器件選型

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

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

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

3.2 數據融合算法推導

由系統原理知，組合導航算法實現的關鍵是快速準確地將RFID存儲的數據與SINS解算的數據進行融合。算法建立以位置為融合量的系統數學模型，在此基礎上設計Kalman濾波器估計并補償誤差。其中組合導航算法中的SINS解算部分依據第2章進行。

3.2.1 建立系統誤差模型狀態方程

該組合導航系統基于兩種導航系統的位置信息進行Kalman濾波，選擇經度、緯度以及高度作為量測量。

取狀態變量：

依據式（4）～（12）列寫捷聯式慣性導航系統誤差模型狀態方程，表達式如下：

其中，XSINS是系統的狀態向量；ASINS為系統的狀態矩陣；GSINS為系統噪聲系數矩陣；wSINS為系統的噪聲向量。狀態矩陣ASINS為15×15的矩陣，系統噪聲矩陣為：

噪聲系數矩陣為：

系統噪聲的方差強度陣按下式求取：

其中q即為系統噪聲的方差強度陣。

對狀態方程進行離散化處理，處理后的狀態方程為：

其中一步轉移矩陣為：

3.2.2 建立系統量測方程

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

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

由式（16）可推出系統量測方程的矢量形式：

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

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

根據誤差的數學模型，建立Kalman濾波方程：

至此，完成了組合導航算法的數據融合部分的算法推導。

4 組合導航系統驗證

4.1 系統原理仿真

為了分析和驗證，首先對系統進行仿真研究。仿真工具選用MATLAB，陀螺漂移與加速度計零偏均包含兩部分，即隨機常值部分和具有指數型相關部分。短時間內，僅考慮隨機常值部分。

在仿真過程中，通過預設軌跡得到慣性測量單元的基本數據，其中在預設軌跡中選取兩處RFID修正點，修正點間隔距離為1 200 m。軌跡初始經、緯度為115.0°、38.9°，高度為-500 m。載體速度為2 m/s，運行高度不變。姿態誤差初始值δφ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為載體經過修正點時，在UKF和Kalman濾波條件下的姿態誤差變化曲線。UKF為Kalman濾波的改進型，適用于SINS這種非線性系統，但是通過對比分析發現兩種濾波方法效果基本一致。出現這種結果的原因是一定精度的導航系統的誤差量都可看做小量，非線性方程中關于誤差量的高階項都可看做高階小量而略去不計，誤差方程可描述為線性方程，所以兩者效果基本一致。而對于工程應用來說，UKF需要對方程進行UT變換且算法相對復雜，因此系統選擇Kalman濾波作為數據融合算法基礎。另外由圖知載體經過修正點時，姿態誤差逐漸收斂至零值附近，即系統在修正點準確地估計出了陀螺的常值漂移和加速度計的常值零偏，并且得到補償。為了驗證姿態誤差收斂速度與位置信息時間間隔的關系，圖3（a）中修正點位置信息時間間隔為1 s，圖3（b）間隔為0.5 s。對比兩圖可得，提供修正位置信息間隔時間越短，姿態誤差收斂得越快。但是，在實際使用時，時間間隔越短意味著射頻標簽使用越多，同時，算法計算量越大。因此，在實際使用時應按照具體情況選取時間間隔。

圖3 修正過程中姿態誤差曲線圖

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

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

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

4.2 實物數據分析

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

表1 修正點導航參數修正量

通過對表1數據分析，系統經過修正點時導航參數得到了修正，并且修正精度達到了預期效果。

5 結論

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

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

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

[1]孫澤宇，李蒙.改進無線傳感器網絡礦井監測系統定位算法[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無線傳感器網絡空間定位算法[J].通信學報，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]賀遠華，黎洪生.距離幾何TOA無線定位算法[J].計算機工程與應用，2010，46（12）：112-114.

[7]崔遜學，趙湛，王成，等.無線傳感器網絡的領域應用與設計技術[M].北京：國防工業出版社，2009.

[8]呂振，劉丹，李春光.基于捷聯慣性導航的井下人員精確定位系統[J].煤炭學報，2009，32（8）：1149-1152.

[9]秦永元.慣性導航[M].北京：科學出版社.2009.

[10]嚴恭敏.車載自主定位定向系統[D].西安：西北工業大學，2006.

HUANG Huixian,WANG Hui,TANG Hongzhong

湘潭大學 信息工程學院，湖南 湘潭 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.

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

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

2011-08-31

2011-11-29

1002-8331（2013）07-0248-04