CN116958439A - 一种满水环境中基于多传感融合的管道三维重建方法 - Google Patents

Abstract

本发明公开了一种满水环境中基于多传感融合的管道三维重建方法，包括：潜航器初始对准；构建两帧图像之间的位姿转换模型；构造两帧点云数据间的位姿转换模型；采集IMU数据并解算潜航器的当前位姿，结合非完整约束以及卡尔曼滤波器不断矫正解算后的位姿；构造的位姿转换模型，构建对应的代价函数，并进行潜航器位姿的优化；构建IMU、声呐、摄像头数据融合模型，使用粒子滤波器进行数据的融合，从而获得可靠的潜航机器人的三维位姿信息；将声呐点云数据进行拼接和坐标变换，最终获得完整的管道三维结构信息。本发明可提高管道三维结构的重建质量以及重建精度。

Description

一种满水环境中基于多传感融合的管道三维重建方法

技术领域

本发明属于满水环境下城市管道缺陷识别技术领域，具体涉及一种满水环境中基于多传感融合的管道三维重建方法。

背景技术

管道作为运输手段，在石油、化工、国防、天然气、排污等多个领域应用广泛，极大方便了人类社区运输，同时也带来了巨大的经济效益。然而，随着管道服役时间的增加，其内壁会附着大量黏性污物，除此之外，在腐蚀和重压等因素耦合作用下，管道内壁不可避免地出现脱节、破裂、错位、塌陷现象，上述因素不仅影响运输效率，而且会带来安全隐患，甚至产生严重的经济损失。因此，通过及时有效的方法对管道进行全面的维护和故障排查，提高管道运行的安全性，能够减少国家的经济损失，保障生态环境的稳定，对我国可持续发展战略具有重要意义。

对于地下管道健康状态检测的工作，由于其环境的特殊性与复杂性，目前业界主要采用装载摄像头的机器人采集管道内的可视化信息，并将视频数据传输至智能检测平台系统，由专业工程师对可视化信息进行检查判读，从而完成管道健康状态的检测。然而上述检测手段受主观因素影响大，专业人员需长时间分析海量视频数据，不仅费时费力、效率低下，而且错漏检率高。更重要的是，上述方法的检测结果不能明确缺陷在管段中的三维位置信息(管道方向上的位置以及环向位置)，而这一点在实际工程中是非常有必要的。

由于传统的电磁波、光波在水中衰减较快，声波成为水下目标探测的有效手段，声呐传感器基于声学原理探测目标物，不仅可用于满水满污环境，而且可提供准确的量化数据，获得多组声波点云数据后，将其统一到全局坐标系后，即可重建出管道内表面的三维结构。然而，管道内部存在着大量的散射体，声波在管道中传播时遇到不均匀的介质会产生散射，形成散射声场，混响影响声呐传感器的测距精度，影响管道三维结构的重建质量。

发明内容

本发明所要解决的技术问题是针对上述现有技术的不足，提供一种满水环境中基于多传感融合的管道三维重建方法，通过融合摄像头、IMU、声呐的数据，基于图优化和卡尔曼滤波器的数据融合，完成管道三维结构的可靠重建，提高管道三维结构的重建质量以及重建精度。

为实现上述技术目的，本发明采取的技术方案为：

一种满水环境中基于多传感融合的管道三维重建方法，包括：

步骤一：采集IMU数据并进行满水环境下潜航器初始对准，然后控制潜航器在管道中运动；

步骤二：在潜航器运行过程中，摄像头模组采集运动图像数据，并构建两帧图像之间的位姿转换模型；

步骤三：多波束声呐传感器采集声波点云数据并进行点云畸变矫正和区域滤波；

步骤四：对步骤三处理后的声波点云数据进行线特征提取，并构造两帧点云数据间的位姿转换模型；

步骤五：采集IMU数据并解算潜航器的当前位姿，结合非完整约束以及卡尔曼滤波器不断矫正解算后的位姿；

步骤六：结合步骤二和步骤四构造的位姿转换模型，构建对应的代价函数，并进行潜航器位姿的优化；

步骤七：融合IMU、声呐、摄像头数据，使用粒子滤波器进行代价函数求解，从而获得可靠的潜航机器人的三维位姿信息，三维位姿态信息即是潜航机器人在管道中的姿态和位置信息，该信息为管道三维点云重建提供支撑；

步骤八：将声呐点云数据进行拼接和坐标变换，结合步骤七得到的三维位姿信息，最终获得完整的管道三维结构信息。

为优化上述技术方案，采取的具体措施还包括：

上述的步骤一读取IMU的角速度和加速度数据，通过姿态分解技术，构造初始对准优化方程，设置权重系数，并四元数估计法求解初始姿态矩阵，对初始姿态矩阵进行精度判断，满足精度要求则实现潜航器初始对准。

上述的初始对准优化方程为：

式中，w_i表示权重系数，表示b系相对于n系的初始姿态矩阵，/>和/>表示第i组量测向量，E₃表示3*3的单位矩阵，det符号表示计算矩阵的行列式；N＞2。

上述的步骤二在潜航器运行过程中，摄像头模组不断采集外部世界的运动图像数据，分析图像不同时刻的帧数据，使用维纳滤波算法去除图像噪点信息后，结合FAST角点检测算法识别出每帧图像的关键点信息，即可构建出两帧图像之间的位姿转换模型。

上述的FAST角点检测算法流程如下：

(1)：在图像中选取像素p，亮度记作I_p，同时设置阈值T，以像素p位中心，选取半径为3的圆上的16个像素点，如果选取的圆上，有连续的N个点的亮度大于I_p+T或者小于I_p-T，那么像素p被认为是特征点，之后不断循环，找到一张图像中所有满足条件的特征点，作为特征点的先验信息；

(2)：对(1)中得到的所有满足条件的特征点进行Harris响应值的计算，然后选取前N个具有最大响应值的角点作为最终的角点集合；

(3)：构造图像金字塔，并在金字塔的每一层上检测角点，同时基于灰度质心法为特征点添加尺度描述；

(4)：计算关键点的描述子信息，采用BRIEF二进制描述子，为(3)中的每个特征点都添加尺度信息。

上述的步骤三包括：多波束声呐传感器采集声波点云数据，利用IMU数据估计潜航器的短时运动状态，进而矫正声呐点云的运动畸变，在点云运动畸变处理过程中同时采用区域滤波算法对点云数据进行去噪。

上述的步骤四选择线特征作为声波点云数据的关键特征，基于曲率区域增长方法分割三维点云平面，对每个点云平面进行二维投影，并进行网格化和二值化，利用稳健的二维线段检测方法获得平面的线特征，最终产生多帧可靠的三维声波点云数据集，并构造出两帧点云数据间的位姿转换模型。

上述的步骤五不断采集IMU的数据，获取潜航器当前时刻的角速度和加速度信息后，利用惯性导航解算方程计算潜航器当前的姿态、速度、位置信息，引入非完整约束，将潜航器横向速度、纵向速度以及航向角变化量基本为零作为观测约束，构造对应的观测模型，同时以惯性导航误差方程作为卡尔曼滤波器的过程模型；在此基础上，基于卡尔曼滤波器对姿态、速度、位置误差进行估计，通过合理的反馈补偿后，即可获得较为精准的潜航器姿态、速度、位置信息。

上述的卡尔曼滤波器包含如下公式：

时间更新：

量测更新：

P_k＝(E-K_kH_k)P_k/k-1 (0-12)

式中，和P_k/k-1分别表示状态的一步预测和一步预测协方差矩阵；

K_k表示卡尔曼滤波增益系数，和P_k表示状态的最优估计值以及对应的协方差矩阵；

Q_k和R_k分别表示过程噪声和观测噪声协方差矩阵。

上述的步骤六构建的代价函数如下所示：

e＝z-h(ξ,p)

式中，z_ij为在位姿ξ_i处观察的路标p_j产生的数据，ξ为位姿对应的李代数形式，p为三维空间的点云信息。

本发明具有以下有益效果：

本发明专利基于多传感融合的方式，基于图优化模型，实现了管道表面三维重建，帮助工程师可以更好的识别出管道缺陷信息；多传感融合的方式不仅可以提高重建模型的精度，而且具有较高的鲁棒性，在外界环境干扰下，系统仍然可以稳定可靠工作。

附图说明

图1为本发明的整体结构示意图；

图2为本发明步骤一中基于IMU的初始对准程序流程图；

图3为本发明步骤二中图像特征点检测流程图；

图4为本发明步骤三和步骤四中声纳点云数据集构建流程图；

图5为本发明步骤五中基于非完整约束的潜航器位姿解算流程图；

图6为本发明方法完整流程图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。

本发明中的步骤虽然用标号进行了排列，但并不用于限定步骤的先后次序，除非明确说明了步骤的次序或者某步骤的执行需要其他步骤作为基础，否则步骤的相对次序是可以调整的。可以理解，本文中所使用的术语“和/或”涉及且涵盖相关联的所列项目中的一者或一者以上的任何和所有可能的组合。

本发明的一种满水环境中基于多传感融合的管道三维重建方法，基于管道三维重建系统实现，如图1所示，该系统主要包括：推进器、主控板、IMU、摄像头、电池仓、多波束声呐，为了满足满水环境中地下管网三维重建的需求，所述设备搭载于一款水下潜航机器人内部，IMU和主控板固定在亚克力管中，并通过USB以及串口的的方式连接，主控板实时采集IMU的加速度和角速度信息，摄像头通过支架固定在潜航器前方，其镜头一直维持在潜航器的运动方向；多波束声呐搭载于潜航器前方位置，基于声学原理采集潜航器与管壁之间的距离点云信息，电池仓内部搭载了一款大容量的锂电池，作为潜航机器人的电力来源，主控板分别通过摄像头、IMU、多波束声呐采集潜航器的运动图像、加速度和角速度量以及声波点云数据，之后结合本专利所提出的基于多传感融合的管道三维重建方法，，完成管道三维结构的重建任务。

如图6所示，本发明基于多传感融合的管道三维重建方法包括以下基本步骤：

步骤一：采集IMU数据并进行满水环境下潜航器初始对准，然后控制潜航器在管道中运动；

为了保证所提出的算法不会陷入局部最优的情况，算法运行前潜航器需保持静止5分钟左右，通过采集5分钟的惯性传感器IMU数据，利用晃动基座初始对准算法计算出初始时刻潜航机器人的横滚、俯仰、航向信息，为图优化算法提供初始先验信息，之后，操作人员可操控潜航器在管道中运动。

所述的IMU采用激光捷联惯导系统，陀螺仪和加速度计的零偏稳定性分别为0.01°/h和10-5g，满足初始对准以及管道位姿检测的需求。

采用基于四元数的Qmethod方法作为步骤一中晃动基座初始对准求解算法。

读取IMU的角速度和加速度数据，通过姿态分解技术，构造初始对准优化方程，设置权重系数，并四元数估计法求解初始姿态矩阵，对初始姿态矩阵进行精度判断，满足精度要求则实现潜航器初始对准。

图2为步骤一中基于IMU的初始对准程序流程图，具体步骤如下：

惯性导航系统速度微分方程如下所示：

其中b系代表潜航器载体坐标系，n系代表导航坐标系，i系代表惯性坐标系。间接法SINS初始对准的目标即确定初始时刻潜航器机体坐标系b系相对于地理坐标系n系的姿态变换矩阵f^d表示加速度计的输出，/>表示b系相对于n系的姿态变换矩阵，/>表示因地球自转效应引起的导航坐标系的旋转，/>为载体在地球表面运动所产生的导航系的旋转。

为了确定初始时刻的姿态变换矩阵采用姿态分解技术并运用矩阵的链式乘积法则，t时刻的姿态旋转矩阵/>可分解为

将方程(0-14)代入(0-13)中可得

当潜航器处于静止或者仅做角运动时，和vⁿ(t)的取值约为零，忽略这两项，并在方程(0-15)两端同时乘以/>方程(0-15)可转化为

式中

其中，ω_ie表示地球自转角速度，L表示地理纬度，和/>分别表示载体坐标系和导航坐标系的旋转变化。

在实际工程中，初始姿态矩阵的求解通常会转化为带约束的最小二乘问题，即：构造出一个与观测量/>和/>相关的损失函数式中N＞2，/>和/>表示带有误差的量测，在满足/>正交约束特性前提下，损失函数取得极小值时，相应的/>即为最优初始姿态矩阵。

式中，w_i表示权重系数，表示b系相对于n系的初始姿态矩阵，/>和/>表示第i组量测向量，E₃表示3*3的单位矩阵，det符号表示计算矩阵的行列式；N＞2。

最后，采用四元数估计法求解方程(0-21)即可完成初始姿态矩阵如若满足精度要求，则将其结果作为最终输出，否则继续读取IMU角速度和加速度信息，不断重复迭代。

步骤二：在潜航器运行过程中，摄像头模组采集运动图像数据，并构建两帧图像之间的位姿转换模型；

在潜航器运行过程中，摄像头模组不断采集外部世界的运动图像数据，分析图像不同时刻的帧数据，使用维纳滤波算法去除图像噪点信息后，结合FAST角点检测算法识别出每帧图像的关键点信息，即可构建出两帧图像之间的位姿转换模型；

角点检测算法的阈值T设置0.2。

如图3为本发明步骤二中两帧图像位姿转换流程图，在完成潜航器初始对准后，摄像头模组会不断采集外部运动图像数据，采用维纳滤波算法去除图像拍摄的诸多噪点信息后，基于FAST角点检测算法完成图像角点信息的检测，FAST角点检测算法流程如下：

(1)：在图像中选取像素p，亮度记作I_p，同时设置阈值T，以像素p位中心，选取半径为3的圆上的16个像素点，如果选取的圆上，有连续的N个点的亮度大于I_p+T或者小于I_p-T，那么像素p可被认为是特征点，之后不断循环，找到一张图像中所有满足条件的特征点，作为特征点的先验信息；

(2)：对(1)中得到的所有满足条件的特征点进行Harris响应值的计算，然后选取前N个具有最大响应值的角点作为最终的角点集合；

(3)：构造图像金字塔，并在金字塔的每一层上检测角点，进而解决尺度问题，同时基于灰度质心法为特征点添加尺度描述；

(4)：计算关键点的描述子信息，采用BRIEF二进制描述子，为(3)中的每个特征点都添加尺度信息。

步骤三：多波束声呐传感器采集声波点云数据并进行点云畸变矫正和区域滤波；

多波束声呐传感器采集声波点云数据，利用IMU数据估计潜航器的短时运动状态，进而矫正声呐点云的运动畸变，在点云运动畸变处理过程中同时采用区域滤波算法对点云数据进行去噪，保证声呐点云数据的可靠性。

步骤四：对步骤三处理后的声波点云数据进行线特征提取，并构造两帧点云数据间的位姿转换模型；

选择线特征作为声波点云数据的关键特征，基于曲率区域增长方法分割三维点云平面，对每个点云平面进行二维投影，并进行网格化和二值化，利用稳健的二维线段检测方法获得平面的线特征，最终产生多帧可靠的三维声波点云数据集，并构造出两帧点云数据间的位姿转换模型。

图4为发明专利权利要求步骤三和步骤四中声纳点云数据集构建流程图，多波束声纳传感器采集声波点云信息，为了矫正声纳点云的运动畸变，本发明专利利用高频的IMU数据估计出潜航器的短时运动状态，从而恢复每个点的运动；在点云运动畸变处理过程中，采用区域滤波算法对点云数据进行去噪，降低管道中大量的混响干扰，保证声呐点云数据的可靠性。

之后，对声波点云数据进行特征点提取，所提取的方式是基于曲率区域增长方法，通过分割三维点云平面，并对每个点云平面二维投影以及网格化和二值化，利用稳健的二维线段检测方法获得平面的线特征，最终产生多帧可靠的三维声波点云数据集，并构造出两帧点云数据间的位姿转换模型。

步骤五：采集IMU数据并解算潜航器的当前位姿，结合非完整约束以及卡尔曼滤波器不断矫正解算后的位姿；

考虑到光照变化以及混响干扰会影响摄像头的成像结果以及点云数据的可靠性，主控板通过采集IMU的角速度和加速度信息，利用惯性导航解算方法计算潜航器的当前姿态和位置，并结合非完整约束以及卡尔曼滤波器不断矫正解算后的姿态和位置量；

所述的非完整约束为潜航器的横向速度和纵向速度为0，潜航器的偏航角恒定。

图5为步骤五中基于非完整约束的潜航器位姿解算流程图，首先主控板不断采集IMU的数据，获取潜航器当前时刻的角速度和加速度信息后，利用惯性导航解算方程计算潜航器当前的姿态、速度、位置信息，考虑到姿态解算算法中的误差会不断累积，为了抑制不断增长的误差，本发明专利引入非完整约束，将潜航器横向速度、纵向速度以及航向角变化量基本为零作为观测约束，构造对应的观测模型，同时以惯性导航误差方程作为卡尔曼滤波器的过程模型；在此基础上，基于卡尔曼滤波器对姿态、速度、位置误差进行估计，通过合理的反馈补偿后，即可获得较为精准的潜航器姿态、速度、位置信息，所采用的卡尔曼滤波器如下所示。

完整的卡尔曼滤波器包含如下5个公式：

时间更新：

量测更新：

P_k＝(E-K_kH_k)P_k/k-1 (0-26)

式中，和P_k/k-1分别表示状态的一步预测和一步预测协方差矩阵；

K_k表示卡尔曼滤波增益系数，和P_k表示状态的最优估计值以及对应的协方差矩阵；

Q_k和R_k分别表示过程噪声和观测噪声协方差矩阵，一般要求Q_k半正定且R_k正定。

步骤六：结合步骤二和步骤四构造的位姿转换模型，构建对应的代价函数，代价函数是各个传感器模型误差项的叠加，步骤二和步骤四分别基于摄像头和声纳构建的位姿转换模型，需要构建代价函数对二者的结果进行进一步优化，从而获得精确的潜航器位姿；

结合步骤二和步骤四构造的位姿转换模型，构建对应的代价函数，并采用图结构描述构建的代价函数，使用谷歌提供的g2o优化器进行潜航器位姿的优化。

构建的代价函数整体的形式如下所示：

e＝z-h(ξ,p)

式中，z_ij为在位姿ξ_i处观察的路标p_j产生的数据，ξ为位姿对应的李代数形式，p为三维空间的点云信息。

步骤七：融合IMU、声呐、摄像头数据，使用粒子滤波器求解步骤六中构建的代价函数的极小值，当粒子群算法收敛或达到最大迭代次数后，所获得的位姿信息即为潜航器的最佳位姿，从而获得可靠的潜航机器人的三维位姿信息，三维位姿态信息即是潜航机器人在管道中的姿态和位置信息，该信息为管道三维点云重建提供支撑；

步骤八：将声呐点云数据进行拼接和坐标变换，结合步骤七得到的三维位姿信息，最终获得完整的管道三维结构信息，可为工程人员识别出管道缺陷提供有力参考。

对于本领域技术人员而言，显然本发明不限于上述示范性实施例的细节，而且在不背离本发明的精神或基本特征的情况下，能够以其他的具体形式实现本发明。因此，无论从哪一点来看，均应将实施例看作是示范性的，而且是非限制性的，本发明的范围由所附权利要求而不是上述说明限定，因此旨在将落在权利要求的等同要件的含义和范围内的所有变化囊括在本发明内。不应将权利要求中的任何附图标记视为限制所涉及的权利要求。

此外，应当理解，虽然本说明书按照实施方式加以描述，但并非每个实施方式仅包含一个独立的技术方案，说明书的这种叙述方式仅仅是为清楚起见，本领域技术人员应当将说明书作为一个整体，各实施例中的技术方案也可以经适当组合，形成本领域技术人员可以理解的其他实施方式。

Claims (10)

1.一种满水环境中基于多传感融合的管道三维重建方法，其特征在于，包括：

步骤一：采集IMU数据并进行满水环境下潜航器初始对准，然后控制潜航器在管道中运动；

步骤二：在潜航器运行过程中，摄像头模组采集运动图像数据，并构建两帧图像之间的位姿转换模型；

步骤三：多波束声呐传感器采集声波点云数据并进行点云畸变矫正和区域滤波；

步骤四：对步骤三处理后的声波点云数据进行线特征提取，并构造两帧点云数据间的位姿转换模型；

步骤五：采集IMU数据并解算潜航器的当前位姿，结合非完整约束以及卡尔曼滤波器不断矫正解算后的位姿；

步骤六：结合步骤二和步骤四构造的位姿转换模型，构建对应的代价函数；

步骤七：融合IMU、声呐、摄像头数据，使用粒子滤波器进行代价函数求解，从而获得可靠的潜航机器人的三维位姿信息，实现对步骤五位姿的优化；

步骤八：将声呐点云数据进行拼接和坐标变换，结合步骤七得到的三维位姿信息，最终获得完整的管道三维结构信息。

2.根据权利要求1所述的一种满水环境中基于多传感融合的管道三维重建方法，其特征在于，所述步骤一读取IMU的角速度和加速度数据，通过姿态分解技术，构造初始对准优化方程，设置权重系数，并四元数估计法求解初始姿态矩阵，对初始姿态矩阵进行精度判断，满足精度要求则实现潜航器初始对准。

3.根据权利要求2所述的一种满水环境中基于多传感融合的管道三维重建方法，其特征在于，所述初始对准优化方程为：

式中，w_i表示权重系数，表示b系相对于n系的初始姿态矩阵，/>和/>表示第i组量测向量，E₃表示3*3的单位矩阵，det符号表示计算矩阵的行列式；N＞2。

4.根据权利要求1所述的一种满水环境中基于多传感融合的管道三维重建方法，其特征在于，所述步骤二在潜航器运行过程中，摄像头模组不断采集外部世界的运动图像数据，分析图像不同时刻的帧数据，使用维纳滤波算法去除图像噪点信息后，结合FAST角点检测算法识别出每帧图像的关键点信息，即可构建出两帧图像之间的位姿转换模型。

5.根据权利要求4所述的一种满水环境中基于多传感融合的管道三维重建方法，其特征在于，所述FAST角点检测算法流程如下：

(1)：在图像中选取像素p，亮度记作I_p，同时设置阈值T，以像素p位中心，选取半径为3的圆上的16个像素点，如果选取的圆上，有连续的N个点的亮度大于I_p+T或者小于I_p-T，那么像素p被认为是特征点，之后不断循环，找到一张图像中所有满足条件的特征点，作为特征点的先验信息；

(2)：对(1)中得到的所有满足条件的特征点进行Harris响应值的计算，然后选取前N个具有最大响应值的角点作为最终的角点集合；

(3)：构造图像金字塔，并在金字塔的每一层上检测角点，同时基于灰度质心法为特征点添加尺度描述；

(4)：计算关键点的描述子信息，采用BRIEF二进制描述子，为(3)中的每个特征点都添加尺度信息。

6.根据权利要求1所述的一种满水环境中基于多传感融合的管道三维重建方法，其特征在于，所述步骤三包括：多波束声呐传感器采集声波点云数据，利用IMU数据估计潜航器的短时运动状态，进而矫正声呐点云的运动畸变，在点云运动畸变处理过程中同时采用区域滤波算法对点云数据进行去噪。

7.根据权利要求1所述的一种满水环境中基于多传感融合的管道三维重建方法，其特征在于，所述步骤四选择线特征作为声波点云数据的关键特征，基于曲率区域增长方法分割三维点云平面，对每个点云平面进行二维投影，并进行网格化和二值化，利用稳健的二维线段检测方法获得平面的线特征，最终产生多帧可靠的三维声波点云数据集，并构造出两帧点云数据间的位姿转换模型。

8.根据权利要求1所述的一种满水环境中基于多传感融合的管道三维重建方法，其特征在于，所述步骤五不断采集IMU的数据，获取潜航器当前时刻的角速度和加速度信息后，利用惯性导航解算方程计算潜航器当前的姿态、速度、位置信息，引入非完整约束，将潜航器横向速度、纵向速度以及航向角变化量基本为零作为观测约束，构造对应的观测模型，同时以惯性导航误差方程作为卡尔曼滤波器的过程模型；在此基础上，基于卡尔曼滤波器对姿态、速度、位置误差进行估计，通过合理的反馈补偿后，即可获得较为精准的潜航器姿态、速度、位置信息。

9.根据权利要求8所述的一种满水环境中基于多传感融合的管道三维重建方法，其特征在于，所述卡尔曼滤波器包含如下公式：

时间更新：

量测更新：

P_k＝(E-K_kH_k)P_k/k-1 (0-6)

式中，和P_k/k-1分别表示状态的一步预测和一步预测协方差矩阵；

K_k表示卡尔曼滤波增益系数，和P_k表示状态的最优估计值以及对应的协方差矩阵；

Q_k和R_k分别表示过程噪声和观测噪声协方差矩阵。

10.根据权利要求1所述的一种满水环境中基于多传感融合的管道三维重建方法，其特征在于，所述步骤六构建的代价函数如下所示：

e＝z-h(ξ，p)

式中，z_ij为在位姿ξ_i处观察的路标p_j产生的数据，ξ为位姿对应的李代数形式，p为三维空间的点云信息。