用于水下导航模型训练的数据采集与构建方法及存储介质

本发明属于水下航行器领域，涉及水下机器人智能导航技术，具体涉及对水下机器人的水下导航进行模型训练的数据采集与构建以及相应的存储介质。

背景技术：

1、作为人类对海洋的开发与探索工作中重要装备，水下航行器因具备精确获取海底地形地貌、海洋气象、水文地质、电磁场、声学特性等水下信息能力，具备长时间进行水下工作的条件得到了迅速发展。高精度的导航系统作为水下航行器的“眼睛”，能为其提供位置、速度和姿态信息，这些信息对于水下航行器的安全运行与回收至关重要，是自主式水下航行器发展的基本挑战。

2、水下环境复杂且信息源缺乏，为了安全、高效地执行各种水下任务，位姿信息的精准度和可靠性显得尤为重要。传统导航方法以惯性测量单元(imu)或惯性导航系统(ins)的信息为基准，辅以多普勒测速仪(dvl)的速度信息、深度计(ips)的深度信息，全球定位系统(gps)的位置信息、超短基线(usbl)的位置等信息进行导航位置推算。导航系统的积分解算原理使其位置误差不断增加，无线电波在水下的迅速衰减使gps在水下无法使用，声波在水中的传输规律使usbl在水下的传输距离受限。因此，水下航行器在水下执行长航时、远距离的任务时，导航系统的位置误差不断累积，位置修正困难。

3、由于水下航行器导航系统的位置误差呈发散趋势，可以认为历史数据的精度总是高于当前时刻，探讨历史数据辅助水下机器人导航的方法有望提升导航系统定位精度。现有技术多采用位置回溯方法或神经网络方法进行历史数据的复用，但位置回溯法多基于之前时刻的几个位置与当前时刻位置进行加权平均，用到的历史数据较少，并且该方法在水下机器人上浮、下潜、转向、翻转等大机动情况下对导航系统精度提升不大，甚至可能是负面影响；根据不同传感器的输出频率对不同更新频率的数据分组进行神经网络学习，以期借鉴历史数据提高导航系统的定位精度，该类方法受制于gps、dvl、usbl等数据的输出频率，同时，基于递归滤波的导航算法只能对上述数据有更新的时刻进行位姿校正，对无数据更新时刻的导航数据无法进行位姿校正，导航系统的定位精度提升频率较低且不够稳定。

技术实现思路

1、发明目的：针对现有技术存在的问题，本发明提供一种用于水下导航模型训练的数据采集与构建方法及存储介质，实现大尺度环境下导航系统的全局优化，生成精度更高、输出频率更快的位置数据。

2、本发明用于水下导航模型训练的数据采集与构建方法采用的技术方案是：将全球定位系统、多普勒测速仪、惯性导航系统、超短基线定位系统、深度计多个传感器以及螺旋桨、方向舵分别连接处理器，还包括以下步骤：

3、步骤1)：处理器对多个传感器进行安装误差标校，多普勒测速仪获取的对底速度是经过误差标校过的对底速度；

4、步骤2)：水下航行器根据预设路径运行，处理器获取各传感器的数据包并同步解析数据；

5、步骤3)：处理器对全球定位系统获取的二维经纬度数据进行野值剔除，匹配超短基线定位系统数据的位姿点，对多普勒测速仪获取的三维对底速度进行速度数据的补充：判断多普勒测速仪提供的三维对底速度和当前对底深度数据是否更新，若更新，则综合影响三维对底速度的变量与对底速度数据共同基于剪枝极限学习机进行设定周期的在线学习，在线构建智能速度模型；若无更新，则基于当前时刻最新的智能速度模型生成当前时刻对底速度，用于无更新时的数据补充；

6、步骤4)：根据多普勒测速仪获取的对底速度计算机载坐标系下艏向和右舷向的位移x，y，状态向量为xk＝[x,y,ξz]k，ξz为水下机器人与北向的夹角；基于所有的状态向量、所有的全球定位系统的位置点集合、所有的超短基线定位系统位置点集合、所有的控制向量集合u＝{ui},i＝1，2，…，m…,i时刻的控制向量为ui＝{ax，ay，wx，wy},ax和ay分别为前向和右舷向的加速度，wx和wy分别为前向和右舷向的角速度，所有的观测值集合z＝{zi}，zi＝[xk，yk，h]，xk和yk分别为北向和东向的位置，h为航向角，构成信度网模型；

7、步骤5)：将信度网模型的求解转化为非线性最小二乘问题，用最速下降法和高斯-牛顿的方法进行线性转化，得到最速下降法的单步步长和单步位移以及高斯-牛顿的单步步长和单步位移，根据两个所述的单步步长与信任区域的关系得到对应的线性化点，对过程模型和量测模型在线性化点上进行泰勒一阶展开，将所述的非线性最小二乘问题转化为线性最小二乘问题；

8、步骤6)：对线性最小二乘问题中的雅可比矩阵系数使用qr分解、吉文斯旋转、增量因式分解和回代法进行求解，根据实际导航情况设置进行周期性变量重排和全局重新线性化的一个阈值，重排后重复步骤5)的线性转化，再重新使用qr分解、吉文斯旋转、增量因式分解和回代法计算全局解，得到带有全球定位系统和超短基线定位系统位置修正的导航定位信息，经计算得到带有修正信息的导航系统单步位移；

9、步骤7)：将全球定位系统、多普勒测速仪、惯性导航系统、超短基线定位系统、深度计获取的数据以及螺旋桨、方向舵提供的信息作为数据集的输入，将步骤6)得到的带有修正信息的导航系统单步位移作为数据集的输出，构建导航数据集。

10、进一步地，步骤4)中，根据多普勒测速仪提供的当前对底速度、深度计ips提供的当前水下机器人位置与水面之间的距离、全球定位系统gps提供的二维经纬度，超短基线定位系统提供的auv位置，在水面航行时，以修正的二维经纬度为基准对当前对底速度进行在线标校，在水下探测时，以超短基线定位系统提供的位置数据为基准在线标校。

11、进一步地，步骤4)中，先判断当前k时刻是否有新的测量值；若有新的测量值，则根据多普勒测速仪的速度计算机载坐标系下艏向和右舷向的位移x，y，若无新的测量值，则结束导航；

12、再判断此时的全球定位系统位置是否有更新，若有更新，则位置为gk＝[utmn，k-utmn，0，utme，k-utme，0]，utmn，k和utme，k为当前gps的经纬度坐标经转换得到的utm北向和东向位移，utmn，0和utme，0为导航系统0时刻的gps经纬度坐标转换得到的utm北向和东向位移；若位置无更新，则gk＝[]；

13、最后判断超短基线定位系统位置是否有更新，有更新，则根据飞行时间换算该位置所处时刻k-m，并得到当时的位置为lk-m＝[xl，k-m-xl，0，yl，k-m-yl，0]，xl，k-m和yl，k-m分别为获取的k-m时刻通过usbl获取的北向和东向位移，xl，0和yl，0为0时刻的北向和东向位移；若无更新，则lk＝[]。

14、本发明用于水下导航模型训练的存储介质采用的技术方案是：所述存储介质存储有计算机程序，计算机程序由处理器加载并执行用于水下导航模型训练的数据采集与构建方法。

15、本发明采用上述技术方案后显示的技术效果是：

16、1、本发明综合考虑了gps、dvl、usbl等辅助传感器的输出频率和数据精度，数据集的输入中dvl速度基于速度模型进行数据补充，补足dvl速度更新率低影响导航精度的短板，数据集的输出变量包含gps和usbl，对全局的位置修正，将gps和usbl的位置校正信息均匀地加到每一个时刻的位移中，规避了gps和usbl数据更新频率低的弊端，规避了递归滤波方法只能对有位置更新时刻的位姿进行校正的弊端，相较于滤波算法只能对有gps更新的时刻进行位置修正具有明显优势。基于上述带有修正信息的数据集，后续可以基于人工智能算法进行学习，形成离线的位置校正模型，亦可基于上述方法构建在线数据集，形成在线的位置校正模型，基于在线的位置校正模型辅助滤波或者优化导航，可应用于水下机器人导航控制等诸多领域，具有广泛的应用前景。

17、2、本发明基于所有观测信息构建图模型，以实现大尺度环境下导航系统的全局优化，生成了精度更高、输出频率更快的位置数据。