一种脚绑IMU辅助的行人室内定位定姿方法

旷 俭，刘 韬，牛小骥

(武汉大学 卫星导航定位技术研究中心，湖北 武汉 430079)

0 引言

现阶段的室内定位技术非常多，比如WiFi[1]、蓝牙[2]、磁场[3]、视觉[4]和超宽带(UWB)[5]等。然而对于不同的定位手段，定位精度和系统成本差异巨大。因此，室内定位技术不能一概而谈，必须针对应用选择合适的定位技术，以期满足用户需求的情况下达到定位精度和定位成本的平衡[6]。

针对车站、机场和医院等公共区域的消费类定位与导航应用需求，WiFi、蓝牙、磁场和视觉等方法已然是主流技术，主要是因为这类方法采用指纹库匹配方法，系统的布设成本低或无布设成本，从而大大降低了系统的构建成本[7]。然而，该类方法迫切需求一种高效率高精度的定位指纹数据库采集方法。

另外，为了准确地评价室内定位技术的性能，需要兼顾静态和动态环境。静态参考值因具有不变性，获取成本低；而动态参考值获取就存在较大难度。现阶段，动态参考值的获取主要有2种方法：规划轨迹和高精度即时定位与测图(SLAM)。其中，规划轨迹方法要求测试人员按照规定的路线进行测试，该方法操作简单，但参考值精度较低[8]；高精度SLAM能够提供高精度的动态参考值，然而设备体积较大且价格昂贵[9]。因此，室内定位性能评价缺少一种操作简单、价格低廉和精度高的参考值获取方法。

本文的主要工作就是使用2个低性能的微机械惯性传感器(MEMS-IMU)设计了一种行人室内定位和定姿方法。该方法主要利用行人足部周期性与地面接触的规律，人体骨骼的相对不变性，以及少量的坐标已知点(即控制点)辅助。该方法具有成本低、操作便捷、精度高和不受环境影响等特点，且能够同时提供位置和姿态信息。基于该方法，能够高效地采集定位指纹数据以及获得动态参考值。

1 系统设计

行人室内定位定姿方法的硬件构成主要分为2个部分：脚绑IMU和背部IMU。整体思路：首先，脚绑IMU结合少量的控制点提供连续可靠的位置坐标；然后，利用脚绑IMU获得的位置修正背部IMU的导航状态，以背部IMU估计的导航状态作为位置和姿态输出值。

系统结构如图1所示。行人正常行走时，脚部位形成了触地、静止和离地3个阶段的周期运动，脚绑IMU通过检测静止时间段，然后使用零速度修正当前导航状态，从而提供高精度的相对位置。为了控制脚绑IMU的位置误差，使用少量的控制点，从而使脚绑IMU提供长、短时间尺度的高精度位置。获得脚绑IMU的位置信息后，并不能直接用于修正背部IMU的导航状态，这是因为脚绑IMU与背部IMU的相对位置关系在一个脚步周期内不断变化。幸运的是，在静止时段的中间时刻，脚绑IMU与背部IMU的相对位置关系具有不变的表现。基于这样的客观事实，通过选择准确的静止时段中间时刻，从而使用脚绑IMU输出的位置信息修正背部IMU，控制背部IMU的导航误差，最后提供准确的位置和姿态。

图1 系统结构Fig.1 System structure

2 算法原理

室内行人定位定姿方法的算法流程如图2所示。由图2思路可以看出，脚绑IMU的数据处理思路和背部IMU的思路基本类似，核心部分都需要进行惯性机械编排，只有在滤波量测更新部分有差异。脚绑IMU使用的量测信息有零速修正和控制点位置修正，背部IMU只有脚绑IMU的位置修正。最后，反向平滑算法被用于进一步提升导航状态的估计精度。

图2 室内行人定位定姿数据处理流程Fig.2 Data processing flow of the pedestrian indoor positioning and orientation system

2.1 惯导解算

低端IMU的零偏和噪声水平普遍都非常大，该类微小的误差改正无法提升最终的导航性能。因此，本文采用的捷联惯导算法不考虑地球自转等微小误差改正项，只考虑传感器零偏和噪声等影响；目的在于降低算法的复杂度，同时不损失算法的自主推算精度。简化后的捷联惯导算法如下[10-11]：

(1)

2.2 滤波设计

捷联惯导算法性能严重依赖传感器本身的性能，使用卡尔曼滤波被融合其他观测信息能够在保证定位性能的基础上，最大程度地降低对传感器的硬件依赖。为了应对系统模型的非线性，本文采用扩展卡尔曼滤波，其中15维的误差状态变量定义如下：

(2)

式中，δpn，δvn，ψ分别为n系下位置误差、速度误差和姿态误差；bg，bf分别为陀螺零偏误差和加速度计零偏误差。离散后的系统误差模型表示如下：

δXk|k-1=Φk|k-1δXk-1|k-1+wk，

(3)

式中，下标k-1，k为历元编号；δXk-1|k-1为误差状态向量的估计值；δXk|k-1为误差状态向量的预测值；wk为系统噪声和测量噪声；Φk|k-1为状态转移矩阵。通过对式(1)进行扰动分析，可得[10-11]：

Φk|k-1=

(4)

2.3 量测更新

零速修正是脚绑IMU的基础量测更新，依赖于行人必须在路面行走的特性。其中，特性是指行人的脚部位与地面存在周期性地接触。通过准确地检测该时间段，可以构建零速度向量，更新导航状态向量。本文采用角速度和比力对零速状态进行检测[12-13]：

(5)

(6)

为了提供可靠的位置估计，当行人行走一段距离(比如50 m)后，需要主动查找一个已知点，从而控制位置误差累积。位置观测方程如下：

(7)

背部IMU的数据处理思路与脚绑PDR的处理思路基本一致，惯导算法和滤波设计相同，只是修正信息有所差异。其中，行走状态时，背部IMU不存在零速修正信息，唯一的修正信息是来自后处理的脚绑IMU位置信息。当行人站立不动时，因行人身体存在不同程度的晃动，无法准确地使用零速修正，但脚绑IMU仍然能够准确检测行人真实状态，此时脚绑IMU按照均匀时间间隔(比如0.5 s)输出位置信息用于修正背部IMU。

2.4 平滑算法

对于非实时定位应用，不使用未来时间点的观测量优化当前状态就显得数据利用率不够充分。最优平滑算法的本质是利用了过去、当前以及未来时刻的所有测量数据来估计当前系统的状态，最终获得比卡尔曼滤波更高精度的状态估计。RTS算法是一种典型的固定区间平滑算法，该方法可利用测量时段内所有量测信息对时段内任一时刻的系统状态量进行估计，具体方程为[11]：

(8)

式中，k=N-1，N-2，…，0；N为观测信息的总数；Ak为平滑增益矩阵。

3 实验结果与分析

3.1 实验描述

本文设计的行人室内定位定姿方法适用于行人行走场景，在室内外具有相同的性能表现。由于室内缺少一种能够覆盖大面积的高精度定位参考，所以在室外场景进行数据采集，目的在于使用卫星定位技术提供高精度(比如cm级)参考真值对本文方法进行性能评估。

测试平台如图3所示。脚绑IMU(YDL-602)为一个消费级MEMS IMU，背部IMU(POS-1200)是一个集成了GNSS板卡和较高性能IMU的组合导航系统，具体的IMU性能参数如表1所示。POS-1200通过胸腰椎固定支架紧紧地附着在测试人员的背部，YDL-602则通过螺钉安装在右靴上。

图3 测试平台Fig.3 Experimental test platform

表1 IMU性能参数Tab.1 IMU performance parameters

采集行人正常步行数据前，需要对IMU的位置关系进行标定测量，从而保证脚绑IMU的位置能够准确投影到背部IMU位置。位置和姿态参考是通过差分GNSS / INS组合导航系统后处理结果给出，解算软件为武汉迈普时空提供的GINS软件。基准位置精度优于5 cm，基准横摇和俯仰精度均优于0.03°，基准偏航精度均优于0.1°。

为了分析本文方法的性能，在武汉市一个空旷地区进行了现场测试。测试距离约为1 900 m，时间约为24 min，平均步行速度约为1.5 m/s，参考轨迹如图4所示。

图4 参考轨迹Fig.4 Reference trajectory

3.2 结果分析

图5给出了不同数据处理策略对脚绑IMU的北向位置误差的改善幅度，其中策略主要包括：脚绑IMU(FM-INS)、脚绑IMU+控制点修正(FM-INS+CP)和脚绑IMU+控制点修正+反向平滑算法(FM-INS+CP+RTS)。可以看出，控制点修正能够有效地消除脚绑IMU的误差累积，RTS算法能够进一步提升定位性能和脚绑IMU的定位稳定性。因此，后续数据处理都采用FM-INS+CP+RTS方案。

图5 不同数据处理策略的北向位置误差Fig.5 North position error of different data processing strategies

使用控制点修正脚绑IMU时，控制点的修正频率不同，系统的定位精度表现存在明显差异。因此，本文通过对5种不同距离间距控制点修正方案(30，40，50，60，70 m)的定位性能进行评价分析，从而为实际使用提供参考。图6给出了不同距离间隔控制点条件下的位置误差均方根，5种不同间距的北向位置误差均小于1 dm，而东向位置误差均小于1.2 dm，垂向位置误差均小于1.5 dm。因此，若只需要提供分米级定位精度，脚绑IMU的修正点距离间隔可以最大放到70 m。

图6 5种距离间隔控制点条件下的位置误差Fig.6 Position errors of the five different distance spacing control point conditions

由前面的数据分析可以看出，随着控制点间距增大，定位误差缓慢增大，但是变化幅度不明显。因此，在使用脚绑IMU估计的位置修正背部IMU时，本文只分析3种不同距离间隔控制点(30，50，70 m)条件下，背部IMU的定位和定姿性能。图8给出了3种不同距离间隔条件下，背部IMU的北方向、东方向和高程误差。

图7 3种距离间隔控制点条件下背部IMU的位置误差Fig.7 Position errors of the back IMU in the three different distance spacing control point conditions

位置误差RMS值如表2所示。可以看出，当控制点间距为30 m时，平面和高度误差的RMS值分别约为5，7 cm；当控制点间距为70 m时，平面和高度误差的RMS值分别约为10，15 cm。与脚绑IMU的定位误差基本一致。

表2 3种距离间隔控制点条件下背部IMU位置误差Tab.2 Position errors of the back IMU in the three different distance spacing control point conditions m

不同距离间隔控制点条件下的横滚角、俯仰角和航向角差异如图8所示。

图8 3种距离间隔控制点条件下背部IMU的姿态差异Fig.8 Attitude differences of the back IMU in the three different distance spacing control point conditions

姿态差异的RMS值如表3所示。可以看出，使用脚绑IMU辅助与使用GNSS辅助，背部IMU输出的姿态差异非常小，RMS值约为0.01°。而航向角差异则与控制点的分布密度呈正相关现象，控制点间距为30 m时，航向角度差异的RMS值约为0.5°；当控制点间距为70 m时，航向角差异的RMS值上升到0.78°。这是因为随着控制点间距增大，脚绑IMU估计的位置误差正在逐渐增大；另外，脚绑IMU输出的位置为滤波估计结果，位置之间存在相关性，破坏了卡尔曼滤波认为观测值噪声为白噪声的假设，通常会降低滤波估计的精度。总的来说，背部IMU仍然能够提供优于1°的航向角估计精度。

表3 3种距离间隔控制点条件下背部IMU姿态差异Tab.3 Attitude differences of the back IMU in the three different distance spacing control point conditions

4 结束语

本文提出了一种脚绑IMU辅助的行人室内定位和定姿方法。该方法首先使用零速修正和少量控制点用于获得脚绑IMU的准确位置；然后，将脚绑IMU估计的位置作为背部IMU的位置修正信息，达到准确的位置和姿态估计。通过在室外空旷地区的现场测试，以GNSS / INS解决方案为参考，对连续误差序列进行评估，验证了该方法的有效性。实验结果表明，该方法在2个相邻控制点之间的距离为50 m时具有亚米级的定位能力。横滚角和俯仰角差异小于0.1°，偏航角精度为1°～2°。与传统的直接控制点校正方法相比，该方法具有显着的准确性和鲁棒性。

所提出的方法具有成本低、不受非视线条件影响、不受测试面积大小影响和操作便捷等优点，在室内环境可提供高精度和可靠的定位定姿解决方案。此外，所提出的方法可以用作其他室内定位技术(例如WLAN和蓝牙)的室内参考解决方案。