DRE-SLAM:Dynamic RGB-D Encoder SLAM for Differential-Drive Robot
DRE-SLAM:基于差速驱动机器人的动态RGB-D编码器SLAM
论文链接:https://www.mdpi.com/2072-4292/11/4/380/html
代码地址:https://github.com/ydsf16/dre_slam
1.概述
估计机器人的运动;在动态和静态环境中构建静态背景OctoMap.
输入:RGB-D摄像机和两个轮编码器信息
输出:2D机器人位姿和静态背景的OctoMap.
2.创新点
- 8代i7处理器即可
- 可以检测预定义(利用YOLO_V3检测预定义的<通过RGB图像预测>),未定义(利用多视图约束预测未定义的<通过深度图
- 图像预测>)移动对象上的动态像素
- 利用子图OctoMap构造方法,最后拼接成完整的OctoMap,可以加速重建过程并减少内存消耗
3.解决问题的准备
- 图1是配备了RGB-D摄像头的差速驱动机器人,每个车轮都安装有编码器
- 三个坐标系:世界坐标系(w),相机坐标系(c),机器人坐标系(r)
- 预先校准机器人外部参数和运动学参数:
外部参数:从相机坐标系(c)到机器人坐标系(r)的4*4变换矩阵,
运动学参数:左右轮系数Kl,Kr和车轮间距b
车轮系数:将单位刻度中的编码器位移转换成以m(米)为单位的车轮位移
- 机器人位姿:从机器人坐标系(r)到世界坐标系(w)的4*4变换矩阵
实际上,机器人的位姿仅包含三个自由度(x,y,θ),故,可由3*1的矢量表示,如式
两种机器人的位姿表达方式,即与
是等价的
其中,T括号里面的数字是矩阵的索引下表
其中,
4.系统概述
4.1 系统框图
输入:RGB-D摄像机和两个轮编码器信息
输出:2D机器人位姿和静态背景的OctoMap.
整个系统是基于稀疏特征的V-SLAM,再加上编码器信息的添加及动态像素的剔出
整个系统共分为四个模块:
- RGB-D编码器跟踪
- 动态像素剔除
- 稀疏映射
- OctoMap构造
4.2 RGB-D编码器跟踪
4.2.1 ORB特征提取
从图像中提取1600个ORB特征,分辨率为960*540像素,图像由Kinect 2.0相机产生。
使用于ORB-SLAM2相同的特征检测策略来获取均匀的特征分布。
4.2.2 编码器集成
这部分就是后一个关键帧和当前帧之间的编码器测量值,以获取机器人位姿的变化。
机器人t时刻的位姿为,则时间t+1时刻的机器人位姿由里程计模型给出:
其中Δs是平移距离,Δθ是旋转角度,Δsl/r是以米为单位的左/右车轮位移。Δsl/r由下式给出:
其中Δel/r是单位刻度中的左/右编码器位移,δl/r为零均值高斯噪声,K为比例因子。噪声的来源主要包括车轮的变形,传输错误,编码器错误,轻微滑动等。
假设机器人位姿服从高斯分布,且机器人在时刻t的位姿是,则机器人在t+1时刻的协方差如下式:
其中是位姿
的雅可比矩阵,
是左右轮位移
的雅可比,
是
的协方差,其中
为:
4.2.3 局部地图跟踪
通过最小化编码器和投影误差,来估计当前机器人的位姿。
当前帧的位姿
通过编码器积分结果从最后一个关键帧的机器人位姿传播当前帧的初始机器人位姿
,
这里大致的思想就是通过两帧之间像素ORB特征匹配来估计机器人的位姿的。但是当前帧的ORB提取到特征点的动态和静态属性未知,故可能会存在错误的匹配。故提出两个解决策略:(1)限制匹配搜索区域;(2)使用深度图像数据去除错误的匹配。地图上的三维点投影到当前帧的图片上,为u。大致如下图所示:
匹配错误的或者匹配不上的就把其对应的深度图像中的点直接删除。
那些还有有效深度的点,就把这些点3D重建出来。
4.3 动态像素剔除
动态像素剔除的方法如下图:
4.3.1 物体检测
从彩色图像中检测预定义的动态对象。直接使用COCO数据集上训练的公开模型,使用YOLO_V3做预定义检测。
4.3.2 基于多视图约束的动态像素剔除
首先剔除动态对象的边界框中的像素。然后,将重新映射的像素投影到3D以创建点云。
使用K-means方法将点云分成几个簇。假设聚类是刚体,这意味着同一聚类中的像素具有相同的运动属性。我们只需要检测哪些集群是动态集群。为了加速动态聚类检测过程,我们仅在聚类中均匀地选择少量像素(例如,100),然后使用多视图约束来判断它们的动态和静态属性。 如果动态像素的数量很大,则确定该簇是动态的。 否则,确定群集是静态的。
我们通过将像素投影到后一个关键帧(关键帧附近)进行比较来检测像素是否是动态的。如果是静态点则将像素u反投影到世界坐标系中。
最终的静态像素是通过剔除动态聚类和潜在动态对象的边界框中的像素获得的,如图4所示。应该注意的是,边界框中的一些静态像素也被移除为动态像素。 这是可以接受的,因为我们专注于消除OctoMap中的动态干扰和动态数据。 剩余的静态像素可以满足OctoMap构造的需要。
4.4 稀疏映射
4.4.1 稀疏映射
这一部分说白了就是BA优化
4.4.2 循环闭合
使用词袋方法DBow2对每个新的关键帧执行循环检测。 一旦检测到循环,将执行姿势图优化。 由于地图点都是静态的,因此闭环避免了移动物体的干扰。 所有上述优化均由Ceres Solver 执行。
4.5 OctoMap构建
OctoMap将3D空间划分为体素,并通过八叉树结构存储这些体素,从而有效降低内存成本。 每个体素都有一个占用概率。 通过积分多个观测值通过二元贝叶斯滤波器来更新该概率:
其中P(o)是先验,通常设置为0.5。
4.5.1 子OctoMap构建
在连续的关键帧内,机器人位姿误差限制在一个小范围内。因此,我们将关键帧构造为局部子OctoMap。子OctoMap是相对于局部坐标系构建的,该坐标框被设置为该子OctoMap中第一个关键帧的机器人坐标系。
4.5.2 完整OctoMap构建
这部分融合了所有子OctoMaps形成一个完整的OctoMap。完整的OctoMap是参考世界坐标系。该过程是将所有子OctoMap体素转换为完整的OctoMap并融合占用概率。
应该注意的是,这种基于子图的方法降低了完整OctoMap的精度,因为在构建子OctoMaps时,点云的精度被压缩到体素大小。此外,我们使用体素的中心点作为其融合子OctoMaps时的位置。
4.6 系统初始化
通过构造第一个RGB-D图像对作为第一个关键帧来初始化系统。动态像素因为没有其他关键帧可用仅通过对象检测进行剔除。