CN111897349B - 一种基于双目视觉的水下机器人自主避障方法 - Google Patents

Abstract

本发明公开了一种基于双目视觉的水下机器人自主避障方法，属于大数据领域，包括建立水下机器人自主避障系统，对双目摄像机进行标定和校准，对视差图进行处理，计算出视差图中所有具有已知视差的像素对应的三维坐标，对预处理连续图像帧进行特征跟踪，利用三维坐标来估计机器人的运动，根据障碍物图像选择一个行为控制策略作为避障策略，提高了水下机器人工作的自主性和机动性，在水下管道检测、水下地形勘测、水下目标跟踪领域有广泛应用，实现了实时检测障碍物，还可以确定水下机器人到目标的距离，本发明通过开发的水下图像校准方法，并将其用于水下双目像机校准，获得了良好的图像效果。

Description

一种基于双目视觉的水下机器人自主避障方法

技术领域

本发明属于机器人技术领域，涉及一种基于双目视觉的水下机器 人自主避障方法。

背景技术

随着科技的进步，水下机器人自主运动能力越来越强，近几年来， 自主水下机器人(AUV)数量显著增加。这些水下机器人往往体积小、 价格便宜，但传感探测能力有限，在标准配置下只有一个彩色相机， 用于研究目的。计算机视觉已经被广泛应用于实现各种水下机器人任 务，如水下动植物科学考察、水下地图绘制、水下三维场景重建、水 下目标对接、水下运动目标跟踪和定位。当前，大型水下机器人通常 采用声纳传感器，这些水下机器人通常采用基于声呐信息的避障方法。然而这种声纳传感器价格昂贵，重量大，限制了其在小型水下机 器人上的应用。在水下机器人自主避障与运动跟踪领域，很少有研究 涉及基于视觉的障碍回避，更不用说使用双目相机。

目前水下机器人自主避障与运动跟踪方法主要有以下2种主要 方案：

第一种方法基于声呐传感器，当前，大型水下机器人通常采用声 纳传感器，这些水下机器人通常采用基于声呐信息的避障方法。然而 声纳传感器价格昂贵，重量大，限制了其在小型水下机器人上的应用。

第二种方法是基于单目摄像机，但是单目摄像机难以获得物体相 对深度的信息，对水下机器人避障和跟踪的运动方向的计算和预判准 确性差。

发明内容

本发明的目的是提供一种基于双目视觉的水下机器人自主避障 方法，解决了现有技术的不足。

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

一种基于双目视觉的水下机器人自主避障方法，包括如下步骤：

步骤1：建立水下机器人自主避障系统，水下机器人自主避障系 统包括双目摄像机、摄像机矫正模块、视差图处理模块、三维信息处 理模块、运动模块和避障控制模块；

摄像机矫正模块用于对双目摄像机进行标定和校准；

步骤2：摄像机矫正模块获取双目摄像机的内部矩阵和外部矩阵， 将双目摄像机的内部矩阵和外部矩阵与一组有助于校正镜头畸变的 参数一起计算，根据计算结果对双目摄像机进行校正；

内部矩阵中的元素包括水平焦距f_x、垂直焦距f_y、倾斜参数s、 表示主要点偏移的分量x₀和分量y₀；

外部矩阵中的元素包括旋转矩阵R和平移向量t；

所述一组有助于校正镜头畸变的参数包括径向畸变参数和切向 畸变参数；

步骤3：摄像机矫正模块通过代数方法计算校正一对图像所需修 正数据，具体包括如下步骤：

步骤A1：根据以下公式获得基本矩阵F：

F＝K′^-T[t]_×RK^-1；

其中：K和K′均为双目摄像机的内部矩阵，R为旋转矩阵，t为 平移向量，运算符[t]_×表示执行向量交叉积的矩阵；

步骤A2：通过基本矩阵F的奇异值分解SVD，获得基本矩阵F的 左零空间和右零空间，其公式如下：

F＝UDV^T；

其中U和V是两个正交矩阵，D是一个对角矩阵，定义D的第i对 角元为F的第i奇异值，U的第i列和V的第i列分别为基本矩阵F的左 奇异向量和右奇异向量；

基本矩阵F的右零空间为V的零奇异值列向量；

基本矩阵F的左零空间为奇异值等于零的U^T行向量；

步骤A3：对于下面的校正步骤，需要找到投影变换H′，该变换 将外极e′映射到无穷远(1,0,0)^T处的点。然后计算匹配投影变换H，它 将剩余的外极e映射到无穷远，即最小距离：

图像本身的校正通过根据各自的投影变换H和H′对两个图像重 新采样来执行；

步骤4：视差图处理模块根据SAD块匹配法和半全局块匹配法， 计算双目摄像机拍摄的视差图，通过校正图像的矩阵运算，表示物体 三维信息；

步骤5：三维信息处理模块根据双目摄像机的内部矩阵和外部矩 阵，计算出视差图中所有具有已知视差的像素对应的三维坐标；

步骤6：双目摄像机所拍摄的连续图像帧经过步骤3到步骤5的 方法处理后生成预处理连续图像帧，并发送给运动模块，运动模块对 预处理连续图像帧进行特征跟踪，利用三维坐标来估计机器人的运 动；

步骤7：在避障控制模块中预先设定多个行为控制策略；

避障控制模块根据步骤6的方法获得到双目摄像机所拍摄到的 障碍物图像，并根据障碍物图像选择一个行为控制策略作为避障策 略。

优选的，在执行步骤2时，参数k₁、k₂和k₃的径向畸变参数用以 下公式描述

x_r＝x(1+k₁r²+k₂r⁴+k₃r⁶)

y_r＝y(1+k₁r²+k₂r⁴+k₃r⁶)

r²＝x²+y²；

径向畸变参数是由标准透镜产生的；

其中x表示世界坐标系坐标x、y表示世界坐标系坐标x、x_r和 y_r表示径向畸变参数，k₁、k₂和k₃表示表示径向畸变参数

切向畸变参数用以下公式描述：

x_t＝x+[2p₁xy+p₂((x²+y²)²+2x²)]

y_t＝y+[2p₂xy+p₁((x²+y²)²+2x²)]；

其中：x表示世界坐标系坐标x、y表示世界坐标系坐标y、参数 p₁和p₂在透镜未与传感器平行安装的情况下出现，x_t和y_t表示切向畸 变参数。

优选的，在执行步骤4时，具体包括如下步骤：

步骤B1：给定一对由像素A和像素B组成的正方形块；

步骤B2：根据一下公式计算SAD块匹配法的相似性度量：

其中，n表示、i表示、j表示，a表示，b表示，

步骤B3：半全局块匹配法利用基于熵的匹配代价获得视差，具 体为将以下能量函数最小化：

其中，第一项为D的差异的所有像素匹配代价之和，第二项为p 的邻域N_p中的所有像素q增加一个常数惩罚P₁，其差异变化最大为1 像素；对于所有较大的差异变化，第三项增加了较大的常数惩罚P₂。

优选的，在执行步骤5时，具体包括如下步骤：

步骤C1：根据以下公式计算深度坐标，即z坐标：

其中f是焦距，b是基线，(x_lp-x_rp)是视差，即p点的左像素和右 像素之间的对应差；

步骤C2：根据以下公式计算横坐标，即x坐标：

其中，x_p为世界坐标系x轴数据；

步骤C3：根据以下公式计算横坐标，即y坐标：

其中，y_p为世界坐标系y轴数据；

步骤C4：根据步骤C1到步骤C3的方法，对视差图中所有具有 已知视差的像素进行三维坐标计算，得到视差图中所有具有已知视差 的像素的三维点云；

步骤C5：根据以下统计过滤器对三维点云进行过滤：

P_d＝{p_i∈P_raw|||p_i-p_j||＞μ+d_threshσ}；

其中，其中P_d是去噪声点云，μ和σ是最近邻距离的平均值和标 准偏差，P_raw表示原始点云数据集合，d_thresh表示滤波阈值，i、j表示 不同的点云数据；

步骤C6：通过点聚类来过滤噪声，去除点数较少或非结构分布的 簇。

优选的，在执行步骤6时，具体步骤如下：

步骤D1：首先从两幅连续图像中提取特征，设定在时刻T₀时，所 述运动模块获取到的预处理图像帧为(prevL，prevR)图像对，在时刻T₁时，所述运动模块获取到的预处理图像帧为(currL，currR)图像对，时刻 T₀与时刻T₁时之间的关系为：T₀＝T₁-ΔT，其中ΔT为一个时间上的常 数；

步骤D2：利用旋转和尺度不变的SURF描述子分别检测 (prevL,prevR)图像对和(currL,currR)图像对中的特征，使用从SURF中提 取的特征描述符对(prevL,prevR)图像对和(currL,currR)图像对在空间和 时间上进行匹配；

步骤D3：利用平方差和SSD法比较(prevL,prevR)图像对和 (currL,currR)图像对的特征的范数，估计运动轨迹。

本发明所述的一种基于双目视觉的水下机器人自主避障方法，提 高了水下机器人工作的自主性和机动性，在水下管道检测、水下地形 勘测、水下目标跟踪领域有广泛应用，实现了实时检测障碍物，还可 以确定水下机器人到目标的距离，本发明通过开发的水下图像校准方 法，并将其用于水下双目像机校准，获得了良好的图像效果。

附图说明

图1为本发明的三维重建方法的数学建模图；

图2是本发明的运动路径估计的三轴偏移曲线图。

具体实施方式

如图1-图2所示的一种基于双目视觉的水下机器人自主避障方 法，包括如下步骤：

步骤1：建立水下机器人自主避障系统，水下机器人自主避障系 统包括双目摄像机、摄像机矫正模块、视差图处理模块、三维信息处 理模块、运动模块和避障控制模块；

摄像机矫正模块用于对双目摄像机进行标定和校准；

步骤2：摄像机矫正模块获取双目摄像机的内部矩阵和外部矩阵， 将双目摄像机的内部矩阵和外部矩阵与一组有助于校正镜头畸变的 参数一起计算，根据计算结果对双目摄像机进行校正；

内部矩阵中的元素包括水平焦距f_x、垂直焦距f_y、倾斜参数s(如 果相机像素不是完全水平设置)、表示主要点偏移的分量x₀和分量y₀；

内部矩阵用以下矩阵K表达：

外部矩阵中的元素包括旋转矩阵R和平移向量t；

外部矩阵包括旋转矩阵R3×3、平移向量t3×1，它们共同描述了 如何把点从世界坐标系转换到摄像机坐标系，旋转矩阵是一个3×3 的正交矩阵，旋转矩阵描述了世界坐标系的坐标轴相对于摄像机坐标轴的方向，平移向量描述了在摄像机坐标系下空间原点的位置。

所述一组有助于校正镜头畸变的参数包括径向畸变参数和切向 畸变参数；

步骤3：摄像机矫正模块通过代数方法计算校正一对图像所需修 正数据，具体包括如下步骤：

步骤A1：

基本矩阵F的源表示方式为以下等式，即满足以下等式的秩为2 的矩阵：

p′^TFp＝0；

其中p和p′是两个摄像机在齐次坐标系中的对应点，因此Fp＝0 表示一条极线，该极线上的p′点必须位于双目视觉的另一幅图像中。

本发明利用摄像机的内禀矩阵K和K′以及旋转矩阵R和平移向 量t，根据以下公式获得基本矩阵F：

F＝K′^-T[t]_×RK^-1；

其中：K和K′均为双目摄像机的内部矩阵，R为旋转矩阵，t为 平移向量，运算符[t]_×表示执行向量交叉积的矩阵；

步骤A2：通过基本矩阵F的奇异值分解SVD，获得基本矩阵F的 左零空间和右零空间，其公式如下：

F＝UDV^T；

其中U和V是两个正交矩阵，D是一个对角矩阵，定义D的第i对 角元为F的第i奇异值，U的第i列和V的第i列分别为基本矩阵F的左 奇异向量和右奇异向量；

基本矩阵F的右零空间为V的零奇异值列向量；

基本矩阵F的左零空间为奇异值等于零的U^T行向量；

步骤A3：对于下面的校正步骤，需要找到投影变换H′，该变换 将外极e′映射到无穷远(1,0,0)^T处的点。然后计算匹配投影变换H，它 将剩余的外极e映射到无穷远，即最小距离：

图像本身的校正通过根据各自的投影变换H和H′对两个图像重 新采样来执行；

摄像机标定是从立体图像中提取三维信息的关键步骤。

摄像机矫正模块获取双目摄像机的内部矩阵和外部矩阵，将双目 摄像机的内部矩阵和外部矩阵与一组有助于校正镜头畸变的参数一 起计算，根据计算结果对双目摄像机进行校正；

包括径向畸变校正和切向畸变校正，畸变是由透镜产生的。

径向畸变可以使用参数k₁、k₂和k₃表示为，

x_r＝x(1+k₁r²+k₂r⁴+k₃r⁶)

y_r＝y(1+k₁r²+k₂r⁴+k₃r⁶)

r²＝x²+y²

切向畸变可以使用参数p₁和p₂表示为，

x_t＝x+[2p₁xy+p₂((x²+y²)²+2x²)]

y_t＝y+[2p₂xy+p₁((x²+y²)²+2x²)]

相机内部矩阵包括是水平焦距f_x、垂直焦距f_y、倾斜参数s以及 表示主要点偏移的分量x₀和y₀，相机内部矩阵用K表示。

相机外部矩阵包括旋转矩阵R和平移向量t。

当计算内、外矩阵时，可以用代数方法进行计算校正。

第一步是计算基本矩阵F：

p′^TFp＝0

点p和p′是两个摄像机在齐次坐标系中的对应点，Fp＝0表示一 条极线，该极线上的点p′位于另一幅图像中。

相机内部矩阵K和K′以及旋转矩阵R和平移向量t可以直接计算 出基本矩阵F

F＝K′^-T[t]_×RK^-1

其中，运算符[t]_×表示执行向量交叉积的矩阵。在得到F之后， 可以通过求F的左、右零空间来确定外极

e′^TF＝0，Fe＝0

再通过矩阵F的奇异值分解(SVD)来实现，如下所示

F＝UDV^T

其中U和V是两个正交矩阵，D是一个对角矩阵。定义D的第i对 角元为F的第i奇异值，U的第i列和V的第i列分别为F的左奇异向量 和右奇异向量，F的右零空间对应于V的零奇异值列向量。F的左零 空间对应于奇异值等于零的U^T行。

接下来的图像校正步骤，需要先找到投影变换矩阵H′，投影变 换矩阵H′将外极e′映射到无穷远处的点。

然后计算匹配投影变换矩阵H，它将剩余的外极e映射到无穷远， 即最小距离：

图像校正通过根据各自的投影变换矩阵H和H′对两个图像重新 采样来执行。

在本实施例中，通过OpenCV的校准函数来实现摄像机标定和校 准方法。

为了评估校准结果的质量，本发明通过计算平均重投影误差进行 评估。重投影误差定义为使用相机矩阵P＝[R|t]^TK，在3D图像中校准 点的实际投影和重建点重投影到相机平面之间的像素距离。重投影误 差主要用于评价摄像机标定的结果，但同时可以通过从重投影误差最 大的标定图像中迭代去除图像来人为降低重投影误差(对图像分辨率 也很敏感)。

本实施例中，测试校准程序是根据OpenCV校准函数构建的。 OpenCV三维校准功能提供了棋盘的自动检测程序。通过对灰度图像 进行直方图均衡和自适应阈值处理，生成二值图像。使用各自的 OpenCV函数以亚像素精度检测棋盘的角。最后对镜头畸变进行了优化估计。在使用的校准程序中，使用初始位置来估计相机的内部矩阵。 通过识别已知的模式，使用直接线性方法计算不考虑镜头畸变的摄像 机内部参数，然后进行优化，以优化结果并计算外部参数，同时考虑 镜头畸变校正。

步骤4：视差图处理模块根据SAD块匹配法和半全局块匹配法， 计算双目摄像机拍摄的视差图，通过校正图像的矩阵运算，表示物体 三维信息；

步骤5：三维信息处理模块根据双目摄像机的内部矩阵和外部矩 阵，计算出视差图中所有具有已知视差的像素对应的三维坐标；

步骤6：双目摄像机所拍摄的连续图像帧经过步骤3到步骤5的 方法处理后生成预处理连续图像帧，并发送给运动模块，运动模块对 预处理连续图像帧进行特征跟踪，利用三维坐标来估计机器人的运 动；

步骤7：在避障控制模块中预先设定多个行为控制策略；

避障控制模块根据步骤6的方法获得到双目摄像机所拍摄到的 障碍物图像，并根据障碍物图像选择一个行为控制策略作为避障策 略。

本实施例中，基于三维视觉的避碰系统作为自主式水下机器人干 预任务控制系统中自主层来实现，自主层包含一个控制策略模块，在 多个预先定义的行为之间进行切换，同时在发生意外事件(如水下机 器人路径上的障碍物)时，由一个反应行为系统控制水下机器人，该 系统向AUV控制系统提供制导输入。

在本实施例中，水下机器人被送到一个已知的障碍物环境中， 目的是测试障碍物的检测，并激发反应行为，以避免障碍物使用机 器人视觉。当点云中的障碍物通过用户数据报协议(UDP)通信发送 到自主层时，水下机器人的方向和估计到障碍物的距离。因此，反应性能要求改变水下机器人的航向。水下机器人转向，直到在三维 图像中不再检测到障碍物，并且在正前方定义了新的路径点。实验 表明来自三维系统的三维信息与水下机器人前视声纳提供的信息一 致。因此，本发明提出的方法能够实现实时性能。为了获得更高的 帧速率，可以通过降低图像的分辨率来提高实时性。

优选的，在执行步骤2时，参数k₁、k₂和k₃的径向畸变参数用以 下公式描述

x_r＝x(1+k₁r²+k₂r⁴+k₃r⁶)

y_r＝y(1+k₁r²+k₂r⁴+k₃r⁶)

r²＝x²+y²；

径向畸变参数是由标准透镜产生的；

其中x表示世界坐标系坐标x、y表示世界坐标系坐标y、x_r和y_r表示径 向畸变参数，k₁、k₂和k₃表示表示径向畸变参数。

切向畸变参数用以下公式描述：

x_t＝x+[2p₁xy+p₂((x²+y²)²+2x²)]

y_t＝y+[2p₂xy+p₁((x²+y²)²+2x²)]；

其中：x表示世界坐标系坐标x、y表示世界坐标系坐标y、参数 p₁和p₂在透镜未与传感器平行安装的情况下出现，x_t和y_t表示切向畸 变参数。

参数p₁和p₂是切向畸变系数，一般通过对模板正常图像和失真图 像进行多次对比实验，通过实验测量结果数据建立方程组进行求解获 得。

优选的，为了实现水下机器人视觉避障功能，需要估计水下机器 人探测到物体的三维世界坐标，因此，在完成摄像机标定和图像校正 后，下一步通过寻找三维图像对之间的对应关系是从三维图像中提取 三维信息，本发明提出通过视差图处理来实现的，本发明提出了一种 简单而快速的像素块相似性度量方法，即SAD块匹配方法，即在执行 步骤4时，具体包括如下步骤：

步骤B1：给定一对由像素A和像素B组成的正方形块；

步骤B2：根据一下公式计算SAD块匹配法的相似性度量：

其中，n表示左右图像矩阵的行列最大值，图像矩阵一般为n*n 矩阵、i表示图像矩阵的行值、j表示图像矩阵的列值，a表示左图 像，b表示右图像；

在默认参数设置的情况下，本实施例通过OpenCV的SAD块匹配函 数来实现；

步骤B3：半全局块匹配法利用基于熵的匹配代价获得视差，具 体为将以下能量函数最小化：

其中，第一项为D的差异的所有像素匹配代价之和，第二项为p 的邻域N_p中的所有像素q增加一个常数惩罚P₁，其差异变化最大为1 像素；对于所有较大的差异变化，第三项增加了较大的常数惩罚P₂。

优选的，在执行步骤5时，具体包括如下步骤：

步骤C1：根据以下公式计算深度坐标，即z坐标：

其中f是焦距，b是基线，(x_lp-x_rp)是视差，即p点的左像素和右 像素之间的对应差；

步骤C2：根据以下公式计算横坐标，即x坐标：

其中，x_p为世界坐标系x轴数据；

步骤C3：根据以下公式计算横坐标，即y坐标：

其中，y_p为世界坐标系y轴数据；

步骤C4：根据步骤C1到步骤C3的方法，对视差图中所有具有 已知视差的像素进行三维坐标计算，得到视差图中所有具有已知视差 的像素的三维点云；

步骤C5：根据以下统计过滤器对三维点云进行过滤：

P_d＝{p_i∈P_raw|||p_i-p_j||＞μ+d_threshσ}；

其中，其中P_d是去噪声点云，μ和σ是最近邻距离的平均值和标 准偏差，P_raw表示原始点云数据集合，d_thresh表示滤波阈值，i、j表示 不同的点云数据；

步骤C6：通过点聚类来过滤噪声,去除点数较少或非结构分布的 簇。

在本实施例中，通过点聚类来过滤噪声，噪声滤波器可用于去除 点数较少、非结构分布等的簇。

簇密度是深度函数，因为在三维三维重建中不确定性增加由于量 化误差，基于密度的空间聚类方法。光学通过将空间上彼此接近的点 放在一个点列表中，实现了在不同密度的数据中找到有意义的簇的能 力。利用这个表可以得到一个簇的层次结构。光学系统的输入参数是 每个簇的最小点数和考虑点对点距离的搜索半径。

本实施例中三维信息提取与处理方法是通过从所有图像(校准和 现场测试数据集)中提取聚类，分析基于聚类密度。视差图是通过使 用窗口大小为7x7(导致噪声视差图)和唯一性阈值为15的SAD度 量进行块匹配生成的。为了达到实时性能，点云被均匀地降采样到原 始点数的10％。用光学方法提取了至少10个点和ε＝0.05的星团。簇 密度是簇点距其质心的平均距离。点云由块匹配和SAD度量生成的视 差图得到，使用3种不同窗口大小(7x7、19x19和55x55)和阈值来 创建点云。实验结果表明，该方法可以提高三维信息提取与处理的性 能。

优选的，在执行步骤6时，具体步骤如下：

步骤D1：首先从两幅连续图像中提取特征，设定在时刻T₀时，所 述运动模块获取到的预处理图像帧为(prevL，prevR)图像对，在时刻T₁时，所述运动模块获取到的预处理图像帧为(currL，currR)图像对，时刻 T₀与时刻T₁时之间的关系为：T₀＝T₁-ΔT，其中ΔT为一个时间上的常 数；

步骤D2：利用旋转和尺度不变的SURF描述子分别检测 (prevL，prevR)图像对和(currL，currR)图像对中的特征，使用从SURF中提 取的特征描述符对(prevL，prevR)图像对和(currL，currR)图像对在空间和 时间上进行匹配；

步骤D3：利用平方差和SSD法比较(prevL，prevR)图像对和 (currL，currR)图像对的特征的范数，估计运动轨迹。

本实施例中，用F_f表示f帧中特征的位置。currL帧和prevL帧中特 征匹配的区别定义为：

(d_u，d_v)＝F_currL-F_prevL：

currL代表T₁时刻双目摄像机获取到的左视角图像，currR代表T₁时 刻双目摄像机获取到的右视角图像，prevL代表T₀时刻双目摄像机获 取到的左视角图像，prevR代表T₀时刻双目摄像机获取到的右视角图 像。

如图1所示，本实施例使用标准的三维重建方法来估计特征的三维位置。根据在上一步骤中找到的匹配特征集，找到最能描述时刻T₀和时刻T₁的特征之间关系的变换(旋转和平移)。沿x轴平移。估计 两帧之间的变换T(x)，其中仅在时刻T₁的特征的3D位置和在时刻T₀和 T₁的图像平面中的点的位置是已知的。本发明通过猜测的变换

估计 特征在时间T₀的3D位置，并将其投影回图像平面获取/>

x的期 望值是使两个投影之间的距离最小化的值。

三维重建方法表述为一个优化问题，其中一个目标函数的最小值 定义为

其中X＝(x，y，z，θ，α，ψ)表示在时间T₀和T₁时两帧之间相机的运动 (平移和旋转)，

是在时间T₁时检测到的所有特征的3D位置，Π是 用于将三维坐标映射到图像平面上的投影矩阵。变量(u₀，v₀)是图像平 面中所有特征在时刻T₀的位置。所有N个特征点的中值误差定义为

其中

是如图1所述的图像平面中估计特征的时刻T₀的位置。 得到的最优值为

如图2所示，本实施例采用了Nelder-Mead单纯形直接搜索算法 实现这种最小化过程。Nelder-Mead单纯形直接搜索算法是一个非线 性无约束无模型优化过程，最小化算法在变量空间上创建一个多面体 (单纯形)，其顶点在不同位置采样函数，该单纯形要么膨胀要么收缩以找到最小值。由于目标函数包含重投影误差的中值，为了估计运 动(2帧之间)，本发明利用IMU提供的旋转信息，估计差分平移， 找到所有六个变量(平移和旋转)，实验结果表明，估计的运动路径 与测量的运动路径在一定程度上一致。

图2中，Xposition为X轴向上的位移，Yposition为Y轴向上的 位移,Zposition为Z轴向上的位移。

本发明所述的一种基于双目视觉的水下机器人自主避障方法，提 高了水下机器人工作的自主性和机动性，在水下管道检测、水下地形 勘测、水下目标跟踪领域有广泛应用，实现了实时检测障碍物，还可 以确定水下机器人到目标的距离，本发明通过开发的水下图像校准方 法，并将其用于水下双目像机校准，获得了良好的图像效果，由于水下浑浊、照明条件等因素使得水下图像采集处理与分析通常比陆上条 件下更加困难，本发明提出方法可以实现对水下双目相机系统的内参 数和外参数进行估计，考虑到水下图像校准质量、水下图像清晰度均 较低，本发明提出使用SAD对块匹配方法计算稠密视差图，实验结果证明匹配精确。本发明利用统计和聚类方法，在不需要先验信息的情况下，对提取的场景三维点云进行后续滤波，得到了较好的效果。

Claims (5)

1.一种基于双目视觉的水下机器人自主避障方法，其特征在于：包括如下步骤：

步骤1：建立水下机器人自主避障系统，水下机器人自主避障系统包括双目摄像机、摄像机矫正模块、视差图处理模块、三维信息处理模块、运动模块和避障控制模块；

摄像机矫正模块用于对双目摄像机进行标定和校准；

步骤2：摄像机矫正模块获取双目摄像机的内部矩阵和外部矩阵，将双目摄像机的内部矩阵和外部矩阵与一组有助于校正镜头畸变的参数一起计算，根据计算结果对双目摄像机进行校正；

内部矩阵中的元素包括水平焦距f_x、垂直焦距f_y、倾斜参数s、表示主要点偏移的分量x₀和分量y₀；

外部矩阵中的元素包括旋转矩阵R和平移向量t；

所述一组有助于校正镜头畸变的参数包括径向畸变参数和切向畸变参数；

步骤3：摄像机矫正模块通过代数方法计算校正一对图像所需修正数据，具体包括如下步骤：

步骤A1：根据以下公式获得基本矩阵F：

F＝K′^-T[t]_×RK^-1；

其中：K和K′均为双目摄像机的内部矩阵，R为旋转矩阵，t为平移向量，运算符[t]_×表示执行向量交叉积的矩阵；

步骤A2：通过基本矩阵F的奇异值分解SVD，获得基本矩阵F的左零空间和右零空间，其公式如下：

F＝UDV^T；

其中U和V是两个正交矩阵，D是一个对角矩阵，定义D的第i对角元为F的第i奇异值，U的第i列和V的第i列分别为基本矩阵F的左奇异向量和右奇异向量；

基本矩阵F的右零空间为V的零奇异值列向量；

基本矩阵F的左零空间为奇异值等于零的U^T行向量；

步骤A3：对于下面的校正步骤，需要找到投影变换H′，该变换将外极e′映射到无穷远(1,0,0)^T处的点；然后计算匹配投影变换H，它将剩余的外极e映射到无穷远，即最小距离：

其中，H是投影变换，x_i是图像像素点坐标，i是点数；

图像本身的校正通过根据各自的投影变换H和H′对两个图像重新采样来执行；

步骤4：视差图处理模块根据SAD块匹配法和半全局块匹配法，计算双目摄像机拍摄的视差图，通过校正图像的矩阵运算，表示物体三维信息；

步骤5：三维信息处理模块根据双目摄像机的内部矩阵和外部矩阵，计算出视差图中所有具有已知视差的像素对应的三维坐标；

步骤6：双目摄像机所拍摄的连续图像帧经过步骤3到步骤5的方法处理后生成预处理连续图像帧，并发送给运动模块，运动模块对预处理连续图像帧进行特征跟踪，利用三维坐标来估计机器人的运动；

步骤7：在避障控制模块中预先设定多个行为控制策略；

避障控制模块根据步骤6的方法获得到双目摄像机所拍摄到的障碍物图像，并根据障碍物图像选择一个行为控制策略作为避障策略。

2.如权利要求1所述的一种基于双目视觉的水下机器人自主避障方法，其特征在于：在执行步骤2时，参数k₁、k₂和k₃的径向畸变参数用以下公式描述

x_r＝x(1+k₁r²+k₂r⁴+k₃r⁶)

y_r＝y(1+k₁r²+k₂r⁴+k₃r⁶)

r²＝x²+y²；

径向畸变参数是由标准透镜产生的；

其中x表示世界坐标系坐标x、y表示世界坐标系坐标y、x_r和y_r表示径向畸变参数；

切向畸变参数用以下公式描述：

x_t＝x+[2p₁xy+p₂((x²+y²)²+2x²)]

y_t＝y+[2p₂xy+p₁((x²+y²)²+2x²)]；

其中：x表示世界坐标系坐标x、y表示世界坐标系坐标y、参数p₁和p₂在透镜未与传感器平行安装的情况下出现，x_t和y_t表示切向畸变参数。

3.如权利要求1所述的一种基于双目视觉的水下机器人自主避障方法，其特征在于：在执行步骤4时，具体包括如下步骤：

步骤B1：给定一对由像素A和像素B组成的正方形块；

步骤B2：根据一下公式计算SAD块匹配法的相似性度量：

其中，A,B表示2类图像目标，像素A和B组成的正方形块，n表示图像像素点数量、i表示行、j表示列，a表示A类图像的灰度值，b表示B类图像的灰度值；

步骤B3：半全局块匹配法利用基于熵的匹配代价获得视差，具体为将以下能量函数最小化：

其中，第一项为D的差异的所有像素匹配代价之和，第二项为p的邻域N_p中的所有像素q增加一个常数惩罚P₁，其差异变化最大为1像素；对于所有较大的差异变化，第三项增加了较大的常数惩罚P₂。

4.如权利要求1所述的一种基于双目视觉的水下机器人自主避障方法，其特征在于：在执行步骤5时，具体包括如下步骤：

步骤C1：根据以下公式计算深度坐标，即z坐标：

其中f是焦距，b是基线，(x_lp-x_rp)是视差，即p点的左像素和右像素之间的对应差；

步骤C2：根据以下公式计算横坐标，即x坐标：

其中，x_p为世界坐标系x轴数据；

步骤C3：根据以下公式计算横坐标，即y坐标：

其中，y_p为世界坐标系y轴数据；

步骤C4：根据步骤C1到步骤C3的方法，对视差图中所有具有已知视差的像素进行三维坐标计算，得到视差图中所有具有已知视差的像素的三维点云；

步骤C5：根据以下统计过滤器对三维点云进行过滤：

P_d＝{p_i∈P_raw|||p_i-p_j||＞μ+d_threshσ}；

其中，其中P_d是去噪声点云，μ和σ是最近邻距离的平均值和标准偏差，P_raw表示原始点云数据集合，d_thresh表示滤波阈值，i、j表示不同的点云数据；

步骤C6：通过点聚类来过滤噪声,去除点数较少或非结构分布的簇。

5.如权利要求1所述的一种基于双目视觉的水下机器人自主避障方法，其特征在于：在执行步骤6时，具体步骤如下：

步骤D1：首先从两幅连续图像中提取特征，设定在时刻T₀时，所述运动模块获取到的预处理图像帧为(prevL,prevR)图像对，在时刻T₁时，所述运动模块获取到的预处理图像帧为(currL,currR)图像对，时刻T₀与时刻T₁时之间的关系为：T₀＝T₁-ΔT，其中ΔT为一个时间上的常数；

步骤D2：利用旋转和尺度不变的SURF描述子分别检测(prevL,prevR)图像对和(currL,currR)图像对中的特征，使用从SURF中提取的特征描述符对(prevL,prevR)图像对和(currL,currR)图像对在空间和时间上进行匹配；

步骤D3：利用平方差和SSD法比较(prevL,prevR)图像对和(currL,currR)图像对的特征的范数，估计运动轨迹。

Images