一种基于多标记物协同引导的自主对接系统的制作方法

本技术涉及水下无人航行器的自动控制领域，尤其是一种基于多标记物协同引导的自主对接系统。

背景技术：

1、深海给人类带来了许多资源的同时也存在着大量的挑战，这使得uuv(unmannedunderwater vehicle，水下无人航行器)成为水下检测、搜索以及能源开采等领域不可或缺的工具。但受体积与载荷的限制，uuv无法长时间不间断地执行任务，须定期对接回收以进行能源补给与信息交换。

2、目前，国内外使用的对接导引系统主要是回笼式，通过在对接站的导向口上搭载电磁、声学、光学等传感器或标记物，引导uuv进入对接站的导向口。但是在uuv进入对接站的导向口之后，主要依靠uuv按照预设的行为和对接站的结构约束，盲冲至对接站末端的对接位置完成对接回收。然而，uuv在对接站内部从导向口至末端对接位置之间的末端区域内的盲冲行为无疑会导致其与对接站之间产生碰撞，不仅影响对接精度还会损坏uuv。

技术实现思路

1、本技术针对上述问题及技术需求，提出了一种基于多标记物协同引导的自主对接系统，本技术的技术方案如下：

2、一种基于多标记物协同引导的自主对接系统，包括如下步骤：

3、自主对接系统包括水下对接站和uuv，uuv搭载有相机；水下对接站包括回笼式结构的对接站本体及其上固定的标记物，对接站本体的尺寸与uuv相匹配，标记物包括引导光源和构成嵌套式结构的多个标识码，引导光源安装于对接站本体的入口处，构成嵌套式结构的多个标识码设置在对接站本体内部的末端并朝向对接站本体的入口；每个标记物有各自对应的引导区域，每个标记物的引导区域是以水下对接站为参照物时标记物在uuv的相机中的成像区域，相邻两个引导区域存在交叉重叠，所有标记物的引导区域的总引导范围覆盖对接站本体内部末端的对接位置至对接站本体的入口外部预定位置处之间的区域；uuv执行的自主对接方法包括：

4、通过搭载的相机实时获取前进方向上的图像并进行标记物检测，确定标记物在uuv坐标系中的位置参量uuv坐标系以uuv重心为坐标原点、以uuv航行前进方向为x轴正方向、以方位向指向uuv左舷为y轴正方向、以俯仰向垂直于xy平面向下为z轴正方向；

5、将标记物在uuv坐标系中的位置参量转换为在对接站坐标系中的位置参量p(x,y,z)；根据误差约束函数确定位置参量p(x,y,z)的x坐标处的y轴约束量ly(x)和z轴约束量lz(x)；误差约束函数在所有标记物的引导区域的总引导范围内关于x坐标是连续分段函数，且误差约束函数指示的约束量随着x坐标的减小而朝0收敛；

6、根据y轴约束量ly(x)和位置参量p(x,y,z)的y坐标确定y轴位置误差，根据z轴约束量lz(x)和位置参量p(x,y,z)的z坐标确定z轴位置误差；

7、根据y轴位置误差利用自主对接控制器调节方位向的运动，根据z轴位置误差利用自主对接控制器调节俯仰向的运动，并控制向前推进，直至到达对接站本体内部末端的对接位置。

8、其进一步的技术方案为，误差约束函数在所有标记物的引导区域的总引导范围内关于x坐标是连续分段函数，误差约束函数在每个单标记物引导区域内的分段函数指示每个轴的约束量随着x坐标的减小而减小，误差约束函数在每个过渡引导区域内的分段函数指示每个轴的约束量存在极大值和极小值且极大值对应的x坐标小于极小值对应的x坐标；其中，每个单标记物引导区域是每个标记物的引导区域中未与其他标记物的引导区域交叉重叠的区域，过渡引导区域是多个标记物的引导区域的交叉重叠区域。

9、其进一步的技术方案为，误差约束函数在每个单标记物引导区域内的分段函数与相邻的过渡引导区域内的分段函数在分界位置处的约束量相等，误差约束函数在每个单标记物引导区域内的分段函数与相邻的过渡引导区域内的分段函数在分界位置处的导数相等。

10、其进一步的技术方案为，对于y轴和z轴中的任意一轴，误差约束函数在每个单标记物引导区域内的分段函数指示当前轴的约束量与x坐标之间呈直线方程形式且l(x)＝kx+f，其中，斜率k和截距f分别与当前轴对应方向的识别定位指标要求以及各个标记物各自对应的引导区域的覆盖范围相关。

11、其进一步的技术方案为，斜率其中，是最靠近对接站本体内部末端的引导区域沿着x坐标的取值范围，p是所有标记物的引导区域的综合定位稳定性和定位误差，pe是所有标记物的引导区域的综合定位误差，po是预设相机视场范围，de是自主对接允许误差。

12、其进一步的技术方案为，任意一个单标记物引导区域内的分段函数的截距其中，[xmin,xmax]是当前单标记物引导区域沿着x坐标的取值范围，ps是当前单标记物引导区域的定位稳定性，pe是当前单标记物引导区域的定位误差，po是预设相机视场范围，de是自主对接允许误差。

13、其进一步的技术方案为，对于y轴和z轴中的任意一轴，误差约束函数在每个过渡引导区域内的分段函数指示当前轴的约束量与x坐标之间呈多项式方程形式且l(x)＝ax3+bx2+cx+d，其中，约束系数a、b、c、d分别与当前轴对应方向的单标记物引导区域内的分段函数以及各个标记物各自对应的引导区域的覆盖范围相关。

14、其进一步的技术方案为，误差约束函数在每个单标记物引导区域内的分段函数指示当前轴的约束量与x坐标之间呈直线方程形式且l(x)＝kx+f；任意一个过渡引导区域内的分段函数的约束系数a、b、c、d为：

15、

16、其中，k是单标记物引导区域内的分段函数的斜率，f2是相邻两个单标记物引导区域中靠近对接站本体内部末端的引导区域的分段函数的截距，f1是相邻两个单标记物引导区域中远离对接站本体内部末端的引导区域的分段函数的截距，是相邻两个单标记物引导区域中靠近对接站本体内部末端的引导区域沿着x坐标的取值范围的最大值，是相邻两个单标记物引导区域中远离对接站本体内部末端的引导区域沿着x坐标的取值范围的最小值。

17、其进一步的技术方案为，引导光源包括围绕对接站本体入口安装的1个主灯源和n个副灯源，主灯源与副灯源的灯光颜色不同，整数参数n≥3，确定标记物在uuv坐标系中的位置参量包括：

18、对获取的图像进行区域分割处理确定具有主灯源灯光颜色的若干个候选主灯源中心坐标，以及确定具有副灯源灯光颜色的若干个候选副灯源中心坐标；

19、从识别到的若干个候选主灯源中心坐标和候选副灯源中心坐标中筛选出满足灯源的物理布局位置关系的一个候选主灯源中心坐标和n个候选副灯源中心坐标得到各个引导光源的像素坐标；

20、对于单个标记物的引导区域，利用pnp算法对像素坐标进行解算得到标记物在相机坐标系中的位置参量p'(x,y,z)；

21、对于相邻两个标记物的引导区域的交叉重叠区域，对解算得到的单个标记物在相机坐标系中的位置参量和进行位姿融合得到标记物在相机坐标系中的位置参量

22、

23、对标记物在相机坐标系中的位置参量p'(x,y,z)进行坐标系转换确定标记物在uuv坐标系中的位置参量

24、其进一步的技术方案为，根据对接精度要求和uuv的尺寸确定对接站本体的尺寸l是对接站本体的长度，w是对接站本体的宽度，h是对接站本体的高度；luuv是uuv的长度，wuuv是uuv的宽度，huuv是uuv的高度；δx是长度方向对接精度，δy是宽度方向对接精度，δz是高度方向对接精度；

25、构成嵌套式结构的多个标识码包括第一标识码和第二标识码，第一标识码的尺寸大于第二标识码且第二标识码嵌套设置在第一标识码中，每个标识码是正方形结构，根据对接站本体的尺寸和长度方向对接精度δx确定嵌套式结构的尺寸lq1是第二标识码的边长，lq2是第一标识码的边长，θ是相机的水平视场角，β是相机的垂直视场角。

26、本技术的有益技术效果是：

27、本技术提出的一种基于多标记物协同引导的自主对接系统，通过在传统回笼式结构对接站本体上设置引导光源和嵌套式结构的多个标识码形成多种标记物引导的自主对接系统，能够实现远距离引导和对接站本体内部的末端引导。使得uuv在对接全流程均具备稳定的对接姿态，有效避免了末端的盲冲行为，确保自主对接的稳定可靠。

28、通过将对接过程中所有标记物的引导区域划分为各个标记物各自对应的单标记物引导区域以及相邻两个标记物的引导区域之间的过渡引导区域，并将各个引导区域的覆盖范围和识别定位指标要求转换为误差约束函数，有效解决了不同标记物引导区域的定位稳定性差异较大造成的控制发散问题，保证了自主对接过程的平稳高效。

29、通过设置1个主灯源和多个副灯源的引导光源，对获取的引导光源图像进行区域分割处理并筛选出满足灯源的物理布局位置关系的各个引导光源的像素坐标，剔除了引导光源由于水面倒影产生的伪目标，有效抑制水面倒影对定位稳定性造成的干扰，提高了引导光源的识别准确率，进而有效提升了自主对接系统的对接精度。