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

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