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

说明书

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

技术领域

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

背景技术

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

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

发明内容

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

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

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

步骤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)采用如下公式预测载体位置：