[发明专利]基于DVL辅助和向量截断化K矩阵的水下载体初始对准方法

权利要求书

1.一种基于DVL辅助和向量截断化K矩阵的水下载体初始对准方法，其特征在于：所述方法包括以下步骤：

(1)定义解算所需的参考标准坐标系；

(2)获取惯性传感器实时数据以及DVL实时数据；

(3)根据载体的比力方程将比力方程两边同时积分，再忽略小量得到如下表达式：进而构造观测矢量ξ和参考矢量r；然后根据步骤(2)获取的惯性传感器实时数据和DVL实时数据，来解算当前时刻的观测矢量ξ和参考矢量r；

所述观测矢量ξ和参考矢量r的构造方法如式(1)至式(4)所示：

(3.1)构造观测矢量ξ：

式中，V^b(0)表示初始时刻DVL采集到的速度矢量，V^b(t)表示t时刻DVL采集到的速度矢量；表示载体坐标系b从初始时刻到t时刻的姿态转移矩阵，b0表示载体坐标系b的初始时刻；f^b表示在载体坐标系中测量到的比力矢量；表示地球自转角速率在导航坐标系中的投影；×表示求该向量的反对称矩阵；ξ为规范化后的观测矢量；β表示直接由比力方程得到的未规范化的观测矢量；

(3.2)构建参考矢量r：

式中，表示从初始时刻到t时刻的导航坐标系n的姿态转移矩阵；n0表示导航坐标系n的初始时刻；gⁿ表示当地重力矢量；r为规范化后的参考矢量；α表示由比力方程得到的未规范化的参考矢量；

(4)利用步骤(3)中解算的观测矢量ξ和参考矢量r来构建向量截断化后的K矩阵，写出截断化后的K矩阵对应的状态方程和量测方程，并采用卡尔曼滤波的方法对K矩阵进行更新；具体过程为：

在卡尔曼滤波开始前，利用步骤(3)解算的观测矢量ξ和参考矢量r来构建K矩阵，并利用K矩阵的零迹性和对称性将K矩阵向量化后再截断成9维的状态向量初值x₀；

卡尔曼滤波开始后，步骤(3)解算的观测矢量ξ和参考矢量r用来构建δK矩阵，同样利用δK矩阵的零迹性和对称性将δK矩阵向量化后再截断成9维的量测向量y，用于对K矩阵进行更新；

(5)从步骤(4)中更新后的K矩阵中提取最优四元数，将最优四元数转化成姿态矩阵方程，再利用姿态矩阵链式方程解算出当前时刻的姿态矩阵，从中计算出失准角，完成初始对准过程。

2.根据权利要求1所述的基于DVL辅助和向量截断化K矩阵的水下载体初始对准方法，其特征在于：步骤(2)中，所述惯性传感器实时数据包括水下自主潜航器的加速度和角速率信息，DVL实时数据包括水下自主潜航器的线速度信息。