一种稳健的室内定位航向角度估计方法

1.本发明属于室内定位技术领域，更具体地，涉及一种稳健的室内定位航向角度估计方法。


背景技术：

2.室内定位技术作为万物互联的重要一环，正改变着人们的日常生活，并在儿童老人监管、安全紧急防控、商业个人定位推销等领域起到重要作用。三轴陀螺仪和加速度计组成的inertial measurement unit(imu)器件体积小、价格便宜、耗电量小，基于imu器件的室内定位技术已经得到广泛应用，例如手机定位、扫地机器人、移动测量小车等。
3.由于imu传感器误差与器件积分机制，其航向角度的漂移一直是室内定位中的一大主要瓶颈问题，且在实际定位中它将产生不可忽略的影响，它是定位精度和可靠性的关键。
4.为了克服航向角度漂移的问题，最常用的方法是借助地球磁场信息获得绝对的航向角度以克服陀螺航向角漂移。但在室内环境下，受建筑物自身磁场与电子设备电磁干扰影响，很难获得稳定可靠的磁场信息。为了探测室内磁场干扰，当地磁场参考模型往往被使用作为参考基准；此外，参考陀螺航向角的变化信息也可用来探测磁突变。但由于室内环境的复杂，仅借助磁场的方法往往不能提供稳定可靠的航向信息。


技术实现要素：

5.本发明通过提供一种稳健的室内定位航向角度估计方法，解决现有技术中室内定位航向角精度较低、可靠性较差的问题。
6.本发明提供一种稳健的室内定位航向角度估计方法，包括以下步骤：
7.步骤1、获取行人运动过程中的imu数据、磁力计数据；基于所述imu数据和所述磁力计数据判断行人的行走轨迹是否为直线路径；若为直线路径，则在经典步长模型辅助下，确定行人行走直线路径长度；
8.步骤2、通过wifi或蓝牙获得粗定位信息，基于所述粗定位信息和所述行人行走直线路径长度，判断行人当前所处位置对应的场景区域类型，并得到场景信息，所述场景信息包括当前场景对应的所有的待选直线轨迹；
9.步骤3、将所述磁力计数据与当地磁场模型进行对比，消除磁干扰粗差；基于步骤1获得的行人运动信息，得到行人行走直线时平均磁角度，消除磁航向角度粗差；
10.步骤4、结合步骤1得到的行人运动信息和步骤2得到的所述场景信息进行建筑物地图角度匹配，得到第一航向角；采用步骤3得到的所述行人行走直线时平均磁角度对所述第一航向角进行校正，得到最终选取的建筑物地图航向角。
11.优选的，所述步骤1中，所述imu数据包括陀螺仪三轴原始数据和加速度计三轴原始数据，所述陀螺仪三轴原始数据记为ω
x
、ω
y
、ω
z
，所述加速度计三轴原始数据记为a
x
、a
y
、a
z
；所述磁力计数据包括磁力计三轴原始数据，记为m
x
、m
y
、m
z
；
12.判断行人的行走轨迹是否为直线路径采用如下公式：
[0013][0014]
|θ
k
‑
θ
k
‑1|＜γ2ꢀꢀ
(2)
[0015]
若满足公式(1)和(2)，则判定行人的行走轨迹为直线路径；
[0016]
其中，n为行人在直行探测窗口长度范围内陀螺仪三轴原始数据的采集数目；为在第i时刻当地水平坐标系下垂直方向的陀螺仪测量值，σ为陀螺噪声；θ
k
为行人行走第k步对应的航向角，θ
k
‑1为行人行走第k
‑
1步对应的航向角，航向角通过结合所述磁力计三轴原始数据和所述加速度计三轴原始数据得到；γ1为第一探测阈值，γ2为第二探测阈值。
[0017]
优选的，所述步骤1中，所述行人行走直线路径长度采用如下公式获得：
[0018][0019]
其中，d为行人行走直线路径长度，s为行人直行步数，直行步数通过探测加速度幅值的波峰数目得到；k为经典步长模型中的刻度因子项；a
max
和a
min
为行人直行过程中在每步探测到的加速度幅值最大值和加速度幅值最小值；a
max
和a
min
通过所述加速度计三轴原始数据变换得到。
[0020]
优选的，所述步骤2中，结合地图信息和建筑物实景，将室内环境划分为多个场景区域；
[0021]
根据所述粗定位信息对应的坐标是否在某个场景区域的坐标范围内，并结合所述行人行走直线路径长度与第三探测阈值的大小关系，判断行人当前所处位置对应的场景区域类型。
[0022]
优选的，将室内环境划分为宽区域、办公区域、走廊区域；
[0023]
判断行人当前所处位置对应的场景区域类型采用如下公式：
[0024][0025]
其中，p
m
表示连续m个粗定位位置，d为行人行走直线路径长度，γ3为第三探测阈值。
[0026]
优选的，所述步骤3中，消除磁干扰粗差采用如下公式：
[0027][0028]
其中，m
x
、m
y
和m
z
为磁力计的实测值，m0为当地参考磁场幅值，γ4为第四探测阈值。
[0029]
优选的，所述步骤3中，消除磁航向角度粗差采用如下公式：
[0030]
[0031][0032]
其中，为行人行走直线时的平均磁角度，θ
i
为行人行走第i步对应的航向角度，s为行人直行步数，γ5为第五探测阈值。
[0033]
优选的，所述步骤4中，进行建筑物地图角度匹配，得到第一航向角采用如下公式：
[0034][0035]
其中，θ
building
为匹配选取的直线轨迹对应的航向角，记为第一航向角；θ
line
为待选直线轨迹对应的航向角，argmin(θ)函数表示取最小角度。
[0036]
优选的，所述步骤4中，得到第一航向角之后，还包括：将所述行人行走直线路径长度与所述匹配选取的直线轨迹对应的直线距离进行对比；所述匹配选取的直线轨迹对应长度排第二的直线距离记为第二长度；若所述行人行走直线路径长度大于所述第二长度，则判断为建筑物地图角度匹配成功。
[0037]
优选的，所述步骤4中，对所述第一航向角进行校正采用如下公式：
[0038][0039]
其中，γ6为第六探测阈值。
[0040]
本发明中提供的一个或多个技术方案，至少具有如下技术效果或优点：
[0041]
在发明中，提出的室内定位航向角度估计方法智能融合了磁场、地图信息，有效克服了室内陀螺航向角度漂移的影响，提高了航向角度估计的稳健性；结合行人行走特点，提出在线估计磁干扰方法，有效缓解了室内磁干扰，为室内稳健运用磁航向角度提供了方案；有效结合室内建筑物角度、场景信息和行人运动信息，从而提高了建筑物地图航向角度的匹配率，加强了航向角度估计的稳定性。
附图说明
[0042]
图1为本发明实施例提供的一种稳健的室内定位航向角度估计方法的流程图。
具体实施方式
[0043]
为了更好的理解上述技术方案，下面将结合说明书附图以及具体的实施方式对上述技术方案进行详细的说明。
[0044]
本实施例提供了一种稳健的室内定位航向角度估计方法，包括以下步骤：
[0045]
步骤1、获取行人运动过程中的imu数据、磁力计数据；基于所述imu数据和所述磁力计数据判断行人的行走轨迹是否为直线路径；若为直线路径，则在经典步长模型辅助下，确定行人行走直线路径长度。
[0046]
即结合imu和磁力计数据，分析在行人运动中航向角速率与航向角的变化特性，判断行人行走轨迹是否为直线路径；在经典步长模型辅助下，确定行人行走直线路径长度，实现运动感知增强。
[0047]
具体的，所述imu数据包括陀螺仪三轴原始数据和加速度计三轴原始数据，所述陀螺仪三轴原始数据记为ω
x
、ω
y
、ω
z
，所述加速度计三轴原始数据记为a
x
、a
y
、a
z
；所述磁力计数据包括磁力计三轴原始数据，记为m
x
、m
y
、m
z
。
[0048]
在行人正常行走时，分析行人在每步内陀螺角速度与航向角度变化，判断行人是否走直线，如公式(1)、(2)所示；若满足公式(1)和(2)，则判定行人的行走轨迹为直线路径。判断行人的行走轨迹为直线路径后，根据经典步长weinberg模型，得到行人直线距离长度d，如公式(3)所示。
[0049][0050]
|θ
k
‑
θ
k
‑1|＜γ2ꢀꢀꢀꢀ
(2)
[0051][0052]
其中，n表示在行人在直行探测窗口长度(例如，每步时间)范围内陀螺仪三轴原始数据的采集数目，为在第i时刻当地水平坐标系下垂直方向的陀螺测量值，σ为陀螺噪声(初始化时，静态采集一段窗口的imu数据，计算这一段数据的方差值得到噪声σ)，θ
k
为行人在第k步对应的航向角，θ
k
‑1为行人行走第k
‑
1步对应的航向角，航向角通过结合所述磁力计三轴原始数据和所述加速度计三轴原始数据得到；γ1和γ2分别为第一探测阈值和第二探测阈值；d为行人行走直线路径长度，s为行人直行步数(探测加速度幅值的波峰数目，即为直行步数)，k为经典步长weinberg模型中的刻度因子项，a
max
和a
min
为行人直行过程中在每步探测到的加速度幅值最大值和最小值；a
max
和a
min
通过所述加速度计三轴原始数据变换得到。
[0053]
步骤2、通过wifi或蓝牙获得粗定位信息，基于所述粗定位信息和所述行人行走直线路径长度，判断行人当前所处位置对应的场景区域类型，并得到场景信息，所述场景信息包括当前场景对应的所有的待选直线轨迹。
[0054]
即结合行人直行路径距离与wifi/蓝牙提供的粗定位结果，判断当前所处场景区域类型，实现情景感知增强。
[0055]
具体的，结合地图信息和建筑物实景，将室内环境划分为多个场景区域；根据所述粗定位信息对应的坐标是否在某个场景区域的坐标范围内，并结合所述行人行走直线路径长度与第三探测阈值的大小关系，判断行人当前所处位置对应的场景区域类型。即根据室内环境，划分室内场景。
[0056]
例如，将室内环境划分为为宽区域、办公区域、走廊区域这三种常见场景(结合地图信息和建筑物实景，人为主观划分)；并根据行人直行路径距离与wifi/蓝牙提供的粗定位结果，判断当前所处场景区域类型t，如公式(4)所示。
[0057][0058]
其中，p
m
表示连续m个粗定位位置，m的值由实验场景确定；d为行人直行距离，γ3为第三探测阈值。
[0059]
步骤3、将所述磁力计数据与当地磁场模型进行对比，消除磁干扰粗差；基于步骤1
获得的行人运动信息，得到行人行走直线时平均磁角度，消除磁航向角度粗差。
[0060]
即结合运动感知内容，在线校正磁干扰，缓解室内磁干扰对磁场角度的影响，进而抑制陀螺航向角度出现大误差。
[0061]
具体的，将磁力计的实测值(m
x
，m
y
和m
z
)与当地磁场模型进行对比，以消除磁干扰粗差(即将不满足公式(5)的磁力计数据进行剔除)，消除磁干扰粗差采用公式(5)。
[0062][0063]
结合步骤1判断结果，得到行人行走直线时平均磁角度消除磁航向角度粗差(即将不满足公式(6)的θ
i
剔除)，消除磁航向角度粗差采用如下公式：
[0064][0065][0066]
其中，m
x
、m
y
和m
z
为磁力计的实测值，m0为当地参考磁场幅值，为行人行走直线时的平均磁角度，θ
i
为由磁力计计算得到的行人行走第i步对应的航向角度，s为行人直行步数，γ4和γ5分别为第四探测阈值和第五探测阈值。
[0067]
步骤4、结合步骤1得到的行人运动信息和步骤2得到的所述场景信息进行建筑物地图角度匹配，得到第一航向角；采用步骤3得到的所述行人行走直线时平均磁角度对所述第一航向角进行校正，得到最终选取的建筑物地图航向角。
[0068]
即结合运动感知和情景感知内容，结合磁场信息，匹配建建筑物地图角度，正确校正陀螺航向角。
[0069]
具体的，结合步骤1得到的行人运动信息与步骤2得到的场景信息，去匹配附近可行的待选直线轨迹；如果行人行走的直线轨迹超过了第二长度的预选轨迹，则进行地图角度匹配(即将所述行人行走直线路径长度与所述匹配选取的直线轨迹对应的直线距离进行对比；所述匹配选取的直线轨迹对应长度排第二的直线距离记为第二长度；若所述行人行走直线路径长度大于所述第二长度，则判断为建筑物地图角度匹配成功)，并运用步骤3得到的磁角度作为控制阈值，防止出现匹配粗差，具体如公式(8)、(9)所示。
[0070][0071][0072]
其中，θ
building
为匹配选取的直线轨迹对应的航向角，记为第一航向角；θ
line
为待选直线轨迹对应的航向角，argmin(θ)函数表示取最小角度；为步骤3确定的行人行走直行时的平均磁角度，γ6为第六探测阈值。
[0073]
基于上述记载，如图1所示，本发明提供一种稳健的室内定位航向角度估计方法，主要包括如下步骤：
[0074]
结合imu传感器和磁力计给出的测量值，判断行人是否走直线，得到直线距离；
[0075]
结合行人直线距离和wifi/蓝牙粗定位结果，区分行人所在的室内场景，实现情景
感知增强；
[0076]
结合当地磁场模型，在线估计磁干扰，提出磁航向角度粗差，并结合行人直线距离长度，辅助建筑物地图航向角度匹配；
[0077]
结合前面三者信息，利用imu角度积分运算特性，引用经典卡尔曼滤波模型，稳健估计与校正imu姿态角度，实现室内定位航向角度稳健输出。
[0078]
综上，本发明提供的方法可以以手机为平台，智能结合手机内置imu、磁场和地图信息，运用运动情景感知增强航向角估计，并结合地图数据的场景信息辅助航向角估计，在室内复杂环境下提供高精度、稳定的航向角度。此方法也适用于室内定位其他应用平台。
[0079]
本发明实施例提供的一种稳健的室内定位航向角度估计方法至少包括如下技术效果：
[0080]
1)提出一种稳健的室内航向角度估计算法，此算法智能融合了磁场、地图信息，有效克服了室内陀螺航向角度漂移的影响，提高了航向角度估计的稳健性；
[0081]
2)结合行人行走特点(行人行走过程中往往选择直线最短路径到达目的地)，提出在线估计磁干扰方法，有效缓解了室内磁干扰，为室内稳健运用磁航向角度提供了方案；
[0082]
3)有效结合室内建筑物角度、场景信息和行人运动信息，从而提高了建筑物地图航向角度的匹配率，加强了航向角度估计的稳定性。
[0083]
最后所应说明的是，以上具体实施方式仅用以说明本发明的技术方案而非限制，尽管参照实例对本发明进行了详细说明，本领域的普通技术人员应当理解，可以对本发明的技术方案进行修改或者等同替换，而不脱离本发明技术方案的精神和范围，其均应涵盖在本发明的权利要求范围当中。