[发明专利]基于2阶不可交换误差补偿模型的圆锥算法

摘要

本发明公开一种基于2阶不可交换误差补偿模型的圆锥算法，属于捷联惯性导航领域，步骤为：在单位算法更新周期内，对陀螺仪的输出采样3次得到采样值；求出整个算法更新周期内的角增量；计算采样值的各1次叉乘项和各2次叉乘项；从陀螺仪的3次采样值出发，利用Bortz方程的旋转矢量1阶不可交换误差补偿模型对角增量进行1阶补偿：利用Bortz方程的2阶不可交换误差补偿模型对前述步骤求得的旋转矢量再进行2阶不可交换误差补偿；把经1阶补偿和2阶补偿后的旋转矢量代替角增量，代入捷联求姿算法，求取机体的飞行姿态。此算法可在不增加采样频率和更新周期的前提下，有效减小传统圆锥算法中的截断误差，提高精度。

主权项

一种基于2阶不可交换误差补偿模型的圆锥算法，其特征在于包括如下步骤：(1)在单位算法更新周期内，对陀螺仪的输出采样3次得到采样值Δθ1，Δθ2，Δθ3；(2)根据下式求出整个算法更新周期内的角增量Δθ：Δθ＝Δθ1+Δθ2+Δθ3                  (1)(3)计算采样值的各1次叉乘项和各2次叉乘项，其中，采样值的1次叉乘项包含以下3项：Δθ1×Δθ3，Δθ1×Δθ2，Δθ2×Δθ3  (2)采样值的2次叉乘项包含以下3项：Δθ1×(Δθ1×Δθ2)，Δθ3×(Δθ1×Δθ2)，Δθ1×(Δθ1×Δθ3)   (3)(4)从陀螺仪的3次采样值出发，利用Bortz方程的旋转矢量1阶不可交换误差补偿模型对角增量Δθ进行1阶补偿：所述Bortz关于1阶不可交换误差补偿模型为： Φ · = ω + 1 2 Φ × ω - - - ( 4 ) 其中的Φ代表更新周期内的旋转矢量，ω代表机体的角速度，两边积分得： Φ = Δθ + 1 2 ∫ t t + h Φ × ωdt - - - ( 5 ) 式(5)通过变换最终得到下式，其中δΦ′代表1阶不可交换误差补偿： Φ = Δθ + δ Φ ′ δ Φ ′ = 9 20 Δθ 1 × Δθ 3 + 27 40 Δθ 2 × ( Δθ 3 - Δθ 1 ) - - - ( 6 ) (5)利用Bortz方程的2阶不可交换误差补偿模型对步骤(4)求得的旋转矢量再进行2阶不可交换误差补偿：所述Bortz关于2阶不可交换误差补偿模型为： Φ · = ω + 1 2 Φ × ω + 1 12 Φ × ( Φ × ω ) . . . - - - ( 7 ) 根据该模型进一步补偿不可交换性误差的方法为： Φ = Δθ + δ Φ ′ + δ Φ ′ ′ δΦ ′ ′ = l 1 Δ θ 1 × ( Δθ 1 × Δθ 2 ) + l 2 Δθ 3 × ( Δθ 1 × Δθ 2 ) + l 3 Δθ 1 × ( Δθ 1 × Δθ 3 ) - - - ( 8 ) 其中，δΦ″即是圆锥算法中的旋转矢量对不可交换性误差的2阶补偿；(6)把经1阶补偿和2阶补偿后的旋转矢量Φ代替角增量，代入捷联求姿算法，求取机体的飞行姿态。