CN107063254B - 一种陀螺地磁组合的姿态解算方法 - Google Patents

Abstract

本发明公开了一种陀螺地磁组合的姿态解算方法，包括以下步骤：步骤1、利用三轴陀螺角速率信息采用旋转矢量法优化算法解算姿态信息；利用地磁信息计算，通过积分比值法求解横滚角信息；步骤2、将地磁信号解算的横滚角和陀螺信号解算的俯仰角和偏航角作为下一时刻解算四元数；步骤3、重复步骤1和步骤2即实现姿态更新。本发明利用陀螺当前角速率、角增量，以及上一时刻角增量信息，计算出姿态信息，并利用两轴地磁信号解算出的横滚角修正以获得更高的精度。

Description

一种陀螺地磁组合的姿态解算方法

技术领域

本发明属于姿态测量领域,具体涉及一种陀螺地磁组合的姿态解算方法。

背景技术

高动态环境下捷联惯导系统的姿态解算是提高系统精度的关键技术。姿态解算是指利用载体传感器的输出计算分析得到姿态信息，包括航向角、俯仰角、横滚角。对作高动态运动处在高动态环境的导弹炮弹等载体来说，姿态测量精度是决定其捷联惯导系统能否正常工作的关键性因素。

旋转弹体绕自身纵轴高速旋转，陀螺仪精度较高，但误差会随着时间累计，需要配合其他惯性器件或传感器进行组合测量姿态角。与陀螺器件相比，地磁的短期精度不高，但其误差不会随时间累积，抗高过载能力也较强。如果将两者进行组合使用，可以大大提高从精度和稳定性。

因此，需要一种新的陀螺地磁组合的姿态解算方法以解决上述问题。

发明内容

本发明的目的是为了解决现有技术中存在的问题，提供一种能够获得更高精度的姿态信息的陀螺地磁组合的姿态解算方法。

为实现上述发明目的，本发明的陀螺地磁组合的姿态解算方法可采用如下技术方案：

一种一种陀螺地磁组合的姿态解算方法，采用陀螺仪和地磁传感器进行数据采集得到陀螺信号和地磁信号，包括如下步骤：

步骤一、利用陀螺角速率信息采用旋转矢量法优化算法解算姿态信息，利用地磁信息通过积分比值法求解横滚角；

步骤二、将步骤一利用地磁信号解算得到的横滚角和陀螺信号解算得到的的俯仰角和偏航角作为下一时刻解算的四元数；

步骤三、重复步骤一和步骤二即实现姿态更新。

更进一步的，步骤1)中利用陀螺角速率信息采用旋转矢量法优化算法解算姿态信息包括以下步骤：

1.1、通过陀螺信号计算旋转矢量Φ(T)；

1.2、根据步骤1.1得到的旋转矢量Φ(T)，计算得到旋转四元数；

1.3、根据步骤1.2的旋转四元数计算姿态的姿态信息。旋转矢量法利用了前一时刻的角速率和角增量求解姿态角。

更进一步的，步骤1)中利用地磁信息通过积分比值法求解横滚角信息包括以下步骤：

2.1、计算模型函数f(θ)的值；

2.2、地磁传感器包括磁传感器S1和磁传感器S2，磁传感器S1和磁传感器S2的测量值分别为H_S1和H_S2，计算测量值H_S1和H_S2为零时的横滚角γ，其中，H_S1为磁传感器S1的测量值，H_S2为磁传感器S2的测量值。

更进一步的，步骤1.1中通过下式计算旋转矢量Φ(T)：

式中，ω₁,ω₂,ω₃为陀螺仪在t,t+T时刻的输出角速率，Δθ₁为当前时刻角增量，Δθ₀为前一时刻的角增量信号。

更进一步的，步骤1.2中旋转四元数q₀,q₁,q₂,q₃为：

式中，ψ表示航向角，θ表示俯仰角，γ表示横滚角。

更进一步的，步骤1.3中所述姿态信息通过下式计算得到弹体俯仰角θ、横滚角γ和航向角ψ：

其中，偏航角为实际航线和计划航线之间的夹角，

上式中，

ψ表示航向角，θ表示俯仰角，γ表示横滚角。

更进一步的，步骤2中的模型函数f(θ)通过下式计算得到：

其中，ψ表示航向角，θ表示俯仰角，γ表示横滚角，h为地磁场矢量H的标量大小，γ为弹体坐标系中磁传感器S2与Ox₁轴夹角。

更进一步的，步骤2.2中磁传感器S1和磁传感器S2的测量值与姿态角之间关系为：

H_S1＝h(cosγsinψcosθ+sinγsinθ)

H_S2＝h(cosθcosψcosλ+cosγsinψcosθsinλ+sinγsinθsinλ)

其中，ψ表示航向角，θ表示俯仰角，γ表示横滚角，h为地磁场矢量H的标量大小，γ为弹体坐标系中磁传感器S2与Ox₁轴夹角。

有益效果：本发明的陀螺地磁组合的姿态解算方法采用陀螺磁传感器组合解算姿态，利用旋转矢量法求解姿态角，地磁传感器利用积分比值法求解横滚角来修正陀螺的累计误差，对陀螺输出进行动态校正，从而提升解算姿态角的精度。通过实验验证可得，本发明使用陀螺磁传感器组合解算姿态方法，相当于单一陀螺或地磁传感器，具有较好的精度和稳定性。

附图说明

图1为本发明的原理示意图；

图2是本发明实施例的陀螺磁传感器安装示意图；

图3是本发明实施的姿态结算的算法流程示意图。

具体实施方式

下面结合附图对本发明作更进一步的说明。下面结合附图和具体实施例，进一步阐明本发明，应理解这些实施例仅是本发明的优选实施方式，在阅读了本发明之后，本领域技术人员对本发明的各种不脱离本发明原理的修改均落于本申请所附权利要求所限定的范围。

下面结合实施例对本发明做进一步的说明。

请参阅图1和图2所示，三轴陀螺仪安装在弹体上，使得三轴陀螺仪对准载体坐标系的三轴，两个非正交的单轴磁传感器S1、S2分别安装在弹体坐标系的Ox₁y₁z₁的原点上，Ox₁轴与弹体纵轴重合，两个敏感轴都在Ox₁z₁平面内，S1沿Oz₁轴安装，S2与Ox₁轴成λ角安装。

1.旋转矢量优化二子样姿态更新算法原理:

设t_k时刻到t_k+1时刻，更新周期T＝t_k+1-t_k，q(T)更新四元数为与Q(t_k+1)旋转四元数有如下关系：

旋转矢量Φ与q(T)关系如下

四元数可以用姿态角表示

而四元数与姿态矩阵间存在以下关系：

设

通过姿态矩阵便可求得弹体俯仰角θ、横滚角γ、航向角ψ

因此通过每一周期计算旋转矢量Φ便可更新四元数信息来获得姿态信息。本文所用到的旋转矢量Φ的计算算法原理如下：

当采用旋转矢量确定载体姿态运动时，有如下简化微分方程

一个角速率计算周期内[t,t+T]用抛物线对角速率进行拟合：

ω＝a+2bτ+3cτ²,0≤τ≤T

设陀螺在t,t+T时刻的角速率输出分别为ω₁,ω₂,ω₃，将Φ在T＝0处用泰勒级数展开，并将Φ的各阶导数用ω₁,ω₂,ω₃来表示，即可得到基于角速率的旋转矢量表达式

加入当前时刻角增量Δθ₁以及前一时刻的角增量Δθ₀，对上式进行圆锥运动优化，设

其中X,Y,Z,K为待定系数，使得在圆锥运动环境下的算法漂移最小。

设Ω为锥运动频率，α为圆锥角，由四元数的性质可推导出一个姿态更新周期内的旋转矢量：

一个姿态更新周期内的陀螺仪的角增量和角速率为：

代入式(1)可求得一个周期的旋转矢量增量，与式(2)作差得到旋转矢量误差，当我们仅考虑引起平台漂移的直流分量部分时，且认为α为小量，则有：

对上式进行泰勒展开级数展开，并令ΩT高次项系数为零，解得

令得到算法

角增量则通过插值积分近似

2.地磁传感器姿态算法

磁传感器S1和S2的测量值H_S1和H_S2与弹体俯仰角θ、横滚角γ、航向角ψ的表达式

H_S1＝h(cosγsinψcosθ+sinγsinθ)

H_S2＝h(cosθcosψcosλ+cosγsinψcosθsinλ+sinγsinθsinλ)

式中，h为地磁场矢量H的标量大小。

积分数学模型：

其中，N表示弹丸旋转一圈的总采样次数，k表示采样时刻；和表示关于横滚角γ的积分运算；/>和/>表示弹丸旋转一圈两个传感器采样值H_S1和H_S2的离散采样点的平方和运算；

根据磁传感器的表达式又可推出

弹丸旋转一圈时，假设夹角λ和航向角ψ不变，通过两个传感器采样值平方和的积分运算，可以得到f(θ)的值。求出f(θ)的解，可获得弹体某一特定时刻的横滚角γ角度值，即可计算出弹体旋转一周内所有时刻的横滚角γ。

本文所用到的地磁传感器求解横滚角γ的算法如下：

假设特定时刻为H_S1或H_S2的零点，测量值H_S1或H_S2为零时，可以消去未知数磁场强度标量h，减少周围干扰磁场环境对计算结果的影响。

(1)磁传感器S1的测量值H_S1＝0时：

cosγsinψcosθ+sinγsinθ＝0

整理得

式中，函数的两个参数ψ和θ不同时为零；

求得当前第K周期的横滚角γ_K应当有正负两个取值，分别对应1,3象限或者2,4象限，此时应根据前一周期横滚角γ_K-1和弹体的转速ω粗略估算当前周期的横滚角正负情况，来选取横滚角γ_K的值。

弹丸每旋转一圈，可以得到磁传感器输出值H_S1(或H_S2)的最大值H_S1max和最小值H_S1min，利用取得H_S1max和H_S1min的时刻点t(H_S1max)和t(H_S1min)，可以粗略计算出ω，即ω＝2(t(H_S1max)-t(H_S1min))。

令其中T为采样周期时间。γ_K正负情况与/>正负相同。

(2)磁传感器S2的测量值H_s2＝0时：

cosθcosψcosλ+cosγsinψcosθsinλ+sinγsinθsinλ＝0

式中，

当|sinλ|＞|cosθcosψ|时，磁传感器S2的测量值H_s2有两个零点，当前第K周期横滚角γ_K有两个解γ_1,K和γ_2,K，此时应计算由第K-1周期的横滚角γ_K-1加上弹体转速ω得到的当前周期的横滚角近似值横滚角取/>与/>较小者对应的γ；

当|sinλ|＝|cosθcosψ|时，磁传感器S2的测量值H_S2有一个零点；

当|sinλ|＜|cosθcosψ|时，磁传感器S2的测量值H_S2不一定有零点,此时求得横滚角γ不准确。

3.将地磁信号解算的各时刻横滚角和陀螺信号解算的各时刻俯仰角和偏航角作为下一步解算的四元数，再用优化二子样算法进行姿态解算，对陀螺输出进行动态校正，提高姿态角精度，减少误差。

Claims (6)

1.一种陀螺地磁组合的姿态解算方法，其特征在于，采用陀螺仪和地磁传感器进行数据采集得到陀螺信号和地磁信号，包括如下步骤：

步骤一、利用陀螺角速率信息采用旋转矢量法优化算法解算姿态信息，利用地磁信息通过积分比值法求解横滚角；

步骤1)中利用陀螺角速率信息采用旋转矢量法优化算法解算姿态信息包括以下步骤：

1.1、通过陀螺信号计算旋转矢量Φ(T)；

步骤1.1中通过下式计算旋转矢量Φ(T)：

式中，ω₁,ω₂,ω₃为陀螺仪在时刻的输出角速率，Δθ₁为当前时刻角增量，Δθ₀为前一时刻的角增量信号；

1.2、根据步骤1.1得到的旋转矢量Φ(T)，计算得到旋转四元数；

1.3、根据步骤1.2的旋转四元数计算姿态的姿态信息；

步骤二、将步骤一利用地磁信号解算得到的横滚角和陀螺信号解算得到的的俯仰角和偏航角作为下一时刻解算的四元数；

步骤三、重复步骤一和步骤二即实现姿态更新。

2.根据权利要求1所述的陀螺地磁组合的姿态解算方法，其特征在于，步骤1)中利用地磁信息通过积分比值法求解横滚角信息包括以下步骤：

2.1、计算模型函数f(θ)的值；

2.2、地磁传感器包括磁传感器S1和磁传感器S2，磁传感器S1和磁传感器S2的测量值分别为H_S1和H_S2，计算测量值H_S1和H_S2为零时的横滚角γ，其中，H_S1为磁传感器S1的测量值，H_S2为磁传感器S2的测量值。

3.根据权利要求1所述的陀螺地磁组合的姿态解算方法，其特征在于，步骤1.2中旋转四元数q₀,q₁,q₂,q₃为：

式中，ψ表示航向角，θ表示俯仰角，γ表示横滚角。

4.根据权利要求1所述的陀螺地磁组合的姿态解算方法，其特征在于，步骤1.3中所述姿态信息通过下式计算得到弹体俯仰角θ、横滚角γ和航向角ψ：

其中，偏航角为实际航线和计划航线之间的夹角，

上式中，

ψ表示航向角，θ表示俯仰角，γ表示横滚角。

5.根据权利要求2所述的陀螺地磁组合的姿态解算方法，其特征在于，步骤2中的模型函数f(θ)通过下式计算得到：

其中，ψ表示航向角，θ表示俯仰角，γ表示横滚角，h为地磁场矢量H的标量大小，γ为弹体坐标系中磁传感器S2与Ox₁轴夹角。

6.根据权利要求2所述的陀螺地磁组合的姿态解算方法，其特征在于，步骤2.2中磁传感器S1和磁传感器S2的测量值与姿态角之间关系为：

H_S1＝h(cosγsinψcosθ+sinγsinθ)

H_S2＝h(cosθcosψcosλ+cosγsinψcosθsinλ+sinγsinθsinλ)

其中，ψ表示航向角，θ表示俯仰角，γ表示横滚角，h为地磁场矢量H的标量大小，γ为弹体坐标系中磁传感器S2与Ox₁轴夹角。