CN110081888A - 一种基于可信度的惯导和地磁融合的室内定位算法 - Google Patents

Abstract

本发明公开了一种基于可信度的惯导和地磁融合的室内定位算法，包括：定位初始阶段在定位区域中选定坐标原点，建立坐标系，将区域按照不同路径划分；通过惯导获取每一步位置坐标，并在每个坐标点获取标准地磁信息，建立惯导地磁指纹库；定位过程中根据初始位置点获取行走坐标，通过路径匹配算法得到惯导信源可信度和路径相似度；根据路径相似度聚类得到局域地磁指纹库；运用改进型粒子滤波算法得到地磁匹配相似度，得出相似度最高的坐标点，与局域地磁指纹库二次匹配，得到地磁信源可信度；将地磁信源和惯导信源进行自适应加权算法融合，得出可信度最大的点即为估计位置。本发明提升定位速度和精度并为普通用户提供便利、快捷、准确的定位服务。

Description

一种基于可信度的惯导和地磁融合的室内定位算法

技术领域

本发明涉及一种室内定位算法，属于室内定位技术领域，尤其是涉及一种基于可信度的惯导和地磁融合的室内定位算法。

背景技术

随着移动互联网的发展，人们对于定位的需求量越来越大，人们的衣食住行各个方面都需要提供位置信息推荐大量的人们感兴趣的内容。尤其是物联网和5G技术的飞速发展，物体与人的联系越来越紧密，为物体和人提供位置信息变得尤为重要。

室内环境更加的复杂多变，由于室内位置空间较小，所以室内定位精度的要求要高于室外定位。针对室内定位的研究，研究人员提出了各种各样的方案，主要有基于红外线的、基于蓝牙的、基于超声波的、基于WIFI的、基于惯性传感器的、基于RFID的、基于ZigBee的、基于计算机视觉的、基于LED的、基于地磁强度的等等。

采用地磁信息进行定位最早应用在机器人的自定位，建筑物内地磁具有长期稳定性，通过构建地磁图谱，再使用匹配算法完成室内定位。IndoorAtlas由芬兰团队开发，精度可达到2米，但在实际使用中，受到使用者的行动幅度限制，行走过快或持有手机高度不同时，定位精度将产生较大的误差。Haverinen等人提出了一种利用粒子滤波算法融合地磁场进行定位的方法能够实现机器人和人员的定位，但该方法要求较高，必须保证终端的朝向和人的朝向一致。

传统的地磁定位也分为离线采集和在线训练两个阶段。离线采集阶段，通过磁强计获取到指纹点的磁场信息，构建地磁指纹库。在线阶段，根据未知点获得的磁场信息，根据匹配算法与指纹库进行比对，匹配度最高的点为未知点的物理位置。传统的地磁定位方法定位精度不高，耗时长，主要原因是指纹库数据采集不准确，定位阶段容易受到硬件和环境的影响而导致地磁数据不稳定造成的。地磁定位往往与其它定位技术相结合，多源融合定位也是目前研究的重点。

利用行人惯性导航技术与地磁定位技术相结合摒弃所有外部设备，只采用智能手机作为整个***的终端，为我们提供了一个低成本、高精度***研究的新思路。但是，关于地磁与惯导定位算法仍然存在一些缺点：

1)构建地磁指纹库费时耗力；

2)行人行走位置估计差距过大；

3)同一位置不同方向地磁信息存在较大的差异；

4)同一位置地磁信息存在一定的采集波动。

地磁与惯导定位算法在定位精度、定位速度和构建地磁指纹库方面仍有很大的提升空间。因此，如何设计出一种高效、高精度的室内定位算法和一套低成本高精度的室内定位***是一个值得攻克的难关，如果解决了这个难关将会给人们带来巨大的便利。

发明内容

针对以上问题，本发明是目的是提供了一种基于可信度的惯导和地磁融合的室内定位算法，以提升定位速度和定位精度，在地磁与惯导结合定位算法思想的基础之上，设计出一种室内定位算法来降低其缺陷对定位性能所造成的影响，为普通用户提供便利、快捷、准确的定位服务。

本发明所采用的技术方案如下：

一种基于可信度的惯导和地磁融合的室内定位算法，包括以下步骤：

1)定位初始化阶段，在定位区域中设定坐标原点，坐标系方向按照北为x轴正向，西为y轴正向划分，然后对区域划分路径段，相邻两个路径段之间的间隔不小于1m；

2)通过惯导计算得出采集者行走在路径段内每一步的位置点坐标，同时并在每个坐标点处获取标准地磁信息，录入建立惯导地磁指纹库；

3)实时定位时，利用初始位置点计算在行走路径中的坐标，然后执行惯导坐标相似度标准化，最后通过路径匹配算法获取惯导信源的可信度和当前所在路径相似度；

4)根据路径相似度聚类缩小惯导地磁指纹库得到局域地磁指纹库；

5)运用改进型粒子滤波算法获取地磁匹配后的估计位置，代入到局域地磁指纹库中进行二次计算，完成地磁信源坐标在指纹库中的可信度获取；

6)对地磁信源可信度和惯导信源可信度进行自适应加权算法融合；得出可惜度最高的位置点，即为待定位点的估计位置。

进一步地，步骤1)具体为：在定位区域内设定原点坐标，假设其中的一点为原点(x₀，y₀)，在该区域内划分坐标系，坐标系的x轴正向为正北方向，坐标系的x轴反向为正南方向，坐标系的y轴正向为正西方向，坐标系的y轴反向为正东方向；在定位区域内划分路径段，并按照l₁，l₂，l₃...的模式进行编号。

进一步地，步骤2)具体为：

构建惯导地磁指纹库需要采集者完成室内定位区域内的数据采集；首先，采集者使用智能手机，分别在划分好的路径段内行走，行走过程中计算惯导产生的每一步坐标：

①惯导坐标计算

对一段路径进行数据信息采集，假设采集者的初始位置为(x₁，y₁)，则通过以下公式完成每一步的坐标的计算：

x₂＝x₁+L_AB·cosθ

y₂＝y₁+L_AB·sinθ

公式中，L_AB代表采集者的步长的估计值，(x₂，y₂)为采集者行走的下一步的坐标点；

②标准地磁信息转换

在采集者进行路径段信息采集时，进行每一步的惯导坐标计算时，还需要完成地磁信息导标准地磁信息的转换；如下为手机坐标到标准地磁坐标信息的转换，其中，绕手机x轴旋转的角为俯仰角，定义为α，手机绕y轴旋转的角为翻滚角，定位为β，手机绕z轴旋转的角为方位角，定义为θ；

定位区域内以路径段为单位对区域进行划分后，完成对每个路径段的数据信息和位置信息的采集；每个路径段的内采集到的信息包含l_i,x,y,U_x,U_y,U_z,D,T八个特征，其中l_i为路径段的标号，x和y为惯导计算获得的位置坐标，U_x，U_y，U_z为经过坐标变换后的三个方向的标准地磁数值，D为经过滤波模型获取到的手机采集地磁信息过程中的方位角，T为位置点生成数据信息入库的时间。

进一步地，步骤3)具体为：

行人坐标计算同步骤二采集者进行坐标计算相同，行人在定位阶段，手持手机在定位区域内完成定位时，首先完成行人行走的每一步的惯导计算坐标，得到行人行走的一段路径和该路径下的坐标点；

1)坐标匹配

获取到行走过程中的路径点若为P₁’,P₂’,P₃’,...,P_j’,...,P_m’，主要通过方向的比较和坐标的比较来对比不同点之间的相似度：

①方向比较，指纹库和实测方位角若分别为D_j和D_j’，采用方位角的正弦函数的绝对值差的绝对值来代表它们之间的相似度：

DirDist_j＝||sinD_j|-|sinD_j'||；

②坐标比较，假设指纹库路径段和实测路径的位置点的坐标分别为(x_j,y_j)和(x_j’,y_j’)，则它们之间的相似度为：

③计算相似距离，假设方向比较权重为W_D，坐标比较的权重为W_L，则两个位置点的之间的相似距离为：

PDIST(P_j,P_j')＝DirDist_j×W_D+LocDist_j×W_L；

④相似距离标准化，将相似距离转化为0到1之间的数，即得到指纹库节点和未知节点的惯导相似度：

2)路径匹配

行人行走的实际路径若为l’，惯导地磁指纹库中若有n个路径段l₁，l₂，l₃...,...,l_i,...l_n；

①实测路径与指纹库路径的相似距离：

②相似距离标准化，将相似距离转化为0到1之间的数：

进一步地，步骤4具体为：

给出近邻阈值，行人行走的轨迹段为l_i，若存在轨迹段(l_j)(i≠j)，满足条件SSIM(l_i,l_j)≤ε，则可称l_j属于l_i的ε-近邻集；完成路径聚类后，选取若干个近邻的路径段，通常取3个及以上，并将其映射到惯导地磁指纹库中，组成局域惯导地磁指纹库，供地磁匹配中使用。

进一步地，步骤5)具体为：

地磁匹配由经过处理得到的标准地磁数据的地磁匹配结果来进行确定；在经典粒子滤波的基础上，提出了通过边界化粒子产生的范围来降低粒子滤波的时间复杂度，即粒子产生的区域通过局域地磁指纹库设定粒子边界，对出边界的粒子做遗弃处理；在估算出待定位点的粗略位置后，完成同局域标准地磁指纹库中位置的二次相似度计算，得到地磁信源在指纹库中不同位置上的可信度；

在已地磁为信源的定位过程中，将上述得到的局域地磁指纹库的边界来处理粒子，对不符合约束的粒子做抛弃处理；

d>D_t；

上述公式中，d为边界的长度，D_t为t时刻粒子与边界的欧式距离，满足该约束的粒子才能成为有效粒子；借助于提出的惯导计算坐标公式可以通过状态转移方程进一步优化下一时刻粒子的生成，最终可以较好的得到地磁信源定位的结果；

粒子滤波的主要步骤如下：

1)粒子初始化

在完成相似路径段的地磁指纹聚类后，得到路径段形成的区域D₁和局域地磁指纹库M₁，初始化阶段，粒子的随机生成，控制在区域D₁中，且保证有足够多的粒子幸存；

2)粒子评估

粒子评估过程中，根据地磁指纹库M₁形成观测模型p(z|s)，由高斯概率密度函数类比得出；s代表不同粒子的位置状态，z代表观测值；在地磁信源完成地磁匹配过程中，p(z|s)代表s能够观测到z的概率；

3)粒子归一化

由粒子评估后的得到粒子和其对应的权重，需要归一化粒子的权重，即完成以下公式操作：

4)粒子重采样

当时间不断增加时，存在的粒子会出现两极化，即权重较高的点比较少；此时进行重采样操作，复制权重较高的粒子，而舍弃掉权重低的粒子来保证定位区域的粒子数目不过少；同时为了保证粒子的多样性，防止粒子退化问题的产生，再进行高权重粒子复制时，给复制粒子的位置一个服从N(0,α²)分布的扰动，复制的粒子权重相同，满足边界约束；

5)估算当前地磁信源的位置坐标

在计算得到的s_i和w_i中，剔除掉权重较小的粒子，也即是去除掉离真实位置点较远的粒子，将权重比较大的粒子缩小到一定的区域内，对该区域内的粒子进行均匀求和得到地磁信源的位置坐标，假设最终收敛的粒子数目为m，则有：

对通过粒子滤波算法得出的定位粗略位置坐标P’进行二次相似度计算，即完成粗略位置坐标与局域地磁指纹库中的坐标进行相似度计算：

假设总共有k个点参与计算，则将最终结果标准化，得到地磁信源在局域指纹库中的可信度：

进一步地，步骤6)具体为：

路径匹配中的轨迹匹配模型计算得到的相似度代表惯导信息在未知点的可信度，地磁匹配计算得到的相似度代表地磁信息在该未知点的可信度；

当行走的时间逐渐增加时，坐标计算中的误差也会跟着增加，所以在对惯导信源可信度进行评估时，将平均误差与行走时间建立如下关系：

y_P＝At+B；

y_p为惯导信源平均误差，其值随着时间的增加而应该逐渐的增加。惯导信源的可信度与其平均误差成反比，由上述公式中获得的y_p可得可信度评估中权值W_P：

α为融合系数，其值大于0，α主要用来均衡惯导信源的权值和地磁信源的权值，防止两者之间出现较大的偏差带来的最终可信度融合的失效；

地磁信源采集平均误差与加速度之间的关系，当加速度增加时，地磁信源采集的数据平均误差明显增加，利用多项式对加速度与平均误差的数据进行曲线拟合，因此可得地磁采集平均误差与加速度之间的关系为：

y_M＝Aa³+Ba²+Ca+D；

A,B,C,D为多项式的系数，y_M为得到的加速度与误差之间的关系值。地磁信源的可信度与最终得到的y_M应该成反比，即当加速度增加时，数据采集的平均误差会加大，导致地磁信源的可信度会下降，则有：

β为融合系数，其值大于0；

根据惯导信源可信度评估结果和地磁信源可信度评估结果进行加权和，基于可信度的联合定位为求得总的位置点的相似度：

SIM(P_j,P_j')＝W_P×PSIM(P_j,P_j')+W_M×MSIM(P_j,P_j')；

总的相似度最大即是可信度最高的惯导地磁指纹库中的位置点为本次定位的位置估算坐标。

借由上述技术方案，本发明具有如下优点和有益技术效果：

1)本发明公开的基于可信度的惯导和地磁融合的室内定位算法在定位初始化阶段快速方便构建基于惯导和地磁的指纹库；在线定位实时定位过程中，先通过惯导匹配和改进型粒子滤波算法获取惯导和地磁信源的可信度，然后采用自适应加权可信度算法融合定位结果；整个算法实施过程可避免定位初始化阶段的耗时耗力现象，提升实时定位过程中的定位速度、定位精度和定位稳定性。

2)本发明基于可信度的惯导和地磁融合的室内定位算法，以提升定位速度、定位精度和定位稳定性为目的，在地磁与惯导结合定位思想基础之上，设计出一种室内定位算法来降低其缺陷对定位性能所造成的影响，为普通用户提供便利、快捷、准确的定位服务。

附图说明

图1是本发明中测试环境平面图；

图2是本发明中地磁数据滤波示意图；

图3是本发明算法流程图；

图4是本发明中改进型粒子滤波算法示意图；

图5是本发明中惯导计算误差随定位时间关系图；

图6是本发明中地磁匹配误差与加速度的变化关系图。

具体实施方式

在下面的描述中阐述了很多具体细节以便于充分理解本发明。但是本发明能够以很多不同于此描述的其他方式来实施，本领域技术人员可以在不违背本发明内涵的情况下做类似推广，因此本发明不受下面公开的具体实施例的限制。

下面结合具体实施例和附图对本发明的技术方案进行清楚、完整的描述。

一种基于可信度的惯导和地磁融合的室内定位算法，包括以下步骤：

步骤1，定位初始化阶段，在定位区域中设定坐标原点，坐标系方向按照北为x轴正向，西为y轴正向划分，然后对区域划分路径段，相邻两个路径段之间的间隔不小于1m；

步骤2，通过惯导计算得出采集者行走在路径段内每一步的位置点坐标，同时并在每个坐标点处获取标准地磁信息，录入建立惯导地磁指纹库；

步骤3，实时定位时，利用初始位置点计算在行走路径中的坐标，然后执行惯导坐标相似度标准化，最后通过路径匹配算法获取惯导信源的可信度和当前所在路径相似度；

步骤4，根据路径相似度聚类缩小惯导地磁指纹库得到局域地磁指纹库；

步骤5，运用改进型粒子滤波算法获取地磁匹配后的估计位置，代入到局域地磁指纹库中进行二次计算，完成地磁信源坐标在指纹库中的可信度获取；

步骤6，对地磁信源可信度和惯导信源可信度进行自适应加权算法融合；得出可惜度最高的位置点，即为待定位点的估计位置。

本发明提供一种基于可信度的惯导和地磁融合的室内定位算法，以提升定位速度和定位精度，在地磁与惯导结合定位算法思想的基础之上，设计出一种室内定位算法来降低其缺陷对定位性能所造成的影响，为普通用户提供便利、快捷、准确的定位服务。

实施例1

一种基于可信度的惯导和地磁融合的室内定位算法，包括以下步骤：

定位初始化阶段，需要完成区域的选择和该区域指纹库的构建，测试环境为某学生公寓，宽约为25m，长约为60m，测试环境平面图如1所示，本实例测试采用华为荣耀Play手机完成数据采集的工作；具体步骤如下：

步骤1)：在定位区域内设定原点坐标，设定测试区域右下角点为原点(0,0)，在该区域内划分坐标系，坐标系的x轴正向为正北方向，坐标系的x轴反向为正南方向，坐标系的y轴正向为正西方向，坐标系的y轴反向为正东方向；在定位区域内划分路径段，并按照l₁，l₂，l₃...l₁₁的模式进行编号；

步骤2)：构建惯导地磁指纹库需要采集者完成室内定位区域内的数据采集；首先，采集者使用智能手机，分别在划分好的路径段内行走，行走过程中计算惯导产生的每一步坐标：

①惯导坐标计算

对一段路径进行数据信息采集，假设采集者的初始位置为(x₁,y₁)，则通过以下公式完成每一步的坐标的计算：

x₂＝x₁+L_AB·cosθ

y₂＝y₁+L_AB·sinθ

公式中，L_AB代表采集者的步长的估计值，(x₂,y₂)为采集者行走的下一步的坐标点；

②标准地磁信息转换

在采集者进行路径段信息采集时，进行每一步的惯导坐标计算时，还需要完成地磁信息导标准地磁信息的转换；

如图1所示，为真实场景内获取到同一点的地磁数据，横线为对该点采集到的数据完成滤波后的得到地磁数据，需要对地磁数据先进行处理，得到稳定的地磁数据值之后，再完成到标准坐标轴的转换；如下为手机坐标到标准地磁坐标信息的转换，其中，绕手机x轴旋转的角为俯仰角，定义为α，手机绕y轴旋转的角为翻滚角，定位为β，手机绕z轴旋转的角为方位角，定义为θ；

定位区域内以路径段为单位对区域进行划分后，完成对每个路径段的数据信息和位置信息的采集；每个路径段的内采集到的信息包含l_i,x,y,U_x,U_y,U_z,D,T八个特征，其中l_i为路径段的标号，x和y为惯导计算获得的位置坐标，U_x，U_y，U_z为经过坐标变换后的三个方向的标准地磁数值，D为经过滤波模型获取到的手机采集地磁信息过程中的方位角，T为位置点生成数据信息入库的时间；

如表1为上述操作完成后构建的惯导地磁指纹库。

表1是惯导地磁指纹库

实时定位过程中，根据初始化阶段创建的惯导地磁指纹库获取行人待定位节点的估计位置，如图3所示的算法流程图，具体的步骤如下：

步骤1)：行人坐标计算同步骤二采集者进行坐标计算相同，行人在定位阶段，手持手机在定位区域内完成定位时，首先完成行人行走的每一步的惯导计算坐标，得到行人行走的一段路径和该路径下的坐标点；

1)坐标匹配

获取到行走过程中的路径点若为P₁’,P₂’,P₃’,...,P_j’,...,P_m’，要通过方向的比较和坐标的比较来对比不同点之间的相似度：

①方向比较，指纹库和实测方位角若分别为D_j和D_j’，采用方位角的正弦函数的绝对值差的绝对值来代表它们之间的相似度：

DirDist_j＝||sinD_j|-|sinD_j'||；

②坐标比较，假设指纹库路径段和实测路径的位置点的坐标分别为(x_j，y_j)和(x_j’，y_j’)，则它们之间的相似度为：

③计算相似距离，假设方向比较权重为W_D，坐标比较的权重为W_L，则两个位置点的之间的相似距离为：

PDIST(Pj,P_j')＝DirDist_j×W_D+LocDist_j×W_L；

④相似距离标准化，将相似距离转化为0到1之间的数，即得到惯导信源的可信度：

2)路径匹配

行人行走的实际路径若为l’，惯导地磁指纹库中若有n个路径段l₁，l₂，l₃...,...,l_i,...l_n；

①实测路径与指纹库路径的相似距离：

②相似距离标准化，将相似距离转化为0到1之间的数：

步骤2)：给出近邻阈值，行人行走的轨迹段为l_i，若存在轨迹段(l_j)(i≠j)，满足条件SSIM(l_i,l_j)≤ε，则可称l_j属于l_i的ε-近邻集；完成路径聚类后，选取若干个近邻的路径段，通常取3个及以上，并将其映射到惯导地磁指纹库中，组成局域惯导地磁指纹库，供地磁匹配中使用；

步骤3)：在已地磁为信源的定位过程中，将上述得到的局域地磁指纹库的边界来处理粒子，对不符合约束的粒子做抛弃处理；

d>D_t；

上述公式中，d为边界的长度，D_t为t时刻粒子与边界的欧式距离，满足该约束的粒子才能成为有效粒子；借助于提出的惯导计算坐标公式可以通过状态转移方程进一步优化下一时刻粒子的生成，最终可以较好的得到地磁信源定位的结果；

图4为改进型粒子滤波的简单过程，主要过程如下：

1)粒子初始化

在完成相似路径段的地磁指纹聚类后，得到路径段形成的区域D₁和局域地磁指纹库M₁，初始化阶段，粒子的随机生成，控制在区域D₁中，且保证有足够多的粒子幸存；

2)粒子评估

粒子评估过程中，根据地磁指纹库M₁形成观测模型p(z|s)，由高斯概率密度函数类比得出；s代表不同粒子的位置状态，z代表观测值；在地磁信源完成地磁匹配过程中，p(z|s)代表s能够观测到z的概率；

3)粒子归一化

由粒子评估后的得到粒子和其对应的权重，需要归一化粒子的权重，即完成以下公式操作：

4)粒子重采样

当时间不断增加时，存在的粒子会出现两极化，即权重较高的点比较少；此时进行重采样操作，复制权重较高的粒子，而舍弃掉权重低的粒子来保证定位区域的粒子数目不过少；同时为了保证粒子的多样性，防止粒子退化问题的产生，再进行高权重粒子复制时，给复制粒子的位置一个服从N(0,α²)分布的扰动，复制的粒子权重相同，满足边界约束；

5)估算当前地磁信源的位置坐标

在计算得到的s_i和w_i中，剔除掉权重较小的粒子，也即是去除掉离真实位置点较远的粒子，将权重比较大的粒子缩小到一定的区域内，对该区域内的粒子进行均匀求和得到地磁信源的位置坐标；假设最终收敛的粒子数目为m，则有：

对通过粒子滤波算法得出的定位粗略位置坐标P’进行二次相似度计算，即完成粗略位置坐标与局域地磁指纹库中的坐标进行相似度计算：

假设总共有k个点参与计算，则将最终结果标准化，得到地磁信源在局域地磁指纹库中的可信度：

步骤4)具体为：

当行走的时间逐渐增加时，坐标计算中的误差也会跟着增加，所以在对惯导信源可信度进行评估时，将平均误差与行走时间建立如下关系，如图5所示关系图：

y_P＝At+B；

y_p为惯导信源平均误差，其值随着时间的增加而应该逐渐的增加，关系图中得出的A为0.1962，B为0.0333；惯导信源的可信度与其平均误差成反比，由上述公式中获得的y_p可得可信度评估中权值W_P：

α为融合系数，其值大于0，α主要用来均衡惯导信源的权值和地磁信源的权值，防止两者之间出现较大的偏差带来的最终可信度融合的失效；

地磁信源采集平均误差与加速度之间的关系，当加速度增加时，地磁信源采集的数据平均误差明显增加，利用多项式对加速度与平均误差的数据进行曲线拟合，如图6所示关系图；因此可得地磁采集平均误差与加速度之间的关系为：

y_M＝Aa³+Ba²+Ca+D；

A,B,C,D为多项式的系数，y_M为得到的加速度与误差之间的关系值；关系图中的A＝-4.382，B＝13.38，C＝-7.928，D＝1.747；地磁信源的可信度与最终得到的y_M应该成反比，即当加速度增加时，数据采集的平均误差会加大，导致地磁信源的可信度会下降，则有：

β为融合系数，其值大于0；

根据惯导信源可信度评估结果和地磁信源可信度评估结果进行加权和，基于可信度的联合定位为求得总的位置点的相似度：

SIM(P_j,P_j')＝W_P×PSIM(P_j,P_j')+W_M×MSIM(P_j,P_j')；

总的相似度最大即是可信度最高的惯导地磁指纹库中的位置点为本次定位的位置估算坐标。

传统地磁匹配算法主要完成将待定位节点位置处的地磁数值与指纹库进行比对，得出相似度最高的点即为最终定位的位置点；

N_uv表示位置(u,v)上的N维特征向量；m表示实时测量N维特征向量；N表示获得数据总点数；N_uv+i表示位置(u,v+i)上的特征量，m_i表示测量的第i个特征量；

惯导辅助地磁定位算法针对行人定位阶段需要匹配整个基准库地磁信息导致的耗时和由于环境影响导致采集信息出现偶然较大误差等问题，引入了惯导技术来减少行人定位时地磁匹配的时间，并且对偶然较大误差有修正作用；

本实例的定位结果如表2所示，利用本实例中的测试数据完成同传统地磁匹配算法和惯导辅助地磁定位算法的对比测试，得出结果：

算法名称	传统地磁定位算法	惯导辅助地磁定位算法	本发明算法
				最大误差(m)	10.80	8.40	5.00
最小误差(m)	0	0	0.07
				平均定位误差(m)	3.49	2.95	1.47
定位提升(％)	57.88	50.17	-

由表2可知，本发明的定位精度更高，定位稳定性更好。

以上所述，仅是本发明的较佳实施例而已，并非对本发明作任何形式上的限制，故凡是未脱离本发明技术方案内容，依据本发明的技术实质对以上实施例所作的任何简单修改、等同变化与修饰,均仍属于本发明技术方案的范围内。

Claims (8)

1.一种基于可信度的惯导和地磁融合的室内定位算法，其特征在于，包括如下步骤：

1)定位初始化阶段，在定位区域中设定坐标原点，坐标系方向按照北为x轴正向，西为y轴正向划分，然后对区域划分路径段，相邻两个路径段之间的间隔不小于1m；

2)通过惯导计算得出采集者行走在路径段内每一步的位置点坐标，同时并在每个坐标点处获取标准地磁信息，录入建立惯导地磁指纹库；

3)实时定位时，利用初始位置点计算在行走路径中的坐标，然后执行惯导坐标相似度标准化，最后通过路径匹配算法获取惯导信源的可信度和当前所在路径相似度；

4)根据路径相似度聚类缩小惯导地磁指纹库得到局域地磁指纹库；

5)运用改进型粒子滤波算法获取地磁匹配后的估计位置，代入到局域地磁指纹库中进行二次计算，完成地磁信源坐标在指纹库中的可信度获取；

6)对地磁信源可信度和惯导信源可信度进行自适应加权算法融合；得出可信度最高的位置点，即为待定位点的估计位置。

2.如权利要求1所述的基于可信度的惯导和地磁融合的室内定位算法，其特征在于，步骤1)具体为：

在定位区域内设定原点坐标，假设其中的一点为原点(x₀，y₀)，在该区域内划分坐标系，坐标系的x轴正向为正北方向，坐标系的x轴反向为正南方向，坐标系的y轴正向为正西方向，坐标系的y轴反向为正东方向；在定位区域内划分路径段，并按照l₁，l₂，l₃...的模式进行编号。

3.如权利要求1所述的基于可信度的惯导和地磁融合的室内定位算法，其特征在于，步骤2)具体为：

1)构建惯导地磁指纹库需要采集者完成室内定位区域内的数据采集

首先，采集者使用智能手机，分别在划分好的路径段内行走，行走过程中计算惯导产生的每一步坐标：

①惯导坐标计算

对一段路径进行数据信息采集，假设采集者的初始位置为(x₁,y₁)，则通过以下公式完成每一步的坐标的计算：

x₂＝x₁+L_AB·cosθ；y₂＝y₁+L_AB·sinθ；

公式中，L_AB代表采集者的步长的估计值，(x₂,y₂)为采集者行走的下一步的坐标点；

②标准地磁信息转换

在采集者进行路径段信息采集时，进行每一步的惯导坐标计算时，还需要完成地磁信息导标准地磁信息的转换；如下为手机坐标到标准地磁坐标信息的转换，其中，绕手机x轴旋转的角为俯仰角，定义为α，手机绕y轴旋转的角为翻滚角，定位为β，手机绕z轴旋转的角为方位角，定义为θ；

2)惯导地磁指纹库构建

定位区域内以路径段为单位对区域进行划分后，完成对每个路径段的数据信息和位置信息的采集；每个路径段的内采集到的信息包含l_i,x,y,U_x,U_y,U_z,D,T八个特征，其中，l_i为路径段的标号，x和y为惯导计算获得的位置坐标，U_x，U_y，U_z为经过坐标变换后的三个方向的标准地磁数值，D为经过滤波模型获取到的手机采集地磁信息过程中的方位角，T为位置点生成数据信息入库的时间。

4.如权利要求1所述的基于可信度的惯导和地磁融合的室内定位算法，其特征在于，步骤3)具体为：

1)行人坐标计算

同步骤2)采集者进行坐标计算相同，行人在定位阶段，手持手机在定位区域内完成定位时，首先完成行人行走的每一步的惯导计算坐标，得到行人行走的相似路径和坐标点；

2)坐标匹配

获取到行走过程中的路径点若为P₁’,P₂’,P₃’,...,P_j’,...,P_m’，主要通过方向的比较和坐标的比较来对比不同点之间的相似度：

①方向比较，指纹库和实测方位角若分别为D_j和D_j’，采用方位角的正弦函数的绝对值差的绝对值来代表它们之间的相似度：

DirDist_j＝||sinD_j|-|sinD_j′||；

②坐标比较，假设指纹库路径段和实测路径的位置点的坐标分别为(x_j,y_j)和(x_j’,y_j’)，则它们之间的相似度为：

③计算相似距离，假设方向比较权重为W_D，坐标比较的权重为W_L，则两个位置点的之间的相似距离为：

PDIST(P_j,P_j')＝DirDist_j×W_D+LocDist_j×W_L；

④相似距离标准化，将相似距离转化为0到1之间的数，即得到指纹库节点和未知节点的惯导信源可信度：

3)路径匹配

行人行走的实际路径若为l’，惯导地磁指纹库中若有n个路径段l₁,l₂,l₃,...,l_i,...l_n；

①实测路径与指纹库路径的相似距离：

②相似距离标准化，将相似距离转化为0到1之间的数：

5.如权利要求1所述的基于可信度的惯导和地磁融合的室内定位算法，其特征在于，步骤4)具体为：

给出近邻阈值，行人行走的轨迹段为l_i，若存在轨迹段(l_j)(i≠j)，满足条件SSIM(l_i,l_j)≤ε，则可称l_j属于l_i的ε-近邻集；完成路径聚类后，选取若干个近邻的路径段，通常取3个及以上，并将其映射到惯导地磁指纹库中，组成局域惯导地磁指纹库，供地磁匹配中使用。

6.如权利要求1所述的基于可信度的惯导和地磁融合的室内定位算法，其特征在于，步骤5)具体为：

地磁匹配由经过处理得到的标准地磁数据的地磁匹配结果来进行确定；在经典粒子滤波的基础上，提出了通过边界化粒子产生的范围来降低粒子滤波的时间复杂度，即粒子产生的区域通过局域地磁指纹库设定粒子边界，对出边界的粒子做遗弃处理；在估算出待定位点的粗略位置后，完成同局域标准地磁指纹库中位置的二次相似度计算，得到地磁信源在指纹库中不同位置上的可信度；

在已地磁为信源的定位过程中，将上述得到的局域地磁指纹库的边界来处理粒子，对不符合约束的粒子做抛弃处理；

d>D_t；

上述公式中，d为边界的长度，D_t为t时刻粒子与边界的欧式距离，满足该约束的粒子才能成为有效粒子；借助于提出的惯导计算坐标公式可以通过状态转移方程进一步优化下一时刻粒子的生成，最终可以较好的得到地磁信源定位的结果。

7.如权利要求6所述的基于可信度的惯导和地磁融合的室内定位算法，其特征在于，改进型粒子滤波的主要步骤如下：

1)粒子初始化

在完成相似路径段的地磁指纹聚类后，得到路径段形成的区域D₁和局域地磁指纹库M₁，初始化阶段，粒子的随机生成，控制在区域D₁中，且保证有足够多的粒子幸存；

2)粒子评估

粒子评估过程中，根据地磁指纹库M₁形成观测模型p(z|s)，由高斯概率密度函数类比得出；s代表不同粒子的位置状态，z代表观测值；在地磁信源完成地磁匹配过程中，p(z|s)代表s能够观测到z的概率；

3)粒子归一化

由粒子评估后的得到粒子和其对应的权重，需要归一化粒子的权重，即完成以下公式操作：

4)粒子重采样

当时间不断增加时，存在的粒子会出现两极化，即权重较高的点比较少，此时进行重采样操作，复制权重较高的粒子，而舍弃掉权重低的粒子来保证定位区域的粒子数目不过少；同时为了保证粒子的多样性，防止粒子退化问题的产生，再进行高权重粒子复制时，给复制粒子的位置一个服从N(0，α²)分布的扰动，复制的粒子权重相同，满足边界约束；

5)估算当前地磁信源的位置坐标

在计算得到的s_i和w_i中，剔除掉权重较小的粒子，也即是去除掉离真实位置点较远的粒子，将权重比较大的粒子缩小到一定的区域内，对该区域内的粒子进行均匀求和得到地磁信源的位置坐标，假设最终收敛的粒子数目为m，则有：

对通过粒子滤波算法得出的定位粗略位置坐标P’进行二次相似度计算，即完成粗略位置坐标与局域地磁指纹库中的坐标进行相似度计算：

假设总共有k个点参与计算，则将最终结果标准化，得到地磁信源在局域地磁指纹库中的可信度：

8.如权利要求1所述的基于可信度的惯导和地磁融合的室内定位算法，其特征在于，步骤6)具体为：

路径匹配中的轨迹匹配模型计算得到的相似度代表惯导信息在未知点的可信度，地磁匹配计算得到的相似度代表地磁信息在该未知点的可信度；

将平均误差与行走时间建立如下关系：

y_P＝At+B；

y_p为惯导信源平均误差，其值随着时间的增加而应该逐渐的增加；惯导信源的可信度与其平均误差成反比，由上述公式中获得的y_p可得可信度评估中权值W_P：

α为融合系数，其值大于0，利用多项式对加速度与平均误差的数据进行曲线拟合，得地磁采集平均误差与加速度之间的关系为：

y_M＝Aa³+Ba²+Ca+D；

A，B，C，D为多项式的系数，y_M为得到的加速度与误差之间的关系值；地磁信源的可信度与最终得到的y_M应该成反比，即当加速度增加时，数据采集的平均误差会加大，导致地磁信源的可信度会下降，则有：

β为融合系数，其值大于0；

根据惯导信源可信度评估结果和地磁信源可信度评估结果进行加权和，基于可信度的联合定位为求得总的位置点的相似度：

SIM(P_j,P_j')＝W_P×PSIM(P_j,P_j')+W_M×MSIM(P_j,P_j')；

总的相似度最大即是可信度最高的惯导地磁指纹库中的位置点为本次定位的位置估算坐标。