CN114549629A - 水下单目视觉估计目标三维位姿的方法 - Google Patents

Abstract

本发明涉及一种水下单目视觉估计目标三维位姿的方法，该方法采用改进的YOLOv3神经网络构建神经网络模型并对神经网络模型进行训练，利用训练后的模型检测目标物图像中的目标物，得到目标物上四个特征点像素坐标；根据构建的折射补偿模型计算折射前后的补偿系数，根据折射前后的补偿系数计算得到补偿后的四个特征点像素坐标；根据补偿后的四个特征点像素坐标，结合已知先验信息，建立位姿估计模型；求解位姿估计模型得到目标物相对于相机的三维位姿。本发明用于估计水下规则目标三维位姿，满足在复杂多变的水下环境里对目标进行准确检测，且位姿估计精度高。

Description

水下单目视觉估计目标三维位姿的方法

技术领域

本发明属于水下检测技术领域，具体地说，涉及一种水下单目视觉估计目标三维位姿的方法。

背景技术

在水下机器人自主作业过程中，通常使用机械臂对目标进行抓取，此时目标姿态等信息的获取尤为重要。当前水下机器人对水下目标三维位姿的估计多使用双目相机进行测量，但双目相机的视场较小、体积较大，在水下机器人作业过程中使用便携性差。单目相机具有结构简单、视场大等特点，更适合用于水下作业，但由于成像限制，单目相机无法直接获取目标深度和姿态信息，因此，若采用单目相机，则存在对目标位姿信息估计难度大的问题，仍需目标标记或人为干预，在应用中的自主化程度低。由于真实的水下环境复杂多变，成像存在对比度低等问题，目标特征降低，现有常用的检测方法难以准确检测到目标，无法进行后续工作。同时，由于水下成像存在不同介质之间的折射，通常情况下的估计方法在水下无法适用。此外，由于水下目标姿态不定，多为三维旋转情况，而现有位姿估计的方仅对目标垂直于相面的二维姿态有较好的估计效果，难以满足真实应用的需要。

发明内容

本发明针对现有水下检测过程中目标姿态估计存在目标检测准确率低、估计精度差等上述问题，提供一种水下单目视觉估计目标三维位姿的方法，用于估计水下规则目标三维位姿，满足在复杂多变的水下环境里对目标进行准确检测，且位姿估计精度高。

为了达到上述目的，本发明提供了一种水下单目视觉估计目标三维位姿的方法，其具体步骤为：

构建神经网络模型步骤：在YOLOv3神经网络的主干网络中加入重组层代替卷积层和池化层，引入实例归一化代替YOLOv3神经网络早期层中的批归一化，建立神经网络模型；

训练神经网络模型步骤：对神经网络模型进行训练得到训练后的神经网络模型；

目标检测步骤：通过训练后的神经网络模型对获取的水下目标物图像中的目标物进行检测，得到目标物四个角点的预测框，取预测框的中点像素坐标作为目标物的特征点坐标，得到目标物上四个特征点像素坐标；

构建折射补偿模型步骤：根据目标物实际位置、发生折射位置及水中折射后的投影位置构建折射补偿模型；

折射补偿步骤：根据折射补偿模型计算折射前后的补偿系数，根据折射前后的补偿系数计算得到补偿后的四个特征点像素坐标；

构建位姿估计模型步骤：根据补偿后的四个特征点像素坐标，结合已知先验信息，在目标物上建立世界坐标系，得到四个特征点在真实世界坐标系下的3D坐标，并以垂直于相机相面的方向为Z轴建立相机坐标系，四个特征点建立世界坐标系与相机坐标系之间的转换关系，该转换关系即为构建的位姿估计模型；

三维位姿估计步骤：求解位姿估计模型得到目标物的世界坐标系与相机坐标系之间的转换关系，该转换关系即为目标物相对于相机的三维位姿。

优选的，构建神经网络模型步骤中，所述重组层是将每个通道上大小为2*2的图像块中4个像素点进行拆解，然后重新排列成4个通道的大小为1*1图像块的结构层。

优选的，构建神经网络模型步骤中，所述实例归一化表示为：

式中，y_ncij为归一化后的值，x_ncij为一个图层中特征图的第ncij个元素，其中，i和j为空间维度，c为特征通道，n为批次中的第n个图像；μ_nc为第n个图像的第c个特征通道中的平均差，σ_nc为第n个图像的第c个特征通道中的方差，∈为常数，H₀为特征图的高度，W₀为特征图的宽度。

优选的，构建神经网络模型步骤中，对YOLOv3神经网络的损失函数进行改进，转化为区域框对角线的损失，则神经网络模型的损失函数表示为：

式中，C_m为目标的类别；P_m为属于某个类别的概率；

为第i个网格的第j个anchorbox是否负责预测该目标，如果负责则为1，否则为0；w₀为预测框的宽，h₀为预测框的高；w₀'为真实框的宽，h₀'为真实框的高，

为特征图中所有网格，s为特征图的边长，

为遍历每个网格对应的预测框，x_i和y_i为特征图中第i个网格的预测框的坐标，x_i′和y_i′为特征图中第i个网格的真实框的坐标，λ_coord为λ_coord对应项的权值系数，λ_noobj为λ_noobj对应项的权值系数，

为第i个网格的第j个anchor box是否不负责预测该目标，如果不负责则为1，否则为0，

为特征图中第j个网格的预测框的类别，

为特征图中第j个网格的真实框的类别，

为特征图中第j个网格的预测框的置信度，

为特征图中第j个网格的真实框的置信度。

优选的，构建神经网络模型步骤中，还对YOLOv3神经网络中的大、中、小三个分支的大分支进行剪枝处理，仅保留用于检测中等目标和小目标的中、小两个分支。

优选的，训练神经网络模型步骤中，在训练神经网络模型之前，首先对待检测区域进行数据集标注，然后设定anchor box的尺寸，通过聚类得到最优的9簇anchor box，此时进行申请网络模型训练。

优选的，对待检测区域进行数据集标注的方法为：(1)将现有的数据集图像旋转随机角度，并在设定范围内随机调整图像亮度；(2)对调整后的图像做自适应直方图均衡化处理，在设定范围内随机调整颜色对比度阈值以及进行像素均衡化的网格大小，得到不同对比度及亮度情况下的数据集样本，待样本满足设定要求后使用labelimg2进行数据集标注。

优选的，构建折射补偿模型步骤中，构建折射补充模型的具体步骤为：

将像素坐标系转换为图像坐标系，有：

式中，x₀为图像坐标系也即相机坐标系下的x轴坐标，y₀为图像坐标系也即相机坐标系下的y轴坐标，u、v分别为像素坐标系中的像素坐标，u₀,v₀为图像坐标系原点在像素坐标系中的位置，

为图像坐标系下x轴上的像元大小，

为图像坐标系下y轴上的像元大小；

通过公式(4)计算折射点在相机坐标系下的坐标，公式(4)表示为：

式中，z₀为相机坐标系下的z轴坐标，h为图像坐标系至折射水面的距离，f为相机焦距；

通过公式(5)计算折射前后的补偿系数，公式(5)表示为：

其中，

O_cO＝f (7)

O_cH＝h_c+d (8)

O_cO′＝h_c (9)

h_c＝f+h (10)

式中，k为目标物折射前后的补偿系数，O为图像坐标系原点，O'为折射水面坐标系原点，P为折射后的投影像素点，α为光线在水面的入射角，β为光线在水面的出射角，O_c为相机坐标系原点，G为折射点Q相对于目标实际位置Q'的垂直辅助点，H为折射水面坐标系原点O'相对于目标实际位置Q'的垂直辅助点，d为折射水面坐标系至目标实际位置的距离；

则有：

进而得到折射补偿模型表示为：

其中，

很小，近似为0处理。

优选的，构建位姿估计模型步骤和三维位姿估计步骤中，世界坐标系与相机坐标系之间的转换关系为[R|T]，其中，R为两个坐标系之间的旋转矩阵，T为两个坐标系之间的平移矩阵。

优选的，三维位姿估计步骤中，求解位姿估计模型得到目标物的世界坐标系与相机坐标系之间的转换关系[R|T]的具体步骤为：

记A′,B′,C′,D′为四个特征点的实际位置，a′,b′,c′,d′为检测到的四个特征点像素坐标位置，o′为相机的光心，分别连接A′,C′与a′,c′；

对于三角形Δo′a′b′和Δo′A′B′，由余弦定理可得：

o′A′²+o′B′²-2o′A′·o′B′·cos<a′,b′>＝A′B′² (14)

对于三角形Δo′b′c′和Δo′B′C′、三角形Δo′a′c′和Δo′A′C′，同理可得：

对公式(14)、公式(15)进行处理，等式两边同时除以o′C′²，记X＝o′A′/oC′，Y＝o′B′/o′C′，得：

X²+Y²-2XYcos＜a′,b′>＝A′B′²/o′C′²

Y²+1²-2Ycos＜b′,c′>＝B′C′²/o′C′² (16)

X²+1²-2Xcos＜a′,c′>＝A′C′²/o′C′²

记V＝A′B′²/o′C′²,UV＝B′C′²/o′C′²,WV＝A′C′²/o′C′²，则：

X²+Y²-2XYcos<a′,b′>-V＝0

Y²+1²-2Ycos<b′,c′>-UV＝0 (17)

X²+1²-2Xcos<a′,c′>-WV＝0

变形可得：

式中，cos<a′,b′>,cos<b′,c′>,cos<a′,c′>三个余弦角从图像中点的位置得到，u＝B′C′²/A′B′²,w＝A′C′²/A′B′²通过真实世界坐标系结合先验信息计算得到；

公式(18)转换成关于X、Y的方程，将三个特征点带入得到四个解，然后将第四个特征点带入得到一个[R|T]最优解；

采用最小二乘法对[R|T]最优解进行迭代，每次迭代后计算一次增益比例p，直到增益比例p大于阈值则迭代结束，判断是否收敛，最终得到优化后的世界坐标系和相机坐标之间的转换关系[R|T]。

与现有技术相比，本发明的优点和积极效果在于：

(1)本发明提供的水下单目视觉估计目标三维位姿的方法，通过对YOLOv3神经网络进行改进，构建神经网络模型，然后对神经网络模型进行训练，通过训练后的神经网络模型对目标物进行检测，相对于传统的检测方法和原始的神经网络，本发明能够在水下复杂成像环境中更好的检测到目标，提高目标物的检测精度。

(2)本发明采用改进的YOLOv3神经网络，去掉负责大目标检测的分支，能够提高计算速度，满足水下计算资源受限的环境。

(3)本发明训练神经网络模型的方法通过少量的数据集样本即可得到更好的训练效果。

(4)本发明对相机在水下工作时的折射问题做出了补偿，大大降低了误差，位姿估计精度高，能够满足实际应用里小目标的作业要求，对于提高水下自主作业的自主性具有重要意义。

附图说明

图1为本发明实施例所述水下单目视觉估计目标三维位姿的方法的流程图；

图2为本发明实施例所述水下单目视觉估计目标三维位姿的方法的检测方式图；

图3为本发明实施例所述重组层的结构示意图；

图4为本发明实施例改进后YOLOv3神经网络的早期层示意图；

图5为本发明实施例所述YOLOv3神经网络的分支改进示意图；

图6为本发明实施例所述YOLOv3神经网络剪枝处理后的分支框图；

图7为本发明实施例构建折射补偿模型的示意图；

图8为本发明实施例所述世界坐标系的示意图；

图9为本发明实施例所述求解世界坐标系与相机坐标系之间的变换关系[R|T]的示意图。

具体实施方式

下面，通过示例性的实施方式对本发明进行具体描述。然而应当理解，在没有进一步叙述的情况下，一个实施方式中的元件、结构和特征也可以有益地结合到其他实施方式中。

参见图1，本发明实施例提供了一种水下单目视觉估计目标三维位姿的方法，检测的目标物为规则形状物体的特征区域，以圆柱体为例，参见图2，检测圆柱体的四个角点，其中，圆柱体代表检规则形状目标物，矩形框代表检测区域。其具体步骤为：

S1、构建神经网络模型步骤：在YOLOv3神经网络的主干网络中加入重组层代替卷积层和池化层，引入实例归一化代替YOLOv3神经网络早期层中的批归一化，建立神经网络模型。

具体地，参见图3，重组层是将每个通道上大小为2*2的图像块中4个像素点进行拆解，然后重新排列成4个通道的大小为1*1图像块的结构层。加入重组层能够更好的保留像素中的局部细节，有利于为后续的位姿估计提供像素信息。

具体地，所述实例归一化表示为：

式中，y_ncij为归一化后的值，x_ncij为一个图层中特征图的第ncij个元素，其中，i和j为空间维度，c为特征通道，n为批次中的第n个图像；μ_nc为第n个图像的第c个特征通道中的平均差，σ_nc为第n个图像的第c个特征通道中的方差，∈为常数，H₀为特征图的高度，W₀为特征图的宽度。

需要说明的是，由于水下环境中拍摄的图像模糊、对比度低，因此，水下使用的网络应该具有更好的特征提取能力。参见图4，在本发明中引入了实例归一化(InstanceNormalization)代替YOLO神经网络的一些早期层中的批归一化(Batch Normalization)，可以帮助丢弃图像的对比度并留下内容信息，间接提高水下的检测能力，且对低级特征(例如线条和纹理)具有更好的学习能力，同时当训练批量较小时(即一次性训练图片较少，此时模型对总体特征掌握较少)具有更好的效果，更适合水下应用。

本步骤中，构建神经网络模型时，还对YOLOv3神经网络的损失函数进行改进，转化为区域框对角线的损失，则神经网络模型的损失函数表示为：

式中，C_m为目标的类别；P_m为属于某个类别的概率；

为第i个网格的第j个anchorbox是否负责预测该目标，如果负责则为1，否则为0；w₀为预测框的宽，h₀为预测框的高；w₀'为真实框的宽，h₀'为真实框的高，

为特征图中所有网格，s为特征图的边长，

为遍历每个网格对应的预测框，x_i和y_i为特征图中第i个网格的预测框的坐标，x_i′和y_i′为特征图中第i个网格的真实框的坐标，λ_coord为λ_coord对应项的权值系数，λ_noobj为λ_noobj对应项的权值系数，

为第i个网格的第j个anchor box是否不负责预测该目标，如果不负责则为1，否则为0，

为特征图中第j个网格的预测框的类别，

为特征图中第j个网格的真实框的类别，

为特征图中第j个网格的预测框的置信度，

为特征图中第j个网格的真实框的置信度。

需要说明的是，在实际水下应用中，目标的姿态是不确定的，因而采集到的图像中目标的长宽比是不确定的，本发明对神经网络模型的损失函数进行改进，转化为区域框对角线的损失。能够提高YOLOv3神经网络的泛化能力，使目标在各种姿态下都可以准确检测。

本步骤中，构建神经网络模型时，参见图5，还对YOLOv3神经网络中的大、中、小三个分支的大分支进行剪枝处理，仅保留用于检测中等目标和小目标的中、小两个分支，改进后的YOLOv3网络的框图参见图6。当检测小目标时，只有两个负责小目标和中等目标的分支发挥作用，负责大目标检测的分支仅在花费计算资源，本发明仅对小目标进行检测，去掉负责大目标检测的分支，能够提高计算速度，满足水下计算资源受限的环境。

S2、训练神经网络模型步骤：对神经网络模型进行训练得到训练后的神经网络模型。

具体地，在训练神经网络模型之前，首先对待检测区域进行数据集标注，然后设定anchor box的尺寸，通过聚类得到最优的9簇anchor box，此时进行申请网络模型训练。

需要说明的是，由于神经网络模型训练过程是对Anchor box进行不断调整得到正确的预测框，所以在对神经网络模型进行训练之前，需要先设定大小合适的Anchor box，从而提高算法识别的准确率，在本发明的应用场景中，待检测的目标特征区域较小，引入K-Means聚类算法，通过聚类得到最优的YOLOv3所需的9簇anchor box。在聚类过程中，首先随机选取9个点作为初始质心，然后遍历整个样本中所有的点，寻找与每个点距离最近的中心点作为一个簇，然后对生成的9个簇重新计算新的质心，直到质心不发生变化或者迭代次数达到最大时聚类完成，此时得到9簇最优的anchor box。

具体地，对待检测区域进行数据集标注的方法为：(1)将现有的数据集图像旋转随机角度，并在设定范围内随机调整图像亮度；(2)对调整后的图像做自适应直方图均衡化处理，在设定范围内随机调整颜色对比度阈值以及进行像素均衡化的网格大小，得到不同对比度及亮度情况下的数据集样本，待样本满足设定要求后使用labelimg2进行数据集标注。需要说明的是，在对水下目标进行检测的过程中，由于真实水下环境中的样本较难获取，在多数应用情况下仅有少数样本，难以满足训练需要。本发明采用上述标注方法可以在数据集有限的情况下发挥更大的作用，得到更好的训练结果。

S3、目标检测步骤：通过训练后的神经网络模型对获取的水下目标物图像中的目标物进行检测，得到目标物四个角点的预测框，取预测框的中点像素坐标作为目标物的特征点坐标，得到目标物上四个特征点像素坐标。

S4、构建折射补偿模型步骤：根据目标物实际位置、发生折射位置及水中折射后的投影位置构建折射补偿模型。

具体地，参见图7，构建折射补充模型的具体步骤为：

将像素坐标系转换为图像坐标系，有：

式中，x₀为图像坐标系也即相机坐标系下的x轴坐标，y₀为图像坐标系也即相机坐标系下的y轴坐标，u、v分别为像素坐标系中的像素坐标，u₀,v₀为图像坐标系原点在像素坐标系中的位置，

为图像坐标系下x轴上的像元大小，

为图像坐标系下y轴上的像元大小；

通过公式(4)计算折射点在相机坐标系下的坐标，公式(4)表示为：

式中，z₀为相机坐标系下的z轴坐标，h为图像坐标系至折射水面(即相机密封罩)的距离，f为相机焦距；

通过公式(5)计算折射前后的补偿系数，公式(5)表示为：

其中，

O_cO＝f (7)

O_cH＝h_c+d (8)

O_cO′＝h_c (9)

h_c＝f+h (10)

式中，k为目标物折射前后的补偿系数，O为图像坐标系原点，O'为折射水面坐标系原点，P为折射后的投影像素点，α为光线在水面的入射角，β为光线在水面的出射角，O_c为相机坐标系原点，G为折射点Q相对于目标实际位置Q'的垂直辅助点，H为折射水面坐标系原点O'相对于目标实际位置Q'的垂直辅助点，d为折射水面坐标系至目标实际位置的距离；

则有：

进而得到折射补偿模型表示为：

其中，

很小，近似为0处理。

S5、折射补偿步骤：根据折射补偿模型计算折射前后的补偿系数，根据折射前后的补偿系数计算得到补偿后的四个特征点像素坐标。

具体地，经过补偿后的像素坐标表示为：(u′,v′)＝k(u,v)。

S6、构建位姿估计模型步骤：根据补偿后的四个特征点像素坐标，结合已知先验信息，在目标物上建立世界坐标系(参见图8)，得到四个特征点在真实世界坐标系下的3D坐标，并以垂直于相机相面的方向为Z轴建立相机坐标系，四个特征点建立世界坐标系与相机坐标系之间的转换关系，该转换关系即为构建的位姿估计模型。

具体地，世界坐标系与相机坐标系之间的转换关系为[R|T]，其中，R为两个坐标系之间的旋转矩阵，T为两个坐标系之间的平移矩阵。

S7、三维位姿估计步骤：求解位姿估计模型得到目标物的世界坐标系与相机坐标系之间的转换关系，该转换关系即为目标物相对于相机的三维位姿。

具体地，求解位姿估计模型得到目标物的世界坐标系与相机坐标系之间的转换关系[R|T]的具体步骤为：

参见图9，记A′,B′,C′,D′为四个特征点的实际位置，a′,b′,c′,d′为检测到的四个特征点像素坐标位置，o′为相机的光心，分别连接A′,C′与a′,c′。

对于三角形Δo′a′b′和Δo′A′B′，由余弦定理可得：

o′A′²+o′B′²-2o′A′·o′B′·cos＜a′,b′>＝A′B′² (14)

对于三角形Δo′b′c′和Δo′B′C′、三角形Δo′a′c′和Δo′A′C′，同理可得：

对公式(14)、公式(15)进行处理，等式两边同时除以o′C′²，记X＝o′A′/oC′，Y＝o′B′/o′C′，得：

X²+Y²-2XYcos<a′,b′>＝A′B′²/o′C′²

Y²+1²-2Ycos<b′,c′>＝B′C′²/o′C′² (16)

X²+1²-2Xcos<a′,c′>＝A′C′²/o′C′²

记V＝A′B′²/o′C′²,UV＝B′C′²/o′C′²,WV＝A′C′²/o′C′²，则：

X²+Y²-2XYcos<a′,b′>-V＝0

Y²+1²-2Ycos<b′,c′>-UV＝0 (17)

X²+1²-2Xcos<a′,c′>-WV＝0

变形可得：

式中，cos<a′,b′>,cos<b′,c′>,cos<a′,c′>三个余弦角从图像中点的位置得到，u＝B′C′²/A′B′²,w＝A′C′²/A′B′²通过真实世界坐标系结合先验信息计算得到；

公式(18)转换成关于X、Y的方程，将三个特征点带入得到四个解，然后将第四个特征点带入得到一个[R|T]最优解。

采用最小二乘法对[R|T]最优解进行迭代，每次迭代后计算一次增益比例p，直到增益比例p大于阈值则迭代结束，判断是否收敛，最终得到优化后的世界坐标系和相机坐标之间的转换关系[R|T]。其具体步骤如下：

将变换矩阵的李代数表示为ξ，建设空间中实际点的世界坐标为P_i＝[X_i,Y_i,Z_i]，实际点所对应投影的像素坐标为u_i＝[u_i,v_i]，根据三维变换和李代数，有：

式中，s_i为目标物至相机的深度，K为相机坐标系下的投影关系；

将[R|T]最优解作为exp(ξ∧)的初值，将公式(19)等式两边的误差求和，构建最小二乘问题，迭代求解使误差最小化，误差函数构建为：

对每个误差项关于优化变量的导数进行求解，即线性化：

e(x+Δx)≈e(x)+JΔx (21)

式中，x为初始值，Δx为迭代增量；

将雅可比矩阵代入利用链式法则进行求导，有：

式(22)中，P′＝[X′,Y′,Z′]为世界坐标P_i利用李代数扰动后的坐标，第一项为误差关于投影点的导数，第二项为变换后的点关于李代数的导数，第一项与第二项相乘得到雅可比矩阵如下：

式中，f_x、f_y为以像素表示的相机焦距；

使用LM算法进行迭代运算，每次迭代的增量公式为：

(H'+λ)Δx＝g (24)

式中，H'＝J^TJ，λ为拉格朗日算子，g＝-J^TF(x)，F(x)为代价函数，T表示转置；

将Δx带入增益比例公式：

式中，p为增益比例；

每次迭代后计算一次p，直到p大于阈值则迭代结束，判断是否收敛，最终得到优化后的世界坐标系和相机坐标之间的转换关系[R|T]，即得到最终的目标相对于相机的三维位姿。

此时，通过相机坐标系和其他执行器(如机械臂)之间的坐标转换，即可实现对目标物的自主作业。

本发明先选用P3P算法，使用四对点中的三对点进行计算，然后根据第四点得到转换关系[R|T]的最优解，然后采用最小二乘问题对该解进行迭代，降低误差，兼顾求解精度与计算速度，实现精准实时位姿估计。

以上所述的具体实施方案对本发明的目的和技术方案进行了进一步的详细说明，熟知本领域的技术人员将理解，以上所述仅为本发明的具体实施方案而已，并非用以限定本发明的保护范围。

Claims (10)

1.一种水下单目视觉估计目标三维位姿的方法，其特征在于，其具体步骤为：

构建神经网络模型步骤：在YOLOv3神经网络的主干网络中加入重组层代替卷积层和池化层，引入实例归一化代替YOLOv3神经网络早期层中的批归一化，建立神经网络模型；

训练神经网络模型步骤：对神经网络模型进行训练得到训练后的神经网络模型；

目标检测步骤：通过训练后的神经网络模型对获取的水下目标物图像中的目标物进行检测，得到目标物四个角点的预测框，取预测框的中点像素坐标作为目标物的特征点坐标，得到目标物上四个特征点像素坐标；

构建折射补偿模型步骤：根据目标物实际位置、发生折射位置及水中折射后的投影位置构建折射补偿模型；

折射补偿步骤：根据折射补偿模型计算折射前后的补偿系数，根据折射前后的补偿系数计算得到补偿后的四个特征点像素坐标；

构建位姿估计模型步骤：根据补偿后的四个特征点像素坐标，结合已知先验信息，在目标物上建立世界坐标系，得到四个特征点在真实世界坐标系下的3D坐标，并以垂直于相机相面的方向为Z轴建立相机坐标系，四个特征点建立世界坐标系与相机坐标系之间的转换关系，该转换关系即为构建的位姿估计模型；

三维位姿估计步骤：求解位姿估计模型得到目标物的世界坐标系与相机坐标系之间的转换关系，该转换关系即为目标物相对于相机的三维位姿。

2.如权利要求1所述的水下单目视觉估计目标三维位姿的方法，其特征在于，构建神经网络模型步骤中，所述重组层是将每个通道上大小为2*2的图像块中4个像素点进行拆解，然后重新排列成4个通道的大小为1*1图像块的结构层。

3.如权利要求1所述的水下单目视觉估计目标三维位姿的方法，其特征在于，构建神经网络模型步骤中，所述实例归一化表示为：

式中，y_ncij为归一化后的值，x_ncij为一个图层中特征图的第ncij个元素，其中，i和j为空间维度，c为特征通道，n为批次中的第n个图像；μ_nc为第n个图像的第c个特征通道中的平均差，σ_nc为第n个图像的第c个特征通道中的方差，∈为常数，H₀为特征图的高度，W₀为特征图的宽度。

4.如权利要求1所述的水下单目视觉估计目标三维位姿的方法，其特征在于，构建神经网络模型步骤中，对YOLOv3神经网络的损失函数进行改进，转化为区域框对角线的损失，则神经网络模型的损失函数表示为：

式中，C_i为目标的类别；P_i为属于某个类别的概率；

为第i个网格的第j个anchor box是否负责预测该目标，如果负责则为1，否则为0；w₀为预测框的宽，h₀为预测框的高；w₀'为真实框的宽，h₀'为真实框的高，

为特征图中所有网格，s为特征图的边长，

为遍历每个网格对应的预测框，x_i和y_i为特征图中第i个网格的预测框的坐标，x_i′和y_i′为特征图中第i个网格的真实框的坐标，λ_coord为λ_coord对应项的权值系数，λ_noobj为λ_noobj对应项的权值系数，

为第i个网格的第j个anchor box是否不负责预测该目标，如果不负责则为1，否则为0，

为特征图中第j个网格的预测框的类别，

为特征图中第j个网格的真实框的类别，

为特征图中第j个网格的预测框的置信度，

为特征图中第j个网格的真实框的置信度。

5.如权利要求1所述的水下单目视觉估计目标三维位姿的方法，其特征在于，构建神经网络模型步骤中，还对YOLOv3神经网络中的大、中、小三个分支的大分支进行剪枝处理，仅保留用于检测中等目标和小目标的中、小两个分支。

6.如权利要求1所述的水下单目视觉估计目标三维位姿的方法，其特征在于，训练神经网络模型步骤中，在训练神经网络模型之前，首先对待检测区域进行数据集标注，然后设定anchor box的尺寸，通过聚类得到最优的9簇anchor box，此时进行申请网络模型训练。

7.如权利要求6所述的水下单目视觉估计目标三维位姿的方法，其特征在于，对待检测区域进行数据集标注的方法为：(1)将现有的数据集图像旋转随机角度，并在设定范围内随机调整图像亮度；(2)对调整后的图像做自适应直方图均衡化处理，在设定范围内随机调整颜色对比度阈值以及进行像素均衡化的网格大小，得到不同对比度及亮度情况下的数据集样本，待样本满足设定要求后使用labelimg2进行数据集标注。

8.如权利要求1所述的水下单目视觉估计目标三维位姿的方法，其特征在于，构建折射补偿模型步骤中，构建折射补充模型的具体步骤为：

将像素坐标系转换为图像坐标系，有：

式中，x₀为图像坐标系也即相机坐标系下的x轴坐标，y₀为图像坐标系也即相机坐标系下的y轴坐标，u、v分别为像素坐标系中的像素坐标，u₀,v₀为图像坐标系原点在像素坐标系中的位置，

为图像坐标系下x轴上的像元大小，

为图像坐标系下y轴上的像元大小；

通过公式(4)计算折射点在相机坐标系下的坐标，公式(4)表示为：

式中，z₀为相机坐标系下的z轴坐标，h为图像坐标系至折射水面的距离，f为相机焦距；

通过公式(5)计算折射前后的补偿系数

其中，

O_cO＝f (7)

O_cH＝h_c+d (8)

O_cO′＝h_c (9)

h_c＝f+h (10)

式中，k为目标物折射前后的补偿系数，O为图像坐标系原点，O'为折射水面坐标系原点，P为折射后的投影像素点，α为光线在水面的入射角，β为光线在水面的出射角，O_c为相机坐标系原点，G为折射点Q相对于目标实际位置Q'的垂直辅助点，H为折射水面坐标系原点O'相对于目标实际位置Q'的垂直辅助点，d为折射水面坐标系至目标实际位置的距离；

则有：

进而得到折射补偿模型表示为：

其中，

很小，近似为0处理。

9.如权利要求1所述的水下单目视觉估计目标三维位姿的方法，其特征在于，构建位姿估计模型步骤和三维位姿估计步骤中，世界坐标系与相机坐标系之间的转换关系为[R|T]，其中，R为两个坐标系之间的旋转矩阵，T为两个坐标系之间的平移矩阵。

10.如权利要求9所述的水下单目视觉估计目标三维位姿的方法，其特征在于，三维位姿估计步骤中，求解位姿估计模型得到目标物的世界坐标系与相机坐标系之间的转换关系[R|T]的具体步骤为：

记A′,B′,C′,D′为四个特征点的实际位置，a′,b′,c′,d′为检测到的四个特征点像素坐标位置，o′为相机的光心，分别连接A′,C′与a′,c′；

对于三角形Δo′a′b′和Δo′A′B′，由余弦定理可得：

o′A′²+o′B′²-2o′A′·o′B′·cos<a′,b′>＝A′B′² (14)

对于三角形Δo′b′c′和Δo′B′C′、三角形Δo′a′c′和Δo′A′C′，同理可得：

对公式(14)、公式(15)进行处理，等式两边同时除以o′C′²，记X＝o′A′/oC′，Y＝o′B′/o′C′，得：

记V＝A′B′²/o′C′²,UV＝B′C′²/o′C′²,WV＝A′C′²/o′C′²，则：

变形可得：

式中，cos<a′,b′>,cos<b′,c′>,cos<a′,c′>三个余弦角从图像中点的位置得到，u＝B′C′²/A′B′²,w＝A′C′²/A′B′²通过真实世界坐标系结合先验信息计算得到；

公式(18)转换成关于X、Y的方程，将三个特征点带入得到四个解，然后将第四个特征点带入得到一个[R|T]最优解；

采用最小二乘法对[R|T]最优解进行迭代，每次迭代后计算一次增益比例p，直到增益比例p大于阈值则迭代结束，判断是否收敛，最终得到优化后的世界坐标系和相机坐标之间的转换关系[R|T]。

Images