CN107036576A - 基于差商法磁测旋转飞行器滚转角的实时解算方法 - Google Patents

Abstract

本发明公开了一种基于差商法磁测旋转飞行器滚转角解算方法，利用捷联在飞行器体内的地磁传感器解算旋转飞行器滚转角，在飞行器飞行过程中，取同一个滚转周期内的三个磁测量点，将前一个磁测量点分别减去后两个磁测量点，然后将其差值作商，得到含有滚转角的数学表达式。本发明不仅简单、计算量小、不需要提前校正地磁传感器、在一个滚转周期的任何位置都能进行解算，而且该方法对传感器刻度因子误差和固定偏差具有很强的鲁棒性。

Description

基于差商法磁测旋转飞行器滚转角的实时解算方法

技术领域

本发明属于飞行器姿态角测量领域，特别是一种基于差商法磁测旋转飞行器滚转角的实时解算方法。

背景技术

地磁场是地球的天然磁场，可利用捷联在飞行器内的地磁传感器对其进行姿态解算。地磁传感器具有高灵敏、小体积、抗高发射过载和解算误差不随时间累积的优点。因此，地磁传感器能够应用于旋转飞行器的滚转角解算。

南京理工大学曹鹏在博士论文“高旋弹丸滚转角的磁测量误差建模及仿真实验研究”中介绍了一种传统磁测滚转角的解算方法。在飞行器的滚转平面内，地磁矢量在弹径平面的分矢量为B_c，其分矢量在飞行器轴坐标系中的量测值为和在飞行器坐标系中的量测值为b_y和b_z。则飞行器滚转角的计算表达式为：

如果运用传统方法解算飞行器滚转角，该方法需要地磁传感器两个敏感轴方向的输出量，同时传感器刻度因子误差和固定偏差对该方法的解算精度有一定的影响。

发明内容

本发明的目的在于提供一种基于差商法磁测旋转飞行器滚转角的实时解算方法，利用地磁传感器单轴的磁测量信号，提高旋转飞行器磁测滚转角解算精度、消除传感器由于刻度因子误差和固定偏差引起的滚转角解算误差。

实现本发明目的的技术解决方案为：一种基于差商法磁测旋转飞行器滚转角的实时解算方法，方法步骤如下：

步骤1：建立地磁传感器敏感轴输出模型：

步骤1-1：对飞行器进行坐标变换：

采用地磁测量飞行器滚转角的过程中，捷联于飞行器内部的地磁传感器敏感方向与飞行器坐标轴方向一致，并利用测量所得的地磁信息进行飞行器姿态解算；

飞行器发射坐标系ox₁y₁z₁向飞行器体坐标系o_sxyz的转换关系为：

分别为地磁场强度在发射坐标系ox₁y₁z₁中各轴的分量，b_x、b_y、b_z分别为地磁场强度在飞行器体坐标系o_sxyz中各轴的分量，为飞行器姿态转换矩阵；θ为飞行器俯仰角，为飞行器偏航角，γ为飞行器滚转角；

步骤1-2：计算参考滚转角γ_B：

当飞行器滚转角γ与参考滚转角γ_B相等时，分矢量B_c在飞行器体坐标系o_sxyz中的分量b_z为0；根据公式(1.2)和公式(1.3)得到b_z的表达式

将γ＝γ_B和b_z＝0代入公式(1.4)得到参考滚转角的表达式：

在飞行器的滚转平面内，地球地磁矢量在弹径平面的分矢量为B_c，b_y和b_z为分矢量为B_c在飞行器体坐标系o_sxyz中的投影分量，即为地磁传感器敏感轴的输出量；和为分矢量为B_c在飞行器轴坐标系o_sx₂y₂z₂中的投影分量；γ_B为地磁场分矢量B_c与y₂轴方向的夹角，即参考滚转角；γ为飞行器体坐标系o_sxyz的y轴方向和飞行器轴坐标系o_sx₂y₂z₂的y₂轴方向的夹角，即飞行器滚转角；

步骤1-3：获得地磁传感器敏感轴输出模型，通过地磁传感器y轴方向的地磁分量大小b_y表示：

飞行器滚转一周，地磁传感器y轴方向的地磁分量大小b_y的表达式为

b_y＝B_c cos(γ-γ_B) (1.6)

在飞行器磁测量环境下，磁场测量误差包括磁场敏感方向刻度因子误差、传感器的固定偏差和测量噪声带来的随机偏差；当只考虑前两种测量误差时，将表达式(1.6)改写为

b_y＝k_yB_c cos(γ-γ_B)+b_s (1.7)

式中，k_y表示地磁传感器y轴方向的刻度因子误差，b_s表示地磁传感器的固定偏差；

步骤2：获取任意两个时刻滚转角间隔：

Δγ为任意两个时刻滚转角间隔，Δt₁₂为任意两个时刻的时间差，为第i个周期的滚转角速率。

步骤3：解算飞行器滚转角。

本发明与现有技术相比，其显著优点在于：(1)方法简单、计算量小、不需要提前校正地磁传感器，在一个滚转周期的任意位置都能进行解算。

(2)对传感器刻度因子误差和固定偏差具有很强的鲁棒性，仅需要单轴的硬件滤波信号就能解算出飞行器滚转角。

附图说明

图1为本发明飞行器坐标转换示意图。

图2为本发明飞行器滚转角解算原理示意图。

图3为本发明地磁传感器输出量示意图。

图4为本发明基于差商法磁测旋转飞行器滚转角的实时解算方法流程图。

具体实施方式

下面结合附图对本发明作进一步详细描述。

结合图4，一种基于差商法磁测旋转飞行器滚转角的实时解算方法，方法步骤如下：

步骤1：建立地磁传感器敏感轴输出模型：

步骤1-1：对飞行器进行坐标变换：

采用地磁测量飞行器滚转角的过程中，捷联于飞行器内部的地磁传感器敏感方向与飞行器坐标轴方向一致，并利用测量所得的地磁信息进行飞行器姿态解算。坐标轴定义如图1所示。

其中坐标系o_sxyz为飞行器体坐标系，坐标系o_sx₂y₂z₂为飞行器轴坐标系，坐标系ox₁y₁z₁为飞行器发射坐标系。θ为飞行器俯仰角，为飞行器偏航角，γ为飞行器滚转角。

飞行器发射坐标系ox₁y₁z₁向飞行器体坐标系o_sxyz的转换关系为：

分别为地磁场强度在发射坐标系ox₁y₁z₁中各轴的分量，b_x、b_y、b_z分别为地磁场强度在飞行器体坐标系o_sxyz中各轴的分量，为飞行器姿态转换矩阵。

步骤1-2：计算参考滚转角γ_B：

在飞行器的滚转平面内，地球地磁矢量在弹径平面的分矢量为B_c，其分矢量在飞行器轴坐标系o_sx₂y₂z₂和在飞行器体坐标系o_sxyz中的投影分量如图2。b_y和b_z均为分矢量为B_c在飞行器体坐标系o_sxyz中的投影分量，同时也是地磁传感器敏感轴的输出量。和为分矢量为B_c在飞行器轴坐标系o_sx₂y₂z₂中的投影分量。γ_B为地磁场分矢量B_c与y₂轴方向的夹角，也称为参考滚转角。γ为飞行器体坐标系o_sxyz的y轴方向和飞行器轴坐标系o_sx₂y₂z₂的y₂轴方向的夹角，也称为飞行器滚转角。

飞行器轴坐标系o_sx₂y₂z₂是一个定坐标系，飞行器体坐标系o_sxyz是一个动坐标系。当飞行器滚转角γ与参考滚转角γ_B相等时，分矢量B_c在飞行器体坐标系o_sxyz中的分量b_z为0。根据公式(1.2)和公式(1.3)得到b_z的表达式

将γ＝γ_B和b_z＝0代入公式(1.4)得到参考滚转角的表达式：

步骤1-3：获得地磁传感器敏感轴输出模型，通过地磁传感器y轴方向的地磁分量大小b_y表示：

飞行器滚转一周，地磁传感器y轴方向的地磁分量大小b_y的表达式为

b_y＝B_c cos(γ-γ_B) (1.6)

在飞行器磁测量环境下，磁场测量误差包括磁场敏感方向刻度因子误差、传感器的固定偏差和测量噪声带来的随机偏差。当只考虑前两种测量误差时，将表达式(1.6)改写为

b_y＝k_yB_c cos(γ-γ_B)+b_s (1.7)

式中，k_y表示地磁传感器y轴方向的刻度因子误差，b_s表示地磁传感器的固定偏差。

步骤2：获取任意两个时刻滚转角间隔：

步骤2-1：计算第i个周期飞行器滚转角速率

在解算第i个周期飞行器滚转角时，其滚转角速率未知。假设在相邻两个滚转周期内，滚转角速率大小相近。因此，可以用第i-1个周期的滚转角速率替代第i个周期的滚转角速率 的表达式用第i-1个周期的时间间隔Δt_i-1表示为

步骤2-2：计算任意两个时刻滚转角间隔Δγ：

在飞行器飞行过程中，在第i个周期内，任意两个时刻滚转角间隔Δγ可以用这两个时刻的时间差Δt₁₂和第i个周期的滚转角速率表示

步骤3：解算飞行器滚转角：

步骤3-1：推导飞行器滚转角解算模型

在飞行器滚转过程中，地磁传感器y轴测量输出量如图3所示，在第i个滚转周期的t₁时刻，假设飞行器滚转角为γ。在t₁时刻之前取两点，分别记为t₂时刻和t₃时刻。其中，t₂时刻与t₁时刻滚转角相差Δγ₁，t₃时刻与t₁时刻滚转角相差Δγ₂。在t₁、t₂、t₃时刻，地磁传感器y轴测量输出量分别记为b_y1、b_y2、b_y3，其表达式为

为了消除磁场敏感方向刻度因子误差k_y和传感器固定偏差b_s对滚转角解算带来的影响，将t₁时刻的测量值分别减去t₂和t₃时刻的测量值，然后将两个差值作商。得到下列表达式

令中间变量由表达式(1.11)整理得

γ＝arctan(Q)+γ_B (1.12)

其中：

中间变量

步骤3-2：求解飞行器滚转角

由表达式(1.12)和表达式(1.13)可知，t₁时刻的滚转角γ只与M、γ_B、Δγ₁、Δγ₂相关，与磁场敏感方向刻度因子和传感器固定偏差无关。一个M值对应两个数学解，然而滚转角却为唯一解。因此，使用以下方法判定滚转角的解：

当b_y1>0且Q>0时，γ-γ_B＝arctan(Q)；

当b_y1>0且Q<0时，γ-γ_B＝arctan(Q)；

当b_y1<0且Q>0时，γ-γ_B＝arctan(Q)-π；

当b_y1<0且Q<0时，γ-γ_B＝arctan(Q)+π。

步骤3-3：补偿异常点滚转角

在滚转角的求解过程中，量测值b_y受到一定的干扰，在b_y过零点附近，测量值与实际值会出现异号的情况，如果使用上述方法判定飞行器滚转角的值，解算结果与真实值可能存在π的误差。

步骤3-3-1、将第i个滚转角的值与第i-1个滚转角的值相减，得到滚转角的差值α＝|γ_i-γ_i-1|；

步骤3-3-2、判断异常点：

如果滚转角的差值α∈(2π/3,4π/3)，则判定第i个滚转角值为异常点，根据下式补偿得到飞行器滚转角：

如果b_y1>0且Q>0时，则γ-γ_B＝arctan(Q)-π；

如果b_y1>0且Q<0时，则γ-γ_B＝arctan(Q)+π；

如果b_y1<0且Q>0时，则γ-γ_B＝arctan(Q)+π；

如果b_y1<0且Q<0时，则γ-γ_B＝arctan(Q)-π；

否则，输出步骤3-2中的飞行器滚转角。

实施例1

为了进一步加深对本发明的理解，下面通过举例的方式，运用本发明所述的基于差商法磁测旋转飞行器滚转角的实时解算方法求解t₁时刻飞行器的滚转角，具体步骤如下：

步骤1：建立地磁传感器敏感轴输出模型

步骤1-1：对飞行器进行坐标变换

假设在t₁时刻，飞行器俯仰角θ为飞行器偏航角为飞行器滚转角γ为

步骤1-2：计算参考滚转角γ_B：

飞行器俯仰角θ为飞行器偏航角为地磁场强度在发射坐标系ox₁y₁z₁中的三个分量为28323nT、为24043nT、为24523nT。将其代入公式(1.5)得到参考滚转角γ_B＝1.7126(rad)。

步骤1-3：获得地磁传感器敏感轴输出模型，通过地磁传感器y轴方向的地磁分量大小b_y表示：

假设磁场敏感方向刻度因子误差k_y为0.85、传感器的固定偏差500nT，地磁场分矢量B_c为21417nT，将参数代入公式(1.7)，得到地磁传感器在t₁时刻的输出值为b_y1为-16394nT。

步骤2：获取任意两个时刻滚转角间隔

步骤2-1：计算第i个周期滚转角速率

假设第i-1个周期的时间间隔Δt_i-1为10ms，代入公式(1.8)得到第i个周期滚转角速率为100r/s。

步骤2-2：计算两个时刻滚转角间隔Δγ

在计算过程中，分别取t₁时刻前的两点，时间间隔分别为0.25ms和0.75ms，将其代入公式(1.9)得到两个滚转角间隔分别为

步骤3：解算飞行器滚转角

步骤3-1：推导飞行器滚转角解算模型

根据公式(1.10)得到t₁、t₂、t₃三个时刻分别对应的地磁传感器y轴测量输出量b_y1＝-16394nT、b_y2＝-6282.7nT、b_y3＝7282.7nT，则中间变量M＝0.427057。将其代入公式(1.13)得到中间变量Q为0.4015。

步骤3-2：求解飞行器滚转角

根据步骤3-2的判断方法，计算得到飞行器在t₁时刻滚转角γ＝5.236(rad)。

步骤3-3：补偿异常点滚转角

根据步骤3-3的判断，飞行器在t₁时刻不是异常点，所以不需要补偿。

将飞行器某时刻滚转角的真实值记为γ_t，计算值记为γ_c，则飞行器在该时刻计算误差ε表达式为

ε＝γ_t-γ_c (1.14)

通过公式(1.14)分析上面算例的计算误差ε为8.5762e-06rad，将误差结果转换为角度ε为4.9138e-04度。从计算误差不到1度可以看出，本发明对传感器的比例因子误差和固定偏差具有很强的鲁棒性，在工程应用方面具有十分重要的意义。

Claims (3)

1.一种基于差商法磁测旋转飞行器滚转角的实时解算方法，其特征在于，方法步骤如下：

步骤1：建立地磁传感器敏感轴输出模型：

步骤1-1：对飞行器进行坐标变换：

采用地磁测量飞行器滚转角的过程中，捷联于飞行器内部的地磁传感器敏感方向与飞行器坐标轴方向一致，并利用测量所得的地磁信息进行飞行器姿态解算；

飞行器发射坐标系ox₁y₁z₁向飞行器体坐标系o_sxyz的转换关系为：

分别为地磁场强度在发射坐标系ox₁y₁z₁中各轴的分量，b_x、b_y、b_z分别为地磁场强度在飞行器体坐标系o_sxyz中各轴的分量，为飞行器姿态转换矩阵；θ为飞行器俯仰角，为飞行器偏航角，γ为飞行器滚转角；

步骤1-2：计算参考滚转角γ_B：

当飞行器滚转角γ与参考滚转角γ_B相等时，分矢量B_c在飞行器体坐标系o_sxyz中的分量b_z为0；根据公式(1.2)和公式(1.3)得到b_z的表达式

将γ＝γ_B和b_z＝0代入公式(1.4)得到参考滚转角的表达式：

在飞行器的滚转平面内，地球地磁矢量在弹径平面的分矢量为B_c，b_y和b_z为分矢量为B_c在飞行器体坐标系o_sxyz中的投影分量，即为地磁传感器敏感轴的输出量；和为分矢量为B_c在飞行器轴坐标系o_sx₂y₂z₂中的投影分量；γ_B为地磁场分矢量B_c与y₂轴方向的夹角，即参考滚转角；γ为飞行器体坐标系o_sxyz的y轴方向和飞行器轴坐标系o_sx₂y₂z₂的y₂轴方向的夹角，即飞行器滚转角；

步骤1-3：获得地磁传感器敏感轴输出模型，通过地磁传感器y轴方向的地磁分量大小b_y表示：

飞行器滚转一周，地磁传感器y轴方向的地磁分量大小b_y的表达式为

b_y＝B_c cos(γ-γ_B) (1.6)

在飞行器磁测量环境下，磁场测量误差包括磁场敏感方向刻度因子误差、传感器的固定偏差和测量噪声带来的随机偏差；当只考虑前两种测量误差时，将表达式(1.6)改写为

b_y＝k_yB_c cos(γ-γ_B)+b_s (1.7)

式中，k_y表示地磁传感器y轴方向的刻度因子误差，b_s表示地磁传感器的固定偏差；

步骤2：获取任意两个时刻滚转角间隔：

Δγ为任意两个时刻滚转角间隔，Δt₁₂为任意两个时刻的时间差，为第i个周期的滚转角速率。

步骤3：解算飞行器滚转角。

2.根据权利要求1所述的基于差商法磁测旋转飞行器滚转角的实时解算方法，其特征在于，上述步骤2中获取任意两个时刻滚转角间隔，具体步骤如下：

步骤2-1：计算第i个周期飞行器滚转角速率

假设在相邻两个滚转周期内，滚转角速率大小相近，因此，用第i-1个周期的滚转角速率替代第i个周期的滚转角速率 的表达式用第i-1个周期的时间间隔Δt_i-1表示为

步骤2-2：计算任意两个时刻滚转角间隔Δγ：

在飞行器飞行过程中，在第i个周期内，任意两个时刻滚转角间隔Δγ用这两个时刻的时间差Δt₁₂和第i个周期的滚转角速率表示

3.根据权利要求1所述的基于差商法磁测旋转飞行器滚转角的实时解算方法，其特征在于，上述步骤3中解算飞行器滚转角，具体步骤如下：

步骤3-1：推导飞行器滚转角解算模型

在飞行器滚转过程中，在第i个滚转周期的t₁时刻，假设飞行器滚转角为γ；在t₁时刻之前取两点，分别记为t₂时刻和t₃时刻；其中，t₂时刻与t₁时刻滚转角相差Δγ₁，t₃时刻与t₁时刻滚转角相差Δγ₂；在t₁、t₂、t₃时刻，地磁传感器y轴测量输出量分别记为b_y1、b_y2、b_y3，其表达式为

为了消除磁场敏感方向刻度因子误差k_y和传感器固定偏差b_s对滚转角解算带来的影响，将t₁时刻的测量值分别减去t₂和t₃时刻的测量值，然后将两个差值作商，得到下列表达式

令中间变量由表达式(1.11)整理得

γ＝arctan(Q)+γ_B (1.12)

其中：

步骤3-2：求解飞行器滚转角

根据下式判定飞行器滚转角的解：

当b_y1>0且Q>0时，γ-γ_B＝arctan(Q)；

当b_y1>0且Q<0时，γ-γ_B＝arctan(Q)；

当b_y1<0且Q>0时，γ-γ_B＝arctan(Q)-π；

当b_y1<0且Q<0时，γ-γ_B＝arctan(Q)+π；

步骤3-3：补偿异常点滚转角：

步骤3-3-1、将第i个滚转角的值与第i-1个滚转角的值相减，得到滚转角的差值α＝|γ_i-γ_i-1|；

步骤3-3-2、判断异常点：

如果滚转角的差值α∈(2π/3,4π/3)，则判定第i个滚转角值为异常点，根据下式补偿得到飞行器滚转角：

如果b_y1>0且Q>0时，则γ-γ_B＝arctan(Q)-π；

如果b_y1>0且Q<0时，则γ-γ_B＝arctan(Q)+π；

如果b_y1<0且Q>0时，则γ-γ_B＝arctan(Q)+π；

如果b_y1<0且Q<0时，则γ-γ_B＝arctan(Q)-π；

否则，输出步骤3-2中的飞行器滚转角。