目标即时定位和构建地图的方法与系统的制作方法
【专利摘要】本申请公开了目标即时定位和构建地图的方法和装置。其中方法包括：1)对在活动范围内移动的目标进行即时定位；2)根据对所述移动目标的定位，即时更新目标所在活动范围的地磁场地图；3)根据所述更新的地磁场地图，对所述目标移动的下一位置进行定位；重复执行步骤2)和3)，直到目标停止移动。该方法可以解决室内目标在没有室内先验磁场地图的条件下，进行即时的目标定位和准确的地磁场地图构建。
【专利说明】目标即时定位和构建地图的方法与系统
【技术领域】
[0001]本发明涉及室内定位及地图构建领域，特别涉及将室内目标的定位和地图构建相结合的方法及系统。
【背景技术】
[0002]同时定位与地图构建的问题是目标利用具有噪声的传感器数据，既要构建出环境中物体的分布情况，同时还要确定自身的位置。同时定位与地图构建的问题的解法可使目标实现真正的自主导航。在过去的十几年里，同时定位与地图构建的问题逐渐成为移动机器人领域的研究热点。通常的做法是利用EKF(Extended Kalman Filter:扩展卡尔曼滤波器)对机器人位置和地标进行同时估计。但是，当地标位置增多，机器人移动的范围变大时，该方法相应的计算量会变大，实时性变差。此外，该方法且需要事先对环境进行部署，布置好路标点，耗费人力，物力。
【发明内容】
[0003]针对上述现有技术的缺陷，本发明提供了目标定位与地图构建的方法，该方法采用运动模型，通过对当前地磁场的测量，对当前位置进行矫正，和利用空间插值算法对局部地图数据进行更新，并融合到全局地图，达到了目标定位和地图构建的同步进行，同时大大减小了运算量，也不需要事先对环境进行部署，节省资源。
[0004]根据本发明的一个方面，提供一种目标定位与地图构建方法，包括:1)对在活动范围内移动的目标进行即时定位；2)根据对所述移动目标的定位，即时更新目标所在活动范围的地磁场地图；3)根据所述更新的地磁场地图，对所述目标移动的下一位置进行定位；重复执行步骤2)和3)，直到目标停止移动。
[0005]在上述方法中，步骤I)可包括:初始化目标在活动范围内的位置和活动范围内的全局地磁场地图，所述目标设置有测量目标运行的距离和转动的角度值和磁场值的传感器；将目标初始位置的地磁场值通过空间插值法加入所述初始化所述全局地磁场地图；测量所述目标运行的控制变量和所述目标当前位置的地磁场值；采用粒子滤波算法对随机移动的目标进行位置估计。
[0006]在一些实施方式中，所述初始化目标在活动范围内的位置和活动范围的地磁场地图的步骤包括:将目标活动的室内范围抽象化为网格坐标，将网格坐标的大小与活动室的实际大小按比例设置。目标在活动室内的初始位置是由目标的内部传感器确定的。
[0007]在一些实施方式中，通过空间插值法如克里金插值法将测得的地磁场值加入初始地图，对目标周围的测量值进行加权以得出未测量位置的预测，
^ (^0) = 'Σ.為ζ)，其中各参数的含义是:入i为第i个位置处的
/—I
测量值的未知权重，Stl是预测位置，N是测量值数目。使用克里金方法时，权重Xi取决于测量点、预测位置的距离和预测位置周围的测量值之间空间关系的拟合模型。[0008]在一些实施方式中，所述将地磁场值使用插值算法加入地图的步骤包括:目标位置估计确定后，在地磁网格地图中抽取位置目标点周围的第一预定范围的网格点数据以将测得的地磁场值插值，然后将插值后得到的所述第一预定范围的网格点融合到第二预定范围的网格地图中。
[0009]同时，本发明还提供了目标定位和地图更新系统，包括:定位模块和地图更新模块，其中所述定位模块对目标在活动范围内的移动进行即时定位；所述地图更新模块根据对目标的定位即时更新目标所在活动范围的地磁场地图，所述定位模块进一步根据所述更新的地磁场地图，对目标下一位置进行定位校正，直到目标停止移动。
[0010]在一些实施方式中，其中所述定位模块包括:初始化模块，配置为对目标初始位置和地磁场网格地图进行初始化；位置估计模块，配置为使用粒子滤波算法和运动学模型对目标初始位置和运动中的位置进行估计，所述地图更新模块包括:插值模块，配置为利用空间插值算法将测得的所述目标活动范围的地磁场值进行插值，和将局部地图融合到全局地图。
[0011 ] 所述位置估计模块可以基于粒子滤波实现，包括:粒子滤波算法模块，配置为对目标的位置估计进行校正；运动模型模块，配置为对目标在活动范围内的移动通过预定数学模型进行分析。
[0012]在一些实施方式中，在所述权重的确定中，采用与磁场的方向和强度向量<HX，Hy,Hz>相应的网格地图构成数据库，分别表示对应的向量分布。由此，可以增加地图的特征，有利于定位。
[0013]在一些实施方式中，所述构建地图模块中包括:插值模块，在目标位置估计确定后，在地磁网格地图中抽取位置目标点周围的预定范围的网格点数据将所测得的地磁场值插值，和融合模块，将插得的预定范围网格点融合到预定范围的网格地图中。
[0014]与现有技术相比，本发明的上述技术方案具有以下优点:由于采用对目标的即时定位和根据即时定位的结果更新磁场地图，又利用更新的磁场地图对目标移动进一步定位，将定位与地图更新结合来准确快速地对目标进行定位和构建连续一致的地图。
[0015]进一步，本发明人通过深入和广泛的研究，面对众多的算法，选择采用校正产生误差的粒子滤波算法对室内地磁场地图的更新，并采用空间协方差最佳插值方法-克里金算法插值，结果可信度高，这两种方法的结合能够更加准确快速地对机器人进行定位和构建连续一致的地图。
[0016]并且，在现在的大多数建筑物内，由于钢筋混泥土结构，家具等能影响磁场波动的物件存在，使室内磁场波动稳定存在，而本发明正是利用室内的磁场波动来实现定位，进而实现即时定位和构建地图。所以，本发明的实现不需要额外布置定位所需的参考点，由此可以减少花费。
【专利附图】
【附图说明】
[0017]图1为本发明一种实施方式中目标定位方法的步骤示意图；
[0018]图2为本发明一种实施方式中初始化目标在活动范围内的初始位置的示意图；
[0019]图3为本发明一种实施方式中活动范围的地磁场网格初始地图的示意图；
[0020]图4为本发明一种实施方式中将目标初始位置的地磁场值通过克里金插值法加入初始地图的示意图；
[0021]图5为本发明一种实施方式中对移动的目标进行位置矫正的示意图；
[0022]图6为本发明一种实施方式中测量目标当前位置的地磁场值示意图；
[0023]图7为本发明一种实施方式中将地磁场值使用插值算法加入局部地图的示意图；
[0024]图8为本发明一种实施方式中将局部地图融合到全局地图的示意图；
[0025]图9为本发明一种实施方式中地磁场的目标即时定位和地图构建系统的组成示意图。
【具体实施方式】
[0026]下面结合附图对发明作进一步详细的说明。
[0027]图1为本发明一种实施方式中的地磁场的即时目标定位和地图构建方法的步骤示意图。如图1所示，本发明一实施方式的目标即时定位和地图构建方法的步骤包括:
[0028]步骤SlOl:对待定位的目标所处的活动区域(本例中以一个活动室为例)进行实地测量，获得室内平面图，并对平面图进行模拟网格地图初始化。对待定位的目标，结合目标内部传感器的数据(Ii, θ i)其中，Ii为第i次移动的距离，Θ i为第i次移动的偏向角。确定其在模拟的初始化网格地图中的位置。图2中A所指位置是目标的初始位置，旁边的黑点是粒子滤波算法中的重要性采样算法对位置空间的采样点，这些采样点的均值表示对目标位置的估计。
[0029]步骤S102:若目标处于初始位置，则粒子均匀的分布在A的周围。A的位置是由目标的内部传感器测得的。随着目标的移动，基于运动模型Xk = f (xk-1? Vk_1； wk),即通过xk_i和控制输入量Vlri能求xk，其中，Xk为k时刻的状态，Vk^1为k_l时刻的控制输入量(即目标运行的距离和转动的角度)，wk为k-Ι时刻到k时刻的相关误差，传感器本身以及周围的环境会使测量带有一定的误差，所以机器人想要依据这些不确切的控制信息来实现精确地定位是很难的，本发明中的误差为随机噪声和高斯白噪声的累加和。本发明中所使用的运动模型为
【权利要求】
1.目标即时定位与构建地图的方法，包括: 1)对在活动范围内移动的目标进行即时定位； 2)根据对所述移动目标的定位，即时更新目标所在活动范围的地磁场地图； 3)根据所述更新的地磁场地图，对所述目标移动的下一位置进行定位； 重复执行步骤2)和3)，直到目标停止移动。
2.根据权利要求1所述的方法，其中所述对目标在活动范围内的移动进行即时定位包括: 初始化目标在活动范围内的位置和活动范围内的全局地磁场地图，所述目标设置有测量目标运行的距离和 转动的角度值和磁场值的传感器； 将目标初始位置的地磁场值通过空间插值法加入所述初始化所述全局地磁场地图； 测量所述目标运行的控制变量和所述目标当前位置的地磁场值； 采用粒子滤波算法对随机移动的目标进行位置估计。
3.根据权利要求2所述的方法，其中所述根据对目标的定位即时更新目标所在活动范围的地磁场地图包括: 在对目标的位置估计确定后，在所述地磁网格地图中抽取位置目标点周围的预定范围的网格点数据将所述测得的地磁场值插值，然后将所得到的预定范围网格点融合到预定范围的网格地图中。
4.根据权利要求1所述的方法，其中，所述初始化目标在活动范围内的位置和活动范围的地磁场地图包括: 将目标活动的室内范围抽象化为网格坐标，所述网格坐标的大小与活动室的实际大小按比例设置。
5.根据权利要求1-4任一项所述的方法，其中，通过克里金插值法将目标初始位置的地磁场值插入初始地磁场地图中，包括: 对所述目标周围的测量值按如下方式进行加权以得出未测量位置的预测
6.根据权利要求5所述的方法，其中，所述拟合模型为圆形，其变差函数表达式如下:
7.根据权利要求2所述的方法，其中所述采用粒子滤波算法对随机移动的目标进行定位估计包括: 测量目标前一时刻到当前时刻移动的距离和偏移的角度，设粒子初始为均匀分布，随着目标的移动，所述粒子按与所述目标相同的运动模型移动，基于非线性动态模型
8.根据权利要求7所述的方法，其中所述运动模型为:
9.目标即时定位和地图构建系统，包括定位模块和地图更新模块，其中 所述定位模块对目标在活动范围内的移动进行即时定位； 所述地图更新模块根据对目标的定位即时更新目标所在活动范围的地磁场地图，所述定位模块进一步根据所述更新的地磁场地图，对目标的下一移动位置进行定位，直到目标停止移动。
10.根据权利要求9所述的系统，其中 所述定位模块包括: 初始化模块，配置为对目标初始位置和地磁场网格地图进行初始化； 位置估计模块，配置为使用粒子滤波算法和运动学模型对目标初始位置和运动中的位置进行估计， 所述地图更新模块包括: 插值模块，配置为利用空间插值算法将测得的所述目标活动范围的地磁场值插值，和 地图融合模块，将局部地图融合到全局地图。
11.根据权利要求10所述的系统，其中 所述位置估计模块包括: 粒子滤波算法模块，配置为对目标的位置估计进行校正； 运动模型单元，配置为对目标在活动范围内的移动通过预定数学模型进行分析。
12.根据权利要求11所述的系统，其中 所述粒子滤波算法模块包括: 初始化粒子分布模块，配置为后面粒子集的移动提供原始的参考； 重要性重采样模块，配置为产生基于目标移动过程中的粒子估计集合； 权重计算模块，配置为对所述粒子赋予相关权重大小，其中与目标特性越接近则权重越大，反之越小； 归一化权重模块，配置为对所述粒子的权重进行归一化处理；有效粒子数确定模块，配置为找出与目标特性接近的粒子数；和 位置估计校正模块，配置为对目标移动位置的估计进行校正。
13.根据权利要求12所述的系统，其中，所述权重计算模块采用的数据库是基于磁场的方向和强度由向量<HX，Hy, Hz>表示的三个网格地图数据，各所述向量分别表示Hx，Hy, Hz的分布，所述三个网格地图的网格大小相同。
14.根据权利要求13所述的系统，其中所述插值模块配置为在目标位置估计确定后，在地磁网格地图中抽取该位置处所述目标周围的预定范围的网格点数据，对所述测得的地磁场值进行插值，并且还包括: 融合模块，将所述插值得到的预定范围的网格点融合到预定范围的网格地图中。
【文档编号】G01C21/08GK104019813SQ201410277471
【公开日】2014年9月3日 申请日期:2014年6月19日 优先权日:2014年6月19日
【发明者】王新珩, 张聪聪, 贾尚杰 申请人:无锡知谷网络科技有限公司