CN104019813A - 目标即时定位和构建地图的方法与系统 - Google Patents

目标即时定位和构建地图的方法与系统 Download PDF

Info

Publication number
CN104019813A
CN104019813A CN201410277471.7A CN201410277471A CN104019813A CN 104019813 A CN104019813 A CN 104019813A CN 201410277471 A CN201410277471 A CN 201410277471A CN 104019813 A CN104019813 A CN 104019813A
Authority
CN
China
Prior art keywords
target
msub
mrow
map
module
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.)
Granted
Application number
CN201410277471.7A
Other languages
English (en)
Other versions
CN104019813B (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.)
Shanghai Yihang Network Technology Co ltd
Original Assignee
Chigoo Interactive 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 Chigoo Interactive Technology Co Ltd filed Critical Chigoo Interactive Technology Co Ltd
Priority to CN201410277471.7A priority Critical patent/CN104019813B/zh
Publication of CN104019813A publication Critical patent/CN104019813A/zh
Priority to KR1020167035078A priority patent/KR101912195B1/ko
Priority to PCT/CN2015/081429 priority patent/WO2015192745A1/zh
Priority to JP2016572528A priority patent/JP6400128B2/ja
Priority to EP15809774.1A priority patent/EP3159658A4/en
Priority to US15/381,307 priority patent/US20170097237A1/en
Application granted granted Critical
Publication of CN104019813B publication Critical patent/CN104019813B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • GPHYSICS
    • G01MEASURING; TESTING
    • G01CMEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/04Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means
    • G01C21/08Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means involving use of the magnetic field of the earth
    • GPHYSICS
    • G01MEASURING; TESTING
    • G01CMEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12Navigation; 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
    • GPHYSICS
    • G01MEASURING; TESTING
    • G01CMEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20Instruments for performing navigational calculations
    • G01C21/206Instruments for performing navigational calculations specially adapted for indoor navigation
    • GPHYSICS
    • G01MEASURING; TESTING
    • G01CMEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/38Electronic maps specially adapted for navigation; Updating thereof
    • G01C21/3804Creation or updating of map data
    • G01C21/3807Creation or updating of map data characterised by the type of data
    • G01C21/383Indoor data
    • GPHYSICS
    • G01MEASURING; TESTING
    • G01CMEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/38Electronic maps specially adapted for navigation; Updating thereof
    • G01C21/3804Creation or updating of map data
    • G01C21/3859Differential updating map data

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)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
  • Navigation (AREA)

Abstract

本申请公开了目标即时定位和构建地图的方法和装置。其中方法包括:1)对在活动范围内移动的目标进行即时定位;2)根据对所述移动目标的定位,即时更新目标所在活动范围的地磁场地图;3)根据所述更新的地磁场地图,对所述目标移动的下一位置进行定位;重复执行步骤2)和3),直到目标停止移动。该方法可以解决室内目标在没有室内先验磁场地图的条件下,进行即时的目标定位和准确的地磁场地图构建。

Description

目标即时定位和构建地图的方法与系统
技术领域
本发明涉及室内定位及地图构建领域,特别涉及将室内目标的定位和地图构建相结合的方法及系统。 
背景技术
同时定位与地图构建的问题是目标利用具有噪声的传感器数据,既要构建出环境中物体的分布情况,同时还要确定自身的位置。同时定位与地图构建的问题的解法可使目标实现真正的自主导航。在过去的十几年里,同时定位与地图构建的问题逐渐成为移动机器人领域的研究热点。通常的做法是利用EKF(Extended Kalman Filter:扩展卡尔曼滤波器)对机器人位置和地标进行同时估计。但是,当地标位置增多,机器人移动的范围变大时,该方法相应的计算量会变大,实时性变差。此外,该方法且需要事先对环境进行部署,布置好路标点,耗费人力,物力。 
发明内容
针对上述现有技术的缺陷,本发明提供了目标定位与地图构建的方法,该方法采用运动模型,通过对当前地磁场的测量,对当前位置进行矫正,和利用空间插值算法对局部地图数据进行更新,并融合到全局地图,达到了目标定位和地图构建的同步进行,同时大大减小了运算量,也不需要事先对环境进行部署,节省资源。 
根据本发明的一个方面,提供一种目标定位与地图构建方法,包括:1)对在活动范围内移动的目标进行即时定位;2)根据对所述移动目标的定位,即时更新目标所在活动范围的地磁场地图;3)根据所述更新的地磁场地图,对所述目标移动的下一位置进行定位;重复执行步骤2)和3),直到目标停止移动。 
在上述方法中,步骤1)可包括:初始化目标在活动范围内的位置和活动范围内的全局地磁场地图,所述目标设置有测量目标运行的距离和转动的角度值和磁场值的传感器;将目标初始位置的地磁场值通过空间插值法加入所述初始化所述全局地磁场地图;测量所述目标运行的控制变量和所述目标当前位置的地磁场值;采用粒子滤波算法对随机移动的目标进行位置估计。 
在一些实施方式中,所述初始化目标在活动范围内的位置和活动范围的地磁场地图的步骤包括:将目标活动的室内范围抽象化为网格坐标,将网格坐标的大小与活动室的实际大小按比例设置。目标在活动室内的初始位置是由目标的内部传感器确定的。 
在一些实施方式中,通过空间插值法如克里金插值法将测得的地磁场值加入初始地图,对目标周围的测量值进行加权以得出未测量位置的预测,其中各参数的含义是:λi为第i个位置处的测量值的未知权重,S0是预测位置,N是测量值数目。使用克里金方法时,权重λi取决于测量点、预测位置的距离和预测位置周围的测量值之间空间关系的拟合模型。 
在一些实施方式中,所述将地磁场值使用插值算法加入地图的步骤包括:目标位置估计确定后,在地磁网格地图中抽取位置目标点周围的第一预定范围的网格点数据以将测得的地磁场值插值,然后将插值后得到的所述第一预定范围的网格点融合到第二预定范围的网格地图中。 
同时,本发明还提供了目标定位和地图更新系统,包括:定位模块和地图更新模块,其中所述定位模块对目标在活动范围内的移动进行即时定位;所述地图更新模块根据对目标的定位即时更新目标所在活动范围的地磁场地图,所述定位模块进一步根据所述更新的地磁场地图,对目标下一位置进行定位校正,直到目标停止移动。 
在一些实施方式中,其中所述定位模块包括:初始化模块,配置为对目标初始位置和地磁场网格地图进行初始化;位置估计模块,配置为使用粒子滤波算法和运动学模型对目标初始位置和运动中的位置进行估计,所述地图更新模块包括:插值模块,配置为利用空间插值算法将测得的所述目标活动范围的地磁场值进行插值,和将局部地图融合到全局地图。 
所述位置估计模块可以基于粒子滤波实现,包括:粒子滤波算法模块,配置为对目标的位置估计进行校正;运动模型模块,配置为对目标在活动范围内的移动通过预定数学模型进行分析。 
在一些实施方式中,在所述权重的确定中,采用与磁场的方向和强度向量<Hx,Hy,Hz>相应的网格地图构成数据库,分别表示对应的向量分布。由此,可以增加地图的特征,有利于定位。 
在一些实施方式中,所述构建地图模块中包括:插值模块,在目标位置估计确定后,在地磁网格地图中抽取位置目标点周围的预定范围的网格点数据将所测得的地磁场值插值,和融合模块,将插得的预定范围网格点融合到预定范围的网格地图中。 
与现有技术相比,本发明的上述技术方案具有以下优点:由于采用对目标的即时定位和根据即时定位的结果更新磁场地图,又利用更新的磁场地图对目 标移动进一步定位,将定位与地图更新结合来准确快速地对目标进行定位和构建连续一致的地图。 
进一步,本发明人通过深入和广泛的研究,面对众多的算法,选择采用校正产生误差的粒子滤波算法对室内地磁场地图的更新,并采用空间协方差最佳插值方法-克里金算法插值,结果可信度高,这两种方法的结合能够更加准确快速地对机器人进行定位和构建连续一致的地图。 
并且,在现在的大多数建筑物内,由于钢筋混泥土结构,家具等能影响磁场波动的物件存在,使室内磁场波动稳定存在,而本发明正是利用室内的磁场波动来实现定位,进而实现即时定位和构建地图。所以,本发明的实现不需要额外布置定位所需的参考点,由此可以减少花费。 
附图说明
图1为本发明一种实施方式中目标定位方法的步骤示意图; 
图2为本发明一种实施方式中初始化目标在活动范围内的初始位置的示意图; 
图3为本发明一种实施方式中活动范围的地磁场网格初始地图的示意图; 
图4为本发明一种实施方式中将目标初始位置的地磁场值通过克里金插值法加入初始地图的示意图; 
图5为本发明一种实施方式中对移动的目标进行位置矫正的示意图; 
图6为本发明一种实施方式中测量目标当前位置的地磁场值示意图; 
图7为本发明一种实施方式中将地磁场值使用插值算法加入局部地图的示意图; 
图8为本发明一种实施方式中将局部地图融合到全局地图的示意图; 
图9为本发明一种实施方式中地磁场的目标即时定位和地图构建系统的组成示意图。 
具体实施方式
下面结合附图对发明作进一步详细的说明。 
图1为本发明一种实施方式中的地磁场的即时目标定位和地图构建方法的步骤示意图。如图1所示,本发明一实施方式的目标即时定位和地图构建方法的步骤包括: 
步骤S101:对待定位的目标所处的活动区域(本例中以一个活动室为例)进行实地测量,获得室内平面图,并对平面图进行模拟网格地图初始化。对待 定位的目标,结合目标内部传感器的数据(li,θi)其中,li为第i次移动的距离,θi为第i次移动的偏向角。确定其在模拟的初始化网格地图中的位置。图2中A所指位置是目标的初始位置,旁边的黑点是粒子滤波算法中的重要性采样算法对位置空间的采样点,这些采样点的均值表示对目标位置的估计。 
步骤S102:若目标处于初始位置,则粒子均匀的分布在A的周围。A的位置是由目标的内部传感器测得的。随着目标的移动,基于运动模型xk=f(xk-1,vk-1,wk),即通过xk-1和控制输入量vk-1能求xk,其中,xk为k时刻的状态,vk-1为k-1时刻的控制输入量(即目标运行的距离和转动的角度),wk为k-1时刻到k时刻的相关误差,传感器本身以及周围的环境会使测量带有一定的误差,所以机器人想要依据这些不确切的控制信息来实现精确地定位是很难的,本发明中的误差为随机噪声和高斯白噪声的累加和。本发明中所使用的运动模型为 
< x t , y t > = MOVE + < x t - 1 , y t - 1 > + 0.5 * randn ( 1,2 ) + 0.01 * wgn ( 1,2,0.05 ) MOVE = [ - l t * sin ( &theta; t ) , l t * cos ( &theta; t ) ] , 0 &le; &theta; t < 90 MOVE = [ - l t * cos ( &theta; t - 90 ) , - l t * sin ( &theta; t - 90 ) ] , 90 &le; &theta; t < 180 MOVE = [ l * cos ( 270 - &theta; t ) , - l t * sin ( 270 - &theta; t ) ] , 180 &le; &theta; t < 270 MOVE = [ l t * cos ( 360 - &theta; t ) , l t * sin ( 360 - &theta; t ) ] , 270 &le; &theta; t < 360
其中,lt为t-1时刻到t时刻目标移动的距离;θt为t时刻目标相对于t-1时刻目标转过的角度,且以正北方向为基准,按逆时针转;randn(1,2)是产生向量为1×2的随机数,wgn(1,2,0.05)产生0.05dBw的1×2的高斯白噪声。设定的所有粒子会随着运动模型移动(如图5所示),得到粒子新的位置的采样值。图5中采样点在室内平面图上的移动过程所示,然后再基于目标所携带的外部传感器检测到的磁场值来估计目标的当前位置。虽然通过控制输入参数可以对目标的位置进行大概估计,但存在偏差,并且这种偏差会累积,因此,在一个实施例中,进一步对目标位置进行矫正,就是使测量的位置和实际的位置一致,尽量消除偏差的累积。 
粒子滤波算法的具体实现为: 
设置初始粒子分布:粒子的初始分布是在目标(机器人)起始位置周围的均匀分布,产生Num=100个粒子粒子权重为基于这种假设,粒子数为100即可。由于粒子滤波算法的一大缺点就是计算量大,实时性差,其中粒子数是决定计算量的大小的关键因素,所以本例中在保证精度的情况下,减少粒子数可以减少计算量,保证实施性。所以粒子滤波的收敛会很快,可以达到实时运行; 
重要性重采样:对于每个粒子,更新状态从根据条件概率分布 即可得到100个粒子的新的位置。其中,是t-1时刻第i 个粒子的状态,Ut是t时刻的控制输入。粒子是从目标所在的位置空间中采样得到的,这些粒子的分布是对目标空间位置的概率分布的表示。 
权重计算:计算权重的理论为此处, 
为真实分布,而为建议性的分布。由于磁场的方向和强度是由向量<Hx,Hy,Hz>表示的,所以本发明采用的数据库是由3个大小相同的网格地图组成,分别表示Hx,Hy,Hz的分布,按此方式,地图的特征会增多,有利于定位。权重的计算是基于指数分布,指数分布的计算复杂度(一般用时间复杂度来表示)是O(N),而高斯分布的计算复杂度是O(N2),N表示粒子数。由此可见本发明基于指数分布计算权重的方法可以减少运算量,加快收敛速度,提高定位精度,其指数分布函数为 
Dis = | R . magx - mapx | + | R . magy - mapy | + | R . magz - mapz | R . w = R . w * &lambda; * exp ( ( - Dis ) / &lambda; ) ,
其中,R.magx为目标在当前位置点测得的磁场在Hx方向的值,mapx为当前位置网格地图上当前位置的值,R.w为粒子的权重。网格地图上的磁场值可能是由插值算法得到的,也可能目标之前经过这里,是直接加入的值,且R.magy、mapy、R.magz、mapz类似。λ的值可以依据实验所得的数据进行合理的调整,本例中为2。 
归一化权重: &omega; k ( i ) = &omega; k i / &Sigma; i = 1 N um &omega; k ( i ) ;
计算有效粒子(如通过权重来确定有效粒子)数设定一个阈值δ,如根据经验值,按离子数的比例设置,可设为粒子总数的75%。若有效粒子数小于这个阈值,根据粒子权值对粒子Xk进行重采样,去除权值小的粒子,按照去除的权值小的粒子数量复制权值大的粒子,粒子总数保持不变; 
估计目标位置:基于上步采样完的粒子,计算粒子空间位置分布的均值,具体是由粒子的权值与位置坐标相乘,然后叠加得到的乘积,估计当前目标位置。 
步骤S103:在步骤S101中所获得的室内模拟网格图(如图3所示)上,采用克里金插值算法将目标外部传感器测得的值插值到模拟网格图中,插值后的效果图如图4,右侧的图是对左侧有插值部分的放大。 
在仿真实验中,本发明的地磁场数据库的获得是通过在10m×10m的活动室范围内,每隔0.5m测一组真实的磁场值,共测有21×21组,然后对这441组数据进行空间插值运算,结果为81×81组数据,由于室内地磁场波动的稳定性, 这81×81组数据就是本例中采用的地磁场数据库,仿真中用于目标测得的实际值。 
步骤S104:在步骤S103中获得的地磁场值首先要插入到局部地磁场中。局部地磁场的产生从全局地磁场中抽取,抽取的规则为:在估计的位置点网格地图中抽取以估计点为中心的10×10网格数据实施插值;若估计点为中心,取不到10×10的局部网格,则抽取包含估计点的网格边角10×10地图。如:估计点的坐标(x,y)满足(abs(y-1)<5&&abs(x-1)<5),则抽取全局地图中横坐标点为1到10的,纵坐标为1到10的区域。插值完成的局部地图(如图7所示)融合到全局地图的过程是抽取的逆过程(如图8所示)。这样可以考虑到磁场数据与估计点周围磁场值的关联性,而且还可以减少插值的运算量。 
步骤S105:在活动范围内,随机移动目标,收集目标内部传感器获得的相关数据,为下一步粒子的更新提供依据。 
步骤S106:检测目标是否停止运动,否就回到步骤S102,重复操作(迭代),直到目标停止运动为止。 
如图9所示,本发明的一种实施方式中还提供了目标即时定位和构建地图系统,包括: 
初始化模块201,配置为对目标初始位置和地磁场网格地图的初始化。 
位置估计模块202,配置为使用粒子滤波算法和运动学模型对目标初始位置和运动中的位置进行估计。 
地图更新模块203,配置为利用空间插值算法对传感器测得的地磁场值进行插值,并将局部地图融合到全局地图。 
其中,在位置估计模块202中,包括: 
粒子滤波算法模块2021,包括: 
初始化粒子分布模块20211,配置为后面粒子集的移动提供原始的参考; 
重要性重采样模块20212,配置为产生基于目标移动过程中的粒子估计集合; 
权重计算模块20213,配置为对所述粒子赋予相关权重大小,其中与目标特性越接近则权重越大,反之越小; 
归一化权重模块20214,配置为对所述粒子的权重进行归一化处理,使所有粒子的权重之和等于一; 
有效粒子数确定模块20215,配置为找出与目标特性接近的粒子数;和 
位置估计校正模块20216,配置为对目标移动位置的估计进行校正。 
通过上述模块的操作,采用如上文所述的粒子滤波方法进行初始粒子分布、重要性重采样、权重计算、归一化权重、计算有效粒子数的操作,估计目标(机器人)的当前位置。 
运动模型单元2022,配置为对目标在活动范围内的移动通过预定数学模型进行分析。本例中采用非线性动态模型,即 x t = f ( x t - 1 , v t - 1 ) y t = h ( x t , n t ) .
该模型主要是利用先验信息对观测值进行预测,在得到观测值后,利用该式得到关于参数的后验信息,从而修正先验信息,再以此修正后的信息作为先验信息进行预测,如此循环进行,不断得到观测值修正目标位置的先验信息(先前位置)。本发明中,xt=f(xt-1,vt-1)与步骤102中的运动模型相同,而yt=h(xt,nt)为粒子基于插值产生的地图来修正位置信息。 
其中,在地图更新模块203中,包括: 
插值模块2031,配置为例如用克里金法对周围的测量值进行加权以得出未测量位置的预测,使用如下公式由数据加权总和得到预测:  λi为第i个位置处的测量值的位置权重,S0为预测位置,Si为预测位置周围的随机点,Z(Si)为位置Si处的磁场值,N为测量值数量,本例中设为100。 
其中,数据点在网格中用坐标点Si(ai,bi)i=1,2,...,n表示;待插点坐标用S0(a0,b0)表示,然后求散乱数据点之间的距离i=1,2,...,n;j=1,2,...,n;将计算所得的距离值按照从小到大排成顺序,然后将距离分成若干组,每组包含一定数量的距离值。将这些距离值组合成距离组(用{h′m}表示): 
m=1,2,...,NH,NH表示距离组的个数,根据得出的{h′m},求出(i=1,...,N(h))分布形状,其中,横坐标是h′m,纵坐标是γ*(h′m),需要选择合适的模型进行拟合,以产生连续的曲线 来表示γ*(h′m)的分布,本例中,采用的半变差拟合模型为圆形: 
其中,c0为块金值,c为基台值,α为变程。进行函数拟合,求出模型参数(如c0,c与α等),从而得到变差函数γ*(h)的表达式,然后再以方程组 &Sigma; i = 1 N &gamma; * ( S i - S j ) &lambda; i - &mu; = &gamma; * ( S 0 - S j ) , ( j = 1 , . . . , N ) &Sigma; i = 1 N &lambda; i = 1 , 其中μ是Lagrange乘子,求解方程组可以求得:权值λi与Lagrange乘子μ,最后,根据 求出S0(a0,b0)处的预测值
地图融合模块2032,配置为局部地磁场的产生从全局地磁场中抽取的,抽取的规则为:在估计的位置点网格地图中抽取以估计点为中心的10×10网格数据实施插值;若估计点在网格地图中的周边位置,则抽取包含估计点的网格边角10×10地图。由于插值完成的局部地图(如图7所示)与插值完成之前的抽取地图的大小不变,所以融合到全局地图的过程(如图8所示)为:将从全局地图中抽取的局部地图按照抽取的位置再覆盖进去。 
最后说明的是:以上所述的仅是本发明的一些实施方式,而非对其限制;尽管参照前述实施例对本发明进行了详细的说明,但本领域的普通技术人员来说,在不脱离本发明创造构思的前提下,还可以做出若干变形和改进,而这些修改和改进,并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。 

Claims (14)

1.目标即时定位与构建地图的方法,包括:
1)对在活动范围内移动的目标进行即时定位;
2)根据对所述移动目标的定位,即时更新目标所在活动范围的地磁场地图;
3)根据所述更新的地磁场地图,对所述目标移动的下一位置进行定位;
重复执行步骤2)和3),直到目标停止移动。
2.根据权利要求1所述的方法,其中所述对目标在活动范围内的移动进行即时定位包括:
初始化目标在活动范围内的位置和活动范围内的全局地磁场地图,所述目标设置有测量目标运行的距离和转动的角度值和磁场值的传感器;
将目标初始位置的地磁场值通过空间插值法加入所述初始化所述全局地磁场地图;
测量所述目标运行的控制变量和所述目标当前位置的地磁场值;
采用粒子滤波算法对随机移动的目标进行位置估计。
3.根据权利要求2所述的方法,其中所述根据对目标的定位即时更新目标所在活动范围的地磁场地图包括:
在对目标的位置估计确定后,在所述地磁网格地图中抽取位置目标点周围的预定范围的网格点数据将所述测得的地磁场值插值,然后将所得到的预定范围网格点融合到预定范围的网格地图中。
4.根据权利要求1所述的方法,其中,所述初始化目标在活动范围内的位置和活动范围的地磁场地图包括:
将目标活动的室内范围抽象化为网格坐标,所述网格坐标的大小与活动室的实际大小按比例设置。
5.根据权利要求1-4任一项所述的方法,其中,通过克里金插值法将目标初始位置的地磁场值插入初始地磁场地图中,包括:
对所述目标周围的测量值按如下方式进行加权以得出未测量位置的预测其中λi为第i个位置处的测量值的未知权重,S0表示预测位置,N为测量值数量,所述权重λi取决于测量点、预测位置的距离和预测位置周围的测量值之间空间关系的拟合模型。
6.根据权利要求5所述的方法,其中,所述拟合模型为圆形,其变差函数表达式如下:
其中,C0为块金值,C为基台值,α为变程。
7.根据权利要求2所述的方法,其中所述采用粒子滤波算法对随机移动的目标进行定位估计包括:
测量目标前一时刻到当前时刻移动的距离和偏移的角度,
设粒子初始为均匀分布,随着目标的移动,所述粒子按与所述目标相同的运动模型移动,基于非线性动态模型 x k = f ( x k - 1 , v k - 1 , w k ) S k = h ( x k , n k ) 通过粒子的位置均值来估计所述目标的位置。
8.根据权利要求7所述的方法,其中所述运动模型为:
< x t , y t > = MOVE + < x t - 1 , y t - 1 > + 0.5 * randn ( 1,2 ) + 0.01 * wgn ( 1,2,0.05 ) MOVE = [ - l t * sin ( &theta; t ) , l t * cos ( &theta; t ) ] , 0 &le; &theta; t < 90 MOVE = [ - l t * cos ( &theta; t - 90 ) , - l t * sin ( &theta; t - 90 ) ] , 90 &le; &theta; t < 180 MOVE = [ l * cos ( 270 - &theta; t ) , - l t * sin ( 270 - &theta; t ) ] , 180 &le; &theta; t < 270 MOVE = [ l t * cos ( 360 - &theta; t ) , l t * sin ( 360 - &theta; t ) ] , 270 &le; &theta; t < 360
其中,lt为t-1时刻到t时刻目标移动的距离;
θt为t时刻目标相对于t-1时刻目标转过的角度,且以正北方向为基准,按逆时针转;randn(1,2)是产生向量为1×2的随机数,wgn(1,2,0.05)产生0.05dBw的1×2的高斯白噪声。
9.目标即时定位和地图构建系统,包括定位模块和地图更新模块,其中
所述定位模块对目标在活动范围内的移动进行即时定位;
所述地图更新模块根据对目标的定位即时更新目标所在活动范围的地磁场地图,
所述定位模块进一步根据所述更新的地磁场地图,对目标的下一移动位置进行定位,直到目标停止移动。
10.根据权利要求9所述的系统,其中
所述定位模块包括:
初始化模块,配置为对目标初始位置和地磁场网格地图进行初始化;
位置估计模块,配置为使用粒子滤波算法和运动学模型对目标初始位置和运动中的位置进行估计,
所述地图更新模块包括:
插值模块,配置为利用空间插值算法将测得的所述目标活动范围的地磁场值插值,和
地图融合模块,将局部地图融合到全局地图。
11.根据权利要求10所述的系统,其中
所述位置估计模块包括:
粒子滤波算法模块,配置为对目标的位置估计进行校正;
运动模型单元,配置为对目标在活动范围内的移动通过预定数学模型进行分析。
12.根据权利要求11所述的系统,其中
所述粒子滤波算法模块包括:
初始化粒子分布模块,配置为后面粒子集的移动提供原始的参考;
重要性重采样模块,配置为产生基于目标移动过程中的粒子估计集合;
权重计算模块,配置为对所述粒子赋予相关权重大小,其中与目标特性越接近则权重越大,反之越小;
归一化权重模块,配置为对所述粒子的权重进行归一化处理;
有效粒子数确定模块,配置为找出与目标特性接近的粒子数;和
位置估计校正模块,配置为对目标移动位置的估计进行校正。
13.根据权利要求12所述的系统,其中,所述权重计算模块采用的数据库是基于磁场的方向和强度由向量<Hx,Hy,Hz>表示的三个网格地图数据,各所述向量分别表示Hx,Hy,Hz的分布,所述三个网格地图的网格大小相同。
14.根据权利要求13所述的系统,其中所述插值模块配置为在目标位置估计确定后,在地磁网格地图中抽取该位置处所述目标周围的预定范围的网格点数据,对所述测得的地磁场值进行插值,并且还包括:
融合模块,将所述插值得到的预定范围的网格点融合到预定范围的网格地图中。
CN201410277471.7A 2014-06-19 2014-06-19 目标即时定位和构建地图的方法与系统 Active CN104019813B (zh)

Priority Applications (6)

Application Number Priority Date Filing Date Title
CN201410277471.7A CN104019813B (zh) 2014-06-19 2014-06-19 目标即时定位和构建地图的方法与系统
KR1020167035078A KR101912195B1 (ko) 2014-06-19 2015-06-15 목표의 실시간 위치 측정과 지도 구축 방법 및 장치
PCT/CN2015/081429 WO2015192745A1 (zh) 2014-06-19 2015-06-15 目标即时定位和构建地图的方法与装置
JP2016572528A JP6400128B2 (ja) 2014-06-19 2015-06-15 目標の実時間測位と地図構築の方法及び装置
EP15809774.1A EP3159658A4 (en) 2014-06-19 2015-06-15 Method and device for real-time target location and map creation
US15/381,307 US20170097237A1 (en) 2014-06-19 2016-12-16 Method and device for real-time object locating and mapping

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201410277471.7A CN104019813B (zh) 2014-06-19 2014-06-19 目标即时定位和构建地图的方法与系统

Publications (2)

Publication Number Publication Date
CN104019813A true CN104019813A (zh) 2014-09-03
CN104019813B CN104019813B (zh) 2017-01-25

Family

ID=51436695

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201410277471.7A Active CN104019813B (zh) 2014-06-19 2014-06-19 目标即时定位和构建地图的方法与系统

Country Status (6)

Country Link
US (1) US20170097237A1 (zh)
EP (1) EP3159658A4 (zh)
JP (1) JP6400128B2 (zh)
KR (1) KR101912195B1 (zh)
CN (1) CN104019813B (zh)
WO (1) WO2015192745A1 (zh)

Cited By (20)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105118015A (zh) * 2015-09-21 2015-12-02 无锡知谷网络科技有限公司 用于公共场所的信息提示方法及移动服务终端
CN105147198A (zh) * 2015-08-10 2015-12-16 深圳先进技术研究院 一种基于扫地机器人的室内地图测绘系统及方法
WO2015192745A1 (zh) * 2014-06-19 2015-12-23 无锡知谷网络科技有限公司 目标即时定位和构建地图的方法与装置
CN105676172A (zh) * 2016-01-11 2016-06-15 无锡知谷网络科技有限公司 簇式磁场定位的方法、装置和系统
CN105928512A (zh) * 2016-04-26 2016-09-07 杭州欣晟达信息技术有限公司 一种基于地磁场的室内定位方法
CN106502253A (zh) * 2016-12-22 2017-03-15 深圳乐行天下科技有限公司 一种移动装置全自主建图方法及装置
CN106595659A (zh) * 2016-11-03 2017-04-26 南京航空航天大学 城市复杂环境下多无人机视觉slam的地图融合方法
CN106767828A (zh) * 2016-12-29 2017-05-31 南京邮电大学 一种手机室内定位解决方法
CN107204015A (zh) * 2017-05-27 2017-09-26 中山大学 基于色彩图像和红外图像融合的即时定位与建图系统
CN107607107A (zh) * 2017-09-14 2018-01-19 斯坦德机器人(深圳)有限公司 一种基于先验信息的Slam方法和装置
WO2018018994A1 (zh) * 2016-07-27 2018-02-01 无锡知谷网络科技有限公司 室内定位方法及定位系统
CN107909271A (zh) * 2017-11-15 2018-04-13 国网四川省电力公司经济技术研究院 一种工程勘察数据管理及评估gis系统
CN108351216A (zh) * 2015-10-05 2018-07-31 日本先锋公司 估计装置、控制方法、程序以及存储介质
CN110445567A (zh) * 2019-08-06 2019-11-12 中国人民解放军国防科技大学 一种电磁频谱地图的构建方法
CN111651547A (zh) * 2020-06-04 2020-09-11 北京四维图新科技股份有限公司 高精度地图数据的获取方法、装置及可读存储介质
CN111879312A (zh) * 2020-07-31 2020-11-03 北京麦钉艾特科技有限公司 一种地磁图在线更新方法
CN112050804A (zh) * 2020-07-31 2020-12-08 东南大学 一种基于地磁梯度的近场磁图构建方法
CN112566009A (zh) * 2019-09-26 2021-03-26 成都易书桥科技有限公司 一种基于地磁的参与式室内定位系统
CN113534826A (zh) * 2020-04-15 2021-10-22 苏州宝时得电动工具有限公司 自移动设备的姿态控制方法、装置及存储介质
CN113566816A (zh) * 2020-04-28 2021-10-29 南宁富桂精密工业有限公司 室内地磁定位方法、服务器及计算机可读存储介质

Families Citing this family (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR101965296B1 (ko) * 2016-11-30 2019-04-19 연세대학교 산학협력단 파티클 간에 지도를 공유하는 이동체의 위치 추정 및 지도 작성 방법 및 장치
JP6673293B2 (ja) * 2017-05-24 2020-03-25 トヨタ自動車株式会社 車両システム
JP6774445B2 (ja) * 2018-02-05 2020-10-21 本田技研工業株式会社 移動体制御システム、移動体及び移動体制御方法
CN108469826B (zh) * 2018-04-23 2021-06-08 宁波Gqy视讯股份有限公司 一种基于机器人的地图生成方法及系统
KR102127359B1 (ko) * 2018-08-22 2020-06-26 한국해양과학기술원 자율수상선을 이용한 수중 자기장 지도 작성 시스템 및 방법
CN111578938B (zh) * 2019-02-19 2022-08-02 珠海格力电器股份有限公司 目标物的定位方法及装置
CN111665470A (zh) * 2019-03-07 2020-09-15 阿里巴巴集团控股有限公司 一种定位方法及装置和机器人
CN110377031B (zh) * 2019-06-28 2022-06-10 炬星科技(深圳)有限公司 运动模型更新方法、装置、电子设备及存储介质
CN113483747B (zh) * 2021-06-25 2023-03-24 武汉科技大学 基于具有墙角信息的语义地图改进amcl定位方法及机器人
CN113701759B (zh) * 2021-08-27 2024-05-03 杭州腓腓科技有限公司 基于可重构超材料的室内同步定位与地图构建方法与系统
US11845429B2 (en) * 2021-09-30 2023-12-19 GM Global Technology Operations LLC Localizing and updating a map using interpolated lane edge data
US11987251B2 (en) * 2021-11-15 2024-05-21 GM Global Technology Operations LLC Adaptive rationalizer for vehicle perception systems toward robust automated driving control
CN114674307B (zh) * 2022-05-26 2022-09-27 苏州魔视智能科技有限公司 一种重定位方法及电子设备

Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101000507A (zh) * 2006-09-29 2007-07-18 浙江大学 移动机器人在未知环境中同时定位与地图构建的方法
CN101354252A (zh) * 2008-09-19 2009-01-28 北京航空航天大学 一种基于多尺度估计的地磁辅助导航算法
CN101404086A (zh) * 2008-04-30 2009-04-08 浙江大学 基于视频的目标跟踪方法及装置
CN101625572A (zh) * 2009-08-10 2010-01-13 浙江大学 基于改进重采样方法和粒子选取的FastSLAM算法
CN101920498A (zh) * 2009-06-16 2010-12-22 泰怡凯电器(苏州)有限公司 实现室内服务机器人同时定位和地图创建的装置及机器人
JP2011174771A (ja) * 2010-02-24 2011-09-08 Clarion Co Ltd 位置推定装置および位置推定方法
CN102256351A (zh) * 2011-05-04 2011-11-23 苏州两江科技有限公司 一种基于无线传感网技术的室内精确定位方法
CN102445201A (zh) * 2011-09-28 2012-05-09 东北林业大学 用于水下载体的地磁异常特征点匹配导航方法
CN103175529A (zh) * 2013-03-01 2013-06-26 上海美迪索科电子科技有限公司 基于室内磁场特征辅助的行人惯性定位系统
CN103838240A (zh) * 2012-11-27 2014-06-04 联想(北京)有限公司 控制方法和电子设备

Family Cites Families (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR100506097B1 (ko) * 2004-02-04 2005-08-03 삼성전자주식회사 자기장 지도 생성 방법 및 장치와 이를 활용한 이동체의포즈 확인 방법 및 장치
JP2009093308A (ja) * 2007-10-05 2009-04-30 Hitachi Industrial Equipment Systems Co Ltd ロボットシステム
KR101833217B1 (ko) * 2011-12-07 2018-03-05 삼성전자주식회사 자기장 지도 기반 측위 시스템에서 이용되는 이동 단말 및 이를 이용한 위치 추정 방법
JP6160036B2 (ja) * 2012-07-23 2017-07-12 株式会社リコー 移動通信装置、及び位置情報通知方法
CN102829782B (zh) * 2012-09-07 2015-06-17 滨州学院 一种地磁辅助惯性导航方法
CN104019813B (zh) * 2014-06-19 2017-01-25 无锡知谷网络科技有限公司 目标即时定位和构建地图的方法与系统

Patent Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101000507A (zh) * 2006-09-29 2007-07-18 浙江大学 移动机器人在未知环境中同时定位与地图构建的方法
CN101404086A (zh) * 2008-04-30 2009-04-08 浙江大学 基于视频的目标跟踪方法及装置
CN101354252A (zh) * 2008-09-19 2009-01-28 北京航空航天大学 一种基于多尺度估计的地磁辅助导航算法
CN101920498A (zh) * 2009-06-16 2010-12-22 泰怡凯电器(苏州)有限公司 实现室内服务机器人同时定位和地图创建的装置及机器人
CN101625572A (zh) * 2009-08-10 2010-01-13 浙江大学 基于改进重采样方法和粒子选取的FastSLAM算法
JP2011174771A (ja) * 2010-02-24 2011-09-08 Clarion Co Ltd 位置推定装置および位置推定方法
CN102256351A (zh) * 2011-05-04 2011-11-23 苏州两江科技有限公司 一种基于无线传感网技术的室内精确定位方法
CN102445201A (zh) * 2011-09-28 2012-05-09 东北林业大学 用于水下载体的地磁异常特征点匹配导航方法
CN103838240A (zh) * 2012-11-27 2014-06-04 联想(北京)有限公司 控制方法和电子设备
CN103175529A (zh) * 2013-03-01 2013-06-26 上海美迪索科电子科技有限公司 基于室内磁场特征辅助的行人惯性定位系统

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
孙枫等: "基于SLAM的水下导航算法及仿真分析", 《仪器仪表学报》 *
王仕成等: "基于SLAM的地磁导航定位", 《火力与指挥控制》 *
王晶: "自主水下航行器同步定位与构图方法研究", 《中国博士学位论文全文数据库工程科技Ⅱ辑》 *
陈卫东;张飞: "移动机器人的同步自定位与地图创建研究进展", 《控制理论与应用》 *

Cited By (25)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2015192745A1 (zh) * 2014-06-19 2015-12-23 无锡知谷网络科技有限公司 目标即时定位和构建地图的方法与装置
CN105147198A (zh) * 2015-08-10 2015-12-16 深圳先进技术研究院 一种基于扫地机器人的室内地图测绘系统及方法
CN105118015A (zh) * 2015-09-21 2015-12-02 无锡知谷网络科技有限公司 用于公共场所的信息提示方法及移动服务终端
CN108351216B (zh) * 2015-10-05 2022-01-18 日本先锋公司 估计装置、控制方法、程序以及存储介质
CN108351216A (zh) * 2015-10-05 2018-07-31 日本先锋公司 估计装置、控制方法、程序以及存储介质
CN105676172A (zh) * 2016-01-11 2016-06-15 无锡知谷网络科技有限公司 簇式磁场定位的方法、装置和系统
CN105928512A (zh) * 2016-04-26 2016-09-07 杭州欣晟达信息技术有限公司 一种基于地磁场的室内定位方法
WO2018018994A1 (zh) * 2016-07-27 2018-02-01 无锡知谷网络科技有限公司 室内定位方法及定位系统
CN106595659A (zh) * 2016-11-03 2017-04-26 南京航空航天大学 城市复杂环境下多无人机视觉slam的地图融合方法
CN106502253A (zh) * 2016-12-22 2017-03-15 深圳乐行天下科技有限公司 一种移动装置全自主建图方法及装置
CN106767828A (zh) * 2016-12-29 2017-05-31 南京邮电大学 一种手机室内定位解决方法
CN107204015A (zh) * 2017-05-27 2017-09-26 中山大学 基于色彩图像和红外图像融合的即时定位与建图系统
CN107607107A (zh) * 2017-09-14 2018-01-19 斯坦德机器人(深圳)有限公司 一种基于先验信息的Slam方法和装置
CN107909271A (zh) * 2017-11-15 2018-04-13 国网四川省电力公司经济技术研究院 一种工程勘察数据管理及评估gis系统
CN110445567A (zh) * 2019-08-06 2019-11-12 中国人民解放军国防科技大学 一种电磁频谱地图的构建方法
CN110445567B (zh) * 2019-08-06 2021-06-25 中国人民解放军国防科技大学 一种电磁频谱地图的构建方法
CN112566009A (zh) * 2019-09-26 2021-03-26 成都易书桥科技有限公司 一种基于地磁的参与式室内定位系统
CN112566009B (zh) * 2019-09-26 2022-12-27 成都易书桥科技有限公司 一种基于地磁的参与式室内定位系统
CN113534826A (zh) * 2020-04-15 2021-10-22 苏州宝时得电动工具有限公司 自移动设备的姿态控制方法、装置及存储介质
CN113534826B (zh) * 2020-04-15 2024-02-23 苏州宝时得电动工具有限公司 自移动设备的姿态控制方法、装置及存储介质
CN113566816A (zh) * 2020-04-28 2021-10-29 南宁富桂精密工业有限公司 室内地磁定位方法、服务器及计算机可读存储介质
CN111651547A (zh) * 2020-06-04 2020-09-11 北京四维图新科技股份有限公司 高精度地图数据的获取方法、装置及可读存储介质
CN111879312A (zh) * 2020-07-31 2020-11-03 北京麦钉艾特科技有限公司 一种地磁图在线更新方法
CN112050804A (zh) * 2020-07-31 2020-12-08 东南大学 一种基于地磁梯度的近场磁图构建方法
CN112050804B (zh) * 2020-07-31 2022-04-08 东南大学 一种基于地磁梯度的近场磁图构建方法

Also Published As

Publication number Publication date
EP3159658A1 (en) 2017-04-26
US20170097237A1 (en) 2017-04-06
WO2015192745A1 (zh) 2015-12-23
KR101912195B1 (ko) 2018-10-26
JP2017520841A (ja) 2017-07-27
CN104019813B (zh) 2017-01-25
KR20170042260A (ko) 2017-04-18
EP3159658A4 (en) 2018-01-24
JP6400128B2 (ja) 2018-10-03

Similar Documents

Publication Publication Date Title
CN104019813B (zh) 目标即时定位和构建地图的方法与系统
Huang et al. A quadratic-complexity observability-constrained unscented Kalman filter for SLAM
Poppinga et al. Fast plane detection and polygonalization in noisy 3D range images
US10288425B2 (en) Generation of map data
CN103644903B (zh) 基于分布式边缘无味粒子滤波的同步定位与地图构建方法
Zhang et al. Algorithms analysis of mobile robot SLAM based on Kalman and particle filter
Taylor et al. Automatic calibration of multi-modal sensor systems using a gradient orientation measure
CN104166989B (zh) 一种用于二维激光雷达点云匹配的快速icp方法
Čurn et al. Data incest in cooperative localisation with the common past-invariant ensemble kalman filter
Masmitja et al. Range-only single-beacon tracking of underwater targets from an autonomous vehicle: From theory to practice
Shojaei et al. Experimental study of iterated Kalman filters for simultaneous localization and mapping of autonomous mobile robots
Li et al. Joint localization based on split covariance intersection on the Lie group
Luo et al. Mobile robot localization based on particle filter
Wang et al. Simultaneous localization and mapping method for geomagnetic aided navigation
Irmisch et al. Simulation framework for a visual-inertial navigation system
Shojaie et al. Iterated unscented SLAM algorithm for navigation of an autonomous mobile robot
Kupervasser et al. Robust positioning of drones for land use monitoring in strong terrain relief using vision-based navigation
CN116520372A (zh) 一种双星时频差定位方法
Liu et al. Collaborative radio SLAM for multiple robots based on WiFi fingerprint similarity
Grzonka et al. Look-ahead proposals for robust grid-based slam with rao-blackwellized particle filters
Sabatta et al. Vision-based path following using the 1D trifocal tensor
CN104467742A (zh) 基于高斯混合模型的传感器网络分布式一致性粒子滤波器
Nawaf et al. Guided underwater survey using semi-global visual odometry
Riaz et al. A fully autonomous indoor mobile robot using SLAM
Garcia-Fernandez et al. Collaborative navigation simulation tool using kalman filter with implicit constraints

Legal Events

Date Code Title Description
C06 Publication
PB01 Publication
C10 Entry into substantive examination
SE01 Entry into force of request for substantive examination
C14 Grant of patent or utility model
GR01 Patent grant
PP01 Preservation of patent right

Effective date of registration: 20190906

Granted publication date: 20170125

PP01 Preservation of patent right
PD01 Discharge of preservation of patent
PD01 Discharge of preservation of patent

Date of cancellation: 20201221

Granted publication date: 20170125

TR01 Transfer of patent right

Effective date of registration: 20201231

Address after: 7 / F, building 18, 3265 Jinhai Road, Fengxian District, Shanghai

Patentee after: Shanghai Yihang Network Technology Co.,Ltd.

Address before: 214028 Wuxi New District, Jiangsu, south of Changjiang Road, south of Shuo Mei Road (B1 block)

Patentee before: CHIGOO INTERACTIVE TECHNOLOGY Co.,Ltd.

TR01 Transfer of patent right
PE01 Entry into force of the registration of the contract for pledge of patent right

Denomination of invention: Method and System for Real Time Target Location and Map Construction

Effective date of registration: 20230510

Granted publication date: 20170125

Pledgee: Industrial Bank Co.,Ltd. Shanghai West sub branch

Pledgor: Shanghai Yihang Network Technology Co.,Ltd.

Registration number: Y2023310000180

PE01 Entry into force of the registration of the contract for pledge of patent right
PC01 Cancellation of the registration of the contract for pledge of patent right

Granted publication date: 20170125

Pledgee: Industrial Bank Co.,Ltd. Shanghai West sub branch

Pledgor: Shanghai Yihang Network Technology Co.,Ltd.

Registration number: Y2023310000180

PC01 Cancellation of the registration of the contract for pledge of patent right
PE01 Entry into force of the registration of the contract for pledge of patent right

Denomination of invention: Methods and systems for real-time target localization and map construction

Granted publication date: 20170125

Pledgee: Industrial Bank Co.,Ltd. Shanghai West sub branch

Pledgor: Shanghai Yihang Network Technology Co.,Ltd.

Registration number: Y2024310000505

PE01 Entry into force of the registration of the contract for pledge of patent right