基于RGB-D数据耦合误差处理的CEP-SLAM

CEP⁃SLAM Based on RGB⁃D Data Coupling Errors Processing

摘要

针对使用RGB-D相机的VSLAM中RGB-D数据存在耦合误差的问题、现有特征提取方法存在的边点误提取问题及恒速运动模型跟踪稳定性差的问题，基于ORB-SLAM2框架提出了CEP-SLAM算法。算法使用恒加速运动模型来设置待跟踪帧的初始位姿；使用优化后的位姿计算帧间视觉里程计并更新恒加速运动模型，结合RGB图与深度图的采集时间差估算位姿偏移，基于该位姿偏移构建对极几何约束，使用二分法查找特征点在深度图对应像素点的位置，对特征点深度进行调整，缓解了RGB-D数据耦合误差对VSLAM的影响；提出一种基于联合方法的关键帧边点剔除算法，通过利用特征点在深度图的邻域信息对待插入关键帧中存在的不良边点进行判断和剔除。使用本文提出的CEP-SLAM算法在TUM公共数据集上进行实验，结果表明本文算法较好地剔除了不良边点，与经典算法相比有更好的鲁棒性、跟踪稳定性和更高的定位精度。

Abstract

Aiming at the issues of coupling errors in RGB-D data， incorrect extraction of edge-point in current methods of feature extraction and poor tracking stability of the constant speed motion model in VSLAM using RGB-D cameras， CEP-SLAM algorithm was proposed based on ORB-SLAM2 framework. The algorithm used a constant acceleration motion model to set the initial pose of the tracked frame； the optimized pose was used to calculate the inter-frame visual odometry and update the constant acceleration motion model. The pose deviation was estimated by combining the constant acceleration motion model and the acquisition time difference of RGB image and depth image. The epipolar geometry constraint was constructed based on the pose deviation and the dichotomy method was used to find the position of the feature-point in the corresponding pixel point of the depth image， and the depth of the feature-point was adjusted to alleviate the impact of RGB-D data coupling errors on VSLAM； a key-frame edge-point culling algorithm based on joint method was proposed. The bad edge-point in the inserted key-frame were judged and culled by using the neighborhood information of feature-point in the depth image. The CEP-SLAM algorithm proposed in this paper was used to conduct experiment on TUM public dataset. The experiment results show that the proposed algorithm can better cull bad edge-point， and has better robustness， tracking stability and higher positioning accuracy compared with classical algorithms.

Graphical abstract

关键词

RGB-D相机 / VSLAM / 恒加速运动模型 / 视觉里程计 / 耦合误差

0 引　言

基于视觉信息的同时定位与建图（Visual Simultaneous Localization and Mapping， VSLAM）^［1］是移动机器人避障、 导航的先决条件， 在新环境中， 机器人依赖传感器获取的图像信息来构建场景地图。

RGB-D相机是VSLAM进行定位和建图的一种核心组件， 通过结构光或者TOF原理^［2］测量物体的深度信息。RGB传感器与深度传感器的位置固定， 有稳定的位姿变换矩阵， 可使用该矩阵将深度图数据投影到RGB图图像坐标系下， 完成数据对齐^［3］。但对齐后的深度图数据存在误差， 该误差包括两部分： 深度传感器的测量误差^［4］， 深度图在与RGB图进行对齐时产生的数据耦合误差。

为缓解深度测量误差对定位与建图的影响， Zhang等^［5］提出了PLD-SLAM， 使用RGB图结合深度图拟合出3D线特征， 为相机位姿加上特征线平面法向量及相机光心与特征线间距的约束。孙超等^［6］使用最小二乘拟合通过连通域分割算法所得平面的方程， 并将其加入优化， 为相机位姿添加平面约束。Yunus等^［7］提出的Manhattan-SLAM结合点线面特征针对不同的环境使用不同的应对策略， 有效提高了算法的稳定性。上述VSLAM算法都使用拟合的方式获取线面方程， 拟合过程中深度测量的正反向误差相互抵消， 缓解了带有测量误差的深度信息直接加入系统的负面影响^［8］。潘心冰^［9］提出为Kinect相机建立随距离平方增长的深度测量误差模型， 将深度测量的不确定性纳入ICP（Iterative Closest Point）^［10］算法的信息矩阵， 缓解了深度测量误差对系统的影响。

目前， 对于RGB-D相机的深度图与RGB图进行对齐时产生的数据耦合误差尚无SLAM算法对其进行处理， 经典的RGBD-SLAM^［11］、 DVO-SLAM^［12］、 ORB-SLAM2^［13］、 ORB-SLAM3^［14］及加入线面特征的Manhattan-SLAM算法均直接使用未处理的深度数据进行定位与建图， 未考虑两个传感器数据采集时间差内相机的小幅位姿偏移， 导致深度的真实值与使用值不符。

VSLAM系统的特征点剔除算法主要针对动态物体， 基于机器学习实现。陈敏^［15］使用Yolo-v3来检测日常生活中可能出现的动态物体， 基于几何约束滤除所有不符合要求的特征点。Ji等^［16］使用K均值的方式对整个深度图进行聚类， 对聚类结果采用多视图几何的方法进行动态点的判断来提出动态点。

对于因物体间遮挡产生的误提取边点， 目前没有相关的处理方法。ORB^［17］、 SIFT^［18］、 SURF^［19］等特征提取方法获取的特征点中存在大量边点， 此类点位于不同物体在二维图像中的连接线上， 且因周围有明显的像素差而会被系统检测到， 然而不同物体在二维图像中的连接线会随着相机的移动发生改变， 连接线上点的空间位置随之改变， 因此， 边点因周围纹理相似易产生误匹配。

少部分边点可在后端优化中通过RANSAC^［20］（RANdom SAmple Consensus）方法过滤掉， 不参与位姿优化。但是， 该方法无法处理可通过内外点校验的边点， 边点的匹配会对对位姿产生错误约束， 影响最终的定位和建图精度。

针对上述问题， 本文主要进行以下研究：

1） 提出一种基于联合方法的关键帧边点剔除算法， 依靠边点所在深度图的圆形邻域信息对关键帧中存在的由物体遮挡产生的边点进行判断和剔除。

2） 以恒加速运动模型替换恒速运动模型， 增强SLAM算法跟踪的稳定性。提出优化深度耦合误差的CEP-SLAM（Coupling Errors Processing-SLAM）算法。该算法使用优化后的帧间视觉里程计更新恒加速运动模型， 结合数据采集时间差估算出深度图与RGB图间相机的位姿变换矩阵， 使用该位姿变换矩阵调整特征点的深度数据， 缓解深度耦合误差对系统的影响。

3） 在TUM^［21］数据集的不同环境、 不同运动情况的视频序列中进行定位和建图， 验证定位精度、 稳定性以及鲁棒性。

1 相关工作

1.1 恒速运动模型的缺陷分析

使用RGB-D数据进行同时定位与建图前， 需对RGB图与深度图进行匹配， 未匹配成功的帧将被剔除， 被剔除帧前后两帧间的时间差大于正常的帧间时间差， 导致帧间时间差的突变， ORB-SLAM2算法的跟踪稳定性受突变帧间时间差的影响较大， 示例如图 1 所示。

图 1 中， 从F1到F4为RGB-D相机在高速运动中采集到的图像序列， 其中， F3由于未成功匹配到深度图被剔除， T_{\mathrm{1}}为优化后的F1到F2的帧间视觉里程计。在算法对F4进行跟踪时， 使用优化后的F2位姿左乘T_{\mathrm{1}}作为F4的初始位姿， 该设置导致F4的初始位姿更接近真实的F3的位姿， 以该视觉里程计对地图点使用重投影的方式进行特征匹配时， 在待测范围内可能找不到正确的匹配点， 导致系统的跟踪稳定性变差。

1.2 边点的定义与分析

理想状态下使用主流特征提取方法提取到的特征点可以分为两类： 第一类是处于同平面或曲面下的空间纹理点， 此类特征点反映图像中一块平面或曲面的纹理信息， 同一空间纹理点对应不同帧中的投影（特征点）在匹配时使用的圆形邻域信息固定， 有较好的鲁棒性； 第二类是物体的空间角点。在物体的棱角与背景环境或其他物体在二维图像中的连接处， 同一空间角点对应不同帧中的投影（特征点）在匹配时使用的圆形邻域信息会发生少量变化， 这是由于圆形邻域中属于物体棱角的像素点相对稳定， 而属于背景环境或其他物体的像素点由于相机位姿的变化发生了改变， 对应同一地图点的两特征点可能无法正常匹配， 鲁棒性比第一类特征点稍差。

在实际应用中， 算法除了获取到以上两类理想特征点外， 还会获取到一类特殊特征点， 此类特殊特征点由物体间的遮挡产生， 例如： 图 2 中桌子的边遮挡地面箭头纹理产生的点、 电脑显示器遮挡易拉罐产生的点以及显示器遮挡显示器支架产生的点。

此类点位于不同物体在二维图像中的连接线上， 但在三维空间中物体间没有直接接触， 连接线周围的点位于三维空间中无接触的不同物体上， 深度差异较大， 因不同物体的纹理或光照强度不同， 圆形邻域有明显的像素差， 符合ORB特征点的提取条件。在跟踪过程中， 随着相机移动， 不同物体在二维图像中的连接线发生改变， 但在连接线上依然存在圆形邻域内纹理相似的点， 但是这些点的空间位置并不一致， 为了方便描述， 本文将此类特殊特征点统称为边缘特征点（简称边点）。圆形邻域相似的边点很可能会产生匹配， 若观测到边点的帧被包装成关键帧， 边点会引入地图， 在后续帧的跟踪中会由于边点间的匹配对相机位姿产生错误约束， 从而引入全局误差。

在进行位姿优化时， RANSAC方法可以过滤少量边点， 而大部分的边点因其空间位置的改变量小， 可以通过RANSAC的内外点校验， 从而影响定位和建图的精度。

图 2 所在视频序列Fr2_xyz的运动比较平缓， 没有快速的旋转与位移， 在该运动情况下， 虽然边点的位置会发生变化， 但其对系统的影响有限。而在有快速旋转或位移的情况下， 由于边点的邻域跨物体特性以及RGB-D相机两传感器数据采集时间差内的位姿偏移， 深度的采集位置可能从一个物体上转移到另一个物体上， 所测位置深度与特征点实际深度有明显的差值。

图 3 中， O₁为RGB图像平面， O₂为与之匹配的深度图像平面， 在两图的数据采集时间差内相机有小幅位姿偏移。点P是RGB图像进行特征提取得到的位于物体C的边缘点， 其投影到O₁上的点p与O₂上点p₁的二维坐标相同， 而p₁对应的三维坐标系下的点P₁与点P并非同一点， 直接使用P₁的深度作为P的深度会带来错误约束。如图 2 中箭头部分的点， 其深度的采集位置可能因位姿偏移由地面转移到桌面边缘。

1.3 数据耦合问题对VSLAM的影响

RGB-D相机内RGB传感器和深度传感器的数据采集并非同步进行， 对于具体的Kinect来说， 采集时间差在0~20 ms之间， 如果相机处于移动状态， 该时间差内相机会有小幅位姿偏移。示例如图 4 所示。

图 4 是TUM数据集中6对匹配的RGB图与深度图， 在采集上述图像时， 相机的运动速度较快， 对比图片边缘区域可以看出6对图像在采集时间差内的位姿偏移较为明显。示例1中RGB图左下方地面可视部分的轮廓形状与深度图明显不同； 示例2中， RGB图左侧窗户下方电脑的可视部分明显大于深度图的电脑可视部分。

上述位姿偏移对深度数据的影响分为两部分： 一是深度采集位置的改变， 二是相机在采集时间差内的位姿偏移导致真实采集点的深度与对应的特征点深度存在差值。

假设RGB图拍摄时刻相机坐标系下的点\mathit{P}={\left[X,Y,Z\right]}^{\mathrm{T}}在经过位姿偏移后在新时刻相机坐标下的点为{\mathit{P}}^{\text{'}}={\left[X^{\text{'}},Y^{\text{'}},Z^{\text{'}}\right]}^{\mathrm{T}}， 其对应关系为

{\mathit{P}}^{\text{'}}=\mathit{R}\mathit{P}+\mathit{t},（1）

式中： \mathit{R}为位姿偏移的旋转矩阵； \mathit{t}为位姿偏移的旋转向量。将式（1）展开得到

X^{\text{'}}=R_{\mathrm{11}}X+R_{\mathrm{12}}Y+R_{\mathrm{13}}Z+t_x,（2）

Y^{\text{'}}=R_{\mathrm{21}}X+R_{\mathrm{22}}Y+R_{\mathrm{23}}Z+t_y,（3）

Z^{\text{'}}=R_{\mathrm{31}}X+R_{\mathrm{32}}Y+R_{\mathrm{33}}Z+t_z,（4）

式中： R_{xy}代表旋转矩阵R中第x行第y列的元素； t_x代表位移向量\mathit{t}第x列的元素。此时， 应该进行深度取值的位置在深度图图像坐标系下的坐标应为{\left[f_x\frac{X^{\text{'}}}{Z^{\text{'}}}+c_x,f_y\frac{Y^{\text{'}}}{Z^{\text{'}}}+c_y\right]}^{\mathrm{T}}， 与实际的取值位置{\left[f_x\frac{X}{Z}+c_x,f_y\frac{Y}{Z}+c_y\right]}^{\mathrm{T}}有一定的差异。其中， f_x， c_x， f_y， c_y为相机的内参； 特征点对应深度图采集位置的深度Z^{\text{'}}与希望得到的测量值Z的转换关系如公式（4）所示。深度观测数据的误差会对位姿产生负面影响， 导致定位精度的下降， 而定位精度的下降又会对新建地图点以及原有地图点的优化造成负面影响， 导致误差的积累。本文将RGB-D相机因位姿偏移产生的深度数据误差定义为数据耦合误差。

2 CEP-SLAM

本文基于ORB-SLAM2框架提出了CEP-SLAM算法， 算法同步运行跟踪、 局部建图、 回环检测三个线程， 本文提出的改进主要集中在跟踪线程。具体步骤如下：

第一步： RGB-D相机获取RGB图与深度图数据， 通过ORB特征提取算法对RGB图提取特征点并计算描述子；

第二步： 利用匹配成功的点对恒加速运动模型设置的初始位姿执行第一次位姿优化。根据局部地图的匹配执行第二次优化， 更新恒加速运动模型， 调整深度数据。使用调整后的数据再次进行优化， 更新恒加速运动模型， 调整深度数据；

第三步： 判断是否需要插入新的关键帧， 若该帧为候选关键帧， 需对其执行边点剔除算法再包装成关键帧转到局部建图执行关键帧的插入。

局部建图线程主要步骤如下：

第一步： 等待关键帧的插入并且拒绝跟踪线程插入新的关键帧；

第二步： 取出队首关键帧计算其Bow特征后更新该关键帧观测到的已有地图点信息， 并将该关键帧提取出的新地图点纳入筛选器进行判断；

第三步： 对通过判断的新地图点使用对极约束通过三角化恢复出其3D位置并加入地图， 检查并融合地图中重复的地图点；

第四步： 更新当前关键帧的连接关系， 对当前关键帧、 当前关键帧的一级连接关键帧以及共视地图点进行局部优化， 保持原有恒加速运动模型， 根据共视点占比剔除冗余关键帧；

第五步： 在完成关键帧的插入后， 将该关键帧投入闭环检测线程；

第六步： 对插入的关键帧进行计数， 每插入10个关键帧， 重置恒加速运动模型。

回环检测线程主要步骤如下：

第一步： 计算出接收关键帧与其所有共视关键帧的最低Bow相似度得分， 并依据该得分从所有不相连的关键帧中找到所有的回环候选帧；

第二步： 通过对候选帧进行连续性检测来对所有的回环候选帧进行滤波， 当初步检测到回环后， 计算并优化当前关键帧与候选关键帧之间的位姿变换矩阵；

第三步： 使用该变换进行正反向的重投影校验， 通过校验认为该回环有效， 通过优化后的位姿变换矩阵对该关键帧的共视关键帧进行位姿传播矫正， 调整共视关键帧的位姿及共视地图点的位置后更新关键帧的共视关系；

第四步： 构建本质图对本质图内的关键帧位姿进行优化， 根据地图点参考关键帧位姿的变化调整地图点的位置。

第五步： 进行全局优化， 重置恒加速运动模型。

算法处理流程如图 5 所示。

2.1 基于联合方法的边点剔除策略

由于RGB图没有空间信息， 无法通过RGB图直接对边点进行剔除， 需要结合深度图进行处理， 同时边点需要还原成地图点插入地图才能对跟踪产生影响。因此， 本文提出了一种边点剔除算法， 依靠边点所在深度图的圆形邻域特性对待插入关键帧中存在的由物体遮挡产生的边点进行判断和剔除。算法通过对由带判断特征点的圆形邻域内像素点构建的数据点进行DBSCAN聚类， 再结合邻域形状进行综合判断， 剔除待插入关键帧中存在的边点。

本文提出了一种黑白棋盘式的降采样算法： 对以特征点为中心， 边长为15的正方形邻域内的所有点进行遍历， 选出所有相互间不存在四连通关系的点组成点集， 再对该点集进行遍历， 在一次遍历中， 通过判断点与圆心的欧式距离来判断该点是否位于圆内， 保留点集中位于圆内的点。例如： 图 6 中白色点被选中， 黑色点被忽略。

本文使用特征的像素坐标{\left[u,v\right]}^{\mathrm{T}}以及其对应的50倍深度来构建数据点， 解决聚类对深度变化不敏感的问题。在完成降采样的选点以及数据点的构建后对数据点集进行DBSCAN聚类，聚类算法的流程如图 7 所示。

图 7 中最少点数目及邻域半径为经验值。聚类算法执行完成后， 如果聚类结果只有一类， 说明该特征点为第一类特征点； 如果聚类结果为两类， 需进一步判断； 如果聚类结果多于两类， 说明该特征点是需要剔除的边点。结果为两类的具体示例如图8所示。

图 8 中第二类特征点在深度图下的圆形邻域为蓝色， 而边点在深度图下的圆形邻域为红色。可以看出， 边点的圆形邻域可近似由一条线段分为两个部分， 而第二类特征点的邻域分隔线不规则。可由该特性判断并剔除聚类结果为两类的特征点中的边点。

针对上述情况， 本文提出一种反映图像块深度梯度方向的深度质心法。深度质心的计算公式为

m_{pq}=\sum _{x,y}{x}^p{y}^{q}depth\left(x,y\right)p,q\in \left\{\mathrm{0,1}\right\}\mathrm{。}（5）

m为以待判定特征点为圆心的圆形图像块（取圆形邻域）， depth\left(x,y\right)为坐标{\left[x,y\right]}^{\mathrm{T}}处的深度值， 图像块的形心O为圆心， 深度质心为C=\left(\frac{m_{\mathrm{10}}}{m_{\mathrm{00}}},\frac{m_{\mathrm{01}}}{m_{\mathrm{00}}}\right)， OC连线所在的直径即为所求直径。

在计算深度质心前需要计算每类中所有点的深度平均值， 将其作为该类内所有点的深度值。消除类内深度差， 避免类内深度变化影响对深度质心的计算。边点的判断原理如图 9 所示。

图 9 中圆形邻域被黑线分割为两个区域， 每个区域对应一类聚类结果。红色直径是邻域的深度质心与圆心的连线， 两条黄色的弦与红色直径垂直， 与圆心距离为1/2半径。在对两类的深度数据分别进行深度统一处理后， 第二类特征点在弦上存在深度的变化， 而边点没有深度变化。

在求出该直径后， 找到直径上位于圆心两侧并距离圆心1/2半径的两个近似点， 分别使用两点结合弦与直径垂直的特性找到位于两条弦上的所有近似点， 判断同一条弦上是否存在深度变化。剔除无深度变化的点； 反之则保留。

由于聚类算法的高耗时性， 本文提出以下滤波策略： 对待插入关键帧的特征点进行数据预处理， 遍历所有特征点， 保留所有圆形邻域内不存在深度突变的特征点， 从而减少聚类次数， 降低跟踪的耗时； 使用本文剔除的黑白棋盘式的降采样算法减少纳入聚类的数据点。

2.2 恒加速运动模型的构建和更新

RGB-D相机以30帧/s的帧率进行数据采集时， 帧间采集时间差大致为33 ms， 在该时段内速度会发生小幅变化， 而加速度几乎不变， 所以本文假设相邻两帧间的加速度不变， 构建恒加速运动模型模拟相机在无先验信息下的运动。在VSLAM中， 相机的运动由旋转矩阵和位移向量组成， 位移向量可以直接求解速度和加速度， 而旋转矩阵并不能直接进行加速度的求解， 所以本文引入欧拉角进行帧间旋转速度及旋转加速度的求解。

欧拉角的三个分量： roll（横滚）、 pitch（俯仰）、 yaw（偏航）， 分别代表相机的旋转运动在空间中三个相互垂直的坐标轴的旋转分量， 欧拉角与旋转矩阵可相互转化。由欧拉角{\left[\psi ,\theta ,\varphi \right]}^T转换的旋转矩阵\mathit{R}如式（6）所示。

\left[\begin{array}{ccc}c\theta c\varphi & c\theta s\varphi & -s\theta \\ s\psi s\theta c\varphi -c\psi s\varphi & s\psi s\theta s\varphi +c\psi c\varphi & s\psi c\theta \\ c\psi s\theta c\varphi +s\psi s\varphi & c\psi s\theta s\varphi -s\psi c\varphi & c\psi c\theta \end{array}\right],（6）

式中： cx表示\mathrm{c}\mathrm{o}\mathrm{s}x， sx表示\mathrm{s}\mathrm{i}\mathrm{n}x。

由旋转矩阵\mathit{R}转换化为欧拉角{\left[\psi ,\theta ,\varphi \right]}^{\mathrm{T}}的公式如式（7）~式（9）所示。

\psi =\mathrm{a}\mathrm{r}\mathrm{c}\mathrm{t}\mathrm{a}\mathrm{n}\frac{R_{\mathrm{33}}}{R_{\mathrm{23}}},（7）

\theta =\mathrm{a}\mathrm{r}\mathrm{c}\mathrm{t}\mathrm{a}\mathrm{n}\frac{\sqrt[]{R_{\mathrm{23}}^{\mathrm{2}}+R_{\mathrm{33}}^{\mathrm{2}}}}{-R_{\mathrm{13}}},（8）

\varphi =\mathrm{a}\mathrm{r}\mathrm{c}\mathrm{t}\mathrm{a}\mathrm{n}\frac{R_{\mathrm{11}}}{R_{\mathrm{12}}},（9）

式中： R_{xy}代表旋转矩阵\mathit{R}中第x行第y列的元素。

在跟踪线程插入新的待跟踪帧时， 先使用优化得到的前一帧瞬时旋转速度与位移速度向量{\mathit{v}}_{\mathrm{0}}={\left[v_x,v_y,v_z,{\omega }_x,{\omega }_y,{\omega }_z\right]}^{\mathrm{T}}以及在待跟踪帧前两帧的时间间隔内保持不变的加速度向量\mathit{a}={\left[a_x,a_y,a_z,a_{\omega x},a_{\omega y},a_{\omega z}\right]}^{\mathrm{T}}， 结合待跟踪帧前一帧到待跟踪帧的时间差t， 估算出该时间段的位姿变换向量\mathit{s}={\left[t_x,t_y,t_z,\psi ,\theta ,\varphi \right]}^{\mathrm{T}}， 表达式为

\mathit{s}={\mathit{v}}_{\mathrm{0}}t+\frac{\mathrm{1}}{\mathrm{2}}\mathit{a}{t}^{\mathrm{2}}\mathrm{。}（10）

将六维向量\mathit{s}转化为位姿变换矩阵， 如式（11）所示。

\mathit{T}=\left[\begin{array}{cc}\mathit{R}& t\\ \mathrm{0}& \mathrm{1}\end{array}\right]，（11）

式中： \mathit{R}由式（6）转化得到； t为向量中的平移部分。

使用该视觉里程计\mathit{T}结合待跟踪帧前一帧的位姿{\mathit{T}}_{\mathrm{0}}来设置待跟踪帧的初始位姿为T_{\mathrm{1}}^{\text{'}}， 如式（12）所示。

{\mathit{T}}_{\mathrm{1}}^{\text{'}}=\mathit{T}{\mathit{T}}_{\mathrm{0}}\mathrm{。}（12）

在完成位姿优化后， 可根据优化后的视觉里程计更新待跟踪帧与前一帧的时间差内的加速度\mathit{a}与拍摄待跟踪帧时刻相机的瞬时速度v_{\mathrm{1}}， 完成对恒加速运动模型的更新。计算公式为

\mathit{a}=\frac{\mathrm{2}\left({\mathit{s}}^{\text{'}}-{\mathit{v}}_{\mathrm{0}}t\right)}{t^{\mathrm{2}}},（13）

{\mathit{v}}_{\mathrm{1}}={\mathit{v}}_{\mathrm{0}}+\mathit{a}t,（14）

式中： {\mathit{s}}^{\text{'}}={\left[t_x,t_y,t_z,\psi ,\theta ,\varphi \right]}^{\mathrm{T}} 为位姿变换矩阵转换所得位姿变换向量； t为待跟踪帧前一帧到待跟踪帧的时间差。

2.3 基于线性插值的数据耦合误差处理

对于待跟踪帧和待跟踪帧的前一帧来说， 使用优化后的待跟踪帧位姿以及前一帧位姿可以计算帧间视觉里程计\mathit{T}， 将其转化为位姿变换向量\mathit{s}={\left[t_x,t_y,t_z,\psi ,\theta ,\varphi \right]}^{\mathrm{T}}， 帧间运动可以由前一帧拍摄时刻的瞬时速度{\mathit{v}}_{\mathrm{0}}={\left[v_x,v_y,v_z,{\omega }_x,{\omega }_y,{\omega }_z\right]}^{\mathrm{T}}、 帧间间隔t， 以及由匀加速运动公式计算出的加速度\mathit{a}={\left[a_x,a_y,a_z,a_{\omega x},a_{\omega y},a_{\omega z}\right]}^{\mathrm{T}}， 以及待跟踪帧拍摄时刻的瞬时速度{\mathit{v}}_{\mathrm{1}}={\left[v_x,v_y,v_z,{\omega }_x,{\omega }_y,{\omega }_z\right]}^{\mathrm{T}}表示。根据RGB图和深度图的采集顺序， 恒加速运动模型可以分为两种情况， 如图 10 所示。

图 10（a） 为深度图在RGB图之后拍摄的匀加速运动模型， t_{\mathrm{0}}表示前一帧RGB图采集时间， t_{\mathrm{R}\mathrm{G}\mathrm{B}}表示待跟踪帧RGB图采集时间， t_{\mathrm{D}\mathrm{e}\mathrm{p}\mathrm{t}\mathrm{h}}表示待跟踪帧深度图采集时间， 蓝色梯形面积即为位姿变换向量s， 红色梯形面积表示位姿偏移向量{\mathit{s}}^{\mathit{\text{'}}}={\left[t_x^{\text{'}},t_y^{\text{'}},t_z^{\text{'}},{\psi }^{\text{'}},{\theta }^{\text{'}},{\varphi }^{\text{'}}\right]}^{\mathrm{T}}， 使用线性插值估算速度v^{\text{'}}， 从而求解该向量， 即

\left\{\begin{array}{l}{v}^{\text{'}}=Linearinterp\left({\mathit{v}}_{\mathrm{0}},{\mathit{v}}_{\mathrm{1}},\frac{t_{\mathrm{D}\mathrm{e}\mathrm{p}\mathrm{t}\mathrm{h}}-t_{\mathrm{R}\mathrm{G}\mathrm{B}}}{t_{\mathrm{R}\mathrm{G}\mathrm{B}}-t_{\mathrm{0}}}\right),\\ {\mathit{s}}^{\mathit{\text{'}}}={\int }_{t_{RGB}}^{t_{Depth}}\left({\mathit{v}}_{\mathrm{1}}+\mathit{a}\left(t_{\mathrm{D}\mathrm{e}\mathrm{p}\mathrm{t}\mathrm{h}}-t_{\mathrm{R}\mathrm{G}\mathrm{B}}\right)\right)\mathrm{d}t\mathrm{。}\end{array}\right.（15）

由于深度图在RGB图之后拍摄， 而优化得到的帧间视觉里程计\mathit{T}是由前一帧指向待跟踪帧的正向视觉里程计。所以， 位姿偏移向量{\mathit{s}}^{\text{'}}转化后的旋转矩阵\mathit{R}与位移向量\mathit{t}也是正向的， 组成的位姿偏移矩阵{\mathit{T}}_{\mathrm{D}\mathrm{R}}由RGB图指向深度图， 为了计算从深度图中还原的地图点在RGB相机坐标系下的坐标， 需要使用反向位姿偏移{\mathit{T}}_{\mathrm{R}\mathrm{D}}， 即

{\mathit{T}}_{\mathrm{R}\mathrm{D}}=\left[\begin{array}{cc}{\mathit{R}}^{-\mathrm{1}}& -{\mathit{R}}^{-\mathrm{1}}t\\ \mathrm{0}& \mathrm{1}\end{array}\right]\mathrm{。}（16）

图 10（b） 为深度图在RGB图之前拍摄的匀加速运动模型， 蓝色梯形面积为位姿变换向量\mathit{s}， 绿色梯形面积表示位姿偏移向量。与式（15）相似， 此时的位姿偏移向量{\mathit{s}}^{\text{'}}的计算公式为

\left\{\begin{array}{l}{\mathit{v}}^{\text{'}}=Linearinterp\left({\mathit{v}}_{\mathrm{0}},{\mathit{v}}_{\mathrm{1}},\frac{t_{\mathrm{R}\mathrm{G}\mathrm{B}}-t_{\mathrm{D}\mathrm{e}\mathrm{p}\mathrm{t}\mathrm{h}}}{t_{\mathrm{R}\mathrm{G}\mathrm{B}}-t_{\mathrm{0}}}\right),\\ {\mathit{s}}^{\text{'}}={\int }_{t_{Depth}}^{t_{RGB}}\left({\mathit{v}}^{\text{'}}+\mathit{a}\left(t_{\mathrm{R}\mathrm{G}\mathrm{B}}-t_{\mathrm{D}\mathrm{e}\mathrm{p}\mathrm{t}\mathrm{h}}\right)\right)\mathrm{d}t\mathrm{。}\end{array}\right.（17）

与第一种情况不同的是， 此时位姿偏移向量转化后的旋转矩阵\mathit{R}与位移向量 t 是由深度图指向RGB图的， 可直接使用式（11）组成位姿偏移{\mathit{T}}_{\mathrm{R}\mathrm{D}}。

估算得到位姿偏移{\mathit{T}}_{\mathrm{R}\mathrm{D}}后， 还需要寻找特征点在深度图中对应的像素点位置， 为了减少深度图上需要进行坐标系转换和投影的像素点数量， 本文提出一种基于对极几何的像素点匹配策略， 对于一幅匹配的深度图和RGB以及估算出的位姿偏移{\mathit{T}}_{\mathrm{R}\mathrm{D}}来说， 有图 11 所示的关系。

图 11 中， I₁为RGB图图像平面， I₂为深度图图像平面， O₁是RGB图拍摄时刻的相机光心， O₂是深度图拍摄时刻的相机光心， e和e^{\text{'}}是极点， l_{\mathrm{1}}和l_{\mathrm{2}}是极线， 对于在I₁上提取出的特征点u来说， 已知估算出的可相互转换的位姿变换矩阵， 根据对极几何原理， 特征点对应地图点P_x 在深度图上的像素点u^{\text{'}}一定在极线l_{\mathrm{2}}上， 在两张图像都是RGB图的对极几何中， 由于O₂P_x 的距离未知， 所以无法确定的u^{\text{'}}在深度图像素坐标系下的坐标， 而深度图带有地图点的深度信息， 可以实现对u^{\text{'}}的定位。根据对极几何原理， 有

{\mathit{U}}^{\mathrm{T}}{\mathit{K}}^{-\mathrm{T}}{\mathit{t}}^{\wedge }\mathit{R}{\mathit{K}}^{-\mathrm{1}}{\mathit{U}}^{\text{'}}=\mathrm{0},（18）

式中： \mathit{U}为特征点u的齐次坐标； {\mathit{U}}^{\text{'}}={\left[u^{\text{'}},v^{\text{'}},\mathrm{1}\right]}^{\mathrm{T}}为其在深度图对应像素点u^{\text{'}}的齐次坐标； {\mathit{K}}^{-\mathrm{1}}为相机内参矩阵的逆矩阵， {\mathit{K}}^{-\mathrm{T}}为{\mathit{K}}^{-\mathrm{1}}的转置， \mathit{R}和\mathit{t}={\left[t_x,t_y,t_z\right]}^{\mathrm{T}}分别为由RGB图指向深度图的位姿偏移{\mathit{T}}_{\mathrm{D}\mathrm{R}}中的旋转与位移部分。

将各项代入式（18）， 可得到特征点对应像素点所在极线l_{\mathrm{2}}的方程， 即

A{u}^{\text{'}}+B{v}^{\text{'}}+C=\mathrm{0}\mathrm{。}（19）

仅使用对极几何不能确定特征点对应深度像素点在极线l_{\mathrm{2}}上的具体位置， 但是对极线上l_{\mathrm{2}}的所有点进行还原和坐标系转换后投影可以得到这些点在RGB图上的像素坐标， 求得坐标均在极线l_{\mathrm{1}}上， 该坐标与特征点坐标按位做差为\mathit{e}={\left[u-u^{\text{'}},v-v^{\text{'}}\right]}^{\mathrm{T}}， \mathit{e}的内积越小， 说明该点距离特征点越近。由极线l_{\mathrm{1}}和l_{\mathrm{2}}间的关系可知， 该内积与直线上的深度像素点坐标之间符合二次曲线的特性， 可以使用二分法的方式对深度图中处于该直线上的所有像素点进行处理， 找出与特征点对应的像素点。具体算法流程如图 12 所示。

需要注意的是二分法最终得到的像素点并不一定是有效点， 共有以下两种情况：

1） 第一种情况是由于硬件问题导致深度测量失效， 具体表现为深度图上有大量深度为0的像素点。在理想的二次函数图像中， 处于极值点的像素点很有可能因为测量失效不能进行重投影， 二分法使用的点集中没有与特征点对应的像素点存在。例如图 13（a） 中二次函数的极值点并不存在。

2） 第二种情况是特征点对应的像素点经过位姿偏移后移出了相机视野， 例如图 13（b） 中二次函数图像只有不包括极值点的部分图像。二分法使用的点集中同样没有与特征点对应的像素点存在。

当出现上述两种情况时， 应该将特征点的深度设为0， 这表示该特征点在深度图对应位置像素点的深度采集失效。由于两种情况有共性特点， 最终二分法得到的像素点并非极值点， 投影误差的内积远大于极值点对应内积。综述所述， 本文引入自适应阈值\frac{A^{\mathrm{2}}+B^{\mathrm{2}}}{B^{\mathrm{2}}}对上述两种情况进行判断和处理， 向上取整。当{\mathit{e}}^{\mathrm{T}}\mathit{e} <\frac{A^{\mathrm{2}}+B^{\mathrm{2}}}{B^{\mathrm{2}}}时， 该像素点有效， 当{\mathit{e}}^{\mathrm{T}}\mathit{e} >\frac{A^{\mathrm{2}}+B^{\mathrm{2}}}{B^{\mathrm{2}}}时， 将对应特征点的深度设置为0。

对于深度有效的像素点{\mathit{U}}^{\text{'}}={\left[u^{\text{'}},v^{\text{'}},\mathrm{1}\right]}^{\mathrm{T}}， 需要使用位姿偏移{\mathit{T}}_{\mathrm{R}\mathrm{D}}计算对应特征点的深度， 即

\left\{\begin{array}{l}{p}^{\text{'}}=Z^{\text{'}}{\mathit{K}}^{-\mathrm{1}}{\mathit{U}}^{\text{'}},\\ \mathit{P}={\mathit{T}}_{RD}{\mathit{P}}^{\text{'}},\end{array}\right.（20）

式中： Z^{\text{'}}为像素点深度， {\mathit{K}}^{-\mathrm{1}}为内参矩阵的逆， 最终求得\mathit{P}={\left[X,Y,Z,\mathrm{1}\right]}^{\mathrm{T}}的Z即为特征点对应深度。

上述处理过程建立在位移向量\mathit{t}在不为零向量的情况下， 当\mathit{t}为零向量时， 式（18）将无条件成立， 无法求解极线方程， 此时只能进行全深度图的重投影。在实际使用中， \mathit{t}为零向量的情况很少甚至不出现。

3 仿真分析

本文在搭载英特尔Core i7-11700k处理器， 16 G内存， 操作系统为Ubuntu20.04的图形工作站上进行实验， 使用公开数据集TUM来验证所提出算法的定位精度、 稳定性、 鲁棒性及运行效率。

3.1 基于联合方法的边点剔除实验

为了获取边点剔除算法中构建数据点的最优深度拓展倍数， 本文分别使用50倍深度、 100倍深度以及250深度数据在TUM数据集的序列Fr1_desk、 Fr1_desk2以及Fr2_xyz中随机选取一帧进行实验。对3张图像进行ORB特征提取后的图像以及深度图如图 14 所示。

观察图 14 可以发现， 桌面与地图或者凳子在图像的连接线上存在大量的边点。本文分别使用50倍深度、 100倍深度以及250倍深度数据和特征点图像坐标{\left[u,v\right]}^{\mathrm{T}}组合构建三维数据点进行边点剔除， 并使用基于50倍深度的数据去除降采样进行实验作为对照组。效果如图 15 所示。

对上述3帧执行10次边点剔除的平均耗时如表 1 所示。

由图 15 以及表 1 可知， 使用降采样的数据进行边点剔除的效果和不使用降采样的效果相近， 但算法处理边点的耗时大幅减少， 在仅对关键帧进行边点剔除的情况下可以较好地达成VSLAM的实时性要求。在50倍深度的情况下本文提出的算法较好地剔除了因桌子遮挡地面、 电脑显示器遮挡桌面及桌面上物体而产生的误提取点。由于Kinect相机存在随着距离平方增长的随机跳变误差， 深度倍数过高时， 远处的特征点容易因邻域内奇异点数量过多而被误剔除， 例如图15（c）和图 15（d） 的第三行图像中， 桌子右上方的地面由于距离相机较远， 在使用100倍深度和250倍的深度数据进行聚类时， 地面上大量的第一类特征点被误剔除。

3.2 消融实验

由于CEP-SLAM算法加入了聚类操作， 因此需要讨论其对算法运行时间的影响， 同时， 为了验证深度调整与边点剔除算法分别对SLAM产生的影响进行消融实验， 对比了4种算法： ORB-SLAM2算法， 仅加入边点剔除的算法， 使用数据耦合误差处理的算法， 本文提出的CEP-SLAM算法。使用Fr1_desk、 Fr1_desk2以及Fr1_xyz数据集分别进行10次实验取平均值后， 对比绝对轨迹误差的均方根（RMSE）、 关键帧数量、 平均跟踪时间以及跟踪丢失次数， 实验结果如表 2 所示。

通过分析表 2 可知， 仅加入边点剔除算法在Fr1_desk、 Fr1_desk2以及Fr1_xyz数据集下的绝对轨迹误差RMSE相比ORB-SLAM2分别降低了3.8%， 2.3%和4.1%， 加入该算法剔除深度数据的准确性受RGB图与深度图间位姿偏移影响较大的边点， 提高算法的定位精度； 平均跟踪时间延长了14.3%， 16.7%和15.8%， 这是由于加入边点剔除算法后需要大量资源执行聚类， 同时因为特征点数目的减少需更多关键帧确保跟踪顺利进行， 导致跟踪耗时明显变长。单独使用深度调整算法在上述数据集下的绝对轨迹误差RMSE相比ORB-SLAM2分别降低了7.0%， 4.5%和3.1%， 加入深度调整算法有效缓解了深度数据耦合误差的产生与积累； 除Fr1_xyz因关键帧数量较少外， 深度调整算法由于需要系统资源计算两次深度调整导致平均跟踪时间延长了9.5%和4.2%， 关键帧数量大致相同。在使用CEP-SLAM算法时， 定位的绝对轨迹误差RMSE在各数据下相比ORB-SLAM2分别降低了8.2%， 6.3%和6.1%； 平均每帧的跟踪时间分别延长了19.0%， 16.6%和21.1%， 对SLAM的实时性造成了一定的负面影响； 同时， 没有丢失跟踪现象， 算法的定位精度与稳定性优于仅使用深度调整算法或仅使用边点剔除算法。这是由于深度调整算法在获取初始的深度值时， 边点的深度测量位置可能因位姿偏移， 从一个物体的边上移到另一个物体上， 导致还原出的三维坐标带有较大的误差， 影响深度调整的准确性， 使用CEP-SLAM算法可解决该问题， 增强跟踪的稳定性。

3.3 多算法对比实验

本文分别在带有回环的Fr1_room、 快速旋转的Fr1_rpy以及运动平稳并带有回环的Fr2_desk数据集上进行实验， 使用evo工具绘制了APE（Absolute Pose Error）误差随时间变化的曲线， 与ORB-SLAM2算法进行对比， 以此验证本文算法在不同运动情况下运行的鲁棒性、 稳定性和精度， 结果如图 16 所示。

分析图 16 可知： 本文所提CEP-SLAM算法的APE误差的均方根RMSE、 平均数Mean、 中位数Median以及峰值误差均低于ORB-SLAM2算法。为进一步验证本文算法的各项性能， 本文算法在Fr1_desk、 Fr1_desk2、 Fr1_xyz、 Fr1_rpy、 Fr1_room以及Fr2_desk视频序列下运行10次， 与ORB-SLAM2， ORB-SLAM3， ORB-TEDM^［22］以及Manhattan-SLAM进行对比， 使用绝对轨迹误差（ATE）的均方根RMSE以及中位数Median的10次平均值进行比较分析， 结果如表 3 所示。

由于ORB-TEDM算法并未公开源码， 所以本文直接使用论文中现有的RMSE数据。分析表 3 可知： 与ORB-SLAM2算法相比， 除了运动极其缓慢， 且深度图有大量的深度空洞区域导致部分特征点被误剔除的Fr2_xyz外， 本文提出的CEP-SLAM在大部分运动情况下的绝对轨迹误差（ATE）的均方根RMSE和中位数Median更低， RMSE在各个数据集下分别降低了8.2%， 6.1%， 6.3%， 20.1%， 13.6%和14.3%； 中位数Median在各个数据集下分别降低了7.1%， 4.1%， 6.5%， 12.1%， 28.3%和23.3%。

为了更加直观地观测轨迹误差， 本文使用TUM数据集自带的轨迹误差绘制函数将算法估计的轨迹与真实轨迹的对比投影到x⁃y坐标轴上， 并绘制估计轨迹与真实轨迹的差值， 效果如图 17 所示。

观察图 17 可知， 本文提出的CEP-SLAM算法通过加入恒加速运动模型、 剔除边点、 处理RGB-D数据耦合误差等模块， 有效提高了VSLAM的定位精度， 估计轨迹与实际轨迹相近。

4 结 　论

本文基于ORB-SLAM2框架提出了改进RGB-D相机数据耦合方式的CEP-SLAM算法。通过使用特征点深度图邻域内点跨物体以及邻域形状的特性剔除深度误差较大的边点； 使用恒加速运动模型来设置每个待跟踪帧的初始位姿； 经过两次位姿优化逐步对特征点的深度观测数据进行调整， 有效提高了算法的定位和建图精度。基于TUM公共数据集与各类VSLAM算法进行对比实验， 结果表明本文所提算法具有更高精度、 更好的稳定性和鲁棒性。

由于深度图不能和RGB图进行特征匹配从而优化获得更准确的位姿， 深度数据仍存在一定的耦合误差， 该误差在跟踪脆弱的情况下会对算法产生较大的负面影响； 同时， 引入聚类算法增大了计算开销， 小幅延长了算法的跟踪耗时。如何对深度图的位姿进行优化以进一步提高深度观测数据的可靠性， 在满足算法实时性需求的前提下将边点剔除， 加强算法在存在深度空洞位置边点剔除的可靠性， 是今后的主要研究方向。

参考文献

[1] SERVIÈRES M， RENAUDIN V， DUPUIS A， et al. Visual and visual-inertial SLAM： State of the art， classification， and experimental benchmarking［J］. Journal of Sensors， 2021， 2021： 2054828.

[2] ZOLLHÖFER M， STOTKO P， GÖRLITZ A， et al. State of the art on 3D reconstruction with RGB‐D cameras［C］//Computer Graphics Forum， 2018， 37（2）： 625-652.

[3] 周阳 ．基于RGB-D相机的室内同步定位与地图构建（SLAM）研究［D］．桂林： 桂林电子科技大学， 2023．

[4] 胡泽周 ．Kinect深度传感器深度误差分析和修正方法的研究［J］．测绘通报， 2019（S2）： 239-241．

[5] HU Zezhou. Research on depth error analysis and correction method of Kinect depth sensor［J］. Bulletin of Surveying and Mapping， 2019（S2）： 239-241.（in Chinese）

[6] ZHANG C， HUANG T， ZHANG R， et al. PLD-SLAM： A new RGB-D SLAM method with point and line features for indoor dynamic scene［J］. ISPRS International Journal of Geo-Information， 2021， 10（3）： 163.

[7] 孙超， 朱勇杰， 余林波， 等 ．基于点面特征融合的RGB-D SLAM算法［J］．计算机工程， 2023， 49（6）： 201-207．

[8] SUN Chao， ZHU Yongjie， YU Linbo， et al. RGB-D SLAM algorithm based on point-surface feature fusion ［J］. Computer Engineering， 2023， 49（6）： 201-207.（in Chinese）

[9] YUNUS R， LI Y， TOMBARI F. Manhattanslam： Robust planar tracking and mapping leveraging mixture of Manhattan frames［C］//2021 IEEE International Conference on Robotics and Automation （ICRA）. IEEE， 2021： 6687-6693.

[10] 曾攀 ．点面特征融合的室内视觉定位方法研究［D］．南京： 东南大学， 2020．

[11] 潘心冰 ．基于Kinect的移动机器人SLAM技术研究［D］．哈尔滨： 哈尔滨工业大学， 2017．

[12] 刘玉珍， 张强， 林森 ．一种改进的基于快速点特征直方图的ICP点云配准算法［J］．激光与光电子学进展， 2021， 58（6）： 283-290．

[13] LIU Yuzhen， ZHANG Qiang， LIN Sen. An improved ICP point cloud registration algorithm based on fast point feature histogram ［J］. Advances in Laser and Optoelectronics， 2021， 58（6）： 283-290.（in Chinese）

[14] ENDRES F， HESS J， STURM J， et al. 3-D mapping with an RGB-D camera［J］. IEEE Transactions on Robotics， 2013， 30（1）： 177-187.

[15] KERL C， STURM J， CREMERS D. Dense visual SLAM for RGB-D cameras［C］//2013 IEEE/RSJ International Conference on Intelligent Robots and Systems. IEEE， 2013： 2100-2106.

[16] MUR-ARTAL R， TARDÓS J D. ORB-SLAM2： An open-source slam system for monocular， stereo， and rgb-d cameras［J］. IEEE Transactions on Robotics， 2017， 33（5）： 1255-1262.

[17] CAMPOS C， ELVIRA R， RODRÍGUEZ J J G， et al. ORB-SLAM3： An accurate open-source library for visual， visual–inertial， and multimap slam［J］. IEEE Transactions on Robotics， 2021， 37（6）： 1874-1890.

[18] 陈敏 ．基于改进YOLO与光流约束的视觉SLAM算法［D］．南昌： 华东交通大学， 2022．

[19] JI T， WANG C， XIE L. Towards real-time semantic RGB-D SLAM in dynamic environments［C］//2021 IEEE International Conference on Robotics and Automation（ICRA）. IEEE， 2021： 11175-11181.

[20] WANG J L， WANG L， LI Z， et al. P-2.26： Research and implementation of augmented reality system based on improved ORB features［C］//SID Symposium Digest of Technical Papers， 2023， 54（S1）： 583-587.

[21] ZHANG W， ZHAO Y. An improved SIFT algorithm for registration between SAR and optical images［J］. Scientific Reports， 2023， 13（1）： 6346.

[22] YANG M， LI J， LI Z， et al. Self-adaptive SURF for image-to-video matching［J］. Signal， Image and Video Processing， 2023，18（1）： 751-759.

[23] FISCHLER M A， BOLLES R C. Random sample consensus： a paradigm for model fitting with applications to image analysis and automated cartography［J］. Communications of the ACM， 1981， 24（6）： 381-395.

[24] STURM J， ENGELHARD N， ENDRES F， et al. A benchmark for the evaluation of RGB-D SLAM systems［C］//2012 IEEE/RSJ international conference on intelligent robots and systems. IEEE， 2012： 573-580.

[25] YUAN J， ZHU S， TANG K， et al. ORB-TEDM： An RGB-D SLAM approach fusing ORB triangulation estimates and depth measurements［J］. IEEE Transactions on Instrumentation and Measurement， 2022， 71： 5006315.

基金资助

国家自然科学基金资助项目（62272426）; 山西省科技成果转化引导专项（202104021301055）; 山西省自然科学基金资助项目（202303021211153）