[发明专利]基于角加速度计/陀螺/加速度计的惯性导航解算方法

权利要求书

1.基于角加速度计/陀螺/加速度计的惯性导航解算方法，其特征在于，包括如下步骤：

步骤1，周期读取k时刻载体的传感器信息，包括三轴角加速度计输出三轴陀螺输出三轴加速度计输出

步骤2，根据k时刻的传感器信息，预测k时刻载体的角速度、姿态、速度、位置信息；具体过程如下：

1)采用如下公式预测载体角速度：

其中，为k时刻机体系相对于惯性系的角速度在机体系X、Y、Z轴上的分量，为k-1时刻机体系相对于惯性系的角速度在机体系X、Y、Z轴上的分量，为k时刻机体系相对于惯性系的角加速度在机体系X、Y、Z轴上的分量，ΔT为离散采样周期；

2)采用如下公式预测载体姿态：

其中，a＝x,y,z，q₀(k)、q₁(k)、q₂(k)、q₃(k)为k时刻的姿态四元数，q₀(k-1)、q₁(k-1)、q₂(k-1)、q₃(k-1)为k-1时刻的姿态四元数，为k时刻机体系相对于导航系的角速度在机体系X、Y、Z轴上的分量，为k-1时刻的姿态转移矩阵，为k-1时刻导航系相对于惯性系的角速度在导航系X、Y、Z轴上的分量；

3)采用如下公式预测载体速度：

其中，为k时刻载体速度在导航系X、Y、Z轴上的分量，为k-1时刻载体速度在导航系X、Y、Z轴上的分量，L(k-1)、h(k-1)为k-1时刻载体的纬度、高度，R_M、R_N为地球的子午圈、卯酉圈半径，为k时刻三轴加速度计在X、Y、Z轴上的输出分量，ω_ie为地球自转角速度，g为重力加速度；

4)采用如下公式预测载体位置：

其中，λ(k)、L(k)、h(k)为k时刻载体的经度、纬度与高度，λ(k-1)为k-1时刻载体的经度；

步骤3，通过卡尔曼滤波器，对k时刻载体的角速度进行校正。

2.根据权利要求1所述基于角加速度计/陀螺/加速度计的惯性导航解算方法，其特征在于，所述k-1时刻的姿态转移矩阵为：

其中，q₀(k-1)、q₁(k-1)、q₂(k-1)、q₃(k-1)为k-1时刻的姿态四元数。

3.根据权利要求1所述基于角加速度计/陀螺/加速度计的惯性导航解算方法，其特征在于，所述k-1时刻导航系相对于惯性系的角速度在导航系X、Y、Z轴上的分量为：

其中，为k-1时刻载体速度在导航系X、Y轴上的分量，L(k-1)、h(k-1)为k-1时刻载体的纬度、高度，R_M、R_N为地球的子午圈、卯酉圈半径，ω_ie为地球自转角速度，上标T表示转置。

4.根据权利要求1所述基于角加速度计/陀螺/加速度计的惯性导航解算方法，其特征在于，所述步骤3的具体过程如下：

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

P(k|k-1)＝A(k,k-1)P(k-1|k-1)A(k,k-1)^T+G(k-1)W(k-1)G(k-1)^T

其中，A(k,k-1)为滤波器k-1时刻到k时刻的滤波器一步转移矩阵，P(k-1|k-1)为k-1时刻的状态估计均方差，P(k|k-1)为k-1时刻到k时刻的一步预测均方差，G(k-1)为滤波器k-1时刻的滤波器噪声系数矩阵，W(k-1)为k-1时刻状态噪声，上标T表示转置；

2)计算k时刻扩展卡尔曼滤波器滤波增益K(k)：

K(k)＝P(k|k-1)H(k)^T[H(k)P(k|k-1)H(k)^T+R(k)]^-1

其中，K(k)为k时刻的滤波增益，H(k)为k时刻量测矩阵，R(k)为k时刻的量测噪声，上标-1表示求逆；

3)计算k时刻扩展卡尔曼滤波器状态估计值

其中，为k时刻状态量的估计值，为k-1到k时刻的状态变量一步预测值，Y(k)为k时刻的量测值，为k时刻三轴陀螺在X、Y、Z轴上的输出分量；

4)计算k时刻扩展卡尔曼滤波器估计均方误差P(k|k)：

P(k|k)＝[I-K(k)H(k)]P(k|k-1)

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

5)基于卡尔曼滤波器，通过三轴陀螺的输出Y(k)对状态中的角速度进行校正。