CN110798806B - 一种基于地磁强度的卡尔曼滤波室内轨迹跟踪算法 - Google Patents
一种基于地磁强度的卡尔曼滤波室内轨迹跟踪算法 Download PDFInfo
- Publication number
- CN110798806B CN110798806B CN201911044891.XA CN201911044891A CN110798806B CN 110798806 B CN110798806 B CN 110798806B CN 201911044891 A CN201911044891 A CN 201911044891A CN 110798806 B CN110798806 B CN 110798806B
- Authority
- CN
- China
- Prior art keywords
- geomagnetic
- data
- kalman filtering
- path
- point
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Expired - Fee Related
Links
- 238000001914 filtration Methods 0.000 title claims abstract description 42
- 238000000034 method Methods 0.000 claims abstract description 27
- 238000005259 measurement Methods 0.000 claims description 44
- 239000013598 vector Substances 0.000 claims description 15
- 239000011159 matrix material Substances 0.000 claims description 9
- 238000010586 diagram Methods 0.000 claims description 6
- 230000005540 biological transmission Effects 0.000 claims description 4
- 230000007613 environmental effect Effects 0.000 claims description 4
- 230000009466 transformation Effects 0.000 claims description 4
- 238000000844 transformation Methods 0.000 claims description 4
- 238000004458 analytical method Methods 0.000 claims description 3
- 230000008859 change Effects 0.000 claims description 3
- 238000013499 data model Methods 0.000 claims description 3
- 238000013501 data transformation Methods 0.000 claims description 3
- 238000011985 exploratory data analysis Methods 0.000 claims description 3
- 230000008569 process Effects 0.000 claims description 3
- 238000004088 simulation Methods 0.000 claims description 3
- 230000001502 supplementing effect Effects 0.000 claims description 3
- 238000012360 testing method Methods 0.000 claims description 3
- 230000002194 synthesizing effect Effects 0.000 claims description 2
- 230000005389 magnetism Effects 0.000 claims 2
- 238000005457 optimization Methods 0.000 description 5
- 238000005516 engineering process Methods 0.000 description 3
- 238000004891 communication Methods 0.000 description 2
- 238000011161 development Methods 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 238000012545 processing Methods 0.000 description 2
- 230000004075 alteration Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000013480 data collection Methods 0.000 description 1
- 230000005358 geomagnetic field Effects 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012544 monitoring process Methods 0.000 description 1
- 230000002093 peripheral effect Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
Images
Classifications
-
- H—ELECTRICITY
- H04—ELECTRIC COMMUNICATION TECHNIQUE
- H04W—WIRELESS COMMUNICATION NETWORKS
- H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
- H04W4/30—Services specially adapted for particular environments, situations or purposes
- H04W4/38—Services specially adapted for particular environments, situations or purposes for collecting sensor information
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/04—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means
- G01C21/08—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means involving use of the magnetic field of the earth
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
- G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation
-
- H—ELECTRICITY
- H04—ELECTRIC COMMUNICATION TECHNIQUE
- H04W—WIRELESS COMMUNICATION NETWORKS
- H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
- H04W4/02—Services making use of location information
- H04W4/029—Location-based management or tracking services
-
- H—ELECTRICITY
- H04—ELECTRIC COMMUNICATION TECHNIQUE
- H04W—WIRELESS COMMUNICATION NETWORKS
- H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
- H04W4/30—Services specially adapted for particular environments, situations or purposes
- H04W4/33—Services specially adapted for particular environments, situations or purposes for indoor environments, e.g. buildings
Landscapes
- Engineering & Computer Science (AREA)
- Remote Sensing (AREA)
- Radar, Positioning & Navigation (AREA)
- Computer Networks & Wireless Communication (AREA)
- Signal Processing (AREA)
- Automation & Control Theory (AREA)
- General Physics & Mathematics (AREA)
- Physics & Mathematics (AREA)
- Life Sciences & Earth Sciences (AREA)
- Geology (AREA)
- General Life Sciences & Earth Sciences (AREA)
- Environmental & Geological Engineering (AREA)
- Navigation (AREA)
Abstract
本发明涉及无线传感网络技术领域，提供了一种基于地磁强度的卡尔曼滤波室内轨迹跟踪算法，包括S1‑S4四个步骤。本发明涉及一种基于地磁强度的卡尔曼滤波室内轨迹跟踪算法，通过借助地球磁场强度在每个位置的唯一性的定位功能，并合理运用三边定位法以及卡尔曼滤波对轨迹进行计算，最后通过路径加和得到最终的路径，可以使得处于运动状态时依然能够有更高的定位精度。
Description
技术领域
本发明涉及无线传感网络技术领域，具体为一种基于地磁强度的卡尔曼滤波室内轨迹跟踪算法。
背景技术
定位在生活中应用十分广泛，现今也是生活中不可或缺的一部分，对于出行等各方面十分方便。随着生活水平的提高，社会的发展，人们对定位的要求也更多，不仅在室外的需要定位，而且逐步对室内的定位也涉及需求。例如在高大的大厦，宽阔的商场，人们也需要精准的定位，因此，现如今对在室内的定位也有很大的意义。
无线传感器网是由大量的集成了传感、数据收集、处理和无线通信能力的小体积、低成本的传感器节点构成的自治网络，是一个通过感知、采集和处理网络覆盖区域中被感知对象的信息来监视和控制一个特定地理区域的智能网络信息***。无线传感器网络定位方法较多，目前基于RSSI的定位方法能充分利用已有的无线网基础设施，无需增加额外的硬件装置，具有功耗低、成本低的优点。但室内环境中多径效应和非视距传输条件的影响导致室内定位***面临很多技术挑战。
近年来，利用WiFi，蓝牙，超宽带等技术达到的定位精度理想，但是需要铺设大量的外设，成本较高。由于地磁场是天然存在的，并且随着移动端技术的发展，移动端的内置传感器可以获取地磁信息。磁定位作为一种新兴的室内定位方法，借助地球磁场强度在每个位置的唯一性实现定位功能，其优势在于安全可靠、成本低廉、对环境要求低等，并能通过智能手机内置的地磁传感器即可实现定位。芬兰的IndoorAtlas公司一直在致力于地磁室内定位技术的研究，最近几年，其给出了在地下1000米的矿洞中使用地磁定位确定位置的解决方案，其地磁室内定位精度在国际上处于领先位置，平均精度可以达到1-2米。但是当运动状态发生变化时，该软件定位精度下降甚至无法定位。
发明内容
本发明的目的在于提供一种基于地磁强度的卡尔曼滤波室内轨迹跟踪算法，通过借助地球磁场强度在每个位置的唯一性的定位功能，并合理运用三边定位法以及卡尔曼滤波对轨迹进行计算，最后通过路径加和得到最终的路径，可以使得处于运动状态时依然能够有更高的定位精度。
为实现上述目的，本发明实施例提供如下技术方案：一种基于地磁强度的卡尔曼滤波室内轨迹跟踪算法，包括如下步骤：
S1，在待测室内区域内，随机部署zigbee信号强度测量网络；
S2，采用上位机获得路径上各点接收到的RSSI并计算出对应距离d；与此同时采用地磁测量模块沿着相同的路径依次测得路径上各点的地磁数据；
S3，采用三边定位法依据所述上位机计算出的对应距离求解路径路径上各点位置的测量坐标，并用卡尔曼滤波算法对轨迹进行跟踪定位以获得路径；与此同时，采用基于Hausdorff的距离度量算法进行地磁数据的搜索匹配，并对轨迹进行跟踪以获得路径；
S4，采用适宜该环境的加权系数将所述S3步骤中获得的两个路径加和，以输出最终路径，该最终路径即为当前位置至定位位置的路径。
进一步，在所述S1步骤具体为：在a(m)×a(m)待测的室内区域内，随机部署n个传感器，传感器的节点坐标为：(xi,yi)，其中i＝1,2,...,n.n≥3。
进一步，在所述S2步骤中，计算出对应距离d具体为：
协调器节点坐标为(m,n)，终端节点与协调器之间采用ZigBee自组网方式进行无线数据传输，协调器与上位机使用串口进行通信，并通过上位机获得协调器接收到的终端节点发射出来的无线信号强度，计算该环境下的环境参数A，n，利用所得的信号强度通过公式RSSI＝-(10nlgd+A)计算出对应距离d。
进一步，在所述S3步骤中：
采用三边定位法求解路径上各点位置的测量坐标，并用卡尔曼滤波算法对轨迹进行跟踪定位以获得路径具体为：
S300，带入所述S2步骤中求得的对应距离d，采用三边算法求解路径上各点位置：
设定均匀部署在室内区域中的传感器节数为三个，三个传感器所在的参考节点的位置坐标分别为A(x1,y1)、B(x2,y2)、C(x3,y3)，协调器的位置为待确定节点M，其坐标为M(m,n)，M到三个参考节点的距离分别是d1、d2和d3，如下式：
由上式得出节点M的位置坐标为
S301，采用卡尔曼滤波模拟运动轨迹：
将所述S300步骤测得轨迹上每个点的测量坐标依次代入卡尔曼滤波模型中进行滤波，卡尔曼滤波的递推思想按前一时刻的最佳估计值来预测当前时刻的状态值，然后通过当前时刻的观测值来修正该状态值，得到当前时刻的最佳估计值，以此递推下去，在这个过程中，主要包括预测和更新两部分：
其中第一个公式为状态预测部分公式，第二个公式为状态更新部分公式，其中x0和∑0为初始状态均值和方差，Kt为卡尔曼滤波增益矩阵，∑t为增益矩阵协方差，
为前一时刻的最佳估计值，
为当前时刻的状态预测值，
为当前时刻的最佳估计值，yt为当前时刻的观测值。
进一步，在所述S2步骤中，地磁数据的采集分为两部分：地磁数据库的建立与测量轨迹地磁数据获取。
进一步，所述地磁数据库的建立具体为：采用九轴姿态传感器在定点区域采集地磁数据，建立空间地磁数据模型；
所述测量轨迹地磁数据获取具体为：采集人员在获取整块区域的地磁强度时需要每隔b(m)做近似匀速直线运动进行数据采集，采集时将地磁测量模块尽量保持水平并始终朝一个方向，获取测量轨迹地磁数据时，在轨迹上选取与测量RSSI数据相同的测量点，在每个测量点上通过地磁测量模块逐个测量地磁强度。
进一步，在所述S3步骤中，
采用基于Hausdorff的距离度量算法进行地磁数据的搜索匹配，并对轨迹进行跟踪以获得路径具体为：
S310，克里金插值：借助于ArcGIS软件GA功能进行普通克里金插值，对采集的地磁数据进行插值，设在x,y轴为该地区的横纵边，z轴为地磁数据；
S311，绘制地磁基准图：克里金插值的结果将补充纵边方向未测量的地磁数据，最终形成整个地磁数据库，以该地区的横纵边分别为x,y轴，地磁的总强度为z轴使用MATLAB绘制出地磁基准图；
S312，基于地磁数据求解运动轨迹：根据地磁数据来定位测量点以及求解运动轨迹，上述步骤已经获取了整个区域的地磁，包括xyz三个方向的地磁数据以及总的地磁数据；在建立地磁数据库以后，再进行未知的点的地磁数据测量，获取地磁分量和总量；再通过hausdorff距离匹配算法，进行地磁匹配。其中地磁数据是一个四维向量(x,y,z分量和总地磁)，总的地磁强度使用下式计算：
因此在匹配时需要用到范数来度量两个向量之间的距离，此时就要用到马氏距离；由于马氏距离既可以判别某一空间点列的归属问题，又可以计算某一样本任一一点相对于一已知点的距离，而区分远近，故可从马氏距离的来定义距离测度，其数学表达式如下式：
||mt-nt||＝[(mt-nt)TR-1(mt-nt)]1/2，
当进行逐个计算，并找出相似马氏距离最小的一个向量，即找到最相似的一个向量后，通过这个地磁特征向量在地磁数据库中的位置，即可推算出该地磁对应的位置坐标。
进一步，在所述S310步骤前，判断数据是否能够进行克里金差值的方法具体为：进行探索性数据分析时，对数据的直方图与正态QQ图进行观察，观察数据的分布情况，通过软件提供的数据变换对数据进行不同的变换，选择最佳正态分布数据；在全局趋势分析中，通过测量点在地图中被投影到两个面上：YZ面和XZ面，为一个正交平面；通过投影能作出一条拟合曲线，用其来模拟特定方向上的存在趋势；若模拟曲线为一条直线，则说明不存在这趋势。
进一步，在所述S4步骤具体为：
S40，评估该环境下加权系数m的大小，为了得到使定位精度较高的加权系数值，将对该环境下多条不同路径再进行测试，将每个权重系数值计算得到的轨迹下式计算其精度，并求得精度最高时对应的最佳权重系数m0，综合多条路径得到的最佳权重系数m0评估出当前环境适用的权重系数m，
S41，利用卡尔曼算法基于已测出的RSSI值对移动的运动轨迹上的若干点进行跟踪估计，获得n个轨迹上点的坐标值P(1)，P(2)，…，P(n)，
S42，利用在与上个步骤相同的轨迹点上测得的地磁数据进行数据匹配，估计出n个轨迹上点的坐标值Q(1)，Q(2)，…，Q(n)，
S43，将所述S3步骤中获得的两条路径对应点迹作加权平均获得一条更优的轨迹，该轨迹上各点的加权平均模型的数学表达式为如下，
R(n)＝mP(n)+(1-m)Q(n)，
其中R(n)表示获得n个融合轨迹上点的坐标值，P(n)、Q(n)分别表示S3步骤获得n个轨迹上点的坐标值，m表示该环境下的加和权重系数。
进一步，在所述S40步骤中，参数m会随环境的变化而变化，因此当环境发生改变的时候，参数应该重新进行评估。
与现有技术相比，本发明的有益效果是：一种基于地磁强度的卡尔曼滤波室内轨迹跟踪算法，通过借助地球磁场强度在每个位置的唯一性的定位功能，并合理运用三边定位法以及卡尔曼滤波对轨迹进行计算，最后通过路径加和得到最终的路径，可以使得处于运动状态时依然能够有更高的定位精度。
附图说明
图1为本发明实施例提供的一种基于地磁强度的卡尔曼滤波室内轨迹跟踪算法的步骤流程图；
图2为本发明实施例提供的一种基于地磁强度的卡尔曼滤波室内轨迹跟踪算法的克里金插值结果图；
图3为本发明实施例提供的一种基于地磁强度的卡尔曼滤波室内轨迹跟踪算法的地磁基准图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其它实施例，都属于本发明保护的范围。
请参阅图1，本发明实施例提供一种基于地磁强度的卡尔曼滤波室内轨迹跟踪算法，包括如下步骤：S1，在待测室内区域内，随机部署zigbee信号强度测量网络；S2，采用上位机获得路径上各点接收到的RSSI并计算出对应距离d；与此同时采用地磁测量模块沿着相同的路径依次测得路径上各点的地磁数据；S3，采用三边定位法依据所述上位机计算出的对应距离求解路径路径上各点位置的测量坐标，并用卡尔曼滤波算法对轨迹进行跟踪定位以获得路径；与此同时，采用基于Hausdorff的距离度量算法进行地磁数据的搜索匹配，并对轨迹进行跟踪以获得路径；S4，采用适宜该环境的加权系数将所述S3步骤中获得的两个路径加和，以输出最终路径，该最终路径即为当前位置至定位位置的路径。在本实施例中，通过借助地球磁场强度在每个位置的唯一性的定位功能，并合理运用三边定位法以及卡尔曼滤波对轨迹进行计算，最后通过路径加和得到最终的路径，可以使得处于运动状态时依然能够有更高的定位精度。
作为本发明实施例的优化方案，在所述S1步骤具体为：在a(m)×a(m)待测的室内区域内，随机部署n个传感器，传感器的节点坐标为：(xi,yi)，其中i＝1,2,...,n.n≥3。例如：在待测的7.2(m)x7.2(m)室内区域内，随机部署18个传感器，传感器的节点坐标为：(xi,yi)，其中i＝1,2,..,18.，传感器节点坐标如表1所示。
表1传感器节点坐标
作为本发明实施例的优化方案，在所述S2步骤中，计算出对应距离具体为：协调器节点坐标为(m,n)，终端节点与协调器之间采用ZigBee自组网方式进行无线数据传输，协调器与上位机使用串口进行通信。通过上位机可获得协调器接收到的终端节点发射出来的无线信号强度。待测路径上各点真实位置坐标如表2所示，传感器节点坐标及其待测路径上10个节点的接收信号强度(RSSI)如表3所示。
表2待测路径上各点真实位置坐标
表3传感器节点坐标及其待测路径上10个节点的接收信号强度
作为本发明实施例的优化方案，在所述S2步骤中，地磁数据的采集分为两部分：地磁数据库的建立与测量轨迹地磁数据获取。所述地磁数据库的建立具体为：采用九轴姿态传感器在定点区域采集地磁数据，建立空间地磁数据模型；所述测量轨迹地磁数据获取具体为：由于区域的特殊性，采集人员在获取整块区域的地磁强度时需要每隔0.8(m)做近似匀速直线运动进行数据采集，采集时将地磁测量模块尽量保持水平并始终朝一个方向，获取测量轨迹地磁数据时，在轨迹上选取与测量RSSI数据相同的测量点，在每个测量点上通过地磁测量模块逐个测量地磁强度。
作为本发明实施例的优化方案，在所述S3步骤中：
采用三边定位法求解路径上各点位置的测量坐标，并用卡尔曼滤波算法对轨迹进行跟踪定位以获得路径具体为：
求解环境参数，使用模型简化Shadowing模型：
RSSI＝-(10nlgd+A)； (1)
取距离无线信号发射源1m处所接收到的平均信号强度值作为信号强度值作为式中的射频参数A，n为损耗因子，它们的值都与环境有关。提前测量一组随机数据用于求解环境参数，采用Matlab拟合工具箱Curve Fitting Tool将测量数据进行拟合，求解出该环境下的参数A，n。
采用上述公式计算所述S2步骤中测得RSSI值分别对应的距离d；
带入对应距离d，采用三边算法求解路径上各点位置：
设定均匀部署在室内区域中的传感器节数为三个，三个传感器所在的参考节点的位置坐标分别为A(x1,y1)、B(x2,y2)、C(x3,y3)，协调器的位置为待确定节点M，其坐标为M(m,n)。M到三个参考节点的距离分别是d1、d2和d3，如下式：
由上式得出节点M的位置坐标为
计算所得路径上各点测量坐标如表4所示：
表4三边算法计算得路径上各点坐标
采用卡尔曼滤波模拟运动轨迹：
将上述测得轨迹上每个点的测量坐标依次代入卡尔曼滤波模型中进行滤波，卡尔曼滤波的递推思想按前一时刻的最佳估计值来预测当前时刻的状态值，然后通过当前时刻的观测值来修正该状态值，得到当前时刻的最佳估计值，以此递推下去，在这个过程中，主要包括预测和更新两部分：
其中第一个公式为状态预测部分公式，第二个公式为状态更新部分公式，其中x0和∑0为初始状态均值和方差，Kt为卡尔曼滤波增益矩阵，∑t为增益矩阵协方差，
为前一时刻的最佳估计值，
为当前时刻的状态预测值，
为当前时刻的最佳估计值，yt为当前时刻的观测值。滤波后轨迹坐标如表5所示：
表5卡尔曼滤波后轨迹上各点坐标
作为本发明实施例的优化方案，在所述S3步骤中，采用基于Hausdorff的距离度量算法进行地磁数据的搜索匹配，并对轨迹进行跟踪以获得路径具体为：
克里金插值：借助于ArcGIS软件GA功能进行普通克里金插值，对采集的地磁数据进行插值，设在x,y轴为该地区的横纵边，z轴为地磁数据；克里金插值要求是数据符合正态分布且不存在趋势；判断数据是否能够进行克里金差值的方法具体为：进行探索性数据分析时，对数据的直方图与正态QQ图进行观察，观察数据的分布情况，通过软件提供的数据变换对数据进行不同的变换，选择最佳正态分布数据；在全局趋势分析中，通过测量点在地图中被投影到两个面上：YZ面和XZ面，为一个正交平面；通过投影能作出一条拟合曲线，用其来模拟特定方向上的存在趋势；若模拟曲线为一条直线，则说明不存在这趋势；
地统计导向中选择克里金法，对之前选择的数据集选择对应的变换与趋势。确定插值参数后，软件开始根据要求插值，最后生成插值数据与插值示意图。克里金插值结果如图2所示。
绘制地磁基准图：克里金插值的结果将补充纵边方向未测量的地磁数据，最终形成整个地磁数据库，以该地区的横纵边分别为x,y轴，地磁的总强度为z轴使用MATLAB绘制出地磁基准图；地磁基准图如图3所示.
基于地磁数据求解运动轨迹：根据地磁数据来定位测量点以及求解运动轨迹，上述步骤已经获取了整个区域的地磁，包括xyz三个方向的地磁数据以及总的地磁数据；在建立地磁数据库以后，再进行未知的点的地磁数据测量，获取地磁分量和总量；再通过hausdorff距离匹配算法，进行地磁匹配。其中地磁数据是一个四维向量(x,y,z分量和总地磁)，总的地磁强度使用下式计算：
因此在匹配时需要用到范数来度量两个向量之间的距离，此时就要用到马氏距离；由于马氏距离既可以判别某一空间点列的归属问题，又可以计算某一样本任一一点相对于一已知点的距离，而区分远近，故可从马氏距离的来定义距离测度，其数学表达式如下式：
||mt-nt||＝[(mt-nt)TR-1(mt-nt)]1/2， (7)
当进行逐个计算，并找出相似马氏距离最小的一个向量，即找到最相似的一个向量后，通过这个地磁特征向量在地磁数据库中的位置，即可推算出该地磁对应的位置坐标。通过地磁数据匹配后得到路径上各点的位置坐标如表6所示：
表6通过地磁数据匹配后得到路径上各点的位置坐标
作为本发明实施例的优化方案，在所述S4步骤具体为：
评估该环境下加权系数m的大小，为了得到使定位精度较高的加权系数值，将对该环境下多条不同路径再进行测试，将每个权重系数值计算得到的轨迹下式计算其精度，并求得精度最高时对应的最佳权重系数m0，
综合多条路径得到的最佳权重系数m0评估出当前环境适用的权重系数m.参数m会随环境的变化而变化，因此当环境发生改变的时候，参数应该重新进行评估。
利用卡尔曼算法基于已测出的RSSI值对移动的运动轨迹上的若干点进行跟踪估计，获得n个轨迹上点的坐标值P(1)，P(2)，…，P(n)，
利用在与上个步骤相同的轨迹点上测得的地磁数据进行数据匹配，估计出n个轨迹上点的坐标值Q(1)，Q(2)，…，Q(n)，
将所述S3步骤中获得的两条路径对应点迹作加权平均获得一条更优的轨迹，该轨迹上各点的加权平均模型的数学表达式为如下，
R(n)＝mP(n)+(1-m)Q(n)， (9)
其中R(n)表示获得n个融合轨迹上点的坐标值，P(n)、Q(n)分别表示S3步骤获得n个轨迹上点的坐标值，m表示该环境下的加和权重系数，该环境下的加和权重系数m＝0.1。融合后路径上各点的位置坐标如表7所示：
表7融合后路径上各点的位置坐标
尽管已经示出和描述了本发明的实施例，对于本领域的普通技术人员而言，可以理解在不脱离本发明的原理和精神的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由所附权利要求及其等同物限定。
Claims (10)
1.一种基于地磁强度的卡尔曼滤波室内轨迹跟踪算法，其特征在于，包括如下步骤：
S1，在待测室内区域内，随机部署zigbee信号强度测量网络；
S2，采用上位机获得路径上各点接收到的RSSI并计算出对应距离d；与此同时采用地磁测量模块沿着相同的路径依次测得路径上各点的地磁数据；
S3，采用三边定位法依据所述上位机计算出的对应距离求解路径路径上各点位置的测量坐标，并用卡尔曼滤波算法对轨迹进行跟踪定位以获得路径；与此同时，采用基于Hausdorff的距离度量算法进行地磁数据的搜索匹配，并对轨迹进行跟踪以获得路径；
S4，采用适宜环境的加权系数将所述S3步骤中获得的两个路径加和，以输出最终路径，该最终路径即为当前位置至定位位置的路径。
2.如权利要求1所述的一种基于地磁强度的卡尔曼滤波室内轨迹跟踪算法，其特征在于，在所述S1步骤具体为：在a×a待测的室内区域内，随机部署n个传感器，传感器的节点坐标为：(xi,yi)，其中i＝1,2,...,n.n≥3，a的单位为米。
3.如权利要求1所述的一种基于地磁强度的卡尔曼滤波室内轨迹跟踪算法，其特征在于，在所述S2步骤中，计算出对应距离d具体为：
协调器节点坐标为(m,n)，终端节点与协调器之间采用ZigBee自组网方式进行无线数据传输，协调器与上位机使用串口进行通信，并通过上位机获得协调器接收到的终端节点发射出来的无线信号强度，计算该环境下的环境参数A，n，利用所得的信号强度通过公式RSSI＝-(10nlgd+A)计算出对应距离d。
4.如权利要求3所述的一种基于地磁强度的卡尔曼滤波室内轨迹跟踪算法，其特征在于，在所述S3步骤中：
采用三边定位法求解路径上各点位置的测量坐标，并用卡尔曼滤波算法对轨迹进行跟踪定位以获得路径具体为：
S300，带入所述S2步骤中求得的对应距离d，采用三边算法求解路径上各点位置：
设定均匀部署在室内区域中的传感器节数为三个，三个传感器所在的参考节点的位置坐标分别为A(x1,y1)、B(x2,y2)、C(x3,y3)，协调器的位置为待确定节点M，其坐标为M(m,n)，M到三个参考节点的距离分别是d1、d2和d3，如下式：
由上式得出节点M的位置坐标为
S301，采用卡尔曼滤波模拟运动轨迹：
将所述S300步骤测得轨迹上每个点的测量坐标依次代入卡尔曼滤波模型中进行滤波，卡尔曼滤波的递推思想按前一时刻的最佳估计值来预测当前时刻的状态值，然后通过当前时刻的观测值来修正该状态值，得到当前时刻的最佳估计值，以此递推下去，在这个过程中，主要包括预测和更新两部分：
其中
为状态预测部分公式，
为状态更新部分公式，其中x0和∑0为初始状态均值和方差，Kt为卡尔曼滤波增益矩阵，∑t为增益矩阵协方差，
为前一时刻的最佳估计值，
为当前时刻的状态预测值，
为当前时刻的最佳估计值，yt为当前时刻的观测值。
5.如权利要求1所述的一种基于地磁强度的卡尔曼滤波室内轨迹跟踪算法，其特征在于，在所述S2步骤中，地磁数据的采集分为两部分：地磁数据库的建立与测量轨迹地磁数据获取。
6.如权利要求5所述的一种基于地磁强度的卡尔曼滤波室内轨迹跟踪算法，其特征在于，
所述地磁数据库的建立具体为：采用九轴姿态传感器在定点区域采集地磁数据，建立空间地磁数据模型；
所述测量轨迹地磁数据获取具体为：采集人员在获取整块区域的地磁强度时需要每隔b米做近似匀速直线运动进行数据采集，采集时将地磁测量模块尽量保持水平并始终朝一个方向，获取测量轨迹地磁数据时，在轨迹上选取与测量RSSI数据相同的测量点，在每个测量点上通过地磁测量模块逐个测量地磁强度。
7.如权利要求1所述的一种基于地磁强度的卡尔曼滤波室内轨迹跟踪算法，其特征在于，在所述S3步骤中，
采用基于Hausdorff的距离度量算法进行地磁数据的搜索匹配，并对轨迹进行跟踪以获得路径具体为：
S310，克里金插值：借助于ArcGIS软件GA功能进行普通克里金插值，对采集的地磁数据进行插值，设在x,y轴为待绘制的地磁基准图 的横纵边，z轴为地磁数据；
S311，绘制地磁基准图：克里金插值的结果将补充纵边方向未测量的地磁数据，最终形成整个地磁数据库，以待绘制的地磁基准图的横纵边分别为x,y轴，地磁的总强度为z轴使用MATLAB绘制出地磁基准图；
S312，基于地磁数据求解运动轨迹：根据地磁数据来定位测量点以及求解运动轨迹，上述步骤已经获取了整个区域的地磁，包括xyz三个方向的地磁数据以及总的地磁数据；在建立地磁数据库以后，再进行未知的点的地磁数据测量，获取地磁分量和总量；再通过hausdorff距离匹配算法，进行地磁匹配，其中地磁数据是一个四维向量(x,y,z分量和总地磁)，总的地磁强度使用下式计算：
因此在匹配时需要用到范数来度量两个向量之间的距离，此时就要用到马氏距离；由于马氏距离既可以判别某一空间点列的归属问题，又可以计算某一样本任一一点相对于一已知点的距离，而区分远近，故可从马氏距离的来定义距离测度，其数学表达式如下式：
||mt-nt||＝[(mt-nt)TR-1(mt-nt)]1/2，
当进行逐个计算，并找出相似马氏距离最小的一个向量，即找到最相似的一个向量后，通过这个地磁特征向量在地磁数据库中的位置，即可推算出该地磁对应的位置坐标。
8.如权利要求7所述的一种基于地磁强度的卡尔曼滤波室内轨迹跟踪算法，其特征在于，在所述S310步骤前，判断数据是否能够进行克里金差值的方法具体为：进行探索性数据分析时，对数据的直方图与正态QQ图进行观察，观察数据的分布情况，通过软件提供的数据变换对数据进行不同的变换，选择最佳正态分布数据；在全局趋势分析中，通过测量点在地图中被投影到两个面上：YZ面和XZ面，为一个正交平面；通过投影能作出一条拟合曲线，用其来模拟特定方向上的存在趋势；若模拟曲线为一条直线，则说明不存在这趋势。
9.如权利要求1所述的一种基于地磁强度的卡尔曼滤波室内轨迹跟踪算法，其特征在于，在所述S4步骤具体为：
S40，评估该环境下加权系数m的大小，为了得到使定位精度较高的加权系数值，将对该环境下多条不同路径再进行测试，将每个权重系数值计算得到的轨迹下式计算其精度，并求得精度最高时对应的最佳权重系数m0，综合多条路径得到的最佳权重系数m0评估出当前环境适用的权重系数m，
S41，利用卡尔曼算法基于已测出的RSSI值对移动的运动轨迹上的若干点进行跟踪估计，获得n个轨迹上点的坐标值P(1)，P(2)，…，P(n)，
S42，利用在与上个步骤相同的轨迹点上测得的地磁数据进行数据匹配，估计出n个轨迹上点的坐标值Q(1)，Q(2)，…，Q(n)，
S43，将所述S3步骤中获得的两条路径对应点迹作加权平均获得一条更优的轨迹，该轨迹上各点的加权平均模型的数学表达式为如下，
R(n)＝mP(n)+(1-m)Q(n)，
其中R(n)表示获得n个融合轨迹上点的坐标值，P(n)、Q(n)分别表示S3步骤获得n个轨迹上点的坐标值，m表示该环境下的加和权重系数。
10.如权利要求9所述的一种基于地磁强度的卡尔曼滤波室内轨迹跟踪算法，其特征在于：在所述S40步骤中，参数m会随环境的变化而变化，因此当环境发生改变的时候，参数应该重新进行评估。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201911044891.XA CN110798806B (zh) | 2019-10-30 | 2019-10-30 | 一种基于地磁强度的卡尔曼滤波室内轨迹跟踪算法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201911044891.XA CN110798806B (zh) | 2019-10-30 | 2019-10-30 | 一种基于地磁强度的卡尔曼滤波室内轨迹跟踪算法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN110798806A CN110798806A (zh) | 2020-02-14 |
| CN110798806B true CN110798806B (zh) | 2021-03-12 |
Family
ID=69442076
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201911044891.XA Expired - Fee Related CN110798806B (zh) | 2019-10-30 | 2019-10-30 | 一种基于地磁强度的卡尔曼滤波室内轨迹跟踪算法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN110798806B (zh) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112929818A (zh) * | 2021-02-05 | 2021-06-08 | 华东师范大学 | 一种基于卡尔曼滤波和泛克里金插值的室内定位方法 |
Family Cites Families (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102927980B (zh) * | 2012-10-12 | 2017-05-10 | 深圳市宇恒互动科技开发有限公司 | 一种基于三维多点无线与微惯导的室内定位***、方法 |
| KR102350199B1 (ko) * | 2016-01-05 | 2022-01-14 | 삼성전자주식회사 | 단말에서 위치 추정 방법 및 장치 |
| JP6910846B2 (ja) * | 2017-05-22 | 2021-07-28 | 東芝テック株式会社 | リーダー装置、及びセンシングシステム |
| CN108120436A (zh) * | 2017-12-18 | 2018-06-05 | 北京工业大学 | 一种iBeacon辅助地磁室内实景导航方法 |
| CN108668245A (zh) * | 2018-04-13 | 2018-10-16 | 哈尔滨工业大学 | 一种基于WiFi与地磁场数据库的室内定位方法 |
| CN109298436A (zh) * | 2018-05-15 | 2019-02-01 | 重庆邮电大学 | 一种多信息融合的室内定位与导航方法 |
| CN109218983B (zh) * | 2018-06-28 | 2020-09-18 | 中国人民解放军国防科技大学 | 一种定位方法及定位*** |
-
2019
- 2019-10-30 CN CN201911044891.XA patent/CN110798806B/zh not_active Expired - Fee Related
Also Published As
| Publication number | Publication date |
|---|---|
| CN110798806A (zh) | 2020-02-14 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Yang et al. | A novel NLOS error compensation method based IMU for UWB indoor positioning system | |
| Li et al. | Toward robust crowdsourcing-based localization: A fingerprinting accuracy indicator enhanced wireless/magnetic/inertial integration approach | |
| Li et al. | Toward location-enabled IoT (LE-IoT): IoT positioning techniques, error sources, and error mitigation | |
| Xue et al. | Improved Wi-Fi RSSI measurement for indoor localization | |
| CN103402258B (zh) | 一种基于Wi‑Fi的室内定位***和方法 | |
| CN105588566A (zh) | 一种基于蓝牙与mems融合的室内定位***及方法 | |
| CN112533163B (zh) | 基于NB-IoT改进的融合超宽带和蓝牙的室内定位方法 | |
| Xue et al. | A new weighted algorithm based on the uneven spatial resolution of RSSI for indoor localization | |
| CN105004340A (zh) | 结合惯性导航技术和指纹定位技术的定位误差修正方法 | |
| CN104796866A (zh) | 室内定位方法和装置 | |
| KR20180087814A (ko) | 위치 측정 방법 및 시스템 | |
| Guimarães et al. | A motion tracking solution for indoor localization using smartphones | |
| KR20100046048A (ko) | 참조 포지션과의 포지션 일치 결정 장치 및 그 방법 | |
| CN111722180B (zh) | 一种基于卡尔曼滤波的室内行人定位方法、装置及*** | |
| CN114485656B (zh) | 室内定位方法及相关装置 | |
| CN108801267B (zh) | 一种融合多传感器的室内无锚点定位方法 | |
| CN116567531A (zh) | 基于粒子滤波算法的传感器融合室内定位方法及*** | |
| Seco et al. | RFID-based centralized cooperative localization in indoor environments | |
| Song et al. | Implementation of android application for indoor positioning system with estimote BLE beacons | |
| Si et al. | An adaptive weighted Wi-Fi FTM-based positioning method in an NLOS environment | |
| CN110798806B (zh) | 一种基于地磁强度的卡尔曼滤波室内轨迹跟踪算法 | |
| Marquez et al. | Understanding LoRa-based localization: Foundations and challenges | |
| Zhuang et al. | Autonomous WLAN heading and position for smartphones | |
| Pendao et al. | FastGraph enhanced: High accuracy automatic indoor navigation and mapping | |
| Naguib et al. | Scalable and accurate indoor positioning on mobile devices |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant | ||
| CF01 | Termination of patent right due to non-payment of annual fee |
Granted publication date: 20210312 |
|
| CF01 | Termination of patent right due to non-payment of annual fee |