[发明专利]一种基于角速率输入的插值三子样圆锥误差补偿算法

说明书

本发明提供了一种基于角速率输入的插值三子样圆锥误差补偿算法，属于导航算法领域。捷联惯导系统姿态解算中，在陀螺仪的输入为角速率和不损失姿态角更新频率的条件下，以三子样圆锥误差补偿算法为基础，设计了插值三子样圆锥误差补偿算法。通过运用前三个时刻和当前时刻的角速率拟合曲线，在当前时刻的输入角速率和前一时刻的输入角速率中间插值两个输入角速率，完成插值三子样圆锥误差补偿。本发明设计的角速率插值三子样圆锥误差补偿算法的姿态更新频率与传统的三子样圆锥误差补偿算法的姿态更新频率相比提高三倍，与单子样圆锥误差补偿算法的姿态更新频率相同，而姿态角误差明显小于单子样圆锥误差补偿算法。

技术领域

本发明涉及的是一种基于角速率输入的插值三子样圆锥误差补偿算法，属于导航算法领域。

背景技术

姿态角解算算法是捷联惯导系统姿态、速度和位置更新算法的核心，姿态角解算算法常用的有欧拉角法、方向余弦法、四元数法、旋转矢量法等。欧拉角法包含三角函数运算，实时计算有一定的困难，且在一定条件下方程出现退化，不适用全姿态的解算。方向余弦法避免了欧拉角法中的方程退化问题，但计算量大，工程中并不实用。四元数法计算量小，实现简单，工程中较实用，其本质是旋转矢量法中的单子样算法，没有对有限转动过程中的不可交换误差进行补偿，比较适合低动态载体的姿态解算，对于高动态载体，算法漂移会比较严重。旋转矢量法采用多子样算法对不可交换误差进行补偿，算法实现简单，并且可以对系数进行优化，适用于角运动频繁或角振动的载体姿态解算。

发明内容

本发明提供了一种基于角速率输入的插值三子样圆锥误差补偿算法，目的在于提供一种在陀螺仪的输入为角速率和不损失姿态角更新频率的条件下，通过运用前三个时刻和当前时刻的角速率拟合曲线，在当前时刻的输入角速率和前一时刻的输入角速率中间插值两个输入角速率，完成插值三子样圆锥误差补偿的方法。

本发明包括以下步骤：

步骤一：分析传统的输入为角增量的圆锥补偿算法；

步骤二：分析输入为角增量的三子样圆锥补偿算法；

步骤三：设计输入为角速率的插值三子样圆锥误差补偿算法。

本发明还包括：

1.步骤一具体过程为：分析传统的输入为角增量的圆锥补偿算法如下：

设Φ(h)为[t_k,t_k+1]的时间段内的等效旋转矢量，其中h＝t_k+1-t_k。载体在t时刻的姿态运动可以用它描述。当以旋转矢量确定载体姿态运动时，有Bortz方程：

式(1)的一阶简化形式为：

采用一次曲线拟合角速度时：

ω(t_k+τ)＝a+bτ  (3)

对角速率在τ＝0处求导得到：

根据式(2)和式(4)计算Φ(τ)在τ＝0处的各阶导数可得：

对Φ(h)做泰勒级数展开并将式(5)带入可得：

将式(7)中的参数a、b用角增量的形式表示，记

当i取1,2时，可用Δθ₁和Δθ₂表示出参数a、b，带入到式(7)中得到：