[发明专利]RGBD相机大型三维场景构建方法及系统

权利要求书

1.一种基于Cartographer算法的RGBD相机大型三维场景构建方法，其特征是，该方法包括以下步骤：

获取RGBD相机的深度图像与彩色图像，生成三维点云，并将点云配准到激光雷达参考系；

获取机器人姿态数据和激光雷达的扫描点数据，利用Cartographerd算法的局部优化算法对其进行处理，生成子图和机器人里程计信息；

根据机器人里程计信息，将生成的子图与对应的点云进行绑定，拼接子图生成三维地图；

所述将子图与对应的点云进行绑定的步骤包括：

记录生成的子图的位姿信息和时间戳，将生成子图时产生的时间戳与RGBD相机采集深度图像的时间戳进行对比，选择时间上最靠近子图的RGBD相机深度图像信息；

将时间上最靠近子图的RGBD相机深度图像信息和机器人位姿信息进行时间同步；

计算生成子图时机器人与子图之间的水平位移差和转动的角度差，形成旋转矩阵，使用旋转矩阵将点云变换到其对应子图的参考系中；

将变换到子图参考系中的点云与子图信息绑定，并对点云进行位移，将变换后的点云平移到子图空间所在位置；

还包括当子图位姿发生变化时，将位姿出现变化的子图所绑定的点云按照子图变化进行重新平移的步骤；

将生成的子图与对应点云进行绑定的具体步骤如下：

在机器人前进过程中，随着激光雷达扫描点的不断更新，Cartographer会通过ROS话题“/tf”的形式不断的输出机器人位姿信息；记录该位姿信息为δ＝[δ_x，δ_y，δ_z，δ_pitch，δ_roll，δ_yaw]^T，并通过对比/tf话题与图像话题的时间戳与实时RGBD相机的深度图像、RGB图像进行时间同步；同时，每当一个新的子图产生后，记录新生成子图的位姿信息为φ＝[φ_x，φ_y，φ_z，φ_pitch，φ_roll，φ_yaw]^T和时间戳，通过与图像的时间戳进行对比，选择最靠近子图的深度图像信息和机器人位姿信息。

2.根据权利要求1所述的基于Cartographer算法的RGBD相机大型三维场景构建方法，其特征是，所述点云生成方法为：

对RGBD相机进行配准，将深度相机参考系配准到彩色相机参考系下；

根据配准后深度图像中的相应的像素值计算彩色图像中的任一点在空间中的三维位置信息；

依次遍历整幅深度图像，得到相机对应的点云。

3.根据权利要求1所述的基于Cartographer算法的RGBD相机大型三维场景构建方法，其特征是，所述将点云配准到激光雷达参考系的步骤包括：

获取机器人底盘上彩色相机光心与激光雷达中心的距离及其在偏航角上的偏离的角度；

使用变换矩阵对相机点云中的每个点的坐标进行变换，将激光雷达的扫描点集与点云进行配准。

4.根据权利要求1所述的基于Cartographer算法的RGBD相机大型三维场景构建方法，其特征是，所述将位姿出现变化的子图所绑定的点云按照子图变化进行重新平移的步骤包括：

判断位姿出现变化的子图所在的区域点云，将该区域点云中的点云按照子图变化重新平移到子图空间所在位置。

5.一种基于Cartographer算法的RGBD相机大型三维场景构建系统，采用如权利要求1-4任一项所述的基于Cartographer算法的RGBD相机大型三维场景构建方法，其特征是，该系统包括：

点云配准模块，用于获取RGBD相机的深度图像与彩色图像，生成三维点云，并将点云配准到激光雷达参考系；

子图生成模块，用于获取机器人姿态数据和激光雷达的扫描点数据，利用Cartographerd算法的局部优化算法对其进行处理，生成子图和机器人里程计信息；

点云绑定模块，用于根据机器人里程计信息，将生成的子图与对应的点云进行绑定；

三维构建模块，用于拼接子图生成三维地图。