CN115147576A

CN115147576A - 一种基于关键特征的水下机器人对接单目视觉导引方法

Info

Publication number: CN115147576A
Application number: CN202210770567.1A
Authority: CN
Inventors: 徐元欣; 马鑫奇; 刘诚; 陈首旭; 曾庆锋; 单文才; 张彩宝; 李心慧; 陶子寅; 王鹏
Original assignee: Zhejiang University ZJU
Current assignee: Zhejiang University ZJU
Priority date: 2022-06-30
Filing date: 2022-06-30
Publication date: 2022-10-04

Abstract

本发明公开了一种实时精准的，并基于关键特征主动识别的水下机器人对接单目视觉导引方法，该方法由远端视觉引导与近端视觉引导两部分组成，在视觉作用远端时检测坞站外部机械装置关键点，视觉作用近端时通过预测坞站内部目标物关键特征来推断关键点所在位置。本发明设计了远端视觉引导方案，采用目标检测网络检测坞站机械装置关键点像素位置，并使用一种两步位姿估计法提高位姿解算的精度；设计了近端视觉引导方案，采用目标区域稠密矢量作为关键特征，并基于矢量投票和矢量投影的两步关键点位置估计方法提高关键点像素位置的准确度，进一步使用两步位姿估计法提高位姿解算精度。

Description

一种基于关键特征的水下机器人对接单目视觉导引方法

技术领域

本发明属于水下机器人回收对接技术领域，尤其涉及一种基于关键特征的水下机器人对接单目视觉导引方法。

背景技术

大型水下移动平台在执行水下探测任务时，其庞大的结构和复杂的运动特性易受到地形的限制，移动平台搭载的水下机器人因其隐蔽性好和灵活性高，常代替移动平台进行水下作业任务。远海固定作业平台也常采用小型水下机器人开展附近海域的水下探测与开采作业，该作业平台会搭载固定静态回收装置来安置水下机器人。无论是移动平台对应的移动对接场景还是静态装置对应的固定对接场景，水下机器人均采用自身携带的电池供给能源，而水下回收对接技术能够使机器人在长时间水下作业后自主进入到回收平台中完成电池充电与信息交换等行为。视觉导引是回收对接的重要阶段，常采用视觉的方法获取机器人与目标的相对位置与姿态，并通过导航、路径规划和控制等模块实现水下自主对接。

目前机器人视觉导引方法通常采用灯阵的被动光学引导方案，这种方案采用水下引导灯组成的发光灯阵作为水下目标物，并通过视觉方法提取引导灯在图像中的像素位置，结合其先验3D坐标信息解算机器人与灯阵间的相对位姿。在实现本发明的过程中，发明人发现现有技术中至少存在如下问题：

1.水下漂浮物的干扰和水体的散射易导致视觉方法提取引导灯光中心存在误差；2.引导灯光自身特征较少，只能用于深海黑暗环境，当存在其他光源干扰时易导致引导灯中心关键点识别错误；3.引导灯超出相机视野后易导致关键点的缺失而无法解算相对位姿。

发明内容

针对现有技术的不足，本申请实施例的目的是提供一种基于关键特征的水下机器人对接单目视觉导引方法。

根据本申请实施例的第一方面，提供一种基于关键特征的水下机器人对接单目视觉导引方法，包括：

获取水下图像并进行预处理；

将预处理后的水下图像输入近端视觉导引的深度学习模型PVNet中以检测坞站内部的目标物所占的图像像素总数值；

若所述总数值小于等于预定阈值，则当前阶段为远端视觉引导，将预处理后的水下图像输入训练好的关键点检测网络，得到坞站外部关键点距离坞站中心的位置偏差，并计算得到所述坞站外部关键点所在的像素2D位置；将所述坞站外部关键点的像素2D位置结合其先验世界3D坐标形成点对映射；根据所述点对映射，利用两步位姿估计法得到坞站中心相对于机器人的姿态与位置；

若所述总数值大于预定阈值，则当前阶段为近端视觉引导，将预处理后的水下图像输入训练好的关键特征检测网络，得到目标物的区域掩码图和单位矢量特征图，通过所述区域掩码图提取在所述单位矢量特征图中目标区域的单位矢量关键特征，其中所述目标区域为所述目标物在所述预处理后的水下图像中所占区域；根据目标区域的单位矢量关键特征，采用随机抽样一致性准则选取关键点的候选像素2D坐标和其内点集，采用矢量投影的的方式对所述内点集进行内点拟合得到优化的关键点的像素2D坐标；将优化的像素2D坐标和先验3D坐标形成点对映射；根据所述点对映射，利用两步位姿估计法中得到坞站内部目标物相对于机器人的位姿，并通过刚体变换转化为坞站中心相对于机器人的位姿；

根据坞站中心相对于机器人的位姿，规划水下机器人的路径并根据所述路径生成控制指令，以对所述水下机器人进行导引。

进一步地，所述关键点检测网络的训练过程包括：

获取第一训练集，其中所述第一训练集包括若干带有坞站外部关键点和中心点标识的图像以及每个图像对应的关键点位置真实值；

根据所述第一训练集，利用第一损失函数对所述关键点检测网络进行训练，其中所述第一损失函数Loss_far为

Loss_far＝λ_clsLoss_cls+λ_sizeLoss_size

其中Loss_cls指类别置信度损失，以真实坞站中心点的像素2D坐标为正样本Y_xyc，而

为所述关键点检测网络在(x，y，c)位置上预测得到的类别置信度值；Loss_size为关键点偏差损失；N为正样本总数；α,β为温度系数，用于调节训练困难难易度的比例；

为预测的位置偏差；

为真实位置偏差。

进一步地，所述第一训练集的获取过程包括：

采用了labelme软件对采集的水下图像中的坞站外部角点和中心点进行人工标识；

对标识后的水下图像以中心点为坞站的类别点化表示，并计算得到角点相对中心点的偏差，作为所述坞站的关键点位置真实值。

进一步地，将预处理后的水下图像输入训练好的关键点检测网络，得到坞站外部关键点距离坞站中心的位置偏差，包括：

将预处理后的水下图像输入训练好的关键点检测网络，得到类别置信图和关键点偏差图；

在类别置信图中选取出具有最大得分的点，即为坞站中心点所在像素2D坐标，并以此像素2D坐标在关键点偏差图中的相同位置处提取得到各关键点偏差，通过中心点坐标和关键点偏差计算得到各关键点的像素2D坐标。

进一步地，所述关键点检测网络的训练过程包括：

获取第二训练集，其中所述第二训练集为若干带有目标物相对于机器人的姿态和位置真实值的图像；

根据所述第二训练集，利用第二损失函数对所述关键点检测网络进行训练，其中所述第二损失函数Loss为

Loss＝Loss_vertex+Loss_mask

式中，Loss_vertex表示单位矢量图损失，p_i∈O表示只对目标区域内的值做损失计算；Loss_mask表示目标区域掩码图损失，采用交叉熵损失形式计算，其中

表示第i个点预测得到的类别属于目标和背景的概率，y_i表示该点的真实标签值；D为矢量图张量的维度总数；

为真实单位矢量图；

为预测单位矢量图；x_i为真实矢量图和预测矢量图在第i个像素上的差值。

进一步地，所述第二训练集的获取过程包括：

采用opencv aruco库构建用于位姿标定的aruco标定板，并进行实物制作；

将坞站内部目标物置于标定板上，并以不同的姿态和位置对带有目标物的标定板进行图像采集；

对于采集得到的每张图像，使用opencv aruco库检测标定板上的二维码，根据所述二维码计算得到标定板原点相对于相机的位姿结果，以旋转矩阵和平移向量表示；

采用标定得到的旋转矩阵和平移向量，结合已知的目标物点云模型构建目标区域掩码图mask；

对于采集图像标定得到的位姿集合，采样得到用于渲染的新旋转矩阵和新平移向量；

以其他公开数据集的图片为背景图片，使用blender软件和所述目标点云模型，根据所述新旋转矩阵和新平移向量渲染得到用于增强的数据集图片；

将带有真实位姿结果的原始采集数据集划分为第二训练集和测试集，并将渲染的增强数据集加入至所述第二训练集中用于网络训练。

进一步地，根据目标区域的单位矢量关键特征，采用随机抽样一致性准则选取关键点的候选像素2D坐标和其内点集，包括：

(1)从所述目标区域的单位矢量关键特征的点集中选取若干组两两对应的点，对于对应的点

根据其单位矢量关键特征

计算两个单位矢量关键特征所在的直线：

将所述两个单位矢量关键特征所在的直线的相交点作为第k个关键点的第i个假设点h_k,i，从而得到假设点点集B，其中假设点h_k,i根据下式得到

(2)对所有假设点采用矢量投票进行RANSAC内点数统计：

其中w_k,i为第k个关键点的第i个假设点的投票数；

为内积表达式；I为指示器函数，当满足函数括号内的不等式时，指示器函数输出为1，反之为0；ξ为阈值

(3)对当前第k个关键点的所有假设点内点数进行统计，选取具有最多内点数的假设点作为该关键点的候选像素2D坐标；

(4)重复步骤(1)-(3)，直到得到所有关键点的候选像素2D坐标。

进一步地，利用两步位姿估计法中得到坞站内部目标物相对于机器人的位姿，并通过刚体变换转化为坞站中心相对于机器人的位姿，包括：

(1)采用几何位姿估计法EPnP解算得到位姿迭代初值：

1)将世界坐标系下的关键点点集p^w采用PCA主成分分析方法得到世界坐标系下的控制点

和每个点的控制系数a_ij：

2)通过相机成像模型构建以下等式；

其中f_x,f_y,u₀,v₀为相机内参，u_i,v_i,(i＝1,...,N)为第i个优化的关键点的像素2D坐标，a_ij,j＝1,...,4表示第i个关键点的第j个控制系数；

将所述等式变化为矩阵Mc＝0形式，通过对M^TM特征值分解得到表达式

其中w_i为第i个特征向量，采用下述目标函数求得加权系数β_i，从而进一步求得控制点c：

建立以下残差公式：

E＝[e₁₂ e₁₃ e₁₄ e₂₃ e₂₄ e₃₄]^T,

其中

为第i个控制点的第k个特征向量，设

雅克比矩阵为J＝[J₁₂J₁₃ J₁₄ J₂₃ J₂₄ J₃₄]^T，则得到雅克比矩阵：

采用下式高斯牛顿迭代公式迭代更新获取β：

进一步得到相机坐标系下的控制点坐标

3)使用相机坐标系下的控制点系数和控制点坐标重构关键点相机坐标，以及世界坐标系下的控制点系数和控制点坐标重构关键点直接坐标，并进行以下公式的去中心化

其中

重构得到的关键点相机坐标，

为相机坐标系下关键点点集平均坐标；

为重构得到的关键点世界坐标，

为世界坐标系下关键点点集平均坐标；

4)对W＝(p^c)^Tp^w进行SVD分解得[U∑V]＝SVD(W)，则旋转矩阵初值和平移向量初值分别为R₀＝UV^T,

将旋转矩阵初值和平移向量初值作为位姿迭代初值；

(2)以所述位姿迭代初值为搜索起始点，采用基于最小重投误差的目标函数迭代优化位姿，得到得到坞站内部目标物相对于机器人的相对位姿：

根据基于最小重投误差的目标函数构建以下残差矩阵：

E＝[e₁,e₂,...,e_N]^T

其中基于最小重投误差的目标函数为

式中，K为相机内参矩阵

为第k个关键点在世界坐标系下的先验3D坐标，

为第k个关键点在像素坐标系下的像素2D坐标；

采用高斯牛顿方法求解所述目标函数，求得关于旋转R的雅克比矩阵：

其中表示vector()行向量化操作，则矩阵J_R大小为N×9，而残差矩阵E的大小为N×1，使用高斯牛顿迭代公式更新矩阵R行向量化后的向量R_v：

R_v＝R_v+ΔR_v

将向量R_v解行向量化后可得旋转矩阵R；

对于平移向量t，得其雅克比矩阵为：

矩阵J_t大小为N×3，使用高斯牛顿迭代公式更新向量t'：

t'＝t'+Δt'

采用上述高斯牛顿公式不断迭代，直到误差小于一定阈值或迭代次数达到一定上限，最终得到坞站内部目标物相对于机器人的相对位姿，所述相对位姿包括旋转矩阵R和平移向量t'。

(3)通过刚体变换转化为坞站中心相对于机器人的位姿

由以下刚体变换进一步转化为坞站中心相对于相机的位姿：

t＝t′+RΔt

其中t′表示坞站内部目标物相对于机器人的平移向量，t′表示坞站中心相对于机器人的平移向量，Δt则表示内部目标物与坞站中心的固定三轴偏差。

根据下式，将旋转矩阵R变换为三轴欧拉角角度向量e，用于表示坞站中心与机器人之间的姿态偏差：

其中，所述平移向量t和三轴欧拉角角度向量e用于规划水下机器人的路径并根据所述路径生成控制指令。

根据本申请实施例的第二方面，提供一种基于关键特征的水下机器人对接单目视觉导引装置，包括：

获取模块，用于获取水下图像并进行预处理；

检测模块，用于将预处理后的水下图像输入近端视觉导引的深度学习模型中以检测坞站内部的目标物所占的图像像素总数值；

第一位姿计算模块，用于若所述总数值小于预定阈值，则当前阶段为远端视觉引导，将预处理后的水下图像输入训练好的关键点检测网络，得到坞站外部关键点距离坞站中心的位置偏差，并计算得到所述坞站外部关键点所在的像素2D位置；将所述坞站外部关键点的像素2D位置结合其先验世界3D坐标形成点对映射；根据所述点对映射，利用两步位姿估计法得到坞站中心相对于机器人的姿态与位置；

第二位姿计算模块，用于若所述总数值大于预定阈值，则当前阶段为近端视觉引导，将预处理后的水下图像输入训练好的关键特征检测网络，得到目标物的区域掩码图和单位矢量特征图，通过所述区域掩码图提取在所述单位矢量特征图中目标区域的单位矢量关键特征，其中所述目标区域为所述目标物在所述预处理后的水下图像中所占区域；根据目标区域的单位矢量关键特征，采用随机抽样一致性准则选取关键点的候选像素2D坐标和其内点集，采用矢量投影的的方式对所述内点集进行内点拟合得到优化的关键点的像素2D坐标；将优化的像素2D坐标和先验3D坐标形成点对映射；根据所述点对映射，利用两步位姿估计法中得到坞站内部目标物相对于机器人的位姿，并通过刚体变换转化为坞站中心相对于机器人的位姿；

导引模块，用于根据坞站中心相对于机器人的位姿，规划水下机器人的路径并根据所述路径生成控制指令，以对所述水下机器人进行导引。

根据本申请实施例的第三方面，提供一种水下机器人，所述水下机器人通过如第一方面所述的基于关键特征的水下机器人对接单目视觉导引方法进行回收对接。

本申请的实施例提供的技术方案可以包括以下有益效果：

(1)本发明提出了一种基于关键特征的水下机器人对接单目视觉导引方法，相比现有的引导方法，本方法具有更高主动性、靠干扰性、稳定性和精准性；

(2)本发明中的远端视觉引导能够主动识别水下坞站外部机械结构的关键点位置，近端视觉引导能够主动识别坞站内部目标物关键特征并推断关键点位置，进一步获取坞站相对于机器人的位姿信息，相比于传统灯阵引导方法具有更高主动性，且不易受环境干扰，当相机超出视野时也能够从目标部分区域解算得到位姿结果，具有更高鲁棒性；

(3)本发明中的位姿解算部分提出一种两步位姿估计法，相比于普通PnP算法能够解算得到更高准确度的位姿结果。

应当理解的是，以上的一般描述和后文的细节描述仅是示例性和解释性的，并不能限制本申请。

附图说明

此处的附图被并入说明书中并构成本说明书的一部分，示出了符合本申请的实施例，并与说明书一起用于解释本申请的原理。

图1是根据一示例性实例示出的一种基于关键特征的水下机器人对接单目视觉导引方法的流程图。

图2是根据一示例性实例示出的一种数据集采集流程图；其中，(a)为远端视觉引导的关键点检测网络数据集采集流程，(b)为近端视觉引导的关键特征检测网络数据集采集流程。

图3是根据一示例性实例示出的一种近端视觉引导关键特征检测网络数据集相关方法；其中，(a)为数据集mask生成方法流程图，(b)为数据集增强方法流程图。

图4为是根据一示例性实例示出的一种球体采样法示意图；其中，(a)为球体示意图，(b)为球体采样过程示意图。

图5是根据一示例性实例示出的一种近端视觉引导内点拟合示意图。

图6是根据一示例性实例示出的一种基于关键特征的水下机器人对接单目视觉导引装置的框图。

具体实施方式

这里将详细地对示例性实施例进行说明，其示例表示在附图中。下面的描述涉及附图时，除非另有表示，不同附图中的相同数字表示相同或相似的要素。以下示例性实施例中所描述的实施方式并不代表与本申请相一致的所有实施方式。

在本申请使用的术语是仅仅出于描述特定实施例的目的，而非旨在限制本申请。在本申请和所附权利要求书中所使用的单数形式的“一种”、“所述”和“该”也旨在包括多数形式，除非上下文清楚地表示其他含义。还应当理解，本文中使用的术语“和/或”是指并包含一个或多个相关联的列出项目的任何或所有可能组合。

应当理解，尽管在本申请可能采用术语第一、第二、第三等来描述各种信息，但这些信息不应限于这些术语。这些术语仅用来将同一类型的信息彼此区分开。例如，在不脱离本申请范围的情况下，第一信息也可以被称为第二信息，类似地，第二信息也可以被称为第一信息。取决于语境，如在此所使用的词语“如果”可以被解释成为“在……时”或“当……时”或“响应于确定”。

需要说明的是，本申请中涉及的“相对于机器人的位姿”均为相对于机器人上相机的位姿。

图1是根据一示例性实施例示出的一种基于关键特征的水下机器人对接单目视觉导引方法的流程图，如图1所示，该方法可以包括以下步骤：

步骤S11：获取水下图像并进行预处理；

步骤S12：将预处理后的水下图像输入近端视觉导引的深度学习模型PVNet中以检测坞站内部的目标物所占的图像像素总数值；

步骤S13：若所述总数值小于等于预定阈值，则当前阶段为远端视觉引导，将预处理后的水下图像输入训练好的关键点检测网络，得到坞站外部关键点距离坞站中心的位置偏差，并计算得到所述坞站外部关键点所在的像素2D位置；将所述坞站外部关键点的像素2D位置结合其先验世界3D坐标形成点对映射；根据所述点对映射，利用两步位姿估计法得到坞站中心相对于机器人的姿态与位置；

步骤S14：若所述总数值大于预定阈值，则当前阶段为近端视觉引导，将预处理后的水下图像输入训练好的关键特征检测网络，得到目标物的区域掩码图和单位矢量特征图，通过所述区域掩码图提取在所述单位矢量特征图中目标区域的单位矢量关键特征，其中所述目标区域为所述目标物在所述预处理后的水下图像中所占区域；根据目标区域的单位矢量关键特征，采用随机抽样一致性准则选取关键点的候选像素2D坐标和其内点集，采用矢量投影的的方式对所述内点集进行内点拟合得到优化的关键点的像素2D坐标；将优化的像素2D坐标和先验3D坐标形成点对映射；根据所述点对映射，利用两步位姿估计法中得到坞站内部目标物相对于机器人的位姿，并通过刚体变换转化为坞站中心相对于机器人的位姿；

步骤S15：根据坞站中心相对于机器人的位姿，规划水下机器人的路径并根据所述路径生成控制指令，以对所述水下机器人进行导引。

由上述实施例可知，本申请提出了一种基于关键特征的水下机器人对接单目视觉导引方法，相比现有的引导方法，本方法具有更高主动性、靠干扰性、稳定性和精准性；本申请中的远端视觉引导能够主动识别水下坞站外部机械结构的关键点位置，近端视觉引导能够主动识别坞站内部目标物关键特征并推断关键点位置，进一步获取坞站相对于机器人的位姿信息，相比于传统灯阵引导方法具有更高主动性，且不易受环境干扰，当相机超出视野时也能够从目标部分区域解算得到位姿结果，具有更高鲁棒性；本申请中的位姿解算部分提出一种两步位姿估计法，相比于普通PnP算法能够解算得到更高准确度的位姿结果。

在步骤S11的具体实施中，获取水下图像并进行预处理；

具体地，首先机器人通过内置摄像头获取水下图像，对得到的水下图像进行图像预处理，主要包含了图像畸变矫正、图像去噪和基于暗通道先验的图像增强三部分。图像畸变矫正旨在降低图像因相机机械结构导致的图像失真度，图像畸变主要可分为径向畸变和切向畸变，其中径向畸变是沿着相机透镜半径方向分布的畸变，是由于光线在远离透镜中心的地方比靠近中心的地方更加弯曲导致的，可采用下述主点周围的泰勒级数展开式的前几项描述：

其中

表示畸变的原图上的像素位置，(u,v)表示校正后的像素位置；而切向畸变是由于透镜本身与相机传感器平面不平行所产生的，通常采用以下数学表达式进行描述：

在相机内参标定时采用大量标定图像对相机畸变系数进行标定，并在视觉引导时采用该畸变系数逆向求解得到畸变矫正的图像。

图像去噪旨在降低水下漂浮物的干扰，本方法视觉引导图像去噪采用中值滤波方法，对于图像中的每个像素，采用该像素邻域内的像素中值替换原像素值，假设图像中目标像素值在滤波前为I_pre(x,y)，滤波后为I_post(x,y)，通过计算邻域s的中值像素替代原目标像素，可由下式表示：

I_post(x,y)＝mid{I_pre(x+Δx,y+Δy),(Δx,Δy)∈s}

基于暗通道先验的图像增强旨在加强水下图像的清晰度，并突出目标物在图像中的结构特征。暗通道先验是指原始图像像素RGB通道中某一通道值会存在较低的现象。对于实际图像而言，求得暗通道先验图的过程是一个局部滤波的过程，即对于增强图像J上的某个像素x，定义J^dark(x)为该像素要求得的暗通道值，J^c(x)为该像素c通道的像素值，则暗通道可用下列表达式计算得到：

其中

表示求得该像素点RGB三通道中的最小值，y∈Ω(x)表示在像素点x的一定邻域窗口内搜索，则上式对像素x而言，采用该像素点周围一定窗口内所有像素中最小通道值作为该像素的暗通道值。以下述归一化加权模型定义水下图像：

I^c(x)＝J^c(x)t(x)+A^c(1-t(x))

其中I^c(x)表示输入原始图像像素点x的c通道值，J^c(x)表示要求得的增强图像像素点x的c通道值，A^c表示图像c通道上的水体光成分值，t(x)表示透光率函数。假设像素x一定邻域窗口内的透射率是常数，对上式两边取最小后可得：

根据以下暗通道定义：

带入可得透射率表达式，同时为了防止增强过于彻底导致恢复的图像景物不自然，引入权值w，得到以下最终透射率表达式：

带入归一化加权模型中，对透射率取下限，得到最终图像增强的表达式：

对于水体光成分A的获取，首先根据暗通道大小提取最大的前0.1％像素，后在输入的原始图像上寻找这些对应像素具有最高亮度的RGB通道值来作为A。以上即为基于暗通道先验的图像增强部分。

在步骤S12的具体实施中，将预处理后的水下图像输入近端视觉导引的深度学习模型PVNet中以检测坞站内部的目标物所占的图像像素总数值；

具体地，预定阈值大约为占图像总像素的1/64左右。这是一个超参数，在具体实施中根据实际情况中目标物大小和图像大小来调整。

在步骤S13的具体实施中，若所述总数值小于等于预定阈值，则当前阶段为远端视觉引导，将预处理后的水下图像输入训练好的关键点检测网络，得到坞站外部关键点距离坞站中心的位置偏差，并计算得到所述坞站外部关键点所在的像素2D位置；将所述坞站外部关键点的像素2D位置结合其先验世界3D坐标形成点对映射；根据所述点对映射，利用两步位姿估计法得到坞站中心相对于机器人的姿态与位置；

具体地，所述关键点检测网络的训练过程包括：

(1)获取第一训练集，其中所述第一训练集包括若干带有坞站外部关键点和中心点标识的图像以及每个图像对应的关键点位置真实值；这么设计是因为远端视觉引导的关键点检测网络和近端视觉引导的关键特征检测网络均为深度学习监督网络，作为监督形式的学习方式，需要大量带有真实标签的数据集训练网络来使其达到收敛。对上述两个网络分别进行数据集采集。

(2)根据所述第一训练集，利用第一损失函数对所述关键点检测网络进行训练，其中所述第一损失函数Loss_far为

Loss_far＝λ_clsLoss_cls+λ_sizeLoss_size

为所述关键点检测网络在(x，y，c)位置上预测得到的类别置信度值，对于正样本Y_xyc＝1，当预测值

较大时，损失值权值

较小，反之则较大，这样可以在训练时平衡难易样本的预测程度；Loss_size为关键点偏差损失，对关键点像素2D坐标相对于中心点坐标的偏差采用L1损失计算；N为正样本总数；α,β为温度系数，用于调节训练困难难易度的比例；

为预测的位置偏差；

为真实位置偏差；最终损失Loss采用类别置信度损失和关键点偏差损失的加权形式得到，并以该总损失训练网络。

其中，如图2中的(a)所示，所述第一训练集的获取过程包括：

采用了labelme软件对采集的水下图像中的坞站外部角点和中心点进行人工标识；对标识后的水下图像以中心点为坞站的类别点化表示，并计算得到角点相对中心点的偏差，作为所述坞站的关键点位置真实值。

考虑到在距离回收坞站较远时无法获取坞站内部目标物的相关信息，因此在远端视觉引导时本方法采用目标检测网络检测坞站外部机械结构的最小外接长方体角点作为关键点，并结合其先验世界3D坐标解算得到坞站相对于机器人的位置与姿态。

具体地，将预处理后的水下图像输入训练好的关键点检测网络，得到坞站外部关键点距离坞站中心的位置偏差，包括：

其中关于两部位姿估计法的部分在步骤S14中统一进行说明，此处不作赘述。

在步骤S14的具体实施中，若所述总数值大于预定阈值，则当前阶段为近端视觉引导，将预处理后的水下图像输入训练好的关键特征检测网络，得到目标物的区域掩码图和单位矢量特征图，通过所述区域掩码图提取在所述单位矢量特征图中目标区域的单位矢量关键特征，其中所述目标区域为所述目标物在所述预处理后的水下图像中所占区域；根据目标区域的单位矢量关键特征，采用随机抽样一致性准则选取关键点的候选像素2D坐标和其内点集，采用矢量投影的的方式对所述内点集进行内点拟合得到优化的关键点的像素2D坐标；将优化的像素2D坐标和先验3D坐标形成点对映射；根据所述点对映射，利用两步位姿估计法中得到坞站内部目标物相对于机器人的位姿，并通过刚体变换转化为坞站中心相对于机器人的位姿；

当机器人与坞站相对距离较近且坞站内部目标物占据一定的图像区域时，进入解算精度较高的近端视觉引导，近端视觉引导主要采用深度网络预测关键点相关的矢量关键特征，然后基于随机抽样一致性RANSAC和矢量投影推断关键点像素2D位置，最后采用与远端视觉引导相同的两步位姿估计法得到坞站内部目标物相对于机器人的位置与姿态，并通过刚体变换转化为坞站相对于机器人的位姿结果。近端视觉引导的流程如图6所示。

具体地，所述关键点检测网络的训练过程包括：

(1)获取第二训练集，其中所述第二训练集为若干带有目标物相对于相机姿态和位置真实值的图像；这么设计是因为远端视觉引导的关键点检测网络和近端视觉引导的关键特征检测网络均为深度学习监督网络，作为监督形式的学习方式，需要大量带有真实标签的数据集训练网络来使其达到收敛。

(2)根据所述第二训练集，利用第二损失函数对所述关键点检测网络进行训练，其中所述第二损失函数Loss为

Loss＝Loss_vertex+Loss_mask

式中，Loss_vertex表示单位矢量图损失，p_i∈O表示只对目标区域内的值做损失计算，对真实和预测的单位矢量图采用smooth L1损失计算，smooth L1损失在两者绝对误差大于1时为L1损失形式，能够提供稳定的下降梯度，而小于1时为L2损失形式，能够提供适应于误差变化的下降梯度；Loss_mask表示目标区域掩码图损失，采用交叉熵损失形式计算，其中

为真实单位矢量图；

为预测单位矢量图；x_i为真实矢量图和预测矢量图在第i个像素上的差值。最终损失Loss采用两者1:1加权形式，采用该损失训练网络直到收敛。在得到训练完毕的关键特征检测网络后，将经过图像预处理的水下图像输入到该网络中，得到单位矢量特征图和目标区域掩码图，并使用这两个结果提取目标区域的单位矢量特征集合，用于估计关键点像素2D位置。

其中，如图2中的(b)所示，所述第二训练集的获取过程包括：

采用opencv aruco库构建用于位姿标定的aruco标定板，并进行实物制作；将坞站内部目标物置于标定板上，并以不同的姿态和位置对带有目标物的标定板进行图像采集；对于采集得到的每张图像，使用opencv aruco库检测标定板上的二维码，根据所述二维码计算得到标定板原点相对于相机的位姿结果，以旋转矩阵和平移向量表示；采用标定得到的旋转矩阵和平移向量，结合已知的目标物点云模型构建目标区域掩码图mask；对于采集图像标定得到的位姿集合，采样得到用于渲染的新旋转矩阵和新平移向量；以其他公开数据集的图片为背景图片，使用blender软件和所述目标点云模型，根据所述新旋转矩阵和新平移向量渲染得到用于增强的数据集图片；将带有真实位姿结果的原始采集数据集划分为第二训练集和测试集，并将渲染的增强数据集加入至所述第二训练集中用于网络训练。

目标区域掩码图mask构建流程如图3中的(a)所示：首先采用标定得到的旋转矩阵和平移向量对标准帧下的点云模型进行变换，并进行插值处理使点云模型中的点更加密集；对上述所有的点根据相机成像模型投影至像素坐标系中，并在黑色背景图片下以这些点为中心构建适当半径的白色圆点；将上述构建完的图片进行二值化处理，后使用图形学开闭处理使目标区域形成一个闭合的整体，得到目标区域掩码图mask。

近端视觉引导关键特征网络数据集采集中的新旋转矩阵和新平移向量采样流程如图3中的(b)所示：首先将标定得到的位姿结果形成位姿集合，其中旋转矩阵变化为欧拉角向量；对该位姿集合中的平移向量采用以下表达式进行高斯核密度估计：

其中K₀()即为核密度函数，h为带宽，根据上式求得平移向量的概率密度函数f(x)后，采用该概率密度函数进行重采样得到用于渲染的新平移向量。

该位姿集合中的欧拉角姿态向量为三轴角度偏差，分别为俯仰角角度偏差、航向角角度偏差和横滚角角度偏差，由于水下机器人实际航行时俯仰角和航向角的变化较大，因此对于这两个角度采用球体采样法获取较大范围的采样值，其中球形采样法通过在半径为1的球体上进行采样并转化为相对于球心的相对角度得到欧拉角向量，采样点的坐标可由下式表示：

上述表达式将球体自下而上的z轴分为N段，每一段会与球体平面相交得到一个XoY平面，如图4中的(a)所示，使得球体的每条纬线上只存在一个采样点，而根据上式的x_n与y_n可知，上下两个相邻的XoY平面上的采样点之间在经度方向上相差φ圈，通过设置

为黄金比例值周围的小数0.618来使采样点呈现均匀且混乱的分布形式。采样过程如图4中的(b)虚线路线所示。对于横滚角，采用高斯核密度估计对原始集合进行概率密度函数估计，并进行重采样得到采样值，最终构建得到新欧拉角向量，并转化为旋转矩阵。

最后将得到的新旋转矩阵和新平移向量，结合目标点云模型和其他数据集背景图片，使用blender得到渲染图片，届时得到带有位姿结果增强数据集。在获取上述带有真实位姿结果和真实目标区域掩码图的训练集后，对目标点云模型使用图4所示的FPS最远点采样算法得到目标关键点先验世界3D坐标，在网络训练的预处理时使用相机模型和真实位姿将关键点重投至像素坐标系中，并利用该像素2D坐标和真实掩码图构建目标区域像素指向关键点2D坐标的单位矢量图，将掩码图和单位矢量图作为真实标签用于网络训练。

具体地，根据目标区域的单位矢量关键特征，采用随机抽样一致性准则选取关键点的候选像素2D坐标和其内点集，包括：

(1)从所述目标区域的单位矢量关键特征的点集A中选取若干组两两对应的点，对于对应的点

根据其单位矢量关键特征

计算两个单位矢量关键特征所在的直线：

(2)对于每个假设点，如果该点更加接近于真实关键点位置，则目标区域其他像素的指向真实关键点的单位矢量

和指向假设点的单位矢量

的夹角应该较小，而单位矢量的夹角可由矢量内积的形式间接表示，因此为方便计算，当两个单位矢量内积

大于某阈值时，视该像素为当前假设点的一个内点，对所有假设点采用矢量投票进行RANSAC内点数统计：

其中w_k,i为第k个关键点的第i个假设点的投票数；

(4)重复步骤(1)-(3)，直到得到所有关键点的候选像素2D坐标。

在上述获取关键点的初步位置后，采用一种基于矢量投影的内点拟合方法优化其像素2D位置，内定拟合示意图如图5所示，其中真实关键点为H，P为该关键点在上述RANSAC方法中的内点之一，可得向量

其中O为图像原点。由于向量

的单位向量

可以由上述网络预测的矢量图得到，假设该单位向量是百分百准确的，在已知向量

的情况下，可以得到垂直于向量

的单位向量

则对于真实关键点H来说向量

在单位向量n上的投影PA,HB的长度相等，即

但网络预测的单位矢量存在一定偏差，则可以通过多个类似点P的内点根据上述几何关系进行拟合得到较为精准的关键点位置。假设第i个内点为p_i＝[p_ix,p_iy]^T，且根据预测的矢量图得到该内点指向关键点的单位向量n_i＝[d_iy,-d_ix]^T，由于n_i为单位向量，因此内点位置向量在其上的投影PA可用

内积形式表示，要求得的最终关键点位置为s，则投影HB可表示为s^Tn_i，对于最终关键点的位置拟合即变成了以下目标函数的求解：

上述表达式目标为最小化所有内点矢量投影距离的和，考虑上述目标函数为典型的最小二乘形式，则对其转化后最优关键点位置要满足以下表达式：

将上述等式左右转置得

并使用b表示

得到：

则可以得到最优关键点拟合的位置为：

将所有m个内点合并表示为矩阵形式A＝[n₁...n_m]^T，上式可转换为所有内点的拟合形式：

s＝(A^TA)^-1A^Tb

最终s即为基于矢量投影内点拟合得到的优化关键点位置，对所有初步关键点使用上述优化方法得到用于位姿解算的关键点像素2D坐标点集。

具体地，所述两步位姿估计法指首先采用EPnP获取位姿迭代初值，再基于最小重投距离迭代优化位姿，具体包括：

(1)采用几何位姿估计法EPnP解算得到位姿迭代初值：

和每个点的控制系数a_ij：

2)通过相机成像模型构建以下等式；

其中f_x,f_y,u₀,v₀为相机内参，u_i,v_i,(i＝1,...,N)为第i个坞站外部关键点所在的像素2D位置或第i个优化的关键点的像素2D坐标，a_ij,j＝1,...,4表示第i个关键点的第j个控制系数；

建立以下残差公式：

E＝[e₁₂ e₁₃ e₁₄ e₂₃ e₂₄ e₃₄]^T,

其中

为第i个控制点的第k个特征向量，设

雅克比矩阵为J＝[J₁₂ J₁₃ J₁₄ J₂₃ J₂₄ J₃₄]^T，则得到雅克比矩阵：

采用下式高斯牛顿迭代公式迭代更新获取β：

进一步得到相机坐标系下的控制点坐标

其中

重构得到的关键点相机坐标，

为相机坐标系下关键点点集平均坐标；

为重构得到的关键点世界坐标，

为世界坐标系下关键点点集平均坐标；

将旋转矩阵初值和平移向量初值作为位姿迭代初值；

根据基于最小重投误差的目标函数构建以下残差矩阵：

E＝[e₁,e₂,...,e_N]^T

其中基于最小重投误差的目标函数为

式中，K为相机内参矩阵

为第k个关键点在世界坐标系下的先验3D坐标，

为第k个关键点在像素坐标系下的像素2D坐标；

R_v＝R_v+ΔR_v

将向量R_v解行向量化后可得旋转矩阵R；

对于平移向量t，得其雅克比矩阵为：

矩阵J_t大小为N×3，使用高斯牛顿迭代公式更新向量t'：

t'＝t'+Δt'

采用上述高斯牛顿公式不断迭代，直到误差小于一定阈值或迭代次数达到一定上限，最终得到坞站内部目标物或坞站中心相对于机器人的相对位姿，所述相对位姿包括旋转矩阵R和平移向量t'。

对于近端视觉导引，还需要通过刚体变换转化为坞站中心相对于机器人的位姿：

由以下刚体变换进一步转化为坞站中心相对于相机的位姿：

t＝t′+RΔt

根据平移向量t和三轴欧拉角角度向量e，规划水下机器人的路径并根据所述路径生成控制指令。

在步骤S15的具体实施中，根据坞站中心相对于机器人的位姿，规划水下机器人的路径并根据所述路径生成控制指令，以对所述水下机器人进行导引。

具体地，导航模块根据坞站中心相对于机器人的位姿和自身当前所在的世界坐标计算坞站的世界坐标，规划模块根据坞站的世界坐标和机器人的世界坐标进行轨迹的计算，并根据轨迹上的点来得到当前机器人需要运动的方向和大小，控制模块则根据该方向和大小来控制机器人运动装置，从而进行导引。

与前述的基于关键特征的水下机器人对接单目视觉导引方法的实施例相对应，本申请还提供了基于关键特征的水下机器人对接单目视觉导引装置的实施例。

图6是根据一示例性实施例示出的一种基于关键特征的水下机器人对接单目视觉导引装置框图。参照图6，该装置可以包括：

获取模块21，用于获取水下图像并进行预处理；

检测模块22，用于将预处理后的水下图像输入近端视觉导引的深度学习模型中以检测坞站内部的目标物所占的图像像素总数值；

第一位姿计算模块23，用于若所述总数值小于预定阈值，则当前阶段为远端视觉引导，将预处理后的水下图像输入训练好的关键点检测网络，得到坞站外部关键点距离坞站中心的位置偏差，并计算得到所述坞站外部关键点所在的像素2D位置；将所述坞站外部关键点的像素2D位置结合其先验世界3D坐标形成点对映射；根据所述点对映射，利用两步位姿估计法得到坞站中心相对于机器人的姿态与位置；

第二位姿计算模块24，用于若所述总数值大于预定阈值，则当前阶段为近端视觉引导，将预处理后的水下图像输入训练好的关键特征检测网络，得到目标物的区域掩码图和单位矢量特征图，通过所述区域掩码图提取在所述单位矢量特征图中目标区域的单位矢量关键特征，其中所述目标区域为所述目标物在所述预处理后的水下图像中所占区域；根据目标区域的单位矢量关键特征，采用随机抽样一致性准则选取关键点的候选像素2D坐标和其内点集，采用矢量投影的的方式对所述内点集进行内点拟合得到优化的关键点的像素2D坐标；将优化的像素2D坐标和先验3D坐标形成点对映射；根据所述点对映射，利用两步位姿估计法中得到坞站内部目标物相对于机器人的位姿，并通过刚体变换转化为坞站中心相对于机器人的位姿；

导引模块25，用于根据坞站中心相对于机器人的位姿，规划水下机器人的路径并根据所述路径生成控制指令，以对所述水下机器人进行导引。

关于上述实施例中的装置，其中各个模块执行操作的具体方式已经在有关该方法的实施例中进行了详细描述，此处将不做详细阐述说明。

对于装置实施例而言，由于其基本对应于方法实施例，所以相关之处参见方法实施例的部分说明即可。以上所描述的装置实施例仅仅是示意性的，其中所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部模块来实现本申请方案的目的。本领域普通技术人员在不付出创造性劳动的情况下，即可以理解并实施。

相应地，本申请还提供一种电子设备，包括：一个或多个处理器；存储器，用于存储一个或多个程序；当所述一个或多个程序被所述一个或多个处理器执行，使得所述一个或多个处理器实现如上述的基于关键特征的水下机器人对接单目视觉导引方法。

相应地，本申请还提供一种计算机可读存储介质，其上存储有计算机指令，其特征在于，该指令被处理器执行时实现如上述的基于关键特征的水下机器人对接单目视觉导引方法。

本申请实施例还提供一种水下机器人，所述水下机器人通过上述的基于关键特征的水下机器人对接单目视觉导引方法进行回收对接。

本领域技术人员在考虑说明书及实践这里公开的内容后，将容易想到本申请的其它实施方案。本申请旨在涵盖本申请的任何变型、用途或者适应性变化，这些变型、用途或者适应性变化遵循本申请的一般性原理并包括本申请未公开的本技术领域中的公知常识或惯用技术手段。

应当理解的是，本申请并不局限于上面已经描述并在附图中示出的精确结构，并且可以在不脱离其范围进行各种修改和改变。