一种基于智能视觉的井下移动机器人避障方法与流程

本发明属于移动机器人井下避障技术领域，涉及一种基于智能视觉的井下移动机器人避障方法。

背景技术：

煤矿井下环境大多数都是非结构的，而且具有复杂性、多样性和随机性等特征，移动机器人需要具备较高的环境辨识、路径规划和避障能力，才能准确完成预期任务。当井下移动机器人面对完全未知或者部分未知的环境时，避障、越障是其完成任务的首要工作。目前大多数的井下移动机器人都是基于人工远程操控进行作业，智能性和移动性较差，不具备自主控制以及智能避障功能，遇到较为复杂的井下环境需依靠操作人员的经验进行判断，智能化程度较低。

随着移动机器人系统研究，机器人测出行进过程中的障碍物，一种是采用主动发送探测信号，接收场景中物体反射的回波，通过测量发射信号和返回信号获取场景信息，如超声、激光等避障，只具备局部空间避障特点，在检测范围和检测距离上有较大局限性。另一种是采用双目立体视觉的方法进行避障，利用双目视差计算三维场景信息，然后进行障碍物检测。双目采集的图像易受井下复杂环境的影响，如阴影、亮光等，另外障碍物的检测精度取决于双目图像匹配的准确性，增加双目图像匹配的准确性使匹配算法的计算复杂度增加，耗时增加，影响障碍物检测的实时性。

技术实现要素：

本发明的目的是提供一种基于智能视觉的井下移动机器人避障方法，提高了障碍物的检测精度，同时不额外增加算法的复杂度，使算法对光环境的适应性更强。

本发明是通过以下技术方案来实现：

一种基于智能视觉的井下移动机器人避障方法，包括如下步骤：

步骤1，双目相机采集工作场景的图像信息，得到双目图像，根据双目相机的标定参数对双目图像进行畸变校正；

步骤2，对畸变校正后的双目图像进行立体匹配，计算双目视差图，并对双目视差图进行视差优化；其中，立体匹配采用的立体匹配算法通过自适应窗口结合梯度代价函数实现；

步骤3，根据视差优化后的双目视差图计算工作场景的三维点云数据；

步骤4，在二维图像感兴趣区域中，利用二维图像中像素对应的三维点云数据，根据预先设置的阈值，判断障碍物是否存在；

步骤5，如果障碍物存在，则根据人工势场法进行避障路径规划，并执行步骤6，否则执行步骤7；

步骤6，移动机器人进行避障；

步骤7，移动机器人按照预先规划的路线继续行进。

优选的，步骤2中，立体匹配采用的立体匹配算法具体如下：

定义图像梯度为图像沿x和y方向的一阶偏导数：

其中，i为图像灰度，得到左右相机采集图像的图像梯度图：gl＝(glx+gly)、gr＝(grx+gry)，设左图中一点pl(x,y)对应右图的匹配点pr(x-d,y)，定义匹配代价函数如下：

进一步的，步骤2中，具体方法如下：

步骤21，得到畸变校正过后的双目图像之后，计算图像梯度在x方向和y方向幅值和相位；

步骤22，利用快速超像素预分割方法得到相邻像素颜色空间之间的邻接矩阵，根据该邻接矩阵设计不同尺度的匹配窗口，适应不同的纹理密度；

步骤23，得到不同区域的匹配窗口之后，将左图像素点作为参照源，根据匹配代价函数在右图中搜索对应点区域，得到双目视差图；

步骤24，根据像素块区域设计局部视差直方图视差精化方法并采用左右一致性检测方法对双目视差图中误配点进行去除；

步骤25，调用视差图去除白斑噪声算法、去除断层噪声算法和孔洞填充算法进一步去除双目视差图的噪声以及误配区域，最终得到视差优化后的双目视差图。

优选的，步骤5中，所述人工势场法为改进人工势场法，具体如下：将移动机器人位置、目标点和障碍物位置归一化为点坐标到地图中，目标点对移动机器人产生引力，障碍物对移动机器人产生斥力，根据两者的合力规划移动机器人的行进路径；

移动机器人势场函数表示为：u(x)＝uatt(x)+urep(x)

移动机器人受到的合力为：f＝fatt+frep

目标点引力势场函数为：uatt(x)＝kd²(x,xg)

目标点产生的引力为：

其中，k为引力势场中增益系数；

障碍物斥力场函数为：

障碍物产生的斥力为：

其中，η为斥力势场中的增益系数；

其中，u(x)为移动机器人势场函数，uatt(x)为目标点引力势场，urep(x)为障碍物斥力势场；f为移动机器人受到的合力，fatt为目标点产生的引力，frep为障碍物产生的斥力；x为移动机器人在地图中的位置，xg为目标点在地图中的位置，xo为障碍物在地图中的位置；d(x,xg)为移动机器人与目标点之间距离的绝对值，d(x,xo)为移动机器人与障碍物之间距离的绝对值。

进一步的，步骤5中，具体方法如下：

步骤51，计算障碍物相对移动机器人的距离和角度，判断当前时刻前方障碍物是否阻挡移动机器人运行，若不阻挡移动机器人运行，则继续按照当前路径前进，若阻碍移动机器人运动，则执行步骤52；

步骤52，将移动机器人、障碍物、目标点归一化为点坐标到地图中；

步骤53，由障碍物产生的斥力场和目标点的引力场计算移动机器人当前时刻受力情况；

步骤54，根据障碍物分布情况以及移动机器人在各点的受力情况规划移动机器人行进路径。

优选的，步骤6中，采用模糊控制避障策略进行避障。

进一步的，采用模糊控制避障策略进行避障具体为：

步骤61，将不同区域障碍物相对移动机器人的距离和当前时刻移动机器人的速度作为输入变量输入模糊控制系统；

步骤62，构建障碍物相对移动机器人的距离和当前时刻移动机器人的速度的模糊控制隶属度函数，将输入变量障碍物相对移动机器人的距离和当前时刻移动机器人的速度进行模糊化，得到模糊控制变量；

步骤63，设计模糊控制规则，输入模糊控制变量，利用模糊控制规则进行模糊推理得到移动机器人下一时刻运行速度和运行方向(转向角度)的模糊输出量；

步骤64，对移动机器人下一时刻的运行速度和运行方向的模糊输出量去模糊化，即可得到移动机器人控制量，控制移动机器人进行避障操作。

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

本发明采用双目立体视觉的方法进行避障，对立体匹配算法进行改进，采用自适应窗口结合梯度代价函数的方法实现立体匹配，提高算法对光环境的抗干扰能力，并且在低纹理区也具有良好的匹配效果；通过立体匹配算法获得初始视差后，对初始视差进行视差优化，提高了匹配精度，提高了障碍物的检测精度，同时不额外增加算法的复杂度，使算法对光环境的适应性更强。

进一步的，目前，检测到障碍物移动机器人通过避障控制策略躲避障碍物，常用的方法栅格法存在有环境分辨率与环境信息存储量大的矛盾，人工势法的主要弊端是移动机器人在到达目标点前由于陷入局部最小点，从而无法到达目标点。本发明通过改进人工势场函数，消除人工势场局部最小点问题。

进一步的，采用模糊控制避障策略，提高移动机器人的避障系统性能。

附图说明

图1是井下移动机器人视觉避障系统框图；

图2是井下移动机器人视觉避障过程框图；

图3是视差计算及视差优化过程框图；

图4是人工势场规划机器人避障路径过程框图；

图5是模糊控制避障策略计算机器人动作过程框图；

图6是双目立体视觉模型图；

图7是模糊控制系统结构图。

具体实施方式

下面结合具体的实施例对本发明做进一步的详细说明，所述是对本发明的解释而不是限定。

本发明采用的井下移动机器人视觉避障系统框图如图1所示。

如图2，本发明给出了一种所述基于智能视觉的井下移动机器人避障方法，包括如下步骤：

步骤1，采用双目立体视觉检测障碍物：移动机器人控制双目相机采集工作场景的图像信息，得到双目图像，根据双目相机的标定参数对双目图像进行畸变校正；

步骤2，对畸变校正后的双目图像进行立体匹配，计算双目视差图，并对双目视差图进行视差优化；

步骤3，根据视差优化后的双目视差图计算工作场景的三维点云数据；

步骤4，在二维图像感兴趣区域中，利用二维图像中像素对应的三维点云数据，根据预先设置的阈值，判断障碍物是否存在；

步骤5，如果障碍物存在，则根据人工势场法进行避障路径规划，并执行步骤6，否则执行步骤7；

步骤6，将障碍物的分布情况及当前移动机器人行进速度输入模糊控制系统，输出为下一时刻机器人转向的角度及加速度，进行避障；

步骤7，移动机器人按照预先规划的路线继续行进。

步骤1中，双目立体视觉障碍物检测，采用的双目立体视觉模型(图6)具体如下：

在双目立体视觉模型中，世界坐标系对应双目立体视觉模型的左相机，分别以左右相机像面左上角为原点建立图像像素坐标系，分别为：ul-vl和ur-vr，物理坐标系以左右像面的中心点分别为原点：xl-yl和xr-yr，左右相机光心点分别为ol和or。设空间任意一点p在左右相机成像面的投影点分别为pl和pr，其中(ul0，vl0)和(ur0，vr0)均为光轴与像面的交点称为像面的主点，t为左右相机光心的距离称为基线距，建立双目立体视觉模型，利用三角测量原理计算空间中点p(x，y，z)，计算公式如下：

其中，xl、xr、yl为物理尺寸，进一步转换物理坐标到像素坐标如下：

dx和dy是每个像素在x和y方向上实际物理大小，计算可得：

本发明对立体匹配算法进行改进，设计自适应匹配窗口结合梯度代价函数提高匹配算法的适应性，图像梯度定义为图像沿x和y方向的一阶偏导数：

其中i为图像灰度，即可得到左右相机采集图像的图像梯度图：gl＝(glx+gly)、gr＝(grx+gry)，设左图中一点pl(x,y)对应右图的匹配点pr(x-d,y)，即可定义匹配代价函数：

如图3，步骤2中进行双目图像立体匹配，并进行视差优化的具体方法如下：

步骤21，得到畸变校正过后的双目图像之后，计算图像梯度在x方向和y方向幅值和相位；

步骤22，利用快速超像素预分割方法得到相邻像素颜色空间之间的邻接矩阵，根据该邻接矩阵设计不同尺度的匹配窗口，适应不同的纹理密度；

步骤23，得到不同区域的匹配窗口之后，将左图像素点作为参照源，根据匹配代价函数在右图中搜索对应点区域，得到双目视差图；

步骤24，根据像素块区域设计局部视差直方图视差精化方法并采用左右一致性检测方法对双目视差图中误配点进行去除；

步骤25，调用视差图去除白斑噪声算法、去除断层噪声算法和孔洞填充算法进一步去除双目视差图的噪声以及误配区域，最终得到视差优化后的双目视差图。

步骤5中，采用改进人工势场法进行路径规划。

利用障碍物检测算法得到障碍物的分布情况，将移动机器人位置、目标点和障碍物位置归一化为点坐标到地图中。移动机器人要到达目标点，需要不断向目标点移动，这一过程可以用势力场描述，目标点对机器人产生引力，障碍物对机器人产生斥力，根据她们的合力规划机器人的行进路径。

移动机器人势场函数表示为：u(x)＝uatt(x)+urep(x)

移动机器人受到的合力为：f＝fatt+frep

其中，u(x)为移动机器人势场函数，uatt(x)为目标点引力势场，urep(x)为障碍物斥力势场；f为移动机器人受到的合力，fatt为目标点产生的引力，frep为障碍物产生的斥力。在地图中x为移动机器人的位置，xg为目标点的位置，xo为障碍物的位置。d(x,xg)为移动机器人与目标点之间距离的绝对值，d(x,xo)为移动机器人与障碍物之间距离的绝对值。

目标点引力势场函数为：uatt(x)＝kd²(x,xg)

目标点产生的引力为：

其中，k为引力势场中增益系数。

障碍物斥力场函数为：

障碍物产生的斥力为：

其中，η为斥力势场中的增益系数。

如图4，步骤5中，当检测到存在障碍物时，移动机器人避障路径规划方法具体如下：

步骤51，计算障碍物相对移动机器人的距离和角度，判断当前时刻前方障碍物是否阻挡移动机器人运行，若不阻挡移动机器人运行，则继续按照当前路径前进，若阻碍移动机器人运动，则执行步骤52；

步骤52，将移动机器人、障碍物、目标点归一化为点坐标到地图中；

步骤53，由障碍物产生的斥力场和目标点的引力场计算移动机器人当前时刻所受到的总场强大小和方向；

步骤54，根据障碍物分布情况以及移动机器人在各点的受力情况规划移动机器人行进路径，具体是计算总的势场函数的梯度，接近目标时总的势力场最小，解出移动机器人的方向。

步骤6中，采用模糊控制系统实现机器人避障控制，将检测到的障碍物分布情况和机器人运行速度作为模糊控制器的输入变量，通过模糊控制系统计算下一时刻机器人的转向角度和加速度。模糊控制系统是基于规则的控制系统，其整体结构设计如下图7所示，由输入变量、隶属度函数模糊化、模糊控制规则、去模糊化、输出量构成。利用模糊控制的不确定性质来描述采集工作环境中障碍物分布的不确定性，根据障碍物与机器人之间模糊的距离关系运用模糊语言(如障碍物距离机器人远，近)并引入人类驾驶技术先验知识来设计移动机器人的避障策略。

如图5，步骤6中，获得当前时刻障碍物分布情况后，机器人的避障策略具体如下：

步骤61，获取障碍物检测算法中的障碍物分布情况，将不同区域障碍物相对移动机器人的距离和当前时刻移动机器人的速度作为输入变量输入模糊控制系统；

步骤62，构建障碍物相对移动机器人的距离和当前时刻移动机器人的速度的模糊控制隶属度函数，将输入变量障碍物相对移动机器人的距离和当前时刻移动机器人的速度进行模糊化，得到模糊控制变量；

步骤63，设计模糊控制规则，输入模糊控制变量，利用模糊控制规则进行模糊推理得到移动机器人下一时刻运行速度和运行方向(转向角度)的模糊输出量；

步骤64，对移动机器人下一时刻的运行速度和运行方向的模糊输出量去模糊化，即可得到移动机器人控制量，控制移动机器人进行避障操作。