基于地标点匹配的高精度室内定位算法

周凌柯，鲜 委，龚文龙，李 胜

（南京理工大学 自动化学院，南京 210094）

行人导航系统又称单兵导航系统，用于实时提供行人的速度、位置等导航信息[1]，是单兵数字化系统的重要组成部分。全球定位系统（Global Positioning System,GPS）在室外定位中具有广泛的应用，但是在室内环境下，卫星信号会由于遮挡和多路径效应而不可用，这就使得行人室内定位具有挑战性。基于微机电系统（Micro-Electro-Mechanical System,MEMS）的惯性测量单元（Inertial Measurement Unit,IMU）具备自主性好、体积小、质量小、便于携带等特点[2]，因此被广泛应用于行人导航系统。基于IMU 的行人的室内定位广泛使用零速检测（Zero Velocity Update,ZUPT）与行人航迹推算（Pedestrian Dead Reckoning,PDR）算法，但这些定位算法只能得到行人的相对位置与相对航向，并且定位误差会随着时间而累积，导致定位效果变差[3]。

为了抑制行人室内定位中航向角漂移的问题，Borenstein 提出了启发式航向角削减（Heuristic Drift Elimination,HDE）算法。基于建筑多为矩形这一事实，将建筑方向定义为主方向，通过主方向来约束航向角[4]；Jimenez 则对HDE 算法进行改进，只有检测到行人正在直行时才会执行HDE 算法，从而减小行人在非主方向上行走时HDE 的过度修正[5]；针对HDE方法中判断直行需要设定阈值的问题，Muhammad 提出了一种无阈值的转弯检测技术[6]。然而传统的HDE方法只是将航向角的主方向在360 °内等间隔地分割为4 个或8 个，若行人在非预设的主方向上直行，则HDE 方法无法起到修正作用。

HDE 方法仅能对行人的航向角进行约束，可以增添超宽带[7]、蓝牙[8]、WIFI[9]、音频[10]等外接设备来辅助定位，但这些额外设备的部署与维护进一步增加了人力与财力的消耗。使用地标点辅助定位则具备成本低的优势，在室内定位领域，传感器输出呈现规律性变化的位置点被称为地标点[11]。Jimenez 将建筑物中出现的斜坡作为地标，事先建立包含斜坡位置信息的数据库，若检测到斜坡则对行人位置进行修正[12]。胡倩针对不同类型的地标点设定了置信度的计算公式，提高了匹配的准确率[13]。然而传统的地标点辅助定位需要事先将地标与位置相关联，并不适用于未知环境。

考虑到纯惯导解算时高度通道并不稳定，因此需要额外的信息源去约束高度信息，通常使用气压计进行约束。Zhao 将气压计和PDR 方法融合，通过气压计来计算垂直位移，解决了PDR 只能实现平面二维定位的问题[14]；Jao 提出了一种结合气压计和超声波的混合高度计，并在楼梯、坡道等常见室内地形进行实验。在该高度计的辅助下，ZUPT 的导航精度相较于无辅助时提升91%[15]，但是气压计解算的高度在温度、气压变化的情况下会急剧恶化。

在分析上面所提问题的基础上，本文研究了行人室内行走时的特点，提出了一种基于地标点匹配的高精度室内定位算法。该算法使用IMU 这一单一传感器，包含速度约束、航向角约束、水平位置约束和高度约束四个模块，其中零速检测算法用来约束速度；增强型启发式偏移消除（Enhanced Heuristic Drift Elimination,eHDE）算法实时检测和建立主方向以约束航向角；实时建立地标点约束水平位置，并设计匹配度的计算公式以区分不同的地标点；基于阶梯高度的高度约束算法约束高度，实现了行人室内的高精度定位。

1 融合滤波器设计

本文采用卡尔曼滤波算法融合四个模块，该滤波器的框架如图1 所示。

图1 融合滤波器架构Fig.1 The structure of fusion filter

陀螺仪和加速度计的测量值被用来进行捷联惯导解算。由于惯性传感器的误差会随着时间迅速增长，为了更好地描述惯导误差的传播规律，建立15 维误差状态向量模型δXk。

其中，δψk为k时刻的三个姿态角误差；为东向、北向、天向的速度误差；为东向、北向、天向的位置误差；为陀螺仪零偏向量；为加速度计零偏向量。

将惯导的解算结果当做预测量，不同的运动约束算法将会提供相应误差值的虚拟观测量，将预测量与观测量融合以对行人运动时的导航信息进行约束。

2 模块设计

2.1 速度约束

在足绑式行人惯性导航系统中，零速状态时支撑脚相对于地面是静止的，支撑脚上的速度应该为0，但是由于传感器本身以及惯导算法所带来的误差影响，导致此时的速度并不为0，可以使用零速检测算法检测零速状态以对速度误差进行修正。本文使用文献[16]的姿态假设最优检测（Stance Hypothesis Optimal Detection,SHOE）算法进行零速检测，关于该算法本文不再赘述。若检测到当前时刻为零速状态，可得速度的虚拟观测量为：

为了获得更完整的零速区间，对零速检测结果进行二进制均值滤波。图1 中的“零速点”对应每个零速区间的第一个采样点，当前时刻为零速点是进行航向角约束、地标点约束以及高度约束的前提。

2.2 航向角约束

2.2.1 传统的HDE

HDE 算法需事先计算零速点时刻的步幅航向，设第m个零速点在所有采样点中的序列号为s(m)，s(m)时刻惯导解算的东向、北向位移分别为x(s(m))和y(s(m))，则第m步的步幅航向θs(m)可由相邻两个零速点的东向、北向位移表示(m＞ 1)：

该步的对应步长SL(m)可以表示为：

由“静止”向“运动”过渡可能会出现伪零速点，这些伪零速点的特征是计算的步长过小。设定步长阈值thSL=0.15 m，筛除步长小于该阈值的零速点。

使用主方向的前提是当前时刻为直行状态，设定直行标识符为SLP(m)，角度阈值thθ1=8°，由相邻四个步幅航向判断当前是否为直行状态(m＞ 3)：

取走廊方向为主方向，直行标识符为“1”时进行主方向匹配。设与步幅航向最接近的主方向为θb1(m)，角度阈值thθ2=8°，满足 abs (θs(m) -θb1(m))＜thθ2时匹配成功，此时HDE 算法提供的虚拟观测量为：

2.2.2 改进的HDE 算法

传统的HDE 算法只能在预定义的直线路径上生效，而本文eHDE 算法无需预设主方向，能够在行走过程中实时建立主方向，新的主方向θnew可由相邻四步的步幅航向确定(m＞ 3)：

检测到行人正在直行，则遍历主方向数据库。如果主方向数量为0，则直接建立新的主方向θnew。如果主方向的数量不为0，则搜索与当前步幅航向最接近的主方向为θb1(m)，满足 abs (θs(m) -θb1(m))＜thθ2时用θb1(m)对航向角进行修正。如果不满足修正条件，则搜索与θnew最接近的主方向为θb2(m)，满足abs (θnew-θb2(m))≥2thθ2时建立新的主方向θnew，否则不建立新的主方向。

为了减小行人在非主方向上直行造成的误修正影响，使用自适应量测噪声进行航向角误差的量测更新，该量测噪声的标准差如式(8)所示。

其中，σeHDE=0.013；β=7。

δφ的绝对值越小，卡尔曼滤波对该观测量的置信度越高，即当前的步幅航向与主方向越接近，eHDE的修正作用越明显。

eHDE 算法的流程图如图2 所示。

图2 eHDE 算法流程图Fig.2 The algorithm flow chart of eHDE

2.3 地标点约束

2.3.1 改进的地标点算法

行人在室内建筑物行走时，在某些特定的区域传感器输出会出现规律性的变化。例如在两条直型走廊相交的拐角处航向角会出现90 °左右的变化，而在两个楼梯梯段交接处的平台会出现180 °左右的变化，可以把这些位置点视为地标点，对行人导航的位置信息进行修正。经过对建筑结构的分析，本文设定90 °与180 °这两种类型的地标点。

相较于传统的地标点约束算法，本文的地标点约束算法包含两个改进。一是基于航向角的变化实时检测并建立地标点，无需预知地标点的位置。检测到的地标点可以分为初次访问和重复访问两种情况：将初次访问的地标点纳入地标点数据库，与IMU 解算的位置进行关联；重复访问时则用已有的地标点信息进行位置修正。虽然IMU 解算的位置存在误差，但是该位置由先前时刻的惯导数据解算，其不确定程度小于重复访问时刻，因此基于该信息进行位置修正可以提高定位精度。第二个改进则是设计了一种地标点匹配函数，检测到地标点后取匹配度最高者进行位置修正，避免了单纯以位置区分地标点造成的区分度低、误匹配率高的问题，提高了地标匹配的准确率。

考虑到建筑物的结构在不同的楼层上存在强对称性，本文的地标点仅关注东向、北向这二维位置信息，行走过程中高度方向的位移则通过其他算法进行约束。

2.3.2 角度条件

本文的地标点约束算法通过角度条件去检测地标点。地标点检测需要分析相邻两个零速点步幅航向的角度增量，已知第m步与第m-1步的步幅航向分别为θs(m)与θs(m-1)，则第m步的步幅航向增量Δθs(m)可以表示为：

假设在地标点处经历的步数为N，事先进行的拐角实验表明该步数满足N∊{ 4,5,3,6,2}。易得地标点处的运动满足两个条件，一是对应N步的步幅航向增量具有相同的符号，逆时针经过拐角时，N步步幅航向增量的符号均为正，顺时针则均为负。设条件一的标识符为LF1(m)，则LF1(m)可以表示为：

二是对应N步的步幅航向增量的总和应该接近特定的角度θsum，本文θsum为90 °或180 °。设定条件二的角度阈值为thθ3=8°，则条件二的标识符LF2(m)可以表示为：

第m步是否满足地标点检测的角度条件标识符为LF(m)，只有条件一与条件二均满足时LF(m)才为1。

由于角度条件是根据步幅航向是否变化特定的度数来检测地标点，故检测到地标点的时刻滞后于到达地标点的时刻，所以纳入数据库的地标点不能与检测到地标点时输出的位置信息进行关联。设转向中途点h为N步中满足的第一个零速点，则新纳入的地标点应该与h时刻输出的位置相关联。

2.3.2 地标点匹配度计算

为了进一步提高地标点匹配的准确度，在角度条件的基础上，引入匹配度的概念。纳入新地标点时不仅会存储位置信息(xl,yl)，也会存储初始步幅航向θs(m-N)、末尾步幅航向θs(m)、地标点类型LT以及转向中途点h时刻卡尔曼滤波器输出的东向、北向位置误差的噪声方差、以及该条地标点信息入库时的序列号ID。初始和末尾步幅航向将会用来进行匹配度计算，而噪声方差将会在重复访问地标点进行位置修正时作为东向、北向位置误差的量测噪声。

设第m个零速点满足角度条件，此时遍历地标点数据库。如果此时数据库为空，该地标点肯定是初次访问；如果非空，遍历所有地标点信息，对于数据库中的第c个地标点，可得转向中途点h与其位置的欧式距离为dc。若起始航向角偏差为ΔθSTART，末尾航向角偏差为ΔθEND，第c条地标点信息的类型为LTc，则可根据式(13)计算第c个地标点的匹配度mc。

其中，wd表示距离因素在匹配度中的权重系数；wθ1和wθ2表示方向因素所占的权重系数，并且满足wd+wθ1+wθ2=1。本文取wd=1/2，wθ1=wθ2=1/4。

式(13)表明匹配的前提是地标点类型相同。设定匹配度阈值thm=0.65，若数据库中的最大匹配度mmax大于该阈值，表明匹配成功，可用相应位置信息进行修正；反之则为初次访问，需建立新的地标点。地标点匹配成功时，地标点约束算法输出的虚拟观测量可表示为：

此时地标点约束算法的流程图如图3 所示。

图3 地标点约束算法流程图Fig.3 The algorithm flow chart of landmark constraint algorithm

2.4 高度约束

当行人仅通过楼梯实现高度变化时，只会包含平地走、上楼梯和下楼梯这三种运动状态，由于每一级阶梯的高度固定，相邻两个零速点的高度增量应该接近阶梯高度的某个整数倍。本文关注相对高度，引入累积阶梯变化级数ntotal，ntotal表示当前时刻相对于初始时刻阶梯变化级数的总和，初始值为0。在零速点时刻计算高度观测量，设定每一级阶梯的固定高度为zstair=0.15 m，高度约束的阈值thz=0.05 m，每一步对应的阶梯变化级数为k，则高度误差的观测量δz的求解过程可以用表1 的伪代码实现。

表1 高度约束算法流程Tab.1 The process of height constraint algorithm

3 融合滤波更新

卡尔曼滤波的状态方程与量测方程分别为：

其中，Φk/k-1为系统的状态转移矩阵；Wk-1为k-1时刻的系统白噪声；Zk为观测量；Hk为k时刻系统的量测矩阵；Vk为k时刻的量测噪声。

根据惯导的机械编排可以得到系统的状态转移矩阵Φk/k-1为：

其中，Δt为采样间隔为载体坐标系到导航坐标系的坐标转换矩阵为加速度计在导航坐标系上的反对称矩阵；I 为3 × 3的单位阵；0 为3 × 3的零阵。

惯导的每个采样时刻均会进行时间更新，若当前时刻运动约束算法生效，则进一步进行量测更新，此时量测矩阵Hk由当前虚拟观测量的组合情况决定。

4 实验与分析

4.1 实验场景介绍

开展多楼层行走实验来验证本文高精度三维室内定位算法，该实验路径的起点位于自动化学院的六楼，实验人员在六楼按照L1-L2-L3-L4-L5 的顺序行走，通过楼梯依次进入五楼和四楼，并在五楼和四楼的走廊各行走一周，整个实验历时448 s，起点与终点的水平位置相同，实验人员在六楼的轨迹示意图如图4 所示。

图4 六楼行走轨迹示意图Fig.4 Schematic diagram of walking trajectory on the sixth floor

实验使用荷兰Xsens 公司生产的MTI-300 传感器采集加速度计以及陀螺仪数据，采集数据期间将该传感器固定于人右脚的脚面上，并利用USB 口将数据传输到电脑中，采样频率为100 Hz。该传感器的性能参数如表2 所示。

表2 MTI-300 性能参数Tab.2 The performance parameters of MTI-300

4.2 水平定位结果分析

通过水平定位效果分析各种算法对航向角的约束作用。ZUPT 和ZUPT+HDE 算法的二维轨迹如图5 所示，本文算法的二维轨迹如图6 所示，其中起点位置记为(0,0)，墙面信息事先通过激光测距仪获得。

图5 ZUPT 和ZUPT+HDE 算法的多楼层行走实验的二维轨迹Fig.5 The 2D trajectories of ZUPT and ZUPT+HDE algorithm obtained from the multi-floor walking experiment

图6 本文算法多楼层行走实验的二维轨迹Fig.6 The 2D trajectory of the proposed algorithm obtained from the multi-floor walking experiment

本文算法、ZUPT 算法和ZUPT+HDE 算法得到的终点误差分别为0.2548 m、1.4186 m 和3.4772 m。根据实验的实际情况可知，行走轨迹应该被约束在两侧墙壁之间，但是传统的ZUPT 算法航向角发散严重，导致出现了很多轨迹穿墙的情况，而HDE 算法能够抑制航向角漂移，减少轨迹穿墙的情况，降低了水平定位的终点误差。由图6 可知，相较于ZUPT+HDE 算法，本文算法中的地标点约束算法产生了作用，进一步减少了轨迹穿墙的情况，水平定位的定位误差也随之降低。

4.3 地标点约束算法验证

为了验证实时建立地标点的可行性，用黑色的五角星标记初次访问的地标点，蓝色的圆圈标记重复访问的地标点，同时在初次访问的地标点旁用数字标记其建立的先后顺序，本文算法多楼层行走实验的三维轨迹图如图7 所示。

图7 本文算法多楼层行走实验的三维轨迹Fig.7 The 3D trajectory of the proposed algorithm obtained from the multi-floor walking experiment

根据实验结果可知，整个实验过程共建立了9 个不同的地标点，初次访问的地标点均被正确地建立，而重复访问的地标点也能被正确地检测。为了更好地分析实验结果，地标点约束算法建立的地标点数据库如表3所示，一部分地标点的实际环境图如图8所示。在环境图中用箭头表示对应地标点步幅航向的变化情况，地标点数据库中的序号与环境图中的序号均代表该地标点的建立次序。

表3 多楼层行走实验建立的地标点数据库Tab.3 The landmark database established in the multifloor walking experiment

图8 部分地标点的实际环境图Fig.8 Actual environmental map of some landmarks

本文根据航向角的变化情况设定了两种类型的地标点，航向角变化接近90 °的地标点为类型1，航向角变化接近180 °的地标点为类型2。由数据库信息可知，仅有地标点7 为类型2，这与规划的路径相符。虽然地标点5 和9、6 和8 的地标点类型相同，水平位置也非常接近，但是仍被划分为不同的地标点，这是因为匹配度计算时不仅会考虑水平位置因素，而且也会考虑转向初始和末尾的角度因素，由于地标点5 和9、6 和8 的初始角度和末尾角度均相异，因此被划分为不同的地标点，表明匹配度公式发挥了正确的区分作用。

4.4 高度约束效果分析

为了验证高度约束算法的优越性，本文算法解算高度与ZUPT 解算高度的对比结果如图9 所示。

图9 本文算法与ZUPT 算法解算的高度对比Fig.9 Comparison between the proposed algorithm and ZUPT algorithm in height calculation

由图9 可知，ZUPT 算法解算的高度发散明显，而高度约束算法即使没有气压计的辅助，也能准确反应行走过程中的高度变化情况。通过终点位置的高度误差评判高度估计的准确度，ZUPT 算法的终点高度误差为0.5773 m，而本文算法的终点高度误差为0.0174 m，算法性能提高了96.98%。

4.5 误差分析

由于终点位置已知，可以计算不同算法的终点误差，结果如表4 所示。由表4 可知，相较于ZUPT 算法和ZUPT+HDE 算法，本文算法的终点误差分别减小了82.6%和68.3%，验证了本文算法的有效性。

表4 多楼层行走实验不同算法的终点误差Tab.4 Terminal point error of different algorithms in the multi-floor walking experiment

5 总结

针对室内行人导航系统定位误差大的问题，本文提出了一种基于地标点匹配的行人高精度三维室内定位算法。该算法仅使用IMU 这一单一传感器，其滤波框架包含速度约束、航向角约束、水平位置约束和高度约束四个模块。多楼层行走实验表明该算法能够提高行人室内定位精度，满足室内定位的需求。