CN114756033B - 一种用于井下透水的机器人沿管线巡检方法 - Google Patents

Abstract

本发明的一种用于井下透水的机器人沿管线巡检方法，包括建立水下机器人运动模型，预设巷道的GIS地图模型，预先规划机器人巡检路径；启动水下机器人在巷道内沿预设路径进行巡检；启动视觉传感器及照明装置，进行管道及障碍物的识别；水下机器人通过识别视觉传感器识别管道后，开始沿管道进行巡检，并对周围环境信息及水下机器人的自身数据进行采集；水下机器人沿管道进行巡检，若发现障碍物，进行自主避障。本发明通过绘制矿井的GIS地图模型预先规划水下机器人初始路径，解决了水下机器人路径规划的问题；再通过主摄像头对管线进行搜寻，延管道进行巡检，通过副摄像头对水中漂浮物进行识别自主避障，解决了人为无法巡检的局限性问题。

Description

一种用于井下透水的机器人沿管线巡检方法

技术领域

本发明涉及机器人控制及视觉技术领域，具体涉及一种用于井下透水的机器人沿管线巡检方法。

背景技术

地下矿藏开采过程中，凿穿隔壁，与大的水源(地下河、水库、用水填充的旧矿等)连通，形成矿灾事故，这样一种情况叫井下透水。井下透水会对井下工作人员生命安全造成威胁，并会破坏井下的工作设备，为了降低损失，并此时就需要对巷道内进行巡检。由于井下巷道较深，水下情况较为复杂，又铺有电缆，不可能采用人为进行巡检，所以水下机器人已经成为水下巡检的重要工具。

水下机器人方便灵活、动力充足且便于搭载不同类型的传感器，适用于多种水下作业环境。井下巷道内布有管线，有是人为设计的，可以先通过GIS地图辅助，预先对路径进行规划。机器人进入巷道后，可以通过视觉对管道进行识别，进而实现沿管道进行巡检及自主避障，以实现人为无法完成的任务。因此，对于水下机器人的研制和开发具有重大的战略意义。

发明内容

本发明的目的在于提供一种用于井下透水的机器人沿管线巡检方法，机器人内置stm32单片机，由PID控制机器人工作，由六自由度的水下推进器进行运动控制，通过GIS地图辅助规划路径，视觉传感器进行管道识别和障碍物检测，以此可解决上述背景技术中提出的水下巡检的问题，能够实现水下机器人在矿井内的沿管线的自主巡检。

为实现上述目的，本发明采用了以下技术方案：

一种用于井下透水的机器人沿管线巡检方法，包括以下步骤，

S1、建立水下机器人运动模型，预设巷道的GIS地图模型，预先规划机器人巡检路径；

S2、启动水下机器人在巷道内沿预设路径进行巡检；

S3、启动视觉传感器及照明装置，进行管道及障碍物的识别；

S4、水下机器人通过识别视觉传感器识别管道后，开始沿管道进行巡检，并对周围环境信息及水下机器人的自身数据进行采集；

S5、水下机器人沿管道进行巡检，若发现障碍物，进行自主避障。

进一步的，所述步骤S1中建立水下机器人运动模型包括通过固定坐标系和运动坐标系来建立，各个坐标都按照右手法则来确定相应的方位；

运动坐标系O-xyz原点O位于机器人重心处，固定坐标系E-ξηζ原点E取地球上任一点，Eξ轴以机器人的主航向为正向；Eη轴与Eξ轴相互垂直即根据右手法则与Eξ轴成90°角；Eζ轴始终垂直于ξEη坐标平面，并以正向指向地球中心；

通过建立相对坐标系O-xyz将水下机器人在水下的相对运动信息转换为相对大地的绝对运动信息，实现机器人的定位；

运动坐标系位于机器人本体上，相对于地面有角速度和加速度；分析水下机器人运动时，需要在固定坐标系之中建立运动方程，再转移到运动坐标系中，设置表征水下机器人自身位置的、以水下机器人为中心点的相对坐标系O-xyz，且相对坐标系O-xyz与绝对坐标系E-ξηζ之间能够相互转换；转换关系如下：

由

可得

故：

根据以上公式实现水下机器人的相对坐标系与绝对坐标系之间的转换。

进一步的，所述步骤S1中建立水下机器人运动模型包括通过坐标轴的建立，以实现水下机器人在矿井中定位，具体包括：

用动系下的沿三个轴的平移运动和绕三个轴的旋转运动来分别表示：

1)平移运动方程

由牛顿第二定理可得，水下机器人受到的力的表达式如下

重心的速度V_G分为以下两项：

V_G＝+Ω×R_G

其中，V为动系原点在定系中的速度；Ω为动系的转动角速度；R_G为重心到坐标原点的距离；Ω×R_G—牵连速度；

则重心加速度

的矢量表达式如下：

其中

为动系下原点的加速度，

为角加速度；

将V＝(u v w)^T，

Ω＝(p q r)^T，

R_G＝(_Gy_Gz_G)^T，F_∑＝(XYZ)^T分别带入上式，可得动系中三个分力的表达式，及水下机器人平移运动方程：

2)旋转运动方程

水下机器人对动系原点O的相对动量矩与所受力矩的关系，由质点系对动点的相对动量矩得到，具体关系如下：

其中，H′_O为相对速度计算的质点系对动系原点O的相对动量矩，即

M_O是动系原点O的外力矩；a_O是动系原点O的加速度，即

根据相对动量矩的相关定义得G′_O＝_OΩ，I_O是机器人相对于动系原点O在各个坐标轴下的转动惯量：

而且有：

水下机器人在结构上关于xOz和xOy面对称，则I_xy＝_xz＝_yz＝0，那么：

将

H′_O＝_OΩ代入式得：

由向量在定系和动系中的导数关系得：

将原点的角速度、重心坐标、原点速度、加速度即Ω＝(p q r)^T，R_G＝(_Gy_Gz_G)^T，V＝(u v w)^T，

分别代入上式可得水下机器人三个坐标轴的旋转运动方程：

进一步的，所述步骤S1中建立水下机器人运动模型还包括航速控制和深度控制，其中航速控制包括前行速度和角速度两个变量，前行速度和角速度分别用于控制水下机器人前进的快慢和方向，通过调节左前、左后、右前、右后四个推进器的转速控制水下机器人在前行过程中的前行速度和角速度，航速控制的动力源为机器人内部的水平驱动电机，即用PID控制器控制推进器转速来调节水下机器人的航速；其航速偏差由速度传感器测得；

深度调节控制通过调节左中、右中推进器转速，使水下机器人来实现变深运动，升降推进器的动力源为相应的竖直驱动电机，选用两个独立的PID控制器串级连接来分别控制升降推进器的专属和深度进而实现水下机器人的深度控制。

进一步的，步骤S3中对管线的识别包括水下机器人视觉部分使用是OPEN CV组件进行管线及障碍物识别，视觉传感器捕捉管线的过程使用OPEN CV软件进行管线搜寻，先要捕捉管线的THRESHOLD值，THRESHOLD值是OPEN CV进行图像识别的时候从图像中提取的颜色阈值，通过OPEN CV的阈值编辑工具获取想要的THRESHOLD值。

进一步的，所述S5中水下机器人沿管道进行巡检，若发现障碍物，进行自主避障，具体包括机器人进入巷道以后，开启照明装置来改善周围环境的可视条件，使用主副两个摄像头进行图像收集，主摄像头用于搜寻管线，副摄像头用于识别障碍物，并在水下采集到画面后，通过对比多组预先采集到的图片的阈值在巷道内进行管线搜寻及障碍物识别；水下机器人在水下搜寻到管线后，通过控制机器人上的推进器沿管线方向行驶，并在发现障碍物后可从障碍物旁绕行。

进一步的，步骤S3中对管线的识别包括预先使用摄像头进行管线图像采集，预先收集多组管线及常见障碍物图片，并提取其管线及障碍物的阈值并保存至机器人系统中；所述摄像头在水下机器人进入巷道过程中，于巷道内对周围环境连续摄像，自动对比识别管线表面前后两帧画面的变化，像差分析管线的节点形状，完成管线的识别，进而使水下机器人可以沿管线行驶，具体包括以下步骤：

步骤一，水下机器人进入巷道过程中，将管线和设备连接处作为巡检的起始点，所述摄像机拍下第1帧图像P₁作为目标模板；

步骤二：水下机器人行进过程中，取得第2帧图像P₂，从第2帧图像P₂开始，在其中心提取n*n的子集z₂，将其叠放在第1帧图像P₁上，则在P₁上有一个与之对应的子集z₁，把子集z₂在平面上逐元平移，得到z₂和z₁的像素差，得到它们在不同位置的相似度量；

步骤三：设置一个阈值Y，相似度量突变接近阈值Y时，表示搜索到相似程度最高点，也就是像素差为零的管线位置，水下机器人通过电机控制推进器使其自动前进，未搜索到时，水下机器人停止前进；

步骤四，依次类推，水下机器人行进过程中，所述摄像机对管线表面连续拍照，取得第i帧图像P_i，在其中心提取n*n的子集z_i，将其叠放在第i-1帧图像P_i-1上，则在P_i-1上有一个与之对应的子集z_i-1，把子集z_i-1在平面上逐元平移，得到z_i和z_i-1的像素差，得到它们在不同位置的相似度量，与阈值Y比较后，判断是否出现管线，若出现则水下机器人继续巡检；

步骤五：待水下机器人巡检至管线尽头时，帧图像P_end为巷道环境本底，相似度量突变为0，巡检停止，水下机器人继续搜寻其他管线。

进一步的，步骤S5中水下机器人避障过程采用边缘检测算法，具体包括：

在未接近障碍物时，摄像头中没有障碍物画面，此时摄像头拍摄画面的数字化图像全部设置为白色，并设定其阈值；水下机器人据障碍物设定距离以后，其摄像头可捕捉到障碍物，且其障碍物画面的数字化图像会表现为黑色；在水下机器人进入巷道内沿管线搜寻后，启用一个副摄像头进项障碍物识别，对副摄像头采集到的数字化图像中每一个点与黑白阈值进行比较，1代表白色，0代表黑色；然后对于二值化后的图像，该算法首先寻找边缘，计数画面中黑点的数目，如果黑点的数目超过阈值，认为该列有障碍物，此时记录下黑白变化的边缘，并且初步判断障碍物大小；如果黑点的数目未超过阈值，认为该列没有障碍物，则认为该列不存在障碍物边缘。

进一步的，所述步骤S5中水下机器人避障过程包括设定摄像头捕捉到障碍物的固定距离为l，在距离为l的范围内摄像头捕捉画面黑白变化的边缘的阈值作为控制器的输入；控制器的另一个输入为水下机器人的水平前进速度u，水下机器人机体方向与目标的夹角

作为模糊控制器的第3个输入，控制器的输出为水下机器人水平面的左加速度

和右加速度

将水下机器人的下潜速度w′作为模糊控制器的4个输入：

式中:

为加速度；

为加速度的边界范围，下标*为左l或右r，k为模糊输入量个数；

的权重系数；

对于模糊控制器输出的水下机器人的左、右加速度进行积分，得

进而得其水平前进速度u_d，以及角速度ω′_d，即

式中:L为向左和向右推进器之间的距离；

对于ω′_d，将以上两式整理得

引入阻尼信号，将角速度期望指令设计为

式中:k_r为角速度的阻尼系数；

对于w′，通过引入阻尼系数，

将下潜速度期望指令设计为：

式中:kw为下潜速度的阻尼系数。

由上述技术方案可知，由于矿井较深且巷道结构较为复杂，本发明通过对关键场景建模，建立水下机器人运动坐标，并建立水下机器人运动模型；运用GIS地图追踪技术辅助预先对路径进行设置；机器人进入巷道后，可开启照明装置，借助视觉传感器进行管线及障碍物的识别，使机器人可以沿管道进行巡检；水下机器人通过识别视觉传感器识别管道后，开始沿管道进行巡检，并对周围环境信息及水下机器人的自身数据进行采集；机器人在巡检途中遇到障碍物后，可以识别出障碍物并进行自主避障，从而使水下机器人可以继续实现巡检任务。

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

本公开采用水下机器人在巷道内沿管线进行巡检，机器人选采用六自由度的水下推进器及改进后的自适应PID算法应用于水下机器人运动控制；以stm32单片机为控制主板；可通过GIS地图辅助规划路径，视觉部分采用OPEN CV，通过摄像头采集巷道内的图片并对比阈值进行管线及障碍物的识别，在识别管线及障碍物后水下机器人进行巡检和避障；通过keil5单片机编程软件进行编程，使水下机器人在水下采集到图像后，可通过控制机器人上的推进器沿管线方向行驶，并在发现障碍物后可从障碍物旁绕行。由于矿下透水后，矿井内环境较为复杂，人为无法在巷道内进行巡检，所以水下机器人就成为了不可或缺的巡检工具。由于矿井较深且结构复杂，可通过绘制矿井的GIS地图模型预先规划水下机器人初始路径，解决了水下机器人路径规划的问题；再通过一个主摄像头对管线进行搜寻，可延管道进行巡检，并可以通过一个副摄像头对水中漂浮物进行识别，识别后可以进行自主避障，以实现矿灾后矿井结构与预设地图模型不同的问题；由解决了无法人为巡检的局限性问题，并可在此基础上可实现机器人的其他功能。

附图说明

图1是本发明实施例的水下机器人工作流程图；

图2是本发明实施例的水下机器人工作过程示意图；

图3是本发明实施例的水下机器人硬件组成图；

图4是本发明实施例的改进后的自适应PID算法控制图；

图5是本发明实施例的水下机器人的自主巡检控制系统的速度控制及深度控制过程图；

图6是本发明实施例的设置OPEN CV软件通过视觉传感器拍摄管线图片并获取管线阈值的过程图；

图7是本发明实施例的设置水下机器人通过对比图像阈值搜寻管线的过程图；

图8是本发明实施例的水下机器人在水下巡检过程中管线搜寻的过程图；

图9是本发明实施例的水下机器人在巡检过程中自主避障的工作流程图；

图10为本发明实施例的视觉部分的阈值采集图片；

图11是本发明实施例的水下机器人运动坐标系。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。

图1为水下机器人在巷道内巡检总流程图，结合图1进行说明；

水下机器人自主巡检的方法，包括以下过程：

S1、建立水下机器人运动模型；预设巷道的GIS地图模型，预先规划机器人巡检路径以便于管线搜寻；

S2、启动水下机器人在巷道内沿预设路径进行巡检；

S3、启动视觉传感器及照明装置，采用一个主摄像头进行管线识别；

S4、水下机器人通过识别视觉传感器识别管道后，开始沿管道进行巡检，并对周围环境信息及水下机器人的自身数据进行采集；

S5、水下机器人沿管道进行巡检，采用一个副摄像头进行障碍物识别，若发现障碍物，进行自主避障；

其中，步骤S1规划水下机器人的运动路径，绘制出矿井内部地图，同时建立水下机器人运动模型，使机器人可以先按预先规划的路径进行巡检；步骤S3为水下机器人通过照明装置改变机器人周围可视条件，并且通过视觉传感器及一个主摄像头搜寻巷道内铺设的管线装置；步骤S5为水下机器人避障过程，若发现水中漂浮的异物，则水下机器人从异物旁绕行以避开异物。

具体的说，本发明实施例的井下三维巷道模型的研究是在三维建模的基础上展开的；利用三维软件建模；用专业的计算机辅助工具，如CAD、CATIA、3ds MAX等三维建模软件，通过软件内提供的工作台，设计草图，建立三维模型，利用计算机技术对几何元素进行操作，创建三维模型；可通过预设的GIS地图模型辅助预设路径，时期可以在巷道内搜索管线；通常情况下，该方法创建的三维模型与真实场景差距较大，但是可以通过纹理渲染提高三维模型的真实感，适用于游戏和工业设计等领域；为了方便观察，在此可以通过CAD建立简化的二维地图模型。

具体的，所述步骤S2中，可借助在所绘制巷道的GIS地图模型中预设水下机器人的简单运动路径，通过计算机编程，将程序导入水下机器人中，使水下机器人可以预先在巷道内行驶，以便于搜索管线。

更进一步地，通过以设定水下机器人的运动模型，可以通过固定坐标系和运动坐标系来建立，各个坐标都按照右手法则来确定相应的方位。运动坐标系(O-xyz)也称为载体坐标系，简称“动系”，原点O一般位于机器人重心处。固定坐标系(E-ξηζ)也称为惯性坐标系，简称“定系”，原点E取地球上任一点，Eξ轴以机器人的主航向为正向；Eη轴与Eξ轴相互垂直即根据右手法则与Eξ轴成90°角；Eζ轴始终垂直于ξEη坐标平面，并以正向指向地球中心；

更进一步地，通过建立相对坐标系O-xyz将水下机器人在水下的相对运动信息转换为相对大地的绝对运动信息；可实现机器人的定位；运动坐标系位于机器人本体上，相对于地面有角速度和加速度；分析水下机器人运动时，需要在固定坐标系之中建立运动方程，再转移到运动坐标系中，设置表征水下机器人自身位置的、以水下机器人为中心点的相对坐标系O-xyz，且相对坐标系O-xyz与绝对坐标系E-ξηζ之间能够相互转换。具体的说，分析水下机器人运动时，需要在固定坐标系之中建立运动方程，再转移到运动坐标系中，设置表征水下机器人自身位置的、以水下机器人为中心点的相对坐标系O-xyz，且相对坐标系O-xyz与绝对坐标系E-ξηζ之间能够相互转换；转换关系如下：

由

可得

故：

根据以上公式实现水下机器人的相对坐标系与绝对坐标系之间的转换；

如图11所示，通过坐标轴的建立，可实现水下机器人在矿井中定位；

水下机器人在水下的运动实质上是一种六自由度的空间运动，可以用动系下的沿三个轴的平移运动和绕三个轴的旋转运动来分别表示：

1)平移运动方程

由牛顿第二定理可得，水下机器人受到的力的表达式如下

为了满足水下机器人运动的一般性，那么在定系中来看，重心的速度V_G可分为以下两项：

V_G＝+Ω×R_G

其中，V—动系原点在定系中的速度；Ω—动系的转动角速度；R_G—重心到坐标原点的距离；Ω×R_G—牵连速度。

则重心加速度

的矢量表达式如下：

其中

—动系下原点的加速度，

—角加速度。

将V＝(u v w)^T，

Ω＝(p q r)^T，

R_G＝(_Gy_Gz_G)^T，F_∑＝(XYZ)^T分别带入上式，可得动系中三个分力的表达式，及水下机器人平移运动方程：

2)旋转运动方程

水下机器人对动系原点O的相对动量矩与所受力矩的关系，可由质点系对动点的相对动量矩得到，具体关系如下：

其中，H′_O—相对速度计算的质点系对动系原点O的相对动量矩，即

是动系原点O的外力矩；a_O是动系原点O的加速度，即

根据相对动量矩的相关定义可得H′_O＝_OΩ，I_O是机器人相对于动系原点O在各个坐标轴下的转动惯量：

而且有：

本公开的水下机器人在结构上关于xOz和xOy面大致对称，则I_xy＝_xz＝I_yz＝0，那么：

将

H′_O＝_OΩ代入式得：

由向量在定系和动系中的导数关系可得：

将原点的角速度、重心坐标、原点速度、加速度即Ω＝(p q r)^T，R_G＝(_Gy_Gz_G)^T，V＝(u v w)^T，

分别代入上式可得水下机器人三个坐标轴的旋转运动方程：

本实施例的水下机器人运动控制主要包含航速控制和深度控制；水下机器人在水下的运动实质上是一种六自由度的空间运动，水下机器人的动力由水下机器人内部的驱动电机来提供，水下机器人的运动有六自由度的水下推进器提供，分别包括左前、左中、左后、右前、右中、右后六个推进器，左前、左后、右前、右后四个推进器用于航速控制，左中、右中两个推进器用于深度控制，经过一系列的运动，从而构建水下机器人的运动系统。

所述航速控制巡检控制系统包括前行速度和角速度两个变量，前行速度和角速度分别用于控制水下机器人前进的快慢和方向，通过调节左前、左后、右前、右后四个推进器的转速控制水下机器人在前行过程中的前行速度和角速度，航速控制的动力源为机器人内部的水平驱动电机，即用PID控制器控制推进器转速来调节水下机器人的航速；其航速偏差由速度传感器测得；深度调节主要是通过调节左中、右中推进器转速，使水下机器人来实现变深运动，升降推进器的动力源为相应的竖直驱动电机，此时创新性地选用两个独立的PID控制器串级连接来分别控制升降推进器的专属和深度进而实现水下机器人的深度控制；串级PID控制器对水下机器人深度控制有很好的调节能力，其深度值偏差分别通过深度计获取；由此，可实现水下机器人在矿井内的运动；

所述水下机器人照明及识别管线，使用照明装置在摄像头可捕捉的范围内进行照明，使用OPEN CV机器视觉开发组件进行管线识别。可通过编程来调用图像处理的算法来进行开发；OPEN CV内置了Micro Python解释器，可以用Python特性编写所有的逻辑，还可以调用很多的Python库；可使用一个主摄像头来实现水下机器人在巷道内对管线的识别；具体如下：水下机器人视觉部分可使用是OPEN CV组件进行管线及障碍物识别，OPEN CV是一个较为精确的机器视觉开发组件。相较于OPEN MV，OPEN CV视觉开发组件识别精度较高，故在此使用OPENCV视觉开发组件进行视觉识别；由于OPEN CV使用成本较高，而管线结构较为简单，因此在实验中可以使用成本较低的OPEN MV视觉开发组件进行实验；在实际使用中，可通过编程来调用图像处理的算法来进行视觉开发，OPEN CV内置了大量的可调用图像算法，比如：滤波、颜色追踪等；另外，OPEN CV内置了Micro Python解释器，所以可以直接用Python编程，可以用Python特性编写所有的逻辑，还可以调用很多的Python库；由此可使用一个主摄像头及来实现水下机器人在巷道内对管线的识别；

视觉传感器捕捉管线的过程使用OPEN CV软件进行管线搜寻，先要捕捉管线的THRESHOLD值，THRESHOLD值是OPEN CV进行图像识别的时候从图像中提取的颜色阈值，这个参数非常重要，这决定了识别的目标物是否准确，可以通过OPEN CV的阈值编辑工具获取想要的THRESHOLD值；由于巷道内有粗细程度不同的管线及大小形状不同的障碍物，因此可以收集多组管线及可能出现的障碍物图像，通过阈值编辑器获取想要的阈值，并记录下来导入水下机器人程序中，使机器人可通过阈值来搜寻巷道中的管线；

水下机器人内置stm32单片机，可通过Keil5软件编写水下机器人运动程序，并将OPEN CV的阈值编辑工具获取的THRESHOLD值导入机器人程序中；机器人可通过摄像头来采集巷道内的图像，并对比多组已获取的阈值来搜寻管线；搜索到管线后，可设置机器人沿管线方向行走，以完成巡检及其它任务；

由于发生矿灾后，井下透水后，会有一些物体在水中悬浮以及建筑坍塌造成的障碍，使水下机器人无法正常行驶，在此基础上，再添加一个副摄像头，使水下机器人在巡检的过程中可以捕捉巷道内的信息，并实现对障碍物的识别；障碍物被识别后，可通过推进器调整运动方向，使机器人做出避障动作；避开障碍物后，水下机器人可沿管线方向继续巡检；具体说，所述的水下机器人在遇到障碍物后，可能无法正常巡检；在此基础上，可以添加一个副摄像头，由此可通过副摄像头捕捉巷道内的信息，并实现对障碍物的识别；障碍物被识别后，可通过推进器调整运动方向，使机器人做出避障动作；避开障碍物后，水下机器人可沿管线方向继续巡检。

具体的说本实施例机器人以stm32单片机为控制主板，水下机器人内置运动电机，采用左前、左后、右前、右后、左中、右中六个推进器应用于水下机器人运动，左前、左后、右前、右后四个推进器为水平推进器以控制水下机器人水平运动及速度，左中、右中两个推进器为竖直推进器以控制水下机器人在水中的深度；

水下机器人可通过keil5单片机编程软件对机器人的工作进行编程；可通过GIS地图辅助建立巷道模型，以便于水下机器人进行的定位及预先规划路径，以便于水下机器人可以安全进入巷道；机器人进入巷道以后，开启照明装置来改善周围环境的可视条件，使用主副两个摄像头进行图像收集，主摄像头用于搜寻管线，副摄像头用于识别障碍物，并在水下采集到画面后，通过对比多组预先采集到的图片的阈值在巷道内进行管线搜寻及障碍物识别；水下机器人在水下搜寻到管线后，可通过控制机器人上的推进器沿管线方向行驶，并在发现障碍物后可从障碍物旁绕行。

视觉部分采用OPEN CV，可预先使用摄像头进行管线图像采集，预先收集多组管线及常见障碍物图片，并提取其管线及障碍物的阈值并保存至机器人系统中；所述摄像头在水下机器人进入巷道过程中，于巷道内对周围环境连续摄像，自动对比识别管线表面前后两帧画面的变化，像差分析管线的节点形状，完成管线的识别，进而使水下机器人可以沿管线行驶，具体包括以下步骤：

步骤一，水下机器人进入巷道过程中，将管线和设备连接处作为巡检的起始点，所述摄像机拍下第1帧图像P₁作为目标模板；

步骤二：水下机器人行进过程中，取得第2帧图像P₂，从第2帧图像P₂开始，在其中心提取n*n的子集z₂，将其叠放在第1帧图像P₁上，则在P₁上有一个与之对应的子集z₁，把子集z₂在平面上逐元平移，得到z₂和z₁的像素差，得到它们在不同位置的相似度量；

步骤三：设置一个阈值Y，相似度量突变接近阈值Y时，表示搜索到相似程度最高点，也就是像素差为零的管线位置，水下机器人通过电机控制推进器使其自动前进，未搜索到时，水下机器人停止前进；

步骤四，依次类推，水下机器人行进过程中，所述摄像机对管线表面连续拍照，取得第i帧图像P_i，在其中心提取n*n的子集z_i，将其叠放在第i-1帧图像P_i-1上，则在P_i-1上有一个与之对应的子集z_i-1，把子集z_i-1在平面上逐元平移，得到z_i和z_i-1的像素差，得到它们在不同位置的相似度量，与阈值Y比较后，判断是否出现管线，若出现则水下机器人继续巡检；

步骤五：待水下机器人巡检至管线尽头时，帧图像P_end为巷道环境本底，相似度量突变为0，巡检停止，水下机器人继续搜寻其他管线。

水下机器人避障过程采用边缘检测算法；由于在矿井内水下环境较为特殊，在未接近障碍物时，摄像头中没有障碍物画面，此时摄像头拍摄画面的数字化图像全部设置为白色，并设定其阈值；水下机器人据障碍物一定距离以后，其摄像头可捕捉到障碍物，且其障碍物画面的数字化图像会表现为黑色；在水下机器人进入巷道内沿管线搜寻后，启用一个副摄像头进项障碍物识别，对副摄像头采集到的数字化图像中每一个点与黑白阈值进行比较，1代表白色，0代表黑色。然后对于二值化后的图像，该算法首先寻找边缘。计数画面中黑点的数目，如果黑点的数目超过阈值，认为该列有障碍物，此时记录下黑白变化的边缘，并且可以初步判断障碍物大小；如果黑点的数目未超过阈值，认为该列没有障碍物，则认为该列不存在障碍物边缘；

避障过程中可设定摄像头捕捉到障碍物的固定距离为l，在距离为l的范围内摄像头捕捉画面黑白变化的边缘的阈值作为控制器的输入；为了防止水下机器人速度过快导致避障不及时，所以控制器的另一个输入为水下机器人的水平前进速度u，为使水下机器人能够准确避开障碍物，水下机器人机体方向与目标的夹角

作为模糊控制器的第3个输入，控制器的输出为水下机器人水平面的左加速度

和右加速度

将水下机器人的下潜速度w′作为模糊控制器的4个输入

式中:

为加速度；

为加速度的边界范围，下标*为左(l)或右(r)，k为模糊输入量个数；

为

的权重系数；

对于模糊控制器输出的水下机器人的左、右加速度进行积分，可得

进而可得其水平前进速度u_d，以及角速度ω′_d，即

式中:L为向左和向右推进器之间的距离

对于ω′_d，将以上两式整理可得

由于ω′_d容易在水下机器人航向控制时产生震荡，为了让系统更稳定，可引入阻尼信号，将角速度期望指令设计为

式中:k_r为角速度的阻尼系数

对于w′，系统同样不稳定，通过引入阻尼系数，

可将下潜速度期望指令设计为：

式中:kw为下潜速度的阻尼系数；

水下机器人避障的视觉拍摄过程与管线识别相似，其具体避障步骤为：在巷道内沿管线进行巡检；启动摄像头，采集水下机器人前方图像；通过识别图像的阈值判断前方是否由障碍物；若没有则继续沿管线巡检；若有障碍物，通过数值化图像阈值大小计算障碍物大小，并反馈给控制器；控制器控制各推进器转速，以改变水下机器人前进速度、角速度及下潜速度；最终水下机器人完成越障动作。

以下结合附图具体说明：

图1为水下机器人在巷道内巡检总流程图，结合图1进行说明；

如图1所示，为实现水下机器人在巷道内巡检，可先建立水下机器人运动模型，并借助GIS地图进行辅助；绘制巷道的GIS地图模型，规划水下机器人预先运动路径，使机器人进入巷道后，可预先沿预设路径进入巷道，以便于搜寻管线；

如图1所示，在水下机器人进入巷道后，先沿预设路径巡检，并启动照明装置，改善摄像头可捕捉范围内的可视条件，并开启视觉传感器及主摄像头，搜寻巷道内铺设的管线；若搜寻到管线，则水下机器人可沿管线进行巡检；若未搜寻到管线，机器人继续进行管线搜寻；

如图1所示，在水下机器人巡检过程中，开启副摄像头，使水下机器人在巡检的过程中可以捕捉巷道内的信息，并实现对障碍物的识别；若前方出现障碍物，水下机器人执行避障动作；若无障碍物，水下机器人继续巡检工作，直到巡检工作完成；

图2是水下机器人在巷道内自主巡检的方法示意图，结合图2进行说明，包括以下步骤：1启动水下机器人在巷道内沿预设路径进行巡检；2启动视觉传感器及照明装置，进行管道及障碍物的识别；3水下机器人通过识别视觉传感器识别管道后，开始沿管道进行巡检，并对周围环境信息及水下机器人的自身数据进行采集；4水下机器人沿管道进行巡检，若发现障碍物，进行自主避障；

图3为水下机器人硬件组成，水下机器人以stm32单片机为控制主板；外置左前、左后、右前、右后四个水平推进器，左中、右中两个竖直推进器，并设有主、副两个摄像头及照明灯；内置速度传感器、深度传感器、视觉传感器等多传感器，并有运动电机为机器人运动提供动力；

图4为改进后的自适应PID算法控制图。视觉传感器用于反馈视觉识别信号，速度传感器及深度传感器用于反馈运动控制信号，水下机器人工作的控制指令指令由上位机编程导入控制系统中，最终由控制器处理视觉识别信号、运动控制信号及控制指令并控制水下机器人工作；

图5为基于改进后的自适应PID算法水下机器人速度及深度控制总图，速度及深度控制主要是通过PID控制器来调节左前、左中、左后、右前、右中、右后六个推进器，左前、左后、右前、右后推进器用于航速控制，左中、右中推进器用于深度控制；

如图5所示，深度调节主要是通过PID控制器来调节左中、右中两个升降推进器转速，使水下机器人来实现变深运动，升降推进器的动力源为机器人内部相应的水平驱动电机来提供；根据水下机器人的实际运动路径及运动状态，其深度值偏差分别通过深度计获取；在获得深度值偏差后，反馈给PID控制器，再由控制器控制推进器以完成水下机器人的深度调节；

如图5所示，在航速控调节系统中，所述自主巡线控制系统包括前行速度和角速度两个变量，前行速度和角速度分别用于控制水下机器人前进的快慢和方向；通过调节左前、左后、右前、右后推进器的转速控制水下机器人在前行过程中的前行速度和角速度，通过对水下机器人前进的快慢和方向的不断调整，完成水下机器人的自主巡线过程。水下机器人在自主巡线过程中，前行速度的控制是基于专家驾驶经验规则控制，在实际巡线过程中要根据路径的情况和已经完成巡线的质量来控制前行速度。其中，巡线路径的情况和已经完成巡线的质量可以用两个控制量进行描述，即预设运动路径的位置偏差和角度偏差；所述航速控制巡检控制系统包括前行速度和角速度两个变量，前行速度和角速度分别用于控制水下机器人前进的快慢和方向，航速控制的动力源为机器人内部的水平驱动电机，即用PID控制器控制推进器转速来调节水下机器人的航速；其航速偏差由速度传感器测得；

图6为设置视觉传感器捕捉管线的过程，本过程使用OPEN CV软件进行管线搜寻，先要捕捉管线的THRESHOLD值，THRESHOLD值是OPEN CV进行图像识别的时候从图像中提取的颜色阈值，这个参数非常重要，这决定了识别的目标物是否准确，可以通过OPEN CV的阈值编辑工具获取想要的THRESHOLD值。可通过摄像头去采集管线图像，由于巷道内有粗细程度不同的管线，因此可以收集几组图像，通过阈值编辑器获取想要的阈值，并记录下来导入水下机器人程序中，使机器人可通过阈值搜寻巷道中的管线；

图7为设置水下机器人通过对比图像阈值搜寻管线的过程，水下机器人内置stm32单片机，可通过Keil5软件编写水下机器人运动程序，并将OPEN CV的阈值编辑工具获取的THRESHOLD值导入机器人程序中；机器人可通过摄像头来采集巷道内的图像，并对比已获取的阈值来搜寻管线；搜索到管线后，可使设置机器人沿管线方向行走，以完成巡检及其它任务；

图8是水下机器人在水下巡检过程中管线搜寻的过程：水下机器人下水沿预设路径进行巡检；再进入巷道后开启照明装置，以改善巷道内的可视条件；开启视觉传感器，使用主摄像头对管线进行搜寻；在捕捉到管线后，水下机器人沿管线进行巡检。

图9是水下机器人避障过程：水下机器人在巷道内沿管线进行巡检；启动摄像头，采集水下机器人前方图像；通过对比图像的阈值判断前方是否由障碍物；若没有则继续沿管线巡检；若有障碍物，通过数值化图像阈值大小计算障碍物大小，并反馈给控制器；控制器控制各推进器转速，以改变水下机器人前进速度、角速度及下潜速度；最终水下机器人完成越障动作。

图10为阈值采集图片，左边为管线实物图，右边为管线数字化图像；由于OPEN CV使用成本较高，而管线结构较为简单，因此在实验中可以使用成本较低且识别原理相同的OPEN MV视觉开发组件进行实验；保持画面中是想要是识别的图像，像图10中，如果要识别一根管道，就保持图10所示画面，通过阈值编辑器，调整下面滑块位置，在右边的数字化图像中使要识别的物体变成白色，其他物体变成黑色，如图10所示，管道上依然保留的黑色部分是因为原图像的部分有反光导致的，将下面的阈值复制即可，这就是要的阈值。由于巷道内可视条件及管线粗细不同，为减小误差，可以多收集几组图像，保留其阈值进行对比。

综上所述，本发明实施例采用水下机器人进行巷道内的巡检工作，机器人内置stm32单片机、电源、及传感器，采用自适应PID算法应用于水下机器人运动控制中，采用六自由度推进器及驱动电机提供动力；可通过GIS地图辅助规划路径，采用OPEN MV机器视觉软件及摄像头采集图片并提取需要的阈值，通过摄像头采集巷道内的图片并对比阈值进行管线及障碍物的识别，再识别管线及障碍物后水下机器人进行巡检和避障；解决了人为无法巡检的局限性问题。

以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。

Claims (7)

1.一种用于井下透水的机器人沿管线巡检方法，其特征在于，包括以下步骤，

S1、建立水下机器人运动模型，预设巷道的GIS地图模型，预先规划机器人巡检路径；

S2、启动水下机器人在巷道内沿预设路径进行巡检；

S3、启动视觉传感器及照明装置，进行管道及障碍物的识别；

S4、水下机器人通过视觉传感器识别管道后，开始沿管道进行巡检，并对周围环境信息及水下机器人的自身数据进行采集；

S5、水下机器人沿管道进行巡检，若发现障碍物，进行自主避障；

所述步骤S3中对管线的识别包括预先使用摄像头进行管线图像采集，预先收集多组管线及常见障碍物图片，并提取其管线及障碍物的阈值并保存至机器人系统中；所述摄像头在水下机器人进入巷道过程中，于巷道内对周围环境连续摄像，自动对比识别管线表面前后两帧画面的变化，像差分析管线的节点形状，完成管线的识别，进而使水下机器人可以沿管线行驶；

所述步骤S3具体包括以下步骤：

步骤一，水下机器人进入巷道过程中，将管线和设备连接处作为巡检的起始点，摄像机拍下第1帧图像P₁作为目标模板；

步骤二：水下机器人行进过程中，取得第2帧图像P₂，从第2帧图像P₂开始，在其中心提取n＊n的子集z₂，将其叠放在第1帧图像P₁上，则在P₁上有一个与之对应的子集z₁，把子集z₂在平面上逐元平移，得到z₂和z₁的像素差，得到它们在不同位置的相似度量；

步骤三：设置一个阈值Y，相似度量突变接近阈值Y时，表示搜索到相似程度最高点，也就是像素差为零的管线位置，水下机器人通过电机控制推进器使其自动前进，未搜索到时，水下机器人停止前进；

步骤四，依次类推，水下机器人行进过程中，所述摄像机对管线表面连续拍照，取得第i帧图像P_i，在其中心提取n＊n的子集z_i，将其叠放在第i-1帧图像P_i-1上，则在P_i-1上有一个与之对应的子集z_i-1，把子集z_i-1在平面上逐元平移，得到z_i和z_i-1的像素差，得到它们在不同位置的相似度量，与阈值Y比较后，判断是否出现管线，若出现则水下机器人继续巡检；

步骤五：待水下机器人巡检至管线尽头时，帧图像P_end为巷道环境本底，相似度量突变为0，巡检停止，水下机器人继续搜寻其他管线；

所述S5中水下机器人沿管道进行巡检，若发现障碍物，进行自主避障，具体包括机器人进入巷道以后，开启照明装置来改善周围环境的可视条件，使用主副两个摄像头进行图像收集，主摄像头用于搜寻管线，副摄像头用于识别障碍物，并在水下采集到画面后，通过对比多组预先采集到的图片的阈值在巷道内进行管线搜寻及障碍物识别；水下机器人在水下搜寻到管线后，通过控制机器人上的推进器沿管线方向行驶，并在发现障碍物后可从障碍物旁绕行。

2.根据权利要求1所述的一种用于井下透水的机器人沿管线巡检方法，其特征在于，所述步骤S5中水下机器人避障过程采用边缘检测算法，具体包括：

在未接近障碍物时，摄像头中没有障碍物画面，此时摄像头拍摄画面的数字化图像全部设置为白色，并设定其阈值；水下机器人据障碍物设定距离以后，其摄像头可捕捉到障碍物，且其障碍物画面的数字化图像会表现为黑色；在水下机器人进入巷道内沿管线搜寻后，启用一个副摄像头进行障碍物识别，对副摄像头采集到的数字化图像中每一个点与黑白阈值进行比较，1代表白色，0代表黑色；然后对于二值化后的图像，该算法首先寻找边缘，计数画面中黑点的数目，如果黑点的数目超过阈值，认为该列有障碍物，此时记录下黑白变化的边缘，并且初步判断障碍物大小；如果黑点的数目未超过阈值，认为该列没有障碍物，则认为该列不存在障碍物边缘。

3.根据权利要求1所述的用于井下透水的机器人沿管线巡检方法，其特征在于：所述步骤S1中建立水下机器人运动模型包括通过固定坐标系和运动坐标系来建立，各个坐标都按照右手法则来确定相应的方位；

运动坐标系O-xyz原点O位于机器人重心处，固定坐标系E-ξηζ原点E取地球上任一点，Eξ轴以机器人的主航向为正向；Eη轴与Eξ轴相互垂直即根据右手法则与Eξ轴成90°角；Eζ轴始终垂直于ξEη坐标平面，并以正向指向地球中心；

通过建立运动坐标系O-xyz将水下机器人在水下的相对运动信息转换为相对大地的绝对运动信息，实现机器人的定位；

运动坐标系位于机器人本体上，相对于地面有角速度和加速度；分析水下机器人运动时，需要在固定坐标系之中建立运动方程，再转移到运动坐标系中，设置表征水下机器人自身位置的、以水下机器人为中心点的运动坐标系O-xyz，且运动坐标系O-xyz与固定坐标系E-ξηζ之间能够相互转换；转换关系如下：

故：

根据以上公式实现水下机器人的运动坐标系与固定坐标系之间的转换。

4.根据权利要求1所述的用于井下透水的机器人沿管线巡检方法，其特征在于：所述步骤S1中建立水下机器人运动模型包括通过坐标轴的建立，以实现水下机器人在矿井中定位，具体包括：

用动系下的沿三个轴的平移运动和绕三个轴的旋转运动来分别表示：

1)平移运动方程

由牛顿第二定理可得，水下机器人受到的力的表达式如下

重心的速度V_G分为以下两项：

V_G＝V+Ω×R_G (4)

其中，V为动系原点在定系中的速度；Ω为动系的转动角速度；R_G为重心到坐标原点的距离；Ω×R_G-牵连速度；

则重心加速度

的矢量表达式如下：

其中，

为动系下原点的加速度，

为角加速度；

将V＝(u v w)^T，

Ω＝(p q r)^T，

R_G＝(x_G y_G z_G)^T，F_∑＝(XY Z)^T

分别带入上式，可得动系中三个分力的表达式，及水下机器人平移运动方程：

2)旋转运动方程

水下机器人对动系原点O的相对动量矩与所受力矩的关系，由质点系对动点的相对动量矩得到，具体关系如下：

其中，H′_O为相对速度计算的质点系对动系原点O的相对动量矩，即

M_O是动系原点O的外力矩；a_O是动系原点O的加速度，即

根据相对动量矩的相关定义得H′_O＝I_OΩ，I_O是机器人相对于动系原点O在各个坐标轴下的转动惯量：

而且有：

水下机器人在结构上关于xOz和xOy面对称，则I_xy＝I_xz＝I_yz＝0，那么：

将

H′_O＝I_OΩ代入式得：

由向量在定系和动系中的导数关系得：

将原点的角速度、重心坐标、原点速度、加速度即Ω＝(p q r)^T，R_G＝(x_G y_G z_G)^T，V＝(uv w)^T，

分别代入上式可得水下机器人三个坐标轴的旋转运动方程：

5.根据权利要求1所述的用于井下透水的机器人沿管线巡检方法，其特征在于：所述步骤S1中建立水下机器人运动模型还包括航速控制和深度控制，其中航速控制包括前行速度和角速度两个变量，前行速度和角速度分别用于控制水下机器人前进的快慢和方向，通过调节左前、左后、右前、右后四个推进器的转速控制水下机器人在前行过程中的前行速度和角速度，航速控制的动力源为机器人内部的水平驱动电机，即用模糊PID控制器控制推进器转速来调节水下机器人的航速；其航速偏差由速度传感器测得；

深度调节控制通过调节左中、右中推进器转速，使水下机器人来实现变深运动，升降推进器的动力源为相应的竖直驱动电机，选用两个独立的模糊PID控制器串级连接来分别控制升降推进器的转速和深度进而实现水下机器人的深度控制。

6.根据权利要求1所述的用于井下透水的机器人沿管线巡检方法，其特征在于：步骤S3中对管线的识别包括水下机器人视觉部分使用是OPEN CV组件进行管线及障碍物识别，视觉传感器捕捉管线的过程使用OPEN CV软件进行管线搜寻，先要捕捉管线的THRESHOLD值，THRESHOLD值是OPEN CV进行图像识别的时候从图像中提取的颜色阈值，通过OPEN CV的阈值编辑工具获取想要的THRESHOLD值。

7.根据权利要求1所述的用于井下透水的机器人沿管线巡检方法，其特征在于：所述步骤S5中水下机器人避障过程包括设定摄像头捕捉到障碍物的固定距离为I，在距离为I的范围内摄像头捕捉画面黑白变化的边缘的阈值作为控制器的输入；控制器的另一个输入为水下机器人的水平前进速度u，水下机器人机体方向与目标的夹角

作为模糊控制器的第3个输入，控制器的输出为水下机器人水平面的左加速度

和右加速度

将水下机器人的下潜速度w′作为模糊控制器的4个输入：

式中：

为加速度；

为加速度的边界范围，下标＊为左I或右r，k为模糊输入量个数；

为

的权重系数；

对于模糊控制器输出的水下机器人的左、右加速度进行积分，得

进而得其水平前进速度u_d，以及角速度ω′_d，即

式中：L为向左和向右推进器之间的距离；

对于ω′_d，将以上两式整理得

引入阻尼信号，将角速度期望指令设计为

式中：k_r为角速度的阻尼系数；

对于w′，通过引入阻尼系数，

将下潜速度期望指令设计为：

式中：k_w为下潜速度的阻尼系数。

Images