CN110223348B - 基于rgb-d相机的机器人场景自适应位姿估计方法 - Google Patents

Abstract

本发明公开了一种基于RGB‑D相机的机器人场景自适应位姿估计方法，基于RGB‑D相机获取的相邻帧的场景二维彩色图像信息和按二维彩色图像像素对应的空间深度信息，在二维彩色图像特征点充足时，采用ORB算子提取特征，采用本发明提出的匹配策略进行精确地匹配，基于匹配特征点的位姿估计算法求解三维位姿估计；在特征点不足时，采用本发明提出的改进ICP算法求解三维位姿估计；然后，本发明设计出完整的切换准则融合上述两种位姿估计方法；最后，本发明采用光束平差算法优化以上两种方法求得的位姿估计，最终得到平滑和准确的三维位姿估计；本发明的三维位姿估计算法具有鲁棒性高、精度高、计算量小、可适应不同场景等突出优点。

Description

基于RGB-D相机的机器人场景自适应位姿估计方法

技术领域

本发明属于机器人控制领域，特别涉及一种基于RGB-D相机的机器人场景自适应位姿估计方法。

背景技术

实时的鲁棒的高精度三维位姿估计是机器人领域的研究难点和热点之一，其目标是实时估计出相邻两相邻时刻机器人三维空间位姿变化量，是机器人SLAM(实时定位和地图创建)，运动跟踪，AR(增强现实)等领域的核心内容。传统基于惯性传感器的导航系统现被广泛应用于位姿估计，但是其存在漂移、误差累计等问题，因此其对应位姿估计的精度低、可靠性较差。相对于惯性导航系统，基于视觉的位姿估计不存在物理漂移问题，且通过全局范围的视觉闭环检测可有效消除累积误差，具有较高的稳定性和精确度。目前，基于单目或双目的纯视觉的位姿估计计算量小，精度高，但是绝对尺度信息获取不稳定，且要求场景特征丰富，在特征不明显的场景(如走廊，玻璃墙)容易丢失；基于三维激光测距仪的位姿估计能直接有效获取深度信息和绝对尺度信息，利用三维场景深度信息通过基于ICP的三维点云匹配可进行有尺度的位姿估计，但是由于噪声、误差和不匹配区域比例的增长，ICP迭代算法容易陷入局部优化解，从而该类算法仅在较小空间变换范围内或基于较好的初始估计情况下可获得准确的估计结果。为了实现精确可靠的三维位姿估计，有研究者采用高频率采样获取相邻帧对应的三维信息，从而将相邻帧空间变换限制在较小范围，但是，高频率采样必然导致计算量巨大，难以满足实时性，并且三维激光测距仪硬件成本昂贵，重量大，不利于搭载到轻便的小型机器人身上。

一方面，基于纯视觉的方法的能获取丰富图像信息且求解速度快，但尺度信息无法计算；另一方面，基于深度信息方法能获取绝对尺度信息且精确度高，但计算量大且价格昂贵。因此，融合好二者的优点是提高三维位姿估计可靠性的解决思路。近年来，随着以KINECT为代表的RGB-D相机的市场化，能同时获取二维彩色图像信息和三维深度信息，为两种运动方法的融合提供了硬件支持和实践的便利性。基于此有研究者提出利用相邻两帧之间图像的不变的特征点，通过深度信息读取特征点的深度信息，构成三维点集来求解三维运动的初始估计，以提高ICP算法的估计性能。但是该类方法的缺陷在于严重依赖于特征点选择，首先特征点集中的误匹配点对将对三维估计初始值造成严重影响；其次，该算法仅对图像特征点较多的场景有效，若场景特征点稀疏则容易跟踪失败或误差极大，这都会直接导致算法失败，从而无法获得稳定的位姿估计。在实际的机器人操作环境中，有的区域场景特征丰富，有的区域场景特征稀疏，单一考虑的位姿估计方法难以奏效。

综上所述，如何充分利用三维视觉系统(如KINECT)获取的二维彩色图像信息和三维深度信息，在不同场景下满足机器人的实际需求，鲁棒的实时获取精确的三维运动信息，是本领域急需解决的技术问题。

发明内容

本发明所要解决的技术问题是提供一种基于RGB-D相机的机器人场景自适应位姿估计方法。RGB-D相机获取的相邻帧的场景二维彩色图像信息和按二维彩色图像像素对应的空间深度信息，在二维彩色图像特征点充足时，采用ORB算子提取特征，采用本发明文提出的匹配策略进行精确地匹配，基于匹配特征点的位姿估计算法求解出相对位姿变换；在特征点不足时，采用本发明提出的改进ICP算法求解相对位姿变换；并且，本发明设计出完整的切换准则融合上述两种位姿估计的求解方法。

一种基于RGB-D相机的机器人场景自适应位姿估计方法，包括以下步骤：

步骤1：RGB-D相机分别在t时刻和t+1时刻获取相邻帧场景的二维彩色图像I_t和I_t+1、彩色图像对应的三维点云D_t和D_t+1；

所述RGB-D相机可同时获取场景的二维彩色图像I_t与三维点云D_t，其中二维彩色图像I与三维点云D按二维彩色图像像素点一一对应，即二维彩色图像中第u行、第v列的像素点I_t(u,v)与三维点云中对应的三维点D_t(u,v)(x,y,z)相对应，三维点D_t(u,v)(x,y,z)是指二维彩色图像中第u行、第v列的像素点对应的深度信息；所述三维点云D_t是指二维彩色图像所有像素点对应的三维点集合。

步骤2：分别提取二维彩色图像I_t和I_t+1的ORB特征点集合

和

得到相邻帧场景的粗匹配特征点对集合M₀；

步骤3：采用对称性匹配策略和RANSAC算法，对粗匹配特征点对集合M₀中的误匹配特征点对进行剔除，得到精确的二维匹配点对集合M_t,t+1；

步骤4：从精确的二维匹配点对集合M_t,t+1中，选取符合匹配关系且具有有效深度信息的特征点对{P_i ^t(x,y,z),P_i ^t+1(x,y,z)}，其中i∈{1,…,N_match}，N_match为具有有效深度信息的三维匹配点对数；

步骤5：判断N_match是否大于求解位姿估计所需要的最小匹配特征点对数ε_match，若大于，执行基于特征匹配的三维位姿估计算法，得到位姿估计T_last和二维彩色图像内点数I_last，进入步骤6；否则，进入步骤7；

其中，ε_match取值为10；

所述ε_match指采用RANSAC的SVD分解法对N_match中所有点对进行求解的最小值。理论上的数值是4，考虑到传感器测量误差，经过实验对比，设为10。

所述基于特征匹配的三维位姿估计算法是指基于RANSAC的SVD分解法求取位姿估计。

步骤6：依据A_T判断求得的位姿估计T_last是否有效：若A_T≥ε_T成立，则基于特征匹配的三维位姿估计求得的位姿估计T_last有效，进入步骤8，否则，进入步骤7；

其中，A_T表示RANSAC位姿估计精度，是指内点总数N_I与内点总数的均方根误差E_I的比值，ε_T为判断位姿估计是否有效所设定的阈值；

步骤7：利用步骤1获得的三维点云D_t和D_t+1执行改进的ICP位姿估计，得到位姿估计T_last；

步骤8：利用步骤1-7获取的位姿估计，通过最小化重投影误差进行BA优化，获得平滑后的位姿估计。

进一步地，所述利用步骤1获得的三维点云D_t和D_t+1执行改进的ICP位姿估计，得到位姿估计T_last的过程如下：

7.1：提取三维点云D_t和D_t+1中点的法向量；

步骤如下：

设三维点云D_t中的一个点为P＝(x_p,y_p,z_p)^T，且以该点为中心内核半径为r的邻域内的点的数量为N，则其邻域内点的集合为{p_i(x_i,y_i,z_i)}，i＝{1,2，…,N}，

根据主成分分析法，其质心

和协方差矩阵∑_p分别为：

假设由邻域内点集合拟合的平面为S，则该平面的法向量即为矩阵Σ_p的最小特征值所对应的特征向量，设该特征向量为n，即法向量为n，若平面S为最佳拟合平面，则有：

其中，d表示拟合平面与坐标原点间的距离，最小化目标函数可得到最佳拟合平面S(n,d)。

步骤7.2：对三维点云D_t和D_t+1进行双边滤波处理，得到滤波后的三维点云D_t′和D_t+1′；

步骤如下：

对于步骤7.1中三维点云D_t和D_t+1中的点P＝(x_p,y_p,z_p)^T以及其对应的法向量n，点P为中心内核半径为r的邻域内的点的数量为N，设点P邻域内点的集合为KNN(P)，表达式为{P_i(x_i,y_i,z_i)}，i＝{0,1,…,N}；

定义双边滤波为：

P′＝P+αn

其中，P′为滤波后的新点，α为双边滤波因子，具体表达如下：

其中，<n-n_i>为向量的点积，n_i为集合KNN(P)中点P_i的法向量，ω₁，ω₂分别是空间域权重和特征域权重，表达式如下：

其中，参数σ₁为采样点P的邻域半径，σ₂为邻域内点的标准偏差；

σ₁为邻域半径r,σ₂为领域中所有点与P的标准偏差；

循环三维点云中每一个点后，得到滤波后的三维点云D_t′和D_t+1′；

步骤7.3：三维点云D_t′和D_t+1′的降采样；

步骤如下：

首先，利用步骤7.1中的法向量提取方法，提取三维点云D_t′和D_t+1′中所有点的法向量；

然后，根据三维点云中点的法向量n与x、y和z轴的夹角，取间隔角度45度，将所有点划分为4³个类，其中三个维度上所含类的个数均为4；

最后，随机地从每个类中该类中的1％个点，舍去其它点，得到降采样后的三维点云{p_i}和点云{q_j}；

实测某一原始点云个数为325200，降采样后点云个数为3252，压缩率为1％。

步骤7.4：对于步骤7.3获得的点云{p_i}和点云{q_j}，采用FLANN搜寻方法遍历的搜寻点云{p_i}在点云{q_j}中的最近邻点，建立点云{p_i}和点云{q_j}之间的映射关系β_k：

其中，k为迭代次数，k的最大值k_max＝20，β_k为经k次迭代后计算出来的映射关系，T_k-1为上一次迭代计算后求得的位姿估计，初始值k＝1，T₀为前一帧图像的位姿估计；

步骤7.5：根据步骤7.4中建立的映射关系β_k，采用基于对应点间的中值距离的剔除法和基于表面法向量夹角的剔除法，剔除错误关系匹配点对；

剔除步骤为：

首先，遍历映射β_k中所有匹配点对，若

剔除此匹配点对；

然后，遍历映射β_k中所有匹配点对，求出匹配点对距离的中值距离d_mid；

再次，遍历映射β_k中所有匹配点对，若

剔除此匹配点对；

其中，法向量

表示点p_i在点云{p_i}表面的法向量，法向量

表示点q_j在点云{q_j}表面的法向量，

表示法向量

和法向量

的夹角，角度阈值θ_threshold经过实验对比设为1°；

表示点p_i和点q_j之间欧式距离的绝对值，距离阈值d_threshold等于中值距离d_mid；

步骤7.6：采用基于点到面的距离作为目标函数，通过调整变换矩阵T使得剩余匹配点对之间距离的平方和最小化，求得T_k；

其中，{p_i,q_i′}是通过步骤7.5剔除错误关系匹配点对后，得到剩余匹配点对集合，i＝(1,...,n)；ω_i为权值，

表示p_i与q_i′中深度值较大的点所对应的深度值；

步骤7.7：当T_k-T_k-1＞T_threshold或k>＝k_max满足时，迭代终止，令T_last＝T_k，得到当前帧图像的位姿估计T_last，若不满足，令k＝k+1，返回步骤7.4。

每次迭代变化的是映射β_k。因为T_k-1在变，映射就会发生变化。

进一步地，当连续获得K帧图像的位姿估计后，采用光束平差法对当前帧图像在内的前M帧图像的位姿估计进行优化，M_w取值为10。

控制优化的规模以达到实时目的。

进一步地，所述采用光束平差法对当前帧图像在内的前M帧图像的位姿估计进行优化的具体过程如下：

步骤8.1：获取K帧新图像，采用步骤1-7所述方法求取最新获得的第K-M+1帧到最新获得的第K帧图像中每一帧图像对应的位姿估计，并获得对应连续M帧图像的位姿估计集合T＝[T₁,…T_j]，j＝(1,..,M)；

假设连续的M帧图像可以同时观测到N个ORB特征点，Q_i表示第i个ORB特征点对应的中的空间三维点，T_j表示第j帧图像对应的位姿估计，P(T_j,Q_i)表示空间三维点Q_i经过T_j变换，投影到图像上二维成像点；q_ij表示第i个ORB特征点对应的空间三维点在第j帧图像上的成像点；

步骤8.2：构建目标函数

并计算目标函数的雅克比矩阵

以及信息矩阵A＝JJ^T；

其中，ω_ij表示指示参数，若第i个空间三维点能够被第j帧图像观测到，则ω_ij＝1，反之，ω_ij＝0；

步骤8.3：求出更新矩阵δ_T；

通过步骤8.2求出的雅克比矩阵J和信息矩阵A，结合方程(A+uI)δ_T＝J^Tε，求解出更新矩阵δ_T；

其中，更新矩阵δ_T表示对T的变分，u为阻尼参数且u＝10^-3，ε＝q_ij-P(T_j,Q_i)；

步骤8.4：若||δ_T||＜ε，则停止迭代，且输出本次迭代的结果δ_T，转到步骤8.5；否则令u′＝u/10，转到步骤8.2继续计算；

步骤8.5：更新位姿估计集合T′＝T+δ_T，则得到相机新的估计投影点P′(T_j,Q_i)；如果

ε_T表示迭代终止阈值常量，则认为达到终止条件，输出最终的连续M帧图像位姿估计集合T＝T′，输出T；否则，令u＝10u，P(T_j,Q_i)＝P′(T_j,Q_i)，跳转到步骤8.2。

进一步地，所述基于特征匹配的三维位姿估计算法是指结合RANSAC的SVD分解法求取位姿估计，具体步骤如下：

步骤a：设最大迭代次数为k_max＝20，当前迭代次数为k，且初始值为1，最终位姿估计T_last和最终内点集合I_last为空，设初始内点集合对应的均方根误差E₀＝999；

步骤b：从步骤4中获取的N_match对有效特征点对{P_i ^t(x,y,z),P_i ^t+1(x,y,z)}中随机选取n对匹配点对{P_pi,Q_qi}，i＝(1,..,n)，利用SVD算法计算出位姿估计T_k和内点集合I_k，以及内点集合I_k对应的内点个数N_k；

步骤c：根据位姿估计T_k，将n对匹配点对{P_pi,Q_qi}中源特征点集P_pi投影到目标特征点集Q_qi，计算投影后的误差e_p：

e_p＝|T_kP_pi-Q_qi|

若误差e_p大于设定的阈值，则剔除该匹配点对，否则保留；遍历n对点，得到新的匹配点对{P_p′_i,Q_q′_i}，根据新的匹配点对{P_p′_i,Q_q′_i}获取新的内点集合I_k′，以及内点个数N_k′，并计算内点的比例N_R＝N_k′/N_k；

步骤d：假如内点的比例N_R小于内点比例设定阈值，则认为该位姿估计T_k是无效，返回步骤b；否则，认为位姿估计T_k初步满足要求；

然后，通过新的内点集合I_k′利用SVD算法重新计算位姿估计T_k′，并结合有效特征点对{P_p′_i,Q_q′_i}，计算I_k′对应的均方根误差E_k：

其中，N_n为有效特征点对{P_p′_i,Q_q′_i}中的对数；

步骤e：若E_k＞E_k-1，说明T_k′优于T_k′_-1，此时，更新位姿估计T_last＝T_k′，内点集合I_last＝I_k′和内点个数N_I＝N_k′，均方根误差E_I＝E_k；否则，令k＝k+1，重复步骤b到步骤d，直到达到最大迭代次数k_max，结束计算。

进一步地，所述步骤3中利用粗匹配特征点对M₀，剔除其中误匹配点对，最终得到相邻场景精确的二维匹配点对集合M_t,t+1的具体步骤如下：

3.1基于对称性匹配策略剔除误匹配；

对t+1时刻的二维彩色图像I_t+1的任一图像特征点

依次从t时刻的二维彩色图像I_t的图像特征点中找出基于汉明距离的最近邻匹配的特征点F_t ⁱ，构成匹配特征点对集合M₀′，M₀和M₀′的交集构成新的匹配图像特征点对集合M₁，剔除非对称的误匹配图像特征点对；

3.2采用RANSAC算法对匹配特征点对集合M₁中的误匹配进一步剔除，得到最终的匹配特征点对M₀点对集合M_t,t+1。

进一步地，所述步骤2中利用ORB特征获取粗匹配M₀的具体步骤如下：

步骤2.1：对t时刻和t+1时刻获取的相邻场景的二维彩色图像I_t和I_t+1，利用ORB算法分别提取旋转不变特征点集合F_t ⁱ和

步骤2.2：对F_t的任一特征点F_t ⁱ，从t+1时刻获取的二维彩色图像特征点F_t+1中，依次找出与F_t ⁱ匹配的特征点

构成匹配特征点对

步骤2.3：遍历F_t中所有特征点：对F_t中所有特征点执行步骤2.2，得到粗匹配图像特征点对集合M₀。

有益效果

本发明可实现机器人实时位姿估计，具有鲁棒性高，计算量小，可适应不同场景等优点，有效满足了机器人自定位、地图创建、三维运动跟踪、增强现实、虚拟现实等应用领域中对三维运动信息的迫切需求。

与现有三维位姿估计方法相比，本发明的先进性表现在：

1，采用可同时获取二维彩色图像信息和深度信息的市场化传感器，提高了信息获取的便利性，进一步体现了本发明提出的算法的实践性；

2，本发明提出了基于特征匹配的三维位姿估计算法，和传统的SHIT或SURF算子相比，采用速度更快ORB的算子，并且采用结合RANSAC的SVD分解法求取位姿估计，比经典的求解更加稳定，容错率更高；

3，本发明提出了改进的ICP算法求解位姿估计，首先预处理部分先后进行双边滤波和采用基于PCA方法进行降采样，提高了深度点准确度的同时减少了后面迭代过程的计算量，并且，和传统采用点到点的距离作为目标函数度量方法不同，本发明采用点到面的距离作为目标函数的度量，收敛速度和精确度远高于经典ICP算法；

4，本发明设计出完整的方案去融合基于特征匹配和基于ICP算法两种三维位姿估计方法，使得本发明整体的位姿估计方法精确度高，鲁棒性高，可适应于特征点单一或丰富等不同场景的需求；

5，本发明在位姿估计的后端加入了局部优化部分(Local Bundle Adjustment，LBA),采用固定窗口的形式去控制优化的帧数，提高优化速度的同时也消除了局部误差。

附图说明

图1为本发明整体流程图；

图2为原始粗匹配和剔除误匹配后的精确匹配对照图，其中，图(a)为原始粗匹配时的示意图，图(b)为经过对称性匹配策略和RANSANC剔除误匹配后的精确匹配示意图；

图3为本发明提出的改进ICP算法中点云预处理过程示意图，其中，图(a)为双边滤波前后的彩色点云图，(b)图为降采样前后的点云图；

图4为基于特征匹配的三维位姿估计实验效果图，其中，图(a)为机器人运行轨迹，图(b)显示了实验场景中采集到的部分图片，图(c)表示将每两帧对应的相对位姿估计转换到全局坐标系下得到最终的运动轨迹；

图5为改进的ICP算法求解三维位姿估计实验效果图，其中，图(a)为机器人运行轨迹以及起始位置处的彩色点云，图(b)为其中两帧点云配准前后状态的对比(图中分别用红色和绿色表示)，图(c)显示了这两组点云在20次迭代过程中误差的衰减曲线；

图6为融合基于特征匹配和ICP算法两种三维位姿估计实验效果图，其中，图(a)为机器人直线运行时采集到的部分图像，该场景单一并且特征稀少，图(b)为对应的运动轨迹如，图(c)为对应的三维地图；

图7局部优化(LBA)实验效果图，其中，图(a)为实验获取的轨迹对比图，图(b)为对应的三维地图，(c)为优化前后关键帧之间的相对位姿估计的误差曲线。

具体实施方式

下面将结合附图和实施例对本发明做进一步的说明。

所述RGB-D相机可同时获取场景的二维彩色图像I与三维点云D，其中二维彩色图像I_t(u,v)与三维点云D_t(u,v)按二维彩色图像像素点一一对应，即二维彩色图像中第u行、第v列的像素点I_t(u,v)与三维点云D_(u,v)(x,y,z)相对应，三维点云D_(u,v)(x,y,z)是指二维彩色图像中第u行、第v列的像素点的深度信息；所述三维点云D是指二维彩色图像所有像素点对应的三维空间点集合。

如图1所示，为本发明的流程图，一种基于RGB-D相机的机器人场景自适应位姿估计方法，包括以下步骤：

步骤1：RGB-D相机分别可在在t时刻和t+1时刻获取相邻帧场景的二维彩色图像I_t和I_t+1、彩色图像对应的三维点云D_t和D_t+1。

步骤2：图像I_t和I_t-1分别提取ORB特征，得到相邻帧场景的匹配图像特征点对集合M；

ORB是一种图像局部特征描述算子，提取特征的速度极快，对平移、旋转、尺度缩放等具有良好的不变性。ORB采用了oFAST算法进行特征检测，oFAST在FAST算子的基础上，采用尺度金字塔进行了优化，因而可以获得具有尺度信息的FAST特征检测算子，并且使用灰度质心法给关键点添加了方向信息，从而弥补了FAST算子不具有旋转不变性的缺陷。

ORB特征采用rBRIEF算法作为特征描述子，rBRIEF算法在BRIEF的基础上添加了方向信息，补了BRIEF算法不具备旋转不变性的缺陷，其主要思想是在特征点周围选取若干对特征点，将这些点的灰度值组合成一个二进制的串，该二进制串即可作为该特征点的特征描述子；具体方法见参考文献1。

步骤3：剔除集合M₀中的误匹配图像特征点对，采用对称性匹配策略和RANSAC算法进行误匹配剔除，得到最终获取的精确的二维匹配点对M_t,t+1；如图2，a图为未经剔除的特征点匹配对M₀，b图为经过本发明提出的剔除误匹配对后的结果，分别为基于对称性匹配策略剔除和基于RANSAC算法的剔除，两种剔除方法步骤如下：

3.1基于对称性匹配策略剔除误匹配；

对t+1时刻的二维彩色图像I_t+1的任一图像特征点

依次从t时刻的二维彩色图像I_t的图像特征点中找出基于汉明距离的最近邻匹配的特征点F_t ⁱ，构成匹配特征点对集合M₀′，M₀和M₀′的交集构成新的匹配图像特征点对集合M₁，剔除非对称的误匹配图像特征点对；

3.2采用RANSAC算法对匹配特征点对集合M₁中的误匹配进一步剔除，得到最终的匹配特征点对集合M_t,t+1；

对于场景中提取的相邻两幅图像，它们之间应当满足极线几何约束，因此，当两幅图像中的特征点进行匹配时，仅仅接受落在对应极线中的匹配点，这样就可以进一步剔除大量误匹配点。步骤如下：

步骤a:随机选择M₁中的s_M组匹配特征点对组成随机样本集M₂，并基于该样本计算匹配模型H；即使得匹配特征点

对满足如下方程：

其中

步骤b:用步骤a中计算的模型H，对M₁未选中的其它样本数据进行测试，若

则该特征点对适用于估计的模型H，并称之为模型H的内点，放入M₂；否则认为它是外点；其中

表示特征点

相对于模型H的映射点

和匹配对应点

的欧式距离，T_d为距离阈值，*表示矩阵乘运算；

步骤c:重复k次采样，选择内点数目最多的集合M₂；

其中，ε为M₁中的误匹配点对占所有匹配点对的比例，p为设定的匹配成功概率期望值，s为采样数目，s＞＝4；

步骤d:利用M₂中的样本数据重新估计计算模型参数H；

步骤e:利用模型H重新测试样本集M₁，确定新的集合M₂；反复执行步骤d,步骤e，直至M₂中的匹配点对数目前后相差不超过5％-10％为止，这时令M_(t,t+1)＝M₂。

步骤4：在步骤3求出的精确二维匹配点对M_t,t+1，对于其中所有符合匹配关系的特征点，在步骤1中获取的三维点云D_t和D_t+1中，索引其对应的深度信息以获取特征点的有效三维信息，分别设为P_i ^t(x,y,z)和P_i ^t+1(x,y,z)，得到有效特征点对{P_i ^t(x,y,z),P_i ^t+1(x,y,z)}，其中i∈{1,…,N_match}，N_match为具有有效深度信息的三维匹配点对数。

步骤5：判断步骤4中得到的N_match是否大于结合RANSAC的SVD分解法求解位姿估计所需要的最小匹配特征点对数ε_match(理论上至少需要4对不共线的匹配点，为了提高鲁棒性和精确度，本发明将ε_match设为10)，若大于，说明场景特征点充足，进入步骤6；否则，说明场景特征点不足，执行改进的ICP(Iterative Closest Point，迭代最近点)位姿估计算法得到T_last，此时转到步骤8。

步骤6：在场景特征点充足的情况下，结合RANSAC的SVD分解法求取位姿估计T_last。

步骤如下：

步骤6.1：设最大迭代次数为k_max＝20，当前迭代次数为k，且初始值为1，最终位姿估计T_last和最终内点集合I_last为空，设初始内点集合对应的均方根误差E₀＝999；

步骤6.2：从步骤4中获取的N_match对有效特征点对{P_i ^t(x,y,z),P_i ^t+1(x,y,z)}中随机选取n对匹配点对{P_pi,Q_qi}，i＝(1,..,n)，利用SVD算法计算出位姿估计T_k和内点集合I_k，以及内点集合I_k对应的内点个数N_k；

步骤6.3：根据位姿估计T_k，将n对匹配点对{P_pi,Q_qi}中源特征点集P_pi投影到目标特征点集Q_qi，计算投影后的误差e_p：

e_p＝|T_kP_pi-Q_qi|

若误差e_p大于设定的阈值，则剔除该匹配点对，否则保留；遍历n对点，得到新的匹配点对{P_p′_i,Q_q′_i}，根据新的匹配点对{P_p′_i,Q_q′_i}获取新的内点集合I_k′，以及内点个数N_k′，并计算内点的比例N_R＝N_k′/N_k；

步骤6.4：假如内点的比例N_R小于阈值，则认为该位姿估计T_k是无效，返回步骤b；否则，认为位姿估计T_k初步满足要求。

然后，通过新的内点集合I_k′利用SVD算法重新计算位姿估计T_k′，并结合有效特征点对{P_p′_i,Q_q′_i}，计算I_k′对应的均方根误差：

其中，N_n为有效特征点对{P_p′_i,Q_q′_i}中的对数。

步骤6.5：若E_k＞E_k-1，说明T_k′优于T_k′_-1，此时，更新位姿估计T_last＝T_k′，内点集合I_last＝I_k′和内点个数N_I＝N_k′，均方根误差E_I＝E_k；否则，令k＝k+1，重复步骤6.2到步骤6.4，直到达到最大迭代次数k_max，结束计算。

步骤7：判断步骤6得到的T_last是否有效。具体步骤为:设A_T表示T_last位姿估计的精度：

其中内点个数N_I和其对应的均方根误差E_I为步骤6中数据。判断A_T≥ε_T是否成立，其中，ε_T为判断位姿估计是否有效所设定的阈值，若有效，说明此时的位姿估计T_last精度较好，此时跳转到步骤9，否则，说明此时的位姿估计T_last精度不够好，此时执行步骤8。

步骤8：将相邻场景二维彩色图像I_t和I_t+1对应的深度图像转换成三维点云D_t和D_t+1、然后依次对点云进行滤波、降采样以及法向量提取等预处理，然后采用本发明提出的改进的ICP迭代计算，最后判断迭代过程是否满足终止条件，若满足则获取相对位姿估计T_last，否则继续执行ICP迭代；

改进的ICP算法步骤如下：

步骤8.1：提取三维点云D_t和D_t+1中点的法向量；

设三维点云D_t中的一个点为P＝(x_p,y_p,z_p)^T，且以该点为中心内核半径为r的邻域内的点的数量为N，则其邻域内点的集合为{p_i(x_i,y_i,z_i)}，i＝{1,2，…,N}，

根据主成分分析法，其质心

和协方差矩阵∑_p分别为：

假设由邻域内点集合拟合的平面为S，则该平面的法向量即为矩阵Σ_p的最小特征值所对应的特征向量，设该特征向量为n，即法向量为n，且已被单位化，若平面S为最佳拟合平面，则有：

其中，d表示拟合平面与坐标原点间的距离，最小化目标函数可得到最佳拟合平面S(n,d)。

步骤8.2：对三维点云D_t和D_t+1进行双边滤波处理，得到滤波后的三维点云D_t′和D_t+1′；

步骤如下：

对于步骤8.1中三维点云D_t和D_t+1中的点P＝(x_p,y_p,z_p)^T以及其对应的法向量n，点P为中心内核半径为r的邻域内的点的数量为N，设点P邻域内点的集合为KNN(P)，表达式为{P_i(x_i,y_i,z_i)}，i＝{0,1,…,N}；

定义双边滤波为：

P′＝P+αn

其中，P′为滤波后的新点，α为双边滤波因子，具体表达如下：

其中，<n-n_i>为向量的点积，n_i为集合KNN(P)中点P_i的法向量，ω₁，ω₂分别是空间域权重和特征域权重，表达式如下：

其中，参数σ₁为采样点P的邻域半径，σ₂为邻域内点的标准偏差；

σ₁为邻域半径r,σ₂为领域中所有点与P的标准偏差；

循环三维点云中每一个点后，得到滤波后的三维点云D_t′和D_t+1′；

步骤8.3：三维点云D_t′和D_t+1′的降采样；

步骤如下：

首先，利用步骤8.1中的法向量提取方法，提取三维点云D_t′和D_t+1′中所有点的法向量；

然后，根据三维点云中点的法向量n与x、y和z轴的夹角，取间隔角度45度，将所有点划分为4³个类，其中三个维度上所含类的个数均为4；

最后，随机地从每个类中该类中的1％个点，舍去其它点，得到降采样后的三维点云{p_i}和点云{q_j}；

实测某一原始点云个数为325200，降采样后点云个数为3252，压缩率为1％。

步骤8.4：对于步骤8.3获得的点云{p_i}和点云{q_j}，采用FLANN搜寻方法遍历的搜寻点云{p_i}在点云{q_j}中的最近邻点，建立点云{p_i}和点云{q_j}之间的映射关系β_k：

其中，k为迭代次数，k的最大值k_max＝20，β_k为经k次迭代后计算出来的映射关系，T_k-1为上一次迭代计算后求得的位姿估计，初始值k＝1，T₀为前一帧图像的位姿估计；

步骤8.5：根据步骤8.4中建立的映射关系β_k，采用基于对应点间的中值距离的剔除法和基于表面法向量夹角的剔除法，剔除错误关系匹配点对；

剔除步骤为：

首先，遍历映射β_k中所有匹配点对，若

剔除此匹配点对；

然后，遍历映射β_k中所有匹配点对，求出匹配点对距离的中值距离d_mid；

再次，遍历映射β_k中所有匹配点对，若

剔除此匹配点对；

其中，法向量

表示点p_i在点云{p_i}表面的法向量，法向量

表示点q_j在点云{q_j}表面的法向量，

表示法向量

和法向量

的夹角，角度阈值θ_threshold经过实验对比设为1°；

表示点p_i和点q_j之间欧式距离的绝对值，距离阈值d_threshold等于中值距离d_mid；

步骤8.6：采用基于点到面的距离作为目标函数，通过调整变换矩阵T使得剩余匹配点对之间距离的平方和最小化，求得T_k；

其中，{p_i,q_i′}是通过步骤8.5剔除错误关系匹配点对后，得到剩余匹配点对集合，i＝(1,...,n)；ω_i为权值，

表示p_i与q_i′中深度值较大的点所对应的深度值；

步骤8.7：当T_k-T_k-1＞T_threshold或k>＝k_max满足时，迭代终止，令T_last＝T_k，得到当前帧图像的位姿估计T_last，若不满足，令k＝k+1，返回步骤8.4。

每次迭代变化的是映射β_k。因为T_k-1在变，映射就会发生变化。

步骤9：利用步骤1-7获取的位姿估计，通过最小化重投影误差进行BA优化，获得平滑后的位姿估计。步骤如下：

步骤9.1：获取K帧新图像，采用步骤1-7所述方法求取最新获得的第K-M+1帧到最新获得的第K帧图像中每一帧图像对应的位姿估计，并获得对应连续M帧图像的位姿估计集合T＝[T₁,…T_j]，j＝(1,..,M)；

令连续的M帧图像可以同时观测到N个ORB特征点，Q_i表示第i个ORB特征点对应的中的空间三维点，T_j表示第j帧图像对应的位姿估计，P(T_j,Q_i)表示空间三维点Q_i经过T_j变换，投影到图像上成像点；q_ij表示第i个ORB特征点对应的空间三维点在第j帧图像上的成像点；

步骤9.2：构建目标函数

并计算目标函数的雅克比矩阵

以及信息矩阵A＝JJ^T；其中，ω_ij表示指示参数，若第i个空间三维点能够被第j帧图像观测到，则ω_ij＝1，反之，ω_ij＝0；

步骤9.3：求出更新矩阵δ_T；通过步骤8.2求出的雅克比矩阵J和信息矩阵A，结合方程(A+uI)δ_T＝J^Tε，求解出更新矩阵δ_T；其中，更新矩阵δ_T表示对T的变分，u为阻尼参数且u＝10-³，ε＝q_ij-P(T_j,Q_i)；

步骤9.4：若||δ_T||＜ε，则停止迭代，且输出本次迭代的结果δ_T，转到步骤8.5；否则令u′＝u/10，转到步骤8.2继续计算；

步骤9.5：更新位姿估计集合T′＝T+δ_T，则得到相机新的估计投影点P′(T_j,Q_i)；如果

其中，ε_T表示迭代终止阈值常量，则认为达到终止条件，输出最终的连续M帧图像位姿估计集合T＝T′，输出T；否则，令u＝10u，P(T_j,Q_i)＝P′(T_j,Q_i)，回到步骤9.2。

步骤10：若机器人系统运动结束，无需对机器人位姿运动进行估计，退出三维位姿估计；否则，转至步骤1重复继续进行与下一个相邻帧的三维位姿估计。

本发明的试验结果与对比效果如图4-图7所示。

如图4，为验证本发明提出的基于特征匹配的三维位姿估计，所选实验场景为湖南大学除冰实验室，特征点比较丰富。其中，图(a)显示了机器人运行轨迹，图(b)显示了实验场景中采集到的部分图片，其中机器人是沿着红色条带寻线运行的，实验中共采集到320帧图像。从图中可以直观地看到，该场景具有相对充足的特征点，因此这里采用基于特征点的位姿估计方法，首先对图像提取ORB特征，然后经过特征匹配、剔除误匹配后采用结合RANSAC的SVD分解算法求解位姿估计。

将每两帧对应的相对位姿估计转换到全局坐标系下得到最终的运动轨迹，如图(c)所示，其中“*”表示图像所对应的坐标位置，图中仅显示关键帧所在位置。该轨迹所对应的绝对误差的均方根为ATE_RMSE＝0.33，且平均耗时为0.039秒，基本满足实时的要求。由此可见，对于该场景单独采用基于特征的方法能够完整且相对有效地求解出位姿估计，但计算得到的轨迹与真实轨迹存在一定的误差。

如图5，为验证本发明提出的改进的ICP算法求取三维位姿估计，所选试验场景为湖南大学机器人实验室的转弯部分作为实验场景，仅采用深度数据进行试验。其中，机器人运行轨迹以及起始位置处的彩色点云如图(a)所示，实验共采集了18帧图像，获取对应的深度图像信息。图(b)显示了其中两帧点云配准前后状态的对比(图中分别用红色和绿色表示)，由图可知，配准后两帧点云得到较为完整的融合(从图中蓝色椭圆和紫色椭圆处可明显观察到)，图(c)显示了这两组点云在20次迭代过程中误差的衰减曲线，由图可知，误差在前几次迭代过程中衰减较快，经过后面若干次迭代后缓慢地趋于稳定，且最终误差值降低到0.011。图(d)显示了本文改进的ICP算法对18帧点云的配准结果，表(Ⅰ)为改进的ICP算法和经典ICP算法的对比。可知，在不加初值的情况下仍然能够得到较为准确的结果，但计算量大和速度慢。

表(I)配准精度和速度对比

如图6，为验证本发明提出的融合基于特征点和ICP算法的三维位姿估计算法，所选实验场景为某工程训练中心走廊，机器人沿直线运行32.5m，实验中共采集到162帧图像。其中，图(a)为机器人直线运行时采集到的部分图像，可知该场景单一且特征稀少，对于位姿估计的求解存在很大挑战，如果直接采用基于特征的求解方法则将出现很大的偏差，甚至出现完全错误的结果。运动轨迹如图(b)所示，所对应的绝对误差的均方根为ATE_RMSE＝0.63，对应的三维地图如图(c)所示。由图6可知。本发提出的鲁棒的三维位姿估计方法对环境单一、场景特征不充足的情况能够取得较好的效果。

如图7，为验证局部优化后的效果，图4展示的是基于特征点的三维运动的效果，我们在该实验上添加局部优化部分。实验获取的效果如图(a)所示，对应的三维地图如图(b)所示，由上述轨迹图可以看出，经局部优化后整个轨迹的平滑了许多，该轨迹所对应的绝对误差的均方根为ATE_RMSE＝0.24，而图4展示的未加局部优化时的，绝对误差的均方根为ATE_RMSE＝0.33。优化前后关键帧之间的相对位姿估计的误差曲线如图(c)所示，由图可知，经优化后局部误差得到大幅度减小。

根据上述实验分析，本发明算法在三维估计的精确度和稳定性方面均优于现有算法。

参考文献1:E.Rublee,V.Rabaud,K.Konolige,and G.Bradski,“ORB:Anefficient alternative to SIFT or SURF,”in Proc.IEEE Int.Conf.Comput.Vision,Barcelona,Spain,Nov.2011,pp.2564–2571.

Claims (7)

1.一种基于RGB-D相机的机器人场景自适应位姿估计方法其特征在于，包括以下步骤：

步骤1：RGB-D相机分别在t时刻和t+1时刻获取相邻帧场景的二维彩色图像I_t和I_t+1、彩色图像对应的三维点云D_t和D_t+1；

步骤2：分别提取二维彩色图像I_t和I_t+1的ORB特征点集合

和

得到相邻帧场景的粗匹配特征点对集合M₀；

步骤3：采用对称性匹配策略和RANSAC算法，对粗匹配特征点对集合M₀中的误匹配特征点对进行剔除，得到精确的二维匹配点对集合M_t,t+1；

步骤4：从精确的二维匹配点对集合M_t,t+1中，选取符合匹配关系且具有有效深度信息的特征点对

其中i∈{1,…,N_match}，N_match为具有有效深度信息的三维匹配点对数；

步骤5：判断N_match是否大于求解位姿估计所需要的最小匹配特征点对数ε_match，若大于，执行基于特征匹配的三维位姿估计算法，得到位姿估计T_last和二维彩色图像内点数I_last，进入步骤6；否则，进入步骤7；

其中，ε_match取值为10；

步骤6：依据A_T判断求得的位姿估计T_last是否有效：若A_T≥ε_T成立，则基于特征匹配的三维位姿估计求得的位姿估计T_last有效，进入步骤8，否则，进入步骤7；

其中，A_T表示RANSAC位姿估计精度，是指内点总数N_I与内点总数的均方根误差E_I的比值，ε_T为判断位姿估计是否有效所设定的阈值；

步骤7：利用步骤1获得的三维点云D_t和D_t+1执行改进的ICP位姿估计，得到位姿估计T_last；

步骤8：利用步骤1-7获取的位姿估计，通过最小化重投影误差进行BA优化，获得平滑后的位姿估计。

2.根据权利要求1所述的方法，其特征在于，所述利用步骤1获得的三维点云D_t和D_t+1执行改进的ICP位姿估计，得到位姿估计T_last的过程如下：

步骤7.1：对于步骤1中获取的三维点云D_t和D_t+1，进行点云预处理后，基于法向量空间进行降采样，得到原始点云1％的三维点云数据，分别设为点云{p_i}和点云{q_j}，其对应的点数分别为m和n；

所述点云预处理依次为双边滤波，基于PCA算法的法向量计算；

步骤7.2：对于步骤7.1获得的点云{p_i}和点云{q_j}，采用FLANN搜寻方法遍历的搜寻点云{p_i}在点云{q_j}中的最近邻点，建立点云{p_i}和点云{q_j}之间的映射关系β_k：

其中，k为迭代次数，k的最大值k_max＝20，β_k为经k次迭代后计算出来的映射关系，T_k-1为上一次迭代计算后求得的位姿估计，初始值k＝1，T₀为前一帧图像的位姿估计；

步骤7.3：根据步骤7.2中建立的映射关系β_k，采用基于对应点间的中值距离的剔除法和基于表面法向量夹角的剔除法，剔除错误关系匹配点对；

剔除步骤为：

首先，遍历映射β_k中所有匹配点对，若

剔除此匹配点对；

然后，遍历映射β_k中所有匹配点对，求出匹配点对距离的中值距离d_mid；

再次，遍历映射β_k中所有匹配点对，若

剔除此匹配点对；

其中，法向量

表示点p_i在点云{p_i}表面的法向量，法向量

表示点q_j在点云{q_j}表面的法向量，

表示法向量

和法向量

的夹角，角度阈值θ_threshold经过实验对比设为1°；

表示点p_i和点q_j之间欧式距离的绝对值，距离阈值d_threshold等于中值距离d_mid；

步骤7.4：采用基于点到面的距离作为目标函数，通过调整变换矩阵T使得剩余匹配点对之间距离的平方和最小化，求得T_k；

其中，{p_i,q′_i}是通过步骤7.3剔除错误关系匹配点对后，得到剩余匹配点对集合，i＝(1,...,n)；ω_i为权值，

表示p_i与q_i′中深度值较大的点所对应的深度值；

步骤7.5：当T_k-T_k-1＞T_threshold或k>＝k_max满足时，迭代终止，令T_last＝T_k，得到当前帧图像的位姿估计T_last，若不满足，令k＝k+1，返回步骤7.2。

3.根据权利要求1所述的方法，其特征在于，当连续获得K帧图像的位姿估计后，采用光束平差法对当前帧图像在内的前M帧图像的位姿估计进行优化，M_w取值为10。

4.根据权利要求3所述的方法，其特征在于，所述采用光束平差法对当前帧图像在内的前M帧图像的位姿估计进行优化的具体过程如下：

步骤8.1：获取K帧新图像，采用步骤1-7所述方法求取最新获得的第K-M+1帧到最新获得的第K帧图像中每一帧图像对应的位姿估计，并获得对应连续M帧图像的位姿估计集合T＝[T₁,…T_j]，j＝(1,..,M)；

假设连续的M帧图像可以同时观测到N个ORB特征点，Q_i表示第i个ORB特征点对应的中的空间三维点，T_j表示第j帧图像对应的位姿估计，P(T_j,Q_i)表示空间三维点Q_i经过T_j变换，投影到图像上二维成像点；q_ij表示第i个ORB特征点对应的空间三维点在第j帧图像上的成像点；

步骤8.2：构建目标函数

并计算目标函数的雅克比矩阵

以及信息矩阵A＝JJ^T；

其中，ω_ij表示指示参数，若第i个空间三维点能够被第j帧图像观测到，则ω_ij＝1，反之，ω_ij＝0；

步骤8.3：求出更新矩阵δ_T；

通过步骤8.2求出的雅克比矩阵J和信息矩阵A，结合方程(A+uI)δ_T＝J^Tε，求解出更新矩阵δ_T；

其中，更新矩阵δ_T表示对T的变分，u为阻尼参数且u＝10^-3，ε＝q_ij-P(T_j,Q_i)；

步骤8.4：若||δ_T||＜ε，则停止迭代，且输出本次迭代的结果δ_T，转到步骤8.5；否则令u′＝u/10，转到步骤8.2继续计算；

步骤8.5：更新位姿估计集合T′＝T+δ_T，则得到相机新的估计投影点P′(T_j,Q_i)；如果

其中，ε_T表示迭代终止阈值常量，则说明达到了迭代终止条件，输出最终的连续M帧图像位姿估计集合T＝T′，输出T；否则，令u＝10u，P(T_j,Q_i)＝P′(T_j,Q_i)，跳转到步骤8.2。

5.根据权利要求1-4任一项所述的方法，其特征在于，所述基于特征匹配的三维位姿估计算法是指结合RANSAC的SVD分解法求取位姿估计，具体步骤如下：

步骤a：设最大迭代次数为k_max＝20，当前迭代次数为k，且初始值为1，最终位姿估计T_last和最终内点集合I_last为空，设初始内点集合对应的均方根误差E₀＝999；

步骤b：从步骤4中获取的N_match对有效特征点对{P_i ^t(x,y,z),P_i ^t+1(x,y,z)}中随机选取n对匹配点对{P_pi,Q_qi}，i＝(1,..,n)，利用SVD算法计算出位姿估计T_k和内点集合I_k，以及内点集合I_k对应的内点个数N_k；

步骤c：根据位姿估计T_k，将n对匹配点对{P_pi,Q_qi}中源特征点集P_pi投影到目标特征点集Q_qi，计算投影后的误差e_p：

e_p＝|T_kP_pi-Q_qi|

若误差e_p大于设定的阈值，则剔除该匹配点对，否则保留；遍历n对点，得到新的匹配点对{P′_pi,Q′_qi}，根据新的匹配点对{P′_pi,Q′_qi}获取新的内点集合I′_k，以及内点个数N′_k，并计算内点的比例N_R＝N′_k/N_k；

步骤d：假如内点的比例N_R小于内点比例设定阈值，则认为该位姿估计T_k是无效，返回步骤b；否则，认为位姿估计T_k初步满足要求；

然后，通过新的内点集合I′_k利用SVD算法重新计算位姿估计T′_k，并结合有效特征点对{P′_pi,Q′_qi}，计算I_k′对应的均方根误差E_k：

其中，N_n为有效特征点对{P′_pi,Q′_qi}中的对数；

步骤e：若E_k＞E_k-1，说明T′_k优于T′_k-1，此时，更新位姿估计T_last＝T′_k，内点集合I_last＝I′_k和内点个数N_I＝N′_k，均方根误差E_I＝E_k；否则，令k＝k+1，重复步骤b到步骤d，直到达到最大迭代次数k_max，结束计算。

6.根据权利要求1-4任一项所述的方法，其特征在于，所述步骤3中利用粗匹配特征点对M₀，剔除其中误匹配点对，最终得到相邻场景精确的二维匹配点对集合M_t,t+1的具体步骤如下：

3.1基于对称性匹配策略剔除误匹配；

对t+1时刻的二维彩色图像I_t+1的任一图像特征点

依次从t时刻的二维彩色图像I_t的图像特征点中找出基于汉明距离的最近邻匹配的特征点F_t ⁱ，构成匹配特征点对集合M′₀，M₀和M′₀的交集构成新的匹配图像特征点对集合M₁，剔除非对称的误匹配图像特征点对；

3.2采用RANSAC算法对匹配特征点对集合M₁中的误匹配进一步剔除，得到最终的匹配特征点对M₀点对集合M_t,t+1。

7.根据权利要求1-4任一项所述的方法，其特征在于，所述步骤2中利用ORB特征获取粗匹配M₀的具体步骤如下：

步骤2.1：对t时刻和t+1时刻获取的相邻场景的二维彩色图像I_t和I_t+1，利用ORB算法分别提取旋转不变特征点集合F_t ⁱ和

步骤2.2：对F_t的任一特征点F_t ⁱ，从t+1时刻获取的二维彩色图像特征点F_t+1中，依次找出与F_t ⁱ匹配的特征点

构成匹配特征点对

步骤2.3：遍历F_t中所有特征点：对F_t中所有特征点执行步骤2.2，得到粗匹配图像特征点对集合M₀。

Images