本发明涉及一种捷联惯导系统优化方法，尤其涉及一种基于等效旋转矢量算法的捷联惯导系统优化方法。
背景技术：
在捷联惯导系统的姿态、速度和位置更新算法中，姿态算法对整个系统精度的影响最大，它是算法研究和设计的核心。姿态算法有欧拉角法、方向余弦法、四元数法、旋转矢量法等。欧拉角算法包含三角运算，实时计算有一定的困难，且在一定条件下方程出现退化，不适用全姿态的解算。方向余弦法避免了欧拉角法中的方程退化问题，但计算量大，工程中并不实用。四元数法计算量小，实现简单，工程中较实用，其本质是旋转矢量法中的单子样算法，没有对有限转动过程中的不可交换误差进行补偿，比较适合低动态载体的姿态解算，对于高动态载体，算法漂移会比较严重。旋转矢量法采用多子样算法对不可交换误差进行补偿，算法实现简单，并且可以对系数进行优化，适用于角运动频繁或角振动的载体姿态解算。
现有技术中公开了一种申请号为cn201310632713.5、名称为角速率输入的姿态算法结构与参数优化方法的发明专利，该方法以角增量为参数进行求解，激光陀螺一般输出为角增量，但是光纤陀螺输出为角速率，如果把角速率转化为角增量，很难保证旋转矢量精度不受影响，这一差异使得使用角增量代替旋转矢量进行姿态更新时会产生误差，并且误差随时间会不断累积。
技术实现要素：
本发明的目的在于提供一种易于实现，有效补偿光纤惯导的圆锥误差的基于等效旋转矢量算法的捷联惯导系统优化方法。
为了实现上述目的，本发明提供如下技术方案：
本发明是一种基于等效旋转矢量算法的捷联惯导系统优化方法，其特点是，该方法为，以圆锥运动轨迹作为测试条件，对旋转矢量算法中的系数做优化，根据圆锥运动可得到理论四元数，利用旋转矢量得到计算四元数，对理论四元数与计算四元数作差，得误差四元数，对误差四元数进行泰勒级数展开，通过选择最优化系数，使得误差项为零。
优化地，该方法包括如下具体步骤，
(1)设置载体圆锥运动轨迹、采样周期t、更新周期h以及四元数初值；
(2)计算角增量
在一个计算周期(t，t+h)内对陀螺的角速率进行采样，假设姿态更新周期h内输出的角速率可表示为：
ω(h)＝a+2bh+3ch2(1)
记角增量为：
可计算δθ(0)及其各阶导数，如下：
由于姿态更新周期h为毫秒级，则旋转矢量φ(h)≈δθ(h)；
(3)计算等效旋转矢量
旋转矢量的微分方程如下：
忽略高阶小量，式(4)可以写成：
可计算φ(0)的各阶导数，如下：
将φ(h)用泰勒级数展开，得：
设光纤陀螺在每个周期内某一时刻t的角速率为ω，则t＝0,t＝h/2,t＝h的角速率分别为：ω1,ω2,ω3，可以用陀螺角速率估计a,b,c的大小，如下：
将式(7)代入式(8),其中：
可知：
并考虑陀螺的角增量输出，则旋转矢量可用下式估计：
φ＝δθ+xh2ω1×ω3+yh2ω2×(ω3-ω1)(10)
式中：
(4)根据圆锥运动可得到理论四元数q(h)，利用旋转矢量φ(h)得到计算四元数
(5)对理论四元数q(h)与计算四元数
(6)对误差四元数
优化地，步骤(1)中载体圆锥运动轨迹为
φ＝[αcos(ωt)αsin(ωt)]t
式中，ω为圆锥运动角频率，α为圆锥运动半锥角，t为采样周期，t为仿真时间，则该向量对应的理论四元数为
优化地，步骤(4)中利用旋转矢量φ(h)得到计算四元数
式中，α为圆锥运动半锥角；
当t≥t，则再次采用圆锥运动轨迹重新求得理论四元数q(h)，重复步骤(5)、(6)，选择最优化系数。
优化地，所述圆锥运动的参数设置为：半锥角α＝1°，锥运动角频率ω＝2π，采样周期为0.005s，仿真时间为30s。
优化地，步骤(1)中陀螺的采样次数至少三次，优选3-8次，进一步优选5次。
与现有技术相比，本发明根据光纤陀螺输出为角速率的特点，在旋转矢量姿态更新算法的基础上研究了一种新的圆锥误差补偿表达式，等效旋转矢量使用角增量和角速率，以角速率为主，其精度高于二子样算法，运算量相当，该方法还能够有效补偿光纤惯导的圆锥误差。在只有角速率输入时，改进算法圆锥补偿精度比传统优化算法要高2个数量级，且计算量比传统优化算法小，该算法适合于由光纤陀螺构成的捷联惯导系统。
附图说明
此处所说明的附图用来提供对本发明的进一步理解，构成本发明的一部分，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。在附图中：
图1为本发明所述方法的仿真流程图；
图2为采用传统二子样优化算法的角速率输入下姿态误差曲线图；
图3为采用本发明所述方法在角增量输入下姿态误差曲线图。
具体实施方式
为了使本发明所要解决的技术问题、技术方案及有益效果更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。
参照图1-3，本发明提供了一种基于等效旋转矢量算法的捷联惯导系统优化方法，该方法为，以圆锥运动轨迹作为测试条件，对旋转矢量算法中的系数做优化，根据圆锥运动可得到理论四元数，利用旋转矢量得到计算四元数，对理论四元数与计算四元数作差，得误差四元数，对误差四元数进行泰勒级数展开，通过选择最优化系数，使得误差项为零。
该方法包括如下具体步骤，
(1)设置载体圆锥运动轨迹、采样周期t、更新周期h以及四元数初值；
(2)计算角增量
在一个计算周期(t，t+h)内对陀螺的角速率进行采样，假设姿态更新周期h内输出的角速率可表示为：
ω(h)＝a+2bh+3ch2(1)
记角增量为：
可计算δθ(0)及其各阶导数，如下：
由于姿态更新周期h为毫秒级，则旋转矢量φ(h)≈δθ(h)；
(3)计算等效旋转矢量
旋转矢量的微分方程如下：
忽略高阶小量，式(4)可以写成：
可计算φ(0)的各阶导数，如下：
将φ(h)用泰勒级数展开，得：
设光纤陀螺在每个周期内某一时刻t的角速率为ω，则t＝0,t＝h/2,t＝h的角速率分别为：ω1,ω2,ω3，可以用陀螺角速率估计a,b,c的大小，如下：
将式(7)代入式(8),其中：
可知：
并考虑陀螺的角增量输出，则旋转矢量可用下式估计：
φ＝δθ+xh2ω1×ω3+yh2ω2×(ω3-ω1)(10)
式中：
(4)根据圆锥运动可得到理论四元数q(h)，利用旋转矢量φ(h)得到计算四元数
(5)对理论四元数q(h)与计算四元数
(6)对误差四元数
步骤(1)中载体圆锥运动轨迹为
φ＝[αcos(ωt)αsin(ωt)]t
式中，ω为圆锥运动角频率，α为圆锥运动半锥角，t为采样周期，t为仿真时间，则该向量对应的理论四元数为
步骤(4)中利用旋转矢量φ(h)得到计算四元数
式中，α为圆锥运动半锥角；
当t≥t，则再次采用圆锥运动轨迹重新求得理论四元数q(h)，重复步骤(5)、(6)，选择最优化系数。
所述圆锥运动的参数设置为：半锥角α＝1°，锥运动角频率ω＝2π，采样周期为0.005s，仿真时间为30s。
步骤(1)中陀螺的采样次数至少三次。
图2为角速率直接积分转换为角增量，利用旋转矢量二子样优化算法仿真；图3为利用公式(11)，角速率转换为角增量，即利用新型的旋转矢量二子样算法仿真。由图2和图3可知，在只有角速率输入时，改进算法圆锥补偿精度比传统优化算法要高2个数量级，且计算量比传统优化算法小，该算法适合于由光纤陀螺构成的捷联惯导系统。
本发明根据光纤陀螺输出为角速率的特点，在旋转矢量姿态更新算法的基础上研究了一种新的圆锥误差补偿表达式，等效旋转矢量使用角增量和角速率，以角速率为主，其精度高于二子样算法，运算量相当；利用光纤陀螺输出的角速率，构造等效旋转矢量，解决非定轴转动引起的转动不可交换误差，能够有效补偿光纤惯导的圆锥误差。本发明易于工程实现，为光纤陀螺捷联系统姿态算法解算提供了一个更为有效的途径。
以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以所述权利要求的保护范围为准。