基于历史信息约束的室内行人三维惯性定位误差修正方法

丁一鸣，熊 智，曹志国，李婉玲，王钲淳

（南京航空航天大学 自动化学院 导航研究中心，南京 211106）

惯性行人定位系统在消防安保、地下巡检、智慧城市等领域具有巨大的潜在应用价值，然而由于惯性传感器（Inertial Measurement Unit, IMU）噪声的影响，惯性行人定位系统的定位误差随时间累积。尤其是在高度估计中，由于重力场的变化、哥氏加速度和惯性平台姿态角的影响以及加速度计本身的误差，使得惯性定位系统的高度误差呈现发散的趋势。

国内外学者针对行人惯性定位系统累计误差的抑制技术进行了深入研究，按照是否需要其他信源可以划分为两类：一类是多源信息融合方法，这类方法使用如卫星定位系统（Global Navigation Satellite System,GNSS）[1]、WiFi[2]、超宽带（Ultra-Wide Band, UWB）[3]等被动定位技术，或如视觉[4]、激光雷达[5]、气压计[6]等自主定位技术与惯性定位进行融合，修正惯性系统的定位误差。这类系统能够实现长时间稳定的行人室内定位，同时也受使用环境、体积成本等因素的影响。与被动定位技术融合的行人定位方法由于需要在环境中预先部署辅助定位设施，因此其使用范围相对受限，与主动定位技术融合的定位系统体积较大、成本和功耗较高。另一类是将惯性传感器与人体固联，如足部[7]、胸部[8]、腿部[9]固联，使用人体运动模型建立虚拟观测约束惯性累积误差。这类行人定位系统无需任何辅助定位设施，具有自主性好、可靠性高、适用范围广等优势，但由于人体运动模型对位置的观测性较弱，因此该类系统能够在短时间内保持较好的定位性能，但其定位误差仍呈发散趋势，难以适应长时间精准定位需求。

为满足行人长时间稳定定位需求，Puyol等人[10]提出一种针对行人惯性航位推算系统的后端优化方法，使用六边形网格表征地图，采用FastSLAM[11]算法架构估计行人位姿与地图的联合后验概率分布来消除行人惯性航位推算系统的累计误差。该方法在提高纯惯性定位系统长时间定位精度稳定性的同时无需任何其他传感器，对室内惯性行人定位的发展具有重要意义。然而这种方法中六边形的地图表征形式和穿边概率的表征方式存储空间大、计算复杂度高，难以在低成本系统中实际应用，是一种准实时行人惯性定位方法。文献[12]提出一种基于四边形占用栅格地图的惯性构图与误差修正方法，该方法使用四边形占用地图表征空间可通行区域的概率分布，降低了算法所需存储空间，提高了计算效率，能够在低成本系统中实现惯性地图的实时构建与行人惯性系统误差的实时修正。但是该算法仅适用于行人水平方向定位误差修正与二维地图构建。

为此本文将基于四边形占用栅格地图的惯性构图与误差修正方法进行推广，提出一种基于历史信息约束的室内行人三维惯性定位误差修正方法，使用六面体构建相应区域的三维占用栅格地图。并利用行人的历史位姿修正地图的构建误差和惯性系统的累计误差，提高纯惯性系统长时间水平和高度方向上定位精度的稳定性。本文搭建了半物理实验平台，验证了所提出的方法的有效性。

1 基于三维惯性构图的室内行人定位算法

1.1 系统架构

基于三维惯性构图的室内行人导航系统由行人惯性航位推算和后端构图优化两部分组成，其本质是估计行人三维位姿x1:t和地图Θ1:t的联合概率分布：

其中u1:t为行人惯性航位推算的步长-航向变化-高度变化控制向量，z1:t为根据行人对环境可通行区域的虚拟观测。根据Rao-Blackwellize法则[11]，式(1)可分解为：

其中， Θ= {θ1,θ2,… ,θN},θi为三维占用栅格地图中的第i个六面体，N为地图中六面体总数量。次分解表明对于位姿和地图的联合后验概率分布问题可以分解为一个位姿估计问题和N个已知行人位姿的地图六面体可通行概率估计问题。

本系统中行人惯性航位推算部分将惯性传感器固联于足部，惯导系统经过导航解算实时输出行人的位置、速度、姿态等导航信息，利用足部运动的周期性零速特性分割步态，并通过卡尔曼滤波对导航参数进行修正，输出行人每一步的步长、航向变化与高度变化构成控制向量。利用控制向量，通过构建行人三维航位推算模型获得行人三维位姿的一步预测。后端构图优化利用前端的输出构建三维占用地图，当行人再次经过已构建地图区域时优化地图与位姿估计，并将优化后的导航参数作为系统的导航结果输出。算法架构如图1所示。

图1 基于三维惯性构图的室内行人定位算法架构图Fig.1 Architecture diagram for indoor pedestrian 3D inertial-SLAM

本文主要针对后端构图优化算法进行设计，关于足绑式零速修正行人惯性航位推算系统这里不再赘述，读者可以参考文献[12]。

1.2 三维地图建模

本文对基于四边形占用栅格地图的惯性构图方法进行推广，使用六面体占用地图划分行人的运动空间。图2展示了一个楼梯情景下六面体环境表征示意图，图中三维空间被等尺寸六面体均匀分割，其中灰色六面体为楼梯建筑结构，红色曲线为行人的运动轨迹。当行人初次到达该空间中，所有的六面体皆为未知区域，随着行人登上楼梯，行人运动轨迹所历经的六面体可通行的概率变大（对应图中蓝色六面体）。每一个六面体包含三维地图坐标系下位置索引和该六面体为可通行区域的后验概率上标θ表示该坐标为地图坐标系的索引，与行人的位置作区分。将六面体长、宽、高分别记为lθ、wθ、hθ，则载体位置p=[px,py,pz]T与六面体索引存在如下关系：

图2 楼梯情景下六面体环境表征示意图Fig.2 Schematic representation of the hexahedral environment in the staircase scenario

其中＜*＞表示对向量中的每一个元素进行四舍五入运算。值得注意的是，为了减小地图离散化造成的误差，六面体各边长参数的选择要尽量保证根据位姿所构建的可通行区域是连续的，因此通常将六面体的长、宽设置为平均步长的2倍长，将六面体的高度选择为运动平均高度变化的2倍长。

六面体的可通行后验概率使用静态二值贝叶斯滤波器进行迭代估计：

其中

为六面体可通行后验概率对数比，x1:t为行人1到t时刻的位姿，z1:t为1到t时刻行人对所在六面体的观测，θi为地图中第i个六面体。p（θi|zt,xt） ∊ （ 0.5,1]为行人对六面体可通行概率的反观测模型，p(θi)为六面体可通行概率的先验概率，由于事先没有环境地图信息，因此对于环境中未被经历过六面体的可通行先验概率皆为0.5。

1.3 地图与位姿误差修正

由于行人和地图的传播过程是非线性的，同时其分布并非高斯分布，因此本文采用粒子滤波器对式(4)进行估计。每个粒子的状态包括行人所有时刻的位姿和地图：

根据行人航位推算模型，建立状态方程如下：

其中上标[m]表示第m个粒子。

权重更新后根据粒子当前状态结合1.2节所述地图的静态二值贝叶斯滤波器更新地图各六边形为可通行区域的后验概率。再依据粒子的权重对粒子集合进行重要性重采样，重采样后的粒子服从目标分布，即得到了粒子和地图的联合后验概率分布。最后将位姿估计期望反馈到行人惯性航位推算系统中，修正行人位姿。

2 实验结果分析

2.1 实验条件

本文所提算法为基于零速修正行人惯性定位算法的级联滤波器，为了验证所提算法的性能，利用原始的零速修正算法进行了对比试验。为了避免粒子滤波中蒙特卡洛抽样随机性对结果的影响，在相同测试条件下，进行了10次对比试验。实验环境为南京航空航天大学自动化学院1号楼，实验楼卫星俯视图如图3所示，实验楼平面图如图4所示，占地面积约为3000平方米，共五层，层高为3米，单层走廊通道周长约为200米。为评估系统定位精度，使用高精度轮式里程计预先标定了如图4红色圆点所示的四个真值点。

图3 实验环境卫星图Fig.3 Satellite image of experimental environment

图4 实验环境平面图Fig.4 Floor plan of the experimental environment

实验使用For Sense公司的FSS-IMU6132型IMU固联于足部，安装方式如图5所示。

图5 足绑式惯性传感器安装示意图Fig.5 Foot-mounted inertial sensor installation diagram

IMU性能参数如表1所示，IMU采样频率为100 Hz。测试人员在真值点1处出发，沿蓝色轨迹运动，并在两处楼梯上下楼两层，轨迹重复多次后最终回到起点。测试总时长为34分钟，总路程为1992米。本实验中所提算法的滤波器相关参数设置如表2所示。

表1 传感器性能Tab.1 sensor performance

表2 滤波器参数设置Tab.2 filter parameters setting

2.2 实验结果分析

2.2.1 行人惯性航位推算系统定位结果及分析

行人惯性航位推算系统利用足部IMU测量足部运动的加速度和角速度，通过捷联惯导解算和零速修正获得行人运动控制向量，并经过三维航位推算模型最终获得行人的三维位姿估计。图6为原始行人惯性航位推算系统的定位轨迹，图7为对应轨迹的俯视图，可以看到当行人运动第一圈时水平定位误差较小，但随行人经过多次转弯后，由于传感器噪声、标度因数误差等因素，航向逐渐偏离真值，轨迹逐渐偏转。图8为行人惯性航位推算系统定位误差的侧视图，图中高度估计和水平定位趋势相同，初始时高度估计误差较小，但随时间推移逐渐发散，且发散速度较快。这主要是因为行人每一步触地时垂直方向加速度变化速度快、范围大，激励出的加速度噪声较大、成分复杂。

图6 行人惯性航位推算系统定位结果（三维视图）Fig.6 Inertial pedestrian dead reckoning system positioning results（3D view）

图7 行人惯性航位推算系统定位结果（俯视图）Fig7 Inertial pedestrian dead reckoning system positioning results (top view)

图8 行人惯性航位推算系统定位结果（侧视图）Fig.8 Inertial pedestrian dead reckoning system positioning results (side view)

图9 -12中红色曲线分别为原始行人惯性航位推算系统在真值点处x方向、y方向、高度方向三维定位误差，可以看到，每一步定位误差经过行人行位推算模型逐步累积，水平定位误差逐渐增大，又由于缺少航向和位置观测，水平定位误差和航向的累积误差得不到有效修正。

图9 行人惯性航位推算系统定位结果与本文方法x方向定位误差对比图Fig.9 Comparison of inertial pedestrian dead reckoning system with the x-directional positioning error of the proposed method

图10 行人惯性航位推算系统定位结果与本文方法y方向定位误差对比图Fig.10 Comparison of inertial pedestrian dead reckoning system with the y-directional positioning error of the proposed method

图11 行人惯性航位推算系统定位结果与本文方法高度定位误差对比图Fig.11 Comparison of inertial pedestrian dead reckoning system with the height positioning error of the proposed method

图12 行人惯性航位推算系统定位结果与本文方法三维定位误差对比图Fig.12 Comparison of inertial pedestrian dead reckoning system with the proposed method for 3D positioning errors

2.2.2 基于历史信息约束的室内行人三维惯性定位系统地图构建结果及分析

图13 -15为本文所提方法的地图构建结果，图中颜色随高度变化，注意图中坐标轴为六面体索引，即水平方向索引总数与表2中六面体对应边长之积为所构地图的实际长度。从俯视图（图14）中可以看到地图中可通行区域与图4中的走廊区域基本一致，其中红色线框处蓝色四边形实际应与黄色四边形重合，对应三维视图红色框处蓝色楼层x为30处走廊x方向估计出现偏差。

图13 基于历史信息约束的室内行人三维惯性定位系统构图结果（三维视图）Fig.13 Proposed method mapping results(3D view)

图14 基于历史信息约束的室内行人三维惯性定位系统构图结果（俯视图）Fig.14 Proposed method mapping results(top view)

图15 基于历史信息约束的室内行人三维惯性定位系统构图结果（侧视图）Fig.15 Proposed method mapping results(side view)

导致该偏差主要原因有二：其一是粒子不足以表征对应时刻的状态分布。行人航位推算模型的非线性特性和地图分布的非高斯特性，本文使用粒子滤波表征行人位姿和地图分布。在定位初始时行人位姿的不确定较小，7000个粒子尚能够较为准确地表征粒子状态分布，但随着行人的运动位姿和地图的不确定增加，粒子数逐渐不足以完整地表征状态的联合分布，当行人运动到已经构建的可通行区域时，状态估计无法收敛到真值处。

其二地图的离散化误差也使得定位里程较真值里程更短。行人每到达一个新的六面体时根据前一个时刻的地图更新权重，此时新的六面体可通行概率为0.5，其附近已经历过的六面体可通行概率经过地图更新后较新的六面体更大，当位姿的一步预测分布有一部分落入已经历过的六面体时，该部分粒子被赋予较高权重，粒子位姿的期望即更倾向于已经构建的六面体所在位置，即与真值之间产生偏差，这部分偏差经过地图更新留存在地图估计中，在此后的估计过程中持续造成影响。

2.2.3 基于三维惯性构图的室内行人导航系统定位结果及分析

图16为本文所提方法平均定位结果的三维视图，图17和图18分别为对应的俯视图与侧视图。从图16可以看到本文所提方法能够较准确地反映行人的运动轨迹。10次试验的平均三维定位误差如图11中蓝色曲线所示，其中灰色区域表示这10次试验三维定位误差的标准差，在可以看到相较原始行人惯性航位推算系统误差（红色曲线）随时间累积的情况，本文定位误差始终稳定在较低水平。

图16 基于历史信息约束的室内行人三维惯性定位系统定位结果（三维视图）Fig.16 Proposed method mapping results(3D view)

可以观察到的是蓝色曲线虽然保持在较低水平，但仍存在固定偏差，该偏差即为地图构建偏差对定位估计的影响导致。对应图17可以看到，在右侧两个转角处本文方法的位置估计与起点的距离和真值与起点的距离相比较短。高度估计上由于地图构建所需六面体个数较少，地图构建偏差较小，对高度估计的影响也较小，如图18所示，因此高度估计相对准确。表3为原始行人惯性航位推算系统与本文方法在所有真值点处定位均方根误差对比，可以看到通过级联后端构图优化算法，足绑式行人惯性航位推算系统的三维定位精度由22.7268米/1992米提升至3.0712米/1992米,性能提升约86.5%。

图17 基于历史信息约束的室内行人三维惯性定位系统定位结果（俯视图）Fig.17 Proposed method mapping results(top view)

图18 基于历史信息约束的室内行人三维惯性定位系统定位结果（侧视图）Fig.18 Proposed method mapping results(side view)

表3 行人惯性航位推算系统与本文方法均方根误差Tab.3 pedestrian inertial odometer with proposed method root mean square error

3 结 论

本文针对惯性行人航位推算系统的累积误差问题提出了一种仅使用惯性信息的三维构图与定位方法。通过在行人惯性航位推算系统后端级联构图优化算法，使用六面体表征行人运动环境，在Fast SLAM框架下采用静态二值贝叶斯滤波估计环境中的可通行区域，并利用构建的可通行区域地图修正系统的定位误差。本文结合实际室内环境，对算法进行了验证，实验结果表明本文提出的三维惯性构图室内行人导航方法能够有效抑制行人惯性航位推算系统的累积误差，提升定位精度。可以预见的是本文算法并不仅限于行人载体，有助于提升封闭环境下机器人、飞行器等多种载体的纯惯性性能。