CN104007460B - 一种消防员单兵定位导航装置 - Google Patents

Abstract

本发明提供了一种消防员单兵定位导航装置，包括卫星导航信息接收模块、电子罗盘模块、气压计模块、电子地图模块、存储模块、MEMS惯导模块、图像信息码读取模块和中央处理器模块；卫星导航信息得出第一定位信息；电子罗盘模块获得第二定位信息；气压计模块获得第三定位信息；电子地图模块显示地图，并将中央处理器模块传来的综合定位信息在地图上显示；MEMS惯导模块得到第四定位信息；图像信息码读取模块读取预先设置的图像信息码，得到校正信息；中央处理器模块对接收到的校正信息、第一定位信息、第二定位信息、第三定位信息和第四定位信息进行数据融合得到综合定位信息。本发明广泛应用于消防救灾领域。

Description

一种消防员单兵定位导航装置

技术领域

本发明涉及一种室内定位导航装置，特别是消防员进入室内火场所使用的定位导航装置。

背景技术

火灾对人的生命和财产安全有着巨大的威胁，特别是在城市中，高层建筑失火后救援工作非常困难。当消防员进入火灾现场时，由于烟雾干扰、建筑物结构改变等因素，使得消防员难以获知自身位置，极大地影响了救援。

人们尝试利用卫星定位系统(例如GPS等技术)进行消防员火场定位导航。众所周知，室内由于墙体的阻碍，不容易接收到卫星信号，因此，在室内利用卫星定位技术使得消防员获知自身位置很难稳定、可靠地实现。

为此，人们开发了多种能够在室内进行自主导航定位的装置供消防员进入火场时使用。例如，申请号为201210580902.8的中国专利申请(名称为：一种消防员室内外3D无缝定位及姿态检测系统及方法)，提出利用佩戴于使用者腰部及大腿外侧的两个惯性导航模块、电子罗盘和气压计推算人员航迹，完成室内定位。该技术方案存在以下不足：

1、惯性导航模块对加速度进行积分，其输出误差累积到一定程度，其定位精度会随时间的增加而下降，最终导致定位装置不可用。

2、电子罗盘通过测量地磁信息获得航向、俯仰和横滚等姿态信息。在建筑物中容易受到周围电磁环境和金属的影响，在环境复杂的火灾现场，电子罗盘的输出往往会受到干扰而导致定位装置失效。

3、气压计通过测量周围气压的变化获得高度信息。火灾现场温度变化大，往往存在高温，而温度的变化导致气压变化，从而使得气压计输出的数据不正确。

从以上分析可以看到，现有的消防员室内定位装置存在可靠性低的问题。

发明内容

为了解决现有消防员室内定位装置存在的可靠性低的问题，本发明提供了一种消防员单兵定位导航装置，能够方便消防员进行室内外导航、定位。

本发明的技术方案如下：

一种消防员单兵定位导航装置，包括卫星导航信息接收模块、电子罗盘模块、气压计模块、电子地图模块、存储模块、MEMS惯导模块、图像信息码读取模块和中央处理器模块；所述卫星导航信息接收模块、所述电子罗盘模块、所述气压计模块、所述电子地图模块、所述存储模块、所述MEMS惯导模块和所述图像信息码读取模块分别连接于所述中央处理器模块；

所述卫星导航信息接收模块接收卫星定位信号并将得出的第一定位信息传输给所述中央处理器模块，并供所述MEMS惯导模块进行初始对准；

所述电子罗盘模块获得携带所述定位导航装置的携带者的移动方向和移动姿态信息形成第二定位信息，将所述第二定位信息传输给所述中央处理器模块，并供所述MEMS惯导模块进行所述初始对准；

所述气压计模块获得所述携带者的高度信息形成第三定位信息，传输给所述中央处理器模块，并供所述MEMS惯导模块进行所述初始对准；

所述电子地图模块显示地图，并将所述中央处理器模块传来的综合定位信息在所述地图上显示；

所述存储模块存储所述地图的信息；

MEMS惯导模块测量所述携带者的加速度和角速度，得到所述携带者的速度信息、位置信息和姿态信息形成第四定位信息，并将所述第四定位信息传输给所述中央处理器模块；

所述图像信息码读取模块读取预先设置的图像信息码，得到校正信息并传输给所述中央处理器模块；所述图像信息码对应其所在位置的位置信息；

所述中央处理器模块对接收到的所述校正信息、所述第一定位信息、所述第二定位信息、所述第三定位信息和所述第四定位信息进行数据融合得到所述综合定位信息。

所述图像信息码读取模块得到所述校正信息的方法采用卡尔曼滤波方法，状态方程为：

$\left[\begin{array}{c}{\stackrel{\·}{X}}_I\\ {\stackrel{\·}{X}}_P\end{array}\right]=\left[\begin{array}{cc}{F}_I& 0\\ 0& 0_{3\×3}\end{array}\right]\left[\begin{array}{c}{X}_I\\ X_P\end{array}\right]+\left[\begin{array}{cc}{G}_I& 0\\ 0& I_{3\×3}\end{array}\right]\left[\begin{array}{c}{W}_I\\ W_P\end{array}\right]$

其中，为所述惯性单元的状态量，δL_I、δλ_I和δh分别为纬度误差、经度误差和高度误差，δV_E、δV_N和δV_U分别为东向、北向和天向速度误差，φ_E、φ_N、φ_U为三个方向的失准角，和为所述加速度计的等效零偏，ε_x、ε_y、ε_z为所述陀螺仪的等效零偏，W_I＝[w_ax w_ay w_az w_gx w_gy w_gz]为所述加速度计和所述陀螺仪的噪声；

$G_I=\left[\begin{array}{c}{0}_{3\×6}\\ \begin{array}{cccccc}{c}_{11}& c_{21}& c_{31}& 0& 0& 0\\ c_{12}& c_{22}& c_{32}& 0& 0& 0\\ c_{13}& c_{23}& c_{33}& 0& 0& 0\\ 0& 0& 0& c_{11}& c_{21}& c_{31}\\ 0& 0& 0& c_{12}& c_{22}& c_{32}\\ 0& 0& 0& c_{13}& c_{23}& c_{33}\end{array}\\ 0_{6\×6}\end{array}\right]$

ω_ie为地球自转角速度，L为当地纬度，R为地球半径；X_p＝[δL_P δλ _Pδh_P]为所述图像信息码对应的经度误差、纬度误差和高度误差；W_p＝[w_L w_λ w_h]^T为系统噪声，均为零均值白噪声。

观测方程为：

$Z_{IP}=\left[\begin{array}{c}{\mathrm{\δL}}_I-{\mathrm{\δL}}_P\\ {\mathrm{\δ\λ}}_I-{\mathrm{\δ\λ}}_P\\ \mathrm{\δ}h-{\mathrm{\δh}}_P\end{array}\right]+\left[\begin{array}{c}{v}_L^{IP}\\ v_{\mathrm{\λ}}^{IP}\\ v_h^{IP}\end{array}\right]=H_{IP}\left[\begin{array}{c}{X}_I\\ X_P\end{array}\right]+\left[\begin{array}{c}{v}_L^{IP}\\ v_{\mathrm{\λ}}^{IP}\\ v_h^{IP}\end{array}\right]$

其中，H_IP＝[I_3×3 0_3×12 -I_3×3]为观测方程系数矩阵，和为观测量中的零均值高斯白噪声。

所述MEMS惯导模块进行所述初始对准采用卡尔曼滤波方法，状态方程为：

$\left[\begin{array}{c}{\stackrel{\·}{X}}_I\\ {\stackrel{\·}{X}}_G\\ {\stackrel{\·}{h}}_B\\ {\stackrel{\·}{\mathrm{\φ}}}_C\end{array}\right]=\left[\begin{array}{cccc}{F}_I& 0& 0& 0\\ 0& F_G& 0& 0\\ 0& 0& 0& 0\\ 0& 0& 0& 0\end{array}\right]\left[\begin{array}{c}{X}_I\\ X_G\\ h_B\\ {\mathrm{\φ}}_C\end{array}\right]+\left[\begin{array}{cccc}{G}_I& 0& 0& 0\\ 0& I_{2\×2}& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{c}{W}_I\\ W_G\\ w_B\\ w_C\end{array}\right]$

其中，为所述MEMS惯导模块的状态量，δL_I、δλ_I分别为纬度误差和经度误差，δh为高度误差，δV_E、δV_N和δV_U分别为东向、北向和天向速度误差，φ_E、φ_N、φ_U为三个方向的失准角，和为所述MEMS惯导模块中加速度计的等效零偏，ε_x、ε_y、ε_z为所述MEMS惯导模块中陀螺仪的等效零偏，

$G_I=\left[\begin{array}{c}{0}_{3\×6}\\ \begin{array}{cccccc}{c}_{11}& c_{21}& c_{31}& 0& 0& 0\\ c_{12}& c_{22}& c_{32}& 0& 0& 0\\ c_{13}& c_{23}& c_{33}& 0& 0& 0\\ 0& 0& 0& c_{11}& c_{21}& c_{31}\\ 0& 0& 0& c_{12}& c_{22}& c_{32}\\ 0& 0& 0& c_{13}& c_{23}& c_{33}\end{array}\\ 0_{6\×6}\end{array}\right]$

ω_ie为地球自转角速度，L为当地纬度，R为地球半径；X_G＝[δL_G δλ_G]为卫星导航单元输出的位置误差；

τ_LG和τ_λG为相关时间常数，h_B为所述气压计模块输出的高度，φ_C为所述电子罗盘模块输出的方位角，W_I＝[w_ax w_ay w_az w_gx w_gy w_gz]为所述MEMS惯导模块中加速度计和陀螺仪的噪声，w_B为所述气压计模块的误差，w_C为所述电子罗盘模块的误差；观测方程为：

$Z_{IC}=\left[\begin{array}{c}{\mathrm{\δL}}_I-{\mathrm{\δL}}_G\\ {\mathrm{\δ\λ}}_I-{\mathrm{\δ\λ}}_G\\ \mathrm{\δ}h-{\mathrm{\δh}}_B\\ {\mathrm{\δ\φ}}_U-{\mathrm{\δ\φ}}_C\end{array}\right]+\left[\begin{array}{c}{v}_L^{IC}\\ v_{\mathrm{\λ}}^{IC}\\ v_h^{IC}\\ v_{\mathrm{\φ}}^{IC}\end{array}\right]=H_{IC}\left[\begin{array}{c}{X}_I\\ X_G\\ h_B\\ {\mathrm{\φ}}_C\end{array}\right]+\left[\begin{array}{c}{v}_L^{IC}\\ v_{\mathrm{\λ}}^{IC}\\ v_h^{IC}\\ v_{\mathrm{\φ}}^{IC}\end{array}\right]$

其中为观测方程系数矩阵，和为观测量中的零均值高斯白噪声。

所述消防员单兵定位导航装置还包括与所述中央处理器模块连接的RFID模块，所述RFID模块包括RFID阅读器和RFID应答器；所述RFID阅读器与设置在其他消防员单兵定位导航装置上的RFID应答器进行通讯，获得该消防员单兵定位导航装置的定位信息，并在所述电子地图模块显示所述定位信息；所述RFID应答器用于与其他消防员单兵定位导航装置上的RFID阅读器进行所述通讯。

所述中央处理器模块在所述MEMS惯导模块完成所述初始对准后禁用所述气压计模块和所述电子罗盘模块。

所述消防员单兵定位导航装置还包括与所述中央处理器模块连接的无线通讯模块；所述无线通讯模块用于与外部的服务器进行数据通讯。

所述存储模块存储所述消防员单兵定位导航装置的移动路径信息。

所述消防员单兵定位导航装置记录所述MEMS惯导模块独立工作的时间，当所述时间达到预定值后，发出进行校正的提示信息。

本发明的技术效果：

本发明的消防员单兵定位导航装置，利用卫星导航信息接收模块、电子罗盘模块和气压计模块获取初始的位置信息，当消防员进入火场后利用MEMS惯导模块进行惯性导航。由于MEMS惯导模块在工作一段时间后输出误差累积，需要进行校正。图像信息码读取模块读取预先设置在火场建筑物内的图像信息码(所述图像信息码包含所在位置信息)，获得定位校正信息。中央处理器模块对校正信息和MEMS惯导模块输出的定位信息进行数据融合，获得高精度的定位信息，使得本发明的消防员单兵定位导航装置在火场内能够持续可靠地工作，实现了本发明的目的。

附图说明

图1为本发明消防员单兵定位导航装置第一个实施例的结构原理图。

图2为本发明消防员单兵定位导航装置第二个实施例的结构原理图。

图3为图2所示消防员单兵定位导航装置的工作流程图。

图4为MEMS惯导模块初始对准的流程图。

具体实施方式

在对本发明的技术方案进行详细描述前，对其中的措辞进行统一说明。

携带者：特指携带本发明消防员单兵定位导航装置的人，为了描述简便，统一用携带者进行指代。

以下结合附图对本发明的技术方案进行详细说明。

图1显示了本发明消防员单兵定位导航装置第一个实例的原理。消防员单兵定位导航装置包括卫星导航信息接收模块、电子罗盘模块、气压计模块、电子地图模块、存储模块、MEMS惯导模块、图像信息码读取模块和中央处理器模块。卫星导航信息接收模块、电子罗盘模块、气压计模块、电子地图模块、存储模块、MEMS惯导模块和图像信息码读取模块分别连接于中央处理器模块。

其中，卫星导航信息接收模块包括接收天线，用于接收导航卫星(例如GPS、Galileo和北斗卫星系统)的导航信号，输出携带者的速度、位置、方向等信息。这些信息形成第一定位信息，被传输给中央处理器模块。第一定位信息也被传输给MEMS惯导模块，供MEMS惯导模块进行初始对准。在本实施例中，卫星导航信息接收模块中采用了瑞士u-blox公司生产的UBX-G6010-ST卫星导航接收机芯片。

电子罗盘模块通过测量磁通量，获得携带者的移动方向和移动姿态信息形成第二定位信息。第二定位信息被传输给中央处理器模块。第二定位信息也被传输给MEMS惯导模块，并供MEMS惯导模块进行初始对准。在本实施例中，电子罗盘模块中采用了美国Honeywell公司生产的HMC5883L三轴电子罗盘芯片。

气压计模块通过测量携带者所处位置的气压，获得携带者的高度信息，从而形成第三定位信息。第三定位信息传输给中央处理器模块，也被传输给MEMS惯导模块，供MEMS惯导模块进行初始对准。在本实施例中，气压计模块中采用了美国MEAS公司生产的MS5607-02BA03气压高度计芯片。

MEMS惯导模块包含加速度计和陀螺仪，测量携带者的加速度和角速度，得到携带者的速度信息、位置信息和姿态信息形成第四定位信息。第四定位信息传输给中央处理器模块。在本实施例中，MEMS惯导模块采用了美国AD公司生产的ADIS16300惯性测量模块。

图像信息码读取模块读取预先设置的图像信息码(例如条形码或二维码)。这些图像信息码设置在建筑物的关键部位，对应其所在位置的位置信息。在失火时，携带者利用图像信息码读取模块读取图像信息码，从而得到图像信息码对应的位置信息(例如楼层、本楼层中的位置、以及面对该图像信息码时的方向信息等)。该位置信息形成校正信息。这一校正信息被传输给中央处理器模块。

中央处理器模块对接收到的所述校正信息、所述第一定位信息、所述第二定位信息、所述第三定位信息和所述第四定位信息进行数据融合(例如卡尔曼滤波方法)得到所述综合定位信息。中央处理器模块采用了DSP(digital signal processor)处理器。

存储模块存储建筑物的地图信息。

电子地图模块从存储模块中调取地图信息并显示地图，同时将中央处理器模块传来的所述综合定位信息在所述地图上显示。电子地图模块包含显示屏和输入输出模块以实现显示地图等功能，这属于现有技术，在此不再赘述。

MEMS惯导模块进行所述初始对准采用卡尔曼滤波方法，状态方程为：

$\left[\begin{array}{c}{\stackrel{\·}{X}}_I\\ {\stackrel{\·}{X}}_G\\ {\stackrel{\·}{h}}_B\\ {\stackrel{\·}{\mathrm{\φ}}}_C\end{array}\right]=\left[\begin{array}{cccc}{F}_I& 0& 0& 0\\ 0& F_G& 0& 0\\ 0& 0& 0& 0\\ 0& 0& 0& 0\end{array}\right]\left[\begin{array}{c}{X}_I\\ X_G\\ h_B\\ {\mathrm{\φ}}_C\end{array}\right]+\left[\begin{array}{cccc}{G}_I& 0& 0& 0\\ 0& I_{2\×2}& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{c}{W}_I\\ W_G\\ w_B\\ w_C\end{array}\right]$

其中，为MEMS惯导模块的状态量，δL_I、δλ_I分别为纬度误差和经度误差，δh为高度误差，δV_E、δV_N和δV_U分别为东向、北向和天向速度误差，φ_E、φ_N、φ_U为三个方向的失准角，和为MEMS惯导模块中加速度计的等效零偏，ε_x、ε_y、ε_z为MEMS惯导模块中陀螺仪的等效零偏。在初始对准时，携带者站在原地不动，速度为零，因此有

$G_I=\left[\begin{array}{c}{0}_{3\×6}\\ \begin{array}{cccccc}{c}_{11}& c_{21}& c_{31}& 0& 0& 0\\ c_{12}& c_{22}& c_{32}& 0& 0& 0\\ c_{13}& c_{23}& c_{33}& 0& 0& 0\\ 0& 0& 0& c_{11}& c_{21}& c_{31}\\ 0& 0& 0& c_{12}& c_{22}& c_{32}\\ 0& 0& 0& c_{13}& c_{23}& c_{33}\end{array}\\ 0_{6\×6}\end{array}\right]$

ω_ie为地球自转角速度，L为当地纬度，R为地球半径。X_G＝[δL_G δλ_G]为卫星导航信息接收模块输出的位置误差，该误差可以用一阶马尔可夫过程来近似拟合，表示为：

$\left\{\begin{array}{c}\mathrm{\δ}{\stackrel{\·}{L}}_G=-\frac{{\mathrm{\δL}}_G}{{\mathrm{\τ}}_{LG}}+w_{LG}\\ \mathrm{\δ}{\stackrel{\·}{\mathrm{\λ}}}_G=-\frac{{\mathrm{\δ\λ}}_G}{{\mathrm{\τ}}_{\mathrm{\λ}G}}+w_{\mathrm{\λ}G}\end{array}\right.$

其中τ_LG和τ_λG为相关时间常数，w_LG和w_λG为零均值高斯白噪声，因此有h_B为气压计模块输出高度，φ_C为电子罗盘模块输出的方位角，W_I＝[w_axw_ay w_az w_gx w_gy w_gz]为MEMS惯导模块中加速度计和陀螺仪的噪声，w_B为气压计模块的误差，w_C为电子罗盘模块的误差。将MEMS惯导模块输出的位置、高度、姿态角分别与卫星导航信息接收模块输出的位置、气压计模块输出的高度、电子罗盘模块输出的方位角做差可以得到观测方程：

$Z_{IC}=\left[\begin{array}{c}{\mathrm{\δL}}_I-{\mathrm{\δL}}_G\\ {\mathrm{\δ\λ}}_I-{\mathrm{\δ\λ}}_G\\ \mathrm{\δ}h-{\mathrm{\δh}}_B\\ {\mathrm{\δ\φ}}_U-{\mathrm{\δ\φ}}_C\end{array}\right]+\left[\begin{array}{c}{v}_L^{IC}\\ v_{\mathrm{\λ}}^{IC}\\ v_h^{IC}\\ v_{\mathrm{\φ}}^{IC}\end{array}\right]=H_{IC}\left[\begin{array}{c}{X}_I\\ X_G\\ h_B\\ {\mathrm{\φ}}_C\end{array}\right]+\left[\begin{array}{c}{v}_L^{IC}\\ v_{\mathrm{\λ}}^{IC}\\ v_h^{IC}\\ v_{\mathrm{\φ}}^{IC}\end{array}\right]$

其中为观测方程系数矩阵和为观测量中的零均值高斯白噪声。

图像信息码读取模块获得所述校正信息的方法采用卡尔曼滤波方法，状态方程为：

$\left[\begin{array}{c}{\stackrel{\·}{X}}_I\\ {\stackrel{\·}{X}}_P\end{array}\right]=\left[\begin{array}{cc}{F}_I& 0\\ 0& 0_{3\×3}\end{array}\right]\left[\begin{array}{c}{X}_I\\ X_P\end{array}\right]+\left[\begin{array}{cc}{G}_I& 0\\ 0& I_{3\×3}\end{array}\right]\left[\begin{array}{c}{W}_I\\ W_P\end{array}\right]$

此处的F_I和G_I与前述初始对准中的F_I和G_I相同，X_P＝[δL_P δλ_P δh_P]为相应图像信息码对应的经度误差、纬度误差和高度误差，把MEMS惯导模块输出的经度、纬度和高度与图像信息码对应的经度、纬度和高度分别做差，可以得到观测方程：

$Z_{IP}=\left[\begin{array}{c}{\mathrm{\δL}}_I-{\mathrm{\δL}}_P\\ {\mathrm{\δ\λ}}_I-{\mathrm{\δ\λ}}_P\\ \mathrm{\δ}h-{\mathrm{\δh}}_P\end{array}\right]+\left[\begin{array}{c}{v}_L^{IP}\\ v_{\mathrm{\λ}}^{IP}\\ v_h^{IP}\end{array}\right]=H_{IP}\left[\begin{array}{c}{X}_I\\ X_P\end{array}\right]+\left[\begin{array}{c}{v}_L^{IP}\\ v_{\mathrm{\λ}}^{IP}\\ v_h^{IP}\end{array}\right]$

其中H_IP＝[I_3×3 0_3×12 -I_3×3]为观测方程系数矩阵，和为观测量中的零均值高斯白噪声。

图2所示的实施例与图1所示实施例的区别在于：在图1结构基础上增加了无线通讯模块和RFID模块。无线通讯模块和RFID模块分别与中心处理器模块连接。

RFID模块包括RFID阅读器和RFID应答器。RFID阅读器与设置在其他消防员单兵定位导航装置上的RFID应答器进行通讯，在建立通讯连接后，RFID模块能够接收并获得与之建立联系的该消防员单兵定位导航装置的定位信息，例如位置信息、运动信息等。在获得了该消防员单兵定位导航装置的定位信息后，在电子地图模块显示所述定位信息。这样携带者就可以获知其他同伴所在的位置，这在火场这种能见度低的环境中非常有益于消防员之间的协同工作。当然，从前面的叙述中可以获知，RFID应答器的作用在于响应其他消防员单兵定位导航装置上RFID阅读器的通讯请求进行通讯，从而互相获取对方的定位信息。

无线通讯模块的作用是与外部的服务器进行通讯连接，例如WIFI模块。在图2所示实施例中，存储模块存储所述消防员单兵定位导航装置的移动路径信息，生成工作日志，这些工作日志和移动路径信息通过无线通讯模块传输给所述服务器，以便对携带者的工作过程进行记录。当然，服务器也可以通过无线通讯模块向消防员单兵定位导航装置发送一些控制指令、发送地图信息等，在此不再赘述。

在其他变形的实施例中，在图1所示实施例的结构基础上可以单独增加所述无线通讯模块或RFID模块。

图3显示了图2所示消防员单兵定位导航装置实施例的工作过程，以下分别针对各框图所代表的步骤进行说明。

是否进入火场。

在这一步骤中，需要判断是否需要进入火场进行救火，如果无需进入火场，则执行“室外导航模式”；如果需要进入火场，则执行“下载电子地图”步骤。

室外导航模式。

在这一步骤中，采用卫星导航信息接收模块(输出第一定位信息)、电子罗盘模块(输出第二定位信息)、气压计模块(输出第三定位信息)和MEMS惯导模块(输出第四定位信息)进行联合导航模式。如果能够正常接收到卫星定位信号，则以卫星导航信息接收模块给出的定位信息为准。如果不能够正常接收到卫星定位信号，则中央处理器模块将接收到的第一定位信息、第二定位信息、第三定位信息和第四定位信息进行数据融合，得到携带者的位置信息。

下载电子地图。

在这一步骤中，已经确定需要进入火场，则通过无线通讯模块下载火场的电子地图信息到存储模块。为了节省存储模块的空间，通常在需要的时候再下载相应的地图。

进行初始对准。

这一步骤是指MEMS惯导模块进行的初始对准。由于进入火场后，无法依赖卫星信号、气压等进行定位，需要依赖MEMS惯导模块进行航位推算，需要首先进行MEMS惯导模块的初始对准。由于此时尚未进入火场，则MEMS惯导模块接收到所述第一定位信息、所述第二定位信息、所述第三定位信息进行初始对准，具体的初始对准的方法如前所述。

禁用模块。

在这一步骤中，需要禁用(或屏蔽)卫星导航信息接收模块、电子罗盘模块和气压计模块。如前面所分析的，在火场中(通常是建筑物内)难以获得稳定的卫星信号进行定位，电子罗盘模块和气压计模块从原理上也容易受到火场环境的干扰，这些导致这三个模块的输出不具有参考意义，因此在火场中对这三个模块进行屏蔽。进入火场后就依赖MEMS惯导模块进行航位推算。

是否到达校正时间。

MEMS惯导模块工作一定时间后，累积的误差会导致输出的定位信息不准确。因此，消防员单兵定位导航装置记录MEMS惯导模块的工作时间，达到预定时间后，发提醒信息，然后由携带者执行“读取图像信息码”步骤。如果没有达到预定时间，执行“RFID模块发送定位信息”。

读取图像信息码。

在本步骤中，由携带者找到在火场预先设置的用于校正位置的图像信息码(例如二维码、条形码等)。这种图像信息码可以与建筑物内常设的防火设备在一起，以便携带者容易找到。图形信息码对应与其所在位置。

校正。

在本步骤，利用图像信息码读取模块扫描图像信息码可以获得校正信息。获得校正信息的具体方法如前所述。

RFID模块发送定位信息。

本步骤执行前述RFID模块中RFID应答器响应其他消防员单兵定位导航装置中RFID阅读器发出的通讯请求进行通讯，向请求方发送定位信息。

RFID模块接收其他定位信息。

本步骤中，RFID模块中RFID阅读器向邻近的消防员单兵定位导航装置中RFID应答器发出通讯请求，获取对方的定位信息。前面已经述及，在此不再赘述。

显示。

在电子地图模块显携带者当前位置信息，及其他携带者的当前位置信息。

任务是否结束。

在本步骤中，判断是否结束在火场中执行任务，如果没有结束，继续记录MEMS惯导的工作时间，回到“是否到达校正时间”步骤。如果结束，则执行“上传移动路径信息”步骤。

上传移动路径信息。

在本步骤中，携带者执行任务过程中产生的日志及移动路径信息等通过无线通讯模块上传。

至此，消防员单兵定位导航装置工作流程结束。

图4通过流程图的方式对MEMS惯导模块进行初始对准的过程进行了说明。

首先，执行“获取卫星导航信息模块信息”步骤，获得卫星导航信息模块得出的第一定位信息。

第二，执行“获取电子罗盘模块信息”步骤，获得电子罗盘模块得出的第二定位信息。

第三，执行“获取气压计模块信息”步骤，获得气压计模块得出的第三定位信息。

第四，执行“初始对准”步骤，采用签署的初始对准方法进行初始对准。

第五，判断“初始对准是否结束”，如果没有结束，重新回到“获取卫星导航信息模块信息”步骤重复以上步骤。

值得注意的是，以上所述仅为本发明的较佳实施例，并非因此限定本发明的专利保护范围，本发明还可以采用等同技术进行替换。故凡运用本发明的说明书及图示内容所作的等效变化，或直接或间接运用于其他相关技术领域均同理皆包含于本发明所涵盖的范围内。

Claims (7)

1.一种消防员单兵定位导航装置，包括卫星导航信息接收模块、电子罗盘模块、气压计模块、电子地图模块、存储模块、MEMS惯导模块、图像信息码读取模块和中央处理器模块；所述卫星导航信息接收模块、所述电子罗盘模块、所述气压计模块、所述电子地图模块、所述存储模块、所述MEMS惯导模块和所述图像信息码读取模块分别连接于所述中央处理器模块，其特征在于：

所述卫星导航信息接收模块接收卫星定位信号并将得出的第一定位信息传输给所述中央处理器模块，并供所述MEMS惯导模块进行初始对准；

所述电子罗盘模块获得携带所述定位导航装置的携带者的移动方向和移动姿态信息形成第二定位信息，将所述第二定位信息传输给所述中央处理器模块，并供所述MEMS惯导模块进行所述初始对准；

所述气压计模块获得所述携带者的高度信息形成第三定位信息，传输给所述中央处理器模块，并供所述MEMS惯导模块进行所述初始对准；

所述电子地图模块显示地图，并将所述中央处理器模块传来的综合定位信息在所述地图上显示；

所述存储模块存储所述地图的信息；

MEMS惯导模块测量所述携带者的加速度和角速度，得到所述携带者的速度信息、位置信息和姿态信息形成第四定位信息，并将所述第四定位信息传输给所述中央处理器模块；

所述图像信息码读取模块读取预先设置的图像信息码，得到校正信息并传输给所述中央处理器模块；所述图像信息码对应其所在位置的位置信息；

所述中央处理器模块对接收到的所述校正信息、所述第一定位信息、所述第二定位信息、所述第三定位信息和所述第四定位信息进行数据融合得到所述综合定位信息；

所述图像信息码读取模块得到所述校正信息的方法采用卡尔曼滤波方法，状态方程为：

$\left[\begin{array}{c}{\stackrel{\·}{X}}_I\\ {\stackrel{\·}{X}}_P\end{array}\right]=\left[\begin{array}{cc}{F}_I& 0\\ 0& 0_{3\×3}\end{array}\right]\left[\begin{array}{c}{X}_I\\ X_p\end{array}\right]+\left[\begin{array}{cc}{G}_I& 0\\ 0& I_{3\×3}\end{array}\right]\left[\begin{array}{c}{W}_I\\ W_P\end{array}\right]$

其中，为所述MEMS惯导模块的状态量，δL_I、δλ_I分别为纬度误差和经度误差，δh为高度误差，δV_E、δV_N和δV_U分别为东向、北向和天向速度误差，φ_E、φ_N、φ_U为三个方向的失准角，和为所述MEMS惯导模块中加速度计的等效零偏，ε_x、ε_y、ε_z为所述MEMS惯导模块中陀螺仪的等效零偏，W_I＝[w_ax w_ay w_az w_gx w_gy w_gz]为所述加速度计和所述陀螺仪的噪声；

$G_I=\left[\begin{array}{c}{0}_{3\×6}\\ \begin{array}{cccccc}{c}_{11}& c_{21}& c_{31}& 0& 0& 0\\ c_{12}& c_{22}& c_{32}& 0& 0& 0\\ c_{13}& c_{23}& c_{33}& 0& 0& 0\\ 0& 0& 0& c_{11}& c_{21}& c_{31}\\ 0& 0& 0& c_{12}& c_{22}& c_{32}\\ 0& 0& 0& c_{13}& c_{23}& c_{33}\end{array}\\ 0_{6\×6}\end{array}\right]$

$\begin{array}{c}{F}_I=\[\begin{array}{c}\begin{array}{cccccc}0& 0& 0& 0& \frac{1}{R+h}& 0\\ 0& 0& 0& \frac{\sec L}{R+h}& 0& 0\\ 0& 0& 0& 0& 0& 1\\ 0& 0& 0& 0& 2{\mathrm{\ω}}_{ie}\sin L& -2{\mathrm{\ω}}_{ie}\cos L\\ 0& 0& 0& -2{\mathrm{\ω}}_{ie}\sin L& 0& 0\\ 0& 0& 0& 2{\mathrm{\ω}}_{ie}\cos L& 0& 0\\ 0& 0& 0& 0& -\frac{1}{R+h}& 0\\ -{\mathrm{\ω}}_{ie}\sin L& 0& 0& \frac{1}{R+h}& 0& 0\\ {\mathrm{\ω}}_{ie}\cos L& 0& 0& \frac{\tan L}{R+h}& 0& 0\end{array}\\ 0_{6\×6}\end{array}\\ \left.\begin{array}{c}\begin{array}{ccccccccc}0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& -f_U& f_N& c_{11}& c_{21}& c_{31}& 0& 0& 0\\ f_U& 0& -f_E& c_{12}& c_{22}& c_{32}& 0& 0& 0\\ -f_N& f_E& 0& c_{13}& c_{23}& c_{33}& 0& 0& 0\\ 0& {\mathrm{\ω}}_{ie}\sin L& -{\mathrm{\ω}}_{ie}\cos L& 0& 0& 0& c_{11}& c_{21}& c_{31}\\ -{\mathrm{\ω}}_{ie}\sin L& 0& 0& 0& 0& 0& c_{12}& c_{22}& c_{32}\\ {\mathrm{\ω}}_{ie}\cos L& 0& 0& 0& 0& 0& c_{13}& c_{23}& c_{33}\end{array}\\ 0_{6\×9}\end{array}\right]\end{array}$

ω_ie为地球自转角速度，L为当地纬度，R为地球半径；X_P＝[δL_P δλ_P δh_P]为图像信息码对应的经度、纬度和高度误差；W_p＝[w_L w_λ w_h]^T为系统噪声，均为零均值白噪声；

观测方程为：

$Z_{IP}=\left[\begin{array}{c}{\mathrm{\δL}}_I-{\mathrm{\δL}}_P\\ {\mathrm{\δ\λ}}_I-{\mathrm{\δ\λ}}_P\\ \mathrm{\δ}h-{\mathrm{\δh}}_P\end{array}\right]+\left[\begin{array}{c}{v}_L^{IP}\\ v_{\mathrm{\λ}}^{IP}\\ v_h^{IP}\end{array}\right]=H_{IP}\left[\begin{array}{c}{X}_I\\ X_P\end{array}\right]+\left[\begin{array}{c}{v}_L^{IP}\\ v_{\mathrm{\λ}}^{IP}\\ v_h^{IP}\end{array}\right]$

其中，δL_I、δλ_I分别为纬度误差和经度误差，δh为高度误差，H_IP＝[I_3×3 0_3×12 -I_3×3]为观测方程系数矩阵，和为观测量中的零均值高斯白噪声。

2.根据权利要求1所述消防员单兵定位导航装置，其特征在于：所述MEMS惯导模块进行所述初始对准采用卡尔曼滤波方法，状态方程为：

$\left[\begin{array}{c}{\stackrel{\·}{X}}_I\\ {\stackrel{\·}{X}}_G\\ {\stackrel{\·}{h}}_B\\ {\stackrel{\·}{\mathrm{\φ}}}_C\end{array}\right]=\left[\begin{array}{cccc}{F}_I& 0& 0& 0\\ 0& F_G& 0& 0\\ 0& 0& 0& 0\\ 0& 0& 0& 0\end{array}\right]\left[\begin{array}{c}{X}_I\\ X_G\\ h_B\\ {\mathrm{\φ}}_C\end{array}\right]+\left[\begin{array}{cccc}{G}_I& 0& 0& 0\\ 0& I_{2\×2}& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{c}{W}_I\\ W_G\\ w_B\\ w_C\end{array}\right]$

其中，为所述MEMS惯导模块的状态量，δL_I、δλ_I分别为纬度误差和经度误差，δh为高度误差，δV_E、δV_N和δV_U分别为东向、北向和天向速度误差，φ_E、φ_N、φ_U为三个方向的失准角，和为所述MEMS惯导模块中加速度计的等效零偏，ε_x、ε_y、ε_z为所述MEMS惯导模块中陀螺仪的等效零偏，

$G_I=\left[\begin{array}{c}{0}_{3\×6}\\ \begin{array}{cccccc}{c}_{11}& c_{21}& c_{31}& 0& 0& 0\\ c_{12}& c_{22}& c_{32}& 0& 0& 0\\ c_{13}& c_{23}& c_{33}& 0& 0& 0\\ 0& 0& 0& c_{11}& c_{21}& c_{31}\\ 0& 0& 0& c_{12}& c_{22}& c_{32}\\ 0& 0& 0& c_{13}& c_{23}& c_{33}\end{array}\\ 0_{6\×6}\end{array}\right]$

$\begin{array}{c}{F}_I=\[\begin{array}{c}\begin{array}{cccccc}0& 0& 0& 0& \frac{1}{R+h}& 0\\ 0& 0& 0& \frac{\sec L}{R+h}& 0& 0\\ 0& 0& 0& 0& 0& 1\\ 0& 0& 0& 0& 2{\mathrm{\ω}}_{ie}\sin L& -2{\mathrm{\ω}}_{ie}\cos L\\ 0& 0& 0& -2{\mathrm{\ω}}_{ie}\sin L& 0& 0\\ 0& 0& 0& 2{\mathrm{\ω}}_{ie}\cos L& 0& 0\\ 0& 0& 0& 0& -\frac{1}{R+h}& 0\\ -{\mathrm{\ω}}_{ie}\sin L& 0& 0& \frac{1}{R+h}& 0& 0\\ {\mathrm{\ω}}_{ie}\cos L& 0& 0& \frac{\tan L}{R+h}& 0& 0\end{array}\\ 0_{6\×6}\end{array}\\ \left.\begin{array}{c}\begin{array}{ccccccccc}0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& -f_U& f_N& c_{11}& c_{21}& c_{31}& 0& 0& 0\\ f_U& 0& -f_E& c_{12}& c_{22}& c_{32}& 0& 0& 0\\ -f_N& f_E& 0& c_{13}& c_{23}& c_{33}& 0& 0& 0\\ 0& {\mathrm{\ω}}_{ie}\sin L& -{\mathrm{\ω}}_{ie}\cos L& 0& 0& 0& c_{11}& c_{21}& c_{31}\\ -{\mathrm{\ω}}_{ie}\sin L& 0& 0& 0& 0& 0& c_{12}& c_{22}& c_{32}\\ {\mathrm{\ω}}_{ie}\cos L& 0& 0& 0& 0& 0& c_{13}& c_{23}& c_{33}\end{array}\\ 0_{6\×9}\end{array}\right]\end{array}$

ω_ie为地球自转角速度，L为当地纬度，R为地球半径；X_G＝[δL_G δλ_G]为卫星导航单元输出的位置误差；

τ_LG和τ_λG为相关时间常数，h_B为所述气压计模块输出的高度，φ_C为所述电子罗盘模块输出的方位角，W_I＝[w_ax w_ay w_az w_gx w_gy w_gz]为所述MEMS惯导模块中加速度计和陀螺仪的噪声，w_B为所述气压计模块的误差，w_C为所述电子罗盘模块的误差；观测方程为：

$Z_{IC}=\left[\begin{array}{c}{\mathrm{\δL}}_I-{\mathrm{\δL}}_G\\ {\mathrm{\δ\λ}}_I-{\mathrm{\δ\λ}}_G\\ \mathrm{\δ}h-{\mathrm{\δh}}_B\\ {\mathrm{\δ\φ}}_U-{\mathrm{\δ\φ}}_C\end{array}\right]+\left[\begin{array}{c}{v}_L^{IC}\\ v_{\mathrm{\λ}}^{IC}\\ v_h^{IC}\\ v_{\mathrm{\φ}}^{IC}\end{array}\right]=H_{IC}\left[\begin{array}{c}{X}_I\\ X_G\\ h_B\\ {\mathrm{\φ}}_C\end{array}\right]+\left[\begin{array}{c}{v}_L^{IC}\\ v_{\mathrm{\λ}}^{IC}\\ v_h^{IC}\\ v_{\mathrm{\φ}}^{IC}\end{array}\right]$

其中为观测方程系数矩阵，和为观测量中的零均值高斯白噪声。

3.根据权利要求1所述消防员单兵定位导航装置，其特征在于：还包括与所述中央处理器模块连接的RFID模块，所述RFID模块包括RFID阅读器和RFID应答器；所述RFID阅读器与设置在其他消防员单兵定位导航装置上的RFID应答器进行通讯，获得该消防员单兵定位导航装置的定位信息，并在所述电子地图模块显示所述定位信息；所述RFID应答器用于与其他消防员单兵定位导航装置上的RFID阅读器进行所述通讯。

4.根据权利要求1所述消防员单兵定位导航装置，其特征在于：所述中央处理器模块在所述MEMS惯导模块完成所述初始对准后禁用所述气压计模块和所述电子罗盘模块。

5.根据权利要求1所述消防员单兵定位导航装置，其特征在于：还包括与所述中央处理器模块连接的无线通讯模块；所述无线通讯模块用于与外部的服务器进行数据通讯。

6.根据权利要求1所述消防员单兵定位导航装置，其特征在于：所述存储模块存储所述消防员单兵定位导航装置的移动路径信息。

7.根据权利要求1所述消防员单兵定位导航装置，其特征在于：记录所述MEMS惯导模块独立工作的时间，当所述时间达到预定值后，发出进行校正的提示信息。