CN103076619A - 一种消防员室内外3d无缝定位及姿态检测系统及方法 - Google Patents

Abstract

本发明公开了一种消防员室内外3D无缝定位及姿态检测系统及方法。该系统包括一基于STM32单片机的主控制电路板，一基于Sim-900的GPRS数据传输模块，一GPS定位模块，一远程监控客户端以及惯性导航模块I和惯性导航模块II；主控制电路板通过串口分别与GPRS数据传输模块、GPS定位模块以及惯性导航模块I和惯性导航模块II相连；GPRS数据传输模块利用GPRS网络与绑定的远程监控客户端连接通信。本发明实现了消防员室内外3D无缝定位及姿态检测，可准确的对消防员的运动状态进行识别判断，同时可以实现对消防员的室内外3D无缝定位，针对消防人员的应用场合，系统可以扩展有毒气体传感器、烟雾、温度等传感器，对于保障复杂火场环境下作业的消防员的生命安全具有重要意义。

Description

一种消防员室内外3D无缝定位及姿态检测系统及方法

技术领域

本发明涉及一种消防员定位及姿态检测系统，具体涉及一种消防员室内外3D无缝定位及姿态检测系统及方法。属于消防员导航及定位、人体姿态检测、行为分析与识别领域。

背景技术

目前，随着人口的增加，人均土地面积越来越少，建筑楼群逐渐朝大型化、高层化发展。这种结构复杂的高层建筑一旦发生火灾，消防员进入后由于烟雾和楼层结构等各种原因很难进行自我定位，当消防员被困之后，即使有对讲机等无线通讯设备，由于消防员自身并不能对自己所在位置准确判断，因此很难及时向外部人员报告其准确楼层和位置，因而错过最佳营救时机；准确的人体运动状态检测是估计消防员当前生命安全状态的重要依据。不仅仅是消防行业，随着智能手机、iPad等各种智能终端设备的普及，消防员定位及姿态检测技术的市场需求也越来越迫切。

本发明主要涉及两个主要的理论及技术问题：一是消防员的室内外3D无缝定位，二是消防员的运动姿态检测。

消防员导航定位技术是指借助于专用的设备实现对个人的实时定位和跟踪。消防员导航技术通常包括室外导航定位技术及室内导航定位技术。人体姿态检测通常是指利用加速度计、陀螺仪等自包含传感器，通过检测人体特定部位的角度及加速度的变化来实现人的运动姿态检测。对于工作在复杂火场环境中的消防员，准确的室内外3D定位及姿态检测是消防员生命安全的重要保障，也是其完成火场探测和救援任务的基本前提。

目前，消防员室内外导航定位技术按照其定位机制的不同主要分为：基于GPS（全球定位系统）的定位技术、基于RFID（射频标签）的定位技术、基于无线局域网络的定位技术、基于自包含传感器（加速度计、陀螺仪、磁力计等）的定位技术等等。

中国专利申请201010023030.6和201210013467.0都是采用基于射频RFD的消防员定位方法，这种方法的缺点是需要对环境进行特殊改造，要在环境中布置一定数量的RFID读写器，使用不便，精度较低，适合环境简单。基于无线传感网络的方法，如Wi-Fi技术，Zibgee技术等，利用信号强度进行定位，这种方法的不足是需要架设无线传感网络，成本高，且无线信号易受干扰，精度较差。

中国专利申请201010201512.6，采用了一种GNSS（全球卫星导航系统）、UWB（超宽带技术）以及MEMS（微电子机械系统）混合定位的方式，并提出了一种基于卫星信号强度及信噪比的无缝切换规则，该方法的不足之处是环境中需要事先布置好复杂的UWB定位标签，并且该方法采用的MEMS定位技术是传统的积分机制的惯性定位技术，这种传统积分机制的惯性定位在人行走过程中对平台对准的要求很高，而且定位误差会随着时间的增长而不断累积，这种方式也是无法应用到消防员复杂的火场救援环境中的。

文献《基于GPS和自包含传感器的消防员室内外无缝定位算法研究》（2010，中国科学技术大学[D]，p19-25）提出了一种基于GPS和自包含传感器的定位方法，但是该方法并没有考虑人行走过程中身体摆动对偏航角的影响，这样人在行走过程中身体的左右摆动会对行人航迹推算定位算法的分解结果产生严重的影响，该方法只能做到平面范围内的室内外定位，无法提供楼层高度和消防员的运动状态信息。

中国专利申请201010539511.2公开了一种基于Mesh网状网架构的消防员火场定位系统，该系统由无线监控主机、若干无线定位子机和若干无线中继组成，该系统采用惯性定位技术，利用Mesh网进行通信，可以实现消防员的平面定位和姿态检测功能，但是该系统仅采用了传统积分机制的惯性定位技术，而传统积分机制惯性定位技术其定位结果本身就会随着使用时间的累积而发散，因此该方法无法保证较长时间定位的有效性，并且该方法仅依靠惯性定位技术，只能提供相对定位信息而无法提供绝对位置信息，无法及时消除惯性定位的累积误差，也无法提供消防员所在楼层高度信息。

中国专利201110047495.X通过三个实施例公开说明了一种用于火灾现场的，可进行三维定位，并协助消防员迅速实施灭火救援行动的消防员现场导航装置。实施例1中，其三维导航模块包含GPS模组和地磁惯性导航模组，采用惯性导航和地磁导航的组合导航方法。惯性/地磁组合导航的方法受磁力计的输出误差影响严重，而磁力计的输出很容易受到周围环境软硬铁磁效应的影响，该发明中没有对三轴磁力计的输出误差进行补偿，因此该种方法的定位精度受环境因素影响很大，如在钢制结构的建筑物内，磁力计的输出受铁磁效应影响十分严重，采用实施例1的方法则无法得到准确的定位结果。此外，实施例1中GPS模组的定位信息仅作为初始定位参考，没有对GPS定位和惯性定位信息做融合，累积定位误差无法得到适时消除，只能通过重启设备的方式消除累积误差，难以保证长时间定位的准确性。实施例2中，采用RFID进行楼层高度识别，采用GPS模组进行GPS定位。采用RFID进行楼层高度识别，需要事先布置大量RFID电子标签，而GPS在室内环境下的定位结果是不可信的，因此实施例2很难在室内情况下实际应用。实施例3采用信号到达时间差定位算法，这种定位方法受多路效应及障碍物的遮挡影响严重。该发明所公开的方法，无法做到对消防员的运动姿态进行实时监测，没有对多种定位信息进行融合，难以保证长时间的定位精度。

中国专利申请201210203123.6公开了一种消防员灭火救援定位指挥系统，该救援指挥系统包括通信基站、操作显示模块、单兵终端、信标和信标读写器，该方法能够实现消防员的定位，但是需要消防员每到达一个预定位置时就要对所带设备进行相应操作，紧急火场环境下，容易出现遗漏或者错误操作的情况，不是一种主动连续定位方法。

消防员姿态检测目前主要分为基于视觉和基于穿戴式传感器两种。基于视觉的人体姿态检测会严重受到外界环境的影响，比如光照条件、遮挡情况、背景复杂程度等。基于穿戴式传感器的人体姿态检测一般通过检测人体躯干特定部位的角度或者加速度，通过阈值法进行姿态的判断。

中国专利200910028156.X通过两个加速度计作为姿态传感器，通过设定加速度阈值能够识别静止、跑步、跳跃等姿态，但是难以区分弯腰和躺下等动作，并且由于该方法只采用了两个单轴加速度计，因此无法做到三轴加速度的实时测量，装置的佩戴位置及方向也有着严格的限制。

中国专利申请201210185040.9提出了一种基于惯性导航模块和GPS的老年人摔倒检测定位装置，该装置利用神经网络、机器学习等算法能够较为准确的识别出老人摔倒的情况，但是该方法仅依靠GPS定位，当GPS信号出现遮挡时则定位功能失效，无室内定位功能，并且该方法利用一块惯性导航模块，只能实现单一的摔倒检测，无法区分比如弯腰、站立、行走、躺下等多种姿态，不适宜消防员复杂的室内外火场环境的3D无缝定位及姿态检测需求。

综上，现有的消防员定位及姿态检测技术难以同时实现消防员室内外复杂火场环境下的连续3D无缝定位及多种运动姿态检测的需求。

发明内容

本发明的目的是为克服上述现有技术的不足，提供一种消防员室内外3D无缝定位及姿态检测系统及方法。针对消防员室内外复杂火场环境的应用背景，充分结合GPS定位及惯性定位技术的优点，设计一种实时、可靠、准确度高，能够为消防员同时提供室内外复杂环境下连续无缝3D定位及多种运动姿态检测功能的系统及其方法。

为实现上述目的，本发明采用如下技术方案：

一种消防员室内外3D无缝定位及姿态检测系统，包括一基于STM32单片机的主控制电路板，一基于Sim-900的GPRS数据传输模块，一GPS定位模块，一远程监控客户端以及分别佩戴于使用者的腰部及大腿外侧的惯性导航模块I和惯性导航模块II；所述主控制电路板通过串口分别与所述GPRS数据传输模块、GPS定位模块以及惯性导航模块I和惯性导航模块II相连；所述GPRS数据传输模块利用GPRS网络采用基于TCP的Socket通信技术与绑定的远程监控客户端连接通信。

该系统还包括连接于主控制电路板的电源。

所述主控制电路板包括STM32单片机，以及分别与STM32单片机连接的液晶显示模块、存储模块EEPROM24C256、按键、温度传感器、烟雾传感器和扩展I/O口。

所述惯性导航模块I和惯性导航模块II均包括一三轴加速度计，一三轴磁力计，一三轴陀螺仪，所述惯性导航模块I还包括气压计。

一种消防员室内外3D无缝定位及姿态检测系统的检测方法，步骤如下：

1）上电初始化；

2）接收GPS信号，进行GPS定位；

3）楼层高度和运动姿态的识别：利用气压计的数据进行楼层高度的判别，对惯性导航模块I和惯性导航模块II各个传感器三轴磁力计、三轴加速度计、三轴陀螺仪的数据融合之后输出的角度及加速度信息，通过查表法估计当前被测人员的运动状态；若当前运动状态为行走则转入步骤4），否则转入步骤5）；

4）行人航迹推算定位信息：根据主控制电路板中存储的步长及步频参数，利用惯性导航模块I的加速度及偏航角信息进行行人航迹推算定位；

5）远程监控客户端接收消防员定位及姿态检测装置上传的信息，将GPS定位结果和行人航迹推算定位结果进行融合，并通过调用卫星地图的接口函数将最终定位结果标定在地图上，同时远程监控客户端完成当前消防员的运动状态以及火场内部环境温度各类信息的提示功能。

所述步骤2）中GPS定位具体方法如下：

21）判断GPS定位是否为有效定位：判断搜星个数是否大于4，再判断水平精度因子是否小于3，若两个条件均符合则为有效定位并转入步骤22），否则为无效定位并转入步骤23）；

22）判断GPS定位是否为可信定位：首先，解算相邻两帧间GPS经纬度相隔距离，若所述距离小于2m则判断为可信定位，将GPS定位信息作为最终的定位结果，转入步骤23）；否则为不可信定位，转入步骤3)；

23）根据GPS判断结果标记相应标志位或者清0，所述标志位包括：GPS有效标志位，GPS可信标志位。

所述步骤3）中消防员的运动状态包括：行走、站立、弯腰、趴下、坐下、躺下、爬行。

所述步骤3）中对惯性导航模块I和惯性导航模块II各个传感器三轴磁力计、三轴加速度计、三轴陀螺仪的数据融合之后输出角度及加速度信息的原理及过程为：

31）惯性导航模块采集三轴陀螺仪信号，采用四元数姿态表达式，积分求得陀螺仪姿态角，然后采用三轴磁力计和三轴加速度计，利用大地磁场和重力磁场在地理坐标和运动坐标系之间的方向余弦进行绝对角度的解算。最后利用卡尔曼滤波对得到的姿态角度进行融合。最终惯性导航模块可以稳定的输出融合后的姿态角度及三轴加速度信息。采用这种方式的好处是，可以利用加速度计与磁力计克服单独采用陀螺仪引起的姿态角的发散，利用陀螺仪可以克服由于振动对加速度计的影响以及由于软硬铁磁对于磁力计的影响。

四元数与欧拉角的相互转换：

根据欧拉定理，刚体绕固定点的位移也可以是绕该点的若干次有限转动的合成。在欧拉转动中，将参与坐标系转动三次得到星体坐标系。在三次转动中每次的旋转轴是被转动坐标系的某一坐标轴，每次的转动角即为欧拉角。因此，用欧拉角确定的姿态矩阵是三次坐标转换矩阵的乘积。这些坐标转换矩阵都有如下标准形式：

$R_y\left(\mathrm{\θ}\right)=\left[\begin{array}{ccc}\cos \mathrm{\θ}& 0& -\sin \mathrm{\θ}\\ 0& 1& 0\\ \sin \mathrm{\θ}& 0& \cos \mathrm{\θ}\end{array}\right]$ $R_z\left(\mathrm{\ψ}\right)=\left[\begin{array}{ccc}\cos \mathrm{\ψ}& \sin \mathrm{\ψ}& 0\\ -\sin \mathrm{\ψ}& \cos \mathrm{\ψ}& 0\\ 0& 0& 1\end{array}\right]$

其中

为绕x轴（横滚轴）的旋转矩阵，R_y(θ)为绕y轴（俯仰轴）的旋转矩阵，R_z(ψ)为绕z轴（偏航轴）的旋转矩阵，

为绕x轴旋转的角度，θ是绕y轴旋转的角度，ψ为绕z轴旋转的角度。后面公式中出现的相同符号意义相同，不加赘述。

本发明采用Z-Y-X的旋转顺序，于是可以得到利用欧拉角表示的姿态矩阵A：

根据四元数的定义，可以通过旋转轴和旋转的角度构造一个四元数q：

q＝cos(φ/2)+isin(φ/2)cos(β_x)+jsin(φ/2)cos(β_y)+ksin(φ/2)cos(β_z)

其中φ为绕旋转轴旋转的角度，cos(β_x)、cos(β_y)、cos(β_z)分别为旋转轴在x、y、z轴分量。后面公式中出现的相同符号意义相同，不加赘述。

将欧拉角转换为四元数，本发明采用的是欧拉Z-Y-X转动：

第一次先绕Z轴旋转ψ角度，四元数表示为：

第二次先绕Y轴旋转θ角度，四元数表示为：

第三次先绕X轴旋转

角度，四元数表示为：

三轴转动合成为

则有：

利用三角公式：cosφ＝2cos²(φ/2)-1，sinφ＝2sin(φ/2)cos(φ/2)可将四元数转换成姿态矩阵：

$A=\left[\begin{array}{ccc}{q_1}^2+{q_2}^2-{q_3}^2-{q_4}^2& 2(q_2{q}_3+q_1{q}_4)& 2(q_2{q}_4-q_1{q}_3)\\ 2(q_2{q}_3-q_1{q}_4)& {q_1}^2-{q_2}^2+{q_3}^2-{q_4}^2& 2(q_3{q}_4+q_1{q}_2)\\ 2(q_2{q}_3+q_1{q}_4)& 2(q_2{q}_4-q_1{q}_3)& {q_1}^2-{q_2}^2-{q_3}^2+{q_4}^2\end{array}\right]$

由于本算法程序在运行时是以四元数为变量进行运算的，由于四元数不能直观的表示输出的角度，需要将四元数转换成姿态角，由欧拉角和四元数表示的姿态矩阵可得四元数转换姿态角公式为：

θ＝arcsin(-2(q₂q₄-q₁q₃))

$\mathrm{\ψ}=\arctan \left(\frac{2(q_2{q}_3+q_1{q}_4)}{{q_1}^2+{q_2}^2-{q_3}^2-{q_4}^2}\right)$

32）加速度计与陀螺仪数据的融合原理：利用陀螺仪动态性能较好而加速度计稳态精度较高的特点，静态时以加速度计的数据修正陀螺仪的数据，动态时用陀螺仪的值修正加速度计的数据。根据三轴加速度计输出的三轴加速度（Ax，Ay，Az），可以得到横滚角

和俯仰角θ为：

33）磁力计与加速度计融合的原理：当传感器处于倾斜的状态时，磁力计求出的偏航角会产生误差，所以需要用加速度计对磁力计进行倾斜补偿。首先根据三轴加速度数据（Ax，Ay，Az）求取横滚角

和俯仰角θ，随后读取磁力计输出的三轴磁场强度M_b＝[M_x ^b M_y ^b M_z ^b]，然后求出倾斜补偿后的磁力计输出

根据倾斜补偿后的磁力计输出，可以求取偏航角ψ，

$\mathrm{\ψ}=a\tan 2\left(\frac{{M_y}^n}{{M_x}^n}\right)$

所述步骤4）中步长及步频参数的具体存储方法如下：

4a）进入步长训练模式，通过GPS接收机接收并记录刚进入步长训练模式时的经纬度信息；

4b）使用者以固定步频行走一段距离；

4c）通过惯性导航模块I的加速度计的输出加速度信息实现步频探测功能，记录使用者所走步数；

4d）退出步长训练模式，记录此时GPS接收机接收到的经纬度信息；

4e）根据训练初始时刻的经纬度信息和训练结束时刻的经纬度信息，求出使用者所走距离S₁；步长的计算公式为S＝S₁/n，式中S为步长，S₁为所走的距离，n为步频探测结果即所走的步数并询问是否将步长和步频写入主控制电路板的存储模块中保存；

依次循环4a）~4e）则可以得到多组不同步频下对应的步长信息，并能将上述训练的结果存入主控制电路板的存储模块中保存，方便掉电后再次使用。

所述步骤4）中行人航迹推算定位的过程为：

41）步频的探测：以20Hz的采样频率，采集佩戴于惯性导航模块I输出的三轴加速度计信息（Ax，Ay，Az），分别对三轴加速度数据采用窗口长度为5的等权前端点滑动窗口均值法进行滤波处理，对滤波处理后的加速度数据求其矢量和

采用幅值和时间双阈值算法对合加速度进行峰值检测：首先，对融合后的合加速度数据进行峰值点的判断，峰值点Sum_A[i]的判断条件为Sum_A[i]>Sum_A[i-1]&&Sum_A[i]>Sum_A[i+1]；若当前合加速度Sum_A[i]为峰值点，则进一步判断当前合加速度是否为计步峰值点，计步峰值点的判断采用的是幅值判别法，只有满足幅值条件的峰值点才认为计步峰值点，否则认为是局部峰值点；

若当前该合加速度判断为计步峰值点则进入时间阈值判别，只有当两个计步峰值点时间间隔大于0.4s时才认为是合理的计步峰值点，若判断当前合加速度数据为合理计步峰值点时则行走步数加1，并转入步骤42）；

42）根据当前步频，根据存储于主控制电路板中的步频与步长的对应关系，按照查表法，选取当前合适步长S，并转入步骤43）；

43）将腰部惯性导航模块的偏航角信息作为被测人员的航向角；以带阻滤波器抑制身体晃动对偏航角的影响；

44）位置的计算与分解：在每次得到计步峰值点后，都会进行一次位置的解算与分解；假设前一时刻的位置为（E(t₁)，N(t₁)），后一时刻的位置为（E(t₂)，N(t₂)），这段时间内的航向为α(t₁)，步长为S(t₁)，则两个时刻的位置关系为：

E(t₂)＝E(t₁)+S(t₁)×sin(α(t₁))

N(t₂)＝N(t₁)+S(t₁)×cos(α(t₁))。

所述步骤5）中GPS定位和行人航迹推算定位结果融合的准则是：

51）若GPS定位结果是可信定位，则直接将GPS定位结果作为最终的定位结果；

52）若GPS定位结果是有效定位但不是可信定位，则采用混合定位的方式，将GPS定位结果与行人航迹推算定位结果输入卡尔曼滤波器，融合之后的定位信息作为最终定位结果；

53）若GPS定位结果是无效定位，则直接将行人航迹推算的定位结果作为最终的定位结果。

所述步骤5）中消防员定位及姿态检测装置上传的信息包括：GPS定位信息，行人航迹推算定位信息，楼层高度信息，运动姿态信息，环境温度信息。

本发明的有益效果：

本发明实现了消防员室内外3D无缝定位及姿态检测，与传统平面定位方法相比本发明可以实现楼层高度的识别，可准确的对消防员的运动状态进行识别判断，同时可以实现对消防员的室内外3D无缝定位，针对消防人员的应用场合，系统可以扩展有毒气体传感器、烟雾、温度等传感器，方便外部人员及时掌握复杂火场环境中消防员的位置信息、楼层高度信息、运动状态信息以及火场内部的环境信息，对于保障复杂火场环境下作业的消防员的生命安全具有重要意义。除此之外，本发明稍经改造即可应用于空巢老人、医院病人等需要及时定位和姿态监测的特殊人群。本发明使用方便，功能齐全，准确率高，性能稳定，应用范围广，具有很高的应用价值。与传统的方法相比，本发明利用一套装置同时实现了消防员室内外3D无缝定位和姿态检测两大基本功能，所采用定位方法的定位误差不会随着时间的累积而发散，具有定位准确、连续性好、姿态识别准确的优点。

附图说明

图1为本发明的消防员室内外3D无缝定位及姿态检测系统整体框图；

图2为本发明的佩戴示意图；

图3为主控制电路板工作流程图；

图4为远程监控客户端的工作流程图；

图5为本发明的工作原理图；

图6为GPS数据处理流程图；

图7为步长训练流程图；

图8为姿态检测算法流程图；

图9a-c为行人航迹推算定位算法流程图；

其中，1.主控制电路板，2.GPRS数据传输模块，3.GPS定位模块，4.惯性导航模块I，5.惯性导航模块II，6.远程监控客户端，7.GPRS通信，8.三轴加速度计，9.三轴陀螺仪，10.三轴磁力计，11.气压计，12.液晶显示模块，13.存储模块EEPROM24C256，14.按键，15.温度传感器，16.烟雾传感器，17.扩展I/O口，18.STM32单片机，19.电源。

具体实施方式

下面结合附图和实施例对本发明进行进一步的阐述，应该说明的是，下述说明仅是为了解释本发明，并不对其内容进行限定。

如图1，本发明包括一基于STM32单片机的主控制电路板1，一基于Sim-900的GPRS数据传输模块2，一GPS定位模块3，一远程监控客户端6以及分别佩戴于使用者的腰部及大腿外侧的惯性导航模块I4和惯性导航模块II5；主控制电路板1通过串口分别与GPRS数据传输模块2、GPS定位模块3以及惯性导航模块I4和惯性导航模块II5相连；GPRS数据传输模块2利用GPRS网络采用基于TCP的Socket通信技术与绑定的远程监控客户端6连接通信。该系统还包括连接于主控制电路板1的电源19。主控制电路板1包括STM32单片机18，以及分别与STM32单片机18连接的液晶显示模块12、存储模块EEPROM24C256 13、按键14、温度传感器15、烟雾传感器16和扩展I/O口17，其中，存储模块EEPROM24C25613通过I²C通信接口与STM32单片机18通信。惯性导航模块I4和惯性导航模块II5均包括一三轴加速度计8，一三轴磁力计10，一三轴陀螺仪9，所述惯性导航模块I4还包括气压计11。

一种消防员室内外3D无缝定位及姿态检测系统的检测方法，步骤如下：

1）上电初始化：在空旷地带，正确佩戴所述检测系统，上电并初始化各传感器及GPRS数据传输模块2；判断GPRS数据传输模块2是否连网成功，若连网不成功，则判断等待连网时间是否超时，超时则返回初始化步骤，未超时则继续等待，若连网成功则转入步骤2）；

2）接收GPS信号并进行GPS定位；

3）楼层高度和运动姿态的识别：利用气压计11的数据进行楼层高度的判别，对惯性导航模块I4和惯性导航模块II5各个传感器三轴磁力计10、三轴加速度计8、三轴陀螺仪9的数据融合之后输出的角度及加速度信息，通过查表法估计当前被测对象的运动状态；若当前运动状态为行走则转入步骤4），否则转入步骤5）；

4）行人航迹推算定位信息：根据主控制电路板1中存储的步长及步频参数，利用惯性导航模块I4的加速度及偏航角信息进行行人航迹推算定位；

5）远程监控客户端6接收消防员定位及姿态检测装置上传的信息，将GPS定位结果和行人航迹推算定位结果进行融合，并通过调用卫星地图的接口函数将最终定位结果标定在地图上，同时远程监控客户端6完成当前消防员的运动状态以及火场内部环境温度各类信息的提示功能。

所述步骤2）中GPS定位具体方法如下：

21）判断GPS定位是否为有效定位：判断搜星个数是否大于4，再判断水平精度因子是否小于3，若两个条件均符合则为有效定位并转入步骤22），否则为无效定位并转入步骤23）；

22）判断GPS定位是否为可信定位：GPS定位模块3数据更新频率为1Hz，正常人的步行频率一般在2steps/s左右，步长一般不超过身体长度的一半，一般在55cm-80cm之间。考虑到波动及干扰，留出一定的阈量，首先，解算相邻两帧间GPS经纬度相隔距离，若所述距离小于2m则判断为可信定位，将GPS定位信息作为最终的定位结果，转入步骤23）；否则为不可信定位，转入步骤3）；

23）根据GPS判断结果标记相应标志位或者清0，所述标志位包括：GPS有效标志位，GPS可信标志位。

所述步骤3）中消防员的运动状态包括：行走、站立、弯腰、趴下、坐下、躺下、爬行。为了更加准确可靠的识别出消防员的运动状态，本发明采用了两块惯性导航模块。惯性导航模块I4可以测量腰部角度的变化量，惯性导航模块II5可以测量出腿部角度的变化量，根据腰部角度和腿部角度的不同组合可以通过查表法快速准确的识别出站立、行走、弯腰、趴下、躺下、坐下、爬行等运动状态，通过腰部惯性导航模块I4的加速度信息还可识别跳跃等运动状态。与只利用一块惯性导航模块的消防员运动姿态检测方法相比，本方法可以识别更多的运动姿态，且编程实现简单，识别准确度高。

所述楼层高度识别的原理为：首先，根据当前所处楼层高度，通过K3、K4按键进行初始楼层调整，然后对气压计11进行初始化配置，初始化工作完成后读取气压计11数据。海平面上正常标准气压为760mm汞柱，在接近海平面的地区，两点间气压每相差1mm汞柱（1mmHg=1.333mb），则两点高度差约为10.5m。根据公式

$H=44300\×(1-{\left(\frac{p}{p_0}\right)}^{\frac{1}{5.25}})$

即可方便的求解出当前气压对应的高度，其中p为测量点处的压强，p₀为海平面处标准大气压。由于气压受环境温度、风速等影响很大，同一地点不同时刻测得的气压值可能会相差很大，但对应于同一高度差的气压差相对变化量不变，因此本发明进行楼层高度识别时采用的是气压计的相对变化值。

所述步骤3）中对惯性导航模块I4和惯性导航模块II5各传感器（三轴加速度计8、三轴磁力计10、三轴陀螺仪9）数据融合之后输出角度及加速度信息的原理及过程为：

31）惯性导航模块I4和惯性导航模块II5采集三轴陀螺仪9信号，采用四元数姿态表达式，积分求得三轴陀螺仪9姿态角，然后采用三轴磁力计10和三轴加速度计8，利用大地磁场和重力磁场在地理坐标和运动坐标系之间的方向余弦进行绝对角度的解算。最后利用卡尔曼滤波对得到的姿态角度进行融合。最终惯性导航模块I4和惯性导航模块II5可以稳定的输出融合后的姿态角度及三轴加速度信息。采用这种方式的好处是，可以利用三轴加速度计8与三轴磁力计10克服单独采用三轴陀螺仪9引起的姿态角的发散，利用陀螺仪可以克服由于振动对加速度计的影响以及由于软硬铁磁对于磁力计的影响。

四元数与欧拉角的相互转换：

根据欧拉定理，刚体绕固定点的位移也可以是绕该点的若干次有限转动的合成。在欧拉转动中，将参与坐标系转动三次得到星体坐标系。在三次转动中每次的旋转轴是被转动坐标系的某一坐标轴，每次的转动角即为欧拉角。因此，用欧拉角确定的姿态矩阵是三次坐标转换矩阵的乘积。这些坐标转换矩阵都有如下标准形式：

$R_y\left(\mathrm{\θ}\right)=\left[\begin{array}{ccc}\cos \mathrm{\θ}& 0& -\sin \mathrm{\θ}\\ 0& 1& 0\\ \sin \mathrm{\θ}& 0& \cos \mathrm{\θ}\end{array}\right]$ $R_z\left(\mathrm{\ψ}\right)=\left[\begin{array}{ccc}\cos \mathrm{\ψ}& \sin \mathrm{\ψ}& 0\\ -\sin \mathrm{\ψ}& \cos \mathrm{\ψ}& 0\\ 0& 0& 1\end{array}\right]$

其中

为绕x轴（横滚轴）的旋转矩阵，R_y(θ)为绕y轴（俯仰轴）的旋转矩阵，R_z(ψ)为绕z轴（偏航轴）的旋转矩阵，为绕x轴旋转的角度，θ是绕y轴旋转的角度，为绕z轴旋转的角度。后面公式中出现的相同符号意义相同，不加赘述。

本发明采用Z-Y-X的旋转顺序，于是可以得到欧拉角表示的姿态矩阵A：

根据四元数的定义，可以通过旋转轴和旋转的角度构造一个四元数q：

q＝cos(φ/2)+isin(φ/2)cos(β_x)+jsin(φ/2)cos(β_y)+ksin(φ/2)cos(β_z)

式中φ为绕旋转轴旋转的角度，cos(β_x)、cos(β_y)、cos(β_z)分别为旋转轴在x、y、z轴分分量。后面公式中出现的相同符号意义相同，不加赘述。

将欧拉角转换为四元数，本发明采用的是欧拉Z-Y-X转动：

第一次先绕Z轴旋转ψ角度，四元数表示为：

第二次先绕Y轴旋转θ角度，四元数表示为：

第三次先绕X轴旋转

角度，四元数表示为：

三轴转动合成为则有：

利用三角公式：cosφ＝2cos²(φ/2)-1，sinφ＝2sin(φ/2)cos(φ/2)可将四元数转换成姿态矩阵：

$A=\left[\begin{array}{ccc}{q_1}^2+{q_2}^2-{q_3}^2-{q_4}^2& 2(q_2{q}_3+q_1{q}_4)& 2(q_2{q}_4-q_1{q}_3)\\ 2(q_2{q}_3-q_1{q}_4)& {q_1}^2-{q_2}^2+{q_3}^2-{q_4}^2& 2(q_3{q}_4+q_1{q}_2)\\ 2(q_2{q}_3+q_1{q}_4)& 2(q_2{q}_4-q_1{q}_3)& {q_1}^2-{q_2}^2-{q_3}^2+{q_4}^2\end{array}\right]$

由于本算法程序在运行时是以四元数为变量进行运算的，由于四元数不能直观的表示输出的角度，需要将四元数转换成姿态角，由欧拉角和四元数表示的姿态矩阵可得四元数转换姿态角公式为：

θ＝arcsin(-2(q₂q₄-q₁q₃))

$\mathrm{\ψ}=\arctan \left(\frac{2(q_2{q}_3+q_1{q}_4)}{{q_1}^2+{q_2}^2-{q_3}^2-{q_4}^2}\right)$

32）三轴加速度计8与三轴陀螺仪9数据的融合原理：利用三轴陀螺仪9动态性能较好而三轴加速度计8稳态精度较高的特点，静态时以三轴加速度计8的数据修正三轴陀螺仪9的数据，动态时用三轴陀螺仪9的值修正三轴加速度计8的数据。根据三轴加速度计8输出的三轴加速度（Ax，Ay，Az），可以得到横滚角

和俯仰角θ为：

33）三轴磁力计10与三轴加速度计8融合的原理：当传感器处于倾斜的状态时，三轴磁力计10求出的偏航角会产生误差，所以需要用三轴加速度计8对三轴磁力计10进行倾斜补偿。首先根据三轴加速度计8输出的三轴加速度数据（Ax，Ay，Az）求取横滚角

和俯仰角θ，随后读取三轴磁力计10输出的三轴磁场强度M_b＝[M_x ^b M_y ^b M_z ^b]，然后求出倾斜补偿后的磁力计输出

根据倾斜补偿后的磁力计输出，可以求取偏航角ψ：

$\mathrm{\ψ}=a\tan 2\left(\frac{{M_y}^n}{{M_x}^n}\right)$

如图7所示，所述步骤4）中步长及步频参数的具体存储方法如下：

4a）按下K1键进入步长训练模式，通过GPS定位模块3接收并记录刚进入步长训练模式时的经纬度信息；

4b）使用者以固定步频行走一段距离；

4c）通过惯性导航模块I4的三轴加速度计8的输出加速度信息实现步频探测功能，记录使用者所走步数；

4d）再次按下K1键退出步长训练模式，记录此时GPS定位模块3接收到的经纬度信息；

4e）根据训练初始时刻的经纬度信息和训练结束时刻的经纬度信息，求出使用者所走距离S₁；步长的计算公式为S＝S₁/n，式中S为步长，S₁为所走的距离，n为步频探测结果即所走的步数并询问是否将步长和步频写入主控制电路板1的存储模块EEPROM24C25613中保存；

依次循环4a）~4e）则可以得到多组不同步频下对应的步长信息，并能将上述训练的结果存入主控制电路板1的存储模块EEPROM24C256 13中保存，方便掉电后再次使用。

所述步骤4）中行人航迹推算定位的过程为：

41）步频的探测：以20Hz的采样频率，采集佩戴于惯性导航模块I4输出的三轴加速度计8信息（Ax，Ay，Az），分别对三轴加速度8数据采用窗口长度为5的等权前端点滑动窗口均值法进行滤波处理，对滤波处理后的加速度数据求其矢量和

所述滑动窗口均值滤波的作用是减少人行走过程中由于身体晃动而引起的“一步多峰”现象，对加速度波形进行平滑，使之更适于峰值检测；

采用幅值和时间双阈值算法对合加速度进行峰值检测：首先，对融合后的合加速度数据进行峰值点的判断，峰值点Sum_A[i]的判断条件为Sum_A[i]>Sum_A[i-1]&&Sum_A[i]>Sum_A[i+1]；若当前合加速度Sum_A[i]为峰值点，则进一步判断当前合加速度是否为计步峰值点，计步峰值点的判断采用的是幅值判别法，只有满足幅值条件的峰值点才认为计步峰值点，否则认为是局部峰值点；

滑动窗口滤波及双阈值峰值检测算法验证如图9（b）所示，该组曲线表示的是被测人员行走19步所测得的加速度数据，黑色点画线表示原始加速度信息，黑色实心曲线表示经过滑动窗口滤波后的加速度信息；对滤波后的加速度数据采用所述双阈值峰值检测算法检测到的峰值点（黑色圈）数目为19个，峰值检测结果与被测人员实际行走步数一致；

若当前该合加速度判断为计步峰值点则进入时间阈值判别，只有当两个计步峰值点时间间隔大于0.5s时才认为是合理的计步峰值点，若判断当前合加速度数据为合理计步峰值点时则行走步数加1，并转入步骤42）；

42）根据当前步频，根据存储于主控制电路板1中的步频与步长的对应关系，按照查表法，选取当前合适步长S，并转入步骤43）；

43）将惯性导航模块I4的偏航角信息作为被测人员的航向角；以带阻滤波器抑制身体晃动对偏航角的影响；

44）位置的计算与分解：在每次得到计步峰值点后，都会进行一次位置的解算与分解；假设前一时刻的位置为（E(t₁)，N(t₁)），后一时刻的位置为（E(t₂)，N(t₂)），这段时间内的航向为α(t₁)，步长为S(t₁)，如图9（b）所示，则两个时刻的位置关系为：

E(t₂)＝E(t₁)+S(t₁)×sin(α(t₁))

N(t₂)＝N(t₁)+S(t₁)×cos(α(t₁))。

所述步骤5）中GPS定位和行人航迹推算定位结果融合的准则是：

51）若GPS定位结果是可信定位，则直接将GPS定位结果作为最终的定位结果；

52）若GPS定位结果是有效定位但不是可信定位，则采用混合定位的方式，将GPS定位结果与行人航迹推算定位结果输入卡尔曼滤波器，融合之后的定位信息作为最终定位结果；

53）若GPS定位结果是无效定位，则直接将行人航迹推算的定位结果作为最终的定位结果。

所述步骤5）中消防员定位及姿态检测装置上传的信息包括：GPS定位信息，行人航迹推算定位信息，楼层高度信息，运动姿态信息，环境温度信息。

惯性导航模块I4和惯性导航模块II5，通过对三轴加速度计8、三轴陀螺仪9、三轴磁力计10三种传感器的数据采集并做基于扩展卡尔曼滤波的数据融合，采用AHRS算法输出准确稳定的三轴姿态角和三轴加速度信息。惯性导航模块I4中的气压计11用于判断楼层的高度，实现3D定位。GPS定位模块3可以向STM32单片机18输出经纬度信息，在完成步长模型训练之后，STM32单片机18可以利用惯性导航模块I4输出的加速度信息及经过滤波融合之后的偏航角信息完成行人航迹推算定位，并根据惯性导航模块I4、惯性导航模块II5输出的角度及加速度组合，利用查表法完成消防员姿态检测功能，并将经纬度信息、行人航迹推算定位结果、楼层高度信息、运动姿态信息以及温度、烟雾等环境信息按照事先设定的通信格式通过Sim-900GPRS数据传输模块2传输到远程监控客户端6。远程监控客户端6接收GPS定位信息、行人航迹推算定位信息并将上述定位信息做松耦合的扩展卡尔曼滤波，给出最终的定位结果，通过C++调用地图接口函数，将最终定位结果直接显示在地图上；同时，远程监控客户端6还会将接收到的消防员运动姿态信息，楼层高度信息等进行相应的提示，当消防员运动姿态信息出现非正常状态（如趴下时间超过设定阈值）会向外界发出报警信息。

如图2所示，本发明要求使用者将惯性导航模块I4佩戴于腰部，将惯性导航模块II5佩戴于大腿外侧。由于本发明在进行步频探测时采用的是惯性导航模块I4的三轴合加速度信息，因此降低了对惯性导航模块I4的安装要求，但在装置佩戴时要松紧适宜，避免出现惯性导航模块佩戴松动的情况，以免影响到惯性导航模块I4、惯性导航模块II5的角度输出，对行人航迹推算定位和姿态检测产生影响。

如图3、4、5所示，本发明的工作流程为：STM32单片机18进行系统初始化，初始化后对Sim-900GPRS数据传输模块2进行配置，等待其连网成功。Sim-900GPRS数据传输模块2连网成功后，STM32单片机18开始接收GPS信号，并对GPS信号进行处理，GPS信号处理流程如图6所示。若GPS定位为可信定位且K1按下则进入步长模型训练过程，步长模型训练流程如图7所示。根据腰部惯性导航模块I4的气压计11输出值的相对变化量实现楼层高度识别功能。采集腰部惯性导航模块I4及腿部惯性导航模块II5的三轴加速度计8、三轴陀螺仪9、三轴磁力计10的数据，采用基于扩展卡尔曼滤波的方法对数据进行融合，并利用AHRS算法输出稳定准确的俯仰、横滚及偏航角。根据惯性导航模I4和惯性导航模块II5块输出的角度及加速度信息，利用查表法完成姿态检测功能，姿态检测查表流程如图8所示。若当前姿态为行走，则利用惯性导航模块I4输出的三轴加速度和偏航角信息完成行人航迹推算定位，行人航迹推算定位的流程如图9（a）所示。采集温度、烟雾等传感器数据，并通过液晶显示模块12对相应信息进行提示。通过Sim-900GPRS数据传输模块2完成对各类信息的上传。所述需要上传的各类信息包括：GPS定位的经纬度信息、行人航迹推算定位信息、温度信息、运动姿态信息、楼层高度信息等。

远程监控客户端6，接收Sim-900GPRS数据传输模块2传输的GPS定位的经纬度信息、行人航迹推算定位信息、温度信息、运动姿态信息、楼层高度信息。若GPS定位结果是可信定位，则将GPS定位信息作为最终的定位结果；若GPS定位结果是有效定位但不是可信定位，则采用混合定位的方式，将GPS定位信息与行人航迹推算定位信息输入卡尔曼滤波器，融合之后的定位信息作为最终定位结果；若GPS定位结果是无效定位，则将行人航迹推算定位信息作为最终的定位结果。通过调用地图接口函数将定位结果显示在地图上，同时将温度、楼层、运动姿态等其他信息也做出相应的显示，当消防员出现非正常运动姿态（如趴下时间超过设定阈值）会向外界发出报警信息。远程监控客户端6的工作流程图如图4所示。

上述虽然结合附图对本发明的具体实施方式进行了描述，但并非对本发明保护范围的限制，在本发明的技术方案的基础上，本领域技术人员不需要付出创造性劳动即可做出的各种修改或变形仍在本发明的保护范围以内。

Claims (10)

1.一种消防员室内外3D无缝定位及姿态检测系统，包括一基于STM32单片机的主控制电路板，一基于Sim-900的GPRS数据传输模块，一GPS定位模块，一远程监控客户端以及分别佩戴于使用者的腰部及大腿外侧的惯性导航模块I和惯性导航模块II；其特征在于，所述主控制电路板通过串口分别与所述GPRS数据传输模块、GPS定位模块以及惯性导航模块I和惯性导航模块II相连；所述GPRS数据传输模块利用GPRS网络采用基于TCP的Socket通信技术与绑定的远程监控客户端连接通信。

2.如权利要求1所述的一种消防员室内外3D无缝定位及姿态检测系统，其特征在于，该系统还包括连接于主控制电路板的电源。

3.如权利要求1所述的一种消防员室内外3D无缝定位及姿态检测系统，其特征在于，所述主控制电路板包括STM32单片机，以及分别与STM32单片机连接的液晶显示模块、存储模块EEPROM24C256、按键、温度传感器、烟雾传感器和扩展I/O口。

4.如权利要求1所述的一种消防员室内外3D无缝定位及姿态检测系统，其特征在于，所述惯性导航模块I和惯性导航模块II均包括一三轴加速度计，一三轴磁力计，一三轴陀螺仪，所述惯性导航模块I还包括气压计。

5.权利要求1所述的系统的检测方法，其特征在于，步骤如下：

1）上电初始化；

2）接收GPS信号，进行GPS定位；

3）楼层高度和运动姿态识别：利用气压计的数据进行楼层高度的判别，对惯性导航模块I和惯性导航模块II各个传感器三轴加速度计、三轴磁力计、三轴陀螺仪的数据融合之后输出的角度及加速度信息，通过查表法估计当前被测对象的运动状态；若当前运动状态为行走则转入步骤4），否则转入步骤5）；

4）行人航迹推算定位信息：根据主控制电路板中存储的步长及步频参数，利用惯性导航模块I的加速度及偏航角信息进行行人航迹推算定位；

5）远程监控客户端接收消防员定位及姿态检测装置上传的信息，将GPS定位结果和行人航迹推算定位结果进行融合，并通过调用卫星地图的接口函数将最终定位结果标定在地图上，同时远程监控客户端完成当前消防员的运动状态以及火场内部环境温度各类信息的提示功能。

6.如权利要求5所述的检测方法，其特征在于，所述步骤2）中GPS定位具体方法如下：

21）判断GPS定位是否为有效定位：判断搜星个数是否大于4，再判断水平精度因子是否小于3，若两个条件均符合则为有效定位并转入步骤22），否则为无效定位并转入步骤23）；

22）判断GPS定位是否为可信定位：首先，解算相邻两帧间GPS经纬度相隔距离，若所述距离小于2m则判断为可信定位，将GPS定位信息作为最终的定位结果，转入步骤23）；否则为不可信定位，转入步骤3）；

23）根据GPS判断结果标记相应标志位或者清0，所述标志位包括：GPS有效标志位，GPS可信标志位。

7.如权利要求5所述的检测方法，其特征在于，所述步骤3）中消防员的运动状态包括：行走、站立、弯腰、趴下、坐下、躺下、爬行。

8.如权利要求5所述的检测方法，其特征在于，所述步骤4）中步长及步频参数的具体存储方法如下：

4a）进入步长训练模式，通过GPS接收机接收并记录刚进入步长训练模式时的经纬度信息；

4b）使用者以固定步频行走一段距离；

4c）通过惯性导航模块I的加速度计的输出加速度信息实现步频探测功能，记录使用者所走步数；

4d）退出步长训练模式，记录此时GPS接收机接收到的经纬度信息；

4e）根据训练初始时刻的经纬度信息和训练结束时刻的经纬度信息，求出使用者所走距离S₁；步长的计算公式为S＝S₁/n，式中S为步长，S₁为所走的设定距离，n为步频探测结果即所走的步数并询问是否将步长和步频写入主控制电路板的存储模块中保存；

依次循环4a）~4e）则可以得到多组不同步频下对应的步长信息，并能将上述训练的结果存入主控制电路板的存储模块中保存，方便掉电后再次使用。

9.如权利要求5所述的检测方法，其特征在于，所述步骤4）中行人航迹推算定位的过程为：

41）步频的探测：以20Hz的采样频率，采集佩戴于惯性导航模块I输出的三轴加速度计信息（Ax，Ay，Az），分别对三轴加速度数据采用窗口长度为5的等权前端点滑动窗口均值法进行滤波处理，对滤波处理后的加速度数据求其矢量和

采用幅值和时间双阈值算法对合加速度进行峰值检测：首先，对融合后的合加速度数据进行峰值点的判断，峰值点Sum_A[i]的判断条件为Sum_A[i]>Sum_A[i-1]&&Sum_A[i]>Sum_A[i+1]；若当前合加速度Sum_A[i]为峰值点，则进一步判断当前合加速度是否为计步峰值点，计步峰值点的判断采用的是幅值判别法，只有满足幅值条件的峰值点才认为计步峰值点，否则认为是局部峰值点；

若当前该合加速度判断为计步峰值点则进入时间阈值判别，只有当两个计步峰值点时间间隔大于0.5s时才认为是合理的计步峰值点，若判断当前合加速度数据为合理计步峰值点时则行走步数加1，并转入步骤42）；

42）根据当前步频，根据存储于主控制电路板中的步频与步长的对应关系，按照查表法，选取当前合适步长S，并转入步骤43）；

43）将腰部惯性导航模块的偏航角信息作为被测人员的航向角；以带阻滤波器抑制身体晃动对偏航角的影响；

44）位置的计算与分解：在每次得到计步峰值点后，都会进行一次位置的解算与分解；假设前一时刻的位置为（E(t₁)，N(t₁)），后一时刻的位置为（E(t₂)，N(t₂)），这段时间内的航向为α(t₁)，步长为S(t₁)，则两个时刻的位置关系为：

E(t₂)＝E(t₁)+S(t₁)×sin(α(t₁))

N(t₂)＝N(t₁)+S(t₁)×cos(α(t₁))。

10.如权利要求5所述的检测方法，其特征在于，所述步骤5）中GPS定位和行人航迹推算定位结果融合的准则是：

51）若GPS定位结果是可信定位，则直接将GPS定位结果作为最终的定位结果；

52）若GPS定位结果是有效定位但不是可信定位，则采用混合定位的方式，将GPS定位结果与行人航迹推算定位结果输入卡尔曼滤波器，融合之后的定位信息作为最终定位结果；

53）若GPS定位结果是无效定位，则直接将行人航迹推算的定位结果作为最终的定位结果。

Images