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