[发明专利]一种基于等效旋转矢量算法的捷联惯导系统优化方法

说明书

本发明公开一种基于等效旋转矢量算法的捷联惯导系统优化方法，该方法为，以圆锥运动轨迹作为测试条件，对旋转矢量算法中的系数做优化，根据圆锥运动可得到理论四元数，利用旋转矢量得到计算四元数，对理论四元数与计算四元数作差，得误差四元数，对误差四元数进行泰勒级数展开，通过选择最优化系数，使得误差项为零。根据光纤陀螺输出为角速率的特点，使用角增量和角速率，以角速率为主，其精度高于二子样算法，运算量相当，该方法还能够有效补偿光纤惯导的圆锥误差。在只有角速率输入时，改进算法圆锥补偿精度比传统优化算法要高2个数量级，且计算量比传统优化算法小，该算法适合于由光纤陀螺构成的捷联惯导系统。

技术领域

本发明涉及一种捷联惯导系统优化方法，尤其涉及一种基于等效旋转矢量算法的捷联惯导系统优化方法。

背景技术

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

现有技术中公开了一种申请号为CN201310632713.5、名称为角速率输入的姿态算法结构与参数优化方法的发明专利，该方法以角增量为参数进行求解，激光陀螺一般输出为角增量，但是光纤陀螺输出为角速率，如果把角速率转化为角增量，很难保证旋转矢量精度不受影响，这一差异使得使用角增量代替旋转矢量进行姿态更新时会产生误差，并且误差随时间会不断累积。

发明内容

本发明的目的在于提供一种易于实现，有效补偿光纤惯导的圆锥误差的基于等效旋转矢量算法的捷联惯导系统优化方法。

为了实现上述目的，本发明提供如下技术方案：

本发明是一种基于等效旋转矢量算法的捷联惯导系统优化方法，其特点是，该方法为，以圆锥运动轨迹作为测试条件，对旋转矢量算法中的系数做优化，根据圆锥运动可得到理论四元数，利用旋转矢量得到计算四元数，对理论四元数与计算四元数作差，得误差四元数，对误差四元数进行泰勒级数展开，通过选择最优化系数，使得误差项为零。

优化地，该方法包括如下具体步骤，

(1)设置载体圆锥运动轨迹、采样周期T、更新周期h以及四元数初值；

(2)计算角增量

在一个计算周期(T，T+h)内对陀螺的角速率进行采样，假设姿态更新周期h内输出的角速率可表示为：

ω(h)＝a+2bh+3ch² (1)

记角增量为：

可计算Δθ(0)及其各阶导数，如下：

由于姿态更新周期h为毫秒级，则旋转矢量Φ(h)≈Δθ(h)；

(3)计算等效旋转矢量

旋转矢量的微分方程如下：

忽略高阶小量，式(4)可以写成：

可计算Φ(0)的各阶导数，如下：