CN109798877B

CN109798877B - 一种仿生水下机器鱼双目立体测距方法

Info

Publication number: CN109798877B
Application number: CN201910126386.3A
Authority: CN
Inventors: 熊明磊
Original assignee: Boya Gongdao Beijing Robot Technology Co Ltd
Current assignee: Boya Gongdao Beijing Robot Technology Co Ltd
Priority date: 2019-02-20
Filing date: 2019-02-20
Publication date: 2021-08-06
Anticipated expiration: 2039-02-20
Also published as: CN109798877A

Abstract

本发明公开一种仿生水下机器鱼双目立体测距方法，通过SGBM的匹配算法不仅可以获取更多有效深度点，而且在计算速度上也优于SIFT特征提取匹配的算法；同时，在水下场景光线衰减严重的情况下，采用近距离精度更高的图像双目测距，结合远距离声呐探测的方式；不仅弥补声呐近距离流场干扰不足，又能减少远距离双目受光线不足的影响，实现高精度的水下避障探测。

Description

一种仿生水下机器鱼双目立体测距方法

技术领域

本发明涉及机器人视觉检测相关技术领域，具体来说，涉及一种仿生水下机器鱼双目立体测距方法。

背景技术

仿生机器鱼用于完成水下探测任务，一般采用声呐的方式进行障碍物的探测，但在水下场景光线衰减严重的情况下，声呐近距离流场干扰不足，无法进行及时有效的判断；其次，也有仿生机器鱼在水下运动过程中通过双目机器视觉的方式对前方阻碍物进行判断，而现有双目测距过程中采用如SIFT特征提取匹配的方式，不仅获取的有效深度点较少，而且算法复杂度高，无法满足本体在运动过程中对于障碍物体判断躲避的实时性。

针对相关技术中的问题，目前尚未提出有效的解决方案。

发明内容

针对相关技术中的上述技术问题，本发明提出一种仿生水下机器鱼双目立体测距方法，包括以下步骤：

S1相机标定：对双目相机进行标定；在标定过程中，参考相机畸变状态修正理想投影模型；

S2立体匹配：通过极线约束的方式进行匹配，具体的，使用SGBM算法在左右两相机已经行对齐的图像中的同一行上寻找最相似点；

S3双目测距：采用相似三角形原理，基于双目相机视差进行测距；和

S4声呐测距：采用单波束声呐进行测距；

S5：采用无迹kalman滤波融合将S3中双目测距数据和S4中声呐测距数据；得到最终测量数据。

进一步的，在S1相机标定中，具体包括：原始视图、消除畸变、双目校正和图像裁剪。

进一步的，在S1中，消除畸变中参考相机畸变状态具体包括：径向畸变和切向畸变。

进一步的，在S2中，使用SGBM算法具体包括：通过选取两行对齐的图像中每个像素点的disparity，组成一个disparity map，通过将一和disparity map相关的全局能量函数E(D)最小化，求解每个像素p最优disparity；能量函数E(D)为：

其中：D为disparity map；p,q分别代表图像中的两不同像素；Np为像素p的相邻像素点；C(p,Dp)为当前像素点disparity为Dp时，该像素点的cost；P1为第一惩罚系数，它适用于像素p相邻像素中dsparity值与p的dsparity值相差1的那些像素；P2为第二惩罚系数，它适用于像素p相邻像素中dsparity值与p的dsparity值相差大于1的那些像素。

进一步的，在S2中，求解每个像素p最优disparity过程包括分别求解像素p各个方向上的最小cost值；

其中：r为某个指向当前像素p的方向；Lr(p,d)表示沿着当前方向，当目前像素p的disparity取值为d时，其最小cost值。

进一步的，在S2中，将各个方向上的cost值累加，选取累加cost值最小的disparity值作为该像素的最终disparity值。

进一步的，在S3中，基于双目相机视差进行测距中采用下式进行计算：

其中，d为步骤S2中得到的最终disparity值；fTx为步骤S1中相机标定得到的标定结果。

本发明的有益效果：通过SGBM的匹配算法不仅可以获取更多有效深度点，而且在计算速度上也优于SIFT特征提取匹配的算法；同时，在水下场景光线衰减严重的情况下，采用近距离精度更高的图像双目测距，结合远距离声呐探测的方式；不仅弥补声呐近距离流场干扰不足，又能减少远距离双目受光线不足的影响，实现高精度的水下避障探测。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1是根据本发明实施例所述的一种仿生水下机器鱼双目立体测距方法中声呐测距的结构示意图；

图2是根据本发明实施例所述的一种仿生水下机器鱼双目立体测距方法中相机标定的流程图；

图3是根据本发明实施例所述的一种仿生水下机器鱼双目立体测距方法中径向畸变的示意图；

图4是根据本发明实施例所述的一种仿生水下机器鱼双目立体测距方法中切向畸变的示意图；

图5是根据本发明实施例所述的一种仿生水下机器鱼双目立体测距方法中相机标定方法示意图；

图6是根据本发明实施例所述的一种仿生水下机器鱼双目立体测距方法中两相机坐标系匹配变换的示意图；

图7是根据本发明实施例所述的一种仿生水下机器鱼双目立体测距方法中左右两相机采集到的图像匹配示意图；

图8是根据本发明实施例所述的一种仿生水下机器鱼双目立体测距方法原理示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员所获得的所有其他实施例，都属于本发明保护的范围。

如图1-8所示，根据本发明实施例所述的一种仿生水下机器鱼双目立体测距方法，包括以下步骤：

S4声呐测距：采用单波束声呐进行测距；

如图1所示，本实施例中，在仿生水下机器鱼的主体上三个标号分别为1、2、3的单波束声呐，并且图中A、B、C分别表示各对应声呐的收发波束角；其中单波束声呐1位于仿生水下机器鱼主体前端双目相机之间；仿生水下机器鱼主体根据每个传感器获得的距离参数进行实时观测，再反推出主体周围的障碍物大体轮廓，根据障碍物大体轮廓，进行转向和游速判断，进行避障；

如图2所示，本实施例中，在S1相机标定中，具体包括：原始视图、消除畸变、双目校正和图像裁剪；消除畸变中参考相机畸变状态具体包括：径向畸变和切向畸变；理想的摄像机模型是针孔模型，但是实际的镜头不符合这种假设。另外，相机的构造以及制造、安装、工艺等因素也会造成误差，导致相机通常会存在多种非线性畸变，使得上面介绍的针孔相机模型并不能准确地描述最终的成像关系。所以，为了使相机标定结果更加准确，在进行相机标定时应该将相机的非线性畸变考虑进来，修正理想投影模型。

如图3所示，径向畸变产生的主要原因是镜头径向曲率的不规则变化，它会导致图像的扭曲变形，例如，空间中的一条直线成像到图像平面后发生弯曲，变形成一条曲线。这种畸变的特点是以主点为中心，沿径向移动，离的距离越远，产生的变形量就越大。径向畸变是导致图像畸变的主要因素。如图3所示，一个矩形的严重径向失真被较正成理想线性镜头的图像；径向畸变有桶形畸变和枕形畸变两种；径向畸变模型在opencv中被描述为：

xcorrected＝x(1+k1r2+k2r4+k3r6)ycorrected＝y(1+k1r2+k2r4+k3r6)

如图4所示，由于透镜不是完美地平行于图像平面，所以存在切向畸变；这种畸变使得一些区域看上去比预期的近；畸变模型为：

xcorrected＝x+[2p1xy+p2(r2+2x2)]ycorrected＝y+[p1(r2+2y2)+2p2xy]

本实施例中，用以下5个参数描述摄像机畸变，即：

Distortion coefficients＝(k1,k2,p1,p2,k3)

如图5所示，本实施例，在具体标定过程中，固定双目相机位置，棋盘格摆放不同角度拍摄，N个角点的K张图片需要满足：

2·N·K≥6·K+4

即可解得超定线性方程组，得出相机内参(Fx,Fy,Cx,Cy)以及五个畸变参数(k1,k2,p1,p2,k3)；拍摄两组标定图像，分别标定矫正之后，将右边相机放射变换，使左右相机像素行对其，方便匹配计算，最后裁剪出有效区域以计算。

如图6所示，三维世界的一个点在两个相机的成像，我们可以相信，这两个在各自相机的相对位置基本不可能是一样的，而这种位置的差别，也正是我们眼睛区别3D和2D的关键，将右边的摄像机投影到左边，因为他的坐标和左边相机的左边相距Tx(标定测出来的外参数)，所以它相当于在左边的相机对三维世界内的(x-tx,y,z)进行投影，即可得相似三角，这里视差就是d＝x-x‘；图6中，根据双目相机中两相机坐标系间的关系，可以得到：

其中，x_l代表左边相机坐标系中X方向的距离值，x_r代表右边相机坐标系中X方向的距离值；d为步骤S2中得到的最终disparity值；fTx为步骤S1中相机标定得到的标定结果。

如图7所示，本实施例中，在S2中，使用SGBM算法具体包括：通过选取两行对齐的图像中每个像素点的disparity，组成一个disparity map，通过将一和disparity map相关的全局能量函数E(D)最小化，求解每个像素p最优disparity；能量函数E(D)为：

利用上述函数在一个二维图像中寻找最优解是一个NP-complete问题，耗时过于巨大，因此该问题被近似分解为多个一维问题，即线性问题。而且每个一维问题都可以用动态规划来解决。因为1个像素有8个相邻像素，因此一般分解为8个一维问题。本实施例中，从左到右这一方向为例，如图7所示，在S2中，求解每个像素p最优disparity过程包括分别求解像素p各个方向上的最小cost值；

其中：r为某个指向当前像素p的方向；Lr(p,d)表示沿着当前方向，当目前像素p的disparity取值为d时，其最小cost值；

因为Lr(p,d)是会随着当前像素的右移不停增长的，为了防止数值溢出，所以要让它维持在一个较小的数值。

C(p,d)的计算通过如下两个公式计算：

C(p，d)＝min(d(p，p-d，I_L，I_R)，d(p-d，p，I_R，I_L))

即，当前像素p和移动d之后的像素q之间，经过半个像素插值后，寻找两个像素点灰度或者RGB差值的最小值，作为C(p,d)的值。

具体来说：设像素p的灰度/RGB值为I(p)，先从I(p)，(I(p)+I(p-1))/2,(I(p)+I(p+1))/2三个值中选择出和I(q)差值最小的,即d(p,p-d)；然后再从I(q)，(I(q)+I(q-1))/2,(I(q)+I(q+1))/2三个值中选择出和I(p)差值最小的,即d(p-d,p)；最后从两个值中选取最小值，就是C(p,d)；

以上是从一个方向(从左至右)计算出的像素在取值为某一disparity值时的最小cost值。一个像素有8个邻域，所以一共要从8个方向计算(左右，右左，上下，下上，左上右下，右下左上，右上左下，左下右上)这个cost值；其它方向的cost值在此不再赘述；

如图8所示，本实施例中，在S2中，将各个方向上的cost值累加，选取累加cost值最小的disparity值作为该像素的最终disparity值；对于每个像素进行该操作后，就形成了整个图像的disparity map。公式表达如下：

SGBM算法遍历每个像素，针对每个像素的操作和disparity的范围有关，故时间复杂度为：O(w·h·n)。

综上，借助于本发明的上述技术方案，通过SGBM的匹配算法不仅可以获取更多有效深度点，而且在计算速度上也优于SIFT特征提取匹配的算法；同时，在水下场景光线衰减严重的情况下，采用近距离精度更高的图像双目测距，结合远距离声呐探测的方式；不仅弥补声呐近距离流场干扰不足，又能减少远距离双目受光线不足的影响，实现高精度的水下避障探测。

以上仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims

1.一种仿生水下机器鱼双目立体测距方法，其特征在于，包括以下步骤：

S4声呐测距：采用单波束声呐进行测距；

S5：采用无迹kalman滤波融合将S3中双目测距数据和S4中声呐测距数据相结合；得到最终测量数据。

2.根据权利要求1所述的一种仿生水下机器鱼双目立体测距方法，其特征在于，在S1相机标定中，具体包括：原始视图、消除畸变、双目校正和图像裁剪。

3.根据权利要求2所述的一种仿生水下机器鱼双目立体测距方法，其特征在于，在S1中，消除畸变中参考相机畸变状态具体包括：径向畸变和切向畸变。

4.根据权利要求3所述的一种仿生水下机器鱼双目立体测距方法，其特征在于，在S2中，使用SGBM算法具体包括：通过选取两行对齐的图像中每个像素点的disparity，组成一个disparity map，通过将一个disparity map和disparity map相关的全局能量函数E(D)最小化，求解每个像素p最优disparity；能量函数E(D)为：

5.根据权利要求4所述的一种仿生水下机器鱼双目立体测距方法，其特征在于，在S2中，求解每个像素p最优disparity过程包括分别求解像素p各个方向上的最小cost值；

6.根据权利要求5所述的一种仿生水下机器鱼双目立体测距方法，其特征在于，在S2中，将各个方向上的cost值累加，选取累加cost值最小的disparity值作为该像素的最终disparity值。

7.根据权利要求6所述的一种仿生水下机器鱼双目立体测距方法，其特征在于，在S3中，基于双目相机视差进行测距中采用下式进行计算：

其中，d为步骤S2中得到的最终disparity值；fTx为步骤S1中相机标定得到的标定结果；f为焦距；Z为相机的像素坐标系中Z方向上的深度值。