CN110766785B - 一种地下管道实时定位与三维重建装置及方法 - Google Patents

Abstract

本发明公开了一种地下管道实时定位与三维重建装置及方法，装置由管道爬行机器人、处理器、RGB‑D相机和惯性测量单元组成；本发明首先对RGB‑D相机和惯性测量单元进行标定，包括相机的内参以及相机和惯性测量单元的外参，即相机和IMU的相对位置和姿态关系；然后操控管道爬行机器人在管道空间移动获取传感器实时数据；通过算法实时计算爬行机器人在地下管道中的精确位置，并实时重建出地下管道的三维空间模型；本发明实现了爬行机器人在地下管道中的实时定位与地下管道三维重建。

Description

一种地下管道实时定位与三维重建装置及方法

技术领域

本发明属于测绘技术领域，涉及一种地下管道实时定位与三维重建装置及方法，具体涉及一种基于地下管道爬行机器人及利用RGB-D相机和惯性测量单元 (IMU)进行地下管道实时定位与三维重建的装置及方法。

背景技术

地下管道(如城市排水管网)是“城市静脉”，是城市安全稳定运行的基础，是智慧城市建设的重要内容,现代化的地下管道系统成为衡量一个城市基础设施完善程度和城市管理水平的重要标志之一。管道检测是实现对地下管道精细化管理和预防性修复的前提，对维护城市功能正常运转，确保生命财产安全是十分必要的。比如污水和雨水管道承担着城市污水收集和雨水排除任务，其有效发挥功能是保障城市安全运行、保障生命财产安全的前提，因此，全国各大中城市都在大力建设“智慧水务”系统。

由于地下管道的不可见性，总是等到发生事故时才被人们重视，造成大量的经济损失，甚至给生命财产安全带来隐患。目前，普遍公认的解决办法是在地下管道建设竣工和使用阶段应定期对管道现状进行测绘，对潜在的结构性和功能性损坏进行检测并及时修复。对管道现状进行测绘检测是进行精细化管理和预防性修复的前提。由于管道环境的特殊性复杂性，人员难以直接到达，常规测绘方法难以实施。国内外对地下管道检测机器人了较多研发，如特瑞升、雷迪世纪和中仪物联等公司的管道机器人代表了目前国内外产业界的最高水平。

但从管道检测服务行业来看，现有管道检测技术和装备有两大缺陷：

1)数据可视化效果不好，主要靠人眼判读2D图像数据(如CCTV视频(闭路电视监控系统Closed-Circuit Television))，而2D图像数据不能很好反映管道内3D真实情况，另外管道检测耗时耗力，需要较长时间暂停管道使用，甚至需要开挖路面；

2)在管道内缺乏实时定位系统，无法对检测数据进行空间定位，降低了数据的有效利用和系统整体效能发挥(如对决策的支持效率和自动化程度)。

发明内容

为了解决上述技术问题，本发明提供了一种新型自动化、智能化地下管道实时定位与三维重建装置及方法。

本发明的装置所采用的技术方案是：一种地下管道实时定位与三维重建装置，其特征在于：由管道爬行机器人、处理器、RGB-D相机和惯性测量单元组成；

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

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

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

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

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

本发明的方法所采用的技术方案是：一种地下管道实时定位与三维重建方法，其特征在于，包括以下步骤：

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

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

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

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

作为优选，步骤2中所述RGB-D相机和惯性测量单元实时获取数据，包括所述管道爬行机器人的运动加速度和角速度、管道空间内2D图像数据和深度图像数据。

作为优选，步骤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 的测量残差项。

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

其中为待优化的所有状态变量。

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

表示相机和惯性测量单元之间的位置和姿态。

表示第l个特征点在第一次观测时的逆深度信息，m为优化窗口内所有特征点数量；

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

为视觉的重投影误差项，p为Huber核函数，为视觉测量残差，其中表示第l个特征点在第i张图像中位置，表示第l个特征点在第j张图像中位置，表示针孔相机模型的反投影过程；

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

式中g^w表示重力向量，[·]表示四元数的向量部分，是IMU测量的预积分项。

作为优选，利用步骤3确定的管道爬行机器人的相机相对位置和姿态信息，场景的几何结构以及步骤2采集的深度图像数据恢复地下管道的真实三维场景信息，并进行颜色纹理贴图。

与现有技术相比，本发明的有益效果是：

(1)本发明的管道爬行机器人装备具有在管道内外等复杂环境中无缝定位能力。无缝定位带来的益处是：机器人能按照规划的路径自主移动，自动化采集数据；根据采集的数据，机器人可以自动生成路径轨迹。

(2)本发明的管道爬行机器人装备和方法数据采集自动化程度高，采集效率高，不需人工操作。本发明方法集成了RGB-D相机和视觉惯性测量单元两个传感器，能够获得管道爬行机器人在地下管道空间内外移动时采集彩色图像数据，深度图像数据，加速度数据以及陀螺仪数据。和现有的装备与方法相比(如通过人工遥控采集CCTV摄像头数据)，数据采集过程自动化程度显著提高，计算机自动化采集和处理确保了数据的准确性和精确性，多传感器融合方法提高了数据的丰富性、多样性，能够同时采集多维数据。

(3)本发明的管道爬行机器人装备和方法数据处理智能化程度高，精确度高，不需人工操作。本发明装备和方法采用多传感器采集管道空间中多维数据，从而计算爬行机器人在地下管道中的实时位置，生成带有地理坐标的管道现状类型，提取管道几何形状和属性现状等信息。和现有的装备与方法相比(如通过人工遥控采集CCTV摄像头数据)，计算爬行机器人在地下管道内的实时位姿轨迹和恢复地下管道三维模型过程自动化程度显著提高，计算机自动化处理确保了检测结果的完整性和精确性，大大减少了人工现场作业的需求。

附图说明

图1为本发明实施例的方法流程图；

图2为本发明实施例中获得的一个地下管道内空间的视觉数据图，包括彩色图像和深度图像；

图3为本发明实施例中计算得到的爬行机器人在地下管道内的实时位置轨迹和地下管道的稀疏几何结构；

图4和图5为本发明实施例中恢复的场景三维重建模型。

具体实施方式

为了便于本领域普通技术人员理解和实施本发明，下面结合附图及实施例对本发明作进一步的详细描述，应当理解，此处所描述的实施示例仅用于说明和解释本发明，并不用于限定本发明。

本发明提供的一种地下管道实时定位与三维重建装置，由管道爬行机器人、处理器、RGB-D相机和惯性测量单元组成；

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

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

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

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

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

请见图1，本发明提供的一种地下管道实时定位与三维重建方法，包括以下步骤：

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

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

本实施例中，RGB-D相机和惯性测量单元实时获取数据，包括管道爬行机器人的运动加速度和角速度、管道空间内2D图像数据和深度图像数据。

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

本实施例的具体实现包括以下子步骤：

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

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

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

步骤3.1.2：利用光流法在第二张灰度图像上跟踪初始灰度图像上的FAST 特征点，得到第二张图像上跟踪成功的特征点；若第二张图像上跟踪成功的特征点少于预定数量N(本实施例取500个)，则在第二张图像上继续提取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。

目前，管道爬行机器人在地下管道内的定位方案只有测量线缆长度一种方式，通过线缆盘设备测量线缆长度只能获得一维数据，并且一维数据所代表的的实际含义只是管道爬行机器人所行驶的路程长度，并不知道管道爬行机器人在地下管道内的真实三维坐标，而本申请方案相比于现有技术，可以获得管道爬行机器人在地下管道内的实时三维位置信息。

另外目前地下管道的检测主要还是依赖人眼观察视频图像来判别地下管道的缺陷，但是用二维视频图像来表示地下三维管道肯定会带来信息的丢失，导致地下管道信息不直观，人眼判别不可靠。本方案通过RGB-D相机和惯性测量单元可以重建出地下管道的真实三维模型，使得地下管道的检测如身临其境一般。

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

本实施例中，利用步骤3确定的管道爬行机器人的相机相对位置和姿态信息，场景的几何结构以及步骤2采集的深度图像数据恢复地下管道的真实三维场景信息，并进行颜色纹理贴图。

图2为本发明实施例中获得的一个地下管道内空间的视觉数据图，包括彩色图像和深度图像。

图3为本发明实施例中计算得到的爬行机器人在地下管道内的实时位置轨迹和地下管道的稀疏几何结构。

图4和图5为本发明实施例中恢复的场景三维重建模型。

本发明具有在地下管道内外等复杂环境中无缝定位、管道空间数据自动采集、以及管道几何形状和属性现状信息等自动识别和提取等功能，是一种多传感器融合的机器人复杂环境无缝定位、自动化数据采集和智能化目标识别方法和机器人智能装备。

应当理解的是，本说明书未详细阐述的部分均属于现有技术。

应当理解的是，上述针对较佳实施例的描述较为详细，并不能因此而认为是对本发明专利保护范围的限制，本领域的普通技术人员在本发明的启示下，在不脱离本发明权利要求所保护的范围情况下，还可以做出替换或变形，均落入本发明的保护范围之内，本发明的请求保护范围应以所附权利要求为准。

Claims (4)

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

Images