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

摘要

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

主权项

一种消防员室内外3D无缝定位及姿态检测系统的检测方法，其特征在于，所述系统包括一基于STM32单片机的主控制电路板，一基于Sim‑900的GPRS数据传输模块，一GPS定位模块，一远程监控客户端以及分别佩戴于使用者的腰部及大腿外侧的惯性导航模块I和惯性导航模块II；其特征在于，所述主控制电路板通过串口分别与所述GPRS数据传输模块、GPS定位模块以及惯性导航模块I和惯性导航模块II相连；所述GPRS数据传输模块利用GPRS网络采用基于TCP的Socket通信技术与绑定的远程监控客户端连接通信；具体步骤如下：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)中磁力计与加速度计融合的方法为：当传感器处于倾斜的状态时，磁力计求出的偏航角会产生误差，所以需要用加速度计对磁力计进行倾斜补偿。首先根据三轴加速度数据(Ax，Ay，Az)求取横滚角和俯仰角θ，随后读取磁力计输出的三轴磁场强度M_b＝[M_x^b M_y^b M_z^b]，然后求出倾斜补偿后的磁力计输出根据倾斜补偿后的磁力计输出，求取偏航角ψ；所述步骤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₁))；