目标即时定位和构建地图的方法与系统技术方案

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

【技术实现步骤摘要】
目标即时定位和构建地图的方法与系统
本专利技术涉及室内定位及地图构建领域，特别涉及将室内目标的定位和地图构建相结合的方法及系统。
技术介绍
同时定位与地图构建的问题是目标利用具有噪声的传感器数据，既要构建出环境中物体的分布情况，同时还要确定自身的位置。同时定位与地图构建的问题的解法可使目标实现真正的自主导航。在过去的十几年里，同时定位与地图构建的问题逐渐成为移动机器人领域的研究热点。通常的做法是利用EKF(ExtendedKalmanFilter：扩展卡尔曼滤波器)对机器人位置和地标进行同时估计。但是，当地标位置增多，机器人移动的范围变大时，该方法相应的计算量会变大，实时性变差。此外，该方法且需要事先对环境进行部署，布置好路标点，耗费人力，物力。
技术实现思路
针对上述现有技术的缺陷，本专利技术提供了目标定位与地图构建的方法，该方法采用运动模型，通过对当前地磁场的测量，对当前位置进行矫正，和利用空间插值算法对局部地图数据进行更新，并融合到全局地图，达到了目标定位和地图构建的同步进行，同时大大减小了运算量，也不需要事先对环境进行部署，节省资源。根据本专利技术的一个方面，提供一种目标定位与地图构建方法，包括：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时刻的相关误差，传感器本身以及周围的环境会使测量带有一定的误差，所以机器人想要依据这些不确切的控制信息来实现精确地定位是很难的，本专利技术中的误差为随机噪声和高斯白噪声的累加和。本专利技术中所使用的运动模型为xt,yt>=MOVE+xt-1,yt-1>+0.5*randn(1,2)本文档来自技高网...

【技术保护点】
目标即时定位与构建地图的方法，包括：1)对在活动范围内移动的目标进行即时定位；2)根据对所述移动目标的定位，即时更新目标所在活动范围的地磁场地图；3)根据所述更新的地磁场地图，对所述目标移动的下一位置进行定位；重复执行步骤2)和3)，直到目标停止移动。

【技术特征摘要】
1.目标即时定位与构建地图的方法，包括：1)对在活动范围内移动的目标进行即时定位；2)根据对所述移动目标的定位，即时更新目标所在活动范围的地磁场网格地图；3)根据所述更新的地磁场网格地图，对所述目标移动的下一位置进行定位；重复执行步骤2)和3)，直到目标停止移动。2.根据权利要求1所述的方法，其中所述对在活动范围内移动的目标进行即时定位包括：初始化目标在活动范围内的位置和活动范围内的地磁场网格地图，所述目标设置有测量目标运行的距离和转动的角度值和地磁场值的传感器；将目标初始位置的地磁场值通过空间插值法加入所述初始化的地磁场网格地图；测量所述目标运行的控制变量和所述目标当前位置的地磁场值；采用粒子滤波算法对随机移动的目标进行位置估计。3.根据权利要求2所述的方法，其中所述根据对所述移动目标的定位，即时更新目标所在活动范围的地磁场网格地图包括：在确定目标的估计位置后，在所述地磁场网格地图中抽取目标的估计位置点周围的第一预定范围的网格点数据以将所述测得的地磁场值插值，然后将所得到的第一预定范围网格点融合到第二预定范围的地磁场网格地图中。4.根据权利要求2所述的方法，其中，所述初始化目标在活动范围内的位置和活动范围内的地磁场网格地图包括：将目标在室内的活动范围抽象化为网格坐标，所述网格坐标的大小与室内的实际大小按比例设置。5.根据权利要求1-4任一项所述的方法，其中，通过克里金插值法将目标初始位置的地磁场值插入初始地磁场网格地图中，包括：对所述目标周围的测量值按如下方式进行加权以得出未测量位置的预测其中λi为第i个位置处的测量值的未知权重，S0表示预测位置，N为测量值数量，所述权重λi取决于测量点、预测位置的距离和预测位置周围的测量值之间空间关系的拟合模型。6.根据权利要求5所述的方法，其中，所述拟合模型为圆形，其变差函数表达式如下：其中，C0为块金值，C为基台值，α为变程。7.根据权利要求2所述的方法，其中所述采用粒子滤波算法对随机移动的目标进行定位估计包括：测量目标前一时刻到当前时刻移动的距离和偏移的角度，设粒子初始为均匀分布，随着目标的移动，所述粒子按与所述目标相同的运动模型移动，基于非线性动态模型其中，xk为k时刻的状态，vk-1为k-1时刻的控制输入量，wk为k-1时刻到k时刻的相关误差，Sk＝h(xk,nk)表示通过粒子的位置均值来估计所述目标的位置。8.根据权利要求7所述的方法，其中所述运动模型为：

【专利技术属性】
技术研发人员：王新珩，张聪聪，贾尚杰，
申请(专利权)人：无锡知谷网络科技有限公司，
类型：发明
国别省市：江苏;32