基于行为概率分析的室内行人导航方法

吕 品，朱静漪，赖际舟，袁 诚，王鹏宇

（1.南京航空航天大学自动化学院 导航研究中心，南京 211106；2.北京自动化控制设备研究所，北京 100074）

随着科技发展水平的提高，人们对于位置服务[1]的需求愈发凸显。然而，基于卫星的导航定位系统[2]仅能在室外空旷环境实现高精度、长待机的稳定导航服务，面对室内复杂环境时则会有失效的可能[3]。

针对室内环境，目前实现行人导航的方案主要包含无线定位技术[4]和惯性导航技术[5,6]两种。无线定位技术即在室内场景下部署无线通信技术[7]（Wireless Fidelity,Wi-Fi）、蓝牙[8]、超宽带[9]等设施进行环境信息测量，从而实现行人导航定位。与前者相比，惯性导航技术是一种完全自主式的导航定位解决方案。随着微机电系统（Micro-Electro-Mechanical System,MEMS）[10]的快速发展，行人惯性导航系统主要采用MEMS 惯性传感器件，该器件具有体积小、功耗低、价格低廉等优势[11]。但MEMS 器件精度较低，在进行惯性数据采集的同时引入了大量噪声。为了抑制行人导航系统长时间连续积分过程中导航误差的发散，目前通常采用零速修正方法[12]。行人在室内场景行走时，通常包括走路、跑步、上下楼等不同运动状态，但由于不同运动状态的动态特性不同，固定零速阈值误判率较高。因此，如何针对各类运动状态提取准确的零速区间进行误差修正，成为影响导航定位精度的关键。

针对该问题，目前较多研究采用基于经验阈值法和机器学习方法。经验阈值法对不同运动状态设置相应的零速判断阈值，如雷明等[13]对站立、走路、跑步、上楼和下楼等五种主要运动模式设置相应的零速判断阈值。田晓春等[14]对足部传感器输出特征设置多条件阈值进行约束。Zhang R 等[15]通过提取胸部加速度信息增加零速检测的相应阈值判断条件。经验阈值法的阈值设置对经验依赖性强，且对不同行人、不同步态的适应性有限，易出现“误检”、“漏检”现象。机器学习方法通过设计模型判断零速状态，如纪志农等[16]采用支持向量机对不同步态下的零速点和非零速点进行分类，实现零速区间的提取。戴洪德等[17]对运动状态进行分类，采用改进的K-menas 聚类方法划分零速区间。机器学习方法虽然鲁棒性较好，但需要依赖大量数据支持，且计算复杂度较高。

此外，行人在室内行走时通常包含上下楼梯场景，此时惯性导航系统的解算高度会快速发散，因此需要对行人在爬楼阶段进行高度修正。一些研究利用室内高度的先验信息对行人高度进行校正，如刘宇等[18]预先对半层楼高度进行测量，在行人平走状态时将高度修正至半层楼高的整数倍。采用先验信息的方法主要适用于已知合作场景，而在陌生场景中，较多采用气压高度计对高度进行校正。Yang W 等[19]根据前后两步的高度差判断行人运动状态，在上下楼状态融合气压计信息进行高度修正。然而，当长时间采用气压高度计进行辅助时，其高度误差会产生较大漂移。

针对上述问题，本文提出了一种基于行为概率分析的室内行人导航方法。一方面，采用基于广义似然比检验的零速区间检测器对零速区间进行检测，通过区间约束和周期划分得到每一步的步态区间。在各步态区间内提取零速检测值最小的时刻，作为每一步的最大概率零速点并进行导航误差修正。另一方面，针对上下楼状态下惯导高度误差发散的问题，本文提出了一种基于惯性/气压传感器的爬楼高度在线估计方法，对爬楼台阶数、台阶高度进行估计。本文方法基于最大概率提取零速点，对不同步态适应性强，且降低了不稳定的气压计输出对高度修正的影响。通过实验验证，该算法能够在室内场景下实现较高的定位精度。

1 算法总体框架

本文提出的行人导航方法总体框架如图1 所示，主要包含零速检测、上/下楼高度估计、扩展卡尔曼滤波（Extended Kalman filter,EKF）误差修正等部分。

图1 算法总体框架Fig.1 Overall algorithm framework

本文采用基于最大概率的零速点提取方法实现各步态周期内的零速检测，提出一种惯性/气压联合楼梯检测方法实现上下楼运动台阶数的检测，并基于楼梯高度不变的假设实现高度估计，同时提出了基于EKF的高度后处理优化方法。采用EKF 进行融合，在每个步态的最大概率零速点时刻对速度和高度进行误差修正，解算导航定位信息。

零速检测方面，本文首先采用基于广义似然比（Generalized Likelihood Ratio Τest,GLRΤ）的零速检测方法对各运动类型的零速检测值进行初步分析。根据行人足部运动周期性对步态区间进行约束，实现步态划分，并在每一个步态内提取最大概率零速点。

高度估计方面，本文首先搭建长短期记忆网络[20]（Long Short-Τerm Memory,LSΤM）实现上下楼的运动识别，结合气压高度计信息计算台阶平均高度，并提出了一种基于惯性/气压传感器的爬楼高度在线估计方法，实现上下楼阶段的高度估计。

2 基于步态检测的最大概率零速点提取方法

本文提出了一种基于步态检测的最大概率零速点提取方法，流程图如图2 所示。

图2 最大概率零速点提取方法流程图Fig.2 Flow chart of maximum probability zero speed point extraction method

为了提高零速修正算法的鲁棒性，本文首先采用GLRΤ 方法进行零速检测，并基于区间分析方法对零速检测的结果进行划分，得到每一步的步态区间。同时，本文提出了一种基于最大概率的零速检测方法，在每一个步态区间内搜索最大概率零速点，实现跑步等动态较大情况下的零速修正。

2.1 基于GLRΤ 的零速区间检测方法

行人运动过程中的步态周期可划分为运动阶段和触地阶段。在运动阶段，足部开始离开地面并在空中摆动向前迈进；在触地阶段，足部完全接触地面，足部和地面保持一段时间的相对静止，此段时间即为零速区间。为了抑制惯性行人导航系统误差的发散，可以在零速区间对误差进行修正。

在实际的室内运动场景中，行人不同运动的足部动态大小存在差异。为了提高零速检测方法的适应性，本文首先采用基于GLRΤ 的零速区间判断方法，分析行人在走路、跑步和上下楼运动状态下的零速检测值。

GLRΤ 综合考虑加速度和角速度信息，根据式(1)计算k时刻窗口内的检测值。通过比较检测值与阈值来判断当前样本是否处于零速区间。

其中，Tk为k时刻零速检测值；W为窗口大小；、分别为陀螺仪和加速度计的噪声方差；ωk、fk分别为k时刻的陀螺仪和加速度计输出值；g为当地重力加速度；为该窗口内三轴加速度均值。

当检测值小于零速检测器阈值T时，可以认为k时刻的样本数据处于零速区间，此时零速判断值zk为真，如式(2)所示。当行人运动包含多种运动状态时，设置合适的检测器阈值，以检测出各状态下零速点可能存在的区间。考虑到跑步动态较大，此处的零速检测器阈值应该设置得较大一些。

由于在行人不同运动状态中传感器动态输出和零速区间长度存在差异，传统方法在进行零速检测时需要针对不同运动状态调整合适的零速阈值，并在零速区间内进行零速修正。为了减小由量测信息不准引入的误差量，本文在每个步态周期内仅在最大概率零速点处进行一次系统误差修正。基于GLRΤ 划分零速区间，采用基于区间分析的方法对零速区间进行约束，完成步态周期的划分，进而实现在每一个步态周期内提取最大概率零速点。

2.2 基于零速区间分析的步态周期划分方法

综合考虑走路、跑步、上下楼几种运动情况对样本进行分析，首先，进行步态区间长度的约束。根据基于区间分析的步态划分方法，认为每个步态中的非零速区间长度不小于0.12 s，且一个步态的区间长度不小于0.6 s。计算非零速区间长度，若长度小于0.12 s则认为出现了零速点误判，此时实际上的零速点被误判为非零速，将后一段区间划分为零速区间。然后，根据行人运动时足部运动的周期性规律，以每一段零速区间的开始时刻为起点，以下一段非零速区间结束时刻为终点，将其视为一段完整的步态区间。步态区间划分示意图如图3 所示。

图3 步态区间划分示意图Fig.3 Gait interval division diagram

2.3 基于最大概率的零速点提取方法

传统零速修正方法在检测出来的所有零速点上均对误差进行修正，但在对量测和状态进行融合的同时也引入了大量的误差，从而影响导航系统性能。

本文在进行步态划分时，已经基于GLRΤ 实现了零速区间的识别。在一个步态周期中，足部速度为零的最大概率点即在零速检测器检测值最小值点附近，因此，在每个步态的零速区间内可以进一步提取最大概率零速点，认为该时刻足跟完全接触地面，并在下一时刻开始离开地面进入摆动期。

根据式(1)，检测值Tk越小则表示k时刻为零速的概率越大，因此，只要在已获得的零速区间内寻找最小检测值即可提取最大概率零速点，如图4 所示。

图4 最大概率零速点提取示意图Fig.4 Maximum probability zero speed point extraction diagram

与传统的零速修正算法相比，该方法在每个步态周期内仅在最大概率零速点进行一次误差修正，简化了滤波器计算量，同时消除了由于运动状态不同导致的零速误判、漏判情况，提高了导航系统的鲁棒性。

3 基于LSTM 的上下楼运动检测方法

本文采用基于LSΤM 的深度学习网络模型实现上下楼运动检测，模型的输入为66 维特征，样本标签为四种。该深度学习网络模型由输入层、LSΤM 层、全连接层、softmax 层和输出层构成，包含64 个隐藏节点，采用Adam 优化函数，最大迭代周期为100 次。

考虑性别、身高等因素，本实验共组织15 位人员进行数据采集，其中男性7 名、女性8 名，身高范围为1.56～1.82 m，体重范围为52～75 kg。数据采集过程中，实验人员分别以走路、跑步、上楼、下楼步态进行运动。

实验过程中采用蓝牙无线传输数据，采集频率为200 Hz，分别记录三轴加速度、三轴角速度和气压高度信息。共采集数据样本3601 份，各运动状态样本数量如表1 所示。

表1 数据集样本数量Tab.1 Sample number of dataset

经过该模型训练，运动状态识别准确率达98.6%，混淆矩阵如图5 所示。

图5 步态识别混淆矩阵Fig.5 Confusion matrix of gait recognition

4 基于楼梯阶数概率检测的行人爬楼高度估计方法

目前，针对行人室内导航环境的高度修正方法主要是采用气压高度计作为高度参考信息，但气压高度计容易受到温度、湿度等环境因素影响，同时随着行人运动幅度的变化，行人运动时气压高度计的输出值存在较大波动。

针对该问题，本文在行人同楼层运动状态下不参考气压高度计信息，将前一步的高度作为参考进行高度修正。由于建筑物内台阶高度通常是一致的，因此本文在进行行人上下楼高度估计时假设建筑物内台阶高度不变。上下楼运动的高度估计算法具体流程如图6 所示。

图6 上下楼运动的高度估计算法流程图Fig.6 Flow chart of height estimation algorithm for ascending and descending movements

4.1 惯性/气压联合楼梯检测

为了实现行人上下楼步态跨越台阶阶数的判断，本文结合惯性解算高度信息和气压高度计的信息进行分析，算法流程图如图7 所示。在每个上下楼步态的最大概率零速点时刻，截取该步态区间内的惯性解算高度数据和气压高度计数据。设置合适的窗口大小和步长，采用滑动窗口遍历步态数据。

图7 惯性/气压联合楼梯检测算法流程图Fig.7 Flow chart of inertial/pneumatic stair detection algorithm

首先，在每个滑动窗口内分别对惯性解算高度和气压高度数据提取差分特征值，继而计算惯性解算高度差分的能量函数和气压高度差分的能量函数。第k个步态的能量函数的计算方法如式(3)所示。

其中，z k为第k个步态的最大概率零速点；di,m、dp,m分别为m时刻惯性解算高度和气压高度差分值。式(3)的两个能量函数分别反映了当前数据窗口中惯性解算高度和气压高度变化的程度。

其次，根据预先设定的阈值对能量函数进行判断。由于上楼运动和下楼运动的动态特性不同，需要分别设置相应的能量函数阈值t1、t2。如果惯性解算高度差分和气压高度差分的能量函数均低于设定的阈值，则判断当前窗口步态片段未跨越台阶。反之则当前窗口步态片段跨越了一个台阶。

最后，对当前步态的所有窗口的台阶跨越情况进行统计，得到行人上下楼步态中跨越台阶阶数。

4.2 上下楼高度估计

为了减小气压高度计输出不准确对高度带来的影响，结合运动状态识别结果，本文仅在第一段上下楼运动时参考气压高度计高度信息并计算台阶平均高度。基于台阶高度不变假设，在后续上下楼运动时参考台阶高度估计信息进行高度修正。

台阶高度估计量hs初始值为初始上下楼运动基于气压高度计计算的台阶平均高度，如式(4)所示。

其中，Δhp1为第一段上下楼运动的气压高度计高度差；ni为第i步跨越的台阶个数；p、q分别为第一段上下楼的起始和结束步态。

后续上下楼步态的高度变化量如式(5)所示。

为了进一步提高上下楼高度估计的准确性，本文提出了基于EKF 的后处理高度优化方法。在两个楼梯平台之间的上下楼运动过程中，分别计算各步态区间内高度变化量Δhk与惯性解算高度变化量Δhi,k的差值，寻找差值最小时的步态序号j。该段上下楼运动结束后，将台阶高度hs修正为此时基于惯性解算的台阶高度估计量Δhs,i，实现台阶高度估计的优化。

基于惯性解算的台阶高度估计量Δhs,i如式(6)所示。

其中，nj为第j步跨越的台阶个数；Δhi,j为第j步的惯性解算高度变化量。

在下一段上下楼步态高度修正中，采用更新后的台阶估计高度hs。

5 基于EKF 的误差修正算法

5.1 EKF 融合算法

本文采用EKF 算法对九维状态误差进行估计，取状态量如式(7)所示。

其中，Xk为k时刻的系统状态量；Pk、Vk、ψk分别为系统三轴位置、速度、姿态角。

取系统量测量Zk以观测k时刻的速度和高度。

其中，Zv,k为k时刻系统三轴速度理论值与实际值的差值；Zp,k为k时刻高度理论值与实际值的差值。

根据卡尔曼滤波的状态空间模型，状态量Xk和量测量Zk满足式(9)。

其中，W为系统噪声矩阵，R为量测噪声矩阵。

F为系统矩阵：

其中，St为载体系加速度计在导航系投影的斜对称矩阵；0i×j为i×j的零矩阵；Ii×j为i×j的单位矩阵。

H为量测矩阵：

其中，h21=[0 0 1]。

5.2 速度修正

为了使误差修正时刻更加准确，本文只在每个步态的最大概率零速点处进行误差修正。该时刻速度理论值为零，则速度修正量Zv,k为速度理论值与实际值的差值，如式(13)所示。

其中，Vx,k、Vy,k、Vz,k分别为k时刻速度三轴分量。

5.3 高度修正

根据基于步态类型约束的高度修正方案，对当前步态运动类型进行判断。高度修正值Zp,k满足式(13)。

其中，hi,k为k时刻最大概率零速点系统解算高度；hp,k为k时刻最大概率零速点气压高度计高度；lk为当前上下楼步态跨越的台阶个数。

fk为当前步态上下楼标志：

6 实验结果和分析

6.1 实验设备和场景介绍

本文实验采用ForSense IMU614 MEMS-IMU 六轴传感器，并将该传感器固定于实验人员右足鞋垫内足跟位置，如图8 所示。

图8 实验设备与安装位置Fig.8 Experimental equipment and installation position

该MEMS-IMU 传感器内嵌三轴加速度计、三轴陀螺仪，性能参数如表2 所示。气压高度计内嵌于MEMS-IMU 传感器中，性能参数如表3 所示。

表2 ForSense IMU614 传感器相关参数Tab.2 ForSense IMU614 sensor related parameters

表3 气压高度计相关参数Tab.3 Barometric altimeter related parameters

实验地点位于南京航空航天大学自动化学院一号楼三楼至五楼，楼层平面图如图9 所示。单一步态场景下，实验人员于起点A 处出发，以走路姿态沿A-B-C-D-A 绕行两圈后回到起点A。混合步态场景下，实验路径1：实验人员于五楼楼梯口起点A 处出发，沿A-B 方向走路行进至B 处，随即下两层楼梯至三楼楼梯口B 位置，以跑步姿态沿B-A 方向至三楼楼梯口A 处，最后上两层楼梯回到终点A；实验路径2：实验人员于五楼楼梯口起点A 处出发，沿A-B-C-D-A走路行进一圈后回到起点A，随即下两层楼梯至三楼楼梯口位置A，同样以跑步姿态沿A-B-C-D-A 进行一圈后回到三楼楼梯口A 位置，最后上两层楼梯回到终点A。

图9 楼层平面图与实验路径Fig.9 Floor plan and experimental path

6.2 最大概率零速点误差修正算法验证与分析

为了测试本文零速检测方法的导航定位效果，分别采用Openshoe 方法和AdaptiveOpenshoe 方法进行对比实验。Openshoe 方法采用固定零速检测阈值，而AdaptiveOpenshoe 方法在前者的基础上实现了零速检测阈值的自适应调整。

6.2.1 零速检测方法对比结果

三种零速检测算法在行人各运动状态下的零速检测结果对比如图10 所示。

图10 各状态下三种算法零速检测结果对比Fig.10 Comparison of zero speed detection results of three algorithms in different state

为验证最大概率零速检测方法的适用性，针对不同人员采集的实验数据，三种零速检测算法的检测情况如表4 所示。

表4 三种算法步态检测情况对比Tab.4 Comparison of gait detection by three algorithms

由图10 和表4 可知，在行人走路状态下，三种零速检测方法实现零速点提取的成功率均较高；在行人上下楼状态下，Openshoe 算法和AdaptiveOpenshoe算法零速点提取成功率有所降低；在行人跑步状态下，Openshoe 算法成功率仅有11.36%，AdaptiveOpenshoe算法与前者相比零速检测成功率有所提升。对三种零速检测算法的零速点提取平均成功率进行分析可得,最大概率零速修正算法在四种行人运动状态下实现零速点提取的成功率最高，与前两种零速检测算法相比，平均成功率分别提高了24.44%和16.21%。

6.2.2 定位结果对比

1）单一步态场景

单一步态下三种算法导航定位结果和定位误差如图11 和表5 所示。

表5 单一步态下三种算法定位误差对比Tab.5 Comparison of positioning errors of three algorithms under a single gait

图11 单一步态下三种零速检测算法定位结果对比Fig.11 Comparison of positioning results of three zero speed detection algorithms under a single gait

2）混合步态场景

混合步态下三种算法导航定位结果如图12 所示。

图12 混合步态下三种零速检测算法定位结果对比Fig.12 Comparison of positioning results of three zero speed detection algorithms under mixed gait

为了更好地对定位结果进行分析，本文采用导航轨迹的首尾误差与总距离的比值作为精度指标。三种算法的定位误差和定位精度对比如表6-7 所示。

表6 混合步态下三种算法定位误差对比Tab.6 Comparison of positioning errors of three algorithms under mixed gait

表7 混合步态下三种算法平均定位精度对比Tab.7 Comparison of the average positioning errors of three algorithms under mixed gait

表8 高度修正后的两种算法高度误差对比Tab.8 Comparison of height errors of two algorithms after height correction

由图12 和表6-7 可知，由于Openshoe 算法在跑步阶段几乎无法检测出零速时刻，定位误差发散更加明显；AdaptiveOpenshoe 算法由于能够自适应调整零速阈值，一定程度上可以减小定位误差的发散；最大概率零速点修正算法由于可以在所有步态周期内实现零速点提取，定位精度与前两者相比有所提高。与前两种算法相比，所提算法平均定位性能分别提高了55.14%和39.15%。

6.3 高度修正后的导航定位算法验证与分析

在最大概率零速点速度误差修正的基础上，本文分别采用两种高度修正方法进行对比。其中，采用文献[20]的高度修正算法作为对照，该方法在上下楼运动时融合气压高度计输出进行高度修正，在同楼层运动时参考前一步态高度进行高度修正。两种算法的导航定位结果对比如图13 所示。

图13 高度修正后的两种算法定位结果对比Fig.13 Comparison of positioning results of two algorithms after height correction

由图14 可知，基于气压高度计的高度修正算法在上下楼梯时受噪声影响，高度随气压高度计输出波动；本文提出的高度修正方法只在第一段下楼时段参考气压高度计输出，在后续上下楼步态中采用基于惯性/气压传感器的爬楼高度在线估计方法,每一层楼梯平台高度均保持一致，减小了高度误差。

图14 高度修正后的两种算法高度对比Fig.14 Height comparison between two algorithms after height correction

本文以最终高度误差与楼层总高度的比值作为高度估计精度，计算公式如式(16)所示：

其中，hend为最终高度误差；htotal为楼层总高度。

经测量，本实验场景下两层楼的总高度为7.7 m，相较于基于气压高度计的高度修正算法，所提算法的高度估计精度由7.79%降低到0.39%。

7 结论

本文提出了一种室内场景下的惯性行人导航方法，根据广义似然比检测器识别零速区间，实现行人步态周期划分，并在每个步态周期内提取最大概率零速点以实现零速修正；采用长短期记忆深度学习网络进行上下楼步态识别，采用基于概率的楼梯检测方法检测楼梯高度，通过联合使用惯性测量单元和气压高度计的信息判断行人上下楼步态中的台阶跨越情况，降低了由于气压高度计输出不稳定引入的高度误差。实验结果表明本文算法能有效提升室内场景行人导航的定位精度，具有一定的应用价值。