[发明专利]一种静基座条件下低成本陀螺全部零偏快速估计方法

权利要求书

1.一种静基座条件下低成本陀螺全部零偏快速估计方法，其特征在于，包括以下步骤：

(1)在载体静止条件下周期采集k时刻载体的惯性传感器数据，包括三轴陀螺信息三轴加速度计信息f^b(k)；

(2)根据k时刻的惯性传感器数据，预测k时刻载体的姿态、速度、位置信息；

(3)通过卡尔曼滤波器，估计k时刻三轴陀螺的零偏；

(4)跳转至步骤(1)，循环往复。

2.根据权利要求1所述的静基座条件下低成本陀螺全部零偏快速估计方法，其特征在于，步骤(2)所述的预测k时刻载体的姿态过程如下：

其中，q(k)＝[q₀(k) q₁(k) q₂(k) q₃(k)]^T为k时刻的姿态四元数，上标T表示矩阵的转置；

q(k-1)＝[q₀(k-1) q₁(k-1) q₂(k-1) q₃(k-1)]^T为k-1时刻的姿态四元数；ΔT为离散采样周期；

其中，为k时刻机体系相对于导航系的角速率在机体系上的分量；

为k-1时刻的导航系到机体系的姿态转移矩阵；

其中，为k-1时刻地球自转角速率在导航系上的分量，ω_ie为地球自转角速率，L(k-1)为k-1时刻的纬度；

其中，为k-1时刻导航系相对于地球系的角速度在导航系上的分量，为k-1时刻的速度在导航系东北方向上的分量，L(k-1)、h(k-1)为k-1时刻的纬度和高度，R_M、R_N为地球的子午圈、卯酉圈半径。

3.根据权利要求1所述的静基座条件下低成本陀螺全部零偏快速估计方法，其特征在于，步骤(2)所述的预测k时刻载体的速度过程如下：

其中，为k时刻的速度，为k时刻的速度在导航系东北天方向上的分量；

为k-1时刻的速度，为k-1时刻的速度在导航系东北天方向上的分量；

为k时刻的机体系到导航系的姿态转移矩阵；

gⁿ为地球重力加速度在导航系上的分量。

4.根据权利要求1所述的静基座条件下低成本陀螺全部零偏快速估计方法，其特征在于，步骤(2)所述的预测k时刻载体的位置过程如下：

其中，λ(k)、L(k)、h(k)为k时刻的经度、纬度和高度；λ(k-1)、L(k-1)、h(k-1)为k-1时刻的经度、纬度和高度；R_M、R_N为地球的子午圈、卯酉圈半径。

5.根据权利要求1所述的静基座条件下低成本陀螺全部零偏快速估计方法，其特征在于，所述步骤(3)包括以下步骤：

(31)计算状态量的一步预测值

其中：

其中，L为载体的纬度，为三轴加速度计的输出在导航系东北天方向上的分量，即为机体系到导航系的姿态转移矩阵，0_m×n为m×n的零矩阵，Φ_k，k-1为滤波器k-1时刻到k时刻的一步转移矩阵，为k-1时刻到k时刻的状态量一步预测值，为k-1时刻滤波器状态量的估计值，φ_E、φ_N、φ_U为东向、北向、天向平台误差角，δv_E、δv_N为东向、北向速度误差，为陀螺三轴零偏；

(32)计算一步预测均方误差P_k|k-1

其中，P_k|k-1为k-1时刻到k时刻的一步预测均方误差，P_k-1为k-1时刻的状态估计均方误差，上标T表示矩阵转置；

表示取矩阵的第m行，Γ_k-1为滤波器k-1时刻的系统噪声矩阵；

Q_k-1＝diag{ε_gx²，ε_gy²，ε_gz²，ε_ax²，ε_ay²，ε_az²}；

Q_k-1为k-1时刻的系统噪声，diag表示矩阵对角化，其中ε_gx、ε_gy、ε_gz分别为的模型噪声，ε_ax、ε_ay、ε_az分别为的模型噪声；

(33)计算k时刻卡尔曼滤波器的滤波增益K_k：

其中，

M₅＝[secθsinγΔT 0 -secθcosγΔT]；

其中，H_k为k时刻的量测矩阵，I_2×2为2×2的单位矩阵，γ、θ、分别为载体的横滚角、俯仰角、航向角：

其中，q₀、q₁、q₂、q₃为表示载体姿态的四元数；

其中，R_k为k时刻的量测噪声，diag表示矩阵对角化，分别为水平速度量测的噪声，为角度量测的噪声，上标-1表示矩阵求逆；

(34)计算k时刻卡尔曼滤波器的状态估计值

其中，为k时刻滤波器状态量的估计值，为k-1时刻到k时刻的状态量一步预测值，为k时刻的量测值，为k时刻的速度在导航系东北方向上的分量，为k时刻的航向，为k-1时刻的航向；

(35)计算k时刻卡尔曼滤波器的估计均方误差P_k|k：

P_k|k＝(I-K_kH_k)P_k|k-1

其中，P_k|k为k时刻的估计均方误差，I为单位矩阵；

(36)基于卡尔曼滤波器，通过量测值Z_k对状态量中的三轴陀螺零偏进行估计。