一种适用于极地高纬度的AUV多传感器组合回坞导引方法

本发明属于探测，具体涉及一种适用于极地高纬度的auv多传感器组合回坞导引方法。

背景技术：

1、自主水下航行器(autonomous underwater vehicle)，简称auv，是一种无人操控自主导航的水下航行器。作为海洋空间探测器，auv在科研、军事和商业领域有广泛应用如何让auv在水下工作更久，具有更强的自主控制，更快的信息采集以及通信传输能力，是各国科研工作者的目标。由于工作在海洋空间，auv不同于陆上和太空中的无人飞行器，目前限制auv发展的技术难点主要有水下通信速率低，传输距离短，无法实现及时有效的信息传输，以及电池存储技术尚未有重大突破，自身携带的能源有限，导致在水下无法长时间、大范围的巡航探测，所以需要在水下固定点建立接驳站，自动导引auv进入接驳站的坞口，实现安全对接，完成信息传输和能源补给。

2、回坞导引技术是实现auv自动导引到接驳站坞口的关键技术，是保证auv与水下接驳站顺利对接的前提。目前回坞导引方式有声学导引、光学引导和电磁导引。光学导引在非湍急水流和非污染环境中有良好的导引性能，但受到背景照明和水浊度的影响，光学导引容易受到背景照明和水浊度的影响，同时光标必须出现在auv的摄像头视野范围内才能检测到。声学导引有效距离远，但是auv越靠近坞口越需要快速的时间分辨率和精确的空间测量精度，声学导引系统的实时性差测量精度低的特点，使得它难以满足auv近端回坞的要求。而电磁导引能克服光学导引易受海水清晰度影响和声学导引实时性较低的缺点，但同时也容易受到地磁场信号与其他电子设备磁场的干扰。

3、文献《auv电磁导引回坞系统研究与设计》利用电磁导引auv回坞，采用渐进式回坞方法，电磁感应线圈固定在回坞站发射低频交变磁场，磁力仪通过对磁场信号的解析可以实时获得auv与当前位置磁场线方向的夹角和磁场大小。

4、申请号为202010826664.9的发明方法，公开了一种基于深度学习的auv光学引导与测向方法，通过dnn网络结构进行分类，判断方位角区间，再根据方位角区间，通过改进bp神经网络判断具体方位角与斜角，根据相对位置信息引导auv朝对接坞移动靠近，直至auv回坞。

5、申请号为202010138694.0的发明方法，公开了一种基于多声学信标引导的auv水下回收对接系统，包括auv和水下回收平台。auv通过接收声学信号计算与回收平台中轴线的偏差并修正自身位置和姿态，从而实现在水平和垂直方向上与回收平台的精准对接。

6、现有技术存在以下不足之处：

7、1)电磁导引是渐进式回坞，检测的导引信号只与auv当前位置相关，只能获得auv与当前位置磁场线方向的夹角，无法判断对接坞的具体方位。这是不够的。因为当auv接近对接坞时，很有必要知道相对于对接坞的距离和方位，这样auv可以根据相对距离调整对接策略。当距离较远时，auv主要在对接导航设备的导引下通过主推和简单的姿态调整快速逼近；但距离较近时，auv就需要减速并启动动力定位控制，并根据测得的相对距离和方位实现精准的轨迹跟踪或者悬停，从而实现最终的精准对接。

8、2)光学导引容易受到背景照明和水浊度的影响，同时光标必须出现在auv的摄像头视野范围内才能检测到，使得auv在受到干扰后容易丢失视觉跟踪的导引目标。

9、3)声学导引有效距离远，但是auv越靠近坞口越需要快速的时间分辨率和精确的空间测量精度，声学导引系统的实时性差测量精度低的特点，使得它难以满足auv近端回坞的要求。此外，冰下复杂水声环境影响auv的稳健水声通信和定位。这说明在极地冰下仅仅依靠声学是很难有效导引auv回坞的。

10、4)常见的auv导航是基于惯导和罗盘进行的，但是极地的高纬度条件会影响auv惯导和罗盘的导航定位。

技术实现思路

1、针对上述背景技术介绍中存在的问题，本发明的目的在于提供了一种适用于极地高纬度的auv多传感器组合回坞导引方法，实现在极地高纬度条件不依赖于惯导和罗盘的auv回坞导引方法，并且可以实时获取auv与对接坞的相对位置，从而使得auv可以更加精准可靠的回坞。

2、本发明采用的技术方案是：

3、一种适用于极地高纬度的auv多传感器组合回坞导引方法，其具体步骤如下：

4、s1，由usbl引导auv进行远距离回航；

5、s2，当auv达到近距离范围时，基于训练好的自适应学习率bp神经网络，可以由磁力仪获得的磁场大小b与磁场线夹角γ作为输入得到auv相对于对接坞的方位角β与auv到对接坞的距离d；

6、s3，当auv达到近距离范围时，判断视觉是否可以识别光标，若视觉无法识别光标时，自适应学习率bp神经网络通过磁力仪获得的磁场大小b与磁场线夹角γ可以得到方位角β与距离d，回坞导航控制器将通过这一路数据进行导航；若视觉可以识别光标时，机器视觉与usbl的基于视觉置信度的互补滤波融合得到的位置参数以及自适应学习率bp神经网络拟合得到的位置参数，二者通过自适应卡尔曼滤波多信息融合生成最终的方位角β与距离d，并作用于回坞导航控制器；

7、s4，判断auv是否回坞，若auv已经回坞，则结束进程，若auv没有回坞，则重复上述步骤。

8、进一步，步骤s3中在auv回坞时采集有效数据并存储起来用于定期更新自适应学习率bp神经网络。保证样本数据的多样性，并提高自适应学习率bp神经网络的准确度。

9、进一步，步骤s2中的自适应学习率bp神经网络结构，包含输入层、双隐含层、输出层，网络结构具有两输入和两输出，假设系统中共有p个训练样本，期望输出为tn，实际输出为0n，则每个样本的损失函数为：

10、

11、η为学习率，学习率是梯度下降算法中非常重要的一个参数，采用自适应学习率算法，利用tanh函数的非线性特点结合损失函数变化值，使学习率自适应增大或减小，公式如下：

12、

13、η＝η(1+tanh(ep-ep+1))+α；

14、ep-ep+1表示为第p个和第p+1个样本损失函数的变化值，α为超参数；tanh函数值域为(-1,1)，因此学习率不会因为损失函数变化较大而产生过大波动；由于tanh函数调节，学习率会自适应增大，有效提升前期收敛速度，解决固定学习率前期收敛速度慢的问题；后期，随着损失函数值变化接近于0，tanh(ep-ep+1)值接近于0，学习率几乎以原步长速度逐渐收敛，避免学习率线性下降带来的后期步长过小、收敛速度慢或不收敛问题；考虑到部分情况存在学习率可能减小到非常小甚至是0的情况，使后期收敛变得慢，甚至陷入局部最优解，对此引入超参数α，使网络在训练后期，损失函数变化几乎为0，学习率可几乎稳定在某一值，这样可以到达更高的精度要求；

15、根据bp神经网络反向传播的原理，为了使得连接权值和阈值沿着损失函数e的梯度下降的方向改善需要依次计算δwih、δwhj、δθj、δγh，其中δwih为输入层第i个神经元和隐藏层第h个神经元的连接权值变化，δwhj为隐藏层第h个神经元和输出层第j个神经元的连接权值变化，δθj为输出层第j个神经元的阈值变化，δγh为输出层第h个神经元的阈值变化；

16、

17、

18、

19、

20、起初给每个权值和阈值设立初始值，通过学习率的不断调节神经网络经过收敛得到最优的权值和阈值。

21、进一步，步骤s3中基于视觉置信度的互补滤波融合方法如下：

22、在yolov5模型的视觉识别过程中，每个检测框都会被分配一个置信度conf表示该检测框中包含目标的概率，conf的值域为(0,1)，当检测框中包含目标的概率超过阈值时，该检测框被认为是有效的，并且其对应的类别标签和置信度将被输出；由于usbl在近距离对接时误差较大，置信度conf的大小在0到1之间；所以将视觉置信度作为权值调节参数求得两路数据的权重比值，这样就形成互补滤波融合得到最终的测量值；假设w1为视觉信息的权重比，则w1＝conf+λ，w2为usbl信息的权重比，则w2＝1-conf-λ，其中λ作为调节因子，可在不同环境对接时人为调节，在水质较为清澈的水域对接时将其调大，在水质较为混浊的水域内对接时将其调小；β0、d0为互补滤波融合后得到的最终测量数据；互补滤波融合公式为：

23、β0＝w1×β1+w2×β2

24、d0＝w1×d1+w2×d2

25、β1、β2分别为机器视觉与usbl得到的auv与回坞站的方向夹角，d1、d2为机器视觉与usbl得到的auv与回坞站的距离；

26、在融合过程中，如果视觉识别置信度为1，则输出结果就是视觉识别的数据；如果视觉识别置信度为0，则输出结果就是usbl的数据；如果usbl没有数据，则不管conf为多少，融合结果直接用视觉测得的来代替；如果视觉和usbl数据都没有，则将输出结果标记为不可用。

27、进一步，步骤s3中自适应卡尔曼滤波多信息融合过程如下：

28、以其中方位角数据β为例，常规一维卡尔曼滤波的系统状态方程和观测方程分别为：

29、xk＝axk-1

30、zk＝hxk

31、式中：xk为k时刻的系统状态，zk为k时刻的观测值；a为状态转移矩阵；h为测量系数参数矩阵；令自适应学习率bp神经网络拟合数据β3为系统状态带入为βk-，声光互补滤波融合数据β0为观测值带入为βzk，为了获得最优估计值βk，根据卡尔曼滤波算法获得以下五个递推公式，分别为：

32、先验估计状态变量

33、βk-＝aβk-1；

34、先验估计误差协方差

35、pk-＝apk-1at+q

36、卡尔曼增益

37、

38、数据融合方程

39、βk＝βk-+kk(βzk-hβk-)

40、数据融合误差协方差

41、pk＝(i-kkh)pk-

42、式中：βk-是第k个过程的先验估计状态变量，pk-是第k个过程的先验估计误差协方差；q是过程激励噪声协方差，由自适应学习率bp神经网络拟合解算得到，r是测量噪声协方差，由声光互补滤波融合解算得到；kk是第k个过程的卡尔曼增益，pk是第k个过程的滤波误差协方差；

43、引入自适应系数的过程中，需在原有计算变量的基础上加入新息协方差，新息是指实际传感器测量值与传感器预测值之差，用δk表示；实际传感器的测量值用机器视觉与usbl测量数据经过互补滤波融合后表示，传感器预测值用自适应学习率bp神经网络拟合值表示；首先，计算实际的新息协方差矩阵pf,k和预测的新息协方差矩阵pc,k，计算公式为

44、

45、随后，将上述2个协方差矩阵分别对角化为矩阵df,k和矩阵dc,k，通过计算两对角化矩阵关系的迹获得中间参数ka，计算公式为

46、

47、其中，trace为迹求解运算；

48、然后，设置基准参数值ke＝1，通过对比ka和ke得到自适应系数ηk，其对比方法为

49、

50、最后根据上式中得到的自适应系数ηk能够对卡尔曼滤波器中的协方差矩阵进行更新，更新方法为

51、pk＝(i-kkh)pk--ηkkkhpk-

52、当声光互补滤波融合得到的测量值出现较大误差时，r矩阵中的误差较大，估计值的置信程度高于测量值，滤波结果偏向于自适应学习率bp神经网络拟合出来的估计值；当自适应学习率bp神经网络拟合估计值出现较大误差时，q矩阵中的误差较大，测量值的置信程度高于估计值，滤波结果偏向于声光互补滤波融合得到的测量值。本发明引入自适应系数后，当机器视觉或usbl出现环境干扰导致位置导航数据跃变时(即实际传感器测量值跃变)，新息协方差δk会出现瞬时增大的现象，从而使实际的新息协方差矩阵pf,k和预测的新息协方差矩阵pc,k出现不同程度的变化，导致参数ka增大。通过对比ka和基准参数ke可以得到自适应系数ηk，ka增大且大于1时，ηk变为大于0的数值参与到协方差矩阵pk的计算中，通过pk的影响最终使得卡尔曼滤波系数矩阵kk中的数值降低，kk中数值下降使声光互补滤波融合数据的置信程度进一步下降，使得滤波结果βk偏向于bp神经网络拟合得到的数据。相比于常规的卡尔曼滤波算法，自适应系数卡尔曼滤波算法能够在其基础上，进一步减小融合误差并加快误差曲线的收敛性能，降低各个传感器数据跃变干扰对系统产生的影响。

53、进一步，步骤s1前还包括步骤s0，利用声视觉对水下电磁场分布进行训练拟合，基于usbl与机器视觉得到的数据获得auv相对于对接坞的方位角β与auv到对接坞的距离d，基于磁力仪可以获得磁场线夹角γ和当前磁场大小b，将γ和b作为自适应学习率bp神经网络的输入，β和d作为自适应学习率bp神经网络的输出进行训练，最终得到训练好的神经网络，该网络表征了对接坞附近的电磁场分布，即通过神经网络训练拟合了对接坞附近的电磁场分布。

54、进一步，步骤s0的具体步骤如下：

55、s01,首先采用数据自动采集运动控制auv采集用于训练的样本数据；

56、s02，在采集过程中，机器视觉与usbl都可以获得方位角与距离数据，将二者的数据通过基于视觉置信度的互补滤波融合生成最终的方位角β与距离d；磁力仪可以获得当前位置的磁场大小b与磁场线夹角γ，其中磁场线夹角γ则是由auv艏向角与对接坞水平朝向的夹角α减去auv艏向角与磁场线方向的夹角θ所得到；将磁场大小b与磁场线夹角γ作为自适应学习率bp神经网络的输入，方位角β与距离d作为输出，数据采集一次包含这4种数据则为有效数据，有效数据通过自适应学习率bp神经网络进行训练；

57、s03,摄像头获取与对接坞的方位角与距离可以用来判断auv是否已经靠近对接坞且位于中线处；若已经回坞，则自适应学习率bp神经网络开始训练同时auv开始充电，若没有回坞，则保持之前的控制策略继续采集数据。

58、进一步，步骤s01的auv数据自动采集运动控制策略是采用auv扇形区域扫描采集数据，具体步骤如下：

59、s011，auv先从对接坞释放出来到达起始点s，起始点位于对接坞一侧的斜前方，起始点相对于对接坞的方位角满足对接坞上的led灯正好可以被auv视觉识别，起始点相对于对接坞的距离处于auv既可以看到led灯又可以检测到对接坞磁场信号的安全范围的边界；

60、s012，auv采用视觉追踪策略沿着对接坞方向航行的同时采集数据，到达对接坞附近后折返，下次采集航行时则逐渐减小auv相对于对接坞的方位角，直到最后auv沿着对接坞中轴线采集数据；由于对接坞产生磁场具有对称性，auv采集训练数据只需要在对接坞中轴线的一侧进行即可；

61、s013，当auv在对接坞正前方且光标位于相机成像中心处时，auv数据采集完成，并回到对接坞，在坞站内自身开始充电并且完成自适应学习率bp神经网络的训练。

62、或者，步骤s01的auv数据自动采集运动控制策略是采用z字形平扫的方式进行数据采集，具体步骤如下：

63、s011＇，auv从对接坞正前方一定距离出发，auv始终朝着对接坞通过z字形平扫的方式进行数据采集；

64、s012＇，最终到达对接坞进行充电的时候就完成了数据采集。这样auv不用折返，要求auv具有全驱动能力，并且具备良好的动力定位性能。

65、进一步，步骤s01的样本数据可以定期增加，定期增加样本量的训练方法包括auv执行常规任务回坞对接时，若机器视觉能够识别光标且得到的测量数据有效时，auv也会开启数据采集功能，累计采集一段时间数据后用于自适应学习率bp神经网络后续的训练。这样水下电磁场分布模型在auv常规工作时也是不断更新完善的。

66、本发明与现有技术相比，其显著优点包括：

67、(1)采用了高效的数据采集运动控制策略，还可以定期采集新的训练数据，保证采集样本数据的多样性，并在auv对接充电时进行训练，不断自我完善和更新训练网络。此外，在平时回坞对接过程中，即非专门的训练过程中，若机器视觉能够识别光标且得到的测量数据有效时，auv也会开启数据采集功能，累计一段时间数据后用于bp神经网络后续的训练，从而不断更新神经网络模型，这样水下电磁场分布模型在auv常规工作时也是不断更新完善的。

68、(2)将机器视觉与usbl通过基于视觉置信度的互补滤波融合生成方位角与距离，可以有效避免环境水质清晰度影响导致视觉信息失效，提高多种传感器测量的准确性。在用水下声视觉对水下电磁场分布完成训练拟合之后，后面auv在回坞对接过程中即使丢失视觉目标或者usbl信号，也可以利用拟合好的电磁场分布进行定位，再加上电磁场线收敛于对接坞的特性，这使得在只有电磁导引的情况下auv也有条件完成回坞对接，从而实现极地高纬度条件下不依赖于惯导和罗盘的回坞导引，也不依赖于声学传感器(usbl信号可以没有)。

69、(3)将自适应学习率方法引入bp神经网络，利用tanh函数的非线性特点结合损失函数变化值，使学习率自适应增大或减小，同时会加入新的采集数据定时更新训练网络模型，这样更有利于得到网络收敛，对提高水下对接误差的精度，具有显著的作用。

70、(4)使用卡尔曼滤波可以有效克服传感器在水下数据采集时受到的外部环境影响以及传感器自身带来的误差，可以对数据进行实时的更新和处理，该方法使对接过程更加精准可靠。

71、(5)将自适应系数引入卡尔曼滤波多传感器信息融合算法，将光学、声学和电磁学所获得的数据进行融合得到最优估计值，可靠性差的数据自动获得比较小甚至为0的融合权重，通过自适应系数的作用能够修正卡尔曼滤波系数，进一步减小融合误差并加快误差曲线的收敛性能，降低各个传感器数据跃变干扰对系统产生的影响，从而提高对接导引精度，增强回坞导航系统的容错能力，实现不依赖于惯导和罗盘的auv末端回坞导航。