CN106556412A - 一种室内环境下考虑地面约束的rgb‑d视觉里程计方法 - Google Patents

Abstract

本发明属于移动机器人自主定位领域，特别涉及一种室内环境下考虑地面约束的RGB‑D视觉里程计方法。本发明包括：(1)室内场景中，利用搭载在移动平台的RGB‑D相机在移动过程中采集环境中的彩色图像信息和深度图像信息；相邻帧图像分别记为I_s和I_t；(2)根据深度图像信息通过逆投影函数π^‑1求解获得环境的三维点云数据V_s和V_t；(3)ORB特征提取与匹配：采用ORB算法实现RGB‑D图像特征点的提取与匹配。本发明采用ORB算法完成特征提取与匹配的图像预处理过程，较SIFT和SURF算法，快速性提高一个数量级；利用深度图像检测获取点云地面并利用地面信息增强点集对齐，提高运动变换的估计精度。

Description

一种室内环境下考虑地面约束的RGB-D视觉里程计方法

技术领域

本发明属于移动机器人自主定位领域，特别涉及一种室内环境下考虑地面约束的RGB-D视觉里程计方法。

背景技术

在导航系统中，里程计(odometry)是一种利用传感器的移动数据来估算机器人位置随时间改变量的方法，常应用于轮式移动机器人。传统里程计利用罗盘或编码器，通过计量移动机器人车轮旋转的圈数实现定位功能。然而当轮胎打滑时会产生难以消除的很大的偏差，并且由于误差随时间积累，导致了里程计读数愈加不可靠，不能满足对精度要求苛刻的应用场景。视觉里程计(VO，Visual Odometry)技术作为利用连续的图像序列来估计机器人移动距离的方法，仅仅依靠视觉输入信息就可以获得较为精确的本体位姿信息，对环境要求较低，且不存在因打滑等因素导致编码器读数不准的缺陷，得到了导航界的广泛关注。

大部分的视觉里程计技术主要基于单目和双目相机。基于单目的视觉里程计，设备简单，需假定一维信息已知，且精度较差，对噪声敏感；基于双目的视觉里程计，虽然采用立体视觉进行运动估计，但由于其用到的三角变换的非线性特点，使得重建点在深度方向上的不确定度分布不均匀，且噪声各向异性，往往无法得到最优估计。近年，随着RGB-D新型传感器的兴起，其在计算机视觉领域的研究与应用获得了广泛关注。利用RGB-D传感器不仅可以获得环境的彩色图像信息，还能得到其对应的深度图像信息，二者统称为RGB-D信息。

采用RGB-D传感器实现视觉里程计，特征提取与匹配作为数据关联的预处理步骤对系统求解速度的影响很大，传统算法通常采用SIFT和SURF算法，算法速度较慢，难以满足里程计实时输出需求；由于传统算法采用经匹配获取的对应特征点集直接进行运动变换矩阵的求解，未添加环境结构化特征几何约束，算法精确度有待进一步提高。

发明内容

为解决上述问题，兼顾准确性和快速性，本发明的目的在于提出一种采用ORB特征检测算法完成对彩色图像点特征的快速提取和匹配，提高算法快速性；利用深度图像检测获取点云地面并利用地面信息增强点集对齐，提高运动变换的估计精度的室内环境下考虑地面约束的RGB-D视觉里程计方法。

本发明的目的在于提供一种室内环境下考虑地面约束的RGB-D视觉里程计方法

本发明的目的是这样实现的：

本发明包括：

(1)室内场景中，利用搭载在移动平台的RGB-D相机在移动过程中采集环境中的彩色图像信息和深度图像信息；相邻帧图像分别记为I_s和I_t；

(2)根据深度图像信息通过逆投影函数π^-1求解获得环境的三维点云数据V_s和V_t：

其中，p＝(X,Y,Z,1)^T表示环境三维点在相机坐标系下的齐次坐标；m＝(u,v)^T表示环境三维点投影到图像像素坐标系下的坐标；Z＝Z(m)表示该点的深度数据；RGB-D相机横纵方向上以像素为单位的焦距分别为f_x,f_y，光心为(c_x,c_y)；

(3)ORB特征提取与匹配：采用ORB算法实现RGB-D图像特征点的提取与匹配；

(4)以RGB-D图像源帧I_s对应的摄像头位姿的高度、俯仰角信息为先验知识并基于图像源帧到目标帧的相机小位移假设，对源、目标帧RGB-D图像地面最可能区域进行局部搜索，从而快速而准确地对地面区域进行检测；考虑到点云数据异常点的存在，采用引入异常点剔除机制的地面平面检测算法；

(5)添加地面约束：利用地面信息，通过施加扭动T_inc将摄像头变换至新的位姿：

X′＝T_inc·X

其中，平移增量t_inc可利用下式求解得到：

t_inc＝(t_x,t_y,t_z)＝(x′-x,y′-y,z′-z)

式中，(x′,y′,z′)和(x,y,z)分别是平面重心m′和m的空间坐标；

采用欧拉角表达旋转增量 β,ψ表示经旋转变换至绕x,y,z坐标轴分别转过的角度，将欧拉角式转换为四元数，转换关系如下：

式中，字符c，s分别为cos和sin的缩写形式；

(6)根据地面检测是否成功，即所检测获取的地面区域在图像区域占比是否超过20％，设置标志位flag：超过20％，地面拟合参数可靠准确，检测成功，flag＝true，并在地面重合约束下对三维对应点集Fs和Ft施加扭动T_inc，得到F_s′和F_t′；反之，，视作检测失败，flag＝false；

(7)依据标志位不同对地面检测失败和地面检测成功的对应点集利用随机采样一致性算法求解相机运动变换，并对成功进行地面校正的变换矩阵经恢复得到原始变换，从而得到了使原图像F_s、F_t三维匹配点集对齐的刚体变换估计T₁、T₂；

(8)计算T₁、T₂二者对应的配准变换误差e_i，误差较小者即为最佳的里程计运动变换T^*；

所述步骤(3)具体包括以下子步骤：

(3.1)RGB图像快速oFAST角点检测；

(3.2)深度异常特征点剔除；

根据每个oFAST特征点的二维坐标查找深度图像，获取对应的深度信息；根据深度数据有效性阈值判定深度有效性，将不满足阈值要求的FAST角点从特征点集合中剔除，保留符合要求的深度有效点用于ORB特征检测算法后续BRIEF描述子的构建；

(3.3)具有旋转不变性的BRIEF描述子构建；

(3.4)采用Hamming距离D(K₁,K₂)作为特征描述子相似性度量函数，完成特征点的匹配，得到三维对应点集合Fs和Ft：

其中，K₁＝x₀x₁x₂…x₂₅₅，K₂＝y₀y₁y₂…y₂₅₅分别为相邻帧图像对应特征点的ORB特征描述子；

所述所述步骤(4)具体包括以下子步骤：

(4.1)利用步骤(2)生成的环境三维点云数据为输入，利用特征值法求解平面参数a，b，c的初始值；特征值法空间平面方程称为法线式方程为：

ax+by+cz＝d

其中，(a,b,c)为平面的单位法向量，满足条件a²+b²+c²＝1；(x,y,z)为平面上点的空间坐标；d为坐标原点到平面的距离，d≥0；

(4.2)根据a，b，c的初始值计算每个样本点到初始拟合平面的距离d_i：

(4.3)计算标准差：其中

(4.4)判断d_i与2σ的大小关系：若d_i＞2σ，i点被当作异常点剔除；反之，保留；

(4.5)重复步骤(4.2)到步骤(4.4)，直到所有异常点均被剔除，计算获得最优平面参数a，b，c，d，得到地平面的平面拟合方程；

(4.6)利用得到的法向量作为参考信息，采用ISODATA算法对疑似地面点云数据进行法向聚类，将法向接近的点云数据分类到一起，得到地面点云的第一次聚类结果；然后计算点云集各点到拟合平面的距离，将小于设定阈值的点予以保留，得到第二次聚类结果，完成对地面的精确检测，并计算得到地平面点集重心m。

所述步骤(4)的运动变换误差利用关联点集F_s、F_t的欧式距离计算：

本发明的有益效果在于：

本发明采用ORB算法完成特征提取与匹配的图像预处理过程，较SIFT和SURF算法，快速性提高一个数量级；利用深度图像检测获取点云地面并利用地面信息增强点集对齐，提高运动变换的估计精度。

附图说明

图1为本发明步骤3中的ORB特征提取与匹配算法流程示意图；

图2为本发明室内环境下考虑地面约束的RGB-D视觉里程计方法的算法框架示意图。

具体实施方式

下面结合附图对本发明做进一步描述。

本发明公开了一种室内环境下考虑地面约束的RGB-D视觉里程计方法，利用RGB-D相机作为传感器输入设备实现视觉里程计功能，包括ORB特征提取与匹配，地面平面检测及约束添加，RANSAC运动变换估计，构造运动变换误差评价函数评估运动变换估计结果，里程计结果输出等几个步骤。本发明采用ORB算法进行彩色图像的特征提取与匹配，在保证精度要求基础上提高特征检测的快速性；利用深度图像检测点云地面，结合地面平面约束校正视觉里程计位姿变换输出结果，提高视觉里程计输出的估计精度。通过上述方式，本发明能够实时、鲁棒、精确地完成移动机器人的室内自定位，并进一步应用于机器人同步定位与地图构建(SLAM)。本发明提出的考虑地面约束的RGB-D视觉里程计方法的技术方案包含以下要点：

步骤1.利用RGB-D传感器获取室内环境彩色图像和深度图像序列；

步骤2.结合RGB-D传感器内参，求解环境的三维点云数据；

步骤3.ORB特征提取与匹配；

步骤4.引入异常点剔除机制的地面平面检测；

步骤5.地面约束添加；

步骤6.采用随机采样一致性算法计算刚体变换；

步骤7.构造运动变换误差评价函数评估添加地面约束和未添加地面约束的运动估计误差；

步骤8.选取误差较小者作为视觉里程计结果输出。

本发明是一种室内环境下考虑地面约束的RGB-D视觉里程计方法，利用RGB-D相机作为传感器输入设备实现视觉里程计功能，包括ORB特征提取与匹配，地面平面检测及约束添加，RANSAC运动变换估计，构造运动变换误差评价函数评估运动变换估计结果，里程计结果输出几个关键步骤。

如图1所示，ORB特征提取与匹配的具体步骤如下：

步骤1.RGB图像进行快速FAST角点检测：

如果某像素点与其周围领域足够多的像素点处于不同区域，则把该点作为候选FAST角点。对于灰度图像，即如果某点同其周围领域内足够多的像素点相比，灰度值大或者小，该点可能被当作角点。通常采用分割检测判据进行FAST特征检测，即在以像素点p为中心，以r为半径的圆周上如果存在联系的n个像素I_k(k＝1,2,...,n)，通过下式判断该像素点是否为角点：

其中，I_k表示圆周上某像素点的灰度值；I_p表示中心像素点p的灰度值，t距离度量阈值，若CRF＝1的个数大于一个给定的数目阈值T，将该点归为候选点，通常T＝9。

针对FAST本身不具备尺度不变性的缺陷，ORB算法对FAST算法进行了如下改进：首先设定较低的阈值，提取出超过T数目的FAST角点，然后利用Harris角点评价函数对其进行排序，提取前T个FAST角点。最后采用尺度金字塔得到多尺度待测图像，最终筛选出FAST角点。

针对FAST算子不具备旋转不变性的缺陷，ORB算法引入灰度质心法为FAST角点添加方向信息，即通过计算特征点的矩来得到点的方向。图像中某特征点点邻域的(p+q)阶矩的定义式为：

特征点邻域的质心坐标C为：

则质心的方向定义为：

将θ定义该特征点的主方向，则可根据此方向来提取旋转不变性描述子。为提高FAST角点对旋转的适应性，需将计算区域的点的横纵坐标限制在半径为r的圆形区域内，即x,y∈[-r,r]。

步骤2.度异常特征点进行剔除：

根据每个oFAST特征点的二维坐标查找深度图像，获取对应的深度信息；根据深度数据有效性阈值(RGB-D传感器深度相机探测范围有效值为0.7到7米)判定深度有效性，将不满足阈值要求的FAST角点从特征点集合中剔除，保留符合要求的深度有效点用于ORB特征检测算法后续BRIEF描述子的构建。

步骤3.旋转不变性的BRIEF描述子构建：

BRIEF描述子的每一位都是由随机选取的两个二进制点做比较得来的。对于图像块p，定义二进制比较准则τ为：

式中，p(x)为图像块p在像素点x＝(u,v)处的灰度值，p(y)为图像块p在像素点y处的灰度值。任意选取n个(x,y)像素点对，就可以得到BRIEF的n位二进制比特串，即BRIEF特征描述子：

n的选值需要在速度、存储效率以及识别率几个指标间权衡，可以为128，256，512等。本发明ORB算子选用的是256位的描述子，即BRIEF-256。

步骤4.用Hamming距离D(K₁,K₂)作为特征描述子相似性度量函数，完成特征点的匹配，得到三维对应点集合Fs和Ft：

其中，K₁＝x₀x₁x₂…x₂₅₅，K₂＝y₀y₁y₂…y₂₅₅分别为相邻帧图像对应特征点的ORB特征描述子(256位)。

如图2所示，室内环境下考虑地面约束的RGB-D视觉里程计方法具体步骤如下：

步骤1.室内场景中，利用搭载在移动平台的RGB-D相机在移动过程中采集环境中的彩色图像信息和深度图像信息,相邻帧图像分别记为I_s和I_t。

步骤2.根据深度图像信息通过逆投影函数π^-1求解获得环境的三维点云数据V_s和V_t：

其中，p＝(X,Y,Z,1)^T表示环境三维点在相机坐标系下的齐次坐标；m＝(u,v)^T表示环境三维点投影到图像像素坐标系下的坐标；Z＝Z(m)表示该点的深度数据；RGB-D相机横纵方向上以像素为单位的焦距分别为f_x,f_y，光心为(c_x,c_y)。

步骤3.ORB特征提取与匹配：采用ORB(orientedFASTandrotatedBRIEF)算法快速实现RGB-D图像特征点的提取与匹配；

步骤4.以RGB-D图像源帧I_s对应的摄像头位姿的高度、俯仰角信息为先验知识并基于图像源帧到目标帧的相机小位移假设，对源、目标帧RGB-D图像地面最可能区域进行局部搜索，从而快速而准确地对地面区域进行检测。考虑到点云数据异常点的存在，采用引入异常点剔除机制的地面平面检测算法，具体包括以下几个子步骤：

步骤4.1.利用步骤2生成的环境三维点云数据为输入，利用特征值法求解平面参数a，b，c的初始值；特征值法空间平面方程称为法线式方程，表达为：

ax+by+cz＝d(9)

其中，(a,b,c)为平面的单位法向量，满足条件a²+b²+c²＝1；(x,y,z)为平面上点的空间坐标；d为坐标原点到平面的距离，d≥0。

步骤4.2.根据a，b，c的初始值计算每个样本点到初始拟合平面的距离d_i：

步骤4.4.计算标准差：其中

步骤4.5.判断d_i与2σ的大小关系：若d_i＞2σ，i点被当作异常点剔除；反之，保留。

步骤4.6.重复步骤4.2到步骤4.5，直到所有异常点均被剔除，计算获得最优平面参数a，b，c，d，得到地平面的平面拟合方程。

步骤4.7.利用得到的法向量作为参考信息，采用ISODATA算法对疑似地面点云数据进行法向聚类，将法向接近的点云数据分类到一起，得到地面点云的第一次聚类结果。然后计算点云集各点到拟合平面的距离，将小于设定阈值的点予以保留，就得到了第二次聚类结果，从而完成对地面的精确检测，并计算得到地平面点集重心m。

步骤5.添加地面约束：利用地面信息，通过施加扭动T_inc将摄像头变换至新的位姿：

X′＝T_inc·X

其中，平移增量t_inc可利用下式求解得到：

t_inc＝(t_x,t_y,t_z)＝(x′-x,y′-y,z′-z)(12)

式中，(x′,y′,z′)和(x,y,z)分别是平面重心m′和m的空间坐标。

采用欧拉角表达旋转增量 β,ψ表示经旋转变换至绕x,y,z坐标轴分别转过的角度，将欧拉角式转换为四元数，转换关系如下：

式中，字符“c”，“s”分别为“cos”和“sin”的缩写形式。

步骤6.根据地面检测是否成功，即所检测获取的地面区域在图像区域占比是否超过20％，设置标志位flag：超过20％，地面拟合参数可靠准确，检测成功，flag＝true，并在地面重合约束下对三维对应点集Fs和Ft施加扭动T_inc(步骤5)，得到F_s′和F_t′；反之，地面拟合参数存在较大误差，不宜采用，视作检测失败，flag＝false。

步骤7.依据标志位不同对地面检测失败和地面检测成功的对应点集利用随机采样一致性算法求解相机运动变换，并对成功进行地面校正的变换矩阵经恢复得到原始变换，从而得到了使原图像F_s、F_t三维匹配点集对齐的刚体变换估计T₁、T₂；

步骤8.计算T₁、T₂二者对应的配准变换误差e_i，误差较小者即为最佳的里程计运动变换T^*，输出里程计结果。运动变换误差利用关联点集F_s、F_t的欧式距离计算：

Claims (4)

1.一种室内环境下考虑地面约束的RGB-D视觉里程计方法，其特征在于，包括以下几个步骤：

(1)室内场景中，利用搭载在移动平台的RGB-D相机在移动过程中采集环境中的彩色图像信息和深度图像信息；相邻帧图像分别记为I_s和I_t；

(2)根据深度图像信息通过逆投影函数π^-1求解获得环境的三维点云数据V_s和V_t：

$p={\mathrm{\π}}^{-1}(m,Z)={(\frac{u-c_x}{f_x}Z,\frac{v-c_y}{f_y}Z,Z,1)}^T$

其中，p＝(X,Y,Z,1)^T表示环境三维点在相机坐标系下的齐次坐标；m＝(u,v)^T表示环境三维点投影到图像像素坐标系下的坐标；Z＝Z(m)表示该点的深度数据；RGB-D相机横纵方向上以像素为单位的焦距分别为f_x,f_y，光心为(c_x,c_y)；

(3)ORB特征提取与匹配：采用ORB算法实现RGB-D图像特征点的提取与匹配；

(4)以RGB-D图像源帧I_s对应的摄像头位姿的高度、俯仰角信息为先验知识并基于图像源帧到目标帧的相机小位移假设，对源、目标帧RGB-D图像地面最可能区域进行局部搜索，从而快速而准确地对地面区域进行检测；考虑到点云数据异常点的存在，采用引入异常点剔除机制的地面平面检测算法；

(5)添加地面约束：利用地面信息，通过施加扭动T_inc将摄像头变换至新的位姿：

$\begin{array}{c}{X}^{\′}=T_{inc}\·X\\ T_{inc}=\left[\begin{array}{cc}{R}_{inc}& t_{inc}\\ 0& 1\end{array}\right]\end{array};$

其中，平移增量t_inc可利用下式求解得到：

t_inc＝(t_x,t_y,t_z)＝(x′-x,y′-y,z′-z)

式中，(x′,y′,z′)和(x,y,z)分别是平面重心m′和m的空间坐标；

采用欧拉角表达旋转增量 β,ψ表示经旋转变换至绕x,y,z坐标轴分别转过的角度，将欧拉角式转换为四元数，转换关系如下：

式中，字符c，s分别为cos和sin的缩写形式；

(6)根据地面检测是否成功，即所检测获取的地面区域在图像区域占比是否超过20％，设置标志位flag：超过20％，地面拟合参数可靠准确，检测成功，flag＝true，并在地面重合约束下对三维对应点集Fs和Ft施加扭动T_inc，得到F_s′和F_t′；反之，，视作检测失败，flag＝false；

(7)依据标志位不同对地面检测失败和地面检测成功的对应点集利用随机采样一致性算法求解相机运动变换，并对成功进行地面校正的变换矩阵经恢复得到原始变换，从而得到了使原图像F_s、F_t三维匹配点集对齐的刚体变换估计T₁、T₂；

(8)计算T₁、T₂二者对应的配准变换误差e_i，误差较小者即为最佳的里程计运动变换T^*。

2.根据权利要求1所述的一种室内环境下考虑地面约束的RGB-D视觉里程计方法，其特征在于：所述步骤(3)具体包括以下子步骤：

(3.1)RGB图像快速oFAST角点检测；

(3.2)深度异常特征点剔除；

根据每个oFAST特征点的二维坐标查找深度图像，获取对应的深度信息；根据深度数据有效性阈值判定深度有效性，将不满足阈值要求的FAST角点从特征点集合中剔除，保留符合要求的深度有效点用于ORB特征检测算法后续BRIEF描述子的构建；

(3.3)具有旋转不变性的BRIEF描述子构建；

(3.4)采用Hamming距离D(K₁,K₂)作为特征描述子相似性度量函数，完成特征点的匹配，得到三维对应点集合Fs和Ft：

$D(K_1,K_2)=\underset{i=0}{\overset{255}{\Σ}}{x}_i\⊕y_i$

其中，K₁＝x₀x₁x₂…x₂₅₅，K₂＝y₀y₁y₂…y₂₅₅分别为相邻帧图像对应特征点的ORB特征描述子。

3.根据权利要求1所述的一种室内环境下考虑地面约束的RGB-D视觉里程计方法，其特征在于：所述所述步骤(4)具体包括以下子步骤：

(4.1)利用步骤(2)生成的环境三维点云数据为输入，利用特征值法求解平面参数a，b，c的初始值；特征值法空间平面方程称为法线式方程为：

ax+by+cz＝d

其中，(a,b,c)为平面的单位法向量，满足条件a²+b²+c²＝1；(x,y,z)为平面上点的空间坐标；d为坐标原点到平面的距离，d≥0；

(4.2)根据a，b，c的初始值计算每个样本点到初始拟合平面的距离d_i：

$d_i=|a(x_i-\stackrel{\‾}{x})+b(y_i-\stackrel{\‾}{y})+c(z_i-\stackrel{\‾}{z})|$

(4.3)计算标准差：其中

(4.4)判断d_i与2σ的大小关系：若d_i＞2σ，i点被当作异常点剔除；反之，保留；

(4.5)重复步骤(4.2)到步骤(4.4)，直到所有异常点均被剔除，计算获得最优平面参数a，b，c，d，得到地平面的平面拟合方程；

(4.6)利用得到的法向量作为参考信息，采用ISODATA算法对疑似地面点云数据进行法向聚类，将法向接近的点云数据分类到一起，得到地面点云的第一次聚类结果；然后计算点云集各点到拟合平面的距离，将小于设定阈值的点予以保留，得到第二次聚类结果，完成对地面的精确检测，并计算得到地平面点集重心m。

4.根据权利要求1所述的一种室内环境下考虑地面约束的RGB-D视觉里程计方法，其特征在于：所述步骤(4)的运动变换误差利用关联点集F_s、F_t的欧式距离计算：

$E(T,F_s,F_t)=\frac{1}{N}\underset{i=1}{\overset{N}{\Σ}}\left|\right|T\left(p_i\right)-q_i|{|}^2.$