[发明专利]一种水下快速动基座对准方法及装置

权利要求书

1.一种水下快速动基座对准方法，其特征在于，包括以下步骤：

1)获取载体初始时刻的经度和纬度，载体上惯性导航系统输出的比力和角速度信息，以及多普勒测速仪输出的速度观测值；

2)在每个惯性导航更新周期内，根据惯性导航系统输出的角速度信息更新并根据多普勒输出的速度观测值更新其中，多普勒测速仪的更新周期长于惯性导航系统的更新周期，在每个多普勒测速仪更新周期内，载体的速度为恒定值；为载体坐标系b到惯性凝固载体坐标系的方向余弦矩阵，为初始时刻导航坐标系n₀到导航坐标系n的转换矩阵；

3)在每个多普勒测速仪的更新周期内，根据初始时刻的经度、纬度和时间信息更新并建立系统方程和观测方程，利用非线性滤波算法计算为惯性凝固导航坐标系到初始时刻导航坐标系n₀的转换矩阵，为惯性凝固载体坐标系到惯性凝固导航坐标系的方向余弦矩阵；

所述系统方程为：其中，x＝[α β γ]^T，α为方位角误差，β为俯仰角误差，γ为横滚角误差；

所述观测方程为：z＝(I-δC)V₂(t)；其中，z为观测量，δC为姿态误差矩阵，为t时刻载体坐标系b到惯性凝固载体坐标系的方向余弦矩阵，为多普勒载体坐标系d到载体坐标系b的方向余弦矩阵，为t时刻多普勒测速仪输出的速度、为t+Δt时刻载体坐标系b到惯性凝固载体坐标系的方向余弦矩阵、为t+Δt时刻多普勒测速仪输出的速度，t为时间变量，Δt为时间增量，f^b为惯性导航系统输出的比力；为矩阵当前计算值的转置，ω_ie为地球自转角速度，为地球自转角速度在导航坐标系n中的投影，为重力加速度在导航坐标系n中的投影，r_E为地球椭球的卯酉圈曲率半径，L表示纬度，为离心力；

4)根据更新的和计算当前的姿态矩阵并输出当前的姿态角信息；

5)重复步骤2)-4)，直至满足初始对准结束条件；

计算的过程包括：

A：在每个多普勒周期内根据建立的系统方程和观测方程，利用非线性滤波算法求α、β和γ；

B：根据α、β和γ，计算δC；

C：根据δC，利用更新将更新后的作为

所述非线性滤波算法为无迹卡尔曼滤波算法。

2.根据权利要求1所述的水下快速动基座对准方法，其特征在于，为的转置矩阵，的求解过程为：

1)设定初始时刻的

2)在每个惯性导航更新周期内，更新矩阵

在每个惯性导航更新周期，载体北向位置增量和东向位置增量为：

则当前的地理经纬度更新如下：

r_N为地球椭球子午圈曲率半径，矩阵的更新公式如下：

其中：

其中，k为惯性导航的周期数，t_k为第k个周期对应的时间，t_k+1为第k+1个周期对应的时间，r_E为地球椭球的卯酉圈曲率半径，L(t_k)为t_k时刻载体所在位置的纬度，λ(t_k)为t_k时刻载体所在位置的经度，为凝固惯性坐标系到t_k时刻的导航坐标系n_k的转换矩阵。

3.根据权利要求1所述的水下快速动基座对准方法，其特征在于，所述初始对准结束的条件为对准时间超过设定值，所述设定值为5-10分钟。