CN109857123A - 一种基于视觉感知和激光探测的室内slam地图的融合方法 - Google Patents

Abstract

本发明公开了一种基于视觉感知和激光探测的室内SLAM地图的融合方法。该方法包括的步骤有激光探测，构建三维点云图，滤波和投影处理和地图融合，通过这些步骤一方面获得含有地面高度信息的二维激光探测地图，另一方面获得三维视觉感知点云地图，再对三维视觉感知点云地图进行滤波和投影后，获得包含有障碍物信息的二维视觉感知地图，最后对二维激光探测地图和二维视觉感知地图进行融合，得到二维融合地图。通过本方法可以将机器人导航过程中有影响的障碍物信息融合进二维地图信息，生成一种准确度高、信息丰富的二维地图，在室内环境进行机器人移动导航和路径规划时，具有很强的实用性。

Description

一种基于视觉感知和激光探测的室内SLAM地图的融合方法

技术领域

本发明涉及计算机图像领域，尤其涉及一种基于视觉感知和激光探测的室内SLAM地图的融合方法。

背景技术

SLAM技术，即同时自主定位和地图构建技术的简称，SLAM技术是机器人导航的关键技术之一，机器人通过SLAM技术进行即时地图构建和自主定位，来确定自身在环境中的位置，从而进行准确的避障和路径规划。

在进行机器人导航时，针对室内机器人室内环境，考虑到室内机器人系统有限的数据处理能力，我们以二维栅格地图作为机器人的导航地图格式，它的优点是机器人运动过程中里程计信息处理数据量较少，使机器人运动过程中以较少的粒子滤波完成准确的位姿估计，同时为了保证机器人的导航过程中流畅的进行避障，我们选用二维激光数据作为ROS导航框架的避障输入，相比较于数量较多3D点云信息，它需要处理的数据量较少，有利于机器人进行快速准确的进行避障。

然而，由于复杂的地面环境，对于室内的特定障碍物，例如低矮和悬空的障碍物，由于二维的地图和二维的激光数据输入不能有效检测，从而使机器人在避障和路径规划中产生一定的错误。

发明内容

本发明主要解决的技术问题是提供一种基于视觉感知和激光探测的室内SLAM地图的融合方法，解决现有技术中为机器人室内导航提供的SLAM地图存在的避障和路径规划中易于产生错误问题。

为解决上述技术问题，本发明采用的一个技术方案是提供一种基于视觉感知和激光探测的室内SLAM地图的融合方法，包括以下步骤：激光探测，利用激光传感器探测室内通道及障碍物，运行激光探测算法，保存含有地面高度信息的二维激光探测地图；构建三维点云图，利用摄像机拍摄进行视觉感知室内环境，运行视觉感知算法，并保存三维视觉感知点云地图；滤波和投影处理，对所述三维视觉感知点云地图进行滤波，然后进行投影，生成包含有障碍物信息的二维视觉感知地图；地图融合，将所述二维激光探测地图和所述二维视觉感知地图进行融合，得到二维融合地图。

在本发明基于视觉感知和激光探测的室内SLAM地图的融合方法另一实施例中，在构建三维点云图过程中，所述视觉感知算法是利用机器人操作系统进行DSO算法的实时运行，摄像机实时发布图像，DSO节点订阅处理。

在本发明基于视觉感知和激光探测的室内SLAM地图的融合方法另一实施例中，实时运行DSO算法时，还需要对运行的源文件进行修改和编译运行，并且，根据获取到的带有深度信息的图像信息和摄像机内参，进一步获得任何一个像素在以摄像机为参考原点的摄像机坐标系下的位置，与此同时，还获得的摄像机位姿，进一步计算这些像素在标准坐标系下的位置，最终，我们把每个地图点在标准坐标系下的位置求出，由此构建出了一张完整的三维点云地图。

在本发明基于视觉感知和激光探测的室内SLAM地图的融合方法另一实施例中，在构建三维点云图过程中，还包括通过随机一致性算法搜索三维点云地图中的地面方程，所述随机一致性选择算法包括：

(1)考虑一个最小抽样集模型，它的势为n，n为初始化参数模型所需要的最小样本数，一个样本集P，集合P的样本数大于n，从P中随机抽取包含n个样本的P的子集S作为初始化模型M；

(2)余集SC＝P\S中与模型M的误差小于某一设定阈值k的样本集以及S构成S*。S*认为是内点集，它们构成S的一致集；

(3)如果S*集合样本数量大于等于n，认为得到正确的模型参数，并利用集S*采用最小二乘法等方法重新计算新的模型M*；重新抽取新的S，重复以上过程；

(4)在完成一定的抽样次数以后，若未找到一致性集则算法失败；否则选取抽样后得到的最大一致集判断内外点，算法结束。

在本发明基于视觉感知和激光探测的室内SLAM地图的融合方法另一实施例中，在得到地面方程以后，进行旋转处理：选取地面方程中的一点作为坐标原点，在三维点云地图地面与坐标轴xy平面的交点中任选一点，绕这一点进行旋转，使三维点云地图中地面平面法向量平行于Z轴，垂直于标准坐标系，即得到所求三维点云图像。

在本发明基于视觉感知和激光探测的室内SLAM地图的融合方法另一实施例中，对所述三维视觉感知点云地图进行滤波处理中，通过statisticalOutlierRemoval滤波器和voxelgrid滤波器融合的方法对地图噪音进行滤除。

在本发明基于视觉感知和激光探测的室内SLAM地图的融合方法另一实施例中，将StatisticalOutlierRemoval滤波器与voxelgrid滤波器对三维视觉感知点云地图进行串联处理。

在本发明基于视觉感知和激光探测的室内SLAM地图的融合方法另一实施例中，在地图融合中，包括设置地图之间的刚体变换T＝{R，t}，R为旋转参数，t为平移参数，旋转参数R由旋转角θ确定，平移参数t为一个二维的列向量，分别表示为：

在本发明基于视觉感知和激光探测的室内SLAM地图的融合方法另一实施例中，在地图融合中，还包括地图匹配处理，首先，采用边缘提取算法提取地图的边界，将所述二维激光探测地图表示为P，所述二维视觉感知地图为Q，对于任何一组T＝{R，t}，当作用于所述二维视觉感知地图后，若两个地图重合的像素点集为S，由此可以得出二维融合地图的拼接融合模型为：

R^TR＝I_2×2,det(R)＝1

其中λ为控制参数，|.|代表集合中元素个数，ξ_min代表允许的最小重叠百分比。

在本发明基于视觉感知和激光探测的室内SLAM地图的融合方法另一实施例中，所述二维融合地图的拼接融合模型通过迭代运算来优化T＝{R，t}，其中每次迭代的方法为：

第一步：建立待融合地图的点集对应关系：

建立点对：以最近邻搜索算法计算并保留对应点之间的欧氏距离

第二步：根据第一步中的计算结果，计算待融合图像的重叠百分比，更新地图数据点交集

该式的求解方法为：将第一步中保留的欧式距离按升序队列排序，遍历排序后的队列并计算每个点对目标函数值累加，确定此时的点对计算当前的重叠百分比，并更新地图数据点交集

第三步：利用更新后的地图点交集计算更新T＝{R，t}参数：

这里采用最小二乘法进行计算，然后重复执行第一步至第三步，满足|ε_k-ε_k-1|＜ε或达到最大迭代次数后，停止迭代，即可以得到最优化的T＝{R，t}参数。

本发明的有益效果是：本发明公开了一种基于视觉感知和激光探测的室内SLAM地图的融合方法。该方法包括的步骤有激光探测，构建三维点云图，滤波和投影处理和地图融合，通过这些步骤一方面获得含有地面高度信息的二维激光探测地图，另一方面获得三维视觉感知点云地图，再对三维视觉感知点云地图进行滤波和投影后，获得包含有障碍物信息的二维视觉感知地图，最后对二维激光探测地图和二维视觉感知地图进行融合，得到二维融合地图。通过本方法可以将机器人导航过程中有影响的障碍物信息融合进二维地图信息，生成一种准确度高、信息丰富的二维地图，在室内环境进行机器人移动导航和路径规划时，具有很强的实用性。

附图说明

图1是根据本发明基于视觉感知和激光探测的室内SLAM地图的融合方法一实施例的流程图。

具体实施方式

为了便于理解本发明，下面结合附图和具体实施例，对本发明进行更详细的说明。附图中给出了本发明的较佳的实施例。但是，本发明可以以许多不同的形式来实现，并不限于本说明书所描述的实施例。相反地，提供这些实施例的目的是使对本发明的公开内容的理解更加透彻全面。

需要说明的是，除非另有定义，本说明书所使用的所有的技术和科学术语与属于本发明的技术领域的技术人员通常理解的含义相同。在本发明的说明书中所使用的术语只是为了描述具体的实施例的目的，不是用于限制本发明。本说明书所使用的术语“和/或”包括一个或多个相关的所列项目的任意的和所有的组合。

图1显示了本发明基于视觉感知和激光探测的室内SLAM地图的融合方法一实施例的流程图。在图1中，包括步骤有：

步骤S101，激光探测，利用激光传感器探测室内通道及障碍物，运行激光探测算法，保存含有地面高度信息的二维激光探测地图；

步骤S102，构建三维点云图，利用摄像机拍摄进行视觉感知室内环境，运行视觉感知算法，并保存三维视觉感知点云地图；

步骤S103，滤波和投影处理，对所述三维视觉感知点云地图进行滤波，然后进行投影，生成包含有障碍物信息的二维视觉感知地图；

步骤S104，地图融合，将所述二维激光探测地图和所述二维视觉感知地图进行融合，得到二维融合地图。

优选的，在步骤S102中，在构建三维点云图过程中，所述视觉感知算法是利用机器人操作系统(ROS)系统进行DSO算法的实时运行，摄像机实时发布图像，DSO节点订阅处理。

具体而言，利用点云拼接的方法来保存点云地图，由于DSO算法实时运行需要摄像头，实时输入数据，因此我们利用ROS系统编写节点，进行摄像头信息的实时发布，利用DSO算法订阅摄像头图像信息作为输入，进行处理后生成点云图像。

进一步优选的，实时运行DSO算法时，还需要对运行的源文件进行修改和编译运行，由此才能保存地图。并且，根据获取到的带有深度信息的图像信息和摄像机内参，进一步又可以获得任何一个像素在以摄像机为参考原点的摄像机坐标系下的位置，与此同时，还获得的摄像机位姿，进一步计算这些像素在标准坐标系下的位置。最终，我们把每个地图点的在标准坐标系下的位置求出，由此构建出了一张完整的点云地图。

优选的，在步骤S102中，在室内环境中，三维点云图包含地面，书桌，家具等一系列物体，需要求出点云地图中的地面方程，在点云地图中，由于地面组成点云的数量最多，符合随机一致性选择算法的基本思想，该算法主要是根据一组包含异常数据的样本数据集，计算出数据的数学模型参数，得到有效样本数据的算法。我们用随机一致性算法搜索点云地图中的地面方程，例如在PCL库中利用点云分割思路实现，利用随机一致性算法设置平面为目标几何形状，由于随机一致性算法必收敛，因此可以得出地面方程。随机一致性选择算法通过反复选择数据中的一组随机子集来达成搜索目标。被选取的子集被假设为局内点，具体过程如下：

1.考虑一个最小抽样集模型，它的势为n，n为初始化参数模型所需要的最小样本数，一个样本集P，集合P的样本数大于n，从P中随机抽取包含n个样本的P的子集S作为初始化模型M。

2.余集SC＝P\S中与模型M的误差小于某一设定阈值k的样本集以及S构成S*。S*认为是内点集，它们构成S的一致集。

3.如果S*集合样本数量大于等于n，认为得到正确的模型参数，并利用集S*采用最小二乘法等方法重新计算新的模型M*；重新抽取新的S，重复以上过程。

4.在完成一定的抽样次数以后，若未找到一致性集则算法失败；否则选取抽样后得到的最大一致集判断内外点，算法结束。

5.通过估计局内点与模型的错误率来评估模型。

在上述步骤的基础上，我们进行了进一步优化，内容如下：

1.如果在选取子集S时可以根据某些一直的样本特性等采用特定的选取方案或有约束的随机选取来替代原来的完全随机选取。

2.当通过一致集S*计算模型M*后，可以将P中所有与M*的误差小于k的样本加入S*，然后重新计算M*。

在上述随机一致性选择算法中，由于地面平面的样本点大于墙面、桌面等平面样本点，我们选择点云地图中最大的平面阈值t，结合优化内容来选取地面方程，如果选取的阈值大于t，则算法选取失败。否则，选取地面方程成功。从而得出地面方程。

进一步的，在得到地面方程以后，进行旋转处理。选取地面方程中某一点作为坐标原点，在点云地图地面与坐标轴xy平面的交点中任选一点，绕这一点进行旋转，使点云地图中地面平面法向量平行于Z轴，垂直于标准坐标系，即得到所求点云图像。

优选的，从地面方程中任取它与xy平面的一个交点(m,n,k)，经过点(m,n,k)的单位法向量为(p,q,t)，由此，由旋转前向量n1(p,q,t)，旋转后向量n2(p,q,1)，由此我们可以知道三个点(p,q,t)，(p,q,1)和(p,q,0)。设这三个点构成平面P1,可以求出它们的法向量n3，即是同时垂直与n1和n2，并且经过n1和n2的交点，即为点(p,q,0)。得出向量n3就是它们的旋转轴。

优选的，在步骤S103中，对所述三维视觉感知点云地图进行滤波处理中，通过statisticalOutlierRemoval滤波器和voxelgrid滤波器融合的方法对地图噪音进行滤除。

优选的，在用statisticalOutlierRemoval滤波器进行滤波时，对每个像素点的邻域进行一个统计分析，并去除掉那些不符合标准的像素点。优选的，基于在输入数据中对像素点到临近该像素点的距离的分布的计算。对每个像素点，我们计算它到所有临近像素点的平均距离。假设得到的结果是一个高斯分布，其形状由均值和标准差决定，使用全局距离平均值和方差定义一个标准范围，将平均距离在标准范围之外的点定义为离群点，并可从数据集中去除掉。

优选的，具体的处理方法是：读取三维视觉感知点云地图文件，赋予point指针指向该三维视觉感知点云地图文件，创建一个StatisticalOutlierRemoval滤波器，参数设置如下：将每个像素点的临近点的个数设为50,将标准差倍数设为1,表示如果三维视觉感知点云地图文件中的一个像素点超出平均距离一个标准差以上，则该像素点被标记为离群点，然后将其移除。然后将输出结果存储在cloud_filtered中，将剩下的数据点存入外存中，使用可视化显示工具显示。

优选的，在用voxelgrid滤波器进行滤波时，该滤波器实现下采样，对输入的三维点云数据创建一个三维体素栅格，然后在每个体素内中使用所有像素点的重心来近似显示体素中其它像素点，使体素格内所有像素点用一个重心点来近似表示。

优选的，具体的处理方法是：读取所述三维视觉感知点云地图文件，赋予point指针指向所述三维视觉感知点云地图文件，创建一个voxelgrid滤波器对象，设置创建该滤波器的体素大小为1cm，执行滤波处理，输出存储在cloud_filtered中，将数据写入磁盘，进行后续处理或进行可视化显示。

优选的，将StatisticalOutlierRemoval滤波器与voxelgrid滤波器对三维视觉感知点云地图进行串联处理，在保证将该点云地图几何轮廓外的噪音点滤除的基础上，保留其几何结构，减少点云数量，精简点云地图的数据内容，以此来提高检索地面方程的参数精度。

优选的，然后利用直通滤波器进行滤波，直通滤波器可以对某一维度实行一个点云数据滤波，即去掉用户指定范围内部或者外部的点云数据。

优选的，根据点云图像的尺度，结合机器人的实际高度，以及根据点云图像尺度，本文设置Z轴区间为(0.0,1.0)范围内点云图数据保留，投影为二维栅格地图。

进一步的，在步骤S104中，在地图融合中，虽然之前已经对三维视觉感知点云地图进行了去噪和格式的转换，以及转换为二维视觉感知点云地图处理，但由于两幅待融合地图之间选用了不同的参考坐标系，最后得到的两个地图之间的刚体变换参数确定不能从原始地图获得信息，只能依靠算法来确定旋转参数R和平移参数t。

由于地图形式为二维，因此我们可以设置地图之间的刚体变换为T＝{R，t}，R为旋转参数，形式是一个2*2矩阵，唯一的依赖为旋转角θ，平移参数t为一个二维的列向量。

之后的主要工作是对地图进行匹配的确定，根据地图的格式，可以利用ICP进行点集的匹配。首先，采用边缘提取算法提取地图的边界，将所述二维激光探测地图表示为P，所述二维视觉感知地图为Q，对于任何一组T＝{R，t}，当作用于所述二维视觉感知地图后，若两个地图重合的像素点集为S，由此可以得出二维融合地图的拼接融合模型为：

R^TR＝I_2×2,det(R)＝1

其中λ为控制参数，|.|代表集合中元素个数，ξ_min代表允许的最小重叠百分比。

所述二维融合地图的拼接融合模型通过迭代运算来优化T＝{R，t}，其中每次迭代的方法为：

第一步：建立待融合地图的点集对应关系：

建立点对：以最近邻搜索算法计算并保留对应点之间的欧氏距离

第二步：根据第一步中的计算结果，计算待融合图像的重叠百分比，更新地图数据点交集

该式的求解方法为：将第一步中保留的欧式距离按升序队列排序，遍历排序后的队列并计算每个点对目标函数值累加，确定此时的点对计算当前的重叠百分比，并更新地图数据点交集

第三步：利用更新后的地图点交集计算更新T＝{R，t}参数：

这里采用最小二乘法进行计算，然后重复执行第一步至第三步，满足|ε_k-ε_k-1|＜ε或达到最大迭代次数后，停止迭代，即可以得到最优化的T＝{R，t}参数。

由此可见，本发明公开了一种基于视觉感知和激光探测的室内SLAM地图的融合方法。该方法包括的步骤有激光探测，构建三维点云图，滤波和投影处理和地图融合，通过这些步骤一方面获得含有地面高度信息的二维激光探测地图，另一方面获得三维视觉感知点云地图，再对三维视觉感知点云地图进行滤波和投影后，获得包含有障碍物信息的二维视觉感知地图，最后对二维激光探测地图和二维视觉感知地图进行融合，得到二维融合地图。通过本方法可以将机器人导航过程中有影响的障碍物信息融合进二维地图信息，生成一种准确度高、信息丰富的二维地图，在室内环境进行机器人移动导航和路径规划时，具有很强的实用性。

以上所述仅为本发明的实施例，并非因此限制本发明的专利范围，凡是利用本发明说明书及附图内容所作的等效结构变换，或直接或间接运用在其他相关的技术领域，均同理包括在本发明的专利保护范围内。

Claims (10)

1.一种基于视觉感知和激光探测的室内SLAM地图的融合方法，其特征在于，包括以下步骤：

激光探测，利用激光传感器探测室内通道及障碍物，运行激光探测算法，保存含有地面高度信息的二维激光探测地图；

构建三维点云图，利用摄像机拍摄进行视觉感知室内环境，运行视觉感知算法，并保存三维视觉感知点云地图；

滤波和投影处理，对所述三维视觉感知点云地图进行滤波，然后进行投影，生成包含有障碍物信息的二维视觉感知地图；

地图融合，将所述二维激光探测地图和所述二维视觉感知地图进行融合，得到二维融合地图。

2.根据权利要求1所述的基于视觉感知和激光探测的室内SLAM地图的融合方法，其特征在于，在构建三维点云图过程中，所述视觉感知算法是利用机器人操作系统进行实时运行DSO算法，摄像机实时发布图像，DSO节点订阅处理。

3.根据权利要求2所述的基于视觉感知和激光探测的室内SLAM地图的融合方法，其特征在于，实时运行DSO算法时，需要对运行的源文件进行修改和编译运行，并且，根据获取到的带有深度信息的图像信息和摄像机内参，进一步获得任何一个像素在以摄像机为参考原点的摄像机坐标系下的位置，与此同时，还获得的摄像机位姿，进一步计算像素在标准坐标系下的位置，最终，把每个像素点在标准坐标系下的位置求出，由此构建出了一张完整的三维点云地图。

4.根据权利要求3所述的基于视觉感知和激光探测的室内SLAM地图的融合方法，其特征在于，在构建三维点云图过程中，还包括通过随机一致性算法搜索三维点云地图中的地面方程，所述随机一致性选择算法包括：

(1)构建一个最小抽样集模型，它的势为n，n为初始化参数模型所需要的最小样本数，一个样本集合P，集合P的样本数大于n，从P中随机抽取包含n个样本的P的子集S作为初始化模型M；

(2)余集SC＝P\S中与模型M的误差小于设定的阈值k的样本集以及S构成S*，S*认为是内点集，它们构成S的一致集；

(3)如果S*集合样本数量大于或等于n，认为得到正确的模型参数，并利用集S*采用最小二乘法重新计算新的模型M*；重新抽取新的S，重复以上过程；

(4)在完成规定的抽样次数以后，若未找到一致性集则算法失败；否则选取抽样后得到的最大一致集判断内外点，算法结束。

5.根据权利要求4所述的基于视觉感知和激光探测的室内SLAM地图的融合方法，其特征在于，在得到地面方程以后，进行旋转处理：选取地面方程中的一点作为坐标原点，在三维点云地图地面与坐标轴xy平面的交点中选择一点，绕这一点进行旋转，使三维点云地图中地面平面法向量平行于Z轴，垂直于标准坐标系，即得到所求三维点云图像。

6.根据权利要求5所述的基于视觉感知和激光探测的室内SLAM地图的融合方法，其特征在于，对所述三维视觉感知点云地图进行滤波处理中，通过statisticalOutlierRemoval滤波器和voxelgrid滤波器融合的方法对地图噪音进行滤除。

7.根据权利要求6所述的基于视觉感知和激光探测的室内SLAM地图的融合方法，其特征在于，将StatisticalOutlierRemoval滤波器与voxelgrid滤波器对三维视觉感知点云地图进行串联处理。

8.根据权利要求7所述的基于视觉感知和激光探测的室内SLAM地图的融合方法，其特征在于，在地图融合中，包括设置地图之间的刚体变换T＝{R，t}，R为旋转参数，t为平移参数，旋转参数R由旋转角θ确定，平移参数t为一个二维的列向量，分别表示为：

9.根据权利要求8所述的基于视觉感知和激光探测的室内SLAM地图的融合方法，其特征在于，在地图融合中，还包括地图匹配处理，首先，采用边缘提取算法提取地图的边界，将所述二维激光探测地图表示为P，所述二维视觉感知地图为Q，对于任何一组T＝{R，t}，当作用于所述二维视觉感知地图后，若两个地图重合的像素点集为S，由此可以得出二维融合地图的拼接融合模型为：

R^TR＝I_2×2,det(R)＝1

其中λ为控制参数，|.|代表集合中元素个数，ξ_min代表允许的最小重叠百分比。

10.根据权利要求9所述的基于视觉感知和激光探测的室内SLAM地图的融合方法，其特征在于，所述二维融合地图的拼接融合模型通过迭代运算来优化T＝{R，t}，其中每次迭代的方法为：

第一步：建立待融合地图的点集对应关系：

i＝1,2,...,N_q

建立点对：以最近邻搜索算法计算并保留对应点之间的欧氏距离

第二步：根据第一步中的计算结果，计算待融合图像的重叠百分比，更新地图数据点交集

该式的求解方法为：将第一步中保留的欧式距离按升序队列排序，遍历排序后的队列并计算每个点对目标函数值累加，确定此时的点对计算当前的重叠百分比，并更新地图数据点交集

第三步：利用更新后的地图点交集计算更新T＝{R，t}参数：

这里采用最小二乘法进行计算，然后重复执行第一步至第三步，满足|ε_k-ε_k-1|＜ε或达到最大迭代次数后，停止迭代，即可以得到最优化的T＝{R，t}参数。