CN109636897A - 一种基于改进RGB-D SLAM的Octomap优化方法 - Google Patents

Abstract

本发明公开了一种基于改进RGB‑D SLAM的Octomap优化方法，其特征在于，包括如下步骤：1）构造最小误差函数优化RGB‑D SLAM室内定位***相机位姿，获得点云图；2）使用K维树进行点云组织；4）计算平均K‑最近领距离；5）定义异常值并从点云图中删除；6）建立Octomap地图。这种方法能去除Octomap地图中的稀疏异常值，能提高Octomap地图的准确性和紧凑性并减少内存消耗。

Description

一种基于改进RGB-D SLAM的Octomap优化方法

技术领域

本发明涉及RGB-D SLAM室内定位***相机位姿优化和Octomap地图构建的室内定位技术领域，具体是一种基于改进深度图像映射的(RGB+Depth Map simultaneouslocalization and mapping，简称RGB-D SLAM)的基于八叉树的高效概率三维映射框架(Efficient Probabilistic3D Mapping Framework Based on Octrees，简称Octomap)优化方法。

背景技术

目前世界各国对移动机器人的技术研究已经上升到一个新的阶段，而同时定位和映射(SLAM)又是当前机器人技术中最重要的主题之一。自主机器人的可移动性使其代替人类从事危险的任务成为现实，在各种复杂、危险的环境中通过对自身行为的自主规划、自我学习等方式实现对周围环境的主动感知和理解。对于移动机器人，映射是不同导航任务不可或缺的先决条件。它提供了对本地化，防撞和路径规划至关重要的环境模型。在目前比较成熟的技术中有多种地图可供选择，如度量图，拓扑图，点云图，占用图等，也有2D和3D的应用区别。2D地图不足以使机器人在三维空间中移动。因此，构建丰富的环境3D地图是自动移动机器人的关键任务。

随着RGB-D相机尤其是Microsoft Kinect相机的出现，3D地图更容易构建。3D点云图在Visual SLAM中很常见，它已被用于多种3D SLAM***。RGB-D***不仅可以获取场景的彩色图像信息，而且还能获取到场景精确的深度信息，但是RGB-D***所获取的点云图非常稀疏，误差较大而且需要大量的内存资源，无法满足实时要求。

OctoMap是使用基于八叉树的映射框架，能够实现紧凑的3D地图，并且可以使用不用的分辨率进行地图表示，大大节省内存空间。然而，具有稀疏异常值的OctoMap是不可避免的，这可能是由于人类的存在以及传感器的误差而产生。使用原始OctoMap进行机器人导航，稀疏异常值将影响障碍物识别、路径规划，从而影响整个导航任务。

发明内容

本发明的目的是针对现有技术的不足，而提供一种基于改进RGB-D SLAM的Octomap优化方法。这种方法能去除Octomap地图中的稀疏异常值，能提高Octomap地图的准确性和紧凑性并减少内存消耗。

实现本发明目的的技术方案是：

一种基于改进RGB-D SLAM的Octomap优化方法，与现有技术不同处在于，包括如下方法：

1)构造最小误差函数优化RGB-D SLAM室内定位***相机位姿，获得点云图：在RGB-D SLAM***获取点云过程中，对相机位姿估计进行优化，依据图优化理论，通过3D-3D模型，在特征点匹配时建立最小误差模型，过程如下：相邻关键帧F_K和F_k+1，提取它们的特征点并匹配它们的描述符，得到两个特征点集S_k＝{s₁ ^k,s₂ ^k,...,s_n ^k}和S_k+1＝{s₁ ^k+1,s₂ ^k+1,...,s_n ^k+1}，结合给定图像深度的深度信息，得到3D特征点集X_k＝{x₁ ^k,x₂ ^k,...,x_n ^k}和相机内部参数为C，利用下述公式(1)可求得旋转矩阵R和平移向量t为：

其中d₁ ^k,d₁ ^k+1分别表示关键帧F_K和F_k+1对应特征点的深度；

构造每个点的最小误差函数为公式(2)：

整合得到全局优化函数公式(3):

其中n表示特征点索引，T表示矩阵转置，最后使用g2o框架对点云图进行优化，得到全局点云图；

2)使用K维树进行点云组织：初始点云没有任何拓扑，需要以某种形式组织，使用K维树对点云进行组织，K维树是稀疏异常值处理的下一步基础，K维树使用垂直于每个坐标轴的平面来逐步穿过区域，组织点云后，每个点对应于二叉树的节点；

3)为点云设置K值：依据需要为点云设置K值，用于作下一步K最近邻距离计算；

4)计算平均K-最近领距离：K近邻的平均距离用公式(4)计算：

其中n为点索引，得出每个点的平均K最近邻距离d_n，再依据每个点的平均距离计算出全局平均距离μ和标准差σ，计算依据为公式(5)：

其中m为点索引；

5)定义异常值并从点云图中删除：依据统计理论，假设平均距离μ和标准差σ服从高斯分布，平均距离在特定范围之外的点可以被定义为异常值并从点云图中删除，因此设置一个置信区间[c₁,c₂]为公式(6)：

其中α是比例因子，用于调整范围的大小，α的值取决于近邻分析的大小，在置信区间之外的点将被定义为奇异值并从点云图中删除；

6)建立Octomap地图：结合步骤4)和步骤5)调用Octomap库把点云地图转换为Octomap地图，Octomap是基于八叉树的映射框架，通过树结构有效地处理体素，从而产生紧凑的存储器表示，提高准确性。

与现有技术相比，本技术方案的有益效果：

构造最小误差函数优化RGB-D SLAM室内定位***相机位姿，减少在点云获取时的误差，缩短了点云获取时间，K-最近邻和高斯分布相结合，去除Octomap地图中的稀疏异常值，提高Octomap地图的准确性和紧凑性并减少内存消耗。

这种方法能去除Octomap地图中的稀疏异常值，能提高Octomap地图的准确性和紧凑性并减少内存消耗。

附图说明

图1为实施例的方法流程示意图；

图2改进RGB-D SLAM的Octomap优化方法结构示意图。

具体实施方式

下面结合附图和实施例对本发明内容作进一步的说明，但不是对本发明的限定。

实施例：

参照图1，一种基于改进RGB-D SLAM的Octomap优化方法，与现有技术不同处在于，包括如下方法：

1)构造最小误差函数优化RGB-D SLAM室内定位***相机位姿，获得点云图：在RGB-D SLAM***获取点云过程中，对相机位姿估计进行优化，依据图优化理论，通过3D-3D模型，在特征点匹配时建立最小误差模型，过程如下：相邻关键帧F_K和F_k+1，提取它们的特征点并匹配它们的描述符，得到两个特征点集S_k＝{s₁ ^k,s₂ ^k,...,s_n ^k}和S_k+1＝{s₁ ^k+1,s₂ ^k+1,...,s_n ^k+1}，结合给定图像深度的深度信息，得到3D特征点集X_k＝{x₁ ^k,x₂ ^k,...,x_n ^k}和X_k+1＝{x₁ ^{k +1},x₂ ^k+1,...,x_n ^k+1}，相机内部参数为C，利用下述公式(1)可求得旋转矩阵R和平移向量t为：

其中d₁ ^k,d₁ ^k+1分别表示关键帧F_K和F_k+1对应特征点的深度；

构造每个点的最小误差函数为公式(2)：

整合得到全局优化函数公式(3):

其中n表示特征点索引，T表示矩阵转置，最后使用g2o框架对点云图进行优化，得到全局点云图；

2)使用K维树进行点云组织：初始点云没有任何拓扑，需要以某种形式组织，使用K维树对点云进行组织，K维树是稀疏异常值处理的下一步基础，K维树使用垂直于每个坐标轴的平面来逐步穿过区域，组织点云后，每个点对应于二叉树的节点；

3)为点云设置K值：依据需要为点云设置K值，用于作下一步K最近邻距离计算；

4)计算平均K-最近领距离：K近邻的平均距离用公式(4)计算：

其中n为点索引，得出每个点的平均K最近邻距离d_n，再依据每个点的平均距离计算出全局平均距离μ和标准差σ，计算依据为公式(5)：

其中m为点索引；

5)定义异常值并从点云图中删除：依据统计理论，假设平均距离μ和标准差σ服从高斯分布，平均距离在特定范围之外的点可以被定义为异常值并从点云图中删除，因此设置一个置信区间[c₁,c₂]为公式(6)：

其中α是比例因子，用于调整范围的大小，α的值取决于近邻分析的大小，在置信区间之外的点将被定义为奇异值并从点云图中删除；

6)建立Octomap地图：结合步骤4)和步骤5)调用Octomap库把点云地图转换为Octomap地图，Octomap是基于八叉树的映射框架，通过树结构有效地处理体素，从而产生紧凑的存储器表示，提高准确性。

如图2所示，本实施例通过构造最小误差函数优化RGB-D SLAM室内定位***相机位姿，减少迭代次数，提高***获取点云速度和精度；使用K-最近邻和高斯分布相结合的方法去除点云图中的异常稀疏值，进一步提高点云图的精度；再将点云图转化为内存消耗更小，精确度更高，更紧凑的Octomap地图。

Claims (1)

1.一种基于改进RGB-D SLAM的Octomap优化方法，其特征在于，包括如下步骤：

1)构造最小误差函数优化RGB-D SLAM室内定位***相机位姿，获得点云图：在RGB-DSLAM***获取点云过程中，对相机位姿估计进行优化，依据图优化理论，通过3D-3D模型，在特征点匹配时建立最小误差模型，过程如下：相邻关键帧F_K和F_k+1，提取它们的特征点并匹配它们的描述符，得到两个特征点集S_k＝{s₁ ^k,s₂ ^k,...,s_n ^k}和S_k+1＝{s₁ ^k+1,s₂ ^k+1,...,s_n ^k+1}，结合给定图像深度的深度信息，得到3D特征点集X_k＝{x₁ ^k,x₂ ^k,...,x_n ^k}和X_k+1＝{x₁ ^k+1,x₂ ^{k +1},...,x_n ^k+1}，相机内部参数为C，利用下述公式(1)可求得旋转矩阵R和平移向量t为：

其中d₁ ^k,d₁ ^k+1分别表示关键帧F_K和F_k+1对应特征点的深度；

构造每个点的最小误差函数为公式(2)：

整合得到全局优化函数公式(3):

其中n表示特征点索引，T表示矩阵转置，最后使用g2o框架对点云图进行优化，得到全局点云图；

2)使用K维树进行点云组织：使用K维树对点云进行组织，K维树使用垂直于每个坐标轴的平面来逐步穿过区域，每个点对应于二叉树的节点；

3)为点云设置K值：依据需要为点云设置K值；

4)计算平均K-最近领距离：K近邻的平均距离用公式(4)计算：

其中n为点索引，得出每个点的平均K最近邻距离d_n，再依据每个点的平均距离计算出全局平均距离μ和标准差σ，计算依据为公式(5)：

其中m为点索引；

5)定义异常值并从点云图中删除：假设平均距离μ和标准差σ服从高斯分布，平均距离在特定范围之外的点可以被定义为异常值并从点云图中删除，因此设置一个置信区间[c₁,c₂]为公式(6)：

其中α是比例因子，用于调整范围的大小，α的值取决于近邻分析的大小，在置信区间之外的点将被定义为奇异值并从点云图中删除；

6)建立Octomap地图：结合步骤4)和步骤5)调用Octomap库把点云地图转换为Octomap地图。