MEMS惯性鞋式行人导航系统*

宋丽君，路明慧，赵思晗，周 彬，王金虎，郑普亮，刘峰秀

(1.西安建筑科技大学 信息与控制工程学院·西安·710055；2.中国科学院深地资源装备技术工程实验室 中国科学院地质与地球物理研究所·北京·100029；3.上海航天控制技术研究所·上海·201109；4.浙江工业大学 机械工程学院·杭州·310023)

0 引 言

行人导航系统(Pedestrian Navigation System，PNS)是一种实时获取行人航向、位置、速度等信息的导航系统，在国民体系各个行业都有广泛应用的前景[1]。目前的行人导航系统是基于GPS定位设备进行设计，容易受到外界环境的干扰，仅适合在室外开阔环境下工作[2]。而基于微电子机械系统(Micro-Electro-Mechanical System，MEMS)惯性器件的行人导航系统具有自主性强、更新率高、导航信息完整、便于在人体上安装与携带等特点，是对卫星定位系统的有效补充，且随着MEMS传感器精度的逐步提高，基于MEMS器件的行人导航系统的精度也将得到提升[3]。

文献[4]提出一种基于磁、角速率和重力传感器的运动感知和步态校正行人航位推算(Pedestrian Dead Reckoning，PDR)方法，但这种方法仅可得到水平面内二维导航结果，无法提供海拔位置变化，适用范围有局限性。文献[5]根据步行者的步态特征，以陀螺输出角速度矢量和、加计输出比力矢量及滑动方差为检测量，其仅将速度信息作为伪量测量设计了Kalman滤波器，虽然可提供三维导航结果，但其未考虑方位误差的存在对导航精度的影响。文献[6]提出了一种基于条件随机场(Conditional Random Field，CRF)的PDR与伪室内规划的融合方法，以解决PDR累积误差和初始位置不确定的问题，但这种方法需提前构建先验数据库，增加了导航的成本和难度。

本文设计了一种基于MEMS惯性测量组件的行人导航算法，将MEMS惯性测量组件安装于人体足部，在足部着地期间通过零速检测以及零速修正算法估计并修正MEMS惯性导航系统及MEMS惯性器件误差。此外，增加磁强计方位信息，以实现方位误差估计。

1 行人导航系统方案设计

1.1 惯性行人导航系统算法结构

基于MEMS器件的PNS导航算法方案采用捷联惯导系统(Strapdown Inertial Navigation System，SINS)算法，可通过六自由度信息解算得出完整导航信息[7]，但由于MEMS器件精度较低，若不能有效修正位置误差，其将以时间的三次方趋势发散，最终失去导航功能[8]。本文采用SINS算法并将PNS附着固定在鞋子上，结合行走者脚部运动特征，设计了一种零速修正(Zero Velocity Update，ZUPT)算法，以进行实时导航修正，提升导航精度。ZUPT算法结构如图1所示。

图1 PNS导航系统修正算法Fig.1 Navigation algorithm structure of PNS

惯导行人系统算法的研究核心在于步态检测和零速修正算法设计，国内外学者设计了不同的步态检测方法和零速修正方法来提高系统的长航时导航精度[9]。

1.2 MEMS器件安装位置分析

在行人导航系统中，惯性传感器被固连在行人足部，且有内、外两种安装方式[10]，图2为MEMS器件的两种安装方式[11-12]。从某种角度来说，作为载体的鞋子也是硬件系统的一部分。

(a) 内置安装

(b) 外置安装图2 惯性传感器安装方式Fig.2 Installation styles of inertial sensor

考虑到脚背部分的表面积大且骨骼突出部分少这一因素，采取将传感器安装在脚背部的方式，具体为图2(b)中的位置2附近。

2 行人导航系统算法设计

2.1 行人步态特征分析

行人正常水平行走的步态是近似于周期性的一个过程，每一个完整的步态周期中均有四个关键的运动学事件[13]：脚跟着地(Heel-Strike，HS)、全脚着地(Foot-Flat，FF)、脚跟离地(Heel-Off，HO)、脚尖离地(Toe-Off，TO)。图3为行人行走的一个周期性过程。

图3 行走步态的关键事件Fig.3 Key events of walking gait

对行人行走过程中的单只脚进行观察，有明显的“运动—停止—运动—……”交替变换的规律，安装在鞋上的MEMS器件可感测到鞋子与地面的触碰和静止。成人的正常行走过程中，支撑相的典型持续时间约为单步运动持续时间的30%，为0.3～0.5s，这一段时间足够用来进行零速修正[14]。鞋式PNS算法充分利用该规律，在静止时间段进行零速修正，消除位置误差随时间的三次方发散。

2.2 零速检测算法设计

MEMS器件量测数据的输出是静止检测算法的基础，该算法主要是基于对加速度或者角速度的统计特性的分析，来判断某一阶段是否为静止状态[15]。本文同时使用加计输出的比力信息和陀螺的角速度信息设计多条件零速检测算法，这种算法只需要使用三轴陀螺仪以及三轴加速度计输出数据便可判别出零速区间，不需要额外的辅助传感器。该检测算法一共利用了三个条件(记为T1，T2与T3)，以下是其详尽的计算和判断方法。

(1)

2)比力方差，脚步静止期间比力方差需小于设定阈值cσ=1×10-5m/s2。

(2)

(3)

(4)

若同时满足这三个判断条件(T1，T2和T3)，则将该种情况记为0，即表示零速检测结果为静止，否则记为1。

多条件算法零速区间识别方法流程图如图4所示。

图4 多条件约束的零速区间检测方法流程图Fig.4 Flowchart of zero-velocity interval detection method with multi-condition constraints

2.3 零速修正卡尔曼滤波器设计

设系统状态空间表达式为

(5)

其中，F(t)代表状态转移矩阵，X(t)代表状态矩阵，W(t)代表输入矩阵，H(t)代表系统观测矩阵，V(t)代表系统观测噪声。

将式(5)离散化并简记为

(6)

卡尔曼滤波算法为[16]

(7)

(8)

(9)

(10)

(11)

式(7)～式(11)构成离散卡尔曼滤波完整算法。

(12)

卡尔曼滤波器量测方程如下所示

Zk=HkXk+Vk

(13)

由于仅采用速度作为量测量难以对方位误差做出有效估计，因此，采用速度误差和磁强计输出作为量测信息

Δψk=ψk-ψmagk

(14)

其中，ψk表示imu输出信息，ψmagk表示磁航向角。

Zk=[Δψk,δv]=HkXk+Vk

(15)

其中，

零速修正卡尔曼滤波器可实现如下三种功能：速度误差修正、水平姿态误差修正以及运动过程中的累积位置误差的修正。

3 硬件平台搭建

3.1 硬件平台搭建

根据功能需求，选用STM32F405作为MEMS行人导航系统核心处理器，九轴惯性测量单元选用MPU9250，气压传感器选用MS5611。绘制的PCB版图采用Cadence Allegro软件四层板设计。电路板与IMU传感器、MS5611模块紧紧贴合在一起，形成了一个尺寸小且质量小的板块。硬件系统总体的框图如图5所示。

图5 硬件总体结构框图Fig.5 Hardware overall structure block diagram

硬件结构以STM32为核心并与IMU完成数据的采集，计算得出导航信息并传输至PC端。为在初始化阶段得到准确度更高的高度通道信息，又借助MS5611气压高度计对初始高度信息进行采集。STM32与计算机的通信这里采用串口转USB有线模式，这种方法主要在实验前期数据采样的阶段应用，为实验验证阶段做准备。

3.2 PCB设计与系统硬件调试

根据上述各器件的原理图，使用Cadence Allegro软件绘制电路以及PCB布局。本文所绘制的PCB版图如图6所示。采用四层板设计，为信号层-电源层-地层-电源层。

(a) 信号层 (b) 电源层

(c) 地层 (d)电源层图6 PCB版图Fig.6 PCB layout

所搭建的硬件系统实物图如图7所示。

图7 硬件系统实物图Fig.7 Hardware system physical diagram

4 实验验证

实验采用的MEMS惯性单元为本文第3章所设计搭建的行人导航硬件系统，其尺寸为3.7cm×3.7cm×0.15cm，其主要由三轴加计、三轴陀螺和三轴磁强计组成。本文实验过程中将MEMS IMU固定在步行者的一只鞋上，数据输出频率为100Hz，通过RS232-USB传送到笔记本电脑，由VC++数据采集界面保存数据，然后将实验采集数据做离线处理。实验过程中将MEMS IMU固定在步行者的一只鞋上，完整行人导航系统如图8所示。

图8 完整行人导航系统Fig.8 Pedestrian navigation system

下面将对本文所设计的零速检测算法检测效果进行实验验证，这里首先对各个检测条件的有效性进行对比。选取行人行走过程中的6步行走数据，然后依次选取2.2节所提及的三种综合零速检测算法中的单一检测条件(即比力模值零速判断方法、比力方差零速判断方法、角速度模值零速判断方法)进行单条件零速检测，检测结果如图9所示。

图9 单一条件静止检测效果Fig.9 The effect of single condition stance phase detection

从图9可以看出，仅使用单一零速检测条件的方法在某一较短时间区间内的纵坐标值在0和1两个数字极端反复跳变，这表明存在较为明显的误检与漏检情况。即将运动状态判断为静止，静止状态判断为运动。需要特别说明，当图9某一时刻纵坐标值为1时检测为运动状态，值为0时为静止状态。

下面给出使用三条件进行零速检测的检测结果，如图10所示。

图10 三条件复合检测结果Fig.10 The effect of three-condition composite detection

从图10可以明显看出，纵坐标值在0和1两个数字极端的跳变状态基本符合所选取的6步运动数据的实际情况，这表明同时使用比力与角速度信息的多条件检测方法提高了检测结果的准确度。同样地，图10某一时刻纵坐标值为1时检测为运动状态，值为0时为静止状态。

基于第2节所设计的PNS算法，以第3节搭建的行人导航系统作为载体进行直线行走实验。实验地点为西安建筑科技大学南山操场，将行人导航系统安装至步行者脚背部位置，并通过RS232-USB与PC端连接进行实验数据的保存与采集，行人沿正北方进行行走实验，行走时间为5min。图11是进行修正导航并结合磁力计航向信息进行修订之后所得的解算结果，以及未进行修正的惯导解算结果对比图。

(a) 直线行走姿态解算结果

(b) 直线行走速度解算结果

(c) 直线行走位置解算结果图11 行走实验修正算法解算结果及纯惯导解算结果对比图Fig.11 Comparison diagram between the results of walking experiment modified algorithm and pure inertial navigation

分析图11可知，行人直线向正北方向行走时，由修正算法解算出的俯仰角和横滚角符合行人运动规律，以磁力计作为观测量的航向角也与实际情况相符，300s行走时间内经纬度变化与实际情况相符合，解算高度最大值约为0.2m，符合人体行走正常抬脚变化值。而在没有引入修正算法的情况下，300s行走时间内，纯惯性导航解算速度结果严重发散，并且其经度、纬度以及高度变化与实际情况相差较大，MEMS IMU已完全失去导航功能。因此可得出如下结论：本文所设计的导航算法可有效且正确地用于MEMS行人导航系统中，并使系统的导航精度得到较大提升。

5 结 论

以地图匹配作为辅助的卫星导航是当前较为常用的导航系统，但在卫星拒止环境中其难以完成导航任务，为一种非自主式的导航方法。因此，本文展开了基于MEMS惯性器件的鞋式行人导航系统的研究工作，该系统无需提前布置基础设施，仅依靠行人自身固联传感器进行自主导航。将MEMS IMU固定在步行者的鞋上，根据步行者脚步运动特征，采用SINS与PNS结合的零速修正算法，同时，增加磁力计方位信息进行了MEMS IMU实验验证。零速检测算法检验效果实验表明本文所设计的复合条件检测方法与比力模值零速判断方法、比力方差零速判断方法、角速度模值零速判断方法等单一条件静止检测算法相比，提高了检测结果的准确度。进行的半实物实验验证最终实验结果表明，本文所设计的鞋式行人导航算法行之有效，系统可以满足行人导航需求。这对GPS缺失环境下的人体定位具有十分重要的意义，并为鞋式行人导航系统的工程化奠定了重要的理论与实践基础。