CN117029817A - 一种二维栅格地图融合方法及系统 - Google Patents
一种二维栅格地图融合方法及系统 Download PDFInfo
- Publication number
- CN117029817A CN117029817A CN202310773037.7A CN202310773037A CN117029817A CN 117029817 A CN117029817 A CN 117029817A CN 202310773037 A CN202310773037 A CN 202310773037A CN 117029817 A CN117029817 A CN 117029817A
- Authority
- CN
- China
- Prior art keywords
- grid
- robot
- image
- map
- particle
- 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.)
- Pending
Links
- 238000007500 overflow downdraw method Methods 0.000 title claims abstract description 13
- 239000011159 matrix material Substances 0.000 claims abstract description 27
- 238000000034 method Methods 0.000 claims abstract description 27
- 238000013507 mapping Methods 0.000 claims abstract description 15
- 230000008569 process Effects 0.000 claims abstract description 15
- 230000009466 transformation Effects 0.000 claims abstract description 15
- 230000004927 fusion Effects 0.000 claims abstract description 12
- 238000000605 extraction Methods 0.000 claims abstract description 8
- 239000002245 particle Substances 0.000 claims description 132
- 238000005259 measurement Methods 0.000 claims description 36
- 238000012952 Resampling Methods 0.000 claims description 31
- 238000009826 distribution Methods 0.000 claims description 13
- 230000007774 longterm Effects 0.000 claims description 12
- 238000004364 calculation method Methods 0.000 claims description 9
- 238000010276 construction Methods 0.000 claims description 8
- 230000004807 localization Effects 0.000 claims description 7
- 238000005286 illumination Methods 0.000 claims description 6
- 230000008447 perception Effects 0.000 claims description 6
- 238000005070 sampling Methods 0.000 claims description 6
- 230000003044 adaptive effect Effects 0.000 claims description 3
- 241000282472 Canis lupus familiaris Species 0.000 description 10
- 238000010586 diagram Methods 0.000 description 3
- 238000009825 accumulation Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000006870 function Effects 0.000 description 1
- 239000002920 hazardous waste Substances 0.000 description 1
- 239000013049 sediment Substances 0.000 description 1
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/20—Instruments for performing navigational calculations
- G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation
-
- 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/38—Electronic maps specially adapted for navigation; Updating thereof
- G01C21/3804—Creation or updating of map data
- G01C21/3807—Creation or updating of map data characterised by the type of data
- G01C21/383—Indoor data
-
- 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/38—Electronic maps specially adapted for navigation; Updating thereof
- G01C21/3804—Creation or updating of map data
- G01C21/3833—Creation or updating of map data characterised by the source of data
- G01C21/3837—Data obtained from a single source
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明公开了一种二维栅格地图融合方法及系统，构建地图，分别在不同区域的移动机器人，使用其搭载的激光雷达对当前的未知环境进行建图；每个移动机器人建立当前环境的局部栅格地图；移动机器人定位，在移动机器人移动过程中，采用自适应蒙特卡罗定位算法，实现机器人相对世界坐标系的位姿；移动机器人在定位建图结束后，得到两幅栅格地图；提取特征点，并创建创建描述符；特征点提取采用尺度不变特征变换用于合并的局部地图；在两幅栅格地图的图像之间进行描述符的相互匹配，计算两个图像之间的变换或单应性矩阵；将根据单应性矩阵计算的变换应用于图像或图，然后相应地合并两个图。本发明快速可靠且地图融合时间短。
Description
技术领域
本发明涉及机器人领域的地图融合方法，尤其涉及一种二维栅格地图融合方法及系统。
背景技术
从机器人学的角度来看，地图允许机器人确定他们周围的世界是如何出现的。机器人可以凭借地图完成分配的任务并发现新的点。获取机器人可以执行其指定任务的地图之外，构建地图的持续时间在某些情况下也起着至关重要的作用，例如在灾难情况下的搜索和救援操作。这是因为，寻找被困的受害者或应用干预策略大多基于灾区当前的地图；因此，获取地图的持续时间必须尽可能快。在合成孔径雷达领域，一个常见的干预是由人类或训练有素的狗来完成的，以获得快速的效果。然而，当该地区含有核沉降物等危险废物时，这可能是危险的。因此，在这种情况下，机器人可以作为人类或训练有素的狗的替代品。很明显，在大规模区域内使用单个机器人进行干预是非常耗时的。因此，它一般是用一队机器人来完成的。因此，当使用机器人团队而不是单个团队时，映射和干预时间可以减少。然而，多机器人团队中最具挑战性的问题之一是每个机器人提供的部分地图的组合。
用于在多机器人同时定位和映射过程中生成室内环境的全局二维地图，在SLAM过程中，为了在这种环境中找到自己的路，机器人应该能够确定自己相对于由其观测形成的地图的位置。为了解决这一复杂问题，需要同时进行定位和映射的方法。在大型复杂的环境中，单个机器人由于误差积累和所需的时间而不是合理的。因此在这项任务中使用多个机器人并行的趋势，但多机器人SLAM面临的挑战之一是地图合并问题。
因此，亟待解决上述问题。
发明内容
发明目的：本发明的第一目的提供一种快速可靠的二维栅格地图融合方法。
本发明的第二目的是提供一种二维栅格地图融合系统。
技术方案：为实现以上目的，本发明公开了一种二维栅格地图融合方法，包括如下步骤：
(1)构建地图，分别在不同区域的移动机器人，使用其搭载的激光雷达对当前的未知环境进行建图；完成参数配置并运行Gmapping节点，每个移动机器人建立当前环境的局部栅格地图；
(2)移动机器人定位，在移动机器人移动过程中，采用基于KL距离KLD算法自适应调整粒子样本数量的自适应蒙特卡罗定位AMCL算法，实现机器人相对世界坐标系的位姿；移动机器人在定位建图结束后，得到两幅栅格地图；
(3)提取特征点，并创建创建描述符；特征点提取采用尺度不变特征变换SIFT用于合并的局部地图，SIFT是寻找对缩放、旋转和光照不变的关键点或感兴趣点；
(4)在两幅栅格地图的图像之间进行描述符的相互匹配，需要三对匹配特征来计算两个图像之间的变换或单应性矩阵；
(5)对于多于三个对匹配特征，采用RANSAC算法可以用于计算单应性矩阵；
(6)将根据单应性矩阵计算的变换应用于图像或图，然后相应地合并两个图。
其中，步骤(1)中局部栅格地图的建立包括以下步骤：
(1.1)在栅格地图中，设栅格地图中的点Occupied状态的概率为p(s＝0)，Free状态的概率为p(s＝1)，p(s＝0)+p(s＝1)＝1，引入中间值表示栅格地图中点的状态；
(1.2)对于栅格地图中一个点，当新测得一个测量值z～{0，1}，通过用公式更新状态odds(s)；式中p(s＝1|z)表示测量值z发生的情况下s＝1发生的概率，p(s＝0|z)表示测量值z发生的情况下s＝0发生的概率；根据贝叶斯公式：
带入之后：
两边取对数可得：
测量值的模型为的选项只有lofree和looccu，激光雷达的对一个栅格观测结果只有两种：
占据和空闲
用logOdd(s)来表示栅格s的状态S的话，更新规则为：
其中，S+和S-分别表示测量值之后和之前栅格的状态；
一个栅格的初始状态Sinit为：默认栅格空闲和栅格占用的概率都为0.5；
更新一个栅格的状态即为：
S+＝S-+lofree或S+＝S-+looccu
(1.3)假设lofree和looccu为确定的数值，即一正一负；通过激光雷达数据栅格进行判断：如果判定栅格是空闲，就执行S+＝S-+lofree；如果判定栅格是占据，就执行S+＝S-+looccu；在经过许多帧激光雷达数据的洗礼后，每一个栅格都存储了一个值，用设定阈值与栅格存储的值进行比较，来做栅格最终状态的判定。
优选的，步骤(2)具体包括如下步骤：
(2.1)建立地图：使用步骤(1)中生成的当前环境地图；
(2.2)初始化粒子：将机器人的初始位置作为粒子集合的中心，随机生成一定数量的粒子，每个粒子的位置和方向都是随机的；
(2.3)运动模型：根据机器人的运动数据，更新粒子的位置和方向；
二维地图上的定位包括状态空间中机器人位置(x，y)和朝向角θ，MCL算法根据现有的数据估计机器人位姿的后验信息，可用的数据有两种类型，分别为测量数据和里程数据；采用里程计运动模型表示机器人运动p(xi|xt-1，ut)，采用激光的似然域模型表示机器人感知p(zi|xt)，置信度ηbel(xi|z1：t,u1：t)表示在t时刻综合所有过去的测量数据z1：t和控制数据u1：t的有关位姿xt的后验概率，表达式为：
ηbel(xi|z1：t，u1：t)∝p(zi|xt)·∫[p(xi|xt-1，ut)·p(xt-1|z1：t-1，u1：t-1)]dxt-1
式中：表示机器人在t时刻的估计位姿，当接收到里程信息时，通过运动模型采样更新xt；表示粒子的权重，当观察到新的测量数据时，根据测量模型给粒子加权；MCL通过一组加权的粒子表示后验，然而一旦位姿变化不连续，定位就会失败；为了提高定位精度，需要加入大量的粒子，这使得定位收敛速度变慢；
(2.4)计算粒子权重：根据移动机器人传感器的测量数据，计算每个粒子的权重；
AMCL算法是在MCL的基础上改进而来的，根据长期估计权重wslow和短期估计权重wfast判断机器人是否被绑架，若wfast劣于wslow，通过在重采样中增加随机粒子使机器人从绑架中恢复，表达式为
式中，wavg表示所有粒子的平均权重，参数αslow和αfast分别为长期估计和短期估计时平均的指数滤波器的衰减率(0≤αslow≤αfast)；
(2.5)KLD重采样：对粒子集合进行重采样，重采样后的粒子集合更加接近；
AMCl重采样阶段采用KLD算法自适应调整粒子数量，粒子数量可表示为
式中，ε表示真实分布与估计分布之间的误差，z1-δ表示标准正态分布上的1-δ分位点，k表示直方图的非空位，粒子数量上限Mtop与非空位k呈线性关系；在全局定位的初始阶段，需要非常多的样本，k越大，Mtop越高；当全局定位完成后，该问题转化为位置跟踪问题，粒子集中在真实位置附近，k变得很少，Mtop降低；
(2.6)更新机器人位置：根据KLD重采样后的粒子集合，计算机器人在地图上的位置和方向。
再者，步骤(3)具体包括如下步骤：
(3.1)为处理后的图像构造一个尺度空间，是指先拍摄原始图像，然后从原始图像中逐步生成模糊图像；然后将原始图像调整到一半大小，再次生成模糊图像，不断重复；在数学上，通过将高斯算子卷积到图像像素来生成模糊图像：
L(x，y；σ)＝G(x，y，σ)*I(x，y)
其中I(x，y)和L分别是图像和模糊图像，G(x，y，σ)是高斯算子，x是y局部坐标，并且σ是比例参数，*表示卷积运算，高斯算子定义如下：
(3.2)计算高斯差分DoG，根据两个连续量表的差值计算计算高斯差分D(x，y；σ)，如下所示：
D(x，y；σ)＝(G(x，y，kσ)-G(x，y，σ))*I(x，y)＝L(x，y，kσ)-L(x，y，σ)
(3.3)找到图像的关键点，首先在DoG图像中定位近似的局部最大值/最小值，为了找到近似的局部极值，将每个像素与其26个相邻像素进行比较；
如果像素是其所有26个相邻像素中最大或最小的，则将其标记为“关键点”；通常，极值位于像素之间，并且在像素上不完全匹配；在这种情况下，极值仅是“近似的”，并且必须在数学上执行子像素极值搜索；
使用D(x，y；σ)的泰勒展开，子像素极值计算如下：
其中x＝[x，y；σ]T，并且在最后一个近似极值处计算D的导数；使用亚像素极值而不是近似极值作为关键点将增加匹配的几率和SIFT算法的稳定性；
使用图像中的关键点及其比例，每个关键点的尺度与其对应的模糊图像的尺度相同，使得它们具有尺度不变性；
(3.4)为每个关键点分配方向度量以提供方向不变性，
为了将方向分配给关键点，关键点周围的所有像素的梯度幅度和方向计算如下：
θ(x，y)＝tan-1((L(x，y+1)-L(x，y-1))/(L(x+1，y)-L(x-1，y)))
使用计算得到的幅度和方向创建具有36个箱每个10度的直方图，直方图的数量与幅度成正比，所创建的直方图将在某些点处具有峰值，对应的仓指示可分配给关键点的方向，实现方向不变性；
(3.5)为每个检测到的特征或关键点创建描述符，在每个关键点周围考虑划分为4*4窗口的16*16网格，在每个4*4窗口内计算梯度幅度和方向；
根据方位测量结果，生成8位直方图，每个仓中的量值根据离关键点的距离来考虑，离关键点较远的像素对直方图的贡献较小，将此应用于16个4*4窗口，将为每个关键点创建一个4*4*8数组，将4*4*8数组归一化，创建特征向量或特征描述符。
进一步，步骤(4)中描述符的相互匹配具体包括如下步骤：选择来自第一图像的第一特征，并计算其描述符到第二图像的所有特征描述符的距离，第二图像中的匹配特征与所选特征具有最小的“距离”，对于第一图像的所有特征重复此过程。
本发明公开了一种二维栅格地图融合系统，包括：
构建地图模块，用于构建地图，分别在不同区域的移动机器人，使用其搭载的激光雷达对当前的未知环境进行建图；完成参数配置并运行Gmapping节点，每个移动机器人建立当前环境的局部栅格地图；
定位建图模块，用于移动机器人定位，在移动机器人移动过程中，采用基于KL距离KLD算法自适应调整粒子样本数量的自适应蒙特卡罗定位AMCL算法，实现机器人相对世界坐标系的位姿；移动机器人在定位建图结束后，得到两幅栅格地图；
创建描述符模块，用于提取特征点，并创建描述符；特征点提取采用尺度不变特征变换SIFT用于合并的局部地图，SIFT是寻找对缩放、旋转和光照不变的关键点或感兴趣点；
第一矩阵计算模块，用于在两幅栅格地图的图像之间进行描述符的相互匹配，需要三对匹配特征来计算两个图像之间的变换或单应性矩阵；
第二矩阵计算模块，用于对于多于三个对匹配特征，采用RANSAC算法可以用于计算单应性矩阵；
图像合并模块，用于将根据单应性矩阵计算的变换应用于图像或图，然后相应地合并两个图。
其中，构建地图模块中局部栅格地图的建立具体执行步骤为：
在栅格地图中，设栅格地图中的点Occupied状态的概率为p(s＝0)，Free状态的概率为p(s＝1)，p(s＝0)+p(s＝1)＝1，引入中间值表示栅格地图中点的状态；
对于栅格地图中一个点，当新测得一个测量值z～{0，1}，通过用公式 更新状态odds(s)；式中p(s＝1|z)表示测量值z发生的情况下s＝1发生的概率，p(s＝0|z)表示测量值z发生的情况下s＝0发生的概率；根据贝叶斯公式：
带入之后：
两边取对数可得：
测量值的模型为的选项只有lofree和looccu，激光雷达的对一个栅格观测结果只有两种：
占据和空闲
用logOdd(s)来表示栅格s的状态S的话，更新规则为：
其中，S+和S-分别表示测量值之后和之前栅格的状态；
一个栅格的初始状态Sinit为：默认栅格空闲和栅格占用的概率都为0.5；
更新一个栅格的状态即为：
S+＝S-+lofree或S+＝S-+looccu
假设lofree和looccu为确定的数值，即一正一负；通过激光雷达数据栅格进行判断：如果判定栅格是空闲，就执行S+＝S-+lofree；如果判定栅格是占据，就执行S+＝S-+looccu；在经过许多帧激光雷达数据的洗礼后，每一个栅格都存储了一个值，用设定阈值与栅格存储的值进行比较，来做栅格最终状态的判定。
优选的，定位建图模块具体执行步骤为：
建立地图：使用构建地图模块中生成的当前环境地图；
初始化粒子：将机器人的初始位置作为粒子集合的中心，随机生成一定数量的粒子，每个粒子的位置和方向都是随机的；
运动模型：根据机器人的运动数据，更新粒子的位置和方向；
二维地图上的定位包括状态空间中机器人位置(x，y)和朝向角θ，MCL算法根据现有的数据估计机器人位姿的后验信息，可用的数据有两种类型，分别为测量数据和里程数据；采用里程计运动模型表示机器人运动p(xi|xt-1，ut)，采用激光的似然域模型表示机器人感知p(zi|xt)，置信度ηbel(xi|z1：t,u1：t)表示在t时刻综合所有过去的测量数据z1：t和控制数据u1：t的有关位姿xt的后验概率，表达式为：
ηbel(xi|z1：t，u1：t)∝p(zi|xt)·∫[p(xi|xt-1，ut)·p(xt-1|z1：t-1，u1：t-1)]dxt-1
式中：表示机器人在t时刻的估计位姿，当接收到里程信息时，通过运动模型采样更新xt；表示粒子的权重，当观察到新的测量数据时，根据测量模型给粒子加权；MCL通过一组加权的粒子表示后验，然而一旦位姿变化不连续，定位就会失败；为了提高定位精度，需要加入大量的粒子，这使得定位收敛速度变慢；
计算粒子权重：根据移动机器人传感器的测量数据，计算每个粒子的权重；
AMCL算法是在MCL的基础上改进而来的，根据长期估计权重wslow和短期估计权重wfast判断机器人是否被绑架，若wfast劣于wslow，通过在重采样中增加随机粒子使机器人从绑架中恢复，表达式为
式中，wavg表示所有粒子的平均权重，参数αslow和αfast分别为长期估计和短期估计时平均的指数滤波器的衰减率(0≤αslow≤αfast)；
KLD重采样：对粒子集合进行重采样，重采样后的粒子集合更加接近；
AMCl重采样阶段采用KLD算法自适应调整粒子数量，粒子数量可表示为
式中，ε表示真实分布与估计分布之间的误差，z1-δ表示标准正态分布上的1-δ分位点，k表示直方图的非空位，粒子数量上限Mtop与非空位k呈线性关系；在全局定位的初始阶段，需要非常多的样本，k越大，Mtop越高；当全局定位完成后，该问题转化为位置跟踪问题，粒子集中在真实位置附近，k变得很少，Mtop降低；
更新机器人位置：根据KLD重采样后的粒子集合，计算机器人在地图上的位置和方向。
再者，创建描述符模块具体执行步骤为：
为处理后的图像构造一个尺度空间，是指先拍摄原始图像，然后从原始图像中逐步生成模糊图像；然后将原始图像调整到一半大小，再次生成模糊图像，不断重复；在数学上，通过将高斯算子卷积到图像像素来生成模糊图像：
L(x，y；σ)＝G(x，y，σ)*I(x，y)
其中I(x，y)和L分别是图像和模糊图像，G(x，y，σ)是高斯算子，x是y局部坐标，并且σ是比例参数，*表示卷积运算，高斯算子定义如下：
计算高斯差分DoG，根据两个连续量表的差值计算计算高斯差分D(x，y；σ)，如下所示：
D(x，y；σ)＝(G(x，y，kσ)-G(x，y，σ))*I(x，y)＝L(x，y，kσ)-L(x，y，σ)
找到图像的关键点，首先在DoG图像中定位近似的局部最大值/最小值，为了找到近似的局部极值，将每个像素与其26个相邻像素进行比较；
如果像素是其所有26个相邻像素中最大或最小的，则将其标记为“关键点”；通常，极值位于像素之间，并且在像素上不完全匹配；在这种情况下，极值仅是“近似的”，并且必须在数学上执行子像素极值搜索；
使用D(x，y；σ)的泰勒展开，子像素极值计算如下：
其中x＝[x，y；σ]T，并且在最后一个近似极值处计算D的导数；使用亚像素极值而不是近似极值作为关键点将增加匹配的几率和SIFT算法的稳定性；
使用图像中的关键点及其比例，每个关键点的尺度与其对应的模糊图像的尺度相同，使得它们具有尺度不变性；
为每个关键点分配方向度量以提供方向不变性，
为了将方向分配给关键点，关键点周围的所有像素的梯度幅度和方向计算如下：
θ(x，y)＝tan-1((L(x，y+1)-L(x，y-1))/(L(x+1，y)-L(x-1，y)))
使用计算得到的幅度和方向创建具有36个箱每个10度的直方图，直方图的数量与幅度成正比，所创建的直方图将在某些点处具有峰值，对应的仓指示可分配给关键点的方向，实现方向不变性；
为每个检测到的特征或关键点创建描述符，在每个关键点周围考虑划分为4*4窗口的16*16网格，在每个4*4窗口内计算梯度幅度和方向；
根据方位测量结果，生成8位直方图，每个仓中的量值根据离关键点的距离来考虑，离关键点较远的像素对直方图的贡献较小，将此应用于16个4*4窗口，将为每个关键点创建一个4*4*8数组，将4*4*8数组归一化，创建特征向量或特征描述符。
进一步，第一矩阵计算模块中描述符的相互匹配具体执行步骤为：选择来自第一图像的第一特征，并计算其描述符到第二图像的所有特征描述符的距离，第二图像中的匹配特征与所选特征具有最小的“距离”，对于第一图像的所有特征重复此过程。
有益效果：与现有技术相比，本发明具有以下显著优点：本发明使用多移动机器人共同探索并建立环境地图，提高了工作效率，节点式的分布方式，以多台较低性能的机器人，完成较复杂的任务，节省任务成本，当一台机器人出现故障时，其余机器人任能继续完成任务；本发明采用栅格地图作为环境表达方式，该地图类型易于保存和维护，且搭配激光雷达传感器较为稳定；本发明采用了一种图像特征提取算法SIFT，并提出了一种快速可靠的地图合并算法；利用SIFT将地面机器人提供的局部地图转换成图像，并提取图像的特征及其描述符，这些特征是不变的方向和缩放；然后识别出两幅图像之间的匹配特征，最后利用RANSAC计算单应矩阵；不仅地图融合的时间短，在低重叠区域进行也不会失效。
附图说明
图1为本发明的流程示意图；
图2为本发明中使用的主要设备图；
图3为本发明的整个系统框架图；
图4为本发明的移动机器人定位流程图；
图5为本发明的机器人R1建立的局部栅格地图；
图6为本发明的机器人R2建立的局部栅格地图；
图7为本发明的选择关键点周围的像素以创建其描述符图；
图8为本发明的融合后的地图。
具体实施方式
下面结合附图对本发明的技术方案作进一步说明。
实施例1
如图2和图3所示，本发明使用的实验设备为若干辆两轮的差速移动机器人，其上搭载了激光雷达，支持ROS操作系统的PC电脑。
如图1所示，本发明一种二维栅格地图融合方法，包括如下步骤：
(1)构建地图，分别在不同区域的移动机器人，使用其搭载的激光雷达对当前的未知环境进行建图；完成参数配置并运行Gmapping节点，每个移动机器人建立当前环境的局部栅格地图；Gmapping是一个基于2D激光雷达使用RBPF算法完成二维栅格地图构建的SLAM算法。Gmapping功能包订阅机器人的深度信息、IMU信息和里程计信息，同时完成一些必要参数的配置，即可创建并输出基于概率的二维栅格地图。
其中局部栅格地图的建立包括以下步骤：
(1.1)在栅格地图中，设栅格地图中的点Occupied状态的概率为p(s＝0)，Free状态的概率为p(s＝1)，p(s＝0)+p(s＝1)＝1，引入中间值表示栅格地图中点的状态；
(1.2)对于栅格地图中一个点，当新测得一个测量值z～{0，1}，通过用公式更新状态odds(s)；式中p(s＝1|z)表示测量值z发生的情况下s＝1发生的概率，p(s＝0|z)表示测量值z发生的情况下s＝0发生的概率；根据贝叶斯公式：
带入之后：
两边取对数可得：
测量值的模型为的选项只有lofree和looccu，激光雷达的对一个栅格观测结果只有两种：
占据和空闲
用logOdd(s)来表示栅格s的状态S的话，更新规则为：
其中，S+和S-分别表示测量值之后和之前栅格的状态；
一个栅格的初始状态Sinit为：默认栅格空闲和栅格占用的概率都为0.5；
更新一个栅格的状态即为：
S+＝S-+lofree或S+＝S-+looccu
(1.3)假设lofree和looccu为确定的数值，一般情况下一正一负；通过激光雷达数据栅格进行判断：如果判定栅格是空闲，就执行S+＝S-+lofree；如果判定栅格是占据，就执行S+＝S-+looccu；在经过许多帧激光雷达数据的洗礼后，每一个栅格都存储了一个值，用设定阈值与栅格存储的值进行比较，来做栅格最终状态的判定；某一个机器人随机走过一条路径后，就可以测量出这条路径周边的栅格哪些是有障碍物的，哪些是无障碍物的。该机器人会把这些数据保存到自己的数据库中，通过数据库的共享把地图数据同步给其他机器人。
(2)移动机器人定位，在移动机器人移动过程中，采用基于KL距离KLD算法自适应调整粒子样本数量的自适应蒙特卡罗定位AMCL算法，实现机器人相对世界坐标系的位姿；
如图4所示，
(2.1)建立地图：使用步骤(1)中生成的当前环境地图；
(2.2)初始化粒子：将机器人的初始位置作为粒子集合的中心，随机生成一定数量的粒子，每个粒子的位置和方向都是随机的；
(2.3)运动模型：根据机器人的运动数据，更新粒子的位置和方向；运动模型可以是简单的模型，如直线运动或旋转运动，也可以是复杂的模型，如运动学模型或动力学模型。
二维地图上的定位包括状态空间中机器人位置(x，y)和朝向角θ，MCL算法根据现有的数据估计机器人位姿的后验信息，可用的数据有两种类型，分别为测量数据和里程数据；采用里程计运动模型表示机器人运动p(xi|xt-1，ut)，采用激光的似然域模型表示机器人感知p(zi|xt)，置信度ηbel(xi|z1：t,u1：t)表示在t时刻综合所有过去的测量数据z1：t和控制数据u1：t的有关位姿xt的后验概率，表达式为：
ηbel(xi|z1：t，u1：t)∝p(zi|xt)·∫[p(xi|xt-1，ut)·p(xt-1|z1：t-1，u1：t-1)]dxt-1
式中：表示机器人在t时刻的估计位姿，当接收到里程信息时，通过运动模型采样更新xt；表示粒子的权重，当观察到新的测量数据时，根据测量模型给粒子加权；MCL通过一组加权的粒子表示后验，然而MCL算法不能解决机器人绑架问题，一旦位姿变化不连续，定位就会失败；为了提高定位精度，需要加入大量的粒子，这使得定位收敛速度变慢；
(2.4)计算粒子权重：根据移动机器人传感器的测量数据，计算每个粒子的权重；
AMCL算法是在MCL的基础上改进而来的，根据长期估计权重wslow和短期估计权重wfast判断机器人是否被绑架，若wfast劣于wslow，通过在重采样中增加随机粒子使机器人从绑架中恢复，表达式为
式中，wavg表示所有粒子的平均权重，参数αslow和αfast分别为长期估计和短期估计时平均的指数滤波器的衰减率(0≤αslow≤αfast)；
(2.5)KLD重采样：为了控制上述粒子数冗余而设计的；比如在栅格地图中，看粒子占了多少栅格；占得多，说明粒子很分散，在每次迭代重采样的时候，允许粒子数量的上限高一些；占得少，说明粒子都已经集中了，那就将上限设低，采样到这个数就行了。对粒子集合进行重采样，重采样后的粒子集合更加接近。
AMCl重采样阶段采用KLD算法自适应调整粒子数量，粒子数量可表示为
式中，ε表示真实分布与估计分布之间的误差，z1-δ表示标准正态分布上的1-δ分位点，k表示直方图的非空位，粒子数量上限Mtop与非空位k呈线性关系；在全局定位的初始阶段，需要非常多的样本，k越大，Mtop越高；当全局定位完成后，该问题转化为位置跟踪问题，粒子集中在真实位置附近，k变得很少，Mtop降低；通过这种方式，动态调整粒子的数量，从而提高了算法计算效率。
(2.6)更新机器人位置：根据KLD重采样后的粒子集合，计算机器人在地图上的位置和方向；机器人R1和R2在定位建图结束后，得到机器人R1和机器人R2的两张栅格地图，如图5和图6所示。
(3)特征点提取采用尺度不变特征变换SIFT用于合并的局部地图，SIFT的主要思想是寻找对缩放、旋转和光照不变的关键点或感兴趣点；
(3.1)为处理后的图像构造一个尺度空间，是指先拍摄原始图像，然后从原始图像中逐步生成模糊图像；然后将原始图像调整到一半大小，再次生成模糊图像，不断重复；在数学上，通过将高斯算子卷积到图像像素来生成模糊图像：
L(x，y；σ)＝G(x，y，σ)*I(x，y)
其中I(x，y)和L分别是图像和模糊图像，G(x，y，σ)是高斯算子，x是y局部坐标，并且σ是比例参数，*表示卷积运算，高斯算子定义如下：
(3.2)计算高斯差分DoG，根据两个连续量表的差值计算计算高斯差分D(x，y；σ)，如下所示：
D(x，y；σ)＝(G(x，y，kσ)-G(x，y，σ))*I(x，y)＝L(x，y，kσ)-L(x，y，σ)
(3.3)找到图像的关键点，首先在DoG图像中定位近似的局部最大值/最小值，为了找到近似的局部极值，将每个像素与其26个相邻像素进行比较；
如果像素是其所有26个相邻像素中最大或最小的，则将其标记为“关键点”；通常，极值位于像素之间，并且在像素上不完全匹配；在这种情况下，极值仅是“近似的”，并且必须在数学上执行子像素极值搜索；
使用D(x，y；σ)的泰勒展开，子像素极值计算如下：
其中x＝[x，y；σ]T，并且在最后一个近似极值处计算D的导数；使用亚像素极值而不是近似极值作为关键点将增加匹配的几率和SIFT算法的稳定性；
使用图像中的关键点及其比例，每个关键点的尺度与其对应的模糊图像的尺度相同，使得它们具有尺度不变性；
(3.4)为每个关键点分配方向度量以提供方向不变性，
为了将方向分配给关键点，关键点周围的所有像素的梯度幅度和方向计算如下：
θ(x，y)＝tan-1((L(x，y+1)-L(x，y-1))/(L(x+1，y)-L(x-1，y)))
使用计算得到的幅度和方向创建具有36个箱每个10度的直方图，直方图的数量与幅度成正比，所创建的直方图将在某些点处具有峰值，对应的仓指示可分配给关键点的方向，实现方向不变性；
(3.5)为每个检测到的特征或关键点创建描述符，在每个关键点周围考虑划分为4*4窗口的16*16网格，在每个4*4窗口内计算梯度幅度和方向；
如图7所示，根据方位测量结果，生成8位直方图，每个仓中的量值根据离关键点的距离来考虑，离关键点较远的像素对直方图的贡献较小，将此应用于16个4*4窗口，将为每个关键点创建一个4*4*8数组，将4*4*8数组归一化，创建特征向量或特征描述符；
(4)在两幅图像之间进行描述符的相互匹配，选择来自第一图像的第一特征，并计算其描述符到第二图像的所有特征描述符的距离，第二图像中的匹配特征与所选特征具有最小的“距离”，对于第一图像的所有特征重复此过程，仅需要三对匹配特征来计算两个图像之间的变换或单应性矩阵；
(5)对于多于三个对匹配特征，采用RANSAC算法可以用于计算单应性矩阵；
(6)将根据单应性矩阵计算的变换应用于图像或图，然后相应地合并两个图，如图8所示。
实施例2
本发明公开了一种二维栅格地图融合系统，包括：
构建地图模块，用于构建地图，分别在不同区域的移动机器人，使用其搭载的激光雷达对当前的未知环境进行建图；完成参数配置并运行Gmapping节点，每个移动机器人建立当前环境的局部栅格地图；
其中，构建地图模块中局部栅格地图的建立具体执行步骤为：
在栅格地图中，设栅格地图中的点Occupied状态的概率为p(s＝0)，Free状态的概率为p(s=1)，p(s＝0)+p(s=1)＝1，引入中间值表示栅格地图中点的状态；
对于栅格地图中一个点，当新测得一个测量值z～{0，1}，通过用公式 更新状态odds(s)；式中p(s=1|z)表示测量值z发生的情况下s=1发生的概率，p(s＝0|z)表示测量值z发生的情况下s＝0发生的概率；根据贝叶斯公式：
带入之后：
两边取对数可得：
测量值的模型为的选项只有lofree和looccu，激光雷达的对一个栅格观测结果只有两种：
占据和空闲
用logOdd(s)来表示栅格s的状态S的话，更新规则为：
其中，S+和S-分别表示测量值之后和之前栅格的状态；
一个栅格的初始状态Sinit为：默认栅格空闲和栅格占用的概率都为0.5；
更新一个栅格的状态即为：
S+＝S-+lofree或S+＝S-+looccu
假设lofree和looccu为确定的数值，即一正一负；通过激光雷达数据栅格进行判断：如果判定栅格是空闲，就执行S+＝S-+lofree；如果判定栅格是占据，就执行S+＝S-+looccu；在经过许多帧激光雷达数据的洗礼后，每一个栅格都存储了一个值，用设定阈值与栅格存储的值进行比较，来做栅格最终状态的判定。
定位建图模块，用于移动机器人定位，在移动机器人移动过程中，采用基于KL距离KLD算法自适应调整粒子样本数量的自适应蒙特卡罗定位AMCL算法，实现机器人相对世界坐标系的位姿；移动机器人在定位建图结束后，得到两幅栅格地图；
定位建图模块具体执行步骤为：
建立地图：使用构建地图模块中生成的当前环境地图；
初始化粒子：将机器人的初始位置作为粒子集合的中心，随机生成一定数量的粒子，每个粒子的位置和方向都是随机的；
运动模型：根据机器人的运动数据，更新粒子的位置和方向；
二维地图上的定位包括状态空间中机器人位置(x，y)和朝向角θ，MCL算法根据现有的数据估计机器人位姿的后验信息，可用的数据有两种类型，分别为测量数据和里程数据；采用里程计运动模型表示机器人运动p(xi|xt-1，ut)，采用激光的似然域模型表示机器人感知p(zi|xt)，置信度ηbel(xi|z1：t,u1：t)表示在t时刻综合所有过去的测量数据z1：t和控制数据u1：t的有关位姿xt的后验概率，表达式为：
ηbel(xi|z1：t，u1：t)∝p(zi|xt)·∫[p(xi|xt-1，ut)·p(xt-1|z1：t-1，u1：t-1)]dxt-1
式中：表示机器人在t时刻的估计位姿，当接收到里程信息时，通过运动模型采样更新xt；表示粒子的权重，当观察到新的测量数据时，根据测量模型给粒子加权；MCL通过一组加权的粒子表示后验，然而一旦位姿变化不连续，定位就会失败；为了提高定位精度，需要加入大量的粒子，这使得定位收敛速度变慢；
计算粒子权重：根据移动机器人传感器的测量数据，计算每个粒子的权重；
AMCL算法是在MCL的基础上改进而来的，根据长期估计权重wslow和短期估计权重wfast判断机器人是否被绑架，若wfast劣于wslow，通过在重采样中增加随机粒子使机器人从绑架中恢复，表达式为
式中，wavg表示所有粒子的平均权重，参数αslow和αfast分别为长期估计和短期估计时平均的指数滤波器的衰减率(0≤αslow≤αfast)；
KLD重采样：对粒子集合进行重采样，重采样后的粒子集合更加接近；
AMCl重采样阶段采用KLD算法自适应调整粒子数量，粒子数量可表示为
式中，ε表示真实分布与估计分布之间的误差，z1-δ表示标准正态分布上的1-δ分位点，k表示直方图的非空位，粒子数量上限Mtop与非空位k呈线性关系；在全局定位的初始阶段，需要非常多的样本，k越大，Mtop越高；当全局定位完成后，该问题转化为位置跟踪问题，粒子集中在真实位置附近，k变得很少，Mtop降低；
更新机器人位置：根据KLD重采样后的粒子集合，计算机器人在地图上的位置和方向。
创建描述符模块，用于提取特征点，并创建描述符；特征点提取采用尺度不变特征变换SIFT用于合并的局部地图，SIFT是寻找对缩放、旋转和光照不变的关键点或感兴趣点；
创建描述符模块具体执行步骤为：
为处理后的图像构造一个尺度空间，是指先拍摄原始图像，然后从原始图像中逐步生成模糊图像；然后将原始图像调整到一半大小，再次生成模糊图像，不断重复；在数学上，通过将高斯算子卷积到图像像素来生成模糊图像：
L(x，y；σ)＝G(x，y，σ)*I(x，y)
其中I(x，y)和L分别是图像和模糊图像，G(x，y，σ)是高斯算子，x是y局部坐标，并且σ是比例参数，*表示卷积运算，高斯算子定义如下：
计算高斯差分DoG，根据两个连续量表的差值计算计算高斯差分D(x，y；σ)，如下所示：
D(x，y；σ)＝(G(x，y，kσ)-G(x，y，σ))*I(x，y)＝L(x，y，kσ)-L(x，y，σ)
找到图像的关键点，首先在DoG图像中定位近似的局部最大值/最小值，为了找到近似的局部极值，将每个像素与其26个相邻像素进行比较；
如果像素是其所有26个相邻像素中最大或最小的，则将其标记为“关键点”；通常，极值位于像素之间，并且在像素上不完全匹配；在这种情况下，极值仅是“近似的”，并且必须在数学上执行子像素极值搜索；
使用D(x，y；σ)的泰勒展开，子像素极值计算如下：
其中x＝[x，y；σ]T，并且在最后一个近似极值处计算D的导数；使用亚像素极值而不是近似极值作为关键点将增加匹配的几率和SIFT算法的稳定性；
使用图像中的关键点及其比例，每个关键点的尺度与其对应的模糊图像的尺度相同，使得它们具有尺度不变性；
为每个关键点分配方向度量以提供方向不变性，
为了将方向分配给关键点，关键点周围的所有像素的梯度幅度和方向计算如下：
θ(x，y)＝tan-1((L(x，y+1)-L(x，y-1))/(L(x+1，y)-L(x-1，y)))
使用计算得到的幅度和方向创建具有36个箱每个10度的直方图，直方图的数量与幅度成正比，所创建的直方图将在某些点处具有峰值，对应的仓指示可分配给关键点的方向，实现方向不变性；
为每个检测到的特征或关键点创建描述符，在每个关键点周围考虑划分为4*4窗口的16*16网格，在每个4*4窗口内计算梯度幅度和方向；
根据方位测量结果，生成8位直方图，每个仓中的量值根据离关键点的距离来考虑，离关键点较远的像素对直方图的贡献较小，将此应用于16个4*4窗口，将为每个关键点创建一个4*4*8数组，将4*4*8数组归一化，创建特征向量或特征描述符。
第一矩阵计算模块，用于在两幅栅格地图的图像之间进行描述符的相互匹配，需要三对匹配特征来计算两个图像之间的变换或单应性矩阵；
第一矩阵计算模块中描述符的相互匹配具体执行步骤为：选择来自第一图像的第一特征，并计算其描述符到第二图像的所有特征描述符的距离，第二图像中的匹配特征与所选特征具有最小的“距离”，对于第一图像的所有特征重复此过程。
第二矩阵计算模块，用于对于多于三个对匹配特征，采用RANSAC算法可以用于计算单应性矩阵；
图像合并模块，用于将根据单应性矩阵计算的变换应用于图像或图，然后相应地合并两个图。
Claims (10)
1.一种二维栅格地图融合方法，其特征在于，包括如下步骤：
(1)构建地图，分别在不同区域的移动机器人，使用其搭载的激光雷达对当前的未知环境进行建图；完成参数配置并运行Gmapping节点，每个移动机器人建立当前环境的局部栅格地图；
(2)移动机器人定位，在移动机器人移动过程中，采用基于KL距离KLD算法自适应调整粒子样本数量的自适应蒙特卡罗定位AMCL算法，实现机器人相对世界坐标系的位姿；移动机器人在定位建图结束后，得到两幅栅格地图；
(3)提取特征点，并创建创建描述符；特征点提取采用尺度不变特征变换SIFT用于合并的局部地图，SIFT是寻找对缩放、旋转和光照不变的关键点或感兴趣点；
(4)在两幅栅格地图的图像之间进行描述符的相互匹配，需要三对匹配特征来计算两个图像之间的变换或单应性矩阵；
(5)对于多于三个对匹配特征，采用RANSAC算法可以用于计算单应性矩阵；
(6)将根据单应性矩阵计算的变换应用于图像或图，然后相应地合并两个图。
2.根据权利要求1所述的一种二维栅格地图融合方法，其特征在于，所述步骤(1)中局部栅格地图的建立包括以下步骤：
(1.1)在栅格地图中，设栅格地图中的点Occupied状态的概率为p(s＝0)，Free状态的概率为p(s＝1)，p(s＝0)+p(s＝1)＝1，引入中间值表示栅格地图中点的状态；
(1.2)对于栅格地图中一个点，当新测得一个测量值z～{0，1}，通过用公式更新状态odds(s)；式中p(s＝1|z)表示测量值z发生的情况下s＝1发生的概率，p(s＝0|z)表示测量值z发生的情况下s＝0发生的概率；根据贝叶斯公式：
带入之后：
两边取对数可得：
测量值的模型为的选项只有lofree和looccu，激光雷达的对一个栅格观测结果只有两种：
占据和空闲
用logOdd(s)来表示栅格s的状态S的话，更新规则为：
其中，S+和S-分别表示测量值之后和之前栅格的状态；
一个栅格的初始状态Sinit为：默认栅格空闲和栅格占用的概率都为0.5；
更新一个栅格的状态即为：
S+＝S-+lofree或S+＝S-+looccu
(1.3)假设lofree和looccu为确定的数值，即一正一负；通过激光雷达数据栅格进行判断：如果判定栅格是空闲，就执行S+＝S-+lofree；如果判定栅格是占据，就执行S+＝S-+looccu；在经过许多帧激光雷达数据的洗礼后，每一个栅格都存储了一个值，用设定阈值与栅格存储的值进行比较，来做栅格最终状态的判定。
3.根据权利要求2所述的一种二维栅格地图融合方法，其特征在于，所述步骤(2)具体包括如下步骤：
(2.1)建立地图：使用步骤(1)中生成的当前环境地图；
(2.2)初始化粒子：将机器人的初始位置作为粒子集合的中心，随机生成一定数量的粒子，每个粒子的位置和方向都是随机的；
(2.3)运动模型：根据机器人的运动数据，更新粒子的位置和方向；
二维地图上的定位包括状态空间中机器人位置(x，y)和朝向角θ，MCL算法根据现有的数据估计机器人位姿的后验信息，可用的数据有两种类型，分别为测量数据和里程数据；采用里程计运动模型表示机器人运动p(xi|xt-1，ut)，采用激光的似然域模型表示机器人感知p(zi|xt)，置信度ηbel(xi|z1：t，u1：t)表示在t时刻综合所有过去的测量数据z1：t和控制数据u1：t的有关位姿xt的后验概率，表达式为：
ηbel(xi|z1：t，u1：t)∝p(zi|xt)·∫[p(xi|xt-1，ut)·p(xt-1|z1：t-1，u1：t-1)]dxt-1
式中：表示机器人在t时刻的估计位姿，当接收到里程信息时，通过运动模型采样更新xt；表示粒子的权重，当观察到新的测量数据时，根据测量模型给粒子加权；MCL通过一组加权的粒子表示后验，然而一旦位姿变化不连续，定位就会失败；为了提高定位精度，需要加入大量的粒子，这使得定位收敛速度变慢；
(2.4)计算粒子权重：根据移动机器人传感器的测量数据，计算每个粒子的权重；
AMCL算法是在MCL的基础上改进而来的，根据长期估计权重wslow和短期估计权重wfast判断机器人是否被绑架，若wfast劣于wslow，通过在重采样中增加随机粒子使机器人从绑架中恢复，表达式为
式中，wavg表示所有粒子的平均权重，参数αslow和αfast分别为长期估计和短期估计时平均的指数滤波器的衰减率(0≤αslow≤αfast)；
(2.5)KLD重采样：对粒子集合进行重采样，重采样后的粒子集合更加接近；
AMCl重采样阶段采用KLD算法自适应调整粒子数量，粒子数量可表示为
式中，ε表示真实分布与估计分布之间的误差，z1-δ表示标准正态分布上的1-δ分位点，k表示直方图的非空位，粒子数量上限Mtop与非空位k呈线性关系；在全局定位的初始阶段，需要非常多的样本，k越大，Mtop越高；当全局定位完成后，该问题转化为位置跟踪问题，粒子集中在真实位置附近，k变得很少，Mtop降低；
(2.6)更新机器人位置：根据KLD重采样后的粒子集合，计算机器人在地图上的位置和方向。
4.根据权利要求3所述的一种二维栅格地图融合方法，其特征在于，所述步骤(3)具体包括如下步骤：
(3.1)为处理后的图像构造一个尺度空间，是指先拍摄原始图像，然后从原始图像中逐步生成模糊图像；然后将原始图像调整到一半大小，再次生成模糊图像，不断重复；在数学上，通过将高斯算子卷积到图像像素来生成模糊图像：
L(x，y；σ)＝G(x，y，σ)*I(x，y)
其中I(x，y)和L分别是图像和模糊图像，G(x，y，σ)是高斯算子，x是y局部坐标，并且σ是比例参数，*表示卷积运算，高斯算子定义如下：
(3.2)计算高斯差分DoG，根据两个连续量表的差值计算计算高斯差分D(x，y；σ)，如下所示：
D(x，y；σ)＝(G(x，y，kσ)-G(x，y，σ))*I(x，y)＝L(x，y，kσ)-L(x，y，σ)
(3.3)找到图像的关键点，首先在DoG图像中定位近似的局部最大值/最小值，为了找到近似的局部极值，将每个像素与其26个相邻像素进行比较；
如果像素是其所有26个相邻像素中最大或最小的，则将其标记为“关键点”；通常，极值位于像素之间，并且在像素上不完全匹配；在这种情况下，极值仅是“近似的”，并且必须在数学上执行子像素极值搜索；
使用D(x，y；σ)的泰勒展开，子像素极值计算如下：
其中x＝[x，y；σ]T，并且在最后一个近似极值处计算D的导数；使用亚像素极值而不是近似极值作为关键点将增加匹配的几率和SIFT算法的稳定性；
使用图像中的关键点及其比例，每个关键点的尺度与其对应的模糊图像的尺度相同，使得它们具有尺度不变性；
(3.4)为每个关键点分配方向度量以提供方向不变性，
为了将方向分配给关键点，关键点周围的所有像素的梯度幅度和方向计算如下：
θ(x，y)＝tan-1((L(x，y+1)-L(x，y-1))/(L(x+1，y)-L(x-1，y)))
使用计算得到的幅度和方向创建具有36个箱每个10度的直方图，直方图的数量与幅度成正比，所创建的直方图将在某些点处具有峰值，对应的仓指示可分配给关键点的方向，实现方向不变性；
(3.5)为每个检测到的特征或关键点创建描述符，在每个关键点周围考虑划分为4*4窗口的16*16网格，在每个4*4窗口内计算梯度幅度和方向；
根据方位测量结果，生成8位直方图，每个仓中的量值根据离关键点的距离来考虑，离关键点较远的像素对直方图的贡献较小，将此应用于16个4*4窗口，将为每个关键点创建一个4*4*8数组，将4*4*8数组归一化，创建特征向量或特征描述符。
5.根据权利要求4所述的一种二维栅格地图融合方法，其特征在于，所述步骤(4)中描述符的相互匹配具体包括如下步骤：选择来自第一图像的第一特征，并计算其描述符到第二图像的所有特征描述符的距离，第二图像中的匹配特征与所选特征具有最小的“距离”，对于第一图像的所有特征重复此过程。
6.一种二维栅格地图融合系统，其特征在于，包括：
构建地图模块，用于构建地图，分别在不同区域的移动机器人，使用其搭载的激光雷达对当前的未知环境进行建图；完成参数配置并运行Gmapping节点，每个移动机器人建立当前环境的局部栅格地图；
定位建图模块，用于移动机器人定位，在移动机器人移动过程中，采用基于KL距离KLD算法自适应调整粒子样本数量的自适应蒙特卡罗定位AMCL算法，实现机器人相对世界坐标系的位姿；移动机器人在定位建图结束后，得到两幅栅格地图；
创建描述符模块，用于提取特征点，并创建描述符；特征点提取采用尺度不变特征变换SIFT用于合并的局部地图，SIFT是寻找对缩放、旋转和光照不变的关键点或感兴趣点；
第一矩阵计算模块，用于在两幅栅格地图的图像之间进行描述符的相互匹配，需要三对匹配特征来计算两个图像之间的变换或单应性矩阵；
第二矩阵计算模块，用于对于多于三个对匹配特征，采用RANSAC算法可以用于计算单应性矩阵；
图像合并模块，用于将根据单应性矩阵计算的变换应用于图像或图，然后相应地合并两个图。
7.根据权利要求6所述的一种二维栅格地图融合系统，其特征在于，所述构建地图模块中局部栅格地图的建立具体执行步骤为：
在栅格地图中，设栅格地图中的点Occupied状态的概率为p(s＝0)，Free状态的概率为p(s＝1)，p(s＝0)+p(s＝1)＝1，引入中间值表示栅格地图中点的状态；
对于栅格地图中一个点，当新测得一个测量值z～{0，1}，通过用公式 更新状态odds(s)；式中p(s＝1|z)表示测量值z发生的情况下s＝1发生的概率，p(s＝0|z)表示测量值z发生的情况下s＝0发生的概率；根据贝叶斯公式：
带入之后：
两边取对数可得：
测量值的模型为的选项只有lofree和looccu，激光雷达的对一个栅格观测结果只有两种：
占据和空闲
用logOdd(s)来表示栅格s的状态S的话，更新规则为：
其中，S+和S-分别表示测量值之后和之前栅格的状态；
一个栅格的初始状态Sinit为：默认栅格空闲和栅格占用的概率都为0.5；
更新一个栅格的状态即为：
S+＝S-+lofree或S+＝S-+looccu
假设lofree和looccu为确定的数值，即一正一负；通过激光雷达数据栅格进行判断：如果判定栅格是空闲，就执行S+＝S-+lofree；如果判定栅格是占据，就执行
S+＝S-+looccu；在经过许多帧激光雷达数据的洗礼后，每一个栅格都存储了一个值，用设定阈值与栅格存储的值进行比较，来做栅格最终状态的判定。
8.根据权利要求7所述的一种二维栅格地图融合系统，其特征在于，所述定位建图模块具体执行步骤为：
建立地图：使用构建地图模块中生成的当前环境地图；
初始化粒子：将机器人的初始位置作为粒子集合的中心，随机生成一定数量的粒子，每个粒子的位置和方向都是随机的；
运动模型：根据机器人的运动数据，更新粒子的位置和方向；
二维地图上的定位包括状态空间中机器人位置(x，y)和朝向角θ，MCL算法根据现有的数据估计机器人位姿的后验信息，可用的数据有两种类型，分别为测量数据和里程数据；采用里程计运动模型表示机器人运动p(xi|xt-1，ut)，采用激光的似然域模型表示机器人感知p(zi|xt)，置信度ηbel(xi|z1：t,u1：t)表示在t时刻综合所有过去的测量数据z1：t和控制数据u1：t的有关位姿xt的后验概率，表达式为：
ηbel(xi|z1：t，u1：t)∝p(zi|xt)·∫[p(xi|xt-1，ut)·p(xt-1|z1：t-1，u1：t-1)]dxt-1
式中：表示机器人在t时刻的估计位姿，当接收到里程信息时，通过运动模型采样更新xt；表示粒子的权重，当观察到新的测量数据时，根据测量模型给粒子加权；MCL通过一组加权的粒子表示后验，然而一旦位姿变化不连续，定位就会失败；为了提高定位精度，需要加入大量的粒子，这使得定位收敛速度变慢；
计算粒子权重：根据移动机器人传感器的测量数据，计算每个粒子的权重；
AMCL算法是在MCL的基础上改进而来的，根据长期估计权重wslow和短期估计权重wfast判断机器人是否被绑架，若wfast劣于wslow，通过在重采样中增加随机粒子使机器人从绑架中恢复，表达式为
式中，wavg表示所有粒子的平均权重，参数αslow和αfast分别为长期估计和短期估计时平均的指数滤波器的衰减率(0≤αslow≤αfast)；
KLD重采样：对粒子集合进行重采样，重采样后的粒子集合更加接近；
AMCl重采样阶段采用KLD算法自适应调整粒子数量，粒子数量可表示为
式中，ε表示真实分布与估计分布之间的误差，z1-δ表示标准正态分布上的1-δ分位点，k表示直方图的非空位，粒子数量上限Mtop与非空位k呈线性关系；在全局定位的初始阶段，需要非常多的样本，k越大，Mtop越高；当全局定位完成后，该问题转化为位置跟踪问题，粒子集中在真实位置附近，k变得很少，Mtop降低；
更新机器人位置：根据KLD重采样后的粒子集合，计算机器人在地图上的位置和方向。
9.根据权利要求8所述的一种二维栅格地图融合系统，其特征在于，所述创建描述符模块具体执行步骤为：
为处理后的图像构造一个尺度空间，是指先拍摄原始图像，然后从原始图像中逐步生成模糊图像；然后将原始图像调整到一半大小，再次生成模糊图像，不断重复；在数学上，通过将高斯算子卷积到图像像素来生成模糊图像：
L(x，y；σ)＝G(x，y，σ)*I(x，y)
其中I(x，y)和L分别是图像和模糊图像，G(x，y，σ)是高斯算子，x是y局部坐标，并且σ是比例参数，*表示卷积运算，高斯算子定义如下：
计算高斯差分DoG，根据两个连续量表的差值计算计算高斯差分D(x，y；σ)，如下所示：
D(x，y；σ)＝(G(x，y，kσ)-G(x，y，σ))*I(x，y)＝L(x，y，kσ)-L(x，y，σ)
找到图像的关键点，首先在DoG图像中定位近似的局部最大值/最小值，为了找到近似的局部极值，将每个像素与其26个相邻像素进行比较；
如果像素是其所有26个相邻像素中最大或最小的，则将其标记为“关键点”；通常，极值位于像素之间，并且在像素上不完全匹配；在这种情况下，极值仅是“近似的”，并且必须在数学上执行子像素极值搜索；
使用D(x，y；σ)的泰勒展开，子像素极值计算如下：
其中x＝[x，y；σ]T，并且在最后一个近似极值处计算D的导数；使用亚像素极值而不是近似极值作为关键点将增加匹配的几率和SIFT算法的稳定性；
使用图像中的关键点及其比例，每个关键点的尺度与其对应的模糊图像的尺度相同，使得它们具有尺度不变性；
为每个关键点分配方向度量以提供方向不变性，
为了将方向分配给关键点，关键点周围的所有像素的梯度幅度和方向计算如下：
θ(x，y)＝tan-1((L(x，y+1)-L(x，y-1))/(L(x+1，y)-L(x-1，y)))
使用计算得到的幅度和方向创建具有36个箱每个10度的直方图，直方图的数量与幅度成正比，所创建的直方图将在某些点处具有峰值，对应的仓指示可分配给关键点的方向，实现方向不变性；
为每个检测到的特征或关键点创建描述符，在每个关键点周围考虑划分为4*4窗口的16*16网格，在每个4*4窗口内计算梯度幅度和方向；
根据方位测量结果，生成8位直方图，每个仓中的量值根据离关键点的距离来考虑，离关键点较远的像素对直方图的贡献较小，将此应用于16个4*4窗口，将为每个关键点创建一个4*4*8数组，将4*4*8数组归一化，创建特征向量或特征描述符。
10.根据权利要求9所述的一种二维栅格地图融合系统，其特征在于，所述第一矩阵计算模块中描述符的相互匹配具体执行步骤为：选择来自第一图像的第一特征，并计算其描述符到第二图像的所有特征描述符的距离，第二图像中的匹配特征与所选特征具有最小的“距离”，对于第一图像的所有特征重复此过程。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310773037.7A CN117029817A (zh) | 2023-06-28 | 2023-06-28 | 一种二维栅格地图融合方法及系统 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310773037.7A CN117029817A (zh) | 2023-06-28 | 2023-06-28 | 一种二维栅格地图融合方法及系统 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN117029817A true CN117029817A (zh) | 2023-11-10 |
Family
ID=88625179
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310773037.7A Pending CN117029817A (zh) | 2023-06-28 | 2023-06-28 | 一种二维栅格地图融合方法及系统 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117029817A (zh) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117723048A (zh) * | 2023-12-18 | 2024-03-19 | 哈尔滨工业大学 | 一种通信受限下的多机器人压缩通信协同建图方法及系统 |
| CN118168539A (zh) * | 2024-04-29 | 2024-06-11 | 江西理工大学南昌校区 | 一种基于Gmapping算法的建图方法及系统 |
-
2023
- 2023-06-28 CN CN202310773037.7A patent/CN117029817A/zh active Pending
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117723048A (zh) * | 2023-12-18 | 2024-03-19 | 哈尔滨工业大学 | 一种通信受限下的多机器人压缩通信协同建图方法及系统 |
| CN118168539A (zh) * | 2024-04-29 | 2024-06-11 | 江西理工大学南昌校区 | 一种基于Gmapping算法的建图方法及系统 |
| CN118168539B (zh) * | 2024-04-29 | 2024-07-26 | 江西理工大学南昌校区 | 一种基于Gmapping算法的建图方法及系统 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111563442B (zh) | 基于激光雷达的点云和相机图像数据融合的slam方法及系统 | |
| CN117029817A (zh) | 一种二维栅格地图融合方法及系统 | |
| CN111862201B (zh) | 一种基于深度学习的空间非合作目标相对位姿估计方法 | |
| CN111462207A (zh) | 一种融合直接法与特征法的rgb-d同时定位与地图创建方法 | |
| CN111652934A (zh) | 定位方法及地图构建方法、装置、设备、存储介质 | |
| CN112327326A (zh) | 带有障碍物三维信息的二维地图生成方法、系统以及终端 | |
| Moon et al. | View-point invariant 3d classification for mobile robots using a convolutional neural network | |
| CN112651944A (zh) | 基于cad模型的3c部件高精度六维位姿估计方法及系统 | |
| CN112967340A (zh) | 同时定位和地图构建方法、装置、电子设备及存储介质 | |
| CN111812978B (zh) | 一种多无人机协作slam方法与系统 | |
| Han et al. | DiLO: Direct light detection and ranging odometry based on spherical range images for autonomous driving | |
| US20200167650A1 (en) | Hinted neural network | |
| Zhao et al. | Visual odometry-A review of approaches | |
| CN113971697B (zh) | 一种空地协同车辆定位定向方法 | |
| CN117115414B (zh) | 基于深度学习的无gps无人机定位方法及装置 | |
| CN118189959A (zh) | 一种基于yolo姿态估计的无人机目标定位方法 | |
| CN113822996A (zh) | 机器人的位姿估计方法及装置、电子设备、存储介质 | |
| CN113739786A (zh) | 一种面向四足机器人的室内环境感知方法、装置及设备 | |
| CN115902977A (zh) | 基于视觉和gps的变电站机器人双重定位方法及系统 | |
| Sujiwo et al. | Robust and accurate monocular vision-based localization in outdoor environments of real-world robot challenge | |
| Zhang et al. | An improved SLAM algorithm based on feature contour extraction for camera pose estimation | |
| CN114821386A (zh) | 一种基于多视线矢量的四足机器人姿态精确估计方法 | |
| Jaenal et al. | Unsupervised appearance map abstraction for indoor visual place recognition with mobile robots | |
| Falchetti et al. | Probability hypothesis density filter visual simultaneous localization and mapping | |
| Comport et al. | Efficient model-based tracking for robot vision |
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 |