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

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

Real-time SLAM Algorithm Based on RGB-D Data