矿井组合导航系统的设计与应用

黄辉先，王 晖，汤红忠

矿井组合导航系统的设计与应用

黄辉先，王 晖，汤红忠

针对矿井捷联式惯性导航系统（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