基于MEMS惯性传感器和气压计的行人室内导航系统设计*

邹一凡，李 佳，王玮冰

(1.中国科学院微电子研究所,北京 100029; 2.中国科学院大学,北京 100049)

0 引 言

行人导航系统(pedestrian navigation system,PNS)被广泛应用于紧急救援、人员管理、健康护理等场景[1～3]，用于在全球定位系统(GPS)不可用的室内环境中获得行人的准确定位信息。根据实现原理的不同，PNS可分为基于无线信号、基于视觉设备、基于惯性传感器3种类别[4]。其中，基于惯性传感器的PNS主要基于行人航迹推算(pedestrian dead reckoning,PDR)理论，具有不依赖于外部设备、低成本、高可靠性的特点，因此成为了该领域研究的热点。

PDR系统通过将惯性测量单元(inertial measurement unit,IMU)安装至可穿戴设备中，测量行人在运动时的加速度、角速度、空间磁场、气压值等信息，经过一定的数据融合算法，还原出行人运动的空间轨迹。PDR算法可分为基于步长—方向模型的算法以及基于惯性导航系统(inertial navigation system,INS)的算法。其中，前者通过对传感器数据进行分析、建模，对行人每一步的步长与方向进行估计，从而以步为单位更新行人位置；后者通过对传感器的空间姿态进行实时计算，将加速度转换至导航坐标系中进行2次积分，分别获得空间速度与位置。本文即基于后者进行PNS的设计。

当前，基于INS原理的PDR系统有一个通用的、经受过实践检验的算法框架，即INS+扩展卡尔曼滤波(extended Kalman filtering,EKF)+零速更新(zero velocity update,ZUPT)框架，简称IEZ框架。这一框架最初由Foxlin E[5]提出，经由Jiménez A R[6]改善，如今已在事实上成为该领域的基本算法思路[7～9]。为了修正系统在航向角上的误差，在其基础上进一步添加零角速度更新(zero angular rate update,ZARU)、罗盘(compass)、启发式偏移清除(heuristic drift elimination,HDE)、气压计修正等算法，即可获得更加精确的定位结果。

本文基于IEZ+ZARU+HDE+气压计修正的算法思路，自主研发了软硬件系统，设计并实现了完整的行人室内导航系统方案。通过实验对系统性能进行了验证，结果表明本文系统可满足实际应用需求。

1 基于IEZ框架的导航算法设计

1.1 系统总体框架

本文中，使用加速度计+陀螺仪+气压计的传感器组合进行数据采集。系统硬件中包含三轴磁力计，但并没有使用，其原因在于室内环境中，地磁场受到严重的干扰，无法用于对航向角进行修正[10]。整体的算法框图如图1所示。由传感器获得的数据被直接用于INS解算，获得对载体姿态、空间运动速度以及位置的初步估计；使用EKF对INS系统的误差进行估计，其测量值来自多个来源，将在1.2节中进行详细介绍；当脚步落地的零速区间被检测到时，即执行ZUPT与HDE过程。

图1 系统算法框图

1.2 INS算法

图1中最上层为INS部分，利用惯性导航原理对载体姿态、速度与位置进行更新。使用四元数q=[q0q1q2q3]T作为姿态表示方法，使用一阶近似对四元数微分方程求数值解，可得离散化的四元数更新方程

(1)

式中ω=[ωxωyωz]为陀螺仪采集到的三轴角速度数据，T为采样周期。利用上式计算后再将其归一化，即完成了通过陀螺仪数据更新姿态。

(2)

(3)

之后，即可对加速度na进行2次积分，分别得到空间速度nv与位置np。

1.3 EKF算法

EKF算法用于对系统中各个变量的误差进行估计。首先定义15维的状态向量

xk=[δφδωδpδvδa]T

(4)

其中,包含了姿态估计误差δφ,n系中的速度误差δv和位置误差δp，以及b系中的陀螺仪偏移δω和加速度偏移δa，均为三维向量。为了方便利用测量进行更新，此处姿态误差使用欧拉角进行表示，在对姿态进行误差修正时，将-δφ/T代入式(1)即可。

写出系统状态更新方程

(5)

(6)

式中k为第k次采样，wk为系统噪声，I与0均为3×3矩阵，[nak×]为向量nak对应的叉乘矩阵

(7)

之后，系统状态即可随着传感器采样进行迭代更新，直至获得测量值。本文系统中的测量值有：1)由ZARU获得的δω测量值。2)由ZUPT获得的δv测量值。3)由HDE获得的δφ(3)测量值。4)由气压计数据获得的δp(3)测量值。按这一顺序，写出包含所有量的测量矩阵H

(8)

可知，上述测量值并非在同一时间获得：ZARU需要在完全静止状态下测量，ZUPT与HDE需要在步行中的零速区间测量，气压计数据的更新取决于其采样频率。因此，当进行某一测量时，只需使用矩阵H中对应行的元素，例如在零速区间进行测量时，只需保留H的2,3行即可。

之后，即可利用卡尔曼滤波的基础原理，对系统状态进行估计，这里不再进行赘述。将估计所得的误差值从INS所得量中减去，即可得到系统的最终估计值。

1.4 ZUPT与零速区间检测算法

ZUPT利用行人脚步落地后速度为零的特点，将相邻两步内经由INS计算累积得的空间速度向量作为对系统误差的测量，从而用于对系统状态进行估计。易知，对脚步落地区间的测量准确度直接影响着ZUPT的应用效果，需要对零速区间检测算法进行设计。相对于简单的单传感器阈值法，使用基于广义似然比检验(GLRT)的零速检测算法具有更高的准确度，即定义函数

(9)

1.5 HDE算法

在IEZ框架下，由于重力向量本身无法提供在水平面航向上的参考信息，因此,系统在航向角上的误差会逐渐累积，使得定位结果逐渐偏离正常轨迹。同时，由于室内环境下磁干扰严重，无法有效利用地磁向量作为航向的参考。此时，HDE算法即成为解决上述问题的较好方案。

HDE算法的核心思想来自于一个先验假设，即在楼房等室内环境中，多数走廊之间呈现出互相垂直或夹角45°的规律。将这些方向设置为主方向，则行人在多数时间是沿着主方向行走的。运用这一假设，即可利用行人运动时解算所得的航向，与其邻近的主方向进行对应，从而消除航向角的累积偏差。

同时，由于文献[11]中原始的HDE算法不对运动类型进行区分，在行人作非直线运动时反而会造成更大的误差，因此，本文以文献[11]所提出的iHDE算法为基础进行改进，设计了下述算法。

首先，定义行人第k步的步长lk与方向θk如下

(10)

(11)

式中px,k与py,k分别为由INS获得的第k步的x,y位置。之后，定位2个控制变量，分别用于判断行人的步长与行进方向

(12)

(13)

可见，当单步步长大于阈值，且过去5步内的航向变化小于阈值时，两控制量值为1，这代表着行人正在以正常的步态进行直线行走。只有在这种条件下，才利用θk与主方向之间的差值作为HDE测量，从而在其他不规则运动状态时禁用HDE，避免了可能带来的定位精度下降。

2 实验验证

2.1 行人室内导航系统方案设计

本文面向实际应用需求，设计了完整的行人室内导航系统方案，包括数据采集模块的软硬件系统、上位机软件以及上文所述的系统算法。系统整体架构如图2所示。将数据采集模块安装至行人足部后，对九轴传感器(MARG)以及气压传感器数据进行采集、处理后，通过算法对行人当前空间位置进行估计，并将运算结果通过蓝牙传输至上位机。

图2 行人室内导航系统架构

数据采集模块以STM32 L031G6U6作为MCU，搭载九轴传感器ICM—20948与气压传感器BMP280，使用nRF52832通过BLE5.0与上位机进行数据通信。模块整体尺寸为35 mm×35 mm×10 mm，由锂电池进行供电，实物如图3所示。

图3 数据采集模块实物

2.2 实验设计与结果分析

为了验证上述系统及算法的有效性，通过设定运动路径，进行了多组实验。试验对象足部安装采集模块，以125 Hz采集惯性数据，以25 Hz采集气压数据，利用上述算法进行了轨迹的还原。

1)走廊直线路径实验

试验对象在静止一段时间后，沿着一段长40 m，宽1.8 m的走廊边缘，由一端行走至另一端，再沿另一边缘折返回来至出发线。使用本文设计的算法流程进行运动轨迹还原，所得结果如图4所示。

图4 走廊直线路径实验轨迹

可见，本文所设计算法能够较好地还原出行人运动的轨迹。图4中终点坐标与实际位置相差0.79 m，占总路程的0.96 %。

2)组合路径实验

在实际应用场景中，行人在室内的运动路径是多样的，既包括沿直线走廊的行走，也包括沿曲线行走、上下楼梯、乘坐电梯等。为了验证系统在多种运动路径下的定位功能，设计了组合路径进行实验。最终解算所得运动轨迹如图5所示。实验对象由出发点出发，先搭乘电梯，由6楼下降至1楼，随后沿走廊行进。点A至点B为室外行进路段，由B点进入建筑物内后，从C处楼梯上至6楼，随后通过走廊回到出发点处，即为完整的运动路径。

图5 组合路径实验轨迹

可见，由于将气压计应用于对海拔高度进行测量，轨迹在空间Z轴上的误差极小。经计算，完整的运动距离(total traveles distance,TTD)约330 m，轨迹还原所得起始点间的误差为1.10 m，由此可计算得系统定位误差为0.33 %。

3 结 论

本文针对实际应用中对行人室内导航系统的需求，实现了对系统软硬件及算法的设计和实现。通过将MEMS惯性传感器以及气压计安装至行人足部，利用IEZ框架，结合ZARU、HDE、气压计等多种校正方法，将传感器数据融合以获得对行人空间位置的估计。实验结果表明：系统对行人定位的空间误差在1 %以内，验证了其有效性。