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

摘要

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

权利要求

1.一种基于差商法磁测旋转飞行器滚转角的实时解算方法，其特征在于，方法步骤如下：步骤1：建立地磁传感器敏感轴输出模型：

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

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

飞行器发射坐标系ox1y1z1向飞行器体坐标系osxyz的转换关系为：分别为地磁场强度在发射坐标系ox1y1z1中各轴的分量，bx、by、bz分别为地磁场强度在飞行器体坐标系osxyz中各轴的分量， 为飞行器姿态转换矩阵；θ为飞行器俯仰角， 为飞行器偏航角，γ为飞行器滚转角；

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

当飞行器滚转角γ与参考滚转角γB相等时，分矢量Bc在飞行器体坐标系osxyz中的分量bz为0；根据公式(1.2)和公式(1.3)得到bz的表达式将γ＝γB和bz＝0代入公式(1.4)得到参考滚转角的表达式：在飞行器的滚转平面内，地球地磁矢量在飞行器径向平面的分矢量为Bc，by和bz为分矢量Bc在飞行器体坐标系osxyz中的投影分量，即为地磁传感器敏感轴的输出量； 和 为分矢量为Bc在飞行器轴坐标系osx2y2z2中的投影分量；γB为地磁场分矢量Bc与y2轴方向的夹角，即参考滚转角；γ为飞行器体坐标系osxyz的y轴方向和飞行器轴坐标系osx2y2z2的y2轴方向的夹角，即飞行器滚转角；

步骤1-3：获得地磁传感器敏感轴输出模型，通过地磁传感器y轴方向的地磁分量大小by表示：飞行器滚转一周，地磁传感器y轴方向的地磁分量大小by的表达式为by＝Bc cos(γ-γB)                     (1.6)在飞行器磁测量环境下，磁场测量误差包括磁场敏感方向刻度因子误差、传感器的固定偏差和测量噪声带来的随机偏差；当只考虑前两种测量误差时，将表达式(1.6)改写为by＝kyBc cos(γ-γB)+bs                  (1.7)式中，ky表示地磁传感器y轴方向的刻度因子误差，bs表示地磁传感器的固定偏差；

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

Δγ为任意两个时刻滚转角间隔，Δt12为任意两个时刻的时间差， 为第i个周期的滚转角速率；

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

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

假设在相邻两个滚转周期内，滚转角速率大小相近，因此，用第i-1个周期的滚转角速率 替代第i个周期的滚转角速率 的表达式用第i-1个周期的时间间隔Δti-1表示为步骤2-2：计算任意两个时刻滚转角间隔Δγ：在飞行器飞行过程中，在第i个周期内，任意两个时刻滚转角间隔Δγ用这两个时刻的时间差Δt12和第i个周期的滚转角速率 表示

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

在飞行器滚转过程中，在第i个滚转周期的t1时刻，假设飞行器滚转角为γ；在t1时刻之前取两点，分别记为t2时刻和t3时刻；其中，t2时刻与t1时刻滚转角相差Δγ1，t3时刻与t1时刻滚转角相差Δγ2；在t1、t2、t3时刻，地磁传感器y轴测量输出量分别记为by1、by2、by3，其表达式为为了消除磁场敏感方向刻度因子误差ky和传感器固定偏差bs对滚转角解算带来的影响，将t1时刻的测量值分别减去t2和t3时刻的测量值，然后将两个差值作商，得到下列表达式令中间变量 由表达式(1.11)整理得

γ＝arctan(Q)+γB                     (1.12)其中：

中间变量

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

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

当by1>0且Q>0时，γ-γB＝arctan(Q)；

当by1>0且Q<0时，γ-γB＝arctan(Q)；

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

当by1<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个滚转角值为异常点，根据下式补偿得到飞行器滚转角：如果by1>0且Q>0时，则γ-γB＝arctan(Q)-π；

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

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

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

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

说明书

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

技术领域

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

背景技术

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

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

[0004]

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

发明内容

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

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

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

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

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

[0011] 飞行器发射坐标系ox1y1z1向飞行器体坐标系osxyz的转换关系为：

[0012]

[0013]

[0014] 分别为地磁场强度在发射坐标系ox1y1z1中各轴的分量，bx、by、bz分别为地磁场强度在飞行器体坐标系osxyz中各轴的分量， 为飞行器姿态转换矩阵；θ为飞行器俯仰角， 为飞行器偏航角，γ为飞行器滚转角；

[0015] 步骤1-2：计算参考滚转角γB：

[0016] 当飞行器滚转角γ与参考滚转角γB相等时，分矢量Bc在飞行器体坐标系osxyz中的分量bz为0；根据公式(1.2)和公式(1.3)得到bz的表达式

[0017] 将γ＝γB和bz＝0代入公式(1.4)得到参考滚转角的表达式：

[0018]

[0019] 在飞行器的滚转平面内，地球地磁矢量在弹径平面的分矢量为Bc，by和bz为分矢量为Bc在飞行器体坐标系osxyz中的投影分量，即为地磁传感器敏感轴的输出量； 和 为分矢量为Bc在飞行器轴坐标系osx2y2z2中的投影分量；γB为地磁场分矢量Bc与y2轴方向的夹角，即参考滚转角；γ为飞行器体坐标系osxyz的y轴方向和飞行器轴坐标系osx2y2z2的y2轴方向的夹角，即飞行器滚转角；

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

[0021] 飞行器滚转一周，地磁传感器y轴方向的地磁分量大小by的表达式为[0022] by＝Bc cos(γ-γB)  (1.6)

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

[0024] by＝kyBc cos(γ-γB)+bs  (1.7)

[0025] 式中，ky表示地磁传感器y轴方向的刻度因子误差，bs表示地磁传感器的固定偏差；

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

[0027]

[0028] Δγ为任意两个时刻滚转角间隔，Δt12为任意两个时刻的时间差， 为第i个周期的滚转角速率。

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

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

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

附图说明

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

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

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

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

具体实施方式

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

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

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

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

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

[0041] 其中坐标系osxyz为飞行器体坐标系，坐标系osx2y2z2为飞行器轴坐标系，坐标系ox1y1z1为飞行器发射坐标系。θ为飞行器俯仰角， 为飞行器偏航角，γ为飞行器滚转角。

[0042] 飞行器发射坐标系ox1y1z1向飞行器体坐标系osxyz的转换关系为：

[0043]

[0044]

[0045] 分别为地磁场强度在发射坐标系ox1y1z1中各轴的分量，bx、by、bz分别为地磁场强度在飞行器体坐标系osxyz中各轴的分量， 为飞行器姿态转换矩阵。

[0046] 步骤1-2：计算参考滚转角γB：

[0047] 在飞行器的滚转平面内，地球地磁矢量在弹径平面的分矢量为Bc，其分矢量在飞行器轴坐标系osx2y2z2和在飞行器体坐标系osxyz中的投影分量如图2。by和bz均为分矢量为Bc在飞行器体坐标系osxyz中的投影分量，同时也是地磁传感器敏感轴的输出量。 和 为分矢量为Bc在飞行器轴坐标系osx2y2z2中的投影分量。γB为地磁场分矢量Bc与y2轴方向的夹角，也称为参考滚转角。γ为飞行器体坐标系osxyz的y轴方向和飞行器轴坐标系osx2y2z2的y2轴方向的夹角，也称为飞行器滚转角。

[0048] 飞行器轴坐标系osx2y2z2是一个定坐标系，飞行器体坐标系osxyz是一个动坐标系。当飞行器滚转角γ与参考滚转角γB相等时，分矢量Bc在飞行器体坐标系osxyz中的分量bz为0。根据公式(1.2)和公式(1.3)得到bz的表达式

[0049] 将γ＝γB和bz＝0代入公式(1.4)得到参考滚转角的表达式：

[0050]

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

[0052] 飞行器滚转一周，地磁传感器y轴方向的地磁分量大小by的表达式为[0053] by＝Bc cos(γ-γB)  (1.6)

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

[0055] by＝kyBc cos(γ-γB)+bs  (1.7)

[0056] 式中，ky表示地磁传感器y轴方向的刻度因子误差，bs表示地磁传感器的固定偏差。

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

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

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

[0060]

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

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

[0063]

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

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

[0066] 在飞行器滚转过程中，地磁传感器y轴测量输出量如图3所示，在第i个滚转周期的t1时刻，假设飞行器滚转角为γ。在t1时刻之前取两点，分别记为t2时刻和t3时刻。其中，t2时刻与t1时刻滚转角相差Δγ1，t3时刻与t1时刻滚转角相差Δγ2。在t1、t2、t3时刻，地磁传感器y轴测量输出量分别记为by1、by2、by3，其表达式为

[0067]

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

[0069]

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

[0071] γ＝arctan(Q)+γB  (1.12)

[0072] 其中：

[0073] 中间变量

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

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

[0076] 当by1>0且Q>0时，γ-γB＝arctan(Q)；

[0077] 当by1>0且Q<0时，γ-γB＝arctan(Q)；

[0078] 当by1<0且Q>0时，γ-γB＝arctan(Q)-π；

[0079] 当by1<0且Q<0时，γ-γB＝arctan(Q)+π。

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

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

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

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

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

[0085] 如果by1>0且Q>0时，则γ-γB＝arctan(Q)-π；

[0086] 如果by1>0且Q<0时，则γ-γB＝arctan(Q)+π；

[0087] 如果by1<0且Q>0时，则γ-γB＝arctan(Q)+π；

[0088] 如果by1<0且Q<0时，则γ-γB＝arctan(Q)-π；

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

[0090] 实施例1

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

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

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

[0094] 假设在t1时刻，飞行器俯仰角θ为 飞行器偏航角 为 飞行器滚转角γ为[0095] 步骤1-2：计算参考滚转角γB：

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

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

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

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

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

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

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

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

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

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

[0106] 根据公式(1.10)得到t1、t2、t3三个时刻分别对应的地磁传感器y轴测量输出量by1＝-16394nT、by2＝-6282.7nT、by3＝7282.7nT，则中间变量M＝0.427057。将其代入公式(1.13)得到中间变量Q为0.4015。

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

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

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

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

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

[0112] ε＝γt-γc  (1.14)

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