一种声光融合导引的自主水下航行器鲁棒对接回收方法与流程

1.本发明涉及的是一种自主水下航行器的控制方法，更具体地说是在声光融合导引下的自主水下航行器鲁棒对接回收方法。


背景技术：

2.在过去的几十年里，机器人技术在水下环境中的应用迅速发展，auv可以在水下进行搜索、探测和侦查等任务。在完成作业任务后都需要能够实现回收处理，人为的吊装回收不仅成本巨大，而且十分的危险，目前auv远距离通过声学设备导引，近距离通过视觉传感器精准识别水下航行器与对接装置的相对位姿，从而实现auv自主对接的框架得到了国内外学者的广泛追捧。
3.对接回收问题是自主水下航行器研究过程中的关键课题，由于被控对象的非线性、耦合性和复杂环境干扰等特性，自主水下航行器对接回收问题的复杂度大大提升。目前为止，国内外先后开展了关于水下航行器自主回收的研究，其中研究的关键就在于如何使水下航行器能够精准的与回收装置实现对接。初期的研究主要是依靠声学设备比如长基线、短基线、超短基线，进行导航定位，但是由于声信号数据更新频率较低，在近距离精准对接的过程中稳定性和精度较差，对接成功率较低。随着计算机视觉的快速发展，视觉传感器具有高帧率、低噪声、延迟低等优点，视觉传感器在水下静距离对接任务中有着独特的优势。
4.但目前公开的声光融合导航方法还存在控制复杂，auv能源消耗较大等问题，需要结合工程实际，确定合适的控制逻辑，减少控制复杂程度，降低auv能源消耗。


技术实现要素：

5.为解决现有技术存在的问题，本发明提出一种声光融合导引的自主水下航行器鲁棒对接回收方法，既通过声学导航克服了惯性导航存在累计误差的缺点，也解决了单靠声学导航数据更新慢，对接成功率较低的缺点，而且结合工程实际经验，利用测量距离设定声光导航的加权值，控制逻辑简单，在满足系统较高的对接成功率、安全性和对接可靠性的情况下，降低auv能源消耗。
6.本发明的技术方案为：
7.所述一种声光融合导引的自主水下航行器鲁棒对接回收方法，包括以下步骤：
8.步骤1：按照任务需求设定航路点，并根据回收笼位置及回收方向，将最后一个航路点设置在回收笼正前方；水下航行器以航路点导引方式依次完成航路点，并到达位于回收笼正前方的最后一个航路点；
9.步骤2：执行回收任务参考深度为回收笼深度，跟踪的航向为回收笼口的朝向，跟踪的终点为回收笼后方；根据参考航向角ψ
reference
以及auv在大地坐标系下的速度，采用模糊控制直线跟踪方法继续行驶，进行auv自适应导引对接回收；当auv与回收笼距离小于设定阈值后未检测到超短基线usbl所需声源，则结束当前对接回收任务，并判断是否达到最
大尝试次数，若未达到最大尝试次数则重新回到最后一个航路点执行新一次对接任务，若达到最大尝试次数，则上浮等待回收；
10.步骤3：在auv接近回收笼的过程中，采用声光导引组合，根据超短基线usbl 测量结果以及光学手段，结合航行器自身的经纬度lon
cur
和lat
cur
，对回收笼经纬度lond和 latd进行更新；
11.步骤4：当auv调整到回收笼中心附近时，auv相机识别到回收笼底部中心目标光源，auv对回收笼底部中心目标光源进行自适应跟踪与位姿计算结果修正，完成水下航行器的自主对接；
12.若auv未检测到回收笼底部中心目标光源，则结束当前对接回收任务，并判断是否达到最大尝试次数，若未达到最大尝试次数则重新回到最后一个航路点执行新一次对接任务，若达到最大尝试次数，则上浮等待回收。
13.进一步的，步骤2中，参考航向角ψ
reference
通过以下过程得到：
14.参考航向角ψ
reference
的计算方式为：
15.ψ
reference
＝ψ
cross
ψ
init
16.其中ψ
cross
为直线跟踪角，ψ
init
为任务初始设定的参考航向角；
17.ψ
cross
＝arctan(k*l)
18.k为根据auv与回收笼距离而确定的直线跟踪参数；l为航行器与回收笼之间的横向距离。
19.进一步的，步骤2中，在水下航行器距离回收笼100米以外时，选择直线跟踪参数k＝0.03，在水下航行器距离回收站100米及100米以内时，选择直线跟踪参数k＝0.08。
20.进一步的，步骤2中，l为航行器与回收笼之间的横向距离：
21.l＝-de1*cos(ψ)-dn1*sin(ψ)
22.其中北向距离dn1和东向距离de1的近似值为
[0023][0024]
dn1＝(lat
d-lat
cur
)*r
[0025]
式中r为地球子午圈的半径。
[0026]
进一步的，步骤3中，建立声光导引的计算模型：
[0027]
i(x)＝j(x)f(x) (1-j(x))g(x)
[0028]
其中i(x)为声光导引组合下的回收笼经纬度计算值，j(x)为远距离声学系数，取值为1或者0，当检测到回收笼口部位置的光信号个数不足4个时取值为1，反之为0， f(x)为基于usbl的回收笼经纬度测量结果，g(x)为基于光学信号的回收笼经纬度测量结果；
[0029]
利用上一周期更新后的回收笼经纬度lond和latd计算回收笼与auv之间的实时距离
[0030][0031]
在dis》ll时，auv沿着直线驶向回收笼的过程中，根据超短基线usbl测量结果对回收笼经纬度lond和latd进行更新：ll为设定距离阈值；
[0032]
在dis≤ll时，控制水下航行器速度降低，并增加采用光学手段对回收笼的经纬度进行更新。
[0033]
进一步的，根据超短基线usbl测量结果对回收笼经纬度lond和latd进行更新过程为：
[0034]
usbl提供回收笼相对于auv之间的东向距离de2和北向距离dn2，得到更新后的回收笼经纬度lond和latd为：
[0035]
lond＝lon
cur
de2*ρ2[0036]
latd＝lat
cur
dn2*ρ1[0037]
其中纬度更新率ρ1和经度更新率ρ2根据大地测量学估算。
[0038]
进一步的，在回收笼口部位置具有至少4个光源，在回收笼底部中心具有一个光源，回收笼口部位置光源颜色与回收笼底部中心位置光源颜色不同；
[0039]
当dis≤ll时，若auv相机识别到回收笼口部位置的光源数小于4个，则航行器仍继续使用usbl获得的信息来进行回收笼经纬度更新，同时打开辅推加大左右横移量寻找剩下的蓝色光源，并且把速度再次降低；
[0040]
如果当dis《lt后，auv相机识别到回收笼口部位置的光源数仍小于4个，则结束当前对接回收任务，并判断是否达到最大尝试次数，若未达到最大尝试次数则重新回到最后一个航路点执行新一次对接任务，若达到最大尝试次数，则上浮等待回收；
[0041]
如果auv相机识别到回收笼口部位置的光源数达到4个，则将usbl加权降为零，只利用图像解算结果采用直线跟踪导引算法使auv朝回收笼中心靠近。
[0042]
进一步的，ll取为25m，lt取15m。
[0043]
进一步的，利用图像解算结果采用直线跟踪导引算法使auv朝回收笼中心靠近过程中，对采集的图像采用最大类间方差法进行自适应阈值分割，采用形态学梯度边缘检测方法识别目标光源的轮廓形状，并采用epnp算法进行位姿估计，得到auv与回收笼口中心位置的偏差量，包括横向偏差量、纵向偏差量和轴向偏差量；利用辅推控制，将横向偏差量、纵向偏差量调整至接近0，同时将速度降低至能够保持航行状态的最低速度；当轴向偏差量deltaz小于1500mm时，且横向纵向误差满足需求后，不再调整航行器位姿，并且提高主推转速，维持当前航向前进。
[0044]
有益效果
[0045]
本发明所构思的技术方案与现有技术相比，主要具备以下的技术优点如下：
[0046]
1、本发明使用声光融合导引，采用直线跟踪算法对路径进行规划设计，先采用 usbl声学导引对回收对接装置的位置进行更新，不断优化提高测量精度，保证auv 能安全可靠地靠近回收装置；
[0047]
2、本发明建立了声光导引的计算模型，结合了声学导引和光学导引的优点，根据情况切换导引方式，大大提高了对接的成功概率，并且在达到条件时才打开光学检测传感器，节约了能源消耗；
[0048]
3、声光导引过程中，回收装置的位置在不断优化，该方法不仅适用于静态回收装置，也适用于动态回收装置。
[0049]
4、针对识别到不足四个蓝色光源的情况，利用已有的蓝色光源位置和usbl声学导引信息依靠辅推调整左右偏移量，直至找到四个蓝色光源，解决了笼口方向发生移动以及动态笼口不确定方向的问题。
[0050]
本发明的附加方面和优点将在下面的描述中部分给出，部分将从下面的描述中变
得明显，或通过本发明的实践了解到。
附图说明
[0051]
本发明的上述和/或附加的方面和优点从结合下面附图对实施例的描述中将变得明显和容易理解，其中：
[0052]
图1回收对接总体示意图；
[0053]
图2单次对接回收流程图；
[0054]
图3目标光源安装示意图；
[0055]
图4图像处理算法流程图；
[0056]
图5声光融合下的加权系数变化情况；
[0057]
图6阈值分割结果图；
[0058]
图7闭运算结果图；
[0059]
图8 epnp虚拟控制点模型；
[0060]
图9航行器速度变化曲线；
[0061]
图10航行器深度变化曲线；
[0062]
图11相对角度偏差；
[0063]
图12相对位置偏差。
具体实施方式
[0064]
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图和实施例，对本发明进行进一步详细说明。
[0065]
步骤1：按照期望的路径设定航路点，航路点的设置应保证避开障碍物，而为了实现水下航行器自适应导引对接回收，设置的离回收笼最近的最后一个航路点在回收笼正前方，本实施例中处于回收笼正前方700米处。
[0066]
在航路点导引航行器的过程中，航行器通过自身携带的惯性导航系统设备输出解算的自身经度lon
cur
、纬度lat
cur
(本实施例中经纬度均指大地坐标系下的经纬度)和航向角ψ(本实施例中航向角以北偏西为正)，回收笼的经度lond和纬度latd由gps测得，以此来完成对航路点的跟踪。
[0067]
步骤2：当水下航行器以航路点导引方式依次完成航路点，并到达离回收笼最近的最后一个航路点后，执行回收任务，参考深度为回收笼深度，跟踪的航向为回收笼口的朝向，跟踪的终点为回收笼(lond,latd)的后方；此时根据参考航向角ψ
reference
以及 auv在大地坐标系下的速度，采用模糊控制直线跟踪方法继续行驶，进行auv自适应导引对接回收，其中auv在大地坐标系下的速度通过惯性导航系统设备输出，参考航向角ψ
reference
通过以下过程得到：
[0068]
参考航向角ψ
reference
的计算方式为：
[0069]
ψ
reference
＝ψ
cross
ψ
init
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(1)
[0070]
其中ψ
cross
为直线跟踪角，ψ
init
为任务初始设定的参考航向角。
[0071]
ψ
cross
＝arctan(k*l)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(2)
[0072]
k为根据auv与回收笼距离而确定的直线跟踪参数；本实施例中计算直线跟踪角
ψ
cross
时，在距离回收笼100米以外时，选择直线跟踪参数k＝0.03，使得航行器直线跟踪运动轨迹较为平滑，超调较少；在距离回收笼100米及100米以内时，选择直线跟踪参数k＝0.08，使得航行器在跟踪水声定位设备更新的回收笼位置时，航行器可以快速且有效地做出相应位姿调整，保证进入回收笼的航向和位置，即
[0073][0074]
l为航行器与回收笼之间的横向距离：
[0075]
l＝-de1*cos(ψ)-dn1*sin(ψ)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(4)
[0076]
而其中北向距离dn1和东向距离de1的近似值为
[0077][0078]
dn1＝(lat
d-lat
cur
)*r
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(6)
[0079]
式中r为地球子午圈的半径。北向距离dn1和东向距离de1两者均为近似值，但都满足导航精度。
[0080]
本实施例中，当auv与回收笼距离小于500米后未检测到超短基线usbl所需声源，则结束当前对接回收任务，并判断是否达到最大尝试次数，若未达到最大尝试次数则重新回到最后一个航路点执行新一次对接任务，若达到最大尝试次数，则上浮等待回收。
[0081]
步骤3：在auv接近回收笼的过程中，超短基线usbl测量出的信息有：航行器相对于回收笼之间的斜距r，航行器相对于回收笼的方位角θ
yz
(右正左负)，航行器相对于回收笼的俯仰角θ
xz
(上正下负)。由于在水下环境中，回收笼依靠gps定位的经纬度精度不能满足回收需要，因此本发明采用声光导引组合，根据超短基线usbl测量结果以及光学手段，结合航行器自身的经纬度lon
cur
和lat
cur
，对回收笼经纬度lond和latd进行更新，更新方法如下：
[0082]
建立声光导引的计算模型：
[0083]
i(x)＝j(x)f(x) (1-j(x))g(x)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(7)
[0084]
其中i(x)为声光融合下的回收笼经纬度计算值，j(x)为远距离声学系数，取值为 1或者0，当未检测到回收笼口部位置的4个光信号时取值为1，反之为0。f(x)为基于usbl的回收笼经纬度测量结果。g(x)为基于光学信号的回收笼经纬度测量结果。加权系数的变化如图5。
[0085]
利用上一周期更新后的回收笼经纬度lond和latd计算回收笼与auv之间的实时距离dis
[0086][0087]
在dis》25m时，auv沿着直线驶向回收笼的过程中，根据超短基线usbl测量结果对回收笼经纬度lond和latd进行更新：
[0088]
usbl提供回收笼相对于auv之间的东向距离de2和北向距离dn2，得到更新后的回收笼经纬度lond和latd为：
[0089]
lond＝lon
cur
de2*ρ2ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(9)
[0090]
latd＝lat
cur
dn2*ρ1ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(10)
[0091]
纬度更新率ρ1和经度更新率ρ2能够根据大地测量学估算：
[0092][0093][0094]
式中地球的第一偏心率e2＝0.0066943799013，地球半径r为6378137.0米。
[0095]
在dis≤25m时，控制水下航行器速度降低，并增加采用光学手段对回收笼的经纬度进行更新。
[0096]
在回收笼口部位置具有四个呈矩形分布的蓝色光源，在回收笼底部中心具有一个绿色光源。而航行器姿态和深度控制也由主推、舵板逐渐切换至主推、舵板、辅推共同作用，最终将切换至主推、辅推共同控制。
[0097]
当航行器的视觉传感器可以识别到装置口位置的蓝色目标光源时，根据检测到的不同数量的蓝色光源，航行器将作出不同的机动策略。图像处理计算机将相对位姿解算结果传输到航行器的运动控制计算机。检测到四个蓝色光源以后声学导引系统加权降为零，采用辅助推进器进行姿态控制，通过解算结果采用直线跟踪导引算法使航行器朝对接装置中心靠近。
[0098]
光学系统产生水下图像后，首先进行水下图像滤波处理：
[0099]
图像中存在着随机的白点或者黑点这是椒盐噪声的常见特征，绘制样本图像的统计直方图发现存在着类似于高斯正态函数的直方图分布噪声。针对水下图像存在的椒盐噪声特性采用中值滤波(设计一个包括像素周围邻近像素的模板；按照灰度值的大小重新排列模板中的像素；将灰度值最大和最小的像素点排列在模板的两边，取模板中间位置的像素值，最后用该像素值替代原本像素值。)处理。针对水下图像存在的椒盐噪声特性采用均值滤波(首先设计一个包含待处理像素的邻近像素模板；计算模板包含的所有像素均值；最后将计算的均值替换待处理的像素。)实现水下图像的复原。
[0100]
目标光源的安装示意见图3。当航行器的视觉传感器可以识别到回收笼口部蓝色目标光源时，根据检测到的不同的蓝色光源数时，航行器将作出不同的机动策略：
[0101]
当航行器相机看到小于特征点光源数目(n《4)时，无法解算位姿，航行器仍继续使用usbl获得的信息来进行控制，同时打开辅推加大左右横移量寻找剩下的蓝色光源，并且把速度再次降低，避免过快导致未找到全部光源便驶过终点。
[0102]
当dis《15m后，航行器相机仍未能看到4个特征点光源数目时，则结束当前对接回收任务，并判断是否达到最大尝试次数，若未达到最大尝试次数则重新回到最后一个航路点执行新一次对接任务，若达到最大尝试次数，则上浮等待回收。
[0103]
当航行器相机看到对应的特征点光源数目时(n＝4)时，图像处理计算机将auv 与四个特征点光源中心的相对位姿解算结果传输到航行器的运动控制计算机。检测到四个蓝色光源以后声学导引系统usbl加权降为零，采用辅助推进器进行姿态控制，通过解算结果采用直线跟踪导引算法使航行器朝对接装置中心靠近。其解算过程中需要自适应阈值分割，此处采用最大类间方差法(otsu)，按照图像的灰度特征将图像分成背景和目标两部分。由于水下图像背景比较暗，假设图像i(x,y)的大小为m
×
n，前景(及目标)和背景的分割阈
值记为t，图像中像素的灰度值小于阈值t的像素个数记作n1，像素灰度大于阈值t的像素个数记作n2,则有：
[0104][0105][0106]
n1 n2＝m
×nꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(15)
[0107]
ω1 ω2＝1
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(16)
[0108]
μ＝μ1×
ω1 μ2×
ω2ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(17)
[0109]
g＝ω1×
(μ-μ1)2 ω2×
(μ-μ2)2ꢀꢀꢀꢀꢀꢀꢀꢀ
(18)
[0110]
将公式16带入公式17，得到等价公式：
[0111]
g＝ω1×
ω2×
(μ-μ2)2ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(19)
[0112]
其中属于前景的像素点数占整幅图像的比例记为ω1，其平均灰度为μ1，背景像素点数占整幅图像的比例为ω2，其平均灰度为μ2，图像的总平均灰度为μ，类间方差记为g，通过上述公式采用遍历的方法得到使类间方差最大的阈值t，记为自适应的阈值分割标准，对采集到的水下图像按照公式18进行分割求得二值化图像g(x,y)：
[0113][0114]
分割结果见图6。
[0115]
仅通过灰度的相似性进行分割无法完整的识别目标光源的轮廓形状并且光斑区域会出现大小不一的空洞，这样便无法精确的求解出目标光源的中心点像素坐标，此时采用形态学梯度边缘检测方法。形态学的基本运算有膨胀与腐蚀、开运算与闭运算。采用闭运算处理，闭运算的数学表达：
[0116][0117]
其中a为水下图像，b为结构元，由于目标光源的真是形状为圆形，因此把结构元b定义为3
×
3的圆形结构元素形态学的闭运算是对图像先进性膨胀在进行腐蚀，闭运算通过圆形结构元素b与水下图像a进行不同形式的“与”操作将b遍历图像a的所有像素，水下图像中可以使边界向图像中心收缩，去除无意义的水下杂质，缩小前景面积更加逼近目标光源真实形状，并且还可以填充光源内部的小孔以及边缘的小凹陷部分。闭运算后的结果如图7所示，轮廓形状更加逼近光源形状，且识别光斑中的凹陷部分也被很好的填充。
[0118]
把图像进行处理以后，采用epnp算法进行位姿估计。
[0119]
epnp算法是一种o(n)时间复杂度具有封闭形式解的解算方案，该算法通过引入固定数量的虚拟控制点(4个)表示空间中的所有已知点坐标将复杂的多项式求解问题简化，避免了计算过程中未知量的交互利用线性解法计算虚拟控制点在目标坐标系中的位置对位姿进行最终求解，与最精确的迭代方法相比，精度损失很小，运算速度更快。
[0120]
epnp算法采用一组虚拟控制点的加权和来表示参考点，引入个中间标量，原算法中通过构造四个虚拟控制点的方法，只需要引入4
×
3一共12个中间变量，进而求得闭式解。epnp虚拟控制点模型见图8。
[0121]
n个目标点在世界坐标系中的3d坐标已知，及p
iw
，i＝1，2，...，n，可以在世界坐标系中构造四个控制点上标w表示世界坐标系下的点坐标，上标c表示相机坐标系下的点坐标，将参考点的坐标表示为控制点坐标的加权和：
[0122][0123]
其中α
ij
是齐次重心坐标，一旦虚拟控制点确定后，且满足4个控制点不共面的前提，α
ij
,j＝1,2,3,4是唯一确定的。在相机坐标系中，存在同样的加权和关系，相机的外参定义为[r t]，那么虚拟控制点和之间存在如下关系：
[0124][0125][0126]
当选取的第一个虚拟控制点为目标点的重心时，其余的控制点会都排列在数据的主方向上会使位姿估计算法的稳定性增加。设k是相机的内参矩阵，可以通过标定获得，利用用{ui}
i＝1,...,n
表示目标点{pi}
i＝1,...,n
的2d投影点，则有：
[0127][0128]
其中，wi是射影深度因子，将每个控制点用来表示，由相机的针孔模型可知，内参矩阵k可以用焦距系数fu，f
ν
图像坐标原点在像素坐标中的坐标 (uc,vc)。
[0129][0130]
从上式可以得到两个线性方程：
[0131][0132][0133]
可以发现wi已经被消掉，联立所有n个目标点，得到线性的方程：
[0134]
mx＝0
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(29)
[0135]
其中是一个由未知数组成的12向量，m是一个2n
×
12的矩阵
[0136][0137]
其中，vi是与m的n个非奇异值相关的m的右奇异向量的所有列的集合，可以通过12
×
12大小的矩阵m
t
m的零特征向量求得。通过研究发现矩阵m的核空间维数 n＝1,2,3,4，则上式描述的实际上是一个秩亏最小二乘问题，当n取不同的值时，由于相机的外参描述只是坐标变换，虚拟控制点之间的距离已知且没有发生改变，利用可以得到不同的二次约束，下面通过讨论n＝1，2的情况表述求解过程。
[0138]
例如当n＝1时，方程(上)简化为x＝βv，由可得：
[0139][0140]
当n＝2，x＝β1v1 β2v2，相机坐标系中控制点的距离表述变为：
[0141][0142]
因为我们有四个控制点，可以转换为一个含有5个方程的线性系统：
[0143]
lβ＝ρ
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(33)
[0144]
其中l是一个6
×
3的矩阵，由v1和v2组成，ρ是一个由二次距离组成的一个六维的向量。通过对不同的二次约束求解出β
i＝1,...,n
的系数值之后，通过下式求解最小的重投影误差值。
[0145][0146]
最小的重投影误差值对应的β
i＝1,...,n
系数值则为epnp位姿估算最优解。
[0147]
从而得到auv与回收笼蓝色光源中心位置的偏差量，包括横向偏差量、纵向偏差量和轴向偏差量。
[0148]
利用辅推控制，将横向偏差量deltax、纵向偏差量deltay调整至0左右(误差 《＝50mm)同时将速度降低至能够保持航行状态的最低。随着航行器继续前进，当轴向偏差量deltaz小于1500mm时，且横向纵向误差满足需求后，此时不再调整航行器位姿，并且提高主推转速，维持当前航向前进(避免由于相机视野的限制，在距离较近时无法看到全部的蓝色特征光源无法解算位姿而产生错误)。
[0149]
步骤4：当航行器调整到对接装置中心附近时，相机识别到笼内的绿色目标光源，图像处理计算机对绿色光源进行自适应跟踪与位姿计算结果进行修正，从而完成水下航行器的自主对接。
[0150]
若未检测到绿色光源，则结束当前对接回收任务，并判断是否达到最大尝试次数，若未达到最大尝试次数则重新回到最后一个航路点执行新一次对接任务，若达到最大尝试次数，则上浮等待回收。
[0151]
下面通过实验验证本发明设计的声光融合导引的自主水下航行器鲁棒对接回收方法的有效性。
[0152]
首先根据发明内容中步骤1、2所述，首先航行器解算自身经纬度，读取对接装置的深度为3.2m，跟踪的朝向为笼口朝向。然后按照本发明的步骤3-4进行控制器解算，并获得实验结果。图9为航行器速度变化曲线，图10为航行器深度变化曲线，图11 为相对角度偏差图，图12为相对位置偏差。
[0153]
由图9航行器的vx速度曲线可得，在航行器下潜到期望深度前由于存在俯仰角的原因会产生一定的距离倒退，所以任务开始初始航行器的前向速度为负值，当达到期望深度后，主推开始旋转航行器开始加速并且保持0.4m/s的前向航向速度。图10 为截取航行器深度变化曲线中的一部分，在20s左右的时航行器的深度达到期望深度，与航行器开始加速的时间吻合。
[0154]
图11与图12为航行器与对接装置相对的位姿偏差，即相对位姿估计的结果。由图11可以发现航行器与对接装置的俯仰角度偏差与任务设定的角度偏差相近，初始偏航角度偏差为8度左右，最后的解算角度0.04度满足对接角度要求。由图12可以看出，图中z(图像深度距离)、x(水平偏差距离)的变化曲线反映了航行器下潜时后退的趋势。由相对位置偏差可以发现，航行器在1.84m左右丢失目标光源信息，此时横向偏差在0.3m，符合对接要求。水池对接实验数据验证了本论文的控制算法的有效性。
[0155]
尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本发明的限制，本领域的普通技术人员在不脱离本发明的原理和宗旨的情况下在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。