基于MEMS-MARG传感器的消防员室内定位算法

杨 刚，李 强，韩 路，张 鑫

（西安邮电大学 通信与信息工程学院，西安 710121）

常年以来，建筑物火灾事故频有发生，常需要消防员进入火场进行灭火和救援。火场环境烟雾弥漫，能见度低，消防员难以确定自己的具体位置，给灭火和救援带来了很大的困难。由于缺乏成熟的消防员室内定位系统，不仅影响了消防员的救援效率，也极大威胁到消防员的生命安全，造成更大悲剧的发生。

目前关于消防员室内定位算法的研究成果比较少，主要以UWB、RFID、ZigBee、WIFI、惯性导航等方面的定位算法研究为主。文献[1]使用UWB技术实现消防员火场定位，UWB虽然具有较高的定位精度，但是需要提前部署定位基站，并且在室内环境中易受遮挡造成较大的非视距误差。文献[2]使用RFID技术定位，这种方式虽然成本低，但是其抗干扰能力较差。文献[3]使用基于RSSI算法的ZigBee技术实现消防员室内定位，这种定位方式虽然功耗小，但是易受多径效应的影响。上述定位技术具有一定的局限性，在真实火场环境中很难满足消防员精确实时定位的需求。

惯性导航定位技术是一种不依赖任何外部设备或先验数据库，自主性强的室内定位技术[4]，比较适合于消防员在火场中的定位。随着MEMS技术的发展，惯性传感器的体积变得越来越小，成本逐渐降低，被大量用于行人导航系统中[5]。9轴MARG（Magnetic,Angular Rate,and Gravity）系统，是一种包含3轴加速度计，陀螺仪和磁力计的传感器阵列，能够提供相对于重力方向和地球磁场方向的完整测量[6]。行人航迹推算（Pedestrian Dead Reckoning，PDR）算法基于行人步态的运动生理学特征，利用惯性传感器计算行走时的步数、步长和航向信息，由初始位置推算出行人的位置坐标[7-8]。

本文使用MARG系统作为定位数据采集的终端，根据消防员行走过程中所采集的3轴加速度值、3轴角速度值和3轴磁强度值，计算出步数、步长和航向角信息，使用PDR算法结合扩展卡尔曼滤波器（Extended Kalman Filter,EKF）估计出消防员的位置坐标。对于步数检测问题，设计了一种根据加速度幅值波形使用峰值探测法和零点交叉法相结合的方式进行步数检测；对于航向角误差的累积问题，在改进的启发式漂移消除（improve Heuristic Drift Elimination,iHDE）算法[9]的基础上设计了一种航向反馈修正算法，通过直行判定和主导方向判定将消防员的行走状态进行严格分类，对每种状态采取不同的航向修正措施。最后，将EKF和PDR算法融合，推算出消防员的行走轨迹。通过上述方法，可以有效地提高消防员在室内环境中的定位精度。

1 系统介绍

定位系统的算法框架如图1所示。首先通过消防员胸前佩戴的传感器获取行走过程 MARG系统的原始数据，对原始数据进行滤波和校准预处理。对预处理后的数据分别进行以下处理：使用Madgwick算法根据3轴加速度、角速度和磁场强度求出航向角；通过加速度幅值来获取行走步数和步长信息；对获取的原始航向角进行滤波预处理，再经过反馈修正算法修正航向；最后通过PDR算法根据步数、步长和航向数据使用EKF估计出消防员的位置坐标。

图1 系统框架Fig.1 System framework

2 消防员室内定位算法

2.1 坐标系

本文采用的导航坐标系（n系）是东北天（East North Up，ENU）坐标系，X、Y、Z轴分别为正东、正北和法线方向，构成右手坐标系。载体坐标系（b系）是用户自身所定义的坐标系，原点是导航设备的中心，消防员行走中水平前进方向为Y轴，设备的右侧为Y轴，Z轴垂直向上。

在 ENU坐标系下，通过下文介绍的步数检测、步长计算和航向求解及修正使用PDR算法结合EKF计算出消防员的位置坐标。

2.2 步数检测

传统的步频检测法有峰值检测法[10]、零点交叉法、自相关法、快速傅里叶变换法等[11]。考虑到消防员在火场行走的复杂情况，本文通过对加速度幅值的波形分析，使用峰值探测法和零点交叉法结合的方式进行步数检测，具体过程如下所述。

其次将传感器静止放置一定的时间进行加速度数据采样，计算出本地的重力加速度值。将减去的加速度幅值进行Kalman滤波：

状态方程为：

式中，ak为k时刻Kalman滤波的状态变量，预测噪声满足由具体实验环境决定。

观测方程为：

再对经过 Kalman滤波后的结果进行平滑滤波，经过处理后的加速度幅值波形变得更加平滑，如图2所示，这时可以进行步数检测。预设加速度阈值和相邻波峰时间阈值来检测加速度波峰，根据波峰数来确定消防员行走步数M；通过加速度幅值波形零点特征，判断加速度幅值前一点小于 0，当前点大于或等于 0，并且预设相邻零点时间阈值来检测零点，准确标记一步的开始时刻和结束时刻，用于获取一步之内加速度的最大值和最小值来计算步长。根据零点交叉算法标定的每一步开始与结束时刻，求出每一步的航向角平均值，作为下文航向角修正的原始航向角。图2中红色五角星标记为检测到的波峰，绿色圆圈标记为检测到的零点。

图2 加速度幅值波形Fig.2 Acceleration amplitude waveform

2.3 步长估计

本文根据加速度幅值信息使用非线性步长模型[12]来计算消防员行走过程中每一步的步长：

式中，K是模型参数，Amax(m)和Amin(m)代表第m步加速度的最大值和最小值，S(m)表示第m步的步长。

2.4 航向角求解

本文将9轴MARG传感器输出的3轴加速度值、3轴角速率值和3轴磁力值通过Madgwick算法[6]更新四元数来求解航向角。

Madgwick算法的目标是通过加权融合k时刻的陀螺仪算出的四元数和加速度计与磁力计共同计算出的四元数，进而得到最优的四元数融合加权的公式如下所示：

2.5 航向反馈修正算法

本文提出的航向反馈修正算法通过直行判定和主导方向判定将消防员的行走状态进行严格分类，对于每种状态采取不同的航向修正措施，避免出现错误矫正情况的发生，增加了算法的健壮性。此外，用于航向修正的卡尔曼滤波器状态变量只有一维，减少了算法的复杂度和运算成本。

本算法将消防员在室内行走路线分为两类，分别是曲线行走和直线行走。曲线行走时算法不直接修正航向角，由于算法框架融合的磁力计保证了原始航向角具有一定的精确度，所以可以直接使用原始航向角进行坐标计算。直线行走又分为沿着主导方向直线行走和沿着非主导方向直线行走。在主导方向直线行走时使用主导方向值作为航向角参考值，在非主导方向直线行走时使用邻近三步航向角的均值作为航向角参考值，将当前航向角与航向角参考值的差值作为卡尔曼滤波器的观测量，求出最优航向误差估计值，使用当前航向减去最优航向误差估计值得到修正后的航向角。

算法的具体流程如图3所示。首先对原始航向角进行滤波预处理，然后进行直行判断和主导方向判断，再计算航向角偏差，最后对航向角偏差进行卡尔曼滤波处理，进而修正航向角。算法详细步骤如下所述：

到了24周，宝宝体重约650克，4周时间里从20周的300克又增重了350克，平均每周增重90克左右。这时会发现宝宝增重加快了，从16～20周每周的45克到每周90克。相对而言孕妈妈也应该开始增加增重幅度了，每周应该在250克左右，这样到24周，孕妈妈从孕前开始可以增重4千克左右。其中大约2.5千克是必需增重的，1.5千克是为将来母乳喂养宝宝做准备的。

图3 航向反馈修正算法流程图Fig.3 Flow chart of heading feedback algorithm

1）主导方向设定

根据建筑物大多坐南朝北的特点，本文设置了东、西、南、北4个主导方向，如图4所示，主导方向之间的间隔Δ是90°。本文设定航向角的范围是 0°到±180°。

图4 主导方向Fig.4 Dominant direction

2）直行判定

分别用连续 3步航向值减去这三步航向的平均值，并获取最大航向差值和预设的阈值进行比较，如式(10)所示：

式中，m为当前时刻的步数。

当SLP为1时判定为直线行走，可以进入下一项判断；当SLP为0时判定为非直线行走，不进入下一项判定。Thθ为直线行走判定预设的角度阈值。

3）主导方向判断

在判定为直线行走的条件下，使用MOD函数来判断消防员是否在主导方向直行：

当E的绝对值大于35°时，判定为主导方向行走；当E＞0时，行人在邻近主导方向的右侧，当E＜0时，行人在邻近主导方向的左侧。E的阈值设定是参考相关文献[9,13-14]，并经过大量实验确定的。

4）航向角误差计算

在主导方向直行时，航向角误差值δψ(m)等于当前航向角ψs减去邻近主导方向的值；在非主导方向直行时，航向角误差值δψ(m)等于当前航向角减去邻近三步航向角的均值，即：

式中：ψb为与当前航向角ψs最邻近的主导方向的航向值；mean(ψs(j))为邻近三步航向角的均值，j=m,m-1,m-2；δψ(m)作为卡尔曼滤波器的观测量用来修正航向角。

5）航向角误差置信度

式中，σHDE=0.1 rad[9]。在对航向角误差δψ(m)进行Kalman滤波时，将σδψ(m)作为观测噪声大小的参考值。

6）卡尔曼滤波处理

本文使用航向角误差值δψ(m)作为Kalman的观测量来估计出最优航向角误差X(m|m)。卡尔曼滤波算法的详细过程可以参考文献[13]，本文不再赘述。

7）航向角修正

根据 Kalman滤波器求出的最优航向角误差估计X(m|m)，使用当前航向值ψs(m)减去最优航向误差估计值X(m|m)得到修正后的航向值(m)，即：

式中，ψs(m)为修正前的航向角，(m)为修正后的航向角。

2.6 消防员行走轨迹推算

本文将扩展卡尔曼滤波器（EKF）和PDR算法融合，把前文获取到的步数m、步长S(m)、和修正后的航向角(m)代入到EKF中推算出消防员的最优行走轨迹。具体过程如下所述：

系统状态变量为：

式中，E和N为消防员在ENU坐标系中的东向和北向坐标，S为每一步的步长，ψ为每一步的航向角。

根据PDR算法，状态方程为：

由于状态方程(16)是非线性方程，根据EKF的原理，用状态方程计算的雅各比矩阵近似表示状态转移矩阵：

动态预测噪声为：

观测量为：

观测方程为：

观测方程是线性方程，观测矩阵为：

观测噪声矩阵为：

通过EKF的预测更新和测量更新动态方程，可以得到状态变量的最优估计值：

3 实验验证与分析

3.1 数据采集

本次实验使用集成MPU9250传感器的JY901作为实验数据采集平台。JY901的主控部分STM32F051是一款基于ARM 32-bit的Cortex®-M0 CPU，具有32k到64k字节的Flash Memory，工作频率高，功耗低，外设丰富且价格低。MPU9250是一款9轴MARG运动跟踪传感器，由3轴加速度计、3轴陀螺仪和3轴磁力计组成。加速度计误差为±6mg，陀螺仪误差为±5 (°)/s。经过测试，根据MPU9250传感器采集的惯性数据，使用 Madgwick算法求取的原始航向角误差在±10°范围内波动。在实验中，设定传感器的数据采集速率是50 Hz，工作电压是3.3 V，波特率为115 200 Hz。实验场地是西安邮电大学东区逸夫楼八楼自习室，实验中惯性传感器佩戴在测试人员胸前，传感器获取到的数据通过串口传输到PC端，由Matlab软件来处理数据，如图5所示。

图5 数据采集Fig.5 Data acquisition

3.2 实验内容

为了验证本文提出的消防员室内定位算法的性能，设计了3组实验，测试人员胸前佩戴着传感器，分别沿着矩形路线、三角形路线和半圆形路线进行实地测试。

1）矩形路线实验

图6 矩形实验航向角变化Fig.6 Variations of heading angle inrectangular experiment

矩形路线实验是沿着东、南、西、北四个主导方向行走一圈，以验证在这四个主导方向上直行时航向反馈修正算法的性能。图6是测试人员行走过程中航向角的变化，通过对比分析，未经修正的原始航向角波形不平整且偏离真实值，经过航向反馈修正算法处理的航向角更加平滑并且较为接近真实航向值。图7为矩形实验的行走轨迹，无航向修正的PDR算法轨迹图在短时间严重偏离真实轨迹，加了航向反馈修正算法的轨迹图始终接近真实轨迹。

如表1所示，使用的航向反馈修正算法的返回原位置误差（Return Position Error,RPE）为0.9048 m，与未使用相比提升了46.84%，位置RMSE提升了77.90%，航向 RMSE提升了 65.03%。本次实验验证了在主导方向直行时航向反馈修正算法可以很好地修正航向。

图7 矩形实验行走轨迹Fig.7 Rectangular experimental walking track

表1 矩形实验精度分析Tab.1 Accuracy analysis of rectangular experiment

2）三角形路线实验

在三角形路线实验中，行走轨迹为一个直角三角形，两条直角边在主导方向上，一条斜边在非主导方向上。该实验是为了验证在非主导方向直行时，航向反馈修正算法的性能。图8为航向角变化情况，其中前两段为在主导方向直行时的航向角变化，最后一段为在非主导方向直行时的航向角变化。图9为三角形实验的行走轨迹，相比单纯使用PDR算法，添加了航向反馈修正算法生成的轨迹更加接近真实轨迹。

图8 三角形实验航向角变化Fig.8 Variations of heading angle in triangular experiment

图9 三角形实验行走轨迹Fig.9 Triangle experimental walking track

如表2所示，添加的航向反馈修正算法 RPE为1.10m，与未使用航向修正算法相比提升了 45.54%，位置 RMSE提升了 76.81%，航向 RMSE提升了68.21%。三角形实验的结果表明在非主导方向直行时航向反馈修正算法具有良好的精度。

表2 三角形实验精度分析Tab.2 Accuracy analysis of triangle experiment

3）半圆形路线实验

半圆形路线实验设定为直径在主导方向上，圆弧在非主导方向上，这是对航向反馈修正算法设计的挑战性实验。半圆形路线实验的航向角变化如图10所示：第一段是沿着直径在主导方向上直行，航向反馈修正算法会修正航向角；第二段是沿着圆弧曲线行走，由于实际航向角在不断变化，相比于直线行走，没有一个固定的航向角观测值，导致难以计算出航向角偏差，无法对航向角进行直接修正，航向反馈修正算法性能会暂时下降，轨迹推算直接使用原始航向角。实验轨迹如图11所示，由于真实行走轨迹的半圆不是规则的半圆，是一个近似的半圆，所以在本实验的轨迹图中并没有标出真实的轨迹。本文提出的航向反馈修正算法框架融合了磁力计，求出的原始航向角具有一定的精确度，在曲线行走航向修正算法性能下降的情况下，如图11所示，所依据的原始航向信息依然具备一定的准确性。考虑到在多数情况下人的行走是直线行走，所以在曲线行走时导致短暂的算法性能下降不会对最终定位精度产生较大的影响。

图10 半圆形实验航向角变化Fig.10 Variations of heading angle in semicircular experiment

图11 半圆形实验行走轨迹Fig.11 Semicircular experimental walking track

4 结 论

本文以MARG传感器为基础，针对消防员室内火场定位的应用背景，设计了一种导航定位方法。针对PDR算法中的步数检测问题，设计了一种根据加速度幅值波形使用峰值探测法和零点交叉法相结合的方法来准确获取消防员的行走步数。针对惯性导航中航向角误差随时间和行走距离增大的问题，提出了一种航向反馈修正算法来修正航向角。当消防员沿着直线路径行走时，算法会对航向角进行修正，修正后的航向角结合步数和步长信息通过EKF和PDR算法来估计出消防员的位置坐标。算法框架所融合的磁力计增加了航向信息的准确度，即使消防员在非理想路径行走时也可以获得良好的定位精度。综合3组实验，本文所提算法将 RPE提升了 45%以上，航向角和位置的RMSE分别提升65%和76%以上，具有较好的稳定性和可靠性，能适应较为复杂的应用场景。

本文所述的定位方法为消防员在室内火场环境中的位置信息获取提供了一种有效的解决方案。在后续的研究中，将考虑如何解决周围磁场环境对磁力计测量精度的影响。磁力计扰动的改善将有助于提高定位精度，进而保障消防员的自身安全和提高救援效率。