CN107590827A - 一种基于Kinect的室内移动机器人视觉SLAM方法 - Google Patents

Abstract

本发明请求保护一种基于Kinect的室内移动机器人视觉SLAM方法，包括以下步骤：S1，利用Kinect相机采集室内环境的彩色RGB数据与Depth数据；S2，对RGB数据进行特征检测，并实现相邻帧图像之间的快速有效匹配；S3，结合标定后Kinect相机的内参与像素点深度值将2D图像点转换为三维空间点，建立三维点云对应关系；S4，利用RANSAC算法剔除点云匹配中的外点，完成点云粗匹配；S5，采用一种拥有欧氏距离与角度阈值双重限制的ICP算法完成点云的精匹配；S6，在关键帧选取中引入权重，并利用g²o算法对机器人位姿进行优化，最终获取机器人运行轨迹，生成三维点云地图。本发明能够解决视觉SLAM系统中点云配准部分存在易陷入局部最优，匹配误差大等问题，提高点云配准精度。

Description

一种基于Kinect的室内移动机器人视觉SLAM方法

技术领域

本发明属于移动机器人视觉SLAM领域，尤其是一种点云精确配准方法。

背景技术

随着SLAM研究的不断深入，使三维重建成为了机器人研究领域的热潮。从本质上来说，SLAM所有的计算都是对传感器数据的处理，因此，不同传感器的基本方程的参数化有很大的差别。常见的SLAM传感器有IMU、激光和相机。IMU通常包含陀螺仪和加速度计，陀螺仪具有良好的动态响应，但会产生累计误差；加速度计虽不会产生累计误差，但易受到机器运行时的振动干扰；激光传感器精度高、探测范围广，但价格昂贵、功耗大；相机主要分为单目相机、双目相机和RGB-D相机，单目相机无法直接获取图像深度信息，需要通过移动相机来产生深度；双目相机通过视差计算深度，在推算图像中物体距离时计算量大，实时性较差；典型的RGB-D相机，如Kinect，具有价格低廉、信息丰富、更新速度快、能直接获取图像深度等诸多优点，使其成为了RGB-D SLAM系统的主流视觉传感器，受到广大视觉SLAM研究者的青睐。

视觉里程计是RGB-D SLAM系统中尤为重要的部分。视觉里程计的作用是估计相机运动，其中包括特征点检测与提取、相邻帧间配准、迭代最近点等。在视觉里程计部分，通常采用ICP算法去估计相机的运动。然而传统的ICP算法搜寻对应点对相当耗时，且容易出现大量误匹配点对。

发明内容

本发明旨在解决现有ICP算法进行点云配准过程中存在易陷入局部最优、匹配误差大的问题。提出了一种实时性好，鲁棒性强的视觉SLAM。本发明的技术方案如下：

一种基于Kinect的室内移动机器人视觉SLAM方法，其包括以下步骤：

S1、利用Kinect相机采集室内环境的RGB彩色数据与Depth深度数据；

S2、对步骤S1的RGB数据进行特征检测，对相邻帧图像进行匹配；

S3、结合标定后Kinect相机的内参与像素点深度值将2D图像点转换为三维空间点，建立三维点云对应关系；

S4、利用RANSAC随机抽样一致性算法剔除步骤S3中点云匹配的外点，完成点云粗匹配；

S5、采用一种拥有欧氏距离与角度阈值双重限制的ICP迭代最近点算法完成点云的精匹配；

S6、在关键帧选取中引入权重，并利用g²o通用图优化算法对机器人位姿进行优化，最终获取机器人运行轨迹，生成三维点云地图。

进一步的，所述步骤S2对步骤S1的RGB数据进行特征检测，对相邻帧图像进行匹配的过程为：采用具有旋转和尺度不变性的SURF特征对图像关键点进行检测，使用图像“金字塔”构建尺度空间，然后对关键点进行定位，并确定每个关键点的主方向，生成特征描述子，运用FLANN算法对特征描述子进行匹配。

进一步的，所述步骤S3将2D图像点转换为三维空间点的方法为：

图像中任意一点(u,v)，结合该点深度值与相机内参得到其对应的三维空间点坐标(x,y,z)，转换公式如下：

d＝z·s

其中，f_x和f_y分别表示相机在x、y轴上的焦距，c_x和c_y表示相机的光圈中心，s表示深度图的缩放因子，d表示深度数据。

进一步的，所述s为1000，f_x、f_y、c_x、c_y被定义为相机的内参矩阵C，

进一步的，所述步骤S4采用RANSAC随机抽样一致性算法剔除步骤S3中点云匹配中的外点，完成点云粗匹配具体包括步骤：

通过预设一个阈值d将全部匹配点对区分为内点和外点，剔除大于此阈值的匹配点对即剔除了外点对粗匹配的影响，将筛选后的内点进行最小二乘算法下对Kinect相机的初始位姿估计，将源点集A大致配准到目标点集B，粗匹配后的两片点云分别记为P和Q，相机在第i时刻的位姿P_i与第i+1时刻的位姿P_i+1的位姿转换关系为：

P_i+1＝P_iT_i ⁱ⁺¹。

进一步的，所述步骤S5点云精匹配过程为：1.在点集P中选择初始点集P_i0；2.在目标点集Q中搜索与点集P_i0距离最近的点集，使用欧式距离阈值法剔除噪声点，得到配对点云P_i0与Q_i0；3.应用角度阈值法剔除误匹配点对，得到精配对点集P_i1和Q_i1；4.采用SVD算法求得点集P_i1和Q_i1之间的旋转矩阵R与平移矩阵t；5.根据式P_i2＝RP_i1+t计算点集P_i1经一次刚体变换后得到的数据点集P_i2；6.重复步骤3～5，直到满足下式：

其中ε为大于零的阈值，用于判断迭代是否收敛，若收敛则迭代结束。

进一步的，所述欧式距离阈值法为：在粗匹配后，配对点云P中的任一点与其邻点，在另一个配对点云Q中的也应是邻近点，欧氏距离限制如下：

|p_i-q_j|<u

其中p_i与p_inei分别表示配对点云P”中的任意一点与其邻点，q_j与q_jnei分别表示p_i与p_inei在另一配对点云Q”中对应的点，u表示粗匹配后匹配点对间的平均欧式距离，δ表示距离阈值。

进一步的，所述角度阈值法为：采用点到切平面的最近搜索算法，对于给定点p_i及其对应点q_j，通过其邻近点集拟合平面，并求出各自的近似法向量，然后采用角度阈值限制，角度阈值限制如下：

其中n_i和n_j分别为点p_i和q_j的近似法向量，θ为两匹配点对近似法向量的夹角，为经欧氏距离去噪后的点对与所在同一坐标系原点O的夹角，ω和τ为设定的夹角阈值。

进一步的，所述步骤S6在关键帧选取中引入权重，方法为：在相机旋转出现微小变化或平移相对大一段距离后添加关键帧，当新帧到来时，检测其图像特征并与前一关键帧进行变换计算，如果变换超过阈值则添加关键帧，反之，则丢弃，具体选取标准如下：

其中(△x,△y,△z)为平移向量，(α,β,γ)为相对欧拉角，λ₁为平移权重参数，λ₂为欧拉角权重参数。并利用g²o算法对机器人位姿进行优化，最终获取机器人运行轨迹，生成三维点云地图。

本发明的优点及有益效果如下：

本文针对传统ICP(迭代最近点)算法下存在大量错误匹配点对，算法易陷入局部最优，甚至不能收敛等问题，提出了一种改进的ICP(迭代最近点)算法。在精匹配过程中，粗匹配的结果作为精匹配的初值，采用点对间欧式距离阈值法和角度阈值法对误匹配点对进行剔除，以筛选出满足阈值条件的点对，进行精确相机位姿估计。点对间欧式距离阈值法，可去除小于平均点对距离的匹配，且认为同一片点云下任一点与其邻点的拓扑结构不会随刚体变换而变化。经过欧式距离阈值法可剔除大部分点云数据噪声点。再结合角度阈值法，能进一步检测点对匹配的正确性，提高点云初值选取的正确率。此外，在关键帧选取中引入权重概念，大大减少了冗余关键帧。

附图说明

图1是本发明的优选实施例的流程示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、详细地描述。所描述的实施例仅仅是本发明的一部分实施例。

本发明解决上述技术问题的技术方案是：

本发明提供了一种基于Kinect的室内移动机器人视觉SLAM方法，其特征在于，包括以下步骤：

S1，利用Kinect相机采集室内环境的彩色RGB数据与Depth数据。

S2，对RGB数据采用具有旋转和尺度不变性的SURF特征进行图像关键点检测，使用图像“金字塔”构建尺度空间，然后对关键点进行定位，并确定每个关键点的主方向，生成特征描述子，运用FLANN算法实现相邻帧图像之间的快速有效匹配。

S3，结合标定后Kinect相机的内参与像素点深度值将2D图像点转换为三维空间点，建立三维点云对应关系。2D图像点到三维空间点的转换公式如下：

d＝z·s

其中，f_x和f_y分别表示深度相机在x、y轴上的焦距，c_x和c_y表示相机的光圈中心，s表示深度图的缩放因子，即深度图数据与实际距离的比例，由于深度图中都是以毫米为单位，因此通常为1000。通常情况下f_x、f_y、c_x、c_y被定义为相机的内参矩阵C，C的矩阵形式为：

S4，利用RANSAC算法剔除点云匹配中的外点，完成点云粗匹配。点云粗匹配的过程为：通过预设一个阈值d将全部匹配点对区分为内点和外点，剔除大于此阈值的匹配点对即剔除了外点对粗匹配的影响，将筛选后的内点进行最小二乘算法下对Kinect相机的初始位姿估计，可将源点集A大致配准到目标点集B，粗匹配后的两片点云分别记为P和Q。相机在第i时刻的位姿P_i与第i+1时刻的位姿P_i+1的位姿转换关系为：

P_i+1＝P_iT_i ⁱ⁺¹

S5，采用一种拥有欧氏距离与角度阈值双重限制的ICP算法完成点云的精匹配。点云精匹配过程为：1.在点集P中选择初始点集P_i0；2.在目标点集Q中搜索与点集P_i0距离最近的点集，使用欧式距离阈值法剔除噪声点，得到配对点云P_i0与Q_i0；3.应用角度阈值法进一步剔除误匹配点对，得到精配对点集P_i1和Q_i1；4.采用SVD算法求得点集P_i1和Q_i1之间的旋转矩阵R与平移矩阵t；5.根据式P_i2＝RP_i1+t计算点集P_i1经一次刚体变换后得到的数据点集P_i2。6.重复步骤3～5，直到满足下式：

其中ε为大于零的阈值，用于判断迭代是否收敛，若收敛则迭代结束。

所述欧式距离阈值法为：去除小于平均点对距离的匹配，且认为同一片点云下任一点与其邻点的拓扑结构不会随着刚体变换而变化。因此在粗匹配后，配对点云P中的任一点与其邻点，在另一个配对点云Q中的也应是邻近点。欧氏距离限制如下：

|p_i-q_j|<u

其中p_i与p_inei分别表示配对点云P”中的任意一点与其邻点，q_j与q_jnei分别表示p_i与p_inei在另一配对点云Q”中对应的点。u表示粗匹配后匹配点对间的平均欧式距离，δ表示距离阈值。

所述角度阈值法为：采用点到切平面的最近搜索算法，对于给定点p_i及其对应点q_j，通过其邻近点集拟合平面，并求出各自的近似法向量，然后采用角度阈值限制进一步提高点云初值选取的正确率。角度阈值限制如下：

其中n_i和n_j分别为点p_i和q_j的近似法向量，θ为两匹配点对近似法向量的夹角，为经欧氏距离去噪后的点对与所在同一坐标系原点O的夹角，ω和τ为设定的夹角阈值。

S6，在关键帧选取中引入权重，方法为：在相机旋转出现微小变化或平移相对大一段距离后添加关键帧。当新帧到来时，检测其图像特征并与前一关键帧进行变换计算，如果变换超过阈值则添加关键帧，反之，则丢弃。具体选取标准如下：

其中(△x,△y,△z)为平移向量，(α,β,γ)为相对欧拉角，λ₁为平移权重参数，λ₂为欧拉角权重参数。并利用g²o算法对机器人位姿进行优化，最终获取机器人运行轨迹，生成三维点云地图。

以上这些实施例应理解为仅用于说明本发明而不用于限制本发明的保护范围。在阅读了本发明的记载的内容之后，技术人员可以对本发明作各种改动或修改，这些等效变化和修饰同样落入本发明权利要求所限定的范围。

Claims (9)

1.一种基于Kinect的室内移动机器人视觉SLAM方法，其特征在于，包括以下步骤：

S1、利用Kinect相机采集室内环境的RGB彩色数据与Depth深度数据；

S2、对步骤S1的RGB数据进行特征检测，对相邻帧图像进行匹配；

S3、结合标定后Kinect相机的内参与像素点深度值将2D图像点转换为三维空间点，建立三维点云对应关系；

S4、利用RANSAC随机抽样一致性算法剔除步骤S3中点云匹配中的外点，完成点云粗匹配；

S5、采用一种拥有欧氏距离与角度阈值双重限制的ICP迭代最近点算法完成点云的精匹配；

S6、在关键帧选取中引入权重，并利用g²o通用图优化算法对机器人位姿进行优化，最终获取机器人运行轨迹，生成三维点云地图。

2.根据权利要求1所述的基于Kinect的室内移动机器人视觉SLAM方法，其特征在于，所述步骤S2对步骤S1的RGB数据进行特征检测，对相邻帧图像进行匹配的过程为：采用具有旋转和尺度不变性的SURF特征对图像关键点进行检测，使用图像“金字塔”构建尺度空间，然后对关键点进行定位，并确定每个关键点的主方向，生成特征描述子，运用FLANN算法对特征描述子进行匹配。

3.根据权利要求1或2所述的基于Kinect的室内移动机器人视觉SLAM方法，其特征在于，所述步骤S3将2D图像点转换为三维空间点的方法为：

图像中任意一点(u,v)，结合该点深度值与相机内参得到其对应的三维空间点坐标(x,y,z)，转换公式如下：

<mrow> <mi>v</mi> <mo>=</mo> <mfrac> <mrow> <mi>y</mi> <mo>&amp;CenterDot;</mo> <msub> <mi>f</mi> <mi>y</mi> </msub> </mrow> <mi>z</mi> </mfrac> <mo>+</mo> <msub> <mi>c</mi> <mi>y</mi> </msub> </mrow>

<mrow> <mi>u</mi> <mo>=</mo> <mfrac> <mrow> <mi>x</mi> <mo>&amp;CenterDot;</mo> <msub> <mi>f</mi> <mi>x</mi> </msub> </mrow> <mi>z</mi> </mfrac> <mo>+</mo> <msub> <mi>c</mi> <mi>x</mi> </msub> </mrow>

d＝z·s

其中，f_x和f_y分别表示相机在x、y轴上的焦距，c_x和c_y表示相机的光圈中心，s表示深度图的缩放因子，d表示深度数据。

4.根据权利要求3所述的基于Kinect的室内移动机器人视觉SLAM方法，其特征在于，所述s为1000，f_x、f_y、c_x、c_y被定义为相机的内参矩阵C，

<mrow> <mi>C</mi> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>f</mi> <mi>x</mi> </msub> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <msub> <mi>c</mi> <mi>x</mi> </msub> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <msub> <mi>f</mi> <mi>y</mi> </msub> </mtd> <mtd> <msub> <mi>c</mi> <mi>y</mi> </msub> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>1</mn> </mtd> </mtr> </mtable> </mfenced> <mo>.</mo> </mrow>

5.根据权利要求3所述的基于Kinect的室内移动机器人视觉SLAM方法，其特征在于，所述步骤S4采用RANSAC随机抽样一致性算法剔除步骤3中点云匹配中的外点，完成点云粗匹配具体包括步骤：

通过预设一个阈值d将全部匹配点对区分为内点和外点，剔除大于此阈值的匹配点对即剔除了外点对粗匹配的影响，将筛选后的内点进行最小二乘算法下对Kinect相机的初始位姿估计，将源点集A大致配准到目标点集B，粗匹配后的两片点云分别记为P和Q，相机在第i时刻的位姿P_i与第i+1时刻的位姿P_i+1的位姿转换关系为：

P_i+1＝P_iT_i ⁱ⁺¹。

6.根据权利要求5所述的基于Kinect的室内移动机器人视觉SLAM方法，其特征在于，所述步骤S5点云精匹配过程为：1.在点集P中选择初始点集P_i0；2.在目标点集Q中搜索与点集P_i0距离最近的点集，使用欧式距离阈值法剔除噪声点，得到配对点云P_i0与Q_i0；3.应用角度阈值法剔除误匹配点对，得到精配对点集P_i1和Q_i1；4.采用SVD算法求得点集P_i1和Q_i1之间的旋转矩阵R与平移矩阵t；5.根据式P_i2＝RP_i1+t计算点集P_i1经一次刚体变换后得到的数据点集P_i2；6.重复步骤3～5，直到满足下式：

<mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <msub> <mi>d</mi> <mi>m</mi> </msub> <mo>-</mo> <msub> <mi>d</mi> <mrow> <mi>m</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> <mo>&lt;</mo> <mi>&amp;epsiv;</mi> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>d</mi> <mi>m</mi> </msub> <mo>=</mo> <mfrac> <mn>1</mn> <mi>n</mi> </mfrac> <msubsup> <mi>&amp;Sigma;</mi> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>n</mi> </msubsup> <mo>|</mo> <mo>|</mo> <msub> <mi>Q</mi> <mrow> <mi>i</mi> <mi>m</mi> </mrow> </msub> <mo>-</mo> <msub> <mi>P</mi> <mrow> <mi>i</mi> <mi>m</mi> </mrow> </msub> <mo>|</mo> <msup> <mo>|</mo> <mn>2</mn> </msup> </mrow> </mtd> </mtr> </mtable> </mfenced>

其中ε为大于零的阈值，用于判断迭代是否收敛，若收敛则迭代结束。

7.根据权利要求6所述的基于Kinect的室内移动机器人视觉SLAM方法，其特征在于，所述欧式距离阈值法为：在粗匹配后，配对点云P中的任一点与其邻点，在另一个配对点云Q中的也应是邻近点，欧氏距离限制如下：

|p_i-q_j|<u

<mrow> <mo>|</mo> <mfrac> <mrow> <mo>|</mo> <mo>|</mo> <msub> <mi>p</mi> <mi>i</mi> </msub> <mo>-</mo> <msub> <mi>p</mi> <mrow> <mi>i</mi> <mi>n</mi> <mi>e</mi> <mi>i</mi> </mrow> </msub> <mo>|</mo> <mo>|</mo> <mo>-</mo> <mo>|</mo> <mo>|</mo> <msub> <mi>q</mi> <mi>j</mi> </msub> <mo>-</mo> <msub> <mi>q</mi> <mrow> <mi>j</mi> <mi>n</mi> <mi>e</mi> <mi>i</mi> </mrow> </msub> <mo>|</mo> <mo>|</mo> </mrow> <mrow> <mo>|</mo> <mo>|</mo> <msub> <mi>p</mi> <mi>i</mi> </msub> <mo>-</mo> <msub> <mi>p</mi> <mrow> <mi>i</mi> <mi>n</mi> <mi>e</mi> <mi>i</mi> </mrow> </msub> <mo>|</mo> <mo>|</mo> <mo>+</mo> <mo>|</mo> <mo>|</mo> <msub> <mi>q</mi> <mi>j</mi> </msub> <mo>-</mo> <msub> <mi>q</mi> <mrow> <mi>j</mi> <mi>n</mi> <mi>e</mi> <mi>i</mi> </mrow> </msub> <mo>|</mo> <mo>|</mo> </mrow> </mfrac> <mo>|</mo> <mo>=</mo> <mi>&amp;delta;</mi> </mrow>

其中p_i与p_inei分别表示配对点云P”中的任意一点与其邻点，q_j与q_jnei分别表示p_i与p_inei在另一配对点云Q”中对应的点，u表示粗匹配后匹配点对间的平均欧式距离，δ表示距离阈值。

8.根据权利要求6所述的基于Kinect的室内移动机器人视觉SLAM方法，其特征在于，所述角度阈值法为：采用点到切平面的最近搜索算法，对于给定点p_i及其对应点q_j，通过其邻近点集拟合平面，并求出各自的近似法向量，然后采用角度阈值限制，角度阈值限制如下：

<mrow> <mi>c</mi> <mi>o</mi> <mi>s</mi> <mi>&amp;theta;</mi> <mo>=</mo> <mfrac> <mrow> <mover> <msub> <mi>n</mi> <mi>i</mi> </msub> <mo>&amp;RightArrow;</mo> </mover> <mo>&amp;times;</mo> <mover> <msub> <mi>n</mi> <mi>j</mi> </msub> <mo>&amp;RightArrow;</mo> </mover> </mrow> <mrow> <mo>|</mo> <mover> <msub> <mi>n</mi> <mi>i</mi> </msub> <mo>&amp;RightArrow;</mo> </mover> <mo>|</mo> <mo>&amp;times;</mo> <mo>|</mo> <mover> <msub> <mi>n</mi> <mi>j</mi> </msub> <mo>&amp;RightArrow;</mo> </mover> <mo>|</mo> </mrow> </mfrac> <mo>&gt;</mo> <mi>&amp;omega;</mi> </mrow>

其中n_i和n_j分别为点p_i和q_j的近似法向量，θ为两匹配点对近似法向量的夹角，为经欧氏距离去噪后的点对与所在同一坐标系原点O的夹角，ω和τ为设定的夹角阈值。

9.根据权利要求8所述的基于Kinect的室内移动机器人视觉SLAM方法，其特征在于，所述步骤S6在关键帧选取中引入权重，方法为：在相机旋转出现微小变化或平移相对大一段距离后添加关键帧，当新帧到来时，检测其图像特征并与前一关键帧进行变换计算，如果变换超过阈值则添加关键帧，反之，则丢弃，具体选取标准如下：

其中(△x,△y,△z)为平移向量，(α,β,γ)为相对欧拉角，λ₁为平移权重参数，λ₂为欧拉角权重参数。并利用g²o算法对机器人位姿进行优化，最终获取机器人运行轨迹，生成三维点云地图。