本发明涉及水下潜器自定位领域，尤其涉及一种基于单双目融合的潜器坐落式回收光视觉引导方法。
背景技术：
1、水下潜器是海洋开发的重要工具，可携带多种任务模块和传感器，在作业效率、自主能力和环境适应性等多方面存在优势。通常，潜器需通过自身携带能源完成水下作业，在任务进行过程中或完成后，通过自主回收实现能源补充、数据下载、设备检修等操作。因此，潜器能否进行精准、高效自主回收，是决定其能否被广泛应用的重要关键技术。水下自主对接回收具体可分为远距离和近距离两个阶段。在远距离阶段，潜器从远处靠近回收平台，对定位精度要求低，常使用声学引导；在近距离阶段，潜器逐渐调整自身位姿直至坐落至回收平台，对定位精度要求高，是决定回收成功与否的关键阶段，且仅凭声学信息的实现难度较大。同时，光视觉信息具有适应性强、信息丰富、更新频率快等优点，可用于潜器回收近距离阶段的自身位姿测量。
2、公开号为cn202211014006.5的名称为“一种主被动结合潜器水下对接回收系统及方法”的专利提供了一种主被动结合潜器水下对接回收系统及方法，可实现不需要复杂昂贵机械结构的潜器水下对接回收。该方法需要回收平台与潜器之间仅利用声学信号进行交互，从而实现潜器对接回收。
3、公开号为cn202110168684.6的名称为“一种基于视觉的潜器回收装置与方法”的专利提供了一种基于视觉的潜器回收装置与方法，可实现潜器水下对接回收。然而，该方法在回收最后阶段，使用拖动定位而不是自主航行定位，缺乏通用性。
4、公开号为cn202010595066.5的名称为“一种水下机器人回收定位方法”的专利和公开号为cn202010594951.1的名称为“基于双目视觉的水下机器人回收定位方法”的专利分别提供了一种基于光视觉算法的水下潜器自定位方法，实现了潜器回收过程中的自身定位。
5、但上述方法均使用传统图像处理算法，抗干扰能力及鲁棒性较差。
6、因此，本领域的技术人员致力于开发一种基于单双目融合的潜器坐落式回收光视觉引导方法。
技术实现思路
1、有鉴于现有技术的上述缺陷，本发明所要解决的技术问题是现有的特征匹配算法多数依赖先前时刻的帧信息，要求潜器位姿变化幅度不能过大，难以适应于潜器速度较快的情况。
2、为实现上述目的，本发明提供了一种基于单双目融合的潜器坐落式回收光视觉引导方法，其特征在于，所述方法包括以下步骤：
3、s101：潜器利用声学信息引导逐渐接近回收平台，进入近距离光学导引阶段；
4、s103：所述潜器获取单目相机和双目相机，利用所述单目相机和所述双目相机拍摄得到当前帧图像，并对所述当前帧图像进行暗通道先验去雾处理，得到图像质量增强后的去雾图像；
5、s105：利用基于yolo v9的引导灯光源检测算法，得到所述引导灯的特征点在所述去雾图像中的坐标；
6、s107：利用基于匈牙利匹配的特征匹配算法，确立检测得到的二维引导灯特征点和现实世界中三维引导灯特征点的对应关系；
7、s109：判断所述单目相机和所述双目相机中是否含有所述回收平台的图像，并根据判断结果进行单目相机位姿测量和/或双目相机位姿测量；
8、s111：对单目位姿测量结果和双目位姿测量结果进行融合，得到最终位姿。
9、进一步地，所述s103包括如下子步骤：
10、s1031：对所述当前帧图像构造雾气退化模型，所述雾气退化模型为：
11、i(x)＝j(x)t(x)+a(1-t(x))；
12、s1032：对所述当前帧图像采用如下方法计算暗通道idark(x)：
13、
14、s1033：计算大气光值ac：从所述暗通道中选取像素值最大的像素点，找到所述像素点对应于有雾图像中的像素位置以及每个通道中所述像素点的像素平均值，从而计算出每个通道c的所述大气光值ac；
15、s1034：计算透射率t(x)：对于图像中的每一点x以及所述点对应的局部窗口ω(x)，所述点的所述透射率为：
16、
17、s1035：根据所述雾气退化模型，计算得到增强后的所述去雾图像；
18、其中，i(x)为当前帧图像，j(x)是增强后的去雾图像，t(x)是透射率，a是大气光值，c表示r,g,b三通道中的某一通道，ac为通道c的大气光值，ω(x)为矩形窗口，x代表图像中的点，y代表图像ω(x)中的点，ω为去雾系数。
19、进一步地，在所述s105中，以所述去雾图像作为yolo v9目标检测神经网络的输入，得到所述去雾图像中所述引导灯的边界框，通过对所述边界框内的图像进行阈值分割、连通域检测和质心提取，得到所述引导灯的所述特征点在所述去雾图像中的坐标。
20、进一步地，所述s107包括如下子步骤：
21、s1071：计算所述回收平台的设计图和实拍图像中的位置向量，所述位置向量包括质心坐标和各个特征点的位置向量；
22、s1072：对所述实拍图像中的所述位置向量进行校正：根据所述实拍图像中所述特征点的分布情况，计算所述特征点的最小外接矩形框及所述特征点在所述实拍图像上的偏转角度，并根据所述偏转角度对所述特征向量进行校正：
23、
24、s1073：计算所述实拍图像中校正后的所述位置向量和所述设计图中的所述位置向量中两两向量之间的欧氏距离，以所述欧式距离为代价，利用匈牙利匹配算法，确定两组向量之间的对应关系：
25、
26、其中，为实拍图像中第i个特征点的位置向量，为实拍图像中第i个特征点校正后的位置向量，为实拍图像的质心坐标，为实拍图像中第i个特征点的位置坐标，为设计图中第i个特征点对应的位置向量，为设计图的质心坐标，是设计图中第i个特征点的位置坐标，n为特征点数量，i为特征点编号，θ为实拍图像中第i个特征点在实拍图像上的偏转角度，m为两组向量之间的对应关系，d为代价矩阵，上标o表示回收平台设计图中的元素，上标m表示回收平台实拍图像中的元素。
27、进一步地，在所述s109中，当所述双目相机中含有所述回收平台的图像时，进行所述双目相机位姿测量：通过三角测量，确定匹配后的所述特征点在所述双目相机坐标系下的坐标，采用基于svd分解的位姿测量算法计算所述潜器的位姿。
28、进一步地，所述双目相机位姿测量包括如下步骤：
29、s1091：利用相机内参矩阵k，得到相机坐标系下3d坐标[xc,yc,zc]t和像素坐标系下坐标[u,v]t之间的转换关系：
30、
31、s1092：利用相机外参矩阵p，得到三角测量时双目相机坐标系下的三维坐标[xbc,ybc,zbc]t和[xc,yc,zc]t之间的转换关系：
32、
33、将其移项得：
34、
35、s1093：综合左右目相机成像模型，构建三维坐标和二维特征点之间的转换关系，得到所述特征点在相机坐标系下的三维坐标[xbc,ybc,zbc]t的超定方程：
36、
37、s1094：求解所述超定方程，得到所述双目相机中的二维特征点在所述双目相机坐标系下的坐标[xbc,ybc,zbc]t；
38、s1095：在所述双目相机完成三角测量后，基于svd分解，计算所述潜器相对于所述回收平台的相对位姿，
39、最终姿态解为:
40、
41、最终位置解为：
42、t*＝q0-r*p0，
43、其中，xc,yc,zc为相机坐标系下坐标，u,v为像素坐标系下坐标，xbc,ybc,zbc为双目相机坐标系下的坐标，x′c为归一化后x方向坐标，y′c为归一化后y方向坐标，上标l和r分别表示左右目相机的内外参数，下标cl和cr分别表示左右目相机坐标系下的坐标，q0为世界坐标系下的质心坐标，p0为双目相机坐标系下的质心坐标，v,u为对h进行svd分解得到正交矩阵，det()为函数用于计算矩阵行列式；
44、
45、k为相机内参矩阵，fx为使用像素来描述x轴方向焦距的长度，fy为使用像素来描述y轴方向焦距的长度，cx,cy为实际位置；
46、p为相机外参矩阵，r11,r12,r13,r21,r22,r23,r31,r32,r33构成相机旋转矩阵，tx,ty,tz构成相机的平移向量。
47、进一步地，在所述s109中，当所述单目相机中含有所述回收平台的图像时，进行所述单目相机的位姿测量：利用匹配后的所述特征点，使用迭代求解法求解pnp问题得到相机位姿，通过迭代优化相机的位姿参数实现相机位姿的估计，并得到所述潜器的位姿。
48、进一步地，在所述单目相机特征匹配完成后，根据重投影误差函数，构建优化目标，将位姿求解问题转化为优化问题，并利用列文伯格-马夸尔特优化算法，最小化所述重投影误差函数，得到所述相机位姿，所述优化目标为：
49、
50、其中，k是相机内参矩阵，l是相机位姿(r,t)的李群表示，r是相机姿态，t是相机位置，pi为第i个特征点在单目相机像素坐标系下的坐标，qi是第i个特征点在世界坐标系下的坐标，si为点qi的深度信息，n为特征点数量，i为特征点编号。
51、进一步地，在所述s111中，在对所述单目位姿的测量结果和所述双目位姿的测量结果进行融合时，判断单目可见、双目可见或者单双目均可见，根据判断结果分别进行位姿推算。
52、进一步地，所述位姿推算包括：
53、在仅单目可见阶段，以单目测量结果作为最终位姿；
54、在仅双目可见阶段，以双目测量结果作为最终位姿；
55、在单双目均可见阶段，以单双目相机各自观察到的所述引导灯数目为权重进行加权融合，作为最终测量结果；
56、在单双目均不可阶段，则暂停位姿推算，保持下降状态，直至回收任务结束。
57、在本发明的较佳实施方式中，和现有技术相比，本发明具有如下有益效果：
58、1、针对回收过程中的特征匹配问题，本发明仅利用当前帧信息和引导灯设计图信息，设计了不依赖于潜器速度的全向特征匹配算法，能够实现在不同方向、不同速度下的准确匹配，提高了系统的灵活性和稳定性。
59、2、本发明结合暗通道先验去雾和先进的yolo v9目标检测网络，设计的引导光源提取算法能够有效适应不同水质、光照强度的水下环境，实现稳定且准确的引导光源识别，鲁棒性强，可靠性高。
60、3、根据坐落式回收的多阶段特点，本发明设计了面向不同阶段的单、双目引导定位算法，能够根据不同阶段的需求进行精准定位，实现多阶段引导定位，提高了回收的效率和成功率。
61、以下将结合附图对本发明的构思、具体结构及产生的技术效果作进一步说明，以充分地了解本发明的目的、特征和效果。
1.一种基于单双目融合的潜器坐落式回收光视觉引导方法，其特征在于，所述方法包括以下步骤：
2.如权利要求1所述的方法，其特征在于，所述s103包括如下子步骤：
3.如权利要求2所述的方法，其特征在于，在所述s105中，以所述去雾图像作为yolo v9目标检测神经网络的输入，得到所述去雾图像中所述引导灯的边界框，通过对所述边界框内的图像进行阈值分割、连通域检测和质心提取，得到所述引导灯的所述特征点在所述去雾图像中的坐标。
4.如权利要求3所述的方法，其特征在于，所述s107包括如下子步骤：
5.如权利要求4所述的方法，其特征在于，在所述s109中，当所述双目相机中含有所述回收平台的图像时，进行所述双目相机位姿测量：通过三角测量，确定匹配后的所述特征点在所述双目相机坐标系下的坐标，采用基于svd分解的位姿测量算法计算所述潜器的位姿。
6.如权利要求5所述的方法，其特征在于，所述双目相机位姿测量包括如下步骤：
7.如权利要求6所述的方法，其特征在于，在所述s109中，当所述单目相机中含有所述回收平台的图像时，进行所述单目相机的位姿测量：利用匹配后的所述特征点，使用迭代求解法求解pnp问题得到相机位姿，通过迭代优化相机的位姿参数实现相机位姿的估计，并得到所述潜器的位姿。
8.如权利要求7所述的方法，其特征在于，在所述单目相机特征匹配完成后，根据重投影误差函数，构建优化目标，将位姿求解问题转化为优化问题，并利用列文伯格-马夸尔特优化算法，最小化所述重投影误差函数，得到所述相机位姿，所述优化目标为：
9.如权利要求8所述的方法，其特征在于，在所述s111中，在对所述单目位姿的测量结果和所述双目位姿的测量结果进行融合时，判断单目可见、双目可见或者单双目均可见，根据判断结果分别进行位姿推算。
10.如权利要求9所述的方法，其特征在于，所述位姿推算包括：
