CN110260855B - 一种融合行人航位推算、地磁信息与室内地图信息的室内行人导航定位方法 - Google Patents

一种融合行人航位推算、地磁信息与室内地图信息的室内行人导航定位方法 Download PDF

Info

Publication number
CN110260855B
CN110260855B CN201910548616.5A CN201910548616A CN110260855B CN 110260855 B CN110260855 B CN 110260855B CN 201910548616 A CN201910548616 A CN 201910548616A CN 110260855 B CN110260855 B CN 110260855B
Authority
CN
China
Prior art keywords
particle
information
particles
pedestrian
follows
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.)
Active
Application number
CN201910548616.5A
Other languages
English (en)
Other versions
CN110260855A (zh
Inventor
梁源
徐兵
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Hunan QingChuang Power Technology Co.,Ltd.
Hunan Weidao Technology Co., Ltd
Original Assignee
Beijing Yiqing Technology Co ltd
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Beijing Yiqing Technology Co ltd filed Critical Beijing Yiqing Technology Co ltd
Priority to CN201910548616.5A priority Critical patent/CN110260855B/zh
Publication of CN110260855A publication Critical patent/CN110260855A/zh
Application granted granted Critical
Publication of CN110260855B publication Critical patent/CN110260855B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • 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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • 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/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • 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

Landscapes

  • Engineering & Computer Science (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Environmental & Geological Engineering (AREA)
  • General Life Sciences & Earth Sciences (AREA)
  • Geology (AREA)
  • Navigation (AREA)
  • Traffic Control Systems (AREA)

Abstract

本发明提供了一种融合行人航位推算、地磁信息与室内地图信息的室内行人导航定位方法，首先利用粒子滤波方法确定行人的起点与初始航向，在确定行人的起点与初始航向后，以起点和初始航向为中心重新播撒粒子，继续利用室内地图与地磁信息对于粒子进行权重更新，利用轮盘赌的方式进行粒子重采样，同时将粒子信息加权融合的结果作为最终的导航结果进行持续输出。解决了传统单一导航方案无法满足室内导航任务的实际需要的问题，通过引入多源融合算法实现了PDR信息，地磁信息以及地图信息之间的信息优势互补，本方案既具有PDR方案成本低，独立自主导航的优势，同时由于利用地磁信息和地图信息对PDR实时修正，保证了长航时下仍具有令人满意的导航定位精度。

Description

一种融合行人航位推算、地磁信息与室内地图信息的室内行 人导航定位方法
技术领域
本发明属于室内行人导航定位领域，特别涉及一种融合行人航位推算、地磁信息与室内地图信息的室内行人导航定位方法。
背景技术
目前，全球卫星定位系统(Global Navigation Satellite System,GNSS)(例如GPS)是民用领域中最为主流和普及的导航定位技术。在卫星信号的覆盖范围内， GNSS具有良好的定位效果。但是由于卫星信号无法穿透绝大多数障碍物，如高楼，高山等，所以在城市区域以及室内环境下，GNSS无法实现准确导航定位。然而随着基于位置服务(LocationBased Service,LBS)相关应用的快速发展，实现在室内环境下的准确导航定位变得更加迫切。在室内行人定位和导航技术方面，已经有多家商业巨头在此领域布局，如Apple、Google、Qualcomm、Intel、 Cisco、阿里、百度等等。
行人室内定位导航技术主要分为以下几类，基于射频技术的定位方法(包含WIFI，低功耗蓝牙(BLE)，紫蜂(Zigbee)，超宽带(UWB)，全球移动通讯系统(如4G)等)，基于地理特征的定位(包含地磁定位，地图匹配，气压计测高，超声波测距等)，行人航位推算(Pedestrian Dead Reckoning，PDR；利用行人身上可穿戴设备中的MEMS陀螺、MEMS加速度计进行步长估计，航向估计等，从而实现航位递推)等等。其中，基于射频技术的定位方法的优势在于造价低廉，可以进行大范围的部署，但是缺点是电磁波易受到环境的干扰，需要预设基站，定位精度不高；基于地理特征的定位方法的优势在于无需预设基站，可靠性较高，但是缺点在于无法实现独立自主的导航定位；PDR方案的优势在于成本低，无需架设辅助设备、可扩展性强，受环境影响极小，缺点在于长时间精度较差。从上述总结来看，单一的导航方案无法满足室内导航任务的实际需要，有必要采用组合导航方案来实现室内行人导航。此外，由于室内场景一般都有数个出入口(如商场)，如何在没有先验信息(缺少行人起点与初始航向信息)的情况下，独立自主地实现行人起点与初始航向的确定也是一个必须要解决的问题。
发明内容
针对上述存在的问题，本发明的目的是提供一种融合行人航位推算、地磁信息与室内地图信息的室内行人导航定位方法，以实现室内环境下的行人的导航定位。
本发明的目的是通过以下技术方案实现的：
一种融合行人航位推算、地磁信息与室内地图信息的室内行人导航定位方法，其特征在于，所述方法包括以下步骤：
步骤1：在室内导航场景下行人的可能起点处，播撒初始粒子；
步骤2：利用PDR所提供的航向增量与位置增量对初始粒子进行位姿更新，利用室内地图与地磁信息对于初始粒子进行权重更新与粒子淘汰，同时利用 DBSCAN算法检测粒子分布是否满足聚类条件，如果满足则进入步骤3，否则继续执行步骤2；
步骤3：如果DBSCAN算法结果为全部现存粒子可聚为一类，则对所有现存粒子按照其权重信息进行加权求和，加权求和后得到行人起点与初始航向，并对所有粒子进行清空操作；
步骤4：以步骤3中得到的行人起点与初始航向为中心，重新播撒粒子；
步骤5：利用PDR所提供的航向增量与位置增量对粒子进行位姿更新，利用室内地图与地磁信息对于粒子进行权重更新，同时检测是否出现粒子匮乏，如果出现则进行粒子重采样，同时对所有现存粒子按照其权重信息进行加权求和，加权求和后的结果作为行人导航定位最终结果。
进一步的，步骤1中播撒初始粒子按照每一度播撒一个粒子的规则进行粒子播撒，所述初始粒子的数目为：180*可能起点数目，其中每一个粒子中均包含三类信息：行人当前位置(pparticle)，行人当前航向(θparticle)，自身权重(Wparticle)。
进一步的，步骤2中所述位姿更新的方式为：
Figure RE-GDA0002163056130000021
Figure RE-GDA0002163056130000022
其中，k代表当前时刻行人行走计步数，ΔpPDR与ΔθPDR分别代表PDR所提供的位置增量与航向增量，
Figure RE-GDA0002163056130000023
与
Figure RE-GDA0002163056130000024
分别代表第q个粒子在第k步的位置信息与航向信息。
进一步的，步骤2中所述权重更新与粒子淘汰的方式为：
首先，根据粒子的当前位置
Figure RE-GDA0002163056130000031
进行粒子权重
Figure RE-GDA0002163056130000032
进行更新，更新流程如下：
变量包括：
Figure RE-GDA0002163056130000033
为第q个粒子的位置序列；
Figure RE-GDA0002163056130000034
为第q个粒子的地磁信息敏感序列；
Figure RE-GDA0002163056130000035
为根据Pq中的位置信息，在地磁基准图中查询得到的地磁标准信息序列；
计算序列Nq与序列Ηq的MAD算法结果Dq：
Figure RE-GDA0002163056130000036
根据Dq进一步采用Sigmoid函数进行粒子权重更新计算：
Figure RE-GDA0002163056130000037
其中，Sigmoid函数是一个在生物学中常见的S型函数，也称为S型生长曲线。其函数表达式为：
Figure RE-GDA0002163056130000038
在此基础上，进一步根据室内地图信息对粒子进行所述粒子淘汰，粒子淘汰方式如下：
查询
Figure RE-GDA0002163056130000039
是否位于地图中的不可通行区域，如果是，则将第q个粒子淘汰；
当全部粒子权重均完成更新后，进行所有现存粒子的权重进行归一化处理，以其中第q个粒子为例做说明，归一化算法如下：
Figure RE-GDA00021630561300000310
进一步的，步骤3中所述现存粒子的信息加权求和算法如下：
Figure RE-GDA0002163056130000041
Figure RE-GDA0002163056130000042
其中，Pini和θini代表计算得到的行人初始位置与初始航向。
进一步的，步骤4中所述重新散播粒子的规则如下：
第q个粒子的位置信息为：
Figure RE-GDA0002163056130000043
航向信息为：θini+randn(10)；权重信息为：
Figure RE-GDA0002163056130000044
其中，randn(x)函数为生成一个均值为0，方差为x的随机数；Ntotal代表播撒粒子的总数目，此处设置为1000。
进一步的，步骤5中所述位姿更新方式为：
Figure RE-GDA0002163056130000045
Figure RE-GDA0002163056130000046
其中，k代表当时行人行走步数，ΔpPDR与ΔθPDR分别代表PDR所提供的位置增量与航向增量，
Figure RE-GDA0002163056130000047
与
Figure RE-GDA0002163056130000048
分别代表第q个粒子在第k步的位置信息与航向信息。
进一步的，步骤5中所述粒子进行权重更新的流程如下：
变量包括：
Figure RE-GDA0002163056130000049
为第q个粒子的历史位置序列；
Figure RE-GDA00021630561300000410
为第q个粒子的地磁信息敏感序列；
Figure RE-GDA00021630561300000411
为根据P中的历史位置信息，在地磁基准图中查询得到的地磁标准信息序列；
首先计算序列Nq与序列Ηq的MAD算法结果Dq：
Figure RE-GDA00021630561300000412
根据Dq进一步采用Sigmoid函数进行粒子权重更新计算：
Figure RE-GDA00021630561300000413
其中，Sigmoid函数是一个在生物学中常见的S型函数，也称为S型生长曲线。其函数表达式为：
Figure RE-GDA0002163056130000051
再此基础上，进一步根据室内地图信息对粒子权重进行更新，更新方式如下：
查询
Figure RE-GDA0002163056130000052
是否位于地图中的不可通行区域，如果是，将第q个粒子权重更新为：
Figure RE-GDA0002163056130000053
当全部粒子权重均完成更新后，进行所有粒子的权重进行归一化处理，以其中第q个粒子为例做说明，归一化算法如下：
Figure RE-GDA0002163056130000054
进一步的，步骤5中所述粒子匮乏的检测方式为：
计算有效粒子数目
Figure RE-GDA0002163056130000055
当
Figure RE-GDA0002163056130000056
的值小于某一阈值时，认为出现粒子匮乏情况，此时采取重采样措施，其中
Figure RE-GDA0002163056130000057
的计算公式如下：
Figure RE-GDA0002163056130000058
根据粒子权值对离散粒子进行重采样；所述阈值为40～100。
进一步的，步骤5中所述所有现存粒子的权重信息进行加权求和的算法如下：
Figure RE-GDA0002163056130000059
Figure RE-GDA00021630561300000510
其中，Pcurrent和θcurrent代表计算得到的行人当前位置与航向，并作为最终导航定位结果持续对外进行输出。
本发明相比现有技术的有益效果为：
(1)本发明解决了传统的单一导航方案无法满足室内导航任务的实际需要的问题，通过引入多源融合算法实现了PDR信息，地磁信息以及地图信息之间的信息优势互补，使得各种导航手段之间取长补短，本发明中的导航方案既具有 PDR方案成本低，独立自主导航的优势，同时由于利用地磁信息以及地图信息对PDR实时修正，保证了长航时下仍具有令人满意的导航定位精度；
(2)本发明所设计的组合导航方案不再依赖于外部信息提供行人导航的起点与初始航向，可以在先验信息缺失的情况下，独立自主地实现行人导航起点与初始航向准确判断；
(3)本发明所设计的粒子滤波(Particle filter，PF)算法架构，不再采用传统的PF算法中采用粒子方差/有效粒子数等参数衡量粒子收敛性方法，而是采用基于密度的空间聚类算法(Density-Based Spatial Clustering of Applications with Noise，DBSCAN)实现粒子收敛性的在线判别。传统的判断方法在某些情况下会出现判断误判情况，导致PF算法输出错误的导航定位结果；而引入的DBSCAN 算法具有运算时间短，对于噪声影响有较强抑制的特点，采用DBSCAN算法进行粒子收敛性识别(是否所有粒子均可归为一类)，可以快速准确地识别粒子群状态，有效的对噪声等干扰进行隔离，确保PF算法可以及时根据粒子群状态做出正确决断；
(4)本发明所设计的PF算法架构，不再采用传统PF算法中基于后验概率的方式更新粒子权重，而是利用平均绝对差算法(Mean Absolute Deviation， MAD)结合地磁基准图与室内地图实现粒子权重实时更新。传统的更新方法由于更新结果仅依赖于单一数据点，所以对于噪声、野值的适应性较差，而MAD 算法的引入则可以有效解决上述问题，MAD算法通过序列匹配方式进行粒子权重更新，有效抑制偶发数据野值，噪声等信息对于PF算法导航精度的干扰，提升粒子收敛的稳健性。
附图说明
图1为融合行人航位推算、地磁信息与室内地图信息的室内行人导航定位方法整体流程图；
图2为本发明所述步骤2中算法的运算逻辑示意图；
图3为本发明所述步骤5中算法的运算逻辑示意图；
图4为行人行走的真实标准路径示意图；
图5为行人初始起点航向确定阶段初始粒子撒播情况示意图；
图6为行人初始起点确定示意图；
图7为本专利导航方法解算结果(粒子形式)示意图；
图8为本专利导航方法解算结果图。
具体实施方式
下面结合附图并举实施例，对本发明进行详细描述。
本实施例提供了一种融合行人航位推算、地磁信息与室内地图信息的室内行人导航定位方法，利用PF算法实现对PDR信息，地磁信息以及地图信息进行数据融合，在本实施例所设计PF算法中，利用DBSCAN算法实现粒子收敛性的在线识别，利用轮盘赌的方式进行粒子重采样，利用MAD算法计算提取出的地磁信息与可穿戴设备所敏感到的地磁信息之间的符合程度，从而更新该粒子权重，同时查询粒子所在的地图信息是否位于可通行区域，根据查询结果更新该粒子权重。整套方案可以在缺失先验信息的情况下，独立自主地实现行人起点与初始航向的确定。在此基础上，该方案可以有效地实现PDR信息，地磁信息以及地图信息之间的信息优势互补，从而有效提升行人导航定位精度，同时增强算法的稳定性与抗干扰性。
本发明的原理是：将行人所有的可能导航结果以粒子形式进行描述，在初始时刻，所有粒子均被赋予相同的权重信息，代表行人等可能性地处于任意一种导航结果中；粒子的权重信息代表该粒子所代表的导航信息与行人的真实导航结果相一致的概率，权重越大，可能性也就越高；而随着行人在场景内的不断运动，本专利中的算法可以利用地磁信息与地图信息对于相关粒子的权重进行不断更新，以削弱权重的方式逐渐弱化乃至淘汰一些不符合行人实际行走情况的粒子，使得粒子群逐渐收敛于行人的真实导航结果，从而实现行人的室内准确导航定位。系统整体流程图如图1所示。
具体流程包括以下步骤：
步骤1：在室内导航场景下行人的可能起点处，按照一定的规律播撒初始粒子。每一个粒子中所包含三类信息：行人当前位置，行人当前航向，自身权重。为方便后续说明，分别定义为：pparticle，θparticle与Wparticle。其中，对于初始粒子而言，所有粒子权重均一致，为粒子总数目的倒数。而粒子的另外两个参数(位置与航向)的初始确定流程如下：
以商场为例，此时行人的可能起点处为商场的全部的入口，则在每一个入口处均播撒初始粒子。对于每一个入口而言，考虑到行人进入商场后的行走航向为任意方向，即可以在180度范围内随机变化，则按照每一度播撒1个粒子的规则进行粒子播撒，即需要180个粒子，即对于这180个粒子而言，他们有着相同的位置信息，只是每隔1个粒子，航向信息变化1度。则对于整体而言，则总共需要播撒的粒子数目为：180*入口数目，这些粒子每隔1个粒子，航向信息变化1 度；每隔180个粒子，航向信息重置为1度，位置信息更新为下一可能起点处。
步骤2：利用PDR所提供的航向增量与位置增量对粒子进行位姿更新，利用室内地图与地磁信息对于粒子进行权重更新与粒子淘汰，同时利用DBSCAN 算法检测粒子分布是否满足聚类条件，如果满足则进入步骤3，否则继续执行步骤2。本步骤运算逻辑示意图如图2所示。
其中，值得说明的是，本发明中所涉及的PDR算法，并不在本发明的范畴中，本发明中只是利用PDR算法所提供的结果，以实现提高导航精度的目的， PDR算法相当于为本发明的具体实施提供原始数据。PDR的核心功能是将 MEMS陀螺仪和MEMS加速度计转换为行人的航向信息与行人的步长信息，进一步转换为航向增量与位置增量，此处不再对PDR算法进行详细说明。
第一步，PDR算法提供航向增量与位置增量对所有现存粒子的位置与航向进行更新，所述的位姿更新方式为(以其中第q个粒子做说明，下同)：
Figure RE-GDA0002163056130000081
Figure RE-GDA0002163056130000082
其中，k代表当前时刻行人行走计步数，ΔpPDR与ΔθPDR分别代表PDR所提供的位置增量与航向增量，
Figure RE-GDA0002163056130000083
与
Figure RE-GDA0002163056130000084
分别代表第q个粒子在第k步的位置信息与航向信息。
第二步，根据粒子的当前位置
Figure RE-GDA0002163056130000085
进行粒子权重
Figure RE-GDA0002163056130000086
进行更新，更新流程如下：
为方便后续说明，做出如下变量定义：
Figure RE-GDA0002163056130000087
为第q个粒子的位置序列(包含l步，此处l设置为 10)；
Figure RE-GDA0002163056130000088
为第q个粒子的地磁信息敏感序列(即为可穿戴设备上的磁强计所敏感到的地磁信息)；
Figure RE-GDA0002163056130000091
为根据Pq中的位置信息，在地磁基准图中查询得到的地磁标准信息序列；
首先计算序列Nq与序列Ηq的MAD算法结果Dq：
Figure RE-GDA0002163056130000092
根据Dq进一步采用Sigmoid函数进行粒子权重更新计算：
Figure RE-GDA0002163056130000093
其中，Sigmoid函数是一个在生物学中常见的S型函数，也称为S型生长曲线。其函数表达式为：
Figure RE-GDA0002163056130000094
在此基础上，进一步根据室内地图信息对粒子进行淘汰，淘汰方式如下：
查询
Figure RE-GDA0002163056130000095
是否位于地图中的不可通行区域(此处室内地图分为可通行区域与不可通行区域，可通行区域值得是行人可以行走的区域，如走廊；不可通信区域值得是行人无法行走或者穿越的区域，如墙壁)，如果是，则将第q个粒子淘汰。
当全部粒子权重均完成更新后，进行所有现存粒子的权重进行归一化处理，以其中第q个粒子为例做说明，归一化算法如下：
Figure RE-GDA0002163056130000096
第三步，利用DBSCAN算法判断粒子群状态，即判断是否全部现存粒子可以归为一类，具体判断流程如下：
统计全部现存粒子的位置信息，进行DBSCAN聚类分析，该算法是一个比较有代表性的基于密度的聚类算法。与划分和层次聚类方法不同，它将簇定义为密度相连的点的最大集合，能够把具有足够高密度的区域划分为簇，并可在噪声的空间数据库中发现任意形状的聚类。此处不再对DBSCAN算法做详细介绍，仅对一些DBSCAN算法中的相关参数选值进行介绍。在本专利的DBSCAN算法中，设定E邻域半径为2m，核心对象样本点数最小值为6。
如果DBSCAN算法结果为全部现存粒子不可聚为一类，则返回第一步继续进行运算，如果可聚为一类，则继续执行步骤3。
步骤3：如果DBSCAN算法结果为全部现存粒子可聚为一类，则对所有现存粒子按照其权重信息进行加权求和，加权求和后得到行人的起点与初始航向，并对所有粒子进行清空操作。其中，所有现存粒子的信息进行加权求和算法如下：
Figure RE-GDA0002163056130000101
Figure RE-GDA0002163056130000102
其中，Pini和θini代表计算得到的行人初始位置与初始航向。
步骤4：以步骤3中得到的行人起点与初始航向为中心，重新播撒粒子。
此次播撒粒子规则如下(以第q个粒子为例)：
该粒子的位置信息为：
Figure RE-GDA0002163056130000103
航向信息为：θini+randn(10)；权重信息为：
Figure RE-GDA0002163056130000104
其中，randn(x)函数为生成一个均值为0，方差为x的随机数；Ntotal代表播撒粒子的总数目，此处设置为1000。
步骤5：利用PDR所提供的航向增量与位置增量对粒子进行位姿更新，利用室内地图与地磁信息对于粒子进行权重更新，同时检测是否出现粒子匮乏，如果出现则进行粒子重采样，同时对所有粒子按照其权重信息进行加权求和，加权求和后的结果作为行人导航定位最终结果。本步骤运算逻辑示意图如图3所示。
第一步，PDR算法提供航向增量与位置增量对所有现存粒子的位置与航向进行更新，所述位姿更新方式为(以其中第q个粒子做说明，下同)：
Figure RE-GDA0002163056130000105
Figure RE-GDA0002163056130000106
其中，k代表当时行人行走步数，ΔpPDR与ΔθPDR分别代表PDR所提供的位置增量与航向增量，
Figure RE-GDA0002163056130000107
与
Figure RE-GDA0002163056130000108
分别代表第q个粒子在第k步的位置信息与航向信息。
第二步，根据粒子的当前位置
Figure RE-GDA0002163056130000109
进行粒子权重
Figure RE-GDA00021630561300001010
进行更新，更新流程如下：
为方便后续说明，做出如下变量定义：
Figure RE-GDA0002163056130000111
为第q个粒子的历史位置序列(包含l步，此处l设置为10)；
Figure RE-GDA0002163056130000112
为第q个粒子的地磁信息敏感序列(即为可穿戴设备上的磁强计所敏感到的地磁信息)；
Figure RE-GDA0002163056130000113
为根据P中的历史位置信息，在地磁基准图中查询得到的地磁标准信息序列；
首先计算序列Nq与序列Ηq的MAD算法结果Dq：
Figure RE-GDA0002163056130000114
根据Dq进一步采用Sigmoid函数进行粒子权重更新计算：
Figure RE-GDA0002163056130000115
其中，Sigmoid函数是一个在生物学中常见的S型函数，也称为S型生长曲线。其函数表达式为：
Figure RE-GDA0002163056130000116
再此基础上，进一步根据室内地图信息对粒子权重进行更新，更新方式如下：
查询
Figure RE-GDA0002163056130000117
是否位于地图中的不可通行区域(此处地图分为可通行区域与不可通行区域，可通行区域值得是行人可以行走的区域，如走廊；不可通信区域值得是行人无法行走或者穿越的区域，如墙壁；)，如果是，将第q个粒子权重更新为：
Figure RE-GDA0002163056130000118
当全部粒子权重均完成更新后，进行所有粒子的权重进行归一化处理，以其中第q个粒子为例做说明，归一化算法如下：
Figure RE-GDA0002163056130000119
第三步，计算有效粒子数目
Figure RE-GDA00021630561300001110
当
Figure RE-GDA00021630561300001111
的值小于阈值50时，认为出现粒子匮乏情况，此时应当采取重采样措施，其中
Figure RE-GDA00021630561300001112
的计算公式如下：
Figure RE-GDA0002163056130000121
根据粒子权值对离散粒子进行重采样。重采样方法是舍弃权值较小的粒子，代之以权值较大的粒子，有点类似于遗传算法中的“适者生存”原理。本专利中采用轮盘赌的方式实现重采样，具体实现原理如下：
由于
Figure RE-GDA0002163056130000122
所以可以将[0,1]区间分割为1000个子区间，每一个子区间的长度为对应的粒子的权重值。设U是区间[0,1]上的服从均匀分布的随机变量，根据U的值落在哪一个子区间，则将该子区间所对应的粒子信息进行一次复制(传递到子代)，例如，假设U的值落在第q个子区间，则将第q个粒子信息进行一次复制，将复制结果保留到子代中去；重复上述过程1000次，得到新一代的1000个粒子信息，用新一代的粒子替换上一代的粒子。通过这种设置，确保权重较大的粒子可以有更高的概率将自身粒子信息尽可能多的复制到子代。
第四步，对所有现存粒子按照其权重信息进行加权求和，加权求和后得到行人的当前位置与航向。其中，所有现存粒子的信息进行加权求和算法如下：
Figure RE-GDA0002163056130000123
Figure RE-GDA0002163056130000124
其中，Pcurrent和θcurrent代表计算得到的行人当前位置与航向，并作为最终导航定位结果持续对外进行输出。
本实施例所述导航算法运行结果如图4至图8所示。其中，图4为行人行走的真实标准路径示意图；图5为行人初始起点航向确定阶段初始粒子撒播情况示意图，该场景中共有两个入口，分别为图5中左下点迹与右上点迹，此处为了可以在图像上更加直观地体现出各初始粒子航向上的差异，将各初始粒子稍微做了一些平移；图6为行人初始起点确定示意图，图6中点代表通过本专利方法所确定的航向起点，需要特别指出的是，由于起点/航向确定过程中，行人一直处于行走状态，所以此时确定出的起点与行人进入场景的起点存在一定的差异，但该差异并不影响导航精度；图7为本实施例导航方法解算结果(粒子形式)示意图，图8为本实施例导航方法解算结果图，从图5与图8的比较中可以看出，本实施例导航算法计算轨迹基本保证了与基准路线的一致性，虽然偶与基准路线有误差，但是可以迅速收敛至正确轨迹，也就说明本算法具有较强的抗干扰能力和具备长时间下仍维持较好导航精度的能力。

Claims (8)

1.一种融合行人航位推算、地磁信息与室内地图信息的室内行人导航定位方法，其特征在于，所述方法包括以下步骤：
步骤1：在室内导航场景下行人的可能起点处，播撒初始粒子；
步骤2：利用PDR所提供的航向增量与位置增量对初始粒子进行位姿更新，利用室内地图与地磁信息对于初始粒子进行权重更新与粒子淘汰，同时利用DBSCAN算法检测粒子分布是否满足聚类条件，如果满足则进入步骤3，否则继续执行步骤2；
步骤2中所述权重更新与粒子淘汰的方式为：
首先，根据粒子的当前位置
Figure FDA0002886808500000011
进行粒子权重
Figure FDA0002886808500000012
进行更新，更新流程如下：
变量包括：
Figure FDA0002886808500000013
为第q个粒子的位置序列；
Figure FDA0002886808500000014
为第q个粒子的地磁信息敏感序列；
Figure FDA0002886808500000015
为根据Pq中的位置信息，在地磁基准图中查询得到的地磁标准信息序列；
计算序列Nq与序列Hq的MAD算法结果Dq：
Figure FDA0002886808500000016
根据Dq进一步采用Sigmoid函数进行粒子权重更新计算：
Figure FDA0002886808500000017
其中，Sigmoid函数是一个在生物学中常见的S型函数，也称为S型生长曲线，其函数表达式为：
Figure FDA0002886808500000018
在此基础上，进一步根据室内地图信息对粒子进行所述粒子淘汰，粒子淘汰方式如下：
查询
Figure FDA0002886808500000019
是否位于地图中的不可通行区域，如果是，则将第q个粒子淘汰；
当全部粒子权重均完成更新后，进行所有现存粒子的权重进行归一化处理，以其中第q个粒子为例做说明，归一化算法如下：
Figure FDA0002886808500000021
步骤3：如果DBSCAN算法结果为全部现存粒子可聚为一类，则对所有现存粒子按照其权重信息进行加权求和，加权求和后得到行人起点与初始航向，并对所有粒子进行清空操作；
步骤4：以步骤3中得到的行人起点与初始航向为中心，重新播撒粒子；
步骤5：利用PDR所提供的航向增量与位置增量对粒子进行位姿更新，利用室内地图与地磁信息对于粒子进行权重更新，同时检测是否出现粒子匮乏，如果出现则进行粒子重采样，同时对所有现存粒子按照其权重信息进行加权求和，加权求和后的结果作为行人导航定位最终结果；
步骤5中所述粒子进行权重更新的流程如下：
变量包括：
Figure FDA0002886808500000022
为第q个粒子的历史位置序列；
Figure FDA0002886808500000023
为第q个粒子的地磁信息敏感序列；
Figure FDA0002886808500000024
为根据P中的历史位置信息，在地磁基准图中查询得到的地磁标准信息序列；
首先计算序列Nq与序列Hq的MAD算法结果Dq：
Figure FDA0002886808500000025
根据Dq进一步采用Sigmoid函数进行粒子权重更新计算：
Figure FDA0002886808500000026
其中，Sigmoid函数是一个在生物学中常见的S型函数，也称为S型生长曲线，其函数表达式为：
Figure FDA0002886808500000027
在此基础上，进一步根据室内地图信息对粒子进行更新，更新方式如下：
查询
Figure FDA0002886808500000031
是否位于地图中的不可通行区域，如果是，将第q个粒子权重更新为；
Figure FDA0002886808500000032
当全部粒子权重均完成更新后，进行所有粒子的权重进行归一化处理，以其中第q个粒子为例做说明，归一化算法如下：
Figure FDA0002886808500000033
2.根据权利要求1所述的方法，其特征在于，步骤1中播撒初始粒子按照每一度播撒一个粒子的规则进行粒子播撒，所述初始粒子的数目为：180*可能起点数目，其中每一个粒子中均包含三类信息：行人当前位置(pparticle)，行人当前航向(θparticle)，自身权重(Wparticle)。
3.根据权利要求1所述的方法，其特征在于，步骤2中所述位姿更新的方式为：
Figure FDA0002886808500000034
Figure FDA0002886808500000035
其中，k代表当前时刻行人行走计步数，ΔpPDR与ΔθPDR分别代表PDR所提供的位置增量与航向增量，
Figure FDA0002886808500000036
与
Figure FDA0002886808500000037
分别代表第q个粒子在第k步的位置信息与航向信息。
4.根据权利要求1所述的方法，其特征在于，步骤3中所述现存粒子的信息加权求和算法如下：
Figure FDA0002886808500000038
Figure FDA0002886808500000039
其中，Pini和θini代表计算得到的行人初始位置与初始航向。
5.根据权利要求1所述的方法，其特征在于，步骤4中所述重新散播粒子的规则如下：
第q个粒子的位置信息为：
Figure FDA0002886808500000041
航向信息为：θini+randn(10)；权重信息为
Figure FDA0002886808500000042
其中，randn(x)函数为生成一个均值为0，方差为x的随机数；Ntotal代表播撒粒子的总数目，此处设置为1000。
6.根据权利要求1所述的方法，其特征在于，步骤5中所述位姿更新方式为：
Figure FDA0002886808500000043
Figure FDA0002886808500000044
其中，k代表当前时刻行人行走计步数，ΔpPDR与ΔθPDR分别代表PDR所提供的位置增量与航向增量，
Figure FDA0002886808500000045
与
Figure FDA0002886808500000046
分别代表第q个粒子在第k步的位置信息与航向信息。
7.根据权利要求1所述的方法，其特征在于，步骤5中所述粒子匮乏的检测方式为：
计算有效粒子数目
Figure FDA0002886808500000047
当
Figure FDA0002886808500000048
的值小于阈值时，认为出现粒子匮乏情况，此时采取重采样措施，其中
Figure FDA0002886808500000049
的计算公式如下：
Figure FDA00028868085000000410
根据粒子权值对离散粒子进行重采样。
8.根据权利要求1所述的方法，其特征在于，步骤5中所述所有现存粒子的权重信息进行加权求和的算法如下：
Figure FDA00028868085000000411
Figure FDA00028868085000000412
其中，Pcurrent和θcurrent代表计算得到的行人当前位置与航向，并作为最终导航定位结果持续对外进行输出。
CN201910548616.5A 2019-06-24 2019-06-24 一种融合行人航位推算、地磁信息与室内地图信息的室内行人导航定位方法 Active CN110260855B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910548616.5A CN110260855B (zh) 2019-06-24 2019-06-24 一种融合行人航位推算、地磁信息与室内地图信息的室内行人导航定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910548616.5A CN110260855B (zh) 2019-06-24 2019-06-24 一种融合行人航位推算、地磁信息与室内地图信息的室内行人导航定位方法

Publications (2)

Publication Number Publication Date
CN110260855A CN110260855A (zh) 2019-09-20
CN110260855B true CN110260855B (zh) 2021-06-29

Family

ID=67920765

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910548616.5A Active CN110260855B (zh) 2019-06-24 2019-06-24 一种融合行人航位推算、地磁信息与室内地图信息的室内行人导航定位方法

Country Status (1)

Country Link
CN (1) CN110260855B (zh)

Families Citing this family (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111079859B (zh) * 2019-12-31 2020-12-04 哈尔滨工程大学 一种无源多站多目标测向交叉定位与虚假点去除方法
CN111556432B (zh) * 2020-04-21 2022-02-01 西安交通大学 一种基于地图信息筛选匹配的众包指纹库构建方法
CN111551181A (zh) * 2020-05-29 2020-08-18 深圳市南科信息科技有限公司 基于智能手机航位推算以及LiFi识别的室内定位方法
CN111698774B (zh) * 2020-06-23 2022-07-26 北京麦钉艾特科技有限公司 基于多源信息融合的室内定位方法及装置
CN114895111B (zh) * 2022-07-06 2022-10-21 中国人民解放军国防科技大学 一种基于权重分配构建电磁地图的方法和系统

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108632761A (zh) * 2018-04-20 2018-10-09 西安交通大学 一种基于粒子滤波算法的室内定位方法
CN109883428A (zh) * 2019-03-27 2019-06-14 成都电科慧安科技有限公司 一种融合惯导、地磁和WiFi信息的高精度定位方法

Family Cites Families (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106643711A (zh) * 2016-11-25 2017-05-10 北京邮电大学 一种基于手持设备的室内定位方法及系统
CN106949895B (zh) * 2017-04-13 2020-05-19 杭州申昊科技股份有限公司 一种适用于变电站环境下的巡检机器人定位方法
CN107504971A (zh) * 2017-07-05 2017-12-22 桂林电子科技大学 一种基于pdr和地磁的室内定位方法及系统
CN107830862A (zh) * 2017-10-13 2018-03-23 桂林电子科技大学 一种基于智能手机的室内定位行人追踪的方法
CN109061703B (zh) * 2018-06-11 2021-12-28 阿波罗智能技术(北京)有限公司 用于定位的方法、装置、设备和计算机可读存储介质
CN108931245B (zh) * 2018-08-02 2021-09-07 上海思岚科技有限公司 移动机器人的局部自定位方法及设备
CN109084765B (zh) * 2018-08-23 2021-09-07 桂林电子科技大学 一种行人室内行走定位方法、装置及存储介质
CN109447161B (zh) * 2018-10-31 2021-07-13 桂林电子科技大学 一种基于汽车雷达数据的实时增量与自适应聚类方法
CN109556611B (zh) * 2018-11-30 2020-11-10 广州高新兴机器人有限公司 一种基于图优化和粒子滤波的融合定位方法

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108632761A (zh) * 2018-04-20 2018-10-09 西安交通大学 一种基于粒子滤波算法的室内定位方法
CN109883428A (zh) * 2019-03-27 2019-06-14 成都电科慧安科技有限公司 一种融合惯导、地磁和WiFi信息的高精度定位方法

Also Published As

Publication number Publication date
CN110260855A (zh) 2019-09-20

Similar Documents

Publication Publication Date Title
CN110260855B (zh) 一种融合行人航位推算、地磁信息与室内地图信息的室内行人导航定位方法
Li et al. Toward location-enabled IoT (LE-IoT): IoT positioning techniques, error sources, and error mitigation
Guo et al. A survey on fusion-based indoor positioning
CN104796866B (zh) 室内定位方法和装置
Shang et al. Improvement schemes for indoor mobile location estimation: A survey
CN112328718B (zh) 一种基于车辆动态轨迹跟踪的道路拓扑构建系统和方法
CN110933599B (zh) 一种融合uwb与wifi指纹的自适应定位方法
CN108919177B (zh) 一种基于虚拟信源估计与轨迹校正的定位地图构建方法
CN110320495A (zh) 一种基于Wi-Fi、蓝牙和PDR融合定位的室内定位方法
CN105430664B (zh) 一种基于分类拟合预测传播路损的方法和装置
CN107270889B (zh) 一种基于地磁图谱的室内定位方法及定位系统
CN111698774B (zh) 基于多源信息融合的室内定位方法及装置
Khalife et al. Indoor localization based on floor plans and power maps: Non-line of sight to virtual line of sight
CN111970633A (zh) 基于WiFi、蓝牙和步行者行航位推算融合的室内定位方法
CN108828643A (zh) 一种基于灰色预测模型的室内外无缝定位系统及方法
Li et al. Location-Enabled IoT (LE-IoT): A survey of positioning techniques, error sources, and mitigation
CN108801267B (zh) 一种融合多传感器的室内无锚点定位方法
Souli et al. Relative positioning of autonomous systems using signals of opportunity
CN114222240A (zh) 一种基于粒子滤波的多源融合定位方法
Cao et al. WiFi RTT indoor positioning method based on Gaussian process regression for harsh environments
Gong et al. An enhanced indoor positioning solution using dynamic radio fingerprinting spatial context recognition
Xingli et al. Indoor positioning technology based on deep neural networks
Cho et al. Enhancing GNSS performance and detection of road crossing in urban area using deep learning
Ta Smartphone-based indoor positioning using Wi-Fi, inertial sensors and Bluetooth
CN117031519A (zh) 一种多目标三维协同定位方法及系统

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
TR01 Transfer of patent right

Effective date of registration: 20220125

Address after: 425000 Xiaoxiang scientific innovation center 618-619, southeast corner of the intersection of Lingling South Road and Fengtai Road, Changfeng Industrial Park, economic development zone, Yongzhou City, Hunan Province

Patentee after: Hunan QingChuang Power Technology Co.,Ltd.

Patentee after: Hunan Weidao Technology Co., Ltd

Address before: Room 315, 3 / F, building 10, yard 1, JinFang Road, Chaoyang District, Beijing

Patentee before: BEIJING YIQING TECHNOLOGY Co.,Ltd.

TR01 Transfer of patent right