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

权利要求书

1.一种水下定位方法，其特征在于，所述水下定位方法应用于水下载体航行器，所述水下载体航行器包括惯性测量单元以及超短基线传感器，所述水下定位方法包括：

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

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

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

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

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

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

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

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

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

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

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

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

2.如权利要求1所述的水下定位方法，其特征在于，所述惯性测量单元包括陀螺仪，所述惯性数据包括所述陀螺仪检测的角速度数据以及加速度计检测到的物体加速度，所述第一位姿数据包括第一方向，所述根据所述第一位姿数据以及所述惯性测量单元检测到的惯性数据，确定目标线加速度值以及目标角速度值的步骤包括：

根据所述第一方向以及第一旋转矩阵值，确定陀螺仪偏置值，所述第一旋转矩阵值为所述陀螺仪在超短基线传感器的相邻帧之间的旋转矩阵值；

根据所述角速度数据以及所述陀螺仪偏置值，确定所述惯性测量单元在第二预设坐标系下的目标角速度值；

根据所述物体加速度、加速度偏置值、目标重力以及第二旋转矩阵，确定所述目标线加速度值。

3.如权利要求2所述的水下定位方法，其特征在于，所述根据所述第一方向以及第一旋转矩阵值，确定陀螺仪偏置值的步骤包括：

根据所述第一旋转矩阵值、所述第一旋转矩阵值的预设修正系数以及所述第一方向，确定预设损失函数的最小值，将所述预设损失函数的最小值作为所述陀螺仪偏置值。

4.如权利要求2所述的水下定位方法，其特征在于，所述根据所述物体加速度、加速度偏置值、目标重力以及第二旋转矩阵值，确定所述惯性测量单元在第二预设坐标系下的线加速度的步骤之前，还包括：

确定所述参考重力的所述第一自由度以及所述第二自由度；

确定所述第一自由度对应的第一角速度以及所述第二自由度对应的第二角速度；

根据所述第一自由度、所述第二自由度、所述第一角速度以及所述第二角速度，确定所述目标重力。

5.如权利要求2所述的水下定位方法，其特征在于，所述根据所述角速度数据以及所述陀螺仪偏置值，确定所述惯性测量单元在第二预设坐标系下的目标角速度值的步骤以及所述根据所述物体加速度、加速度偏置值、目标重力以及第二旋转矩阵，确定所述目标线加速度值的步骤之间，还包括：

对所述目标角速度值进行积分，将积分后的所述目标角速度值作为所述第二旋转矩阵。