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

Abstract

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

Description

目标即时定位和构建地图的方法与***

技术领域

本发明涉及室内定位及地图构建领域，特别涉及将室内目标的定位和地图构建相结合的方法及***。 

背景技术

同时定位与地图构建的问题是目标利用具有噪声的传感器数据，既要构建出环境中物体的分布情况，同时还要确定自身的位置。同时定位与地图构建的问题的解法可使目标实现真正的自主导航。在过去的十几年里，同时定位与地图构建的问题逐渐成为移动机器人领域的研究热点。通常的做法是利用EKF(Extended Kalman Filter：扩展卡尔曼滤波器)对机器人位置和地标进行同时估计。但是，当地标位置增多，机器人移动的范围变大时，该方法相应的计算量会变大，实时性变差。此外，该方法且需要事先对环境进行部署，布置好路标点，耗费人力，物力。 

发明内容

针对上述现有技术的缺陷，本发明提供了目标定位与地图构建的方法，该方法采用运动模型，通过对当前地磁场的测量，对当前位置进行矫正，和利用空间插值算法对局部地图数据进行更新，并融合到全局地图，达到了目标定位和地图构建的同步进行，同时大大减小了运算量，也不需要事先对环境进行部署，节省资源。 

根据本发明的一个方面，提供一种目标定位与地图构建方法，包括：1)对在活动范围内移动的目标进行即时定位；2)根据对所述移动目标的定位，即时更新目标所在活动范围的地磁场地图；3)根据所述更新的地磁场地图，对所述目标移动的下一位置进行定位；重复执行步骤2)和3)，直到目标停止移动。 

在上述方法中，步骤1)可包括：初始化目标在活动范围内的位置和活动范围内的全局地磁场地图，所述目标设置有测量目标运行的距离和转动的角度值和磁场值的传感器；将目标初始位置的地磁场值通过空间插值法加入所述初始化所述全局地磁场地图；测量所述目标运行的控制变量和所述目标当前位置的地磁场值；采用粒子滤波算法对随机移动的目标进行位置估计。 

在一些实施方式中，所述初始化目标在活动范围内的位置和活动范围的地磁场地图的步骤包括：将目标活动的室内范围抽象化为网格坐标，将网格坐标的大小与活动室的实际大小按比例设置。目标在活动室内的初始位置是由目标的内部传感器确定的。 

在一些实施方式中，通过空间插值法如克里金插值法将测得的地磁场值加入初始地图，对目标周围的测量值进行加权以得出未测量位置的预测，其中各参数的含义是：λ_i为第i个位置处的测量值的未知权重，S₀是预测位置，N是测量值数目。使用克里金方法时，权重λ_i取决于测量点、预测位置的距离和预测位置周围的测量值之间空间关系的拟合模型。 

在一些实施方式中，所述将地磁场值使用插值算法加入地图的步骤包括：目标位置估计确定后，在地磁网格地图中抽取位置目标点周围的第一预定范围的网格点数据以将测得的地磁场值插值，然后将插值后得到的所述第一预定范围的网格点融合到第二预定范围的网格地图中。 

同时，本发明还提供了目标定位和地图更新***，包括：定位模块和地图更新模块，其中所述定位模块对目标在活动范围内的移动进行即时定位；所述地图更新模块根据对目标的定位即时更新目标所在活动范围的地磁场地图，所述定位模块进一步根据所述更新的地磁场地图，对目标下一位置进行定位校正，直到目标停止移动。 

在一些实施方式中，其中所述定位模块包括：初始化模块，配置为对目标初始位置和地磁场网格地图进行初始化；位置估计模块，配置为使用粒子滤波算法和运动学模型对目标初始位置和运动中的位置进行估计，所述地图更新模块包括：插值模块，配置为利用空间插值算法将测得的所述目标活动范围的地磁场值进行插值，和将局部地图融合到全局地图。 

所述位置估计模块可以基于粒子滤波实现，包括：粒子滤波算法模块，配置为对目标的位置估计进行校正；运动模型模块，配置为对目标在活动范围内的移动通过预定数学模型进行分析。 

在一些实施方式中，在所述权重的确定中，采用与磁场的方向和强度向量<H_x，H_y，H_z>相应的网格地图构成数据库，分别表示对应的向量分布。由此，可以增加地图的特征，有利于定位。 

在一些实施方式中，所述构建地图模块中包括：插值模块，在目标位置估计确定后，在地磁网格地图中抽取位置目标点周围的预定范围的网格点数据将所测得的地磁场值插值，和融合模块，将插得的预定范围网格点融合到预定范围的网格地图中。 

与现有技术相比，本发明的上述技术方案具有以下优点：由于采用对目标的即时定位和根据即时定位的结果更新磁场地图，又利用更新的磁场地图对目 标移动进一步定位，将定位与地图更新结合来准确快速地对目标进行定位和构建连续一致的地图。 

进一步，本发明人通过深入和广泛的研究，面对众多的算法，选择采用校正产生误差的粒子滤波算法对室内地磁场地图的更新，并采用空间协方差最佳插值方法-克里金算法插值，结果可信度高，这两种方法的结合能够更加准确快速地对机器人进行定位和构建连续一致的地图。 

并且，在现在的大多数建筑物内，由于钢筋混泥土结构，家具等能影响磁场波动的物件存在，使室内磁场波动稳定存在，而本发明正是利用室内的磁场波动来实现定位，进而实现即时定位和构建地图。所以，本发明的实现不需要额外布置定位所需的参考点，由此可以减少花费。 

附图说明

图1为本发明一种实施方式中目标定位方法的步骤示意图； 

图2为本发明一种实施方式中初始化目标在活动范围内的初始位置的示意图； 

图3为本发明一种实施方式中活动范围的地磁场网格初始地图的示意图； 

图4为本发明一种实施方式中将目标初始位置的地磁场值通过克里金插值法加入初始地图的示意图； 

图5为本发明一种实施方式中对移动的目标进行位置矫正的示意图； 

图6为本发明一种实施方式中测量目标当前位置的地磁场值示意图； 

图7为本发明一种实施方式中将地磁场值使用插值算法加入局部地图的示意图； 

图8为本发明一种实施方式中将局部地图融合到全局地图的示意图； 

图9为本发明一种实施方式中地磁场的目标即时定位和地图构建***的组成示意图。 

具体实施方式

下面结合附图对发明作进一步详细的说明。 

图1为本发明一种实施方式中的地磁场的即时目标定位和地图构建方法的步骤示意图。如图1所示，本发明一实施方式的目标即时定位和地图构建方法的步骤包括： 

步骤S101：对待定位的目标所处的活动区域(本例中以一个活动室为例)进行实地测量，获得室内平面图，并对平面图进行模拟网格地图初始化。对待 定位的目标，结合目标内部传感器的数据(l_i，θ_i)其中，l_i为第i次移动的距离，θ_i为第i次移动的偏向角。确定其在模拟的初始化网格地图中的位置。图2中A所指位置是目标的初始位置，旁边的黑点是粒子滤波算法中的重要性采样算法对位置空间的采样点，这些采样点的均值表示对目标位置的估计。 

步骤S102：若目标处于初始位置，则粒子均匀的分布在A的周围。A的位置是由目标的内部传感器测得的。随着目标的移动，基于运动模型x_k＝f(x_k-1，v_k-1，w_k)，即通过x_k-1和控制输入量v_k-1能求x_k，其中，x_k为k时刻的状态，v_k-1为k-1时刻的控制输入量(即目标运行的距离和转动的角度)，w_k为k-1时刻到k时刻的相关误差，传感器本身以及周围的环境会使测量带有一定的误差，所以机器人想要依据这些不确切的控制信息来实现精确地定位是很难的，本发明中的误差为随机噪声和高斯白噪声的累加和。本发明中所使用的运动模型为 

$\left\{\begin{array}{c}<x_t,y_t>=\mathrm{MOVE}+<x_{t-1},y_{t-1}>+\sqrt{0.5}*\mathrm{randn}\left(\mathrm{1,2}\right)+0.01*\mathrm{wgn}\left(\mathrm{1,2,0.05}\right)\\ \mathrm{MOVE}=[-l_t*\sin \left({\mathrm{\&theta;}}_t\right),l_t*\cos \left({\mathrm{\&theta;}}_t\right)],0\&le;{\mathrm{\&theta;}}_t<90\\ \mathrm{MOVE}=[-l_t*\cos ({\mathrm{\&theta;}}_t-90),-l_t*\sin ({\mathrm{\&theta;}}_t-90)],90\&le;{\mathrm{\&theta;}}_t<180\\ \mathrm{MOVE}=[l*\cos (270-{\mathrm{\&theta;}}_t),-l_t*\sin (270-{\mathrm{\&theta;}}_t)],180\&le;{\mathrm{\&theta;}}_t<270\\ \mathrm{MOVE}=[l_t*\cos (360-{\mathrm{\&theta;}}_t),l_t*\sin (360-{\mathrm{\&theta;}}_t)],270\&le;{\mathrm{\&theta;}}_t<360\end{array}\right.$

其中，l_t为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 个粒子的状态，U_t是t时刻的控制输入。粒子是从目标所在的位置空间中采样得到的，这些粒子的分布是对目标空间位置的概率分布的表示。 

权重计算：计算权重的理论为此处， 

为真实分布，而为建议性的分布。由于磁场的方向和强度是由向量<H_x，H_y，H_z>表示的，所以本发明采用的数据库是由3个大小相同的网格地图组成，分别表示H_x，H_y，H_z的分布，按此方式，地图的特征会增多，有利于定位。权重的计算是基于指数分布，指数分布的计算复杂度(一般用时间复杂度来表示)是O(N)，而高斯分布的计算复杂度是O(N²)，N表示粒子数。由此可见本发明基于指数分布计算权重的方法可以减少运算量，加快收敛速度，提高定位精度，其指数分布函数为 

$\left\{\begin{array}{c}\mathrm{Dis}=\sqrt{|R.\mathrm{magx}-\mathrm{mapx}|+|R.\mathrm{magy}-\mathrm{mapy}|+|R.\mathrm{magz}-\mathrm{mapz}|}\\ R.w=R.w*\mathrm{\&lambda;}*\exp ((-\mathrm{Dis})/\mathrm{\&lambda;})\end{array}\right.,$

其中，R.magx为目标在当前位置点测得的磁场在H_x方向的值，mapx为当前位置网格地图上当前位置的值，R.w为粒子的权重。网格地图上的磁场值可能是由插值算法得到的，也可能目标之前经过这里，是直接加入的值，且R.magy、mapy、R.magz、mapz类似。λ的值可以依据实验所得的数据进行合理的调整，本例中为2。 

归一化权重： ${\mathrm{\&omega;}}_k^{\left(i\right)}={\mathrm{\&omega;}}_k^i/\underset{i=1}{\overset{N_{\mathrm{um}}}{\mathrm{\&Sigma;}}}{\mathrm{\&omega;}}_k^{\left(i\right)};$

计算有效粒子(如通过权重来确定有效粒子)数设定一个阈值δ，如根据经验值，按离子数的比例设置，可设为粒子总数的75％。若有效粒子数小于这个阈值，根据粒子权值对粒子X_k进行重采样，去除权值小的粒子，按照去除的权值小的粒子数量复制权值大的粒子，粒子总数保持不变； 

估计目标位置：基于上步采样完的粒子，计算粒子空间位置分布的均值，具体是由粒子的权值与位置坐标相乘，然后叠加得到的乘积，估计当前目标位置。 

步骤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，配置为对目标在活动范围内的移动通过预定数学模型进行分析。本例中采用非线性动态模型，即 $\left\{\begin{array}{c}{x}_t=f(x_{t-1},v_{t-1})\\ y_t=h(x_t,n_t)\end{array}\right..$

该模型主要是利用先验信息对观测值进行预测，在得到观测值后，利用该式得到关于参数的后验信息，从而修正先验信息，再以此修正后的信息作为先验信息进行预测，如此循环进行，不断得到观测值修正目标位置的先验信息(先前位置)。本发明中，x_t＝f(x_t-1，v_t-1)与步骤102中的运动模型相同，而y_t＝h(x_t，n_t)为粒子基于插值产生的地图来修正位置信息。 

其中，在地图更新模块203中，包括： 

插值模块2031，配置为例如用克里金法对周围的测量值进行加权以得出未测量位置的预测，使用如下公式由数据加权总和得到预测：  λ_i为第i个位置处的测量值的位置权重，S₀为预测位置，S_i为预测位置周围的随机点，Z(S_i)为位置S_i处的磁场值，N为测量值数量，本例中设为100。 

其中，数据点在网格中用坐标点S_i(a_i，b_i)i＝1，2，...，n表示；待插点坐标用S₀(a₀，b₀)表示，然后求散乱数据点之间的距离i＝1，2，...，n；j＝1，2，...，n；将计算所得的距离值按照从小到大排成顺序，然后将距离分成若干组，每组包含一定数量的距离值。将这些距离值组合成距离组(用{h′_m}表示)： 

m＝1，2，...，N_H，N_H表示距离组的个数，根据得出的{h′_m}，求出(i＝1，...，N(h))分布形状，其中，横坐标是h′_m，纵坐标是γ^*(h′_m)，需要选择合适的模型进行拟合，以产生连续的曲线 来表示γ^*(h′_m)的分布，本例中，采用的半变差拟合模型为圆形： 

其中，c₀为块金值，c为基台值，α为变程。进行函数拟合，求出模型参数(如c₀，c与α等)，从而得到变差函数γ^*(h)的表达式，然后再以方程组 $\left\{\begin{array}{c}\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}{\mathrm{\&gamma;}}^{*}(S_i-S_j){\mathrm{\&lambda;}}_i-\mathrm{\&mu;}={\mathrm{\&gamma;}}^{*}(S_0-S_j),(j=1,...,N)\\ \underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}{\mathrm{\&lambda;}}_i=1\end{array}\right.,$ 其中μ是Lagrange乘子，求解方程组可以求得：权值λ_i与Lagrange乘子μ，最后，根据 求出S₀(a₀，b₀)处的预测值

地图融合模块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个位置处的测量值的未知权重，S₀表示预测位置，N为测量值数量，所述权重λ_i取决于测量点、预测位置的距离和预测位置周围的测量值之间空间关系的拟合模型。

6.根据权利要求5所述的方法，其中，所述拟合模型为圆形，其变差函数表达式如下：

其中，C₀为块金值，C为基台值，α为变程。

7.根据权利要求2所述的方法，其中所述采用粒子滤波算法对随机移动的目标进行定位估计包括：

测量目标前一时刻到当前时刻移动的距离和偏移的角度，

设粒子初始为均匀分布，随着目标的移动，所述粒子按与所述目标相同的运动模型移动，基于非线性动态模型 $\left\{\begin{array}{c}{x}_k=f(x_{k-1},v_{k-1},w_k)\\ S_k=h(x_k,n_k)\end{array}\right.$ 通过粒子的位置均值来估计所述目标的位置。

8.根据权利要求7所述的方法，其中所述运动模型为：

$\left\{\begin{array}{c}<x_t,y_t>=\mathrm{MOVE}+<x_{t-1},y_{t-1}>+\sqrt{0.5}*\mathrm{randn}\left(\mathrm{1,2}\right)+0.01*\mathrm{wgn}\left(\mathrm{1,2,0.05}\right)\\ \mathrm{MOVE}=[-l_t*\sin \left({\mathrm{\&theta;}}_t\right),l_t*\cos \left({\mathrm{\&theta;}}_t\right)],0\&le;{\mathrm{\&theta;}}_t<90\\ \mathrm{MOVE}=[-l_t*\cos ({\mathrm{\&theta;}}_t-90),-l_t*\sin ({\mathrm{\&theta;}}_t-90)],90\&le;{\mathrm{\&theta;}}_t<180\\ \mathrm{MOVE}=[l*\cos (270-{\mathrm{\&theta;}}_t),-l_t*\sin (270-{\mathrm{\&theta;}}_t)],180\&le;{\mathrm{\&theta;}}_t<270\\ \mathrm{MOVE}=[l_t*\cos (360-{\mathrm{\&theta;}}_t),l_t*\sin (360-{\mathrm{\&theta;}}_t)],270\&le;{\mathrm{\&theta;}}_t<360\end{array}\right.$

其中，l_t为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所述的***，其中，所述权重计算模块采用的数据库是基于磁场的方向和强度由向量<H_x,H_y,H_z>表示的三个网格地图数据，各所述向量分别表示H_x,H_y,H_z的分布，所述三个网格地图的网格大小相同。

14.根据权利要求13所述的***，其中所述插值模块配置为在目标位置估计确定后，在地磁网格地图中抽取该位置处所述目标周围的预定范围的网格点数据，对所述测得的地磁场值进行插值，并且还包括：

融合模块，将所述插值得到的预定范围的网格点融合到预定范围的网格地图中。