CN104567873B - 针对高动态载体应用的高精度捷联惯导姿态角确定方法 - Google Patents

Abstract

本发明公开了针对高动态载体应用的高精度捷联惯导姿态角确定方法，属于捷联惯导姿态自适应的技术领域。以陀螺采样周期更新α方向角增量、β方向角增量，以机体系更新周期更新机体系方向余弦矩阵或者机体系四元数，以导航系更新周期更新导航系方向余弦矩阵或者导航系四元数，最后由当前时刻的姿态转换矩阵或者姿态四元数解算姿态角。本发明中，机体系旋转更新变化与陀螺采样时间不需要保持一致，导航系旋转变化更新不需要与陀螺采样时间、机体系旋转更新保持一致，减少了计算量，提高了计算效率；无需分别对不同子样数的圆锥补偿式进行推导，从而提高了圆锥补偿算法的适应性。

Description

针对高动态载体应用的高精度捷联惯导姿态角确定方法

技术领域

本发明公开了针对高动态载体应用的高精度捷联惯导姿态角确定方法，属于捷联惯导姿态自适应的技术领域。

背景技术

在捷联惯导系统中，姿态更新算法相对速度位置更新算法更为复杂，算法的解算精度和实时性也是影响捷联惯导姿态解算性能的两个主要因素。特别是在高超飞行器等高动态载体应用中，由于载体的位姿运动特性极其复杂，为保证导航结果能够真实反映高动态载体的运动情况，其对捷联解算的实时性和解算精度都有更高的要求。在高动态惯性捷联姿态算法研究和编排方面，为了提高捷联姿态解算的精度，需要充分利用惯性器件陀螺仪的数据，通常通过提高陀螺仪数据的采样率来提高算法精度。

传统的捷联指北姿态算法，其基本思想是采用等效旋转矢量的方向余弦法或四元数法进行姿态解算，并采用角速度输入或角速率输入的多子样圆锥补偿算法，以及四阶龙格库塔算法等姿态修正算法提高动态情况下的姿态解算精度。但此类算法会随着惯性器件采样率的提高，其就量急剧增加，从而使得导航处理器在提高算法精度的同时，难以满足高动态载体高频率导航输出模式下的实时导航需求。

发明内容

本发明所要解决的技术问题是针对传统捷联指北姿态算法中器件采样率的增加会会导致算法计算量的大幅增加，影响算法的实时性，从而无法满足高速高动态载体的高精度实时导航需求的问题进行设计，在传统捷联指北姿态算法的编排基础上，提供了针对高动态载体应用的高精度捷联惯导姿态角确定方法，以有效满足高速高动态载体的实时性导航解算需求。

本发明为实现上述发明目的采用如下技术方案：

针对高动态载体应用的高精度捷联惯导姿态角确定方法，包括如下步骤：

将与陀螺仪输出相关的机体系旋转矢量φ分解为角增量α与角增量β的和，以陀螺采样周期T_k更新机体系旋转矢量φ，第k次迭代得到的的角增量α、角增量β记为α(k)、β(k)，利用陀螺仪旋转角速率计算第k次迭代的角增量α的变化量Δα(k)，再由第k-1时刻t_k-1的α角增量α(k-1)迭代计算第k时刻t_k的α角增量α(k)，α(k)＝α(k-1)+Δα(k)，再由第k-1时刻t_k-1的α角增量α(k-1)、第k-1次迭代的角增量α的变化量Δα(k-1)和第k次迭代的角增量α的变化量Δα(k)计算第k次迭代的角增量β的变化量Δβ(k)，最后由T_m＝kT_k，经过k次迭代计算得到第k时刻t_k的β角增量β(k)，β(k-1)为第k-1时刻t_k-1的β角增量，以机体系更新周期T_m更新机体系方向余弦矩阵或者机体系四元数其中，T_m＝kT_k，k为大于1的整数，

以导航系更新周期T_n更新导航系方向余弦矩阵或者导航系四元数其中，T_n＝MT_m，M为大于1的整数，

由前一时刻t_n-1的姿态转换矩阵当前时刻t_n的机体系方向余弦矩阵当前时刻t_n的导航系方向余弦确定当前时刻t_n的姿态转换矩阵由前一时刻t_n-1的姿态四元数当前时刻t_n的机体系四元数计算的前一时刻t_n-1的导航系四元数确定当前时刻t_n的姿态四元数最后采用链乘法则计算当前时刻t_n的姿态转换矩阵或者姿态四元数进而解算得到横滚角γ、俯仰角θ、航向角

作为所述针对高动态载体应用的高精度捷联惯导姿态角确定方法的进一步优化方案，利用方向余弦矩阵或四元数相乘法则更新下一时刻的机体系方向余弦矩阵或者机体系四元数：由α角增量α(m)、β角增量β(m)计算机体系旋转矢量φ(m)，φ(m)＝α(m)+β(m)，以机体系更新周期T_m为计算周期，利用机体系旋转矢量φ_m通过毕卡逼近法更新机体系方向余弦矩阵或四元数

I为单位矩阵，φ(m)×表示φ(m)的斜对称矩阵形式，依据T_n＝MT_m，计算第m-1时刻、第m时刻、第m+1时刻、第m+M-1时刻、第n时刻的机体系方向余弦矩阵或者第m-1时刻、第m时刻、第m+1时刻、第m+M-1时刻、第n时刻的机体系四元数 (共计M次)，则有，

第n时刻的机体系方向余弦矩阵

第n时刻的机体系四元数

作为所述针对高动态载体应用的高精度捷联惯导姿态角确定方法的进一步优化方案，根据导航系速度v^N以及当前时刻的位置经度λ、纬度L、高度h计算得到的导航系旋转角速率再对导航系旋转角速率积分得到当前时刻t_n的导航系旋转更新矢量ξ^N，ξ^N×为ξ^N的斜对称矩阵形式，最后利用当前时刻t_n的导航系旋转更新矢量ξ^N通过毕卡逼近法更新导航系方向余弦阵或四元数则有，

当前时刻的导航系方向余弦矩阵 当前时刻的导航系四元数

本发明采用上述技术方案，具有以下有益效果：

1、机体系旋转更新变化与陀螺采样时间不需要保持一致，减少了计算量，导航系旋转变化更新不需要与陀螺采样时间、机体系旋转更新保持一致，进一步减少了计算量，提高了计算效率；

2、经过k次迭代计算得到α角增量α(m)、β角增量β(m)，与传统的多子样角增量或角速率圆锥补偿算法不同，无需分别对不同子样数的圆锥补偿式进行推导，从而提高了圆锥补偿算法的适应性。

附图说明

图1为本发明的原理框图。

图2为陀螺采样周期T_k、机体系更新周期T_m、导航系更新周期T_n之间的时间关系图。

图3为本发明与传统指北姿态算法计算效率的对比曲线图。

具体实施方式

下面结合附图对发明的技术方案进行详细说明。

针对高动态载体应用的高精度捷联惯导姿态角确定方法的原理框图如图1所示，主要包括四个步骤：

(1)以陀螺采样周期T_k更新α角增量α(m)、β角增量β(m)：将α(m)和β(m)的计算划分为若干个周期的α(k)和β(k)进行计算，计算周期为陀螺采样周期T_k，首先计算第k-1时刻α、β角增量变化量α(k)、β(k)，再依据T_m＝kT_k，其中，k取值为k≥1的正整数，经过k步迭代计算后得到α(m)和β(m)，即，

α(k)＝0(t_k＝t_m-1),α(m)＝α(k)(t_k＝t_m)，

β(k)＝0(t_k＝t_m-1),β(m)＝β(k)(t_k＝t_m)，

陀螺采样周期T_k、机体系更新周期T_m、导航系更新周期T_n之间的时间关系如图2所示，这与传统的多子样角增量或角速率圆锥补偿算法不同，无需分别对不同子样数的圆锥补偿式进行推导，从而提高了圆锥补偿算法的适应性；

(2)以机体系更新周期T_m更新机体系方向余弦矩阵或者机体系四元数其中，T_m＝kT_k，k为大于1的整数，计算M次机体系方向余弦矩阵或机体系四元数，每次计算中都利用方向余弦阵或四元数相乘法则，即前一时刻机体系方向余弦矩阵或机体系四元数与当前机体系方向余弦矩阵或机体系四元数相乘得到下一时刻的机体系方向余弦矩阵或机体系四元数，与传统指北姿态算法相比，机体系的旋转更新变化不需要与陀螺的采样时刻保持一致，一定程度上减少了算法的计算量；

(3)以导航系更新周期T_n更新导航系方向余弦矩阵或者导航系四元数其中，T_n＝MT_m，M为大于1的整数，首先利用地球自转角速率ω_ie、当前时刻的导航系速度v^N以及当前时刻的位置λ,L,h计算导航系旋转角速率然后利用积分得到导航系旋转更新矢量ξ^N，最后利用旋转矢量ξ^N通过毕卡逼近法更新导航系方向余弦阵或四元数与传统指北姿态算法相比，导航系的旋转变化更新不需要与陀螺的采样时刻、机体系的旋转变化更新保持一致，进一步减少了算法的计算量；

(4)在更新周期T_n中，首先利用步骤(2)中计算得到的机体系方向余弦阵或四元数前一时刻的姿态方向余弦阵或四元数以及步骤(3)中计算得到的导航系方向余弦阵或四元数根据方向余弦阵或四元数相乘法则计算出当前时刻或即 其中,和为前一时刻的姿态方向余弦阵和四元数最后利用当前时刻的姿态方向余弦阵或解算得到姿态角。

由图3所示的本发明与传统指北姿态算法计算效率的对比曲线图，可知，本发明这种改进型双速计算时间明显少于传统的指北算法，效率更高。

Claims (4)

1.针对高动态载体应用的高精度捷联惯导姿态角确定方法，其特征在于，包括如下步骤：

将与陀螺仪输出相关的机体系旋转矢量φ分解为角增量α与角增量β的和，以陀螺采样周期T_k更新机体系旋转矢量φ，第k次迭代得到的角增量α、角增量β记为α(k)、β(k)，

α(k)＝α(k-1)+Δα(k)，

其中，α(k-1)为第k-1时刻t_k-1的角增量α，Δα(k)为第k时刻t_k角增量α的变化量，β(k-1)为第k-1时刻t_k-1的角增量β，Δα(k-1)为第k-1时刻t_k-1角增量α的变化量，

以机体系更新周期T_m更新机体系方向余弦矩阵或者机体系四元数其中，T_m＝kT_k，k为大于1的整数，

以导航系更新周期T_n更新导航系方向余弦矩阵或者导航系四元数其中，T_n＝MT_m，M为大于1的整数，

由前一时刻t_n-1的姿态转换矩阵当前时刻t_n的机体系方向余弦矩阵当前时刻t_n的导航系方向余弦确定当前时刻t_n的姿态转换矩阵由前一时刻t_n-1的姿态四元数当前时刻t_n的机体系四元数前一时刻t_n-1的导航系四元数确定当前时刻t_n的姿态四元数最后采用链乘法则计算当前时刻t_n的姿态转换矩阵或者姿态四元数进而解算得到横滚角γ、俯仰角θ、航向角

2.根据权利要求1所述的针对高动态载体应用的高精度捷联惯导姿态角确定方法，其特征在于：利用相乘法则计算当前时刻t_n的机体系方向余弦矩阵当前时刻t_n的机体系四元数

其中，分别为第m-1时刻、第m时刻、第m+1时刻、第m+j时刻、第m+M-1时刻、第n时刻的机体系方向余弦矩阵，分别为第m-1时刻、第m时刻、第m+1时刻、第m+j时刻、第m+M-1时刻、第n时刻的机体系四元数，j＝1,2...M，t_n-1＝t_m-1，t_n＝t_m+M。

3.根据权利要求1所述的针对高动态载体应用的高精度捷联惯导姿态角确定方法，其特征在于，当前时刻的导航系方向余弦矩阵 当前时刻的导航系四元数

其中，ξ^N为当前时刻t_n的导航系旋转更新矢量， 为根据导航系速度v^N以及当前时刻的位置经度λ、纬度L、高度h计算得到的导航系旋转角速率，ξ^N×为ξ^N的斜对称矩阵形式。

4.根据权利要求1所述的针对高动态载体应用的高精度捷联惯导姿态角确定方法，其特征在于，第k次迭代角增量α的变化量Δα(k)为： 为陀螺仪旋转角速率。