CN113109762A - 一种用于auv对接回收的光视觉引导方法 - Google Patents

Abstract

本发明属于水下航行器的对接回收技术领域，具体涉及一种用于AUV对接回收的光视觉引导方法。本发明的步骤包括光源识别、光源匹配和立体测距，光源识别是通过自适应识别算法和伪光源判别算法，识别图像中正确的光源成像；光源匹配是将立体图像的左右图像中识别到的光源，通过定义的光源特征进行配对，并利用极线搜索算法保证配对准确性；立体测距是采用三角测量方法计算已配准光源的三维坐标，并通过最小二乘法优化光源位置。本发明可靠性好、精度高，在有干扰的水下环境中能准确识别出基站光源，并得到精确的基站位置。

Description

一种用于AUV对接回收的光视觉引导方法

技术领域

本发明属于水下航行器的对接回收技术领域，具体涉及一种用于AUV对接回收的光视觉引导方法。

背景技术

AUV在水下执行特定任务时，为保证工作效率，需要在水下完成与基站的自主对接，实现给AUV电源充电、下载数据、并上传新的任务计划，从而执行下一个任务。与水面母船回收相比，水下对接回收具有隐蔽性好、自主化程度高、不间断作业等优点。作为AUV对接回收的关键技术之一，水下引导技术可以分为声学引导、电磁引导和光视觉引导。声学引导通常利用超短基线定位，为AUV提供水下基站的大致位置，精确度不高，通常用于对接的远距离阶段。电磁引导在近距离阶段可以得到基站的准确位置，但需要在水下提前布置较大功率的无线电传感器阵列，以克服水对电磁波的衰减，并且不适合基站移动的AUV动态对接。光视觉引导技术，即一种利用AUV自身携带的视觉传感器，主动识别基站的光源，通过一定的方法得到基站位置的技术，因其成本较低、灵活度好、可靠度高等特点，非常适合用于AUV水下对接的近距离引导.

本发明涉及的水下光视觉引导方法，该方法首先利用光源识别模块提取图像中正确的光源位置，接着通过光源匹配模块将立体图像中的光源进行准确的配对，最后经过立体测距模块得到精确的基站位置。

公开日为2015年07月08日、公开号为CN104766312A、发明名称为“一种基于双目光视觉引导的智能水下机器人自主对接方法”的专利申请，该方法的光视觉引导部分虽然与本发明实现相同的目的，但具体在光源识别、光源匹配和计算光源位置部分采用的方法，与发明完全不同。

公开日为2020年09月29日、公开号为CN111721259A、发明名称为“基于双目视觉的水下机器人回收定位方法”的专利申请，该方法实现的是单光源的引导方法，并且没有伪光源剔除的部分。本发明能够实现在复杂干扰环境下的多光源基站定位。

发明内容

本发明的目的在于提供可靠性好、精度高，在有干扰的水下环境中能准确识别出基站光源，并得到精确的基站位置的一种用于AUV对接回收的光视觉引导方法。

本发明的目的通过如下技术方案来实现，包括以下步骤：

步骤1：当AUV进行近距离对接时，利用搭载的立体相机，采集水下基站光源阵列的左右图像对；

所述的水下基站包括锥形导向罩和回收管，回收管与锥形导向罩半径较小的一端连接；所述的光源阵列由四个LED波长为540nm的黄绿光源组成，四个LED光源分别布置在锥形导向罩半径较大一端的上、下、左、右边缘处，整体构成正方形灯阵；

步骤2：对采集到的左右图像对分别进行自适应二值化和伪光源剔除，获取所有光源在图像中的位置，得到左图像的光源集合{Sl_i}和右图像的光源集合{Sr_i}；

步骤3：对左图像的光源集合{Sl_i}和右图像的光源集合{Sr_i}分别进行光源特征提取，对比左右图像中光源特征一致性进行配对，并通过极线搜索判断是否正确匹配：

步骤3.1：计算左图像的光源集合{Sl_i}和右图像的光源集合{Sr_i}中每个光源的灰度方向特征θ_c；

θ_c＝arctan(m₀₁/m₁₀)

其中，(x,y)表示光源成像的像素区域O_patch中像素的位置，I(x,y)表示该像素的灰度值；

步骤3.2：计算左图像的光源集合{Sl_i}和右图像的光源集合{Sr_i}中每个光源的椭圆拟合主方向特征θ_q；

对于光源的轮廓，利用直接最小二乘的椭圆拟合算法，得到光源轮廓近似椭圆的参数；已知平面椭圆的一般方程：

ax²+bxy+cy²+dx+ey+f＝0

令A＝[a,b,c,d,e,f]^T，X＝[x²,xy,y²,x,y,1]^T，将方程写成矩阵形式A^TX＝0；已知一个椭圆方程有6个自由度，需要至少6个已知点来唯一确定一个椭圆，因此拟合椭圆的最优化问题表示为：

其中，D表示数据样本集合，维数为n×6；n表示样本数，即每个光源轮廓边缘像素点的个数；通过求解上式最小二乘问题，即可得到拟合椭圆的参数A＝[a,b,c,d,e,f]^T；

步骤3.3：从左图像的光源集合{Sl_i}中选择一个光源，依次计算其与右图像的光源集合{Sr_i}中四个光源的特征相似度ρ，选择对应相似度ρ最大的光源匹配在一起；若左图像的某一光源与右图像中四个光源的特征相似度ρ均小于设定的阈值，则该光源匹配失败，执行步骤3.4；重复步骤3.3，直至完成四对光源的配对；

其中，θ_cl表示左图像中某一光源的灰度方向特征；θ_cr表示右图像中某一光源的灰度方向特征；θ_ql表示左图像中某一光源的椭圆拟合主方向特征；θ_qr表示右图像中某一光源的椭圆拟合主方向特征；

步骤3.4：对于匹配失败的左图像中某一光源，选取该光源中心P在左相机坐标系下的归一化点P′(x′,y′,1)和较远处任意给定深度值的点P″(x″,y″,z″)，通过双目相机的转移矩阵T＝[R|t]和右相机内参K₂，得到右相机像素平面下的成像点，从而唯一确定极线l₂；选择右图像中在极线l₂上的光源，完成匹配；

步骤4：将每个光源的拟合椭圆的中心作为每个光源的像素中心；利用三角测量原理，计算出每个光源在左相机坐标系下的空间位置，将四个光源的平均数作为水下基站的空间位置(x_o,y_o,z_o)。

本发明还可以包括：

所述的步骤2中对采集到的左右图像对分别进行自适应二值化和伪光源剔除，获取所有光源在图像中的位置，得到左图像的光源集合{Sl_i}和右图像的光源集合{Sr_i}的方法具体为：

步骤2.1：对输入的彩色图像进行灰度化，灰度值范围为[0,255]；

步骤2.2：对灰度图像进行二值化，设置初始阈值thread_gray＝250，对灰度值小于阈值的像素的二值取0，否则取1，得到二值图像；

步骤2.3：对二值图像利用最外层边界跟踪法，对图像中的白色区域进行最外层轮廓检测，找出轮廓边缘的像素点个数在[30,300]范围内的作为候选轮廓；判断候选轮廓个数N_s是否大于4；若候选轮廓个数N_s大于4，则输出最终二值化的图像，执行步骤2.4；否则，令thread_gray＝thread_gray-5，返回步骤2.2；

步骤2.4：对所有候选轮廓几何中心像素点进行HSV彩色域校准，剔除候选轮廓型心像素点的图像通道值H和V不满足60＜H＜120和0.5＜V＜1范围内的无效候选轮廓；其中H表示色调；V表示颜色明亮的程度；

步骤2.5：对剩余候选轮廓进行每次选取四个候选轮廓的无次序不重复组合

对每一个组合中的4个轮廓计算型心像素点之间的距离，若最大距离d_max与最小距离d_min的关系满足d_min＜d_max＜2d_min，则将该组四个候选轮廓存入光源集合{S}。

所述的步骤4中将每个光源的拟合椭圆的中心作为每个光源的像素中心；利用三角测量原理，计算出每个光源在左相机坐标系下的空间位置(x_o,y_o,z_o)的方法具体为：

步骤4.1：确定双目相机的左右两个内参矩阵K₁和K₂，以及外参矩阵T＝[R|t]；获取左图像π₁中的光源像素中心点p₁(u₁,v₁)和右图像π₂中的光源像素中心点p₂(u₂,v₂)；

步骤4.2：求解最小二乘问题

得到z₂的值，进而得出右相机坐标系下中心点的真实位置X₂＝[x₂,y₂,z₂]^T；

其中，X₁′＝[x₁/z₁,y₁/z₁,1]^T和X₂′＝[x₂/z₂,y₂/z₂,1]^T为中心点p₁和p₂的归一化坐标，由下式得出：

其中，z₁和z₂分别为中心点p₁和p₂的深度；

步骤4.3：将z₂和X₂′代入公式z₁X₁′＝z₂RX₂′+t，求出z₁，进而得到左相机坐标系下的中心点的真实位置X₁＝[x₁,y₁,z₁]^T，即光源在左相机坐标系下的真实位置(x_w,y_w,z_w)；

步骤4.4：计算四个光源在左相机坐标系下的真实位置的平均数，输出水下基站的空间位置(x_o,y_o,z_o)。

本发明的有益效果在于：

本发明是一种用于AUV对接回收的光视觉引导方法，步骤包括光源识别、光源匹配和立体测距。本发明中光源识别是通过自适应识别算法和伪光源判别算法，识别图像中正确的光源成像；光源匹配是将立体图像的左右图像中识别到的光源，通过定义的光源特征进行配对，并利用极线搜索算法保证配对准确性；立体测距是采用三角测量方法计算已配准光源的三维坐标，并通过最小二乘法优化光源位置。本发明可靠性好、精度高，在有干扰的水下环境中能准确识别出基站光源，并得到精确的基站位置。

附图说明

图1是本发明的系统流程图。

图2是本发明中水下基站及其光源阵列的示意图。

图3是本发明中光源识别的流程图。

图4是本发明中光源匹配的流程图。

图5是本发明中立体测距的流程图。

具体实施方式

下面结合附图对本发明做进一步描述。

本发明涉及水下航行器(Autonomous Underwater Vehicle,AUV)的对接回收技术领域，是一种用于AUV对接回收的光视觉引导方法，包括光源识别、光源匹配和立体测距。光源识别通过自适应识别算法和伪光源判别算法，识别图像中正确的光源成像。光源匹配指将立体图像的左右图像中识别到的光源，通过定义的光源特征进行配对，并利用极线搜索算法保证配对准确性。立体测距采用三角测量方法计算已配准光源的三维坐标，并通过最小二乘法优化光源位置。本发明提出了一种可靠性好、精度高的水下光视觉引导方法，保证在有干扰的水下环境中准确识别出基站光源，并得到精确的基站位置。

本发明包括以下步骤：

步骤(1)：输入左右图像对：当AUV进行近距离对接时，利用搭载的立体相机，采集基站光源阵列的图像对，并不断地输入到光视觉引导程序中，进入步骤(2)；

步骤(2)：光源识别：对左右图像分别进行自适应二值化和伪光源剔除，得到所有光源在图像中的位置，具体步骤如下：

步骤(2.1)：对输入的彩色图像进行灰度化，灰度值范围为[0,255]，进入步骤(2.2)；

步骤(2.2)：对灰度图像进行二值化，设置初始阈值thread_gray＝250，对灰度值小于阈值的像素的二值取0，否则取1，得到二值图像，计入步骤(2.3)；

步骤(2.3)：对二值图像利用最外层边界跟踪法，对图像中的白色区域进行最外层轮廓检测，找出轮廓边缘的像素点个数在[30,300]范围内的作为候选轮廓，判断候选轮廓个数N_s是否大于4，若不满足条件进入步骤(2.4)，否则进入步骤(2.5)；

步骤(2.4)：将当前阈值减小一定步长thread_gray＝thread_gray-5，返回步骤(2.2)；

步骤(2.5)：得到最终二值化的图像，进入步骤(2.6)。

步骤(2.6)：对所有候选轮廓几何中心像素点进行HSV彩色域校准。本发明选择的是波长为540nm的黄绿光源，因此判断候选轮廓型心像素点的图像通道值H和V是否在60＜H＜120和0.5＜V＜1的范围内，若不满足则剔除无效候选轮廓。进入步骤(2.7)；

步骤(2.7)：几何距离判别：本发明采用4个黄绿光源组成正方形灯阵，因此对剩余候选轮廓进行每次选取四个轮廓的无次序不重复组合

对每一个组合中的4个轮廓计算型心像素点之间的距离，可以得到6个距离，若最大距离d_max与最小距离d_min的关系满足d_min＜d_max＜2d_min，则进入步骤(2.8)，否则直到找出为止；

步骤(2.8)：最终得到4个光源成像轮廓集合{S}。

步骤(3)：光源匹配：通过步骤(2)得到左右图像的光源集合{Sl_i}和{Sr_i}后，对其分别进行光源特征提取，之后对比左右图像中光源特征一致性进行配对，并通过极线搜索判断是否正确匹配。具体步骤如下：

步骤(3.1)：提取灰度质心方向：对每个光源成像的像素区域O_patch中，计算图像区域的矩：

式中，(x,y)表示图像区域任意像素的位置，I(x,y)表示该像素的灰度值，因此可以得到该像素区域的质心：

连接图像区域的几何中心O和质心C，得到一个方向向量OC，则光源的灰度特征方向可以定义为：

θ_c＝arctan(m₀₁/m₁₀)

步骤(3.2)：提取拟合椭圆主方向：检测到光源标识轮廓后，利用直接最小二乘的椭圆拟合算法，得到光源轮廓近似椭圆的参数。已知平面椭圆的一般方程：

ax²+bxy+cy²+dx+ey+f＝0

令A＝[a,b,c,d,e,f]^T，X＝[x²,xy,y²,x,y,1]^T，则将方程写成矩阵形式A^TX＝0。已知一个椭圆方程有6个自由度，需要至少6个已知点来唯一确定一个椭圆，因此拟合椭圆的最优化问题可表示为：

其中D表示数据样本集合，维数为n×6，n表示样本数，即每个光源轮廓边缘像素点的个数。通过求解上式最小二乘问题，即可得到拟合椭圆的参数A。已知椭圆的主方向θ_q与椭圆方程参数A，满足如下公式：

求解上式，即可得到椭圆的主方向θ_q，作为光源的另一个特征。

步骤(3.3)：特征匹配：将左右图像中的光源集合方向特征进行比较，用特征的欧氏距离定义光源相似度

式中，(θ_cl,θ_ql)表示左图像中的某一光源特征，(θ_cr,θ_qr)表示右图像中的某一光源特征。选择左图中的某一光源，依次计算与右图4个光源的特征相似度，选出相似度最大的左右两个光源作为一对。若左图某一光源，与右图像中的4个光源计算的特征相似度均小于一定的阈值，据经验取1，则表示该光源匹配失败，还需要进入步骤(3.4)。

步骤(3.4)：极线判别：π₁和π₂分别表示左右相机的成像平面，光源中心P在左图像上的成像点为p₁，根据针孔相机模型，当光源中心深度P未知时，其在右图像上的成像点应该在极线l₂上。因此，选取中心深度P在左相机坐标系下的归一化点P′(x′,y′,1)和较远处任意给定深度值的点P″(x″,y″,z″)，通过双目相机的转移矩阵T＝[R|t]和右相机内参K₂，即可得到右相机像素平面下的成像点，从而唯一确定极线l₂。步骤(3.3)某个光源匹配失败后，判断右图像上的光源成像点是否在极线l₂上，即可完成匹配。

步骤(4)：立体测距：将步骤(3)光源拟合椭圆的中心，作为每个光源的像素中心，并利用三角测量原理，计算出每个光源在左相机坐标系下的空间位置，最终求取4个光源的平均数，作为水下基站的空间位置，具体步骤如下：

步骤(4.1)：通过相机标定算法一旦确定双目相机的左右两个内参矩阵K₁和K₂，以及外参矩阵T＝[R|t]，即可通过三角测量计算每个匹配好的光源中心点对在相机坐标系下的三维位置。考虑左右图像π₁和π₂，以左图建立坐标系原点，则右图的变换矩阵为T＝[R|t]。相机光心为O₁和O₂。在π₁中检测到光源像素中心点p₁(u₁,v₁)，对应π₂中的光源像素中心点p₂(u₂,v₂)。理论上直线O₁p₁与O₂p₂在场景中会相交于一点P，该点即两个光源像素中心点对应的光源标识在三维场景中的位置。然而由于噪声和检测精度的影响，两条直线往往无法相交。因此，可以通过最小二乘法求解。根据相机位姿变换关系可得：

z₁X₁′＝z₂RX₂′+t

其中，X₁′＝[x₁/z₁,y₁/z₁,1]^T和X₂′＝[x₂/z₂,y₂/z₂,1]^T为中心点p₁和p₂的归一化坐标，可由下式得出：

z₁和z₂为两个中心点的深度。对上式左右两边左乘一个X₁′的反对称矩阵

得到：

该式左侧为零，右侧可看成关于z₂的方程。由于噪声和误差的存在，不能直接求解出深度，因此需要构建一个最小二乘求解：

通过求解上式最小二乘问题，即可得到z₂的值，即得出右相机坐标系下中心点的真实位置X₂＝[x₂,y₂,z₂]^T，再将z₂和X₂′代入公式z₁X₁′＝z₂RX₂′+t，即可求出z₁，最终得到左相机坐标系下的中心点的真实位置X₁＝[x₁,y₁,z₁]^T，即光源在左相机坐标系下的真实位置(x_w,y_w,z_w)，进入步骤(4.2)。

步骤(4.2)：对求得的每个光源位置(x_w,y_w,z_w)，计算平均数，即可得到基站的最终位置(x_o,y_o,z_o)。

步骤(5)：输出基站的三维位置(x_o,y_o,z_o)，结束。

实施例1：

图1表述了本发明的系统流程，具体实现步骤如下：

步骤(1)：输入左右图像对：当AUV进行近距离对接时，利用搭载的立体相机，采集基站光源阵列的图像对，并不断地输入到光视觉引导程序中，进入步骤(2)；

步骤(2)：光源识别：对左右图像分别进行自适应二值化和伪光源剔除，得到所有光源在图像中的位置，进入步骤(3)；

步骤(3)：光源匹配：通过步骤(2)得到左右图像的光源集合{Sl_i}和{Sr_i}后，对其分别进行光源特征提取，之后对比左右图像中光源特征一致性进行配对，并通过极线搜索判断是否正确匹配，进入步骤(4)；

步骤(4)：立体测距：将步骤(3)光源拟合椭圆的中心，作为每个光源的像素中心，并利用三角测量原理，计算出每个光源在左相机坐标系下的空间位置，最终求取4个光源的平均数，作为水下基站的空间位置，进入步骤(5)；

步骤(5)：输出基站的三维位置(x_o,y_o,z_o)，结束。

图2是本发明基站光源阵列安装示意图，具体为：水下基站对接装置主要由锥形导向罩1和回收管3组成作为基站主体，并由支架2固定在水平底座4上，4个LED光源5固定在锥形导向罩1边缘，组成正方形灯阵。

图3是本发明光源识别算法的流程图，具体实现步骤如下：

步骤(1)：对输入的彩色图像进行灰度化，灰度值范围为[0,255]，进入步骤(2)；

步骤(2)：对灰度图像进行二值化，设置初始阈值thread_gray＝250，对灰度值小于阈值的像素的二值取0，否则取1，得到二值图像，计入步骤(3)；

步骤(3)：对二值图像利用最外层边界跟踪法，对图像中的白色区域进行最外层轮廓检测，找出轮廓边缘的像素点个数在[30,300]范围内的作为候选轮廓，判断候选轮廓个数N_s是否大于4，若不满足条件进入步骤(4)，否则进入步骤(5)；

步骤(4)：将当前阈值减小一定步长thread_gray＝thread_gray-5，返回步骤(2)；

步骤(5)：得到最终二值化的图像，进入步骤(6)。

步骤(6)：对所有候选轮廓几何中心像素点进行HSV彩色域校准。本发明选择的是波长为540nm的黄绿光源，因此判断候选轮廓型心像素点的图像通道值H和V是否在60＜H＜120和0.5＜V＜1的范围内，若不满足则剔除无效候选轮廓。进入步骤(7)；

步骤(7)：几何距离判别：本发明采用4个黄绿光源组成正方形灯阵，因此对剩余候选轮廓进行每次选取四个轮廓的无次序不重复组合

对每一个组合中的4个轮廓计算型心像素点之间的距离，可以得到6个距离，若最大距离d_max与最小距离d_min的关系满足d_min＜d_max＜2d_min，则进入步骤(8)，否则直到找出为止；

步骤(8)：最终得到4个光源成像轮廓集合{S}。

图4是本发明光源匹配算法的流程图，具体实现步骤如下：

步骤(1)：提取灰度质心方向：对每个光源成像的像素区域O_patch中，计算图像区域的矩：

式中，(x,y)表示图像区域任意像素的位置，I(x,y)表示该像素的灰度值，因此可以得到该像素区域的质心：

连接图像区域的几何中心O和质心C，得到一个方向向量OC，则光源的灰度特征方向可以定义为：

θ_c＝arctan(m₀₁/m₁₀)

步骤(2)：提取拟合椭圆主方向：检测到光源标识轮廓后，利用直接最小二乘的椭圆拟合算法，得到光源轮廓近似椭圆的参数。已知平面椭圆的一般方程：

ax²+bxy+cy²+dx+ey+f＝0

令A＝[a,b,c,d,e,f]^T，X＝[x²,xy,y²,x,y,1]^T，则将方程写成矩阵形式A^TX＝0。已知一个椭圆方程有6个自由度，需要至少6个已知点来唯一确定一个椭圆，因此拟合椭圆的最优化问题可表示为：

其中D表示数据样本集合，维数为n×6，n表示样本数，即每个光源轮廓边缘像素点的个数。通过求解上式最小二乘问题，即可得到拟合椭圆的参数。已知椭圆的主方向θ_q与椭圆方程参数A，满足如下公式：

求解上式，即可得到椭圆的主方向θ_q，作为光源的另一个特征。

步骤(3)：特征匹配：将左右图像中的光源集合方向特征进行比较，用特征的欧氏距离定义光源相似度

式中，(θ_cl,θ_ql)表示左图像中的某一光源特征，(θ_cr,θ_qr)表示右图像中的某一光源特征。选择左图中的某一光源，依次计算与右图4个光源的特征相似度，选出相似度最大的左右两个光源作为一对。若左图某一光源，与右图像中的4个光源计算的特征相似度均小于一定的阈值，据经验取1，则表示该光源匹配失败，还需要进入步骤(4)。

步骤(4)：极线判别：π₁和π₂分别表示左右相机的成像平面，光源中心P在左图像上的成像点为p₁，根据针孔相机模型，当光源中心深度P未知时，其在右图像上的成像点应该在极线l₂上。因此，选取中心深度P在左相机坐标系下的归一化点P′(x′,y′,1)和较远处任意给定深度值的点P″(x″,y″,z″)，通过双目相机的转移矩阵T＝[R|t]和右相机内参K₂，即可得到右相机像素平面下的成像点，从而唯一确定极线l₂。步骤(3)某个光源匹配失败后，判断右图像上的光源成像点是否在极线l₂上，即可完成匹配。

图5是本发明立体测距算法的流程图，具体实现步骤如下：

步骤(1)：输入配准好的光源对，进入步骤(2)；

步骤(2)：通过相机标定算法一旦确定双目相机的左右两个内参矩阵K₁和K₂，以及外参矩阵T＝[R|t]，即可通过三角测量计算每个匹配好的光源中心点对在相机坐标系下的三维位置。考虑左右图像π₁和π₂，以左图建立坐标系原点，则右图的变换矩阵为T＝[R|t]。相机光心为O₁和O₂。在π₁中检测到光源像素中心点p₁(u₁,v₁)，对应π₂中的光源像素中心点p₂(u₂,v₂)。理论上直线O₁p₁与O₂p₂在场景中会相交于一点P，该点即两个光源像素中心点对应的光源标识在三维场景中的位置。然而由于噪声和检测精度的影响，两条直线往往无法相交。因此，可以通过最小二乘法求解。根据相机位姿变换关系可得：

z₁X₁′＝z₂RX₂′+t

其中，X₁′＝[x₁/z₁,y₁/z₁,1]^T和X₂′＝[x₂/z₂,y₂/z₂,1]^T为中心点p₁和p₂的归一化坐标，可由下式得出：

z₁和z₂为两个中心点的深度。对上式左右两边左乘一个X₁′的反对称矩阵

得到：

该式左侧为零，右侧可看成关于z₂的方程。由于噪声和误差的存在，不能直接求解出深度，因此需要构建一个最小二乘求解：

通过求解上式最小二乘问题，即可得到z₂的值，即得出右相机坐标系下中心点的真实位置X₂＝[x₂,y₂,z₂]^T，再将z₂和X₂′代入公式z₁X₁′＝z₂RX₂′+t，即可求出z₁，最终得到左相机坐标系下的中心点的真实位置X₁＝[x₁,y₁,z₁]^T，即光源在左相机坐标系下的真实位置(x_w,y_w,z_w)，进入步骤(3)；

步骤(3)：对求得的每个光源位置(x_w,y_w,z_w)，计算平均数，即可得到基站的最终位置(x_o,y_o,z_o)，进入步骤(4)；

步骤(4)：输出基站的三维位置(x_o,y_o,z_o)，结束。

以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (3)

1.一种用于AUV对接回收的光视觉引导方法，其特征在于，包括以下步骤：

步骤1：当AUV进行近距离对接时，利用搭载的立体相机，采集水下基站光源阵列的左右图像对；

所述的水下基站包括锥形导向罩和回收管，回收管与锥形导向罩半径较小的一端连接；所述的光源阵列由四个LED波长为540nm的黄绿光源组成，四个LED光源分别布置在锥形导向罩半径较大一端的上、下、左、右边缘处，整体构成正方形灯阵；

步骤2：对采集到的左右图像对分别进行自适应二值化和伪光源剔除，获取所有光源在图像中的位置，得到左图像的光源集合{Sl_i}和右图像的光源集合{Sr_i}；

步骤3：对左图像的光源集合{Sl_i}和右图像的光源集合{Sr_i}分别进行光源特征提取，对比左右图像中光源特征一致性进行配对，并通过极线搜索判断是否正确匹配：

步骤3.1：计算左图像的光源集合{Sl_i}和右图像的光源集合{Sr_i}中每个光源的灰度方向特征θ_c；

θ_c＝arctan(m₀₁/m₁₀)

其中，(x,y)表示光源成像的像素区域O_patch中像素的位置，I(x,y)表示该像素的灰度值；

步骤3.2：计算左图像的光源集合{Sl_i}和右图像的光源集合{Sr_i}中每个光源的椭圆拟合主方向特征θ_q；

对于光源的轮廓，利用直接最小二乘的椭圆拟合算法，得到光源轮廓近似椭圆的参数；已知平面椭圆的一般方程：

ax²+bxy+cy²+dx+ey+f＝0

令A＝[a,b,c,d,e,f]^T，X＝[x²,xy,y²,x,y,1]^T，将方程写成矩阵形式A^TX＝0；已知一个椭圆方程有6个自由度，需要至少6个已知点来唯一确定一个椭圆，因此拟合椭圆的最优化问题表示为：

其中，D表示数据样本集合，维数为n×6；n表示样本数，即每个光源轮廓边缘像素点的个数；通过求解上式最小二乘问题，即可得到拟合椭圆的参数A＝[a,b,c,d,e,f]^T；

步骤3.3：从左图像的光源集合{Sl_i}中选择一个光源，依次计算其与右图像的光源集合{Sr_i}中四个光源的特征相似度ρ，选择对应相似度ρ最大的光源匹配在一起；若左图像的某一光源与右图像中四个光源的特征相似度ρ均小于设定的阈值，则该光源匹配失败，执行步骤3.4；重复步骤3.3，直至完成四对光源的配对；

其中，θ_cl表示左图像中某一光源的灰度方向特征；θ_cr表示右图像中某一光源的灰度方向特征；θ_ql表示左图像中某一光源的椭圆拟合主方向特征；θ_qr表示右图像中某一光源的椭圆拟合主方向特征；

步骤3.4：对于匹配失败的左图像中某一光源，选取该光源中心P在左相机坐标系下的归一化点P′(x′,y′,1)和较远处任意给定深度值的点P″(x″,y″,z″)，通过双目相机的转移矩阵T＝[R|t]和右相机内参K₂，得到右相机像素平面下的成像点，从而唯一确定极线l₂；选择右图像中在极线l₂上的光源，完成匹配；

步骤4：将每个光源的拟合椭圆的中心作为每个光源的像素中心；利用三角测量原理，计算出每个光源在左相机坐标系下的空间位置，将四个光源的平均数作为水下基站的空间位置(x_o,y_o,z_o)。

2.根据权利要求1所述的一种用于AUV对接回收的光视觉引导方法，其特征在于：所述的步骤2中对采集到的左右图像对分别进行自适应二值化和伪光源剔除，获取所有光源在图像中的位置，得到左图像的光源集合{Sl_i}和右图像的光源集合{Sr_i}的方法具体为：

步骤2.1：对输入的彩色图像进行灰度化，灰度值范围为[0,255]；

步骤2.2：对灰度图像进行二值化，设置初始阈值thread_gray＝250，对灰度值小于阈值的像素的二值取0，否则取1，得到二值图像；

步骤2.3：对二值图像利用最外层边界跟踪法，对图像中的白色区域进行最外层轮廓检测，找出轮廓边缘的像素点个数在[30,300]范围内的作为候选轮廓；判断候选轮廓个数N_s是否大于4；若候选轮廓个数N_s大于4，则输出最终二值化的图像，执行步骤2.4；否则，令thread_gray＝thread_gray-5，返回步骤2.2；

步骤2.4：对所有候选轮廓几何中心像素点进行HSV彩色域校准，剔除候选轮廓型心像素点的图像通道值H和V不满足60＜H＜120和0.5＜V＜1范围内的无效候选轮廓；其中H表示色调；V表示颜色明亮的程度；

步骤2.5：对剩余候选轮廓进行每次选取四个候选轮廓的无次序不重复组合

对每一个组合中的4个轮廓计算型心像素点之间的距离，若最大距离d_max与最小距离d_min的关系满足d_min＜d_max＜2d_min，则将该组四个候选轮廓存入光源集合{S}。

3.根据权利要求1或2所述的一种用于AUV对接回收的光视觉引导方法，其特征在于：所述的步骤4中将每个光源的拟合椭圆的中心作为每个光源的像素中心；利用三角测量原理，计算出每个光源在左相机坐标系下的空间位置(x_o,y_o,z_o)的方法具体为：

步骤4.1：确定双目相机的左右两个内参矩阵K₁和K₂，以及外参矩阵T＝[R|t]；获取左图像π₁中的光源像素中心点p₁(u₁,v₁)和右图像π₂中的光源像素中心点p₂(u₂,v₂)；

步骤4.2：求解最小二乘问题

得到z₂的值，进而得出右相机坐标系下中心点的真实位置X₂＝[x₂,y₂,z₂]^T；

其中，X₁′＝[x₁/z₁,y₁/z₁,1]^T和X₂′＝[x₂/z₂,y₂/z₂,1]^T为中心点p₁和p₂的归一化坐标，由下式得出：

其中，z₁和z₂分别为中心点p₁和p₂的深度；

步骤4.3：将z₂和X₂′代入公式z₁X₁′＝z₂RX₂′+t，求出z₁，进而得到左相机坐标系下的中心点的真实位置X₁＝[x₁,y₁,z₁]^T，即光源在左相机坐标系下的真实位置(x_w,y_w,z_w)；

步骤4.4：计算四个光源在左相机坐标系下的真实位置的平均数，输出水下基站的空间位置(x_o,y_o,z_o)。

Images