CN107421546B

CN107421546B - 一种基于空间环境磁场特征的无源组合定位方法

Info

Publication number: CN107421546B
Application number: CN201710744801.2A
Authority: CN
Inventors: 李素敏; 王刚刚; 申志飞
Original assignee: Beijing Mai Nail Aite Technologies Co Ltd
Current assignee: Beijing Mai Nail Aite Technologies Co Ltd
Priority date: 2017-08-25
Filing date: 2017-08-25
Publication date: 2019-10-11
Anticipated expiration: 2037-08-25
Also published as: CN107421546A

Abstract

本发明涉及一种基于空间环境磁场特征的无源组合定位方法，将惯导+地磁+地图进行技术组合进行无源组合定位，不需要任何硬件部署、不向外发射和接收电磁信号、不需要维护、可以分辨楼层、不易受遮挡和干扰的优点；特别是针对磁场信号容易受到环境中不断变化的低频电、磁信号干扰，本发明提出事先对磁传感器进行标校，定位时实时对磁干扰进行识别和消除的处理方法，提高磁场测量精度；同时通过匹配算法的改进，提高地磁定位的稳定性，从而确保了该室内定位系统的鲁棒性和对绝大多数应用场景的适应性。

Description

一种基于空间环境磁场特征的无源组合定位方法

技术领域

本发明涉及定位技术领域，尤其是一种基于空间环境磁场特征的无源组合定位方法。

背景技术

室内定位技术是指一种利用无线移动通信网络，通过对接收到的无线电波的一些参数或其他信息进行测量，根据特定的算法对人、车辆或设备在某一时间所处的地理位置进行精确测定，以便为定位终端用户提供相关的位置信息服务，或进行实时检测和追踪的定位与导航技术。

应用比较多的室内定位技术包括蓝牙、WIFI、射频识别(RFID)、超宽带(UWB)、红外和超声波、Zigbee等。蓝牙定位技术数据传输不受视距的影响，但对于复杂的空间环境，蓝牙系统的稳定性稍差，受噪声信号干扰大；WIFI传输距离较短，功耗较高，受噪声信号干扰大；RFID不具有通信能力，抗干扰能力较差，不便于整合到其他系统之中，且用户的安全隐私保障和国际标准化都不够完善；UWB定位技术功耗较高，同时该系统成本高；红外线只能视线传播，穿透性能极差，布局复杂，成本较高，也极易受灯光、烟雾等环境因素影响；超声波在空气中的衰减较大，不适用于大型场合，反射测距时受多径效应和非视距传播影响很大，造成需要精确分析计算的底层硬件设施投资，成本太高；Zigbee传输速率低，定位精度对算法要求较高。

目前，90％以上的室内定位技术是基于WiFi和蓝牙等无线基站的方式，这种定位方法，要么覆盖范围小，要么需要复杂的部署、硬件和安装，维护成本非常高。

发明内容

鉴于上述的分析，本发明旨在提供一种基于空间环境磁场特征的无源组合定位方法，用以解决传统的无线定位技术覆盖范围小，需要在室内部署无线基站，导致安装和维护成本较高的问题。

本发明的目的主要是通过以下技术方案实现的：

一种基于空间环境磁场特征的无源组合定位方法，包括如下步骤：

步骤S1、数据的采集和存储，包括地磁图数据的存储、地图数据的存储、传感器原始参数的存储、惯性数据的采集、磁场数据的采集；

步骤S2、数据的预处理，包括惯性数据校正、磁场数据校正和干扰磁场的识别分离；

步骤S3、根据校正后的惯性数据进行IMU定位，结合IMU定位的定位结果，根据校正和去除干扰后的磁场数据进行地磁匹配定位，结合数字地图数据，对地磁匹配定位结果进行滤波，并修正IMU参数，得到组合定位结果。

进一步地，所述步骤S2中干扰磁场的识别分离过程包括：

对采集到的磁场数据进行中值滤波；

对滤波后的数据进行一阶差分，放大台阶信号；

去除台阶信号；

对去除台阶信号的地磁数据用测量数据一阶差分的均值代替，并进行积分还原，得到干扰分离后的地磁数据。

进一步地，所述步骤S3包括如下子步骤：

S301、利用校正后惯性数据进行IMU定位；

S302、结合IMU定位的定位结果，通过地磁匹配进行地磁定位；

S302、结合数字地图数据，对地磁匹配定位结果进行滤波，并修正IMU参数，得到组合定位结果。

进一步地，利用校正后惯性数据进行IMU定位方法包括：

1)对俯仰角、横滚角和航向角赋初值；

2)进行航位推算；

3)进行零速检测；当三轴陀螺仪连续在五个周期内数据的极差小于某个接近0的阈值时，判定定位终端处于零速状态，启动零速更新；

4)进行零速更新；如果当前定位终端处于零速，基于卡尔曼滤波器计算当前位置、速度、姿态误差，对位置和姿态进行纠正；

5)输出位置、速度和姿态信息；经过航位推算得出的载体的位置、速度和姿态信息输出用于地磁定位和定位结果的滤波处理；

6)对陀螺仪和加速度计参数修正条件进行判断；当累计超过特定数量的零速检测数据时，可启动陀螺仪和加速度计参数校正模块，所述特定数量范围为5～10；

7)对陀螺仪和加速度计的参数校正；在零速条件下，陀螺仪测量值的均值为陀螺仪的零偏；对一段时间积累的零速条件下的陀螺仪的测量值求取均值，作为陀螺仪新的零偏数据，对原来的陀螺仪零偏数据进行更新替换，重新计算当前陀螺仪的零偏；对加速度计的各轴零偏进行更新替换与陀螺仪的各轴零偏更新替换方法相同。

进一步地，所述航位推算是利用四元数，基于前一次的四元数值，以及当前测量得到的加速度、角速度信息计算当前四元数，得到欧拉转换矩阵，最后基于上组加速度、速度、位置以及当前的欧拉转换矩阵，计算当前加速度、速度以及位置，其具体过程包括：

1)依据初始的姿态角计算初始四元数；

2)计算角速度在一个周期内的积分：

3)获得当前的四元数，并把当前时刻的四元数赋给初始四元数；

4)依据当前四元数计算从载体坐标系到导航坐标系的新的旋转矩阵：

5)更新导航坐标系下加速度值、速度值和位置值；

6)更新导航坐标系下更新载体坐标系相对于导航坐标系的姿态角；

7)重复2)～6)步骤，持续进行航位推算。

进一步地，结合IMU定位结果的地磁匹配定位方法包括以下步骤：

1)系统重新启动后，进行首匹配；

所述首匹配启动全局搜索模式，对连续三次的匹配结果进行一致性判决，在搜索定位过程中，首先顺序测量三段长度均为3～10米的磁场序列，在区域内进行全局搜索，通过匹配相关算法，找到和地磁实时图最相似的地磁基准图所在的位置为PM1，PM2，PM3，此时IMU标记的位置是PA1，PA2，PA3；根据所述匹配定位的位置和IMU标记的位置进行首匹配判决，如果满足判决条件，则根据匹配相关算法的定位结果进行输出，如果不满足判决门限，则继续测量，直到满足判决条件为止；

2)首匹配完成后，进行连续匹配定位，所述连续匹配定位每隔设定时间，进行一次匹配定位，所述匹配定位的范围以首匹配位置结合IMU的指示位置为中心，半径为R的范围内进行搜索和匹配定位，所述R的范围为事先根据IMU的定位误差范围确定，以大于所述定位误差范围为基准。

进一步地，所述匹配相关算法包括绝对平均差匹配方法或积相关匹配方法两种，在采用绝对平均差匹配方法进行匹配时，选择相似度值的最小值所对应的位置为最终的匹配位置，采用积相关匹配方法进行匹配时，选择相似度值的最大值所对应的位置为最终的匹配位置。

进一步地，所述S302中，通过卡尔曼滤波算法，对地磁匹配定位结果进行滤波，结合数字地图数据对定位终端载体的运动区域的限定对定位结果进行滤波，所述限定为定位终端载体只能在数字地图的道路上移动，而不能在道路以外的位置上移动；并修正IMU参数，得到组合定位结果。

本发明有益效果如下：

本发明不需要任何硬件部署、不向外发射和接收电磁信号、不需要维护(除了建筑物结构改变以外)、可以分辨楼层、不易受遮挡和干扰的优点；特别是针对磁场信号容易受到环境中不断变化的低频电、磁信号干扰，本发明提出事先对磁传感器进行标校，定位时实时对磁干扰进行识别和消除的处理方法，提高磁场测量精度；同时通过匹配算法的改进，提高地磁定位的稳定性，从而确保了该室内定位系统的鲁棒性和对绝大多数应用场景的适应性。

附图说明

附图仅用于示出具体实施例的目的，而并不认为是对本发明的限制，在整个附图中，相同的参考符号表示相同的部件。

图1为基于空间环境磁场特征的无源组合定位流程图；

图2为干扰磁场识别分离过程图；

图3为IMU定位模块工作过程图；

图4为地磁定位过程图。

具体实施方式

下面结合附图来具体描述本发明的优选实施例，其中，附图构成本申请一部分，并与本发明的实施例一起用于阐释本发明的原理。

本发明的一个具体实施例，公开了一种基于空间环境磁场特征的无源组合定位方法。所述定位方法是一种将惯导+地磁+地图进行技术组合的无源组合定位方法；如图1所示，具体包括如下步骤：

步骤S1、数据采集和存储，包括地磁图数据的存储、地图数据的存储、传感器原始参数的存储、惯性数据的采集、磁场数据的采集。

1.应用场景内的地磁图数据的存储

所述应用场景内的地磁图数据内容包括平面X坐标、平面Y坐标、右向磁场、前向磁场、向下磁场和总场强度；所述数据为包含磁传感器测量模块和定位模块的地磁图采集装置事先测量所得。

2.应用场景内的地图数据的存储

所述应用场景内的地图数据包括建筑物的结构、构造和楼层房间的布置，建筑物内的区域划分、陈设物品布局和道路规划等，所述地图数据为由应用场景的建筑CAD图和场景布设图的数字化制作生成。

3.传感器原始参数存储

所述原始参数包括磁传感器和Mems IMU的原始参数。所述Mems IMU的原始参数包括：陀螺仪三轴的零偏ω_x0，ω_y0，ω_z0，加速度计三轴的零偏a_x0，a_y0，a_z0，加速度计三轴的刻度因数误差S_x0，S_y0，S_z0，加速度计三轴的交叉耦合因子M_xy，M_yz，M_zx；所述磁传感器原始参数包括：当地磁定位采用矢量特征进行匹配定位时，存储的磁传感器三轴的零偏b_x，b_y，b_z和灵敏度s_x，s_y，s_z；当地磁定位采用标量特征进行定位时，存储的磁传感器三轴的零偏b_x，b_y，b_z、灵敏度s_x，s_y，s_z和不正交度误差u_x，u_y，u_z，所述磁传感器三轴的零偏和灵敏度是在零磁实验室进行测试标定后给出。

4.惯性数据的采集

所述惯性数据为包括陀螺仪和加速度计的惯性传感器测量所得，包括ω_x1，ω_y1，ω_z1，a_x1，a_y1，a_z1；所述ω_x1，ω_y1，ω_z1为载体坐标系下定位终端三个方向的陀螺仪的角速度测量值；所述a_x1，a_y1，a_z1为载体坐标系下定位终端三个方向的加速度计的加速度测量值。

5.磁场数据的采集和量化

所述地磁场数据为磁场传感器测量所得，包括mag_RMx、mag_RMy、mag_RMz；所述mag_RMx、mag_RMy、mag_RMz为载体坐标系下的三轴的模拟量磁场数据；所述地磁场数据经过24位的AD采样后，实现磁场测量数据的数字化，输出数字化后的三轴磁场mag_Rx0、mag_Ry0、mag_Rz0。

步骤S2、数据预处理，包括陀螺仪和加速度计数据校正、磁场数据校正、干扰磁场识别和分离。

1.陀螺仪和加速度计数据校正

用存储的陀螺仪的原始参数对所述采集的角速度测量值进行校正，将所述陀螺仪三轴的零偏和角速度测量值代入式对陀螺的零偏进行校正，得到校正后的陀螺仪三轴的角速度值为ω_x2，ω_y2，ω_z2；

用存储的加速度计的原始参数对所述采集的加速度测量值进行校正，将所述加速度计的原始参数和加速度测量值代入式对加速度计的零偏进行校正，得到校正后的加速度计三轴的加速度值为a_x2，a_y2，a_z2，其中，M_yx＝M_xy；M_zy＝M_yz；M_xz＝M_zx。

通过对陀螺仪和加速度计数据校正，提高后续的IMU定位精度。

2.磁场数据校正

用存储的磁传感器原始参数对量化后的磁场数据进行校正；

当采用矢量匹配时，采用公式对磁场传感器的灵敏度和零点误差进行校正，所述mag_Rx1、mag_Ry1、mag_Rz1为校正后的三轴磁场；

当采用标量匹配时，总场强度mag_Rf＝sqrt(mag_Rx1 ²+mag_Ry1 ²+mag_Rz1 ²)；采用公式对磁场传感器的灵敏度和零点等误差进行校正，所述

所述

3干扰磁场的识别分离

通过对环境内的随机干扰磁场进行识别和去除，确保磁场特征的稳定性。利用磁场进行定位之前，需要对测量得到的磁场数据进行干扰识别与分离，一般的应用环境中，干扰主要来源于各种电气设备，主要体现为一些高频信号以及设备开关产生的台阶信号，针对这两类信号的识别以及分离的流程如图2所示，包括以下步骤：

1)首先对采集到的磁场数据进行中值滤波，中值滤波窗口一般设置为1～2s的采样点数，经过中值滤波，能够有效消除电气设备产生的高频干扰，保留电气设备开关产生的台阶信号以及环境特征信号；

2)对滤波后的数据进行一阶差分，通过差分处理使台阶信号能够凸显出来；

3)通过设置相关的阈值进行检测，当高于该阈值时，可认为是台阶信号，依据经验，阈值设置为最近1s内测量数据一阶差分的均值+3倍标准差；

4)对检测得到的值用1s内测量数据一阶差分的均值代替，并进行积分还原，得到干扰分离后的地磁数据。

步骤S3、进行IMU加地磁加地图的组合定位，组合定位过程包括以下子步骤：

S301、利用校正后的陀螺仪和加速度计数据进行IMU定位

IMU定位利用校正后的陀螺仪和加速度数据，进行移动终端的位置测量，如图3所示，包括以下步骤：

1.对IMU的俯仰角、横滚角和航向角赋初值：所述a_x2，a_y2，a_z2为校正后的加速度计三轴的加速度初值，g为重力加速度。

2.给IMU进行俯仰角、横滚角和航向角赋初值后，进行航位推算。

利用四元数，基于前一次的四元数值，以及当前测量得到的加速度、角速度信息计算当前四元数，得到欧拉转换矩阵，最后基于上组加速度、速度、位置以及当前的欧拉转换矩阵，计算当前加速度、速度以及位置。具体步骤包括：

1)依据初始的姿态角计算初始四元数，

将俯仰角、横滚角和航向角的初值代入式计算从载体坐标系到地理坐标系的旋转矩阵；

根据得到旋转矩阵计算初始四元数：

2)计算角速度在一个周期内的积分：ΔT是积分时间；计算斜对称矩阵：

3)获得当前的四元数Q＝(Icos(φ/2)+sin(φ/2)[Θ]/φ)Q₀，并把当前时刻的四元数赋给Q₀，所述I为4×4的单位矩阵；

5)更新导航坐标系下加速度值：g为重力加速度；更新导航坐标系下速度值：速度初值一般设为0,ΔT为时间间隔；

在实际计算过程中，如果长时间进行航位推算，速度可能会发散到一个非常大的数值，不符合实际情况，这里对速度设置了一个门限，当推算得到的速度超出该门限后，采用该门限速度作为实际的速度进行更新；

更新导航坐标系下位置值：位置初值一般设为0；

更新载体坐标系相对于导航坐标系的姿态角：

7)重复2)～6)步骤，持续进行航位推算。

3.进行零速检测；

当三轴陀螺仪连续在五个周期内数据的极差小于某个接近0的阈值时，判定定位终端处于零速状态，启动零速更新。

4.进行零速更新；

如果当前定位终端处于零速，基于卡尔曼滤波器计算当前位置、速度、姿态误差，对位置和姿态进行纠正。

5.输出位置、速度和姿态信息；

经过航位推算得出的载体的位置、速度和姿态信息输出用于地磁定位和定位结果的滤波处理。

6.对陀螺仪和加速度计参数修正条件进行判断；

当累计超过特定数量的零速检测数据时，可启动陀螺仪和加速度计参数校正模块，这里的特定数量范围为[5,10]。

7.对陀螺仪和加速度计的参数进行校正；

零速条件下，陀螺仪测量值的均值为陀螺仪的零偏；对一段时间积累的零速条件下的陀螺仪的测量值求取均值，作为陀螺仪新的零偏数据，对原来的陀螺仪零偏数据进行更新替换，重新计算当前陀螺仪的零偏；对加速度计的各轴零偏进行更新替换与陀螺仪的各轴零偏更新替换方法相同。

S302、结合IMU定位的定位结果，通过地磁匹配进行地磁定位

所述地磁定位的过程如图4所示，包括：

1.系统重新启动后，进行首匹配

系统重新启动后，第一次进行地磁匹配，即为首匹配。因为需要根据首匹配的结果修正IMU定位的初值，所以对首匹配定位结果的可靠性要求比较高，这里采用了对连续三次的匹配结果进行一致性判决的方法提高首匹配的可靠性；同时由于此时定位终端可能在区域内的任何位置，所以首匹配需要启动全局搜索模式，即在全局范围内搜索定位终端可能在的位置。搜索定位过程如下，在首匹配时，首先顺序测量三段长度均为L米的(L的经验值，3≤L≤10)磁场序列，在区域内进行全局搜索，通过绝对平均差匹配方法或积相关匹配方法，找到和实时磁场测量序列最相似的基准磁场序列所在的位置，记为PM1，PM2，PM3。在匹配相关计算时，如果是标量匹配模式则只对比总场强度的相似性；如果是矢量匹配模式，则对比三个分量的相似性，总的相关值为三个分量相关值的和。此时惯导IMU标记的位置是PA1，PA2，PA3。

对PM1，PM2，PM3三个位置进行一致性判决，如果三次地磁匹配位置连续，则证明地磁匹配可靠，系统进入连续匹配定位模式。

如果满足判决门限，则根据采用的是绝对平均差匹配方法或积相关匹配方法，选择最小相关值或最大相关值的定位结果进行输出，然后进入连续匹配定位模式。如果不满足判决门限，则继续测量，直到满足判决条件为止。

2.连续匹配定位模式，每隔1s，进行一次匹配定位，和首匹配不同，这次不再采用全局搜索方法，以首匹配位置结合IMU的指示位置为中心，半径为R的范围内进行搜索定位，所述半径R为事先根据惯导的定位误差范围确定，以大于所述定位误差范围为基准；匹配方法可以选择绝对平均差匹配方法或积相关匹配方法等匹配相关算法，与首匹配选择的匹配定位方法相同。

步骤S303、对地磁匹配定位结果进行滤波，通过卡尔曼滤波算法，结合数字地图数据对定位终端载体的运动区域的限定对定位结果进行滤波，所述限定为定位终端载体只能在数字地图的道路上移动，而不可能在道路以外的墙壁、家具、设备等定位终端载体不可能出现的位置上移动；并修正IMU参数，得到组合定位结果。

综上所述，本发明实施例提供的基于空间环境磁场特征的无源组合定位方法，不需要任何硬件部署、不向外发射和接收电磁信号、不需要维护(除了建筑物结构改变以外)、可以分辨楼层、不易受遮挡和干扰的优点；特别是针对磁场信号容易受到环境中不断变化的低频电、磁信号干扰，本发明提出事先对磁传感器进行标校，实时对磁干扰进行识别和消除的处理方法，提高磁场测量精度；同时通过匹配算法的改进，提高地磁定位的稳定性，从而确保了该室内定位系统的鲁棒性和对绝大多数应用场景的适应性。

本领域技术人员可以理解，实现上述实施例方法的全部或部分流程，可以通过计算机程序来指令相关的硬件来完成，所述的程序可存储于计算机可读存储介质中。其中，所述计算机可读存储介质为磁盘、光盘、只读存储记忆体或随机存储记忆体等。

以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。

Claims

1.一种基于空间环境磁场特征的无源组合定位方法，其特征在于，包括如下步骤：

步骤S3、根据校正后的惯性数据进行IMU定位，结合IMU定位的定位结果，根据校正和去除干扰后的磁场数据进行地磁匹配定位，结合数字地图数据，对地磁匹配定位结果进行滤波，并修正IMU参数，得到组合定位结果；

所述结合IMU定位的定位结果，根据校正和去除干扰后的磁场数据进行地磁匹配定位，包括以下步骤：

1)系统重新启动后，进行首匹配；

所述首匹配启动全局搜索模式，对连续三次的匹配结果进行一致性判决，在搜索定位过程中，首先顺序测量三段长度均为3～10米的磁场序列，在区域内进行全局搜索，通过匹配相关算法，找到和地磁实时图最相似的地磁基准图所在的位置为PM1，PM2，PM3，此时IMU标记的位置是PA1，PA2，PA3；根据所述匹配定位的位置和IMU标记的位置进行首匹配判决，如果满足判决条件，则根据匹配相关算法的定位结果进行输出，如果不满足判决条件，则继续测量，直到满足判决条件为止；

2.根据权利要求1所述无源组合定位方法，其特征在于，所述步骤S2中干扰磁场的识别分离过程包括：

对采集到的磁场数据进行中值滤波；

对滤波后的数据进行一阶差分，放大台阶信号；

去除台阶信号；

3.根据权利要求1所述无源组合定位方法，其特征在于，所述步骤S3包括如下子步骤：

S301、利用校正后惯性数据进行IMU定位；

S302、结合IMU定位的定位结果，通过地磁匹配进行地磁定位；

4.根据权利要求3所述无源组合定位方法，其特征在于，利用校正后惯性数据进行IMU定位方法包括：

1)对俯仰角、横滚角和航向角赋初值；

2)进行航位推算；

5.根据权利要求4所述无源组合定位方法，其特征在于，所述航位推算是利用四元数，基于前一次的四元数值，以及当前测量得到的加速度、角速度信息计算当前四元数，得到欧拉转换矩阵，最后基于上组加速度、速度、位置以及当前的欧拉转换矩阵，计算当前加速度、速度以及位置，其具体过程包括：

1)依据初始的姿态角计算初始四元数；

2)计算角速度在一个周期内的积分：

5)更新导航坐标系下加速度值、速度值和位置值；

7)重复2)～6)步骤，持续进行航位推算。

7.根据权利要求1所述无源组合定位方法，其特征在于，

所述匹配相关算法包括绝对平均差匹配方法或积相关匹配方法两种，在采用绝对平均差匹配方法进行匹配时，选择相似度值的最小值所对应的位置为最终的匹配位置，采用积相关匹配方法进行匹配时，选择相似度值的最大值所对应的位置为最终的匹配位置。

8.根据权利要求3所述无源组合定位方法，其特征在于，所述S302中，通过卡尔曼滤波算法，对地磁匹配定位结果进行滤波，结合数字地图数据对定位终端载体的运动区域的限定对定位结果进行滤波，所述限定为定位终端载体只能在数字地图的道路上移动，而不能在道路以外的位置上移动；并修正IMU参数，得到组合定位结果。