基于视觉融合的AUV动态对接定位方法

基于视觉融合的auv动态对接定位方法
技术领域
1.本发明涉及auv动态对接技术领域，具体为一种基于视觉融合的auv动态对接定位方法。


背景技术：

2.在对水底地形的勘测和水下机械作业中，auv是其重要的运载平台，单独的auv个体可以在水下实现探测和机械作业功能。但其突出缺点是在面对复杂的水下环境的时候，单个独立的auv受到了体积和长度的限制从而没有办法完成复杂的功能。小体积auv携带的有效载荷有限，大体积auv无法穿越复杂的水下空间。多auv的出现解决了这问题，将小体积和具有不同功能的auv使用串联方式对接，满足了功能的多样性和作业的机动性。
3.对接技术分为静态对接和动态对接。经典的回收对接属于静态对接，静态对接应用在水下充电和信息交换领域，其对接装置固定在确定的位置。动态对接相对静态对接更为复杂，每个单体之间需要在运动的状态下实现对接。动态对接应用于水下搜寻，协同任务等。分体式多auv通过串联方法组成单体auv。小auv之间通过动态对接技术完成彼此的分解和组合。在到达作业区执行任务时分解成小的auv，来应对水下复杂的环境，穿越狭小的空间，提高整体水下探测的效率和作业的机动性，结束作业后组合成一个单体auv。
4.在水下动态对接的技术中，实现小auv之间的定位是关键。定位是指获取对接auv和目标auv之间的距离和相对姿态。在动态对接的过程中将两个auv之间的距离缩小到零同时保证姿态保持一致。动态对接定位的误差对动态对接结果具有较大的影响。当定位的误差较大时，就会出现对接失败和反复对接的情况，最终无法实现对接。
5.目前获取相对定位信息的方法较为成熟的是使用声学设备。但是在近距离使用声学设备获取相对定位信息时，因近距离声学的噪声较大，具有较大的误差，无法实现精准的相对定位，进而影响到动态对接的效果。视觉的补充使用可以弥补声学的缺陷，通过视觉获取定位信息的技术中，目前使用单目视觉的方法较多。单目视觉可以满足静态对接的要求，但是在动态对接中，目标auv与对接auv均处于运动状态，视觉定位的有效帧数减小，且视觉角度有限，导致单目视觉定位精度下降，无法满足动态对接的要求。
6.另外，在采用单目视觉进行定位时，需要利用标志物尺寸解算相对距离和角度，现有方法中，对于标志物尺寸都是在岸上环境下预先测量后，作为已知量输入到单目视觉定位方法中，但申请人在研究过程中发现，在水下实际环境中，在岸上测量的标志物尺寸直接应用到水下后，存在系统误差，导致采用岸上环境测量的标志物尺寸应用到单目视觉定位方法中后，无法得到精确的定位结果。这种系统误差对于静态对接的影响上课接受，但对于动态对接则影响较大，且单目视觉方法本身也无法解算标志物的尺寸，导致目前在水下环境中使用单目视觉方法进行动态对接时，因定位精度不足，而难以动态对接成功。


技术实现要素：

7.动态对接过程中，单目视觉的有效帧数较小，丢失标志物的情况较多，而双目视觉
的有效帧数更少，因此单纯采用单目视觉或双目视觉方法进行动态定位均不能满足动态对接要求。为提高auv通过视觉获取相对定位信息的精度和容错率，尤其是在动态对接过程中的定位精度，解决依靠声学获取相对定位过程中噪声的干扰问题以及单纯采用单目视觉或双目视觉方法存在误差较大和容错率小的问题，本发明提出了一种基于视觉融合的auv动态对接定位方法，采用双目视觉解算出水下环境下能够抵消系统误差的标志物尺寸，并将双目视觉分解为两个单目视觉，分别通过pnp算法计算得出的两组相对定位信息进行信息加权融合，通过信息融合的方式，得到最终的相对定位信息，这样不仅提高了视觉的覆盖范围，同时提高了相对定位信息获取的精度和容错率，既保证了单目的精度又解决了双目视觉丢失率高的问题。
8.本发明的技术方案为：
9.所述一种基于视觉融合的auv动态对接定位方法，包括以下步骤：
10.步骤1：在水下环境中，对接auv利用自身前部安装的双目视觉系统中的两个单目摄像头，分别获取目标auv后部标志物图像，分为左图像和右图像；根据左图像和右图像，利用双目视觉算法，解算得到标志物尺寸；
11.步骤2：在水下对接过程中，对接auv利用自身前部安装的双目视觉系统中的两个单目摄像头，分别获取目标auv后部标志物图像，并依据步骤1解算得到的标志物尺寸，分别利用单目视觉定位算法，解算得到两组定位结果；
12.步骤3：对步骤2得到的两组定位结果进行加权融合，得到最终的auv动态对接定位结果。
13.进一步的，步骤1中，采用双目视觉相似三角形算法，解算得到标志物尺寸。
14.进一步的，步骤2中，分别利用单目视觉pnp定位算法，解算得到两组定位结果。
15.进一步的，步骤3中，最终的定位结果包括距离矩阵r和旋转矩阵t：
[0016][0017]
其中r1,t1为利用左侧单目摄像头拍摄图像，通过单目视觉定位算法解算出的定位结果，r2,t2为利用右侧单目摄像头拍摄图像，通过单目视觉定位算法解算出的定位结果；ρ1,ρ2为两组定位结果的权重。
[0018]
进一步的，目标auv后部标志物包括二维码标志物和矩阵光信标标志物；所述二维码标志物由黑白二维码和辅助照明设备构成；所述矩阵光信标标志物由四个发光灯球构成，四个发光灯球组成矩形，且三个灯球颜色相同，另一个灯球颜色与所述三个灯球颜色不同。
[0019]
进一步的，步骤3中，按照动态对接阶段确定两组定位结果的权重：
[0020]
步骤3.1：根据两组定位结果中的距离矩阵确定所属动态对接阶段，其中第一阶段为距离小于s1的对接阶段，第二阶段为距离处于s1～s2范围内的对接阶段，第三阶段为距离大于s2的对接阶段；
[0021]
步骤3.2：若所属动态对接阶段为第一阶段，则利用二维码标志物，采用四点平均法分别解算左右两组定位结果的权重：
[0022]
对左侧图像中的二维码标志物解算得到二维码四个角点在图像坐标系下的横坐标为x
l1
,x
l2
,x
l3
,x
l4
，纵坐标为y
l1
,y
l2
,y
l3
,y
l4
，图像中心坐标为(x
l0
,y
l0
)，则左侧图像二维码标志物的权重标量值ρ1＝(x
m
‑
x
l0
)2+(y
m
‑
y
l0
)2，其中
[0023][0024]
对右侧图像中的二维码标志物解算得到二维码四个角点在图像坐标系下的横坐标为x
r1
,x
r2
,x
r3
,x
r4
，纵坐标为y
r1
,y
r2
,y
r3
,y
r4
，图像中心坐标为(x
r0
,y
r0
)，则右侧图像二维码标志物的权重标量值ρ2＝(x
n
‑
x
r0
)2+(y
n
‑
y
r0
)2，其中
[0025][0026]
若所属动态对接阶段为第二阶段，则利用二维码标志物，采用起点距离加权法分别解算左右两组定位结果的权重：
[0027][0028][0029]
其中λ1为二维码标志物起始角点权重，λ2＝λ3＝λ4为二维码标志物剩余三个角点的权重，且λ1＞λ2＝λ3＝λ4；
[0030]
若所属动态对接阶段为第三阶段，则利用矩阵光信标标志物，采用起点向量加权法分别解算左右两组定位结果的权重：
[0031]
以图像中矩阵光信标标志物内独立颜色灯球作为1号点，其余三个灯球分别为2，3，4号点；
[0032]
对于左侧图像的1，2，3，4号点，在图像坐标系下的横坐标分别为x
′
l1
,x
′
l2
,x
′
l3
,x
′
l4
，纵坐标为y
′
l1
,y
′
l2
,y
′
l3
,y
′
l4
，并利用公式进行坐标放大：
[0033][0034]
对于右侧图像的1，2，3，4号点，在图像坐标系下的横坐标分别为x
′
r1
,x
′
r2
,x
′
r3
,x
′
r4
，纵坐标为y
′
r1
,y
′
r2
,y
′
r3
,y
′
r4
，并利用公式进行坐标放大：
[0035]
[0036]
其中k大于1；
[0037]
则左侧图像矩阵光信标标志物的权重标量值ρ1＝(x
″
m
‑
x
′
l0
)2+(y
″
m
‑
y
′
l0
)2，其中
[0038][0039]
左侧图像中心坐标为(x
′
l0
,y
′
l0
)；
[0040]
而右侧图像矩阵光信标标志物的权重标量值ρ2＝(x
n
″‑
x
′
r0
)2+(y
n
″‑
y
′
r0
)2，其中
[0041][0042]
右侧图像中心坐标为(x
′
r0
,y
′
r0
)。
[0043]
进一步的，s1取值为2m，s2取值为5m
[0044]
进一步的，利用1号点以及其余三个灯球的坐标，计算得到满足条件
[0045]
|l
24
|＞|l
34
|＞|l
23
|
[0046]
|l
34
|＞|l
23
|
[0047]
l
13
·
l
24
＜0
[0048]
的向量l
13
,l
23
,l
24
,l
34
，下标为向量的起点和终点，从而确定其余三个灯球的编号，从而得到1，2，3，4号点。
[0049]
进一步的，步骤3.1中，根据两组定位结果中的距离矩阵分别确定对接auv与目标auv的距离，取平均值后，依据平均值确定所属动态对接阶段。
[0050]
进一步的，对接auv自身前部安装的双目视觉系统中的两个单目摄像头光轴平行。
[0051]
有益效果
[0052]
本发明提出的基于视觉融合的auv动态对接定位方法，采用双目视觉解算出水下环境下能够抵消系统误差的标志物尺寸，提高了相对定位的计算精度；而且将双目视觉分解为两个单目视觉，分别通过pnp算法计算得出的两组相对定位信息进行信息加权融合，通过信息融合的方式，得到最终的相对定位信息，其中还优选将对接过程分为三个阶段，每个阶段对应相应的融合权重，这样不仅提高了视觉的覆盖范围，同时提高了相对定位信息获取的精度和容错率，既保证了单目的精度又解决了双目视觉丢失率高的问题，为动态对接的控制提供了较为准确的定位信息。
[0053]
本发明的附加方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本发明的实践了解到。
附图说明
[0054]
本发明的上述和/或附加的方面和优点从结合下面附图对实施例的描述中将变得明显和容易理解，其中：
[0055]
图1：本发明的算法示意图；
[0056]
图2：auv动态对接三个阶段对应的融合方法示意图；
[0057]
图3：起点向量加权法中确定灯球编号示意图。
具体实施方式
[0058]
下面详细描述本发明的实施例，所述实施例是示例性的，旨在用于解释本发明，而不能理解为对本发明的限制。
[0059]
本发明提出的基于视觉融合的auv动态对接定位方法，它包含了获取标志物尺寸和两组单目视觉融合两个过程。
[0060]
在使用单目视觉识别标志物和获取相对定位信息的过程中，需要明确知道标志物的尺寸，在算法解算过程中这个尺寸是没有办法修正的；在实际水下环境中，受到水下光照等因素影响，导致岸上测量的标志物物理尺寸与水下环境中需要应用的标志物尺寸之间存在系统误差，从而对整体定位产生影响。
[0061]
本发明中，首先采用双目视觉和基于视差的方法，获取水下环境中较为精准的标志物尺寸信息。而后将双目视觉和单目视觉定位算法相结合，即将双目视觉看做是两个单目视觉，分别使用单目pnp解算方法，得到两组定位信息。
[0062]
由于标志物在成像后图像中的位置是不同的，获得的两组定位信息的准确度和误差也是不一样。考虑图像采集设备的特性，存在鱼眼等多种效应，对图像的干扰程度也是不同的。因此根据标志物分别在两个单目视觉图像中的成像位置，确定两组定位信息的加权求和比例。在auv动态对接的不同阶段采用不同的加权算法，0到2m近距离获取的视觉信息可以忽略标志物内部成像的误差，使用第一种加权方法。标志物四个角点坐标求中心点，计算中心点到成像的像素中心点的距离，根据距离的不同，计算对应的权重比。而2到20m(设定的对接起始距离)，不能忽略标志物内部成像的误差，由于标志物成像的原点(标志物的左上角)在单目视觉获取定位信息(pnp)的计算过程中起到标识的作用，标志物原点准确性影响到获取相对定位获取的精度，所以在计算双目权重之间进行标志物四个角点的权重计算。第二种加权方法，分别求取四个角点分别到成像像素中心点的距离。其中起始角点的距离权重大于其余角点的距离权重，例如起始角点的距离权重取2，其余角点的距离权重为1，第二种加权方法适合auv对接在距离2到5m内，以二维码作为主要识别标志物。第三种加权方法，auv动态对接距离在5到20m，距离较远，二维码识别困难，因此以矩阵光信标作为主要标志物。
[0063]
具体步骤如下：
[0064]
步骤1：在水下环境中，对接auv利用自身前部安装的双目视觉系统中的两个单目摄像头，分别获取目标auv后部标志物图像，分为左图像和右图像；根据左图像和右图像，利用双目视觉算法，解算得到标志物尺寸。此步骤在定位方法中执行一次，得到标志物尺寸后，如果水下环境无较大变化，就可以不再重复执行。
[0065]
本实施例中，对接auv的头端安装的双目视觉系统由两个单目摄像头组成，两个单目摄像头的光轴保持平行，且使用相机标定板对双目视觉系统中的摄像头进行标定，获得摄像头的内参和外参。
[0066]
目标auv尾端安装的标志物包括二维码标志物和矩阵光信标标志物；所述二维码标志物由黑白二维码和辅助照明设备构成；所述矩阵光信标标志物由四个发光led灯球构成，四个发光led灯球组成矩形，矩形的左上角使用蓝色led，其余三个位置使用绿色led。
[0067]
本实施例中，对接auv头端两个摄像头获取目标auv尾端标志物图像，获取到左图像和右图像，根据左图像和右图像，利用双目视觉相似三角形算法，解算得到标志物尺寸，包括二维码的长宽和光信标矩形的长宽。
[0068]
步骤2：在水下对接过程中，对接auv利用自身前部安装的双目视觉系统中的两个单目摄像头，分别获取目标auv后部标志物图像，并依据步骤1解算得到的标志物尺寸，分别利用单目视觉pnp定位算法，解算得到两组定位结果。
[0069]
步骤3：对步骤2得到的两组定位结果进行加权融合，得到最终的auv动态对接定位结果。
[0070]
步骤3中，最终的定位结果包括距离矩阵r和旋转矩阵t：
[0071][0072]
其中r1,t1为利用左侧单目摄像头拍摄图像，通过单目视觉定位算法解算出的定位结果，r2,t2为利用右侧单目摄像头拍摄图像，通过单目视觉定位算法解算出的定位结果；ρ1,ρ2为两组定位结果的权重，该权重按照动态对接阶段确定：
[0073]
步骤3.1：根据两组定位结果中的距离矩阵分别确定对接auv与目标auv的距离，取平均值后，依据平均值确定所属动态对接阶段；其中第一阶段为距离小于2m的对接阶段，第二阶段为距离处于2m～5m范围内的对接阶段，第三阶段为距离大于5m的对接阶段。
[0074]
步骤3.2：若所属动态对接阶段为第一阶段，该阶段为近距离阶段，所以利用二维码标志物，采用四点平均法分别解算左右两组定位结果的权重：
[0075]
图像越靠近中心产生的畸变越小，得到的结果越准确，四点平均法即将四个点的横坐标x和纵坐标y相加以后求取平均数，计算新坐标距离图像中心的距离。
[0076]
通过对左侧图像中的二维码标志物解算得到二维码四个角点在图像坐标系下的横坐标为x
l1
,x
l2
,x
l3
,x
l4
，纵坐标为y
l1
,y
l2
,y
l3
,y
l4
，图像中心坐标为(x
l0
,y
l0
)，则左侧图像二维码标志物的权重标量值ρ1＝(x
m
‑
x
l0
)2+(y
m
‑
y
l0
)2，其中
[0077][0078]
而对右侧图像中的二维码标志物解算得到二维码四个角点在图像坐标系下的横坐标为x
r1
,x
r2
,x
r3
,x
r4
，纵坐标为y
r1
,y
r2
,y
r3
,y
r4
，图像中心坐标为(x
r0
,y
r0
)，则右侧图像二维码标志物的权重标量值ρ2＝(x
n
‑
x
r0
)2+(y
n
‑
y
r0
)2，其中
[0079][0080]
若所属动态对接阶段为第二阶段，也利用二维码标志物，采用起点距离加权法分别解算左右两组定位结果的权重；考虑到在计算定位过程中需要频繁使用二维码起始角点
的坐标，起始角点坐标的准确性对于最后结果具有重要的影响，因此在求取图像中心点到标志物四个点的距离后，使用距离加权提升起始角点的权重比，λ1为二维码标志物起始角点权重，λ2＝λ3＝λ4为二维码标志物剩余三个角点的权重，本实施例中根据二维码标志物的长宽和成像的图像长宽，选取起始角点的权重提高倍数为λ1＝2，也可以根据实际实验结果进行调整。而左右侧图像中标志物权重
[0081][0082][0083]
若所属动态对接阶段为第三阶段，距离较远，利用矩阵光信标标志物，采用起点向量加权法分别解算左右两组定位结果的权重：
[0084]
对标志物在图像中成像的四个像素点进行向量转换，提高起点的权重比，确定光信标的起点的时候，因光信标采用两种不同的颜色，确定左上角绿色led灯球为1号点，而其余三个光球因距离较远，并不容易确认光球的编号，因此，这里基于光信标构成的形状是矩形，使用直角三角形斜边大于直角边的特性确定2，3，4号点。如图3所示，l
12
表示起点是1号点终点是2号点的向量，|l
12
|表示向量l
12
的模长，同理可得向量l
13
,l
23
,l
24
,l
34
，下标表示向量的起点和终点。通过四个灯球的坐标，计算得到满足条件
[0085]
|l
24
|＞|l
34
|＞|l
23
|
[0086]
|l
34
|＞|l
23
|
[0087]
l
13
·
l
24
＜0
[0088]
的向量l
13
,l
23
,l
24
,l
34
，从而确定其余三个灯球的编号，从而得到1，2，3，4号点。
[0089]
对于左侧图像的1，2，3，4号点，在图像坐标系下的横坐标分别为x
′
l1
,x
′
l2
,x
′
l3
,x
′
l4
，纵坐标为y
′
l1
,y
′
l2
,y
′
l3
,y
′
l4
，并利用公式进行坐标放大：
[0090][0091]
对于右侧图像的1，2，3，4号点，在图像坐标系下的横坐标分别为x
′
r1
,x
′
r2
,x
′
r3
,x
′
r4
，纵坐标为y
′
r1
,y
′
r2
,y
′
r3
,y
′
r4
，并利用公式进行坐标放大：
[0092][0093]
其中k大于1，本实施例中取2；
[0094]
则左侧图像矩阵光信标标志物的权重标量值ρ1＝(x
″
m
‑
x
″
l0
)2+(y
″
m
‑
y
′
l0
)2，其中
[0095][0096]
左侧图像中心坐标为(x
′
l0
,y
′
l0
)；
[0097]
而右侧图像矩阵光信标标志物的权重标量值ρ2＝(x
n
″‑
x
′
r0
)2+(y
n
″‑
y
′
r0
)2，其中
[0098][0099]
右侧图像中心坐标为(x
′
r0
,y
′
r0
)。
[0100]
获得两组定位结果的权重后，即可根据加权计算公式得到最终的动态对接定位结果。
[0101]
尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本发明的限制，本领域的普通技术人员在不脱离本发明的原理和宗旨的情况下在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。