[发明专利]一种地下管道实时定位与三维重建装置及方法

权利要求书

1.一种地下管道实时定位与三维重建方法，其特征在于，包括以下步骤：

步骤1：标定RGB-D相机内参以及相机和惯性测量单元的相对位置和姿态信息；

步骤2：管道爬行机器人在管道空间自主移动，RGB-D相机和惯性测量单元实时获取数据；

步骤3：计算确定管道爬行机器人在数据采集移动过程中的精确位置；

步骤3的具体实现包括以下子步骤：

步骤3.1：采用视觉特征跟踪策略建立一套特征点管理策略来有效表示每一帧图像中特征点所在的位置，通过IMU预积分策略使高频IMU数据与低频视觉图像数据配准起来；

所述采用视觉特征跟踪策略建立一套特征点管理策略来有效表示每一帧图像中特征点所在的位置，具体实现包括以下子步骤：

步骤3.1.1：将彩色图像转换为灰度图像，然后在初始灰度图像上提取预定数量N的FAST特征点；

步骤3.1.2：利用光流法在第二张灰度图像上跟踪初始灰度图像上的FAST特征点，得到第二张图像上跟踪成功的特征点；若第二张图像上跟踪成功的特征点少于预定数量N，则在第二张图像上继续提取FAST特征点，使得特征点数量保持稳定；

步骤3.1.3：重复步骤3.1.2，依次在当前图像上跟踪上一张图像的特征点，并提取新特征点，使得特征点总数不变；

步骤3.2：通过多视图几何中的视觉SFM来求解相机位置和姿态信息和场景几何结构，从而构建稀疏的初始地图；通过将带有尺度的IMU预积分和不带尺度的视觉SFM关联起来，从而恢复陀螺仪的偏差、系统的速度、重力矢量以及度量尺度；

步骤3.3：进行相机位置和姿态信息和场景几何结构的优化计算，构建包含三个残差项的优化函数，分别是边缘化的先验信息、视觉的重投影误差项和IMU的测量残差项；

通过优化函数来进行优化计算，优化函数为：

其中为待优化的所有状态变量；λ_l,l∈[0,m]表示第l个特征点在第一次观测时的逆深度信息，m为优化窗口内所有特征点数量；C表示所有的图像数据帧，B表示所有的惯性测量单元数据帧；

表示第k张图像时刻，上标中的w表示世界坐标系，b_k指第k帧图像时刻的惯性坐标系；惯性测量单元在世界坐标系中的位置为速度为姿态为加速度的零偏为b_a和陀螺仪的零偏为b_g，n为总图像帧数量；

表示相机和惯性测量单元之间的位置和姿态所有上下标中，b都代表惯性测量单元坐标系，c代表相机坐标系；

||r_p-H_Pχ||²为边缘化的先验信息，r_p-H_Pχ为边缘化残差，r_p表示边缘化先验信息，H_P为边缘化信息矩阵；

为视觉的重投影误差项，p为Huber核函数，为视觉测量残差，其中

表示第l个特征点在第i张图像中位置，表示第l个特征点在第j张图像中位置，表示针孔相机模型的反投影过程；

为IMU的测量残差项，为IMU测量残差；

式中g^w表示重力向量，[·]表示四元数的向量部分，是IMU测量的预积分项；Δt_k是第k+1帧图像和第k帧图像间的时间间隔；b_abk表示在第k张图像时刻，惯性坐标系中加速度的零偏b_a；b_wbk表示在第k张图像时刻，惯性坐标系中陀螺仪的零偏b_w；

步骤4：利用步骤3确定的管道爬行机器人的位置和步骤1确定的RGB-D相机和惯性测量单元的相对位置和姿态信息，恢复场景的三维重建模型；所述三维重建包括管道的三维尺寸，几何形状和颜色纹理。

2.根据权利要求1所述的地下管道实时定位与三维重建方法，其特征在于：步骤2中所述RGB-D相机和惯性测量单元实时获取数据，包括所述管道爬行机器人的运动加速度和角速度、管道空间内2D图像数据和深度图像数据。

3.根据权利要求1所述的地下管道实时定位与三维重建方法，其特征在于：步骤4中，利用步骤3确定的管道爬行机器人的相机相对位置和姿态信息，场景的几何结构以及步骤2采集的深度图像数据恢复地下管道的真实三维场景信息，并进行颜色纹理贴图。

4.一种地下管道实时定位与三维重建装置，用于实现权利要求1-3任一项所述的方法；其特征在于：由管道爬行机器人、处理器、RGB-D相机和惯性测量单元组成；

所述管道爬行机器人，用于在地下管道空间自主移动；

所述处理器，用于采集并处理RGB-D相机和惯性测量单元数据，计算出管道爬行机器人在地下管道中的实时位置，重建出地下管道的三维模型，最终控制管道爬行机器人的自主移动；

所述RGB-D相机，用于所述管道爬行机器人在地下管道空间内外移动时采集彩色图像数据和深度图像数据；

所述RGB-D相机，用于所述管道爬行机器人在地下管道空间内外移动时采集加速度数据和陀螺仪数据；

所述处理器、RGB-D相机和惯性测量单元固定设置在所述管道爬行机器人上，并通过导线一一与所述管道爬行机器人中设置的中央处理器连接。