腰绑式行人导航系统基于缓存区的启发式航向补偿算法研究

王鹏宇，孟之栋，邓志红

(北京理工大学自动化学院，北京 100081)

0 引言

位置信息在人们日常生活和工作中发挥着日益重要的作用。在现代社会，人们大部分时间都处于写字楼、住宅楼、学校等建筑物内部。在复杂的室内环境下，人们对室内位置服务的需求迅速增加。目前，室内行人导航系统的主流技术方案可分为无线通信定位方案与行人航位推算(Pedestrian Dead Reckoning，PDR)方案两类。其中，无线通信定位技术包括超宽带(Ultra Wide Band，UWB)技术[1]、Bluetooth技术[2]、ZigBee技术[3]、WiFi技术[4]和射频识别技术[5]等。上述方案均需要外部系统设备的支持，在灾难救援等应用背景下，一旦外部设备损坏将导致无线通信定位方案无法有效发挥作用。 PDR方案[6]将微机电惯性传感器模块(Micro-Electro-Mechanical System Inertial Measurement Unit，MEMS IMU)固定于行人身体上，基于惯性参量确定步长模型或通过捷联惯性导航系统解算距离估计行人的行进步长，并由陀螺仪和电子罗盘等辅助信息组合得到每一步的行进方向，即可得到航位推算系统所需的2个关键参数，是一种完全自主无源的定位导航方式，非常适合在无基础设施的室内环境使用，因此是行人导航领域的研究热点之一。目前，MEMS IMU的固定方式有腰绑式、足绑式及肩绑式等。足绑式采集到的惯性数据存在较大抖动，需采用零速修正方法抑制误差发散；肩绑式采集数据更加平稳，但上肢对运动感知有延迟且存在特征缺失；相比之下，腰绑式能够在保证平稳采集惯性数据的同时保留较多运动特征，且对行人正常运动影响较小，符合实际应用需求，特别是对于灾难救援和单兵作战等任务场景。因此，本文在腰绑式行人导航系统的基础上设计了PDR算法并对其进行了验证。

行人行进的步长和方向是PDR系统中2个重要参数。其中步长的估计主要利用行进过程中产生的惯性参量信息，通过构建步长模型实现。常用的步长模型包括线性模型、非线性模型和机器学习模型等。线性模型利用步频、加速度方差、加速度最大值和最小值等特征量估计步长[7]，不同的构建方法具有不同的精度。Weinberg[8]利用人体运动学特征，将垂直加速度的二次积分近似为腰部的垂向位移，建立了非线性步长模型。Wang等[9]利用基于长短时记忆模块和去噪自编码器的神经网络，估计不同环境中不同行走模式下的步长值，并利用手机内置的惯性传感器采集数据训练网络并检验步长估计精度。Tian等[10]将磁力计计算所得航向与角速度积分所得航向做差作为航向误差观测量，修正导航误差参数。Borenstein等[11]利用行人直线行走时天向陀螺输出理论值为零的约束条件，采用带有PI控制器的闭环控制系统补偿陀螺漂移，称为启发式陀螺漂移补偿算法；之后，根据大型建筑物内部都存在4～8个主方向的事实，将当前航向角与主方向角差值反馈至导航系统中，设计修正量对航向误差进行补偿，称为启发式航向漂移消除算法(Heuristic Drift Elimination, HDE)[12]，是目前主流的精度较高的航向误差抑制方法之一。

但HDE方法应用范围有限，如果行人不沿主方向行走或者航向在不断变化，会出现过度修正或误修正。本文在HDE算法的基础上进行改进，提出了一种新的基于缓存区的启发式航向补偿算法。具体将N个复步作为一个缓存区，通过计算缓存区内相邻复步航向差值的方差，分段执行航向修正并控制HDE算法的修正强度，避免过度修正。此外，本文还利用峰值检测法结合三轴加速度矢量和信号对单步进行划分，在此基础上利用步频和加速度方差信息构建线性步长模型，从而构建出完整的腰绑式PDR解决方案。为了验证算法的有效性，在操场等环境开展了定位实验，结果表明了所提方法的有效性。

1 基于合成加速度峰值检测的单步划分

准确的单步划分是建立步长模型、计算步长的基础。由于器件在绑定时存在姿态偏离，且腰部的运动幅度并没有下肢剧烈。为增强加速度信号周期特征，依据文献[13]，对三轴加速度求矢量和并取模值为

(1)

式中，ax(t)、ay(t)、az(t)为三轴加速度输出。

图1 单步划分结果Fig.1 Single step division result

2 基于步频和加速度方差信息的步长估计

PDR系统通过估计单步步长实现距离更新。为了降低步长计算的复杂度，采用文献[14]中的线性模型估计单步步长为

S=a·fstep+b·υ+c

(2)

式中，S为单步步长，a、b、c为模型参数，fstep为步频，υ为加速度方差，可以分别表示为

(3)

在实际应用中，模型参数的确定与行人的身高和体重等特征相关[15]。本文提取行人在不同行走频率下采集到的步长和加速度方差等信息，利用最小二乘法对步频-步长、加速度方差-步长分别进行拟合，得到结果如图2和图3所示。通过对拟合参数的加权平均确定该行人步长模型参数。

图2 步频-步长拟合结果图Fig.2 Step frequency versus step length fitting result chart

图3 加速度方差-步长拟合结果图Fig.3 Acceleration variance versus step length fitting result chart

3 航向解算与修正

3.1 基于四元数法的航向角解算

航向解算精度是保证航位推算准确率的关键因素。捷联惯性导航系统中常用的姿态更新算法有方向余弦法、欧拉角法和四元数法等。其中，四元数法计算量小、精度较高，因此选用该方法进行姿态更新并解算航向。四元数可以表示为

Q=q0+q1i+q2j+q3k

(4)

式中，q0、q1、q2、q3为实数，i、j、k为虚数单位。

利用四元数表示的姿态变换矩阵为

(5)

利用毕卡逼近法由角增量求解四元数。更新完四元数后，结合欧拉角表示的姿态变换矩阵计算航向

(6)

3.2 HDE算法原理

由于MEMS IMU采用的低精度陀螺仪输出存在较大的随机漂移，导致惯导解算的航向角随时间发散，因此必须要引入航向误差抑制措施。HDE算法基于行人主要沿主航向行进的假设，将计算所得航向角与最近主航向之间的差值反馈至导航系统对航向角进行修正。

一般设定0°、90°、180°、270°为4个主航向，间隔角度Δ为90°。首先，将前一步航向角ψi-1映射到[0,Δ]区间中，并将映射结果与Δ/2作差得到控制量为

(7)

式中，MOD(ψi-1,Δ)表示对ψi-1/Δ取余数。Ei的正负代表与主航向相比航向角向左偏或向右偏。当Ei大于0时，表明前一步航向较主航向左偏，因此角速度需要加上1个修正量；反之则表明前一步航向较主航向右偏，角速度需要减去1个修正量，从而使解算航向角向主航向靠近。所以以控制量Ei作为输入，设计一个二态积分控制器，即

Ii=Ii-1+sgn(Ei)·ic

(8)

式中，Ii为反馈量，ic为角速度修正量，与陀螺仪性能有关，这里设为0.005，sgn(Ei)为符号函数，定义为

(9)

将反馈量作用于角速度进行修正，即

ωi=ωout,i+Ii

(10)

式中，ωout,i为陀螺仪输出角速度，ωi为修正后的角速度。结合3.1节航向角解算方法，可以进一步求得修正后的航向角。

3.3 基于缓存区的启发式航向补偿算法

在抢险救灾和单兵作战等复杂环境下，行人行进过程中除沿主航向直行外，还存在许多大转角转向和连续小角度转向等情况。而HDE算法是基于行人沿主航向行进的假设，通过减小当前航向与主航向之间的差值进行修正，因此在转弯或小角度转向时，HDE算法会对航向角的正常变化产生抑制作用，造成过度修正的问题。同时在直线行进时，腰部对于身体晃动造成的航向角突变敏感，因此当遇到异常摆动或避障时，由于每步都进行一次修正，可能导致累积的Ii远大于ic，使得后续修正失效。所以本文引入航向缓存区改进HDE算法的不足。

首先，为解决连续转弯时HDE算法存在过度修正的问题，分析相邻2个复步间航向角差值与转弯之间的关系。取沿操场环形跑道行走的惯性数据进行航向解算。其中，282步之前为转弯区域，之后为直行区域。利用式(11)求解相邻步间航向角差值的方差为

(11)

(12)

图4给出了航向角解算和方差计算结果。可以发现，在航向角连续变化时相邻步间航向角差值的方差较大，而直线行走时航向角差值的方差较小。受该方差特征启示，将每N个复步视为一个缓存区，计算在缓存区内航向角差值的方差。该方差特征用于改变HDE算法的修正强度，即相当于引入一个分段式的滑动窗，依据滑动窗内数据的方差特征动态调整修正量，公式表示为

(13)

式中，i′c为改进后的角速度修正量。

图4 连续转弯时航向角与相邻步航向角差方差示意图Fig.4 Heading angle and variance of adjacent steps heading angle differences during continuous turning

从式(13)可以看出，该算法在行人处于转弯状态时，可以减弱HDE算法的修正强度；当行人处于直行状态时，HDE算法的修正强度基本不变。其中，计算方差的缓存区长度N的选取与行人运动速度有关，当行人匀速行走时，N的经验值为5，即此时利用每5步内的航向角差值方差特征来修正航向具有较高的精度。

改进HDE算法最终的修正角速度为

(14)

同时，为避免每步执行修正可能导致HDE算法失效的问题，将缓存区作为最小单位，在每个缓存区内的航向角仅执行一次修正。由于惯导系统短时精度高，因此在检测到大角度转弯时，即δψ(n)大于设定的阈值thδψ时，禁用修正算法且重置缓存区。

4 实验验证

为了验证所设计PDR导航方案的有效性，选用荷兰Xsens公司生产的MTi-G-710系列MEMS IMU作为实验器材。表1给出了传感器相关参数指标。

表1 MTi-G-710参数指标

将传感器绑定于行人腰侧面进行实验。实验轨迹分为矩形轨迹和操场轨迹两种。缓存区大小设为5个复步，即N取值为5。在矩形轨迹实验中，令行人从起点出发，沿着室外广场的砖缝线行走1个矩形，再回到原点，路径总长为180m。在操场轨迹实验中，令行人沿着操场跑道线以均匀的步速行走1圈，全长为400m。图5和图6分别给出了两种轨迹的定位实验结果。其中，蓝色虚线为纯惯导解算航向角的轨迹图，绿色虚线为使用HDE算法修正航向角的轨迹图，红色实线为使用所设计的基于缓存区的启发式航向补偿算法修正航向角的轨迹图。

图5 矩形轨迹定位结果图Fig.5 Positioning results of rectangular trajectory

图6 操场轨迹定位结果图Fig.6 Positioning results of playground trajectory

从图中可以看出，在矩形轨迹实验中，不进行任何修正的惯导解算方法精度最低，而利用HDE算法和本文算法所得到的轨迹最贴近真实运动轨迹，说明HDE算法对沿主航向直线行走的航向角具有较好的误差抑制作用。定义终点与起点位置误差占行进总距离的百分比为终点定位误差，则在矩形轨迹实验中，所设计算法起点与终点的距离为1.09m，终点定位误差为0.6%。在操场实验中，由于存在连续转弯的2个半圆路径，所以HDE算法会对航向角误修正，导致定位轨迹与真实路径偏差较大。而基于缓存区的改进HDE算法会根据路径特点改变修正量，所以最终的定位轨迹与真实轨迹基本重合，起点与终点的距离为2.12m，终点定位误差为0.5%。

为了更加直观地体现所设计算法对航向角的修正效果，图7给出了操场实验中使用不同方法得到的航向角。定义直行区航向角误差为直行路段(即实际航向角为0°的路段)解算航向与实际航向差值的绝对值，可以看出无修正的惯导解算所得航向角在中期直行路段出现较大偏移，最大直行区航向角误差达到28°；HDE算法虽然在直行路段保证了航向角基本维持不变，但在转弯路段出现了错误修正，导致后续直行区航向角误差接近40°；相较于无修正方法和HDE方法，所设计算法在直行和连续转弯场景下对航向的修正效果都较为理想，最大直行区航向角误差在7°以内。

图7 操场实验中不同方法所得航向角Fig.7 Heading angles obtained by different methods in playground experiment

5 结论

针对腰绑式行人导航系统，本文设计了一套完整的PDR解决方案，并提出了一种基于缓存区的启发式航向补偿算法用于修正航向角。算法分析与实验结果表明：

1)腰绑式行人导航系统采集的加速度矢量和信号具有明显的峰值特性，可以用峰值检测方法准确划分出单步。在此基础上，构建线性步长模型准确估计步长。

2)基于缓存区的启发式航向补偿算法本质上是利用相邻航向角差值的方差对行人行走方式进行区分，根据不同的场景确定HDE算法的修正强度。

3)本文所提出的改进的HDE算法基于行人行进特征对航向角进行修正，如果行人运动状态特别复杂，还可以与磁力计解算所得航向角等外部参考信息进行融合，设计全运动模式下的误差抑制方法。