苗宏胜，李海军，孙 伟，蒋 荣

（北京自动化控制设备研究所，北京 100074）

为满足行人对获取实时高精度位置、速度的需求，目前的行人导航系统普遍依赖卫星提供导航、授时等服务。但是卫星信号易受到干扰，且在室内、隧道、丛林等环境下不能稳定覆盖，迫切需要行人导航系统具有不依赖卫星的自主定位导航能力[1]。

惯性导航系统不受时间、电磁、地域等因素的限制，具有较高的自主性。光学捷联惯导系统可实现高精度自主导航，然而其体积、重量较大，无法满足可穿戴需求；基于微惯性测量单元（Miniature Inertial Measurement Unit,MIMU）的微惯性导航系统体积小、重量轻，是目前惯性导航技术领域研究热点[2]。

行人自主导航系统目前普遍采用零速修正（Zero Velocity Update,ZUPT）算法修正速度、姿态误差，以提高导航精度。然而ZUPT 仅在零速区间对载体作滤波修正，因此零速检测的准确率直接决定了导航精度[3]。然而由于MIMU 存在精度低、噪声大的问题，因此常规零速检测算法存在大量的错判现象，影响导航精度。为解决该问题，文献[4]提出了一种基于支持向量机（Supporting Vector Machine,SVM）的自适应零速检测算法，可有效提高零速检测的准确性和对复杂运动状态的适应性。

由于行人运动幅度大、状态复杂，因此要求MIMU具有较大量程；为提高行人航向精度，又要求MIMU具有较高精度。目前MEMS 惯性器件的技术水平难以同时满足上述需求。为保证量程满足要求，目前MEMS 惯性器件普遍精度较低，导致系统定位精度较低，且航向误差随时间发散。然而ZUPT 算法中航向误差不可观测，不能被修正[2]。为抑制系统航向误差，国内外学者作了大量的研究。Chi Xu 等对视觉辅助的航向修正方法进行了研究，通过点线匹配的方式提升了载体的航向精度，然而当光线较弱，特征点不足时该方法将不再适用[5]。N.Kronenwett 等采用多传感器（包括RGB-D 相机、视觉惯性SLAM 等）融合的方式，实现了对系统航向和位置进行修正，然而同样受到各传感器的限制[6]。陈昌浩等基于物联网及深度神经网络学习的方法实现了基于低精度MIMU 设备的航向误差抑制，然而该算法较为复杂，不易于工程实现[7]。邓志红等采用互补滤波器及启发式滤波器的方法实现了对行人正常行走时航向误差的修正，当剧烈运动时航向误差抑制效果将变差[8]。综上，抑制行人导航系统的航向误差仍然存在较大的困难。

为提高行人导航系统的航向精度，本文提出一种基于自适应零速检测及双MIMU 速度+角速率匹配的行人自主导航算法，在一套导航系统中同时包含两种MIMU，一套大量程MIMU 用于保证数据完整性，一套高精度MIMU 用于提高系统航向精度。当高精度MIMU 数据有效时，通过卡尔曼滤波器（Kalman Filtering,KF）对大量程MIMU 航向陀螺漂移作修正，进而实现对航向误差的抑制；采用基于SVM 的自适应零速检测算法确保零速检测的准确性，当判断足部速度为0 时，采用ZUPT 算法对系统速度、姿态误差等进行修正，实现基于适应零速检测及双MIMU 速度+角速率匹配的高精度行人自主导航。

1 基于ZUPT 的行人导航系统

1.1 行人导航系统原理

人在行走的过程中，足部的运动存在明显的周期性变化，即步态周期。每个步态周期可分为4 个阶段：离地、悬空摆动、落地、持续静止[1]，如图1 所示。其中在足部持续静止的阶段，足部速度恒定为0，该阶段即为零速区间，可通过零速检测算法利用足绑式MIMU 采集到的加速度、角速率的幅值、标准差等参数对当前是否为零速状态进行检测[9]。

图1 单个完整步态周期[4]Fig.1 A complete gait cycle[4]

若判断当前为零速状态时，通过KF 对当前足部的姿态、速度误差等进行估计并修正；若判断当前为非零速状态，则不作修正，以仅通过惯性导航解算获取系统的导航参数[9,10]。基于ZUPT 的行人导航算法流程图如图2 所示。

图2 基于ZUPT 的行人导航算法流程[4]Fig.2 The process of pedestrian navigation algorithm based on ZUPT[4]

1.2 滤波器设计

卡尔曼滤波（KF）是一种通过线性化系统的观测量，对系统状态进行最优估计的算法[10]。线性化后行人自主导航系统的状态空间模型可表示为：

其中，X(t)为状态变量，F(t)为系统矩阵，Z(t)为量测量，H(t)为量测矩阵，w(t)为系统噪声向量，v(t)为量测噪声向量。在ZUPT 算法中，普遍取状态变量：

其中，δVn、δVu、δVe分别为系统北向、天向、东向的速度误差；δL、δh、δλ分别为系统的纬度、高度、经度的误差；ϕn、ϕu、ϕe分别为系统导航坐标系内北、天、东三个方向的失准角；∇x、∇y、∇z分别为载体坐标系内x、y、z三个方向的加速度计零偏；εx、εy、εz分别为载体坐标系内x、y、z三个方向的陀螺漂移。

量测量：

其中，Vn、Vu、Ve分别为惯性导航解算得到的系统北向、天向和东向的速度；分别为北向、天向和东向的速度的基准。在零速时刻有：

量测矩阵：

标准KF 方程如下[10]：

其中，Tf为KF 滤波周期，Tn为导航周期[10]。

由KF 估计出各状态变量的误差后，对各状态变量进行修正。由于ZUPT 算法中系统航向误差及航向陀螺漂移不可观测，故系统航向误差不能被修正，导致系统航向误差随时间发散。

1.3 航向误差的影响

在零速区间内，系统的航向误差近似满足：

式中前两项数值较小，即航向误差的主要来源为航向陀螺（天向陀螺）漂移εu。通过在航向陀螺上人为加入误差并进行仿真，得到航向误差对系统的影响如图3 所示。即航向误差会使轨迹方向发生偏移，进而导致系统定位精度降低。

图3 仿真得航向误差对定位的影响Fig.3 The influence of heading error on positioning by s

2 自适应零速检测算法

目前常用的零速判据包括加速度阈值检测、角速率阈值检测、加速度标准差检测、角速率标准差检测等[4]。

（1）加速度、角速率阈值检测

若三轴加速度、角速度的矢量和分别满足：

则判断当前为零速状态。其中，fx(k)、fy(k)、fz(k)分别为时刻MIMU 输出的三轴加速度，ωx(k)、ωy(k)、ωz(k)分别为三轴角速率，f(k)、ω(k)分别为三轴合加速度、合角速率，g为重力加速度，fth、ωth分别为加速度、角速率的零速检测阈值[4]。

（2）加速度、角速率标准差检测

若三轴加速度、角速率的标准差分别满足：

则判断当前为零速状态。其中σf(k)、σω(k)分别为 时刻MIMU 输出的单轴加速度、角速率的标准差，fk、ωk分别为单轴加速度、角速率，分别为窗口内加速度、角速率的平均值，N为窗口宽度，σfth、σωth分别为单轴加速度、角速率的标准差阈值[4]。

上述各判据的阈值fth、ωth、σfth、σωth等常采用经多次试验确定的常数，对于固定人员的固定运动状态（如正常行走）可以实现较高的准确率。然而，当运动状态复杂时，会出现大量误判和漏判的现象[4]。

为提高零速检测准确率，本文采用基于SVM 的自适应零速检测算法，由滤波窗口内三轴合加速度的峰值fp表征当前运动状态，以fp与三轴角速率、加速度的矢量和、标准差等变量构成样本集：

其中，y表示f、ω、σf、σω等参数，zi为已有样本点的状态，zi=-1 或1，表示当前的足部运动状态，zi=1表示该点为零速点，zi=-1 表示该点为非零速点。

采用SVM 算法对不同步态下零速点和非零速点进行分类，求解零速点和非零速点间的最优超平面。将当前采样点代入所求得超平面方程，即可作根据结果符号判断当前是否为零速状态。即得到零速检测判据（决策函数）：

3 双MIMU 行人导航系统设计

3.1 双MIMU 空间结构设计

人在运动时，足部角速率可达上千度每秒。为保证系统数据的完整性，需要在系统中包含一套上述量程的三轴陀螺仪（大量程陀螺仪），然而该陀螺仪精度仅能达到10 °/h 量级。

为提高该套陀螺仪的精度，需要一组精度较高的陀螺仪为其提供角速率基准，从而对其进行修正。其精度较大量程陀螺仪提升一个数量级，相应地，量程减小一个数量级。

采用3 只大量程陀螺仪及加速度计构成一套大量程MIMU；另采用3 只高精度陀螺仪构成一套高精度MIMU。

为保证系统的小型化和可穿戴性，需要对导航系统内部空间充分利用。为避免姿态转换引入额外的误差，需保证同一轴向的两个陀螺仪的敏感轴近似重合。综合考虑多种因素，系统内部双MIMU 的空间结构设计如图4 所示。

图4 双MIMU 空间结构设计Fig.4 The space structure design for dual MIMUs

3.2 算法设计

由大量程MIMU 采集人体足部运动过程中的角速率和加速度，并全程采用惯性导航算法解算行人当前的姿态、速度、位置。

当通过零速检测算法判断当前为零速状态时，此时系统的角速率也应为0，高精度MIMU 不会超出量程。可同时取速度与双MIMU 间角速率之差作为量测量，即采用速度+角速率匹配方案，通过KF 对系统所有可观测的状态变量进行估计并修正，从而实现对系统导航精度的提升。

当通过零速检测算法判断当前为非零速状态时，若此时高精度MIMU 的三轴输出均未超量程，则取双MIMU 间的角速率之差为观测量，即采用角速率匹配的方案，通过KF 对三轴的陀螺漂移进行估计并修正，从而实现对航向误差的抑制。

当上述条件均不满足时，以惯性导航得到的位置、姿态等作为最终输出。

基于双MIMU 速度+角速率匹配的行人导航算法流程图如图5 所示。

图5 基于双MIMU 速度+角速率匹配的行人导航算法流程Fig.5 The process of pedestrian navigation algorithm based on dual-MIMU velocity &angular rate matching method

3.3 滤波器设计

定义高精度MIMU系统所构建的载体坐标系为m系，大量程MIMU 系统构建的载体坐标系为b 系。两套系统均取地理坐标系为导航坐标系。高精度MIMU到大量程MIMU 系统间的姿态角误差为：

图6 双MIMU 间的空间模型Fig.6 The spatial model of dual MIMUs

由于m 系到b 系间的姿态误差较小，因此可对φx、φy、φz作小角度假设，因此有从m 系到b 系的姿态转换矩阵：

足部角速率分别在m 系和b 系的投影ωm、ωb间满足：

即：

考虑到陀螺漂移与时间延迟的影响，实际上有：

其中，εb为大量程MIMU 的三轴陀螺漂移；εm为高精度MIMU 的三轴陀螺漂移；Tδ为高精度MIMU 相对于大量程MIMU 的时间延迟。由于高精度MIMU 的精度较高，近似认为其相对于大量程MIMU的陀螺漂移可忽略不计，式(19)可简化为：

在引入高精度MIMU 系统后，又引入了4 个未知参数，分别为从m 系到b 系的安装误差φx、φy、φz及双MIMU 间的时间延迟Tδ。因此需将KF 的状态变量扩充为：

取量测量：

在非零速区间内，当三轴高精度陀螺测量值均在量程范围内时，取量测矩阵：

在零速区间内，取：

其中，（ωm×)为由ωm构成的反对称阵。

相应地，还需对Pk、Qk和Rk阵的初始状态P0、Q0和R0进行相应地扩充。

3.4 系统可观性分析

利用奇异值分解理论分析系统可观测度。分别采集导航系统静止朝北（航向角为0 °）和静止朝西（航向角为90 °）时双MIMU 输出的角速率、加速度，并通过导航解算得到北向、天向、东向速度，与双MIMU间角速率之差共同作为观测量，对系统进行可观性分析。

由于在零速区间速度为0，速度误差绝对可观，因此令量测量δVn、δVu、δVe的可观测度为1，得到引入角速率匹配前后系统状态变量的可观测度见表1。

由表1 得在引入角速率匹配后，系统y轴陀螺漂移的可观测度明显提高，由原来的5.45×10-4提高至0.98，由不可观测量变为可观测量。而在人的行走过程中，y轴陀螺又长期近似指天，为航向误差的主要来源，因此可通过对系统三轴陀螺漂移进行补偿，来实现对航向误差的抑制。

表1 各状态变量的可观测度Tab.1 The degree of observability for state varibles

4 试验验证

4.1 试验内容

将一套基于双MIMU 速度+角速率匹配的行人导航系统固定在足部，使系统的x轴与人体行进方向一致；y轴指天；z轴与人体行进方向垂直并指向行进方向的右侧。确保系统样机相对于足部固定，不会出现滑动、脱落等现象[4]。在系统开始工作前，保持足部静止并对系统初始位置、航向等进行装订，系统进入导航状态后开始行走。行走30 min 左右后回到初始位置，并通过机械手段保证与初始方位一致性误差小于0.5 °。

重复进行多次试验，每条次试验中同时利用双MIMU 进行导航解算可得到基于双MIMU 速度+角速率匹配的导航数据，利用大量程MIMU 可得到仅采用速度匹配的导航数据。统计两种方案终止时刻与起始时刻系统航向的差值的绝对值，即为系统的航向误差，取所有条次中航向误差的最大值作为当前系统的航向误差。

采用全球定位系统（Global Positioning System,GPS）对起始点及各转弯处的经纬度进行多次测量，得到其准确位置，作为定位误差计算的基准点。由于每次行走的路线不同，因此基准点依据实际行走的路线选取。统计各转弯处由导航系统与GPS 得到的位置间的距离即为经过该点时的位置误差，取每条次位置误差的最大值作为该条次的定位误差，取所有条次中定位误差的最大值作为当前系统的定位误差。

4.2 试验结果

采用基于SVM 的自适应零速检测算法对零速区间进行检测，采用双MIMU 速度+角速率匹配与仅采用单MIMU 速度匹配时得到的部分导航轨迹如图7 所示。由于在实际行走时，大部分时间段内大致沿着正东、正南、正西或正北方向，因此运动轨迹应大致与坐标轴平行。图中基于双MIMU 的行人导航系统的轨迹更趋近于实际情况，且各转弯处与基准点间的间距较小，说明基于双MIMU 的行人导航系统具有更高的航向及定位精度。

图7 采用不同算法时的部分导航轨迹Fig.7 Fragments of the navigation trajectories when using different algorithms

分别统计采用双MIMU 速度+角速率匹配及仅采用单MIMU 速度匹配时不同条次的航向误差与定位误差，统计结果见表2。

表2 系统的航向与定位误差Tab.2 The heading and positioning errors of system

在其他条件完全相同的条件下，在30 min 内，采用双MIMU 速度+角速率匹配的行人导航系统的航向误差不大于3.44 °，定位误差最大为4.92 m，均达到了较高的水平；而采用速度匹配方案时系统的航向误差最大为7.30 °，定位误差最大为9.62 m。即相较于采用速度匹配，基于双MIMU 速度+角速率匹配的行人导航系统的航向精度提升了52.9%，定位精度提升了48.9%。所设计的基于双MIMU 速度+角速率匹配的行人导航系统可有效抑制系统的航向误差，实现高精度的行人自主导航。

5 结论

本文对基于双MIMU 速度+角速率匹配的行人自主导航系统的结构及算法进行了设计。采用SVM 算法对不同运动状态下的零速点和非零速点分别进行分类，可提高零速检测算法的准确率和对不同运动状态的适应性。在一套行人导航系统中同时包含2 套三轴MIMU，一套具有较大量程但精度较低，用于采集人体运动时足部的角速率和加速度，用以保证数据的完整性；一套具有较高精度但量程较小，用于为大量程MIMU 提供角速率基准。若判断当前为非零速状态，且三轴高精度陀螺仪均未超量程时，采用角速率匹配方案提升大量程MIMU 三轴陀螺漂移的可观测度，通过KF 对其进行估计并修正；若判断当前为零速状态，采用速度+角速率匹配方案，通过KF 对所有的可观测变量进行修正，从而实现对系统航向误差的抑制。

经试验验证，基于双MIMU 速度+角速率匹配的行人自主导航系统的航向精度达到了3.44 °/30 min，定位精度达到了4.92 m/30 min，较仅采用单MIMU 速度匹配时精度有了很大的提升，航向精度提升了52.9%，定位精度提升了48.9%，且导航轨迹更接近实际情况。所设计基于双MIMU 速度+角速率匹配的行人自主导航系统可有效抑制航向误差随时间发散，进而提升系统定位精度，具有一定的实际应用价值。