基于RGB-D传感器的同步定位与建图方法研究

周梦妮， 张国伟,， 卢秋红， 吉志鹏， 宋孟良， 纪栋梁

（1.上海电力大学自动化工程学院，上海200082；2.上海合时智能科技有限公司，上海201100）

0 引 言

本文研究了基于RGB-D传感器的同步定位与建图研究，使用ORB算法得到具有旋转不变性的特征描述子且进行前后帧特征点的匹配，为了提高算法速度和精度，在外点剔除上采用了基于K最近领域算法和使用交叉匹配过滤算法，用绑定深度信息的RANSAC算法得到可靠性高的内点集后实现图像配准及运动估计；消除前段噪声误差的最有效方法是进行闭环检测，当前最有效的闭环检测方法是基于词袋模型的闭环检测法；最后进行图优化算法，使闭环上每相邻节点与边的刚体模型满足最小误差函数。

1 图像感知模块

基于RGB-D传感器需要同时获得周围环境图像及深度信息，由此估计机器人的运动。RGB-D相机搭载光学RGB相机与红外相机，可以在获得RGB图的同时获得图像的深度数据，它通常利用TOF技术（Time of flight，飞行时间）[1]。可采用微软推出的Kinect 2代相机，它由一个红外接收器与一个红外发射器构成，能够较好地抑制外界光谱对传感器的干扰，同时可以得到丰富的场景数据（三维点云信息），利用这些信息可对空间环境进行三维重建[2]。

2 相机标定

相机标定主要是为了解决相机成像的几何模型问题与矫正透镜畸变问题。为了建立起物体坐标与图像坐标之间的关系，确定空间物体表面某点的三维几何位置与其在图像对应点之间的关系（具体对应关系如图1所示）需要在构建成像模型后得到相机的自身属性参数，得到内外参数信息的步骤称为相机标定，最常见的相机标定方法为传统方法中的张正友相机标定法。

由图1中三个坐标系的对应关系得

相机内参数矩阵K为

张正友相机标定法的核心思想是利用棋盘图样推算图像平面与棋盘图样平面之间的单应性矩阵[3]。黑白棋盘的角点坐标是已知的。根据不同角度的棋盘图片角点坐标可以求出相机内参数。

为了取得更丰富的图像信息，通常需要在RGB-D相机镜头前安装透镜，透镜在光线影响下会产生图像畸变，透镜往往是镜头中心对称的，越靠近镜头边缘，畸变现象越明显，这种对称的畸变称为径向畸变。

可转换为拥有相同相机主心的像素坐标：

得到畸变参数k1、k2后，先将图片进行去畸变处理，然后用去畸变的图像坐标估计相机内参数。

3 图像特征提取匹配与运动估计方法

图像特征提取匹配与运动估计是视觉SLAM中重要的一环。作为视觉SLAM的前端，利用获得的视觉信息进行图像特征的提取匹配[4]，结合利用深度信息可求得相邻两帧点云集的对应关系，有了已知的3D-3D对应关系便可以进行运动估计，即相邻两帧3D点对的欧式变换。那么得到最优的路径值，也就是求解最小二乘问题。

3.1 基于视觉传感器的特征提取方法

ORB算法是进行特征算子的检测与描述算法，为了减少描述符提取计算压力，使算法结果更精确，本文对所有检测出的特征点进行深度过滤，过滤后的特征点具有高精度特性，使后续特征点描述速度加快，算法结果更准确。下面将具体介绍改进后具有高精度、尺度不变性且具有旋转不变性的ORB算法[5]。

ORB算法用FAST算法对图像进行特征点检测，FAST算法的本质是寻找角点[6]，其原理是寻找图像像素点的灰度值梯度突变点，基于候选特征点周围一圈的像素值，选定一个半径将半径领域内的像素点与候选特征点进行灰度值的比较，如果灰度值差别足够大，则判定该像素点为特征点，由于FAST特征检测算子没有朝向所以会受图像旋转的干扰，为了解决这一问题，加入灰度质心法（IntensityCentriod）的oFAST特征检测算子具有方向性，灰度质心法假设某特征点与灰度值半径领域内的重心存在偏移，通过计算特征点到重心的向量，就可得到该特征点的主方向。

图像中深度大的特征点往往精度低且降低了算法速度，对得到的所有具有方向的特征点在深度图中进行深度过滤，设定深度阈值，筛选出高精度特征点。ORB算法利用BRIEF描述子描述特征点具备的属性，它计算出的是一个二进制串的特征描述符。比较所有特征点的灰度值，如果P（i）>P（i′），则生成二进制串中的1，否则为0。为了使当前帧所有的特征点具有旋转不变性，改进后的BRIEF描述子在建立坐标系时，使当前特征点为圆心，以特征点与取点区域的形心连线为X轴建立坐标系。

汉明距离越小说明两特征点相似度越高，匹配精度越高[7]。

3.2 外点剔除与运动估计

3.2.1 外点剔除

剔除某些相似物体之间的匹配可提高精度，使后续运动估计算法复杂度降低。本文在汉明距离的基础上提出基于K最近领域算法（KNN）和使用交叉匹配过滤方法对外点进行剔除，最后剩下的高精度匹配对称为内点。

1）KNN算法。设第二帧图上特征点P及其描述P（Vp）在相邻前一帧寻找最近点Q（Vq）与次近点Q˙（Vq˙），特征点P到两点的汉明距离分别为D（Vp,Vq）、D（Vp,Vq˙）。令R=D（Vp,Vq）/D（Vp,Vq˙)，当D(Vp,Vq)远小于D(Vp,Vq˙)时，R越小，则精度越高。在此可设定阈值T，当R>T时，表示点p、q为错误匹配，在多次实验中，可令T=0.62。

2）交叉匹配过滤方法。交叉匹配过滤方法是一种双向筛选方法，在当前帧得到的特征匹配点集为S1={Pi（Vq,Vq˙）|i=1,2…m}，下一帧得到的特征匹配点集为S2={Qi（Vp,Vp˙）|i=1,2…l}，采用交叉匹配思路，对S1、S2进行遍历，凡有（i,j）=(j,h)，当h=i时则保留匹配对，否则删除。

3.2.2 运动估计

在双目相机及RGB-D相机得到3D-3D特征匹配后，通常用ICP算法进行运动估计[8]，ICP（迭代最近点）算法是一种点集对点集算法，该算法可以把上一帧的点云集通过旋转平移得到下一帧的点云集，所以ICP算法求得最小的旋转矩阵和平移向量是关键，得到3D-3D的对应关系（P,P′）可转换为求3D点对的欧式变换：

∀i,pi=Rp′+t。

在上述视觉已提供了特征匹配之后，求解迭代最近点也变得容易，将上式转换为

ei=φi-Rpi+t。

求解最佳运动估计问题也转换为误差函数最小化的问题，即求解R、t：

式中，n为最邻近点对个数。

上述方程可采用SVD奇异值分解法求解旋转矩阵R,平移向量t。

两个点云集之间的旋转矩阵为

误差为3.1648×10-5。

3.3 绑定深度信息的RANSAC算法

使用ICP算法得到匹配点集的“最短路径”之后可采用RANSAC（随机抽样一致性）算法对匹配对进行提纯，RANSAC算法也可以用于图像配准及其拼接[9]。RANSAC算法采用不断迭代的方法，在一组包含外点的配对集中寻找最优参数模型，该最优参数模型通过不断迭代更新模型，直到达到设定的迭代次数K值后停止迭代，取当前最优值，具体步骤如下：

随机选取3个匹配点对，设目标图像角点位置(x,y)，设当前帧图像角度位置为（x′,y′），通过基础矩阵转化可得：

利用基础矩阵H计算目标图像与当前帧图像的投影误差，小于设定阈值的判定为内点并存入内点集Q，如果当前内点集Q元素大于最优内点集，则更新最优内点集。

如果迭代次数大于设定迭代次数K，则停止，否则按上述步骤重复进行。

当RGB-D类传感器精度与深度信息相关的时候，误差大小与物体到摄像头距离成正比[10]，近距离的物体在实现运动估计或图像配准时的精度往往高于远距离的物体，本文提出绑定深度信息的RANSAC三维点配准算法，将步骤（2）设定的阈值与深度信息相关联，近距离的深度权重高，阈值可以表示为

∈i=hdi+v。

如果只有近距离的点成为内点，那么进行图像配准会造成配准精度低的问题，为了加强配准可靠性、稳定性，可适当添加深度较大的点。

表1 内点及可靠性

4 全局优化方法

为了解决前端漂移等噪声引起的误差，需要进行闭环检测和图优化。在进行闭环检测与图优化算法前，先要引出关键帧的定义。相机的移动路线可以把关键帧的位姿作为节点，将位姿之间的刚体转换关系视作为节点与节点间的连线，如图8所示。

后续的全局优化需基于优质的关键帧进行才会具有高效率高精度，关键帧过多会造成信息冗余度大，消耗了计算机资源。总结关键帧的选取有以下几点：关键帧不需要频繁插入，距离上一次定位较近时不需要插入；关键帧上的内点数必须大于设定阈值。如今最常用根据帧间相对距离来选择关键帧，由于深度相机在机器人运动时通过旋转获得的信息量较平移时带来的变化信息大，设帧间相对距离为：

Ed=w1‖Δt2‖2+w2‖（φ,θ,ω）‖2；

w2>w1。

进行闭环检测是消除误差最有效的办法，当机器人再次到达此前识别过的某个点或点附近，由于相机位姿随时间的漂移，导致了地图不精确，此时机器人的建图却到达了别的地方。在前面利用ORB算法对每帧图片进行了特征探测，对其聚类产生了视觉词典，在离线情况下，可以采集大量的场景图片，形成信息量丰富的视觉词典。进行闭环检测时，将关键帧形成的特征向量在视觉词典里进行搜索，判断是否发生闭环。这种方法称为基于词袋模型的方法（Bag of words）[11]。

采用基于位姿图的优化算法，位姿图模型如图8所示。实质为求解最小误差函数的过程[12-13]，总体优化问题变成包含顶点与边的目标函数[14]：

式中：e（xi,xj,zij）为第i节点与第j节点之间位姿误差函数；zij为xi、xj的约束条件，如果xi、xj完全约束于zij，则误差为0；Ωij是设为对角矩阵的信息矩阵，对角阵元素大小表示该误差的重视程度。在RGB-D SLAM环境下，该目标函数可以设为包含旋转矩阵R与平移向量t的函数：

5 结 语

本文研究了基于RGB-D传感器的同步定位与建图方法，针对ORB特征匹配算法中易出现的错误匹配进行改进，加入设置汉明距离小于2倍最小匹配距离达到剔除错误匹配的目的。对基于视觉传感器的物体运动估计进行原理解释及仿真。可见，基于RGB-D传感器的SLAM基础上，可实现机器人在不同环境下的自主导航工作。