一种用于足绑式行人惯性导航的区间搜索零速检测器

陈 泽，潘献飞，陈昶昊，吴美平

（国防科技大学 智能科学学院自动化系，长沙 410073）

行人导航技术具有重要的应用价值，是当前研究的热点。GPS、UWB[1]、WIFI[2]等技术可以为用户提供导航定位信息。但是这类方法需要提前对目标场景布置，并且很容易受到外界环境干扰。基于低成本MIMU（Miniature Inertial Measurement Unit）的行人惯性导航系统不需要提前对室内环境进行测量，不易受到外部环境影响[3-5]。但是MIMU 精度低，定位误差会随着时间迅速累积，因此惯性行人导航系统需要通过其他约束条件对误差进行修正。零速信息可以对导航误差进行修正[6-8]：利用脚部运动的零速状态，通过卡尔曼滤波对导航误差进行估计，可以很好修正导航误差。

实现零速修正的前提是准确判断出脚部的零速状态，基于阈值的检测法和基于机器学习的检测法是目前主要的两种方法。基于阈值的方法包括固定阈值法和自适应阈值法，固定阈值法如SHOE 检测器[8,9]，SHOE 是目前固定阈值法中检测性能较好的方法，然而固定阈值法在工程应用中有较大的缺陷，需要根据实际情况调节阈值，算法鲁棒性较差。自适应阈值法主要是建立自适应阈值调节模型，文献[9]基于步态频率和文献[10]基于步态速度分别建立了对应的自适应阈值调节模型，虽然文中的效果都很好，然而实际中通用模型很难建立。机器学习的检测方法不需要阈值，文献[11]通过贝叶斯估计判断零速状态，设计了SHOE的自适应版本：Adaptive-SHOE，在正常行走下效果较好，跑步等运动并没进行验证。文献[12]通过深度学习的方法，首先设计了运动分类器，针对不同运动，通过深度学习模型直接判断惯性数据是否为零速状态，取得了较好效果，深度学习的方法需要大量数据进行训练，前期数据准备工作量大，并且深度学习会出现过拟合的问题，该模型在其他数据上的可用性有待验证。

本文提出了一种基于区间搜索的自适应零速检测方法。利用脚部运动中零速区间惯性数据分布的特征，对数据差异化放大。以数据收敛性为依据，搜索到最优的零速区间。该方法立足惯性数据本身的特点，避免了阈值调节和运动分类，可以准确找到零速区间，提高了零速修正算法在工程应用上的鲁棒性。

1 零速修正算法分析

1.1 零速修正算法框架

如图1 所示，零速修正算法框架。MIMU 测量得到加速度和角速度，通过捷联惯导算法解算得到位置、速度、姿态信息，通过零速检测器检测到零速状态。利用检测到的零速状态，通过卡尔曼滤波方法对捷联惯导算法计算的位置、速度、姿态进行修正。

图1 零速修正算法框架Fig.1 The framework of ZUPT

零速修正算法中，捷联惯导算法和卡尔曼滤波算法是很成熟的算法，只要零速检测正确，就能很好的修正导航误差。因此零速检测器的性能是决定导航精度的主要因素。

1.2 脚部运动分析

脚部运动是一个不断“运动-停止”的周期性运动，如图2 所示，在行走运动下，脚部运动周期主要分为步态支撑期和步态摆动期，步态支撑期就是“停止”阶段，即零速区间[13]。

图2 行走步态Fig.2 The gait of walking

将脚部运动周期扩展为“运动-停止-运动”，以此作为一个搜索单元。“停止”部分与“运动”部分在惯性数据上有着显著差别。在“停止”部分中，MIMU 测量的加速度值与重力值近似，角速度值近似为零；在“运动”部分中，MIMU 测量的加速度值与重力值相差较大并且波动较大，角速度值不为零且波动较大：

式中，SC是一个搜索单元，Ps是“停止”部分，Pm1和Pm2是两个“运动”部分，M A和MG分别表示在Ps中所有测量的加速度和角速度的平均值。Pm1、Ps、Pm2是有序分布的，即在一个搜索单元中，中间部分是零速区间，两端部分是非零速区间。根据惯性数据在搜索单元内中“静止”和“运动”部分不同的分布特性，可以将零速区间从中分离出来。

2 区间搜索零速检测器

区间搜索零速检测器算法流程如图3 所示，首先利用角速度确定基本的搜索单元，同时将加速度通过非线性函数映射到搜索空间中，在搜索空间中分别通过粗搜索和精搜索确定一个搜索单元中的零速区间。区间搜索法包括搜索区间确定、搜索空间映射、零速区间搜索三个部分。

图3 算法流程图Fig.3 The flow diagram of the algorithm

2.1 搜索单元确定

“运动-停止-运动”的基本搜索单元在脚部运动中是一个步态周期。采用传统的滑动窗口平均方法，对角速度进行滤波处理，通过波峰确定一个搜索单元。

首先计算出MIMU 测量得到的角速度幅值ωk：

式中，ω k为第k时刻的角速度幅值，分别是第k时刻的陀螺仪的角速度测量值。

采用滑动窗口平均的方法对ωk进行滤波：

式中，w是滑动窗口的大小，w取值为MIMU 采样频率的一半，是滑动平均后的角速度幅值。

如图4 所示，绿色线条为原始角速度幅值，蓝色线条为滑动平均滤波后的角速度信号。蓝线上红色峰值点对应到原始角速度幅值上，两个相邻峰值点之间就是一个搜索单元。

图4 搜索单元Fig.4 The search unit

2.2 搜索空间映射

在一个搜索单元中，区分零速区间和非零速区间要将两者的差异进行放大。本文设计了一种非线性函数，将搜索单元中的加速度幅值映射到搜索空间中。在搜索空间中，零速区间内的数据保持平稳，非零速区间内的数据会与零速区间内的数据产生显著差异。

假设g是当地重力加速度幅值，ackc是第k时刻的合加速度幅值，ratiok是acck与g的比值：

从式(4)可以看出，在零速区间内ratiok应当接近于1。因此需要一种非线性函数，使零速区间和非零速区间的差异放大。采用非线性函数为：

式(5)中，f′（x）是f(x)的导函数。当x∈(0,1)时，f′（x）恒小于0；当x∈ (1, ∞)时，f′（x）恒大于0。因此，x=1是f(x)的最小值点。当x离1 越远时，f(x)的值会迅速增大。这就意味着当acck在零速区间时，f（ratiok）会近似为1，并且波动很小；当acck在非零速区间时，f（ratiok）会远大于1，并且波动很大。

如图5 所示，选取“走”和“跑”两种典型的脚部运动下的加速度数据。图5(a)是在“走”的情况下，加速度幅值在一个搜索单元的分布，将其映射到搜索空间中，图5(b)是零速区间在搜索空间中的结果，可以看出零速区间内的数据平稳且基本收敛在1附近。图5(c)是“跑”的情况下，加速度幅值在一个搜索单元内的分布，将其映射到搜索空间中，图5(d)是零速区间在搜索空间中的结果，可以看出零速区间内的数据平稳且基本收敛在1 附近。由于“跑”运动剧烈，加速度变化剧烈，导致在搜索空间中零速区间和非零速区间差异性极大。根据以上分析以及搜索空间中数据的收敛性可以搜索到零速区间。

图5 走和跑情况下的加速度幅值的分布Fig.5 The distribution of the acceleration magnitude in the case of walking and running

2.3 零速区间搜索

本文设计一种分层迭代的方式，在搜索空间中对零速区间进行搜索。首先采用粗搜索的方式，确定零速区间的近似范围。然后采用精搜索的方式，以数据收敛性为基准，对零速区间进行高精度的确定。通过以上两步，搜索到最优的零速区间。

粗搜索的核心是剔除那些绝对不可能在零速区间的点，非零速点的加速度应当与重力值之间有较大的差异。首先需要对MIMU 静态情况下的加速度幅值进行统计，获取初始静态对准阶段的MIMU 惯导数据：

式中，是静态情况下k时刻的加速度幅值，biask是k时刻的加速度幅值与重力值的偏差，PMax是在静态下标定的最大加速度测量误差百分比，与所用MIMU 性能相关。在粗搜索中，放宽测量误差，将其扩大一个数量级，得到粗搜索的基准值VMax：

在搜索空间中的点只要大于VMax就剔除，小于等于VMax的点就留下来进行下一步的精搜索。

图6(a)对应图5(a)的数据，对“走”的搜索单元经过粗搜索之后，可以得到三个区间。通过图5(a)可知，精确的零速区间在interval2 中，在精搜索中对其进行剔除，从而得到精确的零速区间。采用同样的方法对“跑”的情况进行处理，图6(b)对应图5(c)的数据，粗搜索得到两个区间，通过图5(c)所标的零速区间，精确的零速区间在interval2 中，在精搜索中对其进行剔除，从而得到精确的零速区间。

精搜索的核心是要找到最符合零速区间数据分布的最优区间。在搜索空间中，零速区间的数学期望理论值为1。基于这一前提，在搜索空间中如果存在一个区间，该区间的数学期望与1 近似，同时如果舍去区间中任意的点对区间的数学期望影响不大，则可以认为该区间是零速区间。根据运动数据特点，非零速点应当在区间的两端，在搜索空间中比较两端点的大小，将值大的点作为可能的非零速点进行剔除。基于以上的模型，剔除非零速点前区间的数学期望Ep，剔除非零速点后区间的数学期望为Ec，再将剔除前后数学期望的比值通过f(x) 映射到搜索空间中。经过多次迭代f（E c/g）和f（E cEp）应当收敛在1 附近：

式中，ε是一个小值。

精搜索结果如图7 所示。其中图7(a)和7(b)分别是f（E c/g）和f（E cEp）。在“走”的情况下零速区间搜索收敛情况，可以看出基本都收敛于1。图7(c)中绿色是最终搜索到的零速区间。图7(d)和7(e)是“跑”情况下的数据收敛情况，也同样最终基本收敛于1。图7(f)中绿色是最终搜索到的零速区间。

图6 粗搜索结果Fig.6 The result of rough search

图7 精搜索结果Fig.7 The result of fine search

3 实验验证与分析

实验所用MIMU 如图8 所示，MIMU 为自主设计加工，加速度计零偏稳定性约0.05 mg，陀螺零偏稳定性约10 °/h，采样频率400 Hz。为了验证算法的有效性，采用固定阈值法SHOE 和自适应Adaptive-SHOE进行比较，对一般性的行走和复杂情况下走、跑等混合运动进行实验。

图8 实验所用MIMUFig.8 The MIMU used in the experiments

3.1 实验一：正常行走

2020年9月23日，在户外开阔场地随机行走，实验人员携带u-blox-NEO 模块获取GPS 信号，通过事后差分获得基准轨迹，差分精度可以在10 cm 以内。运动的起点与终点为同一点。行走时间约680.34 s，行走距离约503.65 m。

如图9 所示，黑色线条是差分GPS 获得的基准轨迹，绿色线条是区间搜索法解算的轨迹，红色线条是SHOE-1e5(1e5是SHOE 的阈值，无单位)解算的轨迹，蓝色线条是Adaptive-SHOE 解算的轨迹，三种方法解算的结果基本与差分GPS 轨迹一致。SHOE 的结果是需要经过参数调节后才能得到，如图10 所示，如果选取的阈值不当，导航误差就不能被很好修正，这就是固定阈值法在工程应用中的缺陷。

图9 实验1 解算轨迹Fig.9 The trajectory calculated in the experiment 1

统计结果如表1 所示，实验中起点和终点为同一点，SHOE-1e5、Adaptive-SHOE 和区间搜索法对应的起点/终点误差分别为0.65 m、1.01 m 和0.72 m。通过与差分GPS 解算的结果比较，计算得到对应的RMS值分别为1.76 m、2.23 m 和1.39 m。区间搜索法在导航解算精度上与SHOE-1e5和Adaptive-SHOE 的性能相当。

图10 不同阈值解算轨迹Fig.10 The trajectory calculated by different thresholds

表1 实验一结果Tab.1 The result of the experiment 1

表1 中的零速数量表示判定为零速状态的IMU 帧数。定义零速匹配度，例如：SHOE-1e5检测的零速数量为177501 帧，区间搜索法检测的零速数量为179296帧，两种方法检测到相同的零速数量为173134 帧，则其匹配度约为97.54%(173134/177501)。如表2 所示，区间搜索法同Adaptive-SHOE 的匹配度为98.51%。

表2 实验一匹配度Tab.2 The matching degree of the experiment 1

实验一中，在实验者正常行走情况下，区间搜索法的性能与SHOE-1e5和Adaptive-SHOE 相当。区间搜索法和Adaptive-SHOE 可以自适应找到零速区间，而SHOE 需要对阈值进行调节从而找到最合适的阈值。实验一表明，在正常行走的情况下，区间搜索法可以在不调节参数的情况下实现零速的准确检测。

3.2 实验二：复杂运动

2020年9月3日，在室内走廊中沿着同一条路线进行多种运动综合实验。运动包括走、跑、跳。运动时间约1020.51 s，运动距离约1247.21 m，运动的起点和终点为同一点。

如图11 所示，蓝色线条是SHOE-8.6e4解算的轨迹，红线是区间搜索法解算的轨迹，绿色圆点是运动的起点。图12 中的蓝色线条是Adaptive-SHOE 解算的轨迹，轨迹直接发散，Adaptive-SHOE 在该实验中失效，Adaptive-SHOE 需要另外设计运动分类器和对应的零速判断模型，否则无法应对复杂运动的情况。表3 中虽然SHOE-8.6e4和区间搜索法的起点/终点误差相近，实际实验中是沿着同一条路线反复运动，从图11 中可以看出，阈值法的轨迹重合性不如无阈值法好。

如图13，采用SHOE-1e4、SHOE-5e4和SHOE-1e5解算的轨迹，虽然SHOE-1e5在实验一中解算的结果较好，从图13 可以直观看出其在实验二中解算结果较差。对于固定阈值法，无法确定一个通用阈值应对所有情况，因此需要根据不同情况设定阈值，算法的通用性较差。

图11 实验2 解算轨迹Fig.11 The trajectory calculated in the experiment 2

图12 Adaptive-SHOE 解算轨迹Fig.12 The trajectory calculated by Adaptive-SHOE

表3 实验二结果Tab.3 The result of the experiment 2

在表3 中，SHOE-8.6e4和区间搜索法测量得到的零速区间数量相近，但实际上两种方法检测的每个零速区间不同。由于多种运动混合在一起，阈值法要找到适配所有运动的阈值是很难的。如图14 红圈中，为了适应跑步状态，行走状态下会出现误判，缺失零速区间。SHOE-8.6e4和区间搜索法检测到相同的零速数量为193226 帧，匹配度约为89.12%。实验二中，区间搜索法在复杂运动情况下仍然具有较好的性能，并且不需要对参数进行调节，算法的鲁棒性较好。

图13 不同阈值解算轨迹Fig.13 The trajectory calculated by different thresholds

图14 零速区间检测结果Fig.14 The result of the zero-velocity interval

4 结 论

本文提出了一种区间搜索零速检测算法。针对脚部运动特点，将“运动-停止-运动”作为搜索单元。在搜索单元中，基于惯性数据区分的特点，设计了一种数据转换的方法，将加速度数据映射到搜索空间中进行分析和处理。在搜索空间中，通过粗搜索和精搜索分层迭代的方式，找到最优的零速区间。该算法可以自适应地对零速区间进行搜索，不需要对不同情况进行阈值设置。

通过实验对算法进行了测试，以固定阈值法SHOE 和自适应检测法Adaptive-SHOE 作为参考。区间搜索零速检测器可以达到SHOE 在最优阈值状态下检测效果。特别是在多种运动混合的情况下，Adaptive-SHOE 出现失效，但是区间搜索法仍然能够正常工作，具有较好的运动适应性。