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

摘要

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

说明书

技术领域

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

背景技术

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

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

发明内容

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

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

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

以陀螺采样周期T_k更新α方向角增量α(m)、β方向角增量β(m)，

以机体系更新周期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的姿态转换矩阵或者姿态四元数解算姿态角：横滚 角γ、俯仰角θ、航向角

作为所述针对高动态载体应用的高精度捷联惯导姿态角确定方法的进一步 优化方案，以陀螺采样周期T_k为计算周期，经过k次迭代计算得到α方向角增量 α(m)，利用陀螺仪旋转角速率计算第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)， $\mathrm{\Delta \beta }\left(k\right)=\frac{1}{2}(\alpha (k-1)+\frac{1}{6}\mathrm{\Delta \alpha }(k-1))\times \mathrm{\Delta \alpha }\left(k\right),$最后由T_m＝kT_k，经过k次迭代计算 得到β方向角增量β(m)，$\beta \left(k\right)=\beta (k-1)+\frac{1}{2}(\alpha (k-1)+\frac{1}{6}\mathrm{\Delta \alpha }(k-1))\times \mathrm{\Delta \alpha }\left(k\right).$

作为所述针对高动态载体应用的高精度捷联惯导姿态角确定方法的进一步 优化方案，利用方向余弦矩阵或四元数相乘法则更新下一时刻的机体系方向余弦 矩阵或者机体系四元数：由α方向角增量α(m)、β方向角增量β(m)计算机体系 旋转矢量φ(m)，φ(m)＝α(m)+β(m)，以机体系更新周期T_m为计算周期，利用机 体系旋转矢量φ_m通过毕卡逼近法更新机体系方向余弦矩阵或四元数$C_B^B=I+\frac{\sin \phi \left(m\right)}{\phi \left(m\right)}(\phi \left(m\right)\times )+\frac{1-\cos \phi \left(m\right)}{\phi \left(m\right)}(\phi \left(m\right)\times )(\phi \left(m\right)\times ),$$q_B^B={\left(\begin{array}{cc}\cos 0.5\phi \left(m\right)& \frac{\sin 0.5\phi \left(m\right)}{\phi \left(m\right)}\phi \left(m\right)\end{array}\right)}^T,$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通过毕卡逼近法更新导航系方 向余弦阵或四元数则有，

当前时刻的导航系方向余弦矩阵$C_{N(n-1)}^{N\left(n\right)}=I-\frac{\sin {\xi }^N}{{\xi }^N}({\xi }^N\times )+\frac{1-\cos {\xi }^N}{{\xi }^N}({\xi }^N\times )({\xi }^N\times ),$当前时刻的导航系四元数 $q_{N(n-1)}^{N\left(n\right)}:q_{N(n-1)}^{N\left(n\right)}=\left(\begin{array}{cc}\cos 0.5{\xi }^N& -\frac{\sin 0.5{\xi }^N}{{\xi }^N}{\xi }^N\end{array}\right).$

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

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)中 计算得到的导航系方向余弦阵或四元数根据方向余弦阵或四元数相乘 法则计算出当前时刻或即，

$C_{B\left(n\right)}^{N\left(n\right)}=C_{N(n-1)}^{N\left(n\right)}{C}_{B(n-1)}^{N(n-1)}{C}_{B\left(n\right)}^{B(n-1)},$

$q_{B\left(n\right)}^{N\left(n\right)}=q_{N(n-1)}^{N\left(n\right)}{q}_{B(n-1)}^{N(n-1)}{q}_{B\left(n\right)}^{B(n-1)},$

其中,和为前一时刻的姿态方向余弦阵和四元数最后利 用当前时刻的姿态方向余弦阵或解算得到姿态角。

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