地图辅助行人航迹推算技术的室内定位方法

2021-12-02张文超魏东岩

导航定位与授时 2021年6期

胡倩，张文超，魏东岩，袁洪

(1. 中国科学院大学微电子学院，北京 100049； 2. 中国科学院空天信息创新研究院，北京 100094)

0 引言

室内位置服务在人们日常生活和工作中发挥着重要作用。虽然全球导航卫星系统(Global Naviga-tion Satellite System, GNSS)可以提供比较可靠的室外定位导航服务，但不适用于室内环境[1-3]。为了解决该问题，目前研究人员已经提出许多室内定位解决方案，如激光、超声波定位技术、射频识别技术和基于超宽带的定位技术等，但是这些技术需要较大的前期基础设施投入，人力和时间成本较高。与以上几种定位方法不同，行人航迹推算(Pedestrian Dead Reckoning，PDR)技术仅依赖惯性传感器，受外界干扰小，在复杂室内环境下，短时间内可以达到较好的定位效果。但是惯性导航解算算法属于一种递推式导航方法，通过连续测得载体的角度变化和加速度来计算其导航信息，大多数便携式移动设备内置的惯性传感器工艺有限，且精度不高[4]，导致PDR的误差随时间迅速增长，定位结果变差。

为了减少这种累积误差，通常需要融合其他定位方法，常用的方法是用WiFi数据辅助修正轨迹，如文献[5]和文献[6]分别采用极大似然和扩展卡尔曼滤波(Extended Kalman Filter，EKF)算法融合WiFi数据和PDR结果，但是该方法通常需要预先采集WiFi信号的强度数据。若使用室内地图信息参与导航解算过程，算法便捷且成本较低。从惯性传感器数据中可以检测出行人在室内的移动行为信息，将其与地图信息进行匹配，可以约束行人的运动轨迹，从而提高定位精度及稳定性。

在地图辅助定位相关研究中，文献[7]基于条件随机场融合室内地图信息和PDR定位结果，提出了一种轻量级的地图匹配技术，采用维特比算法得到最终定位结果，但定位精度有待提升；文献[8]提出了一种结合地图信息的步数计算算法，可校准步距和方向，提高了步长算法的性能；文献[9]提出了采用卡尔曼滤波融合PDR和室内地标点的算法框架，同时为PDR算法提供新起点，以校正整个系统的累积误差；文献[10]通过对加速度计数据进行分析处理，识别站立、行走、上下楼梯和乘电梯等行为，并与相应的地标进行匹配，对室内行人位置进行校正，但存在较大误匹配问题；文献[11]基于地图信息检测室内特殊位置，并将其定义为路网节点对PDR的累积误差进行校正，但只是点位置校正；文献[12]设计了一种级联结构的卡尔曼粒子滤波算法，将传感器信息和室内地图信息融合，以约束行人轨迹发散问题，但是结构复杂，计算量较大。

本文在分析上述问题的基础上，通过研究行人运动和室内地图的特点，提出了一种地图辅助的室内定位方法。该方法采用EKF融合量测数据和PDR的定位结果，并结合地图信息以优化室内行人运动轨迹。主要贡献包括：提出了一种多地标匹配方法，能够有效提高地标匹配的准确率；使用通行路径和墙面信息辅助约束定位点航向和位置；采用EKF融合地图校正点和PDR的定位结果。

1 地图辅助的融合定位方案

本文采用卡尔曼滤波算法融合PDR和地图校正点。图1所示为融合定位系统结构框图。在PDR解算过程中，陀螺仪的测量值用于计算导航坐标系和载体坐标系之间的姿态转换矩阵，加速度计测量值通过该姿态转换矩阵投影到导航坐标系，然后经过对时间的一次积分得到载体在导航坐标系中的速度，二次积分即可得到位置。但是，由于低成本微机电系统(Micro-Electro-Mechanical System，MEMS)惯性器件(特别是陀螺仪)测量误差随时间增长迅速，导航误差在短时间内迅速累积。因此，引入地标信息进行位置修正，将PDR解算结果视为状态量，地标信息视为量测量，更新解算结果。针对航向发散问题，通过检测行人是否位于走廊等区域，判断轨迹是否穿墙，采用地图信息对定位结果进行约束。

图1 融合定位系统结构框图Fig.1 Block diagram of fusion positioning system

2 基于地标检测的位置约束算法

所谓地标点，即为传感器读数呈现独特、稳定、可识别变化模式的定位点。对于一个地标点，在数学上做如下定义

LP={(x,y,z),(R1,…,Rm)}

(1)

式中，(x,y,z)为地标点的三维坐标；(R1,…,Rm)为不同类型传感器读数下的检测规则，一类地标点可以有多种检测规则。

2.1 地标点检测

传感器是地标点检测的基础，不同传感器输出的数据类型、内容和数据特征均不同。对传感器数据进行特征提取，可定义不同的地标识别规则。行人的运动状态在室内环境下发生变化，可由加速度计幅值变化感知。如图2所示，行人在经过门这一地标时，由于存在推门动作，运动状态会出现由动到静再到动的状态改变特征，提取合加速度acc的方差用于区分行人的静态和动态，计算公式如下

(2)

(3)

其中，T为检测窗口长度；n为数据长度。定义门的识别规则

Rdoor=(LPt|at-t1:t>aσ2&&at:t+t2

&&at+t2:t+t1+t2>aσ2)

(4)

t1和t2分别为动态和静态的检测时长，若某一时刻t满足这一地标识别规则，则认为是一个可能的门地标点。

图2 经过门时加速度计幅值变化Fig.2 The change in the amplitude of acceleration when a user passes through a door

行人在转弯或是经过墙角时，行走方向会发生大幅度改变，这种变化特征可由陀螺仪数据幅值变化检测出来。由于实验采样频率较高易误检，故在不损失数据特征的条件下对数据进行降频处理，将每10个数据点划分为一组，取其均值作为当前陀螺仪数据。为提高检测准确率，采用符号函数判定是否连续角度改变，定义转弯点的识别规则

Rcor=

(5)

其中，θzi为某时刻陀螺仪z轴读数；εgyro为转弯阈值；sgn(θzi-εgyro)为符号函数，定义如下

(6)

若某时刻陀螺仪数据满足式(5)，表示行人当前运动状态是连续的航向改变过程，而不是偶然的航向改变，则认为当前位置点是一个可能的转弯地标点，可用于进一步地标匹配。转弯时陀螺仪z轴幅值变化如图3所示。

图3 转弯时陀螺仪z轴幅值变化Fig.3 The change in the gyroscope readings on the z-axis when a user takes a turn

楼梯或者电梯地标可通过气压计数据来识别。图4所示为上下楼梯时气压计幅值变化。为避免误检，在一次楼梯运动期间只匹配楼梯入口位置，当行人位于楼梯入口时，运动状态由水平移动变为垂直移动，定义楼梯入口的识别规则[13]

Rsta=(LPt|(|pi-pi-1|)<εbar_lev

&& |pi+Kp-pi|)>εbar_ver)

(7)

其中，pi表示以t时刻为中心检测窗口内的气压均值；εbar_lev表示行人水平移动的阈值；εbar_ver表示行人垂直运动的阈值；Kp为一个动态变化的量，初值可设为1，若符号函数值不变，表示当前行人运动状态不变，Kp值逐渐增大。

图4 上下楼梯时气压计幅值变化Fig.4 The changes in air pressure when a user takes stairs or an elevator

2.2 地标匹配

地标匹配是指根据约定好的行为判定规则，当数据特征满足某类地标的特征时，判断当前的地标类型，得到该地标点坐标，再与PDR定位点进行融合计算。对实验楼层存在的所有地标点建库，并存储对应的地标类型、地标序号和三维位置坐标。

使用地标辅助室内行人定位的一个关键挑战在于数据的关联，即当前位置附近存在多个地标时，很难确定检测到的地标与地标库的匹配情况。为了解决该问题，本文提出了设置置信度Val的方法，以表示当前地标点与地标库的匹配程度。Val的计算方法如下

Val(LPk)=δ(Rk,Rt)·(ωθ·h(θk,θt)+

ωd·d(lk,lt))

(8)

其中，k为地标索引；Rk为地标库中参考地标LPk的地标类型；Rt为t时刻检测到的地标类型；θk和θt分别为t时刻PDR定位点与参考地标的参考航向和PDR前后定位点的估计航向；lk和lt分别为t时刻参考地标位置和PDR估计位置；ωθ和ωd分别表示航向因素和距离因素在置信度中的权重系数。δ(Rk,Rt)为狄拉克函数，定义如下

(9)

h(θk,θt)为航向函数，定义如下

(10)

其中，εθ为航向阈值，实验中取0.4rad；d(lk,lt)为距离函数，定义如下

(11)

其中，δl为距离阈值，实验中取5m。当检测点附近有多个可能地标时，选择Val值最大的库地标点作为当前地标点的匹配点，用于滤波更新，若最大Val值为0，则认为当前检测地标为伪地标，将其舍弃。

3 基于地图信息航向约束算法

采用地标约束行人定位轨迹能够一定程度上优化室内定位结果，但仍存在航向发散和穿墙问题，需要结合室内地图信息来解决。室内地图数字化采用矢量法表示，矢量包括点、线和面，点表示室内某点位置，用三轴坐标表示;线表示室内的墙和通行路径等，包括起终点坐标，本文选取走廊中线为通行路径;面表示房间和走廊等区域。本文提出了两种基于地图信息的航向约束方法，这两种方法均为平面位置校正，不涉及高度。

3.1 基于路径信息的航向校正

在建筑物中的走廊等区域，行人通常沿直线行走，这种情况下，连续的步长向量大致平行，且与最近的可通行路径角度平行。通过提取相邻步长定位点，计算滑动窗口内航向，连续步长参考航向可利用式(12)计算

(12)

若窗口内连续步长参考航向和走廊参考航向差的最大值在阈值β范围内，如式(13)所示，但该时刻PDR估计位置偏离当前路径，则朝向当前通行路径校正PDR轨迹。

max(|θ(ki)|)<β

(13)

某一时刻定位点与附近可通行路径的距离Dis可用式(15)计算[14]，(x0,y0)为当前PDR的定位坐标，(x1,y1)和(x2,y2)分别为该通行路径的起终点坐标，则该通行路径方程为

(y2-y1)x-(x2-x1)y+x2y1-x1y2=0

(14)

由点到直线距离公式可知

Dis=

(15)

选取Dis值最小的候选路径作为匹配参考路径，将当前定位点向匹配通行路径中轴线做投影。校正点坐标(xp,yp)计算如下

(16)

3.2 基于墙面信息的航向校正

图5 基于墙面信息的轨迹校正Fig.5 Trajectory correction based on wall information

若α小于角度阈值，即为小角度穿墙，将当前定位点反射至墙面前方的点(如图5中点C)，N为该墙面的起点和终点构成的向量

(17)

(18)

由墙面反射原理可知

(19)

即

=(δx,δy)

(20)

则校正点坐标为

(21)

若α大于角度阈值，即为大角度穿墙的情况。此时认为行人由门进入房间，采用地标匹配方法在地标库中检索距离当前定位点最近的门地标点，作为校正点以约束行人定位轨迹。

4 滤波融合更新

通过文中第2节和第3节提出的修正方法，可以获得当前时刻用于校正PDR定位结果的三轴位置信息(包含地标点和地图校正点)，然后采用EKF进行修正。选取前-右-下为载体坐标系(b系)，北-东-地为导航坐标系(n系)。卡尔曼滤波的状态方程和量测方程如式(22)所示

(22)

其中，Xk为PDR解算结果，即三轴位置、速度和姿态;Zk为地标点或者地图校准点三轴位置;Φk|k-1为状态转移矩阵;Hk为量测矩阵;Wk-1和Vk分别为系统的过程噪声和量测噪声，均为不相关白噪声。卡尔曼滤波算法流程如图6所示。

图6 卡尔曼滤波过程Fig.6 Kalman filtering process

左侧是系统预测回路，Q为系统过程噪声Wk的协方差，按照卡尔曼滤波模型，首先建立状态方程为

(23)

(24)

状态转移矩阵Φk|k-1为

(25)

Hk=[I3×303×303×3]

(26)

由预测回路得到的协方差矩阵P及量测矩阵Hk，可计算EKF滤波增益Kk如下

(27)

其中，R为量测噪声Vk的协方差，可根据经验调整其值以达到最好的校正效果，进而更新当前状态向量和协方差。只需使用当前的量测向量和上一时刻计算的状态向量及其协方差，滤波算法即可递归运行。

5 实验与分析

5.1 实验场景

实验地点为中国科学院光电研究院，设计测试路径包含门、楼梯和转弯点三类地标点，以及走廊等典型室内场景，采用MapInfo软件处理该实验区域室内地图，建立规划路径上的地标点数据库，并确立地标点类型、序号和三维位置之间的索引关系。实验设备包括荷兰Xsens公司生产的MTw Awinda系列惯性器件，用于采集行走过程中的加速度计、陀螺仪和气压计数据，表1所示为传感器相关参数指标，采样频率100Hz。将器件固定于实验人员脚面，如图7所示，数据通过无线传至电脑端接收软件，实验过程中假设行人已知其目的地，并沿最短路径到达。

表1 MTw Awinda设备参数指标

图7 惯性器件及穿戴方式Fig.7 Inertial device and wearing method

5.2 地标与路径匹配结果

实验开始时静止10s，用于陀螺仪减零偏，航向和距离因素在置信度中的权重系数ωθ和ωd分别取0.4和0.6。表2所示为地标检测和航向约束过程中的参数设置。

表2 算法参数设置

根据传感器数据特征，检测用户行走过程中周围地标信息，匹配成功后进行位置修正。在实验设计路径中，共用到8个地标点，其中4个门地标点，用字母d表示，2个转弯地标点，用字母c表示，2个楼梯或电梯地标点，用字母s表示，数字表示各种类型地标点的序号。地标点分布情况如图8所示，走廊部分的通行路径取其中轴线，用粉色实线表示。

图8 地标及路径修正Fig.8 Landmark and path correction

一次试验中，地标匹配置信度Val值如表3所示，匹配结果显示，相较于传统匹配方法，本文所提地标匹配方法采用地标类型、航向和距离三类限制条件，能够有效地提高多地标情况下的匹配正确率。

表3 地标匹配结果

实验中通行路径设计较为简单，不涉及最优匹配路径选择问题，故Dis值最小候选路径即为正确的参考路径。

5.3 轨迹分析

基于同一组惯性数据，使用地图信息赋值初始位置和航向，图9所示为仅零速修正的PDR定位轨迹(蓝色)和本文所提方法修正后的轨迹(红色)。实验人员从光电楼一层大厅出发，经过门、走廊、电梯到达三层，再经过走廊和楼梯回到一层出发点。可以看出，PDR短期定位效果较好，但是随时间会出现误差累积现象，后期定位轨迹发散严重，最终没有回到出发点。而本文所提地图辅助PDR修正算法得到的轨迹，由于修正了PDR的累积误差，且使用室内地图校正了路径和轨迹穿墙现象，定位结果明显优于PDR原始轨迹，闭合误差较小，接近真实轨迹。

图9 PDR定位轨迹和修正轨迹Fig.9 PDR trajectory and modified trajectory

为了验证地标匹配的有效性，图10所示为部分地标匹配局部放大结果，蓝色为仅零速修正的PDR定位轨迹，可以看出，三类地标点都显示出较好的地标匹配修正效果，其中s1电梯地标点由于使用气压计数据检测，故使用其三楼的平面坐标作为校正点。地标点s2和d4距离较近，采用本文所提地标匹配方法仍然能够正确匹配，并将偏离轨迹通过EKF校正至地标点附近。

图10 地标点校正效果局部图Fig.10 The partial maps of landmark correction effect

基于走廊和墙面修正算法的局部效果如图11所示。结果显示，在走廊场景下，若行人轨迹偏离通行路径，校正算法能够很好地保持估计路径与走廊通行路径平行(图11(a))；当行人轨迹小角度穿墙时，能将当前位置校正为远离墙面的位置(图11(b))；当行人轨迹大角度穿墙时，认为行人进入房间，校正至最近的门的位置(图11(c))，从而使校正后的轨迹更加合理和准确。

图11 路径及墙面校正效果局部图Fig.11 The partial maps of path and wall correction effect

5.4 误差分析

本文采用平均定位误差和闭合误差作为定位性能的度量标准，定位误差是指实际位置和估计位置之间的欧式距离，平均定位误差是所有测试位置定位误差的均值，实验中实际位置坐标点通过地图得到。两种方法的误差如表4所示。实验中PDR平均定位误差为3.7854m，闭合误差为6.6877m；而经过地图辅助算法约束之后，使平均定位误差降至1.8435m，闭合误差降至0.6146m，定位精度提高了51.2%，证明了本文提出的地图信息辅助的定位方法在实验后期效果显著。