基于RGB-D数据的实时SLAM算法

来源：机器人杂志2015年第6期北京时间：

作者：付梦印, 吕宪伟, 刘彤, 杨毅, 李星河, 李玉

摘要：提出了一种基于RGB-D数据的实时SLAM（同时定位与地图创建）算法，得到机器人的6D位姿并构建环境的3D地图．首先提取RGB图像的特征点，并利用高斯混合模型得到特征点处的颜色和3维坐标，以及对应的协方差矩阵．然后利用ICP（迭代最近点）算法，得到当前帧特征点与环境模型特征点集的变换矩阵Tt，并在Tt周围撒点采样，得到观测最优的传感器位姿矩阵Pt．之后利用Pt将当前帧的密集点云变换到全局坐标系下，构建环境3D地图．最后，利用卡尔曼滤波器对环境模型特征点集进行更新．为防止特征点较少时，ICP算法误差较大，本文加入粒子采样步骤，有效地提高了定位精度．此外，在将当前帧特征点与环境模型特征点进行数据关联时，引入颜色信息提高了数据关联的准确率．针对FR1基准包，本文算法的最小定位误差为1.7cm，平均定位误差为11.9cm，每帧数据平均处理时间为31ms，可以满足机器人实时SLAM的要求．

关键词：RGB-D, 同时定位与地图创建, 机器人, 迭代最近点, 卡尔曼滤波