[发明专利]水下定位方法、水下载体航行器及计算机可读存储介质

说明书

本发明公开了一种水下定位方法、水下载体航行器及计算机可读存储介质，所述水下定位方法包括：确定所述惯性测量单元在第一预设坐标系下的第一位姿数据；根据所述第一位姿数据以及所述惯性测量单元检测到的惯性数据，确定目标线加速度值以及目标角速度值；根据所述目标线加速度值以及所述目标角速度值，确定所述惯性测量单元在第二预设坐标系下的第二位姿数据；根据所述第一位姿数据、所述第二位姿数据以及预设误差状态代价函数确定水下定位信息，所述预设误差状态代价函数的参数包括所述惯性测量单元对应的第一残差参数以及超短基线传感器对应的第二残差参数，能够提升水下载体航行器的定位信息的准确性。

技术领域

本发明涉及水下导航定位技术领域，尤其涉及一种水下定位方法、水下载体航行器及计算机可读存储介质。

背景技术

在进行水下感知时，需要采用声呐作为视觉感知工具，声呐的分辨力较低，因此在进行图像拼接时需要水下载体航行器的定位信息辅助拼接，定位信息的准确性影响图像拼接的准确性，而水下载体航行器在进行定位时，实时性存在误差抖动现象，从而使得定位信息本身不准确，本发明所要解决的技术问题即为水下载体航行器的定位信息不准确。

发明内容

本发明的主要目的在于提供一种水下定位方法、水下载体航行器及计算机可读存储介质，旨在解决水下载体航行器的定位信息不准确的技术问题。

为实现上述目的，本发明提供一种水下定位方法，所述水下定位方法应用于水下载体航行器，所述水下载体航行器包括惯性测量单元以及超短基线传感器，所述水下定位方法包括：

确定所述惯性测量单元在第一预设坐标系下的第一位姿数据；

根据所述第一位姿数据以及所述惯性测量单元检测到的惯性数据，确定目标线加速度值以及目标角速度值；

根据所述目标线加速度值以及所述目标角速度值，确定所述惯性测量单元在第二预设坐标系下的第二位姿数据；

根据所述第一位姿数据、所述第二位姿数据以及预设误差状态代价函数确定水下定位信息，所述预设误差状态代价函数的参数包括所述惯性测量单元对应的第一残差参数以及超短基线传感器对应的第二残差参数，所述超短基线传感器用于检测位姿修正数据，所述位姿修正数据用于确定所述第二残差参数。

可选地，所述根据所述第一位姿数据、所述第二位姿数据以及预设误差状态代价函数确定水下定位信息的步骤包括：

根据所述第一位姿数据以及所述第二位姿数据，确定所述惯性测量单元的第一残差值，所述第一残差值与所述第一残差参数对应；

获取超短基线传感器检测到的第二残差值，所述第二残差值与所述第二残差参数对应；

根据所述第一残差值以及所述第二残差值，确定所述预设误差状态代价函数的最小值；

根据所述最小值确定所述水下定位信息。

可选地，所述第一位姿数据包括所述惯性测量单元的线速度，所述第二位姿数据包括第二位置以及第二方向，所述根据所述第一位姿数据以及所述第二位姿数据，确定所述惯性测量单元的第一残差值的步骤包括：

根据所述线速度、第一预设角速度以及第二预设角速度，确定第一变量，所述第一预设角速度为第一自由度的角速度，所述第二预设角速度为第二自由度的角速度，所述第一自由度以及所述第二自由度为参考重力的自由度分量，所述第一自由度与所述第二自由度不同，且所述第一自由度与所述第二自由度相互垂直，所述第一预设角速度以及所述第二预设角速度为所述惯性测量单元的角速度的分量；

根据所述第一自由度、所述第二自由度以及所述第二位置，确定矩阵系数；

根据所述第一变量以及所述矩阵系数，确定第二变量；

根据所述第一变量以及所述第二变量，确定所述惯性测量单元的第一残差值。