基于 RGB-D 数据的实时 SLAM 算法
基于 RGB-D 数据的实时 SLAM 算法付梦印 1;2，吕宪伟 1;2，刘 彤 1;2，杨 毅 1;2，李星河 1;2，李 玉 1;2（1.复杂系统智能控制与决策国家重点实验室，北京 100081； 2.北京理工大学自动化学院，北京 100081）摘 要：提出了一种基于 RGB-D 数据的实时 SLAM（同时定位与地图创建）算法，得到机器人的 6D 位姿并构建环境的 3D 地图．首先提取 RGB 图像的特征点，并利用高斯混合模型得到特征点处的颜色和 3 维坐标，以及对应的协方差矩阵．然后利用 ICP（迭代最近点）算法，得到当前帧特征点与环境模型特征点集的变换矩阵 Tt，并在 Tt 周围撒点采样，得到观测最优的传感器位姿矩阵 Pt ．之后利用 Pt 将当前帧的密集点云变换到全局坐标系下，构建环境 3D 地图．最后，利用卡尔曼滤波器对环境模型特征点集进行更新．为防止特征点较少时，ICP 算法误差较大，本文加入粒子采样步骤，有效地提高了定位精度．此外，在将当前帧特征点与环境模型特征点进行数据关联时，引入颜色信息提高了数据关联的准确率．针对 FR1 基准包，本文算法的最小定位误差为 1.7 cm，平均定位误差为 11.9 cm，每帧数据平均处理时间为 31 ms，可以满足机器人实时 SLAM 的要求．
关键词：RGB-D；同时定位与地图创建；机器人；迭代最近点；卡尔曼滤波
中图分类号：TP242 文献标识码：A 文章编号：1002-0446(2015)-06-0683-10
Real-time SLAM Algorithm Based on RGB-D DataFU Mengyin1;2，LU Xianwei ¨ 1;2，LIU Tong1;2，YANG Yi1;2，LI Xinghe1;2，LI Yu1;2(1.Key Laboratory of Intelligent Control and Decision of Complex Systems, Beijing 100081, China;2. School of Automation, Beijing Institute of Technology, Beijing 100081, China)Abstract: A real-time SLAM (simultaneous localization and mapping) algorithm based on RGB-D (RGB-depth) data is proposed, which can get 6D poses of the robot and build the 3D map of the environment. Firstly, key features of the RGB images are extracted, whose 3D coordinates and colors, along with their corresponding covariance matrices, are then calculated using the Gaussian mixture model. Then the transformation matrix Tt between features in the current frame and feature set of the environment model can be gotten by using ICP (iterative closest point) algorithm, and sensor pose matrix Pt for optimal observation is obtained by sampling around Tt. Based on Pt, the dense point clouds in the current frame are then transformed into global coordinate to build the 3D map. Finally, the feature set of the environment model is updated using Kalman filter. To decrease the error of ICP, especially when the features are poor, particle sampling is conducted to improve the localization accuracy. Moreover, the color information is used to increase the success rate of data association between the features in the current frame and those in the environment model. It costs about 31 ms for each frame. In the mean time,the minimum localization error for FR1 benchmark is 1.7 cm and the average error is 11.9 cm. Therefore, the algorithm is accurate and fast enough for real-time SLAM.
Keywords: RGB-D; simultaneous localization and mapping (SLAM); robot; iterative closest point; Kalman filter
页:
[1]