基于单目视觉的水下目标检测与位姿估计

贾 威，迟书凯，葛宪威

（中国海洋大学工程学院，山东 青岛266100）

应用于水下场景的视觉系统主要有单目视觉系统和双目视觉系统[1]。与双目视觉系统相比，单目视觉结构简单，易于配置安装，因此更适合水下的应用需求[2]。但是由于成像原理的限制，单目视觉在原理上无法直接获取深度信息，但结合一定的先验信息可以实现单目视觉的深度估计。

在基于传统视觉方法的单目视觉研究中，张法全等[3-4]对图像进行模糊度监测获取边缘宽度然后通过样条插值算法求取深度信息，但该方法只适用于较近距离下的深度信息求解。王剑等[5]通过改变相机焦距获取多张图像，在固定尺寸和焦距两种情况下成像，利用目标物体尺寸和焦距之间的关系，定量求取深度信息，但该方法要求对相机进行在线标定，不满足实时性的要求。

基于深度学习的单目深度估计方法，根据训练方法的不同可以分为：有监督方法、无监督方法和半监督方法[6]。在有监督的单目深度估计如文献[7—8]的方法中，方法使用的监督信号基于深度图的地面真值，这在一些应用场景下的获取代价是相当昂贵的。在水下自主作业的应用场景中，很难获得具有准确监督信号的水下深度图像作为网络训练使用的数据集。在无监督单目深度估计中，如文献[9—10]的方法中，一般使用帧间的几何约束作为训练过程的监督信号。无监督方法在网络训练中没有使用真值，所以性能和有监督方法的差距很大，而且无监督方法存在着尺度模糊、不一致的问题[6]。

综上所述，无论是传统方法还是基于深度学习的方法，都存在一定的问题。基于深度学习的单目估计方法，不管是有监督方法或是无监督方法在水下的应用场景中都无法出色地完成任务，但卷积神经网络在目标识别与跟踪领域仍有着非常出色的表现。在水下的应用场景中，拍摄得到的图像往往存在清晰度差，边缘锐度低，整体亮度低，目标颜色强度会随着水深逐渐减弱，轮廓信息也会受到一定影响的问题[11]，这就使得传统的目标识别方法会出现误识别，甚至无法识别的情况。深度学习网络方法可以通过多次卷积计算来对图像特征进行提取，能够充分利用图像的像素信息，提高检测性能，同时满足实时性和准确性的要求。在面向水下环境的目标检测中，最新的依赖卷积神经网络的检测器可以分为两阶段检测器如R-CNN、Faster R-CNN 和单阶段检测器如YOLO（You Only Look Once）、SSD（Single Shot Multibox Detector） 两类。两阶段的检测器具有更高的准确性，但在水下应用场景中不能满足实时性和计算资源限制的需求[12]。现有的针对水下环境的单阶段检测器改进主要针对水下小目标的检测性能，以及克服水下成像质量的影响，但算法的实时性较差，同时网络结构复杂，离线移植实现的难度较大。YOLOv3 网络结构相对简单，复杂程度低，便于水下的离线实现，同时YOLO 单阶段检测器更为快速，且对小物体的检测性能更好。

因此，本文基于改进的YOLOv3 算法实现水下目标的检测，同时在神经网络获取到的像素信息的基础上构建单应性模型，结合基于PnP（Perspectiven-Point）问题的改进算法对目标进行实时的位姿估计，算法实时性满足应用需求并且网络结构有利于离线移植实现，可以在水下应用场景中实时获取位置信息。

1 目标检测

本文面向水下自主作业场景，提出一种目标检测与实时位姿估计方法，在目标物体几何规则并结合一定先验信息的基础上可以实现单目视觉下的目标检测与实时位姿估计。在这里以广泛应用于水下装置设备的水密接插件和工件为例验证本文方法可行性。为了实现快速且鲁棒地对目标进行检测，本文在YOLOv3 算法[13]的基础上加以改进，使得算法在水下场景中具备更好的检测效果和更低的误差。基于YOLOv3 网络的检测方法将候选框提取、提取特征、目标分类与目标跟踪统一于一个神经网络中，网络可以从图像中提取候选区域，然后通过图像中的特征来预测目标在图像中的位置和置信度。

基础的YOLOv3 算法对物体的检测无法解决边界框跟随物体旋转的问题，也就是说尽管本文方法的应用场景中目标物体几何形状规则，但使用传统的YOLOv3 网络无法实现边界框与物体相切，且能随着物体姿态的变化保持很好的相切于物体。在水下自主作业的场景中，单目视觉系统需获取物体的深度信息和姿态信息，故对传统的YOLOv3 算法进行改进，在解决跟踪效果的基础上尽可能保证提供较为准确的姿态及深度信息。

1.1 YOLO 网络结构设计

YOLOv3 网络对图像特征的提取主要通过卷积和池化操作来实现，在本文水下自主作业的应用场景中，考虑到水下拍摄画面的雾化模糊问题，以及完成目标检测之后进行位姿估计时对局部特征的需求问题，为了增强YOLOv3 网络在本文应用场景中的适用性，给位姿估计提供精度更高的像素信息，本文采用重组层代替卷积和池化操作来对图像进行特征提取。重组层的结构如图1 所示，重组层将每个通道上的2 ×2 图像块中的4 个像素点进行拆解，并且重新排列成4 通道的1×1 图像块，这样操作可以更好地保留像素中的局部细节，有利于为后续的位姿估计流程提供像素信息。改进后的网络结构如图2 所示。

图1 重组层结构

图2 改进的YOLOv3 网络结构

1.2 损失函数改进

在本文方法的水下应用场景中，目标在水中的姿态是随机的，也就是说物体可能在水中发生旋转，这会导致同一类物体由于拍摄角度的不同出现多种情况的长宽比，而原始YOLOv3 网络的损失函数是长宽的损失，为了提高网络的泛化能力，识别各种姿态下的目标物体，改进模型的损失函数，转化为区域框对角线的损失，损失函数具体如下所示。

在式（1）中，Ci表示目标分类；Pi表示属于某类别的概率，对于存在目标的待检测区域，该区域置信度为1；目标不存在的区域则置信度为0。训练时选用λcoord=λnoobj=0.5 的权重。

1.3 网络训练

制作网络训练使用的数据集，选取在水下环境中拍摄得到的不同姿态不同距离的水密接插件及工件图片制作数据集，使用LabelImg2 工具进行标注。数据集图像总共2 000 张，其中1 200 张作为训练集，600 张作为测试集，200 张作为验证集。

对YOLO 网络的训练过程需要在开始时对预测框（Anchor box）的数量及大小进行设定，由于网络训练过程实质上就是在初始预测框的基础上不断对预测框进行调整，使其与真实框接近的过程，所以设定合适的初始预测框不仅能加快网络的训练过程，而且能提高算法识别的准确率，本文针对水下应用场景的需求，以及目标物体几何形状的特点，将识别目标设定为水密接插件的4 个角点，同时引入K-Means 聚类算法，生成角点目标的最优初始预测框，以实现优化网络性能的目的。

K-Means 算法是一种较为常见的聚类算法[14]，把一个样本集作为算法的输入，算法对样本进行聚类，将相似特征的一类样本聚为一类。在本文的训练模型中共需要9 个先验框，因此在聚类算法中令K=9，聚类得到9 个类簇。聚类在无监督下进行，首先选取9 个中心点，遍历每个样本点进行分配，寻找与每个点距离最近的中心点，距离同一中心点最近的点为一个类簇，这样便完成了一次聚类，然后遍历所有类簇，计算新的质心，直到类簇质心不发生变化或迭代次数达到最大，则聚类完成。其中聚类的目标函数如下。

式中，Box[i]表示聚类结果；Box[j]表示物体框真实值；IOU表示两者间的交并比。

2 位姿估计

通过改进的YOLOv3 目标检测算法实现对角点的检测后，YOLOv3 网络可以输出4 个预测框（Bounding Box）中心点的像素坐标及预测框的长宽数据，至此可以获取目标物上四点在像素坐标系下的2D坐标，同时结合目标物先验信息，可以建立世界坐标系，获取四点在真实世界坐标系下的3D 坐标。

至此，如果在相机不发生移动也就是相机坐标系固定的应用场景中，对目标的位姿估计问题则可以转化为简单的已知世界坐标系下N个空间点的真实坐标及这些空间点在图像中的投影，来计算相机位姿，进而计算物体与相机间的姿态变化矩阵，得到物体位姿的过程。但是在本文的应用场景中，相机在实际水下应用场景下提供视觉信息的过程中可能需要发生移动即相机坐标系会发生改变。

因此，本文将对目标物体的位姿估计问题转化为已知世界坐标系下N个空间点的真实坐标及这些空间点在图像中的投影，来计算相机位姿，进而计算物体与相机间的姿态变化矩阵，得到物体位姿的过程。同时考虑到本文应用场景中相机位姿会随着跟踪过程发生变化，建立物体前后姿态间的单应性模型[15]，结合物体与相机间的姿态变化矩阵，对各种可能情形下的目标位姿进行估计。

2.1 单应性模型

根据计算机视觉领域的单应性概念，水密接插件和相机之间可以构建单应性矩阵来确定目标物体的深度与姿态。对不同姿态下的水密接插件与相机进行单应性模型构建，模型原理如图3 所示，将水密接插件高垂直于水平面时看作零初始状态，此时在物体上建立世界坐标系O0-X0Y0Z0，以圆柱体的高为X轴，以圆柱体横截面矩形的宽为Y轴，以垂直于横截面矩形正方向向外为Z轴。相机坐标系建立为OC-XCYCZC，则零初始状态下的世界坐标系O0-X0Y0Z0与相机坐标系OC-XCYCZC间的关系为（R1，T1），其中，R、T 分别表示两个坐标系间的旋转和平移矩阵。当目标物姿态发生变化，离开零初始状态时，在姿态改变后的物体上仍按照零初始状态下坐标系建立的方法，以圆柱体的高为X轴，以圆柱体横截面矩形的宽为Y轴，以垂直于横截面矩形正方向向外为Z轴建立新的世界坐标系O′-X′Y′Z′，该世界坐标系与相机坐标系OC-XCYCZC之间的关系可以用（R2，T2）来表示。由刚体的平移旋转定理可知，物体两种不同姿态间的转换关系可以用一个单应性矩阵H 描述，且H 应当满足式（3）。

图3 单应性模型的原理图

式中，[x0y0z0]T表示坐标系O0-X0Y0Z0中点的坐标；[x′y′z′]T为同一个点在姿态变化后建立的新世界坐标系O′-X′Y′Z′中的坐标。

在本文方法的验证中，水密接插件的先验信息是固定且已知的，则由式（3）可知，物体当前姿态与零初始状态下姿态间的转换关系可以由单应性矩阵给出，而单应性矩阵可以由同一点在两个坐标系下对应的不同坐标确定。设实际物体上一点在两种姿态下在成像平面上得到的两个投影点分别为U1=[u1v11]T和此时两投影点间关系可表示如下。

理论上通过数据归一化的直接线性变换（DLT）算法，可以求式（5）中的8 个未知量，从而求得单应性矩阵H。但在本文应用场景中，水下自主作业无法携带大量计算资源，DLT 算法计算量较大，所以对位姿的实时估计难以实现。

2.2 PnP 问题

至此，位姿估计问题可以近似为一个PnP问题[16]，PnP 问题的一般内容如下。

（1）给定n个3D 参考点到摄像机图像上2D投影点的匹配点对。

（2）已知3D 点在世界坐标系下的坐标，2D 点在图像坐标系下的坐标。

（3）已知摄像机的内参数矩阵。来求世界坐标系与摄像机坐标系之间的位姿变换矩阵[R|t]。

PnP 问题的一般示意图如图4 所示。

图4 PnP 问题示意图

2.3 PnP 位姿估计模型

在已知先验信息的基础上，结合目标检测算法提供的图像位置信息，建立PnP 位姿估计模型，在对匹配点的基础上，先使用P3P 算法获得模型下的初始转换矩阵[R|T]，然后构建最小二乘问题，使用LM（Levenberg-Marquardt Method） 算法继续对转换矩阵进行迭代求解，实现降低误差的目的。整个位姿估计算法流程如图5 所示。

图5 整体位姿估计算法流程

首先结合物体与相机几何信息构建单应性模型，结合已知先验信息与PnP 原理[16]，可以建立PnP 位姿估计模型，本文将物体的投影按四边形处理，得到的四边形空间角点坐标系模型如图6所示。

图6 四边形空间角点坐标系模型

基于PnP 问题的求解理论，为了同时保证求解的精度与算法的计算速度，实现较为精准的实时位姿估计，本文首先选用P3P 算法使用四对点中的三对点进行计算得到，在图6 中，分别连接A′、C′与a′、c′，A′、B′、C′、D′为水密接插件的4 个角点，a′、b′、c′、d′为角点在相机成像平面上的投影，o′为相机的光心。首先，显然图中的三角形间存在对应关系：Δo′a′b′-Δo′A′B′，Δo′b′c′-Δo′B′C′，Δo′a′c′-Δo′A′C′。对于三角形Δo′a′b和Δo′A′B′，由余弦定理可得式（6）。

对图中另外两组对应三角形，同理可得式（7）。

对于式（7），等式两边同时除以o′C′2，然后记x=o′A′/o′C′，y=o′B′/o′C′，记v=o′B′2/o′C′2，uv=B′C′2/o′C′2，wv=A′C′2/o′C′2，则可得式（8）。

至此，在式（8）中，从图像中点的位置可以得到3 个余弦角cos〈a′，b′〉、cos〈b′，c′〉、cos〈a′，c′〉，同时，u=B′C′2/A′B′2、w=A′C′2/A′B′2可以通过真实世界坐标系结合先验信息计算得到，所以未知量只有x、y，式（8）转化成关于x、y的一个二元二次方程，将3 个点带入，得到4 个解，然后将第4 点带入，得到一个最优解。

得到P3P 算法的最优解后，构建最小二乘问题，对位姿矩阵进行迭代更新，降低误差。在PnP问题的求解中，得到的位姿主要误差产生于相机的重投影误差。将变换矩阵的李代数表示为ξ。假设空间中实际点的世界坐标为Pi=[Xi，Yi，Zi]i，i=1，2，…，M，实际点所对应投影的像素坐标为μi=[ui，vi]，根据三维变换和李代数，有

将前文得到的相机初始位姿作为exp（ξ^）的初值，将等式两边的误差求和，构建最小二乘问题，迭代求解使误差最小化，误差函数构建如下。

然后对每个误差项关于优化变量的导数进行求解，即线性化：e（x+Δx）≈e（x）+J Δx（13），然后将雅可比矩阵J 代入，利用链式法则进行求导。

LM 算法是基于信赖区域理论进行求解非线性最小二乘的算法[17]，基于上述计算，使用LM 算法进行迭代运算，每次迭代的增量公式如下。

然后将Δx带入增益比例p公式。

每次迭代后计算一次p，直到p大于阈值则迭代结束，判断是否收敛，得到最终的优化结果。

3 实验与分析

为了验证本文整体技术路线的可行性及有效性，同时分别对基于YOLOv3 的结构改进目标检测算法和PnP 位姿估计算法的效果进行验证，设计系列相应的实验进行验证分析，实验部分算法运行的计算机平台处理器为i5-9300H，GPU 为GTX1660Ti。

实验主要分为3 部分：①测试本文目标检测网络对水密接插件进行检测的性能，验证网络结构相关改进的有效性；②验证本文位姿估计算法的可行性，评估本文迭代方法的误差优化效果；③进行综合实验，验证本文技术路线及问题解决方法的可行性，评估位姿估计精度及实际适用范围。

3.1 目标检测与跟踪效果分析

首先进行目标检测与跟踪实验，验证本文目标检测算法的性能，同时用改进网络结构的YOLOv3算法与原始算法效果进行对比，验证对网络结构进行改进的有效性和效果。

在水箱中加入碎纸屑模拟海洋环境中的杂质，遮挡光源模拟海洋环境中的弱光情况，在搭建的水下环境中分别拍摄3 种水下接插件及工件不同姿态的图像共1 500 幅制作实验验证集，在本文制作的验证集中分别使用3 种YOLOv3 网络进行检测试验，分析检测效果与检测框与物体切合程度。图7为3 种网络结构检测结果对比。

图7 3 种网络结构检测结果对比

从图像对比可以看出，对YOLOv3 网络进行重新聚类之后，由对目标整体进行检测改变为对角点进行检测，同时对YOLOv3 网络结构进行改进，使得局部特征细节得以保留，在角点像素坐标方面和检验框与物体的切合度方面都有了良好的提升。定义角点输出坐标实验中的误差为目标物角点真实位置坐标与估计得到对应角点位置坐标的偏差，分别使用绝对误差和相对误差对这个偏差进行评估。真实坐标与估计坐标间的欧式距离定义为绝对误差，使用绝对误差与对应位置距坐标系原点欧氏距离的比值作为相对误差。表2 是目标角点真实像素坐标与3 种网络结构输出的物体角点坐标对比，由表2 可得，改进后的YOLOv3 网络大大降低了输出的角点像素坐标误差，与每点真实像素坐标间的相对误差平均在2%左右，基本上可以忽略不计，相较于前两种网络像素坐标精度提高了80%以上，为后续位姿估计提供了更为可靠的数据，效果明显。同时使用置信度0.5 进行筛选后的结果对算法平均准确度（Mean Average Precision，mAp）指标进行性能评估，使用YOLOv5x 及Faster R-CNN 网络在本文验证集中进行测试，表3 为改进YOLOv3 方法与其他方法在验证集图像检测中的对比数据及网络结构的相关性能对比，可见，在验证集中本文改进YOLOv3 算法在平均准确度及处理速度上均有提升，且网络性能相较于目前较新的YOLOv5 和Faster R-CNN 目标检测网络相当，且处理速度较快，同时改进后网络结构更加简单有利于水下的离线移植实现，能够满足水下自主作业场景中实时跟踪检测任务的需要。

表2 角点像素坐标

表3 海南岛水边线提取结果对比

考虑水下的应用场景，在实际的海洋应用场景中噪声的影响不可忽视，尽管实验环境中对海洋环境进行了模拟，但与实际海洋环境仍有一定差距，因此对拍摄得到的3 种不同工件图像添加不同信噪比的高斯白噪声后进行检测并输出角点坐标，验证本文算法应对噪声干扰的鲁棒性，实验结果如图8所示，在不同噪声情况下网络输出的角点坐标与误差如表4 所示。

由图8 和表4 可得，本文目标检测网络在应对噪声干扰的鲁棒性较好，在表4 中，随着噪声的添加，同一姿态下同一物体的角点输出坐标相对误差和绝对误差均有所增加，但误差均保持在5%以内，且与未添加噪声时的估计结果相比，像素误差增大值保持在3 个像素以内，由图8 的检测效果可以看出本文检测网络在面对噪声干扰时对目标的检测具有一定的鲁棒性。同样的在相同噪声条件下网络对红色接插件的检测效果最好，输出的角点坐标误差最低，对黑色工件检测输出的角点坐标误差较高，这是受到水下成像环境的影响，红色在水下环境中的特征更加明显，所以会有较好的检测效果，但误差相对最高的黑色工件输出的角点误差也保持在5%左右，在上述实验中，最高为5.13%，满足后续位姿估计的需求。

表4 不同噪声情况下角点坐标误差

图8 添加噪声情况下检测效果

3.2 PNP 位姿估计模型的验证及分析

在对位姿估计模型的验证分析中，首先进行静态图像的实验验证，第一步进行物体深度估计实验，使用相机在水箱中拍摄得到的目标图像进行验证，拍摄的目标物体处于前文定义的零初始状态，相机拍摄时成像平面垂直于地面。分别将物体放置在100 mm、150 mm、200 mm、250 mm、300 mm、350 mm、400 mm、450 mm 和500 mm 处，每种距离下拍摄10 幅静态图像，最终在每种距离情况下取10 次深度估计的均值与真实值进行比较，分别使用本文算法和双目匹配算法进行深度估计，最终的估计结果如表5 所示。在同一计算平台下算法的运行时间情况如表6 所示。

表5 深度估计结果

表6 算法实时性对比

从表5 可以发现，在实验过程中，当目标处于100~500 mm 范围内时，误差控制在1%~4.5%，随着相机距离目标过近（100~200 mm）或者距离目标过远（400~500 mm）时，误差会变大，但是也在满足需求的范围内，造成这个现象的原因主要是相机成像质量的下降。可以通过提高相机像素来改善这种情况。

表6统计了两种算法在同一计算平台下对共90 幅图像的深度估计使用的总时间，由表6 可以看出，本文提出算法对一幅图像进行估计的时间大约在95 ms，相较于双目匹配算法使用的平均185 ms的时间，算法实时性大幅提升，同时结合表5 中深度估计结果对比，本文算法在近距离下（100~500 mm）的估计结果优于双目匹配算法，且能够实现对目标的姿态估计。

然后进行静态图像下的姿态估计实验，在本文的应用场景中，由于目标物体是圆柱形物体，所以用于描述物体姿态只需要两个旋转角，这里使用飞机姿态角的命名方式，两个角度分别是绕轴旋转的俯仰角和绕轴旋转的航向角。在水下将物体按照预设角度固定好，共8 种姿态，每种姿态拍摄10 幅图像，取10 次估计的均值，与真实姿态进行对比。最终估计结果如表7 所示。

表7 姿态估计结果

由表中数据可以发现，在一定范围内的姿态估计误差基本保持在5%左右，并且在实验过程中对俯仰角θ的变化比较敏感，这是因为如果θ变化超过一定范围时，相机在固定的情况下有可能无法从图像中识别出全部4 个角点，经实验发现，θ在-70˚~50˚时，均能得到较为准确的姿态估计效果，同时处理一幅图像所需时间约为95 ms，满足实时性的要求。

3.3 综合实验与分析

为验证本文整套目标识别跟踪与位姿估计算法的有效性与实时性，设计水下目标实时定位跟踪实验。

在水箱环境中选取3 个不同位置，保证3 个位置都在相机视野内。实验前人工测量3 个位置在相机坐标系下的真实坐标，每个位置都有预先设计好的物体位姿，实时监测人工将物体从位置1 依次移动到位置2、位置3，以及黑色和白色工件在位置4、5 和位置6、7 移动的过程，过程中3 种不同物体在7 个位置时的实时估计结果如图9 所示。俯仰角、航向角和深度估计值如表8 所示。

图9 实时检测结果

由表8 可以看出，3 种不同工件在7 个位置处的俯仰角、航向角和深度估计误差均保持在5%左右，同时实时检测过程中处理速度保持在15 FPS，说明本文的位姿估计算法是合理有效的，能够满足水下自主作业应用中实时性和准确性的要求。但存在一定的适用范围限制，当物体的俯仰角度过大即朝正方向或者负方向旋转超过约80˚时，相机无法将4 个角点全部拍摄到图像中时，则无法对位姿进行估计，部分估计失败情况如图10 所示。

图10 部分估计失败情况

表8 位姿估计结果

4 结 论

针对水下应用场景中对已知先验信息目标物体的目标识别与位姿估计问题，提出了一种基于单目视觉的水下目标检测与实时位姿估计方法。首先，基于YOLOv3 算法，改进网络结构，重新聚类，提出了改进YOLOv3 算法，实现了对目标物体的检测与跟踪，改进后网络结构复杂度降低，有利于水下场景中的移植实现。同时为后续位姿估计算法提供像素坐标信息。然后，结合目标物体的已知先验信息与计算机视觉原理，建立单应性模型，通过单应性模型来求解物体位姿，同时为了解决直接求解计算资源需求大的问题，构建PnP 模型，基于PnP 问题的基础上使用P3P 加LM 算法迭代的方法，获取相机坐标系与世界坐标系间的转换关系，进而求解单应性矩阵，不仅满足实时性的需求，同时通过迭代提高了求解精度，还能满足相机坐标系变化的情况。整体方法能够实现面向水下场景的目标物连续帧图像实时位姿估计。

实验结果表明，本文方法能够基于单目视觉对水下目标进行实时的检测跟踪与位姿估计，与现有的双目匹配算法相比实时性较好，且误差较低，在一定的姿态范围内具有较好的检测效果，且对噪声干扰具有一定的鲁棒性，能够以较为灵活简单的单目视觉系统为水下自主作业提供需要的位置信息。