CN110207697A - 基于角加速度计/陀螺/加速度计的惯性导航解算方法 - Google Patents

Abstract

本发明公开了基于角加速度计/陀螺/加速度计的惯性导航解算方法，设计了一种新的惯性导航方案，其采用角加速度计、陀螺、加速度计传感器对载体的角速度、姿态、速度、位置信息进行预测，通过卡尔曼滤波器对载体的角速度、姿态、速度、位置信息进行解算。其中，角加速度计、加速度计用于更新状态方程，陀螺用于更新量测方程。相对于传统的惯性导航方案，本发明通过引入角加速度计，可以提高载体高动态情况下的导航解算精度。

Description

基于角加速度计/陀螺/加速度计的惯性导航解算方法

技术领域

本发明涉及基于角加速度计/陀螺/加速度计的惯性导航解算方法，属于惯性导航技术领域。

背景技术

惯性导航是一种常见的导航方式，其采用惯性器件，通过递推式的导航方式，对载体的姿态、速度、位置进行解算。惯性导航具有自主性强、不受外界干扰、输出信息全的优势，在航空、航天、航海中具有广泛的应用。

传统的惯性导航方案采用的惯性器件为陀螺与加速度计，其中陀螺测量的是角速度信息，加速度计测量的是加速度信息。当载体进行高动态动作时，陀螺的带宽受限、采样延迟会带来误差，从而影响导航精度。

发明内容

本发明所要解决的技术问题是：提供基于角加速度计/陀螺/加速度计的惯性导航解算方法，通过引入角加速度计，形成一种新的惯性导航方案，并设计相应的惯性导航算法，提高载体在高动态环境下的导航精度。

本发明为解决上述技术问题采用以下技术方案：

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

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

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

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

作为本发明的一种优选方案，所述步骤2的具体过程如下：

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时刻载体的经度。

作为本发明的一种优选方案，所述k-1时刻的姿态转移矩阵为：

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

作为本发明的一种优选方案，所述k-1时刻导航系相对于惯性系的角速度在导航系X、Y、Z轴上的分量为：

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

作为本发明的一种优选方案，所述步骤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)对状态中的角速度 进行校正。

本发明采用以上技术方案与现有技术相比，具有以下技术效果：

本发明可以降低载体在高动态环境下由于陀螺带宽受限、采样延迟引起的角速度估计误差，提高角速度、姿态估计精度。

附图说明

图1是本发明基于角加速度计/陀螺/加速度计的惯性导航解算方法的流程示意图。

图2、图3、图4分别是采用传统惯性解算方法和本发明方法时飞行器在航迹飞行状态下的X轴、Y轴、Z轴角速度误差。

图5、图6、图7分别是采用传统惯性解算方法和本发明方法时飞行器在航迹飞行状态下的横滚角、俯仰角、航向角误差。

具体实施方式

下面详细描述本发明的实施方式，所述实施方式的示例在附图中示出。下面通过参考附图描述的实施方式是示例性的，仅用于解释本发明，而不能解释为对本发明的限制。

如图1所示，为本发明方法的流程图，其具体步骤如下：

步骤一：周期读取k时刻的传感器信息，包括三轴角加速度计 三轴陀螺三轴加速度计

步骤二：预测k时刻载体的角速度、姿态、速度、位置信息：

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

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

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

式中，

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轴上的分量，为k-1时刻载体速度在导航系X、Y、Z轴上的分量，L(k-1)、h(k-1)为k-1时刻载体的纬度和高度，R_M、R_N为地球的子午圈与卯酉圈半径，ω_ie为地球自转角速度；

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

其中，为k时刻载体速度在导航系X、Y、Z轴上的分量；

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

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

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

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时刻的滤波器一步转移矩阵，I_3×3为3×3的单位矩阵，0_3×3为3×3的零矩阵，0_3×4为3×4的零矩阵，0_4×3为4×3的零矩阵，

P(k-1|k-1)为k-1时刻的状态估计均方差，P(k|k-1)为k-1时刻到k时刻的一步预测均方差，G(k-1)为滤波器k-1时刻的滤波器噪声系数矩阵， W＝[ε_rx ε_ry ε_rz ε_ax ε_ay ε_az]^T为k-1时刻状态噪声，ε_rx、ε_ry和ε_rz分别为和的模型噪声，ε_ax、ε_ay和ε_az分别为和的模型噪声，上标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

式中，H(k)＝[I_3×3 0_3×4 0_3×3 0_3×3]，H(k)为k时刻量测矩阵，K(k)为k时刻的滤波增益，R(k)＝diag([ε_ωx ε_ωy ε_ωz]²)为k时刻的量测噪声，diag表示矩阵对角化，其中ε_ωx、ε_ωy、ε_ωz分别为的噪声，上标-1表示求逆；

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

式中，为k时刻状态量的估计值，为k-1到k时刻的状态变量一步预测值，使用步骤二的预测公式计算得到，为k时刻的量测值，通过步骤一的传感器数据读取获得；

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)对状态中的角速度进行校正。

实施例：

采用仿真的形式，对使用本发明方法后的飞行器导航精度进行验证。其中惯性传感器精度设置如下：角加速度计零偏稳定性为150deg/s²，陀螺零偏稳定性为0.01deg/h，加速度计零偏稳定性为30μg。角加速度计、陀螺与加速度计采样频率均为400Hz，陀螺截止频率为100Hz。

令飞行器从地面加速滑跑起飞，在空中依次分别绕飞机的三个轴做大机动旋转运动，依次为各0.5s的正负90°横滚运动，各0.5s的正负45°俯仰运动，0.5s的正向90°航向运动。

图2、图3以及图4为采用传统方法、本发明方法时飞行器在航迹飞行状态下的角速度误差。

图5、图6以及图7为采用传统方法、本发明方法时飞行器在航迹飞行状态下的姿态误差。

以上实施例仅为说明本发明的技术思想，不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明保护范围之内。

Claims (5)

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

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

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

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

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

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

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

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

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

5.根据权利要求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)对状态中的角速度 进行校正。