一种基于多传感器融合的通风管道场景建模与机器人定位方法

1.一种多传感器融合的通风管道场景建模与机器人定位方法，其装置包括双目相机、单线激光雷达、惯性测量单元和对称设置在机器人主体左右两侧的TOF测距传感器，其特征在于，包括以下步骤：
步骤1：默认机器人沿通风管道主管出发，初始位置坐标为（0,0），通过分割雷达数据中管道两侧的点云信息并提取其线特征，通过提取图像中管壁两侧的底部边缘像素信息获得沿管道方向上的消失点，融合点线特征并估计机器人的俯仰角、偏航角以及位置坐标，利用TOF测距传感器数据得到当节管道的宽度信息；
步骤2：若机器人进入非直管构件中，利用机器人当前位姿和初始位姿推算上一直管构件的长度信息；
步骤3：判断机器人在非直管构件中是否转弯，若是，则进入支管定位模式，若否，则返回步骤1中并将初始位置坐标设置为机器人当前位置坐标；
步骤4：支管定位模式下，对双目相机拍摄的图像序列提取ORB特征，通过帧间跟踪匹配解算机器人位姿信息，利用TOF测距传感器数据得到当节管道的宽度信息；
步骤5：若机器人重新返回非直管构件中，将机器人在支管中行进的最远距离作为该支管的长度，返回步骤3。
2.根据权利要求1所述的一种多传感器融合的通风管道场景建模与机器人定位方法，其特征在于，所述步骤1的具体步骤为：
将惯性测量单元和激光雷达数据进行时间同步，根据惯性测量单元数据计算雷达旋转一周时间内机器人的旋转量 和平移量 ，对雷达点云数据 进行畸变校正：
                                        （1）；
其中 为畸变校正后的雷达点云数据；
利用点云分割算法获取管道两侧的雷达点云数据，用随机抽样一致算法对这两组数据进行直线拟合以减小离群值对测量结果的影响，随机抽样一致算法拟合结果中的内点数据占比t用于评价两组拟合结果：
                                           （2）；
其中 为内点数据数量，n为总输入数据量；
t值较大一组的拟合结果 用于表示管道朝向信息，根据管道朝向与雷达起始扫描方向的差值估计机器人相对于管道方向的偏航角 ：
                                        （3）；
获取双目相机图像数据，利用LaneNet神经网络对图像进行语义分割，获得管壁两侧的底部边缘像素信息；通过随机抽样一致算法对两侧的边缘像素点集分别进行直线拟合，两条直线的交点坐标 即为该位置处图像中沿管道方向上的消失点；结合相机投影方程估计机器人的偏航角 和俯仰角 ：
                          （4）；
其中 表示旋转矩阵 的第三列， 表示相机的内参矩阵， 表示消失
点坐标；
利用卡尔曼滤波融合 和 ，获得更加准确、稳定的偏航角 ；
根据此时左右两侧的TOF测距传感器数据 和 以及机器人本体宽度 得到
管道宽度 为：
                          （5）；
假设初始位置（0,0）处左侧TOF测距传感器数据为 、偏航角为 ，则机器人在主管中任意位置的x坐标：
                             （6）；
其中 为在当前位置处的左侧TOF测距传感器数据， 为在当前位置处的偏航角；
沿着管道方向查找机器人当前位置处正前方和正后方的雷达点云信息，根据机器人距离管道前后的距离估计y坐标的值；
当管道机器人在主管中运动时，实时获取位置信息 、偏航角 和管道宽度 。
3.根据权利要求1所述的一种多传感器融合的通风管道场景建模与机器人定位方法，其特征在于，所述步骤2的具体步骤为：
当 数据连续5帧不断变大时，则认为机器人进入非直管构件中；若测量值 和中一个变化另一个不变时，则机器人进入三通管件中，否则机器人进入四通管件中；
根据 数据第一次变化时的机器人位置信息 得到管道的长度 为：
                         （7）；
其中， 和 为机器人通过上一非直管构件后的位置坐标，初始值均为0；
当机器人进入一个非直管构件后，完成对上一段管道长度和宽度的估计。
4.根据权利要求1所述的一种多传感器融合的通风管道场景建模与机器人定位方法，其特征在于，所述步骤3的具体步骤为：
当 数据连续5帧不断变小时，则认为机器人从非直管构件中进入新的直管构件中；
利用惯性传感器获取机器人在非直管构件中行走的角速度值，对角速度进行积分得到旋转量；
若机器人朝向变化超过60度则认为机器人进入通风管道支管中，记录下机器人发生转向的信息；由于支管口径狭窄，不再使用激光雷达的扫描数据，此时由双目相机完成定位工作；
若机器人未进入支管中，则仍采用激光雷达定位，此时将 数据稳定不变时的机器人坐标作为该节管道的开始位置坐标 并返回步骤1中继续完成该节管道的建模。
5.根据权利要求2所述的一种多传感器融合的通风管道场景建模与机器人定位方法，其特征在于，所述步骤4的具体步骤为：
首先对双目相机拍摄的图像提取ORB特征，ORB特征由Oriented FAST关键点和BRIEF描述子组成；Oriented FAST关键点检测需先在图像中选取像素亮度为 的像素点p，以像素p为中心选取半径为3的圆上的16个特征点，此外还需设定一个阈值T，假如选取的圆上有连续12个点的亮度大于 或小于 则认为该像素点为特征点；接着计算该特征点的灰度质心来保证其旋转不变性，对于某一图像块B定义其矩 为：
                        （8）；
其中 为像素点 处的亮度；
通过矩找到图像块的质心C为：
                                        （9）；
根据连接图像块几何中心O和质心C得到方向向量 ，定义特征点的方向角 为：
                                    （10）；
通过在关键点附近随机选取128对点，比较它们两两之间的大小关系生成相应的BRIEF描述子；在对两幅相邻帧完成特征点检测后，以汉明距离为度量匹配两幅图像中的特征点，利用随机抽样一致算法筛选出最终的匹配点对；最后结合双目相机深度数据，利用迭代最近点求解相机位姿；管道宽度的测量是利用TOF测距传感器数据得到的。
6.根据权利要求3所述的一种多传感器融合的通风管道场景建模与机器人定位方法，其特征在于，所述步骤5的具体步骤为：
通风管道支管因存在调节风阀使得机器人无法完全通行，因此使用机器人在支管中的行进最远距离作为该支管的长度，要求在视觉里程计运行时不断记录并更新行进方向轴坐标的最大值；判断机器人返回非直管构件中的方法为当 数据连续5帧不断变大时则认为机器人返回非直管构件中，一旦机器人返回，采用惯性传感器获取机器人朝向变化的角度值来判断机器人的行进状态，最终完成整个通风管道场景的建模。

一种基于多传感器融合的通风管道场景建模与机器人定位\n方法\n技术领域\n[0001] 本发明涉及机器人技术领域，具体涉及一种基于多传感器融合的通风管道场景建模与机器人定位方法。\n背景技术\n[0002] 通风管道是实现室内外空气循环功能的重要载体，被广泛应用在各类建筑设施中。然而随着长时间的使用管道内会滋生大量粉尘、细菌等有害物质，进而危害建筑内人群的生命安全，因此必须对通风管道系统进行定期巡检。通风管道构型复杂、管径狭小且铺设于吊顶之上，利用移动机器人进行巡检作业已成为首选解决方案。\n[0003] 建图和定位功能是管道机器人完成各类巡检任务的基础。然而通风管道管壁具有高反射率使得部分传感器工作性能降低，雷达里程计在管道这种类长廊环境中极易产生误匹配，主管道中视觉特征点稀疏导致视觉里程计精度不高。目前的建图算法构建的尺度地图无法直接从中获取管道的长度、宽度等信息，不满足后续巡检作业的需求。\n[0004] 中国专利申请号为：CN202010298783.1，公布日为2020.08.14的专利文献，公开了一种基于初始姿态角自对准的小径管道机器人定位方法，通过惯性测量单元与里程计的组合解算机器人的姿态信息。惯性测量单元随着运行时间的增长会产生累计误差，里程计数据在管道机器人出现滑移等现象时变得不可靠，均会导致该系统定位精度大幅下降。\n[0005] 中国专利申请号为：CN202210960470.7，公布日为2022.12.09的专利文献，公开了一种多传感器协同的管道机器人精准定位方法，通过紧耦合的方式融合相机、惯性传感器、IMU和北斗定位传感器的数据实现管内定位。但在通风管道环境中北斗定位传感器往往不可用，紧耦合方法对计算量要求较高，未构建有效的管道环境地图不满足机器人后续巡检任务的需求。\n[0006] 目前，如何结合各类传感器的优势，让管道机器人完成复杂管道场景的建模与管内定位是亟需解决的一个问题。\n发明内容\n[0007] 发明目的：为了克服现有技术中存在的不足，提供一种基于多传感器融合的通风管道场景建模与机器人定位方法，融合双目相机、单线激光雷达、惯性测量单元和对称设置在机器人主体左右两侧的TOF测距传感器数据，用于解决通风管道材质的特殊性和结构的复杂性使得传感设备性能严重受限的问题，并提出了具有管道几何信息的建模方法。\n[0008] 技术方案：为实现上述目的，本发明提供一种基于多传感器融合的通风管道场景建模与机器人定位方法，包括以下步骤：\n[0009] 步骤1：默认机器人沿通风管道主管出发，初始位置坐标为（0,0），通过分割雷达数据中管道两侧的点云信息并提取其线特征，通过提取图像中管壁两侧的底部边缘像素信息获得沿管道方向上的消失点，融合点线特征并估计机器人的俯仰角、偏航角以及位置坐标，利用TOF测距传感器数据得到当节管道的宽度信息；\n[0010] 步骤2：若机器人进入非直管构件中，利用机器人当前位姿和初始位姿推算上一直管构件的长度信息；\n[0011] 步骤3：判断机器人在非直管构件中是否转弯，若是，则进入支管定位模式，若否，则返回步骤1中并将初始位置坐标设置为机器人当前位置坐标；\n[0012] 步骤4：支管定位模式下，对双目相机拍摄的图像序列提取ORB特征，通过帧间跟踪匹配解算机器人位姿信息，利用TOF测距传感器数据得到当节管道的宽度信息；\n[0013] 步骤5：若机器人重新返回非直管构件中，将机器人在支管中行进的最远距离作为该支管的长度，返回步骤3。\n[0014] 作为本发明的一种优选实施方式：所述步骤1的具体步骤为：\n[0015] 将惯性测量单元和激光雷达数据进行时间同步，根据惯性测量单元数据计算雷达旋转一周时间内机器人的旋转量 和平移量 ，对雷达点云数据 进行畸变校正：\n[0016]                                         （1）；\n[0017] 其中 为畸变校正后的雷达点云数据；\n[0018] 利用点云分割算法获取管道两侧的雷达点云数据，用随机抽样一致算法对这两组数据进行直线拟合以减小离群值对测量结果的影响，随机抽样一致算法拟合结果中的内点数据占比t用于评价两组拟合结果：\n[0019]                                            （2）；\n[0020] 其中 为内点数据数量，n为总输入数据量；\n[0021] t值较大一组的拟合结果 用于表示管道朝向信息，根据管道朝向与雷达起始扫描方向的差值估计机器人相对于管道方向的偏航角 ：\n[0022]                                           （3）；\n[0023] 获取双目相机图像数据，利用LaneNet神经网络对图像进行语义分割，获得管壁两侧的底部边缘像素信息；通过随机抽样一致算法对两侧的边缘像素点集分别进行直线拟合，两条直线的交点坐标 即为该位置处图像中沿管道方向上的消失点；结合相机投影方程估计机器人的偏航角 和俯仰角 ：\n[0024]                           （4）；\n[0025] 其中 表示旋转矩阵 的第三列， 表示相机的内参矩阵，\n表示消失点坐标；\n[0026] 利用卡尔曼滤波融合 和 ，获得更加准确、稳定的偏航角 ；\n[0027] 根据此时左右两侧的TOF测距传感器数据 和 以及机器人本体宽度得到管道宽度 为：\n[0028]                           （5）；\n[0029] 假设初始位置（0,0）处左侧TOF测距传感器数据为 、偏航角为 ，则机器人在主管中任意位置的x坐标：\n[0030]                              （6）；\n[0031] 其中 为在当前位置处的左侧TOF测距传感器数据， 为在当前位置处的偏航角；\n[0032] 沿着管道方向查找机器人当前位置处正前方和正后方的雷达点云信息，根据机器人距离管道前后的距离估计y坐标的值；\n[0033] 当管道机器人在主管中运动时，实时获取位置信息 、偏航角 和管道宽度。\n[0034] 作为本发明的一种优选实施方式：所述步骤2的具体步骤为：\n[0035] 当 数据连续5帧不断变大时，则认为机器人进入非直管构件中；若测量值和 中一个变化另一个不变时，则机器人进入三通管件中，否则机器人进入四通管件中；根据 数据第一次变化时的机器人位置信息 得到管道的长度\n为：\n[0036]                          （7）；\n[0037] 其中， 和 为机器人通过上一非直管构件后的位置坐标，初始值均为0；\n[0038] 当机器人进入一个非直管构件后，完成对上一段管道长度和宽度的估计。\n[0039] 作为本发明的一种优选实施方式：所述步骤3的具体步骤为：\n[0040] 当 数据连续5帧不断变小时，则认为机器人从非直管构件中进入新的直管构件中；利用惯性传感器获取机器人在非直管构件中行走的角速度值，对角速度进行积分得到旋转量；\n[0041] 若机器人朝向变化超过60度则认为机器人进入通风管道支管中，记录下机器人发生转向的信息；由于支管口径狭窄，不再使用激光雷达的扫描数据，此时由双目相机完成定位工作；\n[0042] 若机器人未进入支管中，则仍采用激光雷达定位，此时将 数据稳定不变时的机器人坐标作为该节管道的开始位置坐标 并返回步骤1中继续完成该节管道的建模。\n[0043] 作为本发明的一种优选实施方式：所述步骤4的具体步骤为：\n[0044] 首先对双目相机拍摄的图像提取ORB特征，ORB特征由Oriented FAST关键点和BRIEF描述子组成；Oriented FAST关键点检测需先在图像中选取像素亮度为 的像素点p，以像素p为中心选取半径为3的圆上的16个特征点，此外还需设定一个阈值T，假如选取的圆上有连续12个点的亮度大于 或小于 则认为该像素点为特征点；接着计算该特征点的灰度质心来保证其旋转不变性，对于某一图像块B定义其矩 为：\n[0045]                         （8）；\n[0046] 其中 为像素点 处的亮度；\n[0047] 通过矩找到图像块的质心C为：\n[0048]                                         （9）；\n[0049] 根据连接图像块几何中心O和质心C得到方向向量 ，定义特征点的方向角为：\n[0050]                                     （10）；\n[0051] 通过在关键点附近随机选取128对点，比较它们两两之间的大小关系生成相应的BRIEF描述子；在对两幅相邻帧完成特征点检测后，以汉明距离为度量匹配两幅图像中的特征点，利用随机抽样一致算法筛选出最终的匹配点对；最后结合双目相机深度数据，利用迭代最近点求解相机位姿；管道宽度的测量与步骤1中类似。\n[0052] 作为本发明的一种优选实施方式：所述步骤5的具体步骤为：\n[0053] 通风管道支管因存在调节风阀使得机器人无法完全通行，因此使用机器人在支管中的行进最远距离作为该支管的长度，要求在视觉里程计运行时不断记录并更新行进方向轴坐标的最大值；判断机器人返回非直管构件中的方法与步骤2中类似，一旦机器人返回，采用步骤3中的方法继续判断机器人的行进状态，最终完成整个通风管道场景的建模。\n[0054] 本发明相比现有技术，具有以下有益效果：\n[0055] 1.本发明采用在管道主管中提取雷达点云线特征和图像消失点进行机器人定位、在支管中使用双目相机数据搭建视觉里程计的方法，解决了激光雷达在狭窄管道中存在测量盲区和视觉里程计在特征点稀疏的环境下转向时极易出现跟踪失败的问题，具有很好的灵活性，可满足机器人不同管道中的定位需求。\n[0056] 2.本发明采用随机抽样一致方法对雷达点云特征点、管壁底部边缘像素点和视觉匹配特征点对进行筛选，减小了计算过程中由离群值、误匹配等因素造成的误差，可实现更加精确地估计机器人的位姿。\n[0057] 3.本发明利用管道两侧边缘构成的一组平行线提取图像中沿管道方向上的消失点，再根据消失点计算机器人的偏航角和俯仰角，并融合基于雷达点云线特征的位姿估计结果，使得后续基于TOF测距传感器的非直管构件检测方法更加鲁棒。\n[0058] 4.本发明采用TOF测距传感器实时获取机器人两侧的测量数据，在推算管道宽度的同时可准确判断机器人是否进入非直管构件中。\n[0059] 5.本发明采用拓扑的方式不断构建管道地图，地图中包含管道的长度和宽度信息，可清晰地展现管道的连接状况，有助于提高管道机器人的巡检效率。\n附图说明\n[0060] 图1显示了本发明所提出的一种基于多传感器融合的通风管道场景建模与机器人定位方法框架图；\n[0061] 图2显示了沿管道方向上的消失点检测方法示意图；\n[0062] 图3显示了基于TOF测距传感器的管道宽度测量方法示意图；\n[0063] 图4显示了非直管构件检测方法示意图；\n[0064] 图5显示了通风管道场景建模效果图。\n具体实施方式\n[0065] 下面结合附图和具体实施例，进一步阐明本发明，应理解这些实例仅用于说明本发明而不用于限制本发明的范围，在阅读了本发明之后，本领域技术人员对本发明的各种等价形式的修改均落于本申请所附权利要求所限定的范围。\n[0066] 本发明提供一种基于多传感器融合的通风管道场景建模与机器人定位方法，该方法包括以下步骤：\n[0067] 步骤1，默认机器人沿通风管道主管出发，初始位置坐标为（0,0），通过分割雷达数据中管道两侧的点云信息并提取其线特征，通过提取图像中管壁两侧的底部边缘像素信息获得沿管道方向上的消失点，融合点线特征并估计机器人的俯仰角、偏航角以及位置坐标，利用TOF测距传感器数据得到当节管道的宽度信息。\n[0068] 将惯性测量单元和激光雷达数据进行时间同步，根据惯性测量单元数据计算雷达旋转一周时间内机器人的旋转量 和平移量 ，对雷达点云数据 进行畸变校正：\n[0069]                                        （1）\n[0070] 其中 为畸变校正后的雷达点云数据。\n[0071] 利用点云分割算法获取管道两侧的雷达点云数据，用随机抽样一致算法对这两组数据进行直线拟合以减小离群值对测量结果的影响，随机抽样一致算法拟合结果中的内点数据占比t用于评价两组拟合结果：\n[0072]                                            （2）\n[0073] 其中 为内点数据数量，n为总输入数据量。\n[0074] t值较大一组的拟合结果 用于表示管道朝向信息，根据管道朝向与雷达起始扫描方向的差值估计机器人相对于管道方向的偏航角 ：\n[0075]                                           （3）\n[0076] 获取双目相机图像数据，利用LaneNet神经网络对图像进行语义分割，获得如图2所示的管壁两侧的底部边缘像素信息。通过随机抽样一致算法对两侧的边缘像素点集分别进行直线拟合，拟合得到的两条直线 和 ，其交点坐标 即为该位置处图像中沿管道方向上的消失点。结合相机投影方程估计机器人的偏航角 和俯仰角 ：\n[0077]                             （4）\n[0078] 其中 表示旋转矩阵 的第三列， 表示相机的内参矩阵， 表\n示消失点坐标。\n[0079] 利用卡尔曼滤波融合 和 ，获得更加准确、稳定的偏航角 。\n[0080] 如图2所示，在得到机器人的偏航角 后，灰色虚线为此时机器人两侧TOF测距传感器获得的测量数据，分别记为 和 。结合机器人本体宽度 得到管道宽度为：\n[0081]                           （5）\n[0082] 假设初始位置（0,0）处左侧TOF测距传感器数据为 、偏航角为 ，则机器人在主管中任意位置的x坐标为：\n[0083]                              （6）\n[0084] 其中 为在当前位置处的左侧TOF测距传感器数据， 为在当前位置处的偏航角。\n[0085] 沿着管道方向查找机器人当前位置处正前方和正后方的雷达点云信息，根据机器人距离管道前后的距离估计y坐标的值。\n[0086] 当管道机器人在主管中运动时，实时获取位置信息 、偏航角 和管道宽度。\n[0087] 步骤2，若机器人进入非直管构件中，利用机器人当前位姿和初始位姿推算上一直管构件的长度信息。\n[0088] 当 数据连续5帧不断变大时，则认为机器人进入非直管构件中。如图3所示，测量值 不断变大而测量值 基本不变，此时机器人进入三通管件中。若测量值和 都不断变大则表示机器人进入四通管件中。根据 数据第一次变化时的机器人位置信息 得到管道的长度 为：\n[0089]                          （7）\n[0090] 其中， 和 为机器人通过上一非直管构件后的位置坐标，初始值均为0。\n[0091] 当机器人进入一个非直管构件后，完成对上一段管道长度和宽度的估计。\n[0092] 步骤3，判断机器人在非直管构件中是否转弯。若是，则进入支管定位模式；若否，则返回步骤1中并将初始位置坐标设置为机器人当前位置坐标。\n[0093] 当 数据连续5帧不断变小时，则认为机器人从非直管构件中进入新的直管构件中。此时利用惯性传感器获取机器人在非直管构件中行走的角速度值，对角速度进行积分得到旋转量。若机器人朝向变化超过60度则认为机器人进入通风管道支管中，记录下机器人发生转向的信息。由于支管口径狭窄，不再使用激光雷达的扫描数据，此时由双目相机完成定位工作。若机器人未进入支管中，则仍采用激光雷达定位，此时将 数据稳定不变时的机器人坐标作为该节管道的开始位置坐标 并返回步骤1中继续完成该节管道的建模。\n[0094] 步骤4，支管定位模式下的流程如图1所示，对双目相机拍摄的图像序列提取ORB特征，通过帧间跟踪匹配解算机器人位姿信息，利用TOF测距传感器数据得到当节管道的宽度信息。\n[0095] 首先对双目相机拍摄的图像提取ORB特征，ORB特征由Oriented FAST关键点和BRIEF描述子组成。Oriented FAST关键点检测需先在图像中选取像素亮度为 的像素点p，以像素p为中心选取半径为3的圆上的16个特征点，此外还需设定一个阈值T，假如选取的圆上有连续12个点的亮度大于 或小于 则认为该像素点为特征点。接着计算该特征点的灰度质心来保证其旋转不变性，对于某一图像块B定义其矩 为：\n[0096]                         （8）\n[0097] 其中 为像素点 处的亮度。\n[0098] 通过矩找到图像块的质心C为：\n[0099]                                         （9）\n[0100] 根据连接图像块几何中心O和质心C得到方向向量 ，定义特征点的方向角为：\n[0101]                                     （10）\n[0102] 通过在关键点附近随机选取128对点，比较它们两两之间的大小关系生成相应的BRIEF描述子。在对两幅相邻帧完成特征点检测后，以汉明距离为度量匹配两幅图像中的特征点，利用随机抽样一致算法筛选出最终的匹配点对。最后结合双目相机深度数据，利用迭代最近点求解相机位姿。管道宽度的测量与步骤1中类似。\n[0103] 步骤5，若机器人重新返回非直管构件中，将机器人在支管中行进的最远距离作为该支管的长度，返回步骤3。\n[0104] 通风管道支管因存在调节风阀使得机器人无法完全通行，因此使用机器人在支管中的行进最远距离作为该支管的长度，要求在视觉里程计运行时不断记录并更新行进方向轴坐标的最大值。判断机器人返回非直管构件中的方法与步骤2中类似，一旦机器人返回，采用步骤3中的方法继续判断机器人的行进状态，最终完成如图4所示的整个通风管道场景的建模。\n[0105] 以上显示和描述了本发明的基本原理、主要特征和本发明的优点。本行业的技术人员应该了解，本发明不受上述实施例的限制，上述实施例和说明书中描述的只是说明本发明的原理，在不脱离本发明精神和范围的前提下，本发明还会有各种变化和改进，这些变化和改进都落入要求保护的本发明范围内。