一种旋转飞行器滚转角解算方法及系统

摘要

本发明公开了一种旋转飞行器滚转角解算方法及系统，通过建立绕飞行器质心动力学方程组为驱动方程，以地磁方位角与俯仰角、偏航角、磁偏角、磁倾角和航向角的关系方程为观测方程，以地磁方位角的实际测量值为观测量，通过扩展卡尔曼滤波估计俯仰角和偏航角；然后解算地磁矢量基准角；最后解算出高精度的滚转角；本发明的旋转飞行器滚转角解算方法及系统，提高了滚转角的解算精度。

权利要求

1.一种旋转飞行器滚转角解算方法，其特征在于：包括：(1)建立绕飞行器质心动力学方程组为驱动方程，以地磁方位角σM与俯仰角 偏航角磁偏角D、磁倾角I和航向角αN的关系方程为观测方程，以地磁方位角σM的实际测量值为观测量，通过扩展卡尔曼滤波估计俯仰角 和偏航角所述绕飞行器质心动力学方程组为：

其中，ωη、ωζ、ωξ分别为旋转飞行器的角速率在弹轴坐标系的η、ζ、ξ轴的投影分量；

v为飞行器的飞行速度，vx、vy、vz为v在地面坐标系的三个分量；

A为赤道转动惯量，C为极转动惯量；

θa为速度高低角，ψ2为速度方向角；

kzz＝ρSldm′zz/2A，ky＝ρSldm″y/2A；

ρ为空气密度，S为飞行器横截面积，l为飞行器长度，d为飞行器直径；m′z为静力矩系数导数，m′zz为赤道阻尼力矩系数导数，m″y为马格努斯力矩系数的二阶导数；

(2)解算地磁矢量基准角γ0：

其中，

Tη、Tζ分别为地磁强度矢量 在弹轴坐标系Oξηζ的Oη轴、Oζ轴的投影分量；在弹轴坐标系Oξηζ中，原点O为旋转飞行器的质心，Oξ轴沿旋转飞行器中轴线方向向前为正，Oη轴位于包含飞行器中轴线的铅垂面内且垂直于Oξ轴向上为正，按右手法则，Oζ轴与Oξη面垂直且向右为正；

TN、TE、TD分别为地磁强度矢量 在北东地地理坐标系的三个分量；

(3)解算滚转角γ：

γ＝γ0+φS；

其中，φS为自转角，

Tby、Tbz分别为地磁强度矢量 在载体坐标系OX1Y1Z1的OY1轴、OZ1轴的投影分量；在载体坐标系OX1Y1Z1中，原点O为旋转飞行器的质心，OX1轴沿旋转飞行器中轴线方向向前为正，OY1轴和OZ1轴位于垂直于旋转飞行器中轴线的赤道面内并互相垂直。

2.根据权利要求1所述的旋转飞行器滚转角解算方法，其特征在于：在所述解算滚转角之后，还包括下述步骤：对滚转角γ进行小波滤波。

3.根据权利要求1所述的旋转飞行器滚转角解算方法，其特征在于：所述地磁方位角σM与俯仰角 偏航角 磁偏角D、磁倾角I和航向角αN的关系方程为：

4.根据权利要求1所述的旋转飞行器滚转角解算方法，其特征在于：地磁方位角σM的实际测量值通过如下公式求得：Tbx、Tby、Tbz分别由安装在旋转飞行器上的地磁传感器的三个敏感轴Sx、Sy、Sz实际测得；

三个敏感轴Sx、Sy、Sz的方向分别对应载体坐标系OX1Y1Z1的OX1轴、OY1轴、OZ1轴的方向。

5.一种旋转飞行器滚转角解算系统，其特征在于：包括：俯仰角和偏航角估计模块，用于建立绕飞行器质心动力学方程组为驱动方程，以地磁方位角σM与俯仰角 偏航角 磁偏角D、磁倾角I和航向角αN的关系方程为观测方程，以地磁方位角σM的实际测量值为观测量，通过扩展卡尔曼滤波估计俯仰角 和偏航角 所述绕飞行器质心动力学方程组为：其中，ωη、ωζ、ωξ分别为旋转飞行器的角速率在弹轴坐标系的η、ζ、ξ轴的投影分量；

v为飞行器的飞行速度，vx、vy、vz为v在地面坐标系的三个分量；

A为赤道转动惯量，C为极转动惯量；

θa为速度高低角，ψ2为速度方向角；

kzz＝ρSldm′zz/2A，ky＝ρSldm″y/2A；

ρ为空气密度，S为飞行器横截面积，l为飞行器长度，d为飞行器直径；m′z为静力矩系数导数，m′zz为赤道阻尼力矩系数导数，m″y为马格努斯力矩系数的二阶导数；

基准角解算模块，用于解算地磁矢量基准角γ0：

其中，Tη、Tζ分别为地磁强度矢量 在弹轴坐标系Oξηζ的Oη轴、Oζ轴的投影分量；在弹轴坐标系Oξηζ中，原点O为旋转飞行器的质心，Oξ轴沿旋转飞行器中轴线方向向前为正，Oη轴位于包含飞行器中轴线的铅垂面内且垂直于Oξ轴向上为正，按右手法则，Oζ轴与Oξη面垂直且向右为正；TN、TE、TD分别为地磁强度矢量 在北东地地理坐标系的三个分量；

自转角解算模块，用于解算自转角φS；

Tby、Tbz分别为地磁强度矢量 在载体坐标系OX1Y1Z1的OY1轴、OZ1轴的投影分量；在载体坐标系OX1Y1Z1中，原点O为旋转飞行器的质心，OX1轴沿旋转飞行器中轴线方向向前为正，OY1轴和OZ1轴位于垂直于旋转飞行器中轴线的赤道面内并互相垂直；

滚转角计算模块，用于解算滚转角γ；γ＝γ0+φS。

6.根据权利要求5所述的旋转飞行器滚转角解算系统，其特征在于：还包括滤波模块，用于对滚转角γ进行小波滤波。

7.根据权利要求5所述的旋转飞行器滚转角解算系统，其特征在于：所述地磁方位角σM与俯仰角 偏航角 磁偏角D、磁倾角I和航向角αN的关系方程为：

8.根据权利要求5所述的旋转飞行器滚转角解算系统，其特征在于：地磁方位角σM的实际测量值通过如下公式求得：Tbx、Tby、Tbz分别由安装在旋转飞行器上的地磁传感器的三个敏感轴Sx、Sy、Sz实际测得；

三个敏感轴Sx、Sy、Sz的方向分别对应载体坐标系OX1Y1Z1的OX1轴、OY1轴、OZ1轴的方向。

说明书

一种旋转飞行器滚转角解算方法及系统

技术领域

[0001] 本发明属于滚转角测算技术领域，具体地说，是涉及一种旋转飞行器滚转角解算方法及系统。

背景技术

[0002] 随着飞行器制导化进程的推进，滚转角的实时准确测量技术成了制导或修正控制的关键技术之一，尤其是对于一些高旋高动态的旋转智能飞行器，滚转角的准确测量已经成了核心瓶颈技术之一。

[0003] 目前，关于旋转飞行器滚转角测量的研究有很多，各有特色，主流测量方法包括卫星导航接收机测量、惯性导航器件测量、地磁传感器测量、太阳方位角测量以及组合测量等方法。在这些方法中，地磁传感器因其低成本、误差不累积、灵敏度高等优势，在完成误差补偿标定之后，非常适合测量低成本旋转飞行器的滚转角。

[0004] 地磁传感器测量滚转角的传统方法存在着较大的局限，主要思路是根据坐标转移矩阵推导滚转角的解算公式，由于公式中存在着俯仰角和偏航角两个未知参数，传统方法会采用两个横向速度角(即速度高低角和速度方位角)分别代替两个横向姿态角(即俯仰角和偏航角)的方法。然而，通过大量实验发现，在实际的旋转飞行器的飞行轨迹中，初始段和末段会由于初始扰动、气流等原因出现纵轴(飞行器中轴线)剧烈摆动的现象，姿态角与速度角的差值较大，可以达到几度，见图1所述。图1中是一组实测轨迹的地磁方位角变化规律图，它反映出了旋转飞行器在空中的姿态摆动规律。飞行轨迹的初始段和末段，摆动剧烈，摆动幅值较大，姿态角与速度角之间存在较大的差值。若采用速度角代替姿态角来解算滚转角，则在初始段和末段会产生较大的误差。

发明内容

[0005] 本发明提供了一种旋转飞行器滚转角解算方法，提高了滚转角的解算精度。

[0006] 为解决上述技术问题，本发明采用下述技术方案予以实现：

[0007] 一种旋转飞行器滚转角解算方法，包括：

[0008] (1)建立绕飞行器质心动力学方程组为驱动方程，以地磁方位角σM与俯仰角 偏航角 磁偏角D、磁倾角I和航向角αN的关系方程为观测方程，以地磁方位角σM的实际测量值为观测量，通过扩展卡尔曼滤波估计俯仰角 和偏航角

[0009] (2)解算地磁矢量基准角γ0：

[0010]

[0011]

[0012]

[0013] 其中，Tη、Tζ分别为地磁强度矢量 在弹轴坐标系Oξηζ的Oη轴、Oζ轴的投影分量；在弹轴坐标系Oξηζ中，原点O为旋转飞行器的质心，Oξ轴沿旋转飞行器中轴线方向向前为正，Oη轴位于包含飞行器中轴线的铅垂面内且垂直于Oξ轴向上为正，按右手法则，Oζ轴与Oξη面垂直且向右为正；

[0014] TN、TE、TD分别为地磁强度矢量 在北东地地理坐标系的三个分量；

[0015] (3)解算滚转角γ：

[0016] γ＝γ0+φS；其中，φS为自转角，

[0017] Tby、Tbz分别为地磁强度矢量 在载体坐标系OX1Y1Z1的OY1轴、OZ1轴的投影分量；在载体坐标系OX1Y1Z1中，原点O为旋转飞行器的质心，OX1轴沿旋转飞行器中轴线方向向前为正，OY1轴和OZ1轴位于垂直于旋转飞行器中轴线的赤道面内并互相垂直。

[0018] 进一步的，在所述解算滚转角之后，还包括下述步骤：对滚转角γ进行小波滤波。

[0019] 又进一步的，所述绕飞行器质心动力学方程组为：

[0020]

[0021] 其中，ωη、ωζ、ωξ分别为旋转飞行器的角速率在弹轴坐标系的η、ζ、ξ轴的投影分量；v为飞行器的飞行速度，vx、vy、vz为v在地面坐标系的三个分量；A为赤道转动惯量，C为极转动惯量；θa为速度高低角，ψ2为速度方向角；

[0022]

[0023] kzz＝ρSldm′zz/2A，ky＝ρSldm″y/2A；ρ为空气密度，S为飞行器横截面积，l为飞行器长度，d为飞行器直径；m′z为静力矩系数导数，m′zz为赤道阻尼力矩系数导数，m″y为马格努斯力矩系数的二阶导数。

[0024] 更进一步的，所述地磁方位角σM与俯仰角 偏航角 磁偏角D、磁倾角I和航向角αN的关系方程为：

[0025]

[0026] 再进一步的，地磁方位角σM的实际测量值通过如下公式求得：

[0027]

[0028] Tbx、Tby、Tbz分别由安装在旋转飞行器上的地磁传感器的三个敏感轴Sx、Sy、Sz实际测得；三个敏感轴Sx、Sy、Sz的方向分别对应载体坐标系OX1Y1Z1的OX1轴、OY1轴、OZ1轴的方向。

[0029] 一种旋转飞行器滚转角解算系统，包括：

[0030] 俯仰角和偏航角估计模块，用于建立绕飞行器质心动力学方程组为驱动方程，以地磁方位角σM与俯仰角 偏航角 磁偏角D、磁倾角I和航向角αN的关系方程为观测方程，以地磁方位角σM的实际测量值为观测量，通过扩展卡尔曼滤波估计俯仰角 和偏航角[0031] 基准角解算模块，用于解算地磁矢量基准角γ0：

[0032]

[0033]

[0034]

[0035] 其中，Tη、Tζ分别为地磁强度矢量 在弹轴坐标系Oξηζ的Oη轴、Oζ轴的投影分量；在弹轴坐标系Oξηζ中，原点O为旋转飞行器的质心，Oξ轴沿旋转飞行器中轴线方向向前为正，Oη轴位于包含飞行器中轴线的铅垂面内且垂直于Oξ轴向上为正，按右手法则，Oζ轴与Oξη面垂直且向右为正；TN、TE、TD分别为地磁强度矢量 在北东地地理坐标系的三个分量；

[0036] 自转角解算模块，用于解算自转角φS；

[0037] Tby、Tbz分别为地磁强度矢量 在载体坐标系OX1Y1Z1的OY1轴、OZ1轴的投影分量；在载体坐标系OX1Y1Z1中，原点O为旋转飞行器的质心，OX1轴沿旋转飞行器中轴线方向向前为正，OY1轴和OZ1轴位于垂直于旋转飞行器中轴线的赤道面内并互相垂直；

[0038] 滚转角计算模块，用于解算滚转角γ；γ＝γ0+φS。

[0039] 进一步的，还包括滤波模块，用于对滚转角γ进行小波滤波。

[0040] 又进一步的，所述绕飞行器质心动力学方程组为：

[0041]

[0042] 其中，ωη、ωζ、ωξ分别为旋转飞行器的角速率在弹轴坐标系的η、ζ、ξ轴的投影分量；v为飞行器的飞行速度，vx、vy、vz为v在地面坐标系的三个分量；A为赤道转动惯量，C为极转动惯量；θa为速度高低角，ψ2为速度方向角；

[0043]

[0044] kzz＝ρSldm′zz/2A，ky＝ρSldm″y/2A；ρ为空气密度，S为飞行器横截面积，l为飞行器长度，d为飞行器直径；m′z为静力矩系数导数，m′zz为赤道阻尼力矩系数导数，m″y为马格努斯力矩系数的二阶导数。

[0045] 更进一步的，所述地磁方位角σM与俯仰角 偏航角 磁偏角D、磁倾角I和航向角αN的关系方程为：

[0046]

[0047] 再进一步的，地磁方位角σM的实际测量值通过如下公式求得：

[0048]

[0049] Tbx、Tby、Tbz分别由安装在旋转飞行器上的地磁传感器的三个敏感轴Sx、Sy、Sz实际测得；三个敏感轴Sx、Sy、Sz的方向分别对应载体坐标系OX1Y1Z1的OX1轴、OY1轴、OZ1轴的方向。

[0050] 与现有技术相比，本发明的优点和积极效果是：本发明的旋转飞行器滚转角解算方法及系统，通过建立绕飞行器质心动力学方程组为驱动方程，以地磁方位角σM与俯仰角偏航角 磁偏角D、磁倾角I和航向角αN的关系方程为观测方程，以地磁方位角σM的实际测量值为观测量，通过扩展卡尔曼滤波估计俯仰角 和偏航角 然后解算地磁矢量基准角γ0；最后解算出高精度的滚转角γ；本实施例的解算方法，提高了滚转角的解算精度。

[0051] 结合附图阅读本发明的具体实施方式后，本发明的其他特点和优点将变得更加清楚。

附图说明

[0052] 图1是飞行器实测轨迹的地磁方位角变化规律图；

[0053] 图2是N系与V系的转动关系；图3是V系与A2系的转动关系；

[0054] 图4是三轴正交地磁传感器的三个敏感轴的示意图；

[0055] 图5是地磁方位角与俯仰角、偏航角、磁偏角、磁倾角、航向角的关系图；

[0056] 图6是地磁强度矢量 在载体坐标系的投影分量示意图；

[0057] 图7是地磁强度矢量 在弹轴坐标系的投影分量示意图；

[0058] 图8是仿真轨迹的速度角θa、ψ2与姿态角 的仿真图；

[0059] 图9是采用姿态角 解算滚转角的误差仿真图；

[0060] 图10是采用速度角θa、ψ2解算滚转角的误差仿真图；

[0061] 图11a是全轨迹俯仰角估计结果仿真图；图11b是全轨迹偏航角估计结果仿真图；图11c是俯仰角估计误差仿真图；图11d是偏航角估计误差仿真图；

[0062] 图12是俯仰角和偏航角的真值加入噪声后的姿态角估计值误差仿真图；

[0063] 图13是加入噪声后的速度角θa、ψ2和姿态角 的仿真图；

[0064] 图14是采用加噪声速度角和姿态角解算滚转角的误差仿真图；

[0065] 图15是采用小波滤波之后的滚转角解算误差仿真图；

[0066] 图16是本发明提出的飞行器滚转角解算方法的一个实施例的流程图；

[0067] 图17是本发明提出的飞行器滚转角解算系统的一个实施例的结构框图。

具体实施方式

[0068] 针对目前滚转角解算方法解算出的滚转角误差较大、精度较低的问题，本发明提出了一种旋转飞行器滚转角解算方法及系统，提高了滚转角的解算精度。为了使本发明的目的、技术方案及优点更加清楚明白，下面结合附图和实施例，对发明的旋转飞行器滚转角解算方法及系统进行详细说明。

[0069] 实施例一、

[0070] 本实施例的旋转飞行器滚转角解算方法，主要包括下述步骤，如图16所示。

[0071] 步骤S1：建立绕飞行器质心动力学方程组为驱动方程，以地磁方位角σM与俯仰角偏航角 磁偏角D、磁倾角I和航向角αN的关系方程为观测方程，以地磁方位角σM的实际测量值为观测量，通过扩展卡尔曼滤波(EKF)估计俯仰角 和偏航角

[0072] 首先，需要建立以下坐标系：

[0073] (1)地面坐标系O1XYZ：地面坐标系固连于地面，其原点O1为发射炮口断面的中心点；O1X轴沿水平线指向射击方向，与北向呈一个夹角，即航向角αN；O1Y轴位于包含发射炮中轴线的铅垂面内，向上为正；O1Z轴与O1Y、O1X轴构成右手法则向右为正,简称E系。

[0074] (2)基准坐标系OXNYNZN：基准坐标系OXNYNZN简称N系。基准坐标系OXNYNZN为右手坐标系，向前上右为正。基准坐标系是由地面坐标系平移到飞行器质心而成，原点O为旋转飞行器的质心，OXN轴指向射击方向，OYN轴垂直于水平面向上为正，根据右手法则，OZN轴与OXNYN面垂直且向右为正。

[0075] (3)弹轴坐标系Oξηζ：无滚动坐标系，随着旋转飞行器摆动但不滚转，简称A1系。原点O为旋转飞行器的质心，其Oξ轴沿旋转飞行器纵轴(飞行器中轴线)方向向前为正，Oη轴位于包含飞行器中轴线的铅垂面内且垂直于Oξ轴向上为正，按右手法则，Oζ轴与Oξη面垂直且向右为正。

[0076] (4)载体坐标系OX1Y1Z1：固连在旋转飞行器上，随着旋转飞行器旋转，简称B系。这个坐标系是随动系，随着飞行器旋转而旋转。原点O为旋转飞行器的质心，OX1轴沿旋转飞行器纵轴(飞行器中轴线)方向向前为正，OY1轴和OZ1轴位于垂直于旋转飞行器中轴线的赤道面内并互相垂直。

[0077] (5)速度坐标系OX2Y2Z2：简称V系。原点O为旋转飞行器的质心，OX2轴与旋转飞行器质心的速度方向一致，OY2轴位于包含旋转飞行器质心的速度矢量的铅垂平面内，垂直于速度矢量方向且向上为正，按右手法则，OZ2轴与OX2Y2面垂直且向右为正。V系是由N系经两次转动而来：第一次是N系绕OZN轴逆时针旋转θa角度到达OX′2Y2ZN位置，第二次是OX′2Y2ZN绕OY2轴顺时针旋转ψ2角，转动关系如附图2所示。其中，θa称为速度高低角，ψ2为速度方向角。

[0078] (6)第二弹轴坐标系Oξη2ζ2：简称A2系。原点O为旋转飞行器的质心，Oξ轴沿旋转飞行器纵轴(飞行器中轴线)方向向前为正。A2系由V系经两次旋转而来：第一次是OX2Y2Z2绕OZ2轴逆时针旋转δ1角度到达Oξ″η2Z2位置，第二次是Oξ″η2Z2绕Oη2轴顺时针旋转δ2角度，转动关系如附图3所示。其中，δ1称为高低攻角，δ2为方向攻角。

[0079] (7)北东地地理坐标系ONED：用来描述地磁矢量，简称E系。原点O为旋转飞行器的质心，ON轴指向地球北；OE轴指向地球东；OD轴垂直于地球表面并指向下。地磁强度矢量在北东地地理坐标系的三个轴的分量分别为TN、TE和TD。

[0080] 然后，还需要在旋转飞行器上安装三轴正交地磁传感器。旋转飞行器的外壳由无磁合金构成，三轴正交地磁传感器安装在外壳内，如附图4所示，三轴正交地磁传感器的三个敏感轴分别为Sx、Sy和Sz，三个敏感轴的方向分别对应载体坐标系OX1Y1Z1(B系)的OX1轴、OY1轴、OZ1轴的方向。

[0081] 下面，对俯仰角 和偏航角 进行估计。

[0082] 飞行器在地磁场中飞行时，其相对于地磁场矢量的瞬时姿态可以由地磁方位角σM表示，而地磁方位角σM又可以由俯仰角 偏航角 磁偏角D、磁倾角I和航向角αN五个角度来表示，如图5所示，关系方程表示成如下公式(1)：

[0083]

[0084] 公式(1)中，航向角αN为已知量，磁偏角D和磁倾角I可以通过GNSS提供的位置信息(经纬高信息)由地磁模型IGRF13计算得到。公式(1)可以比较准确地表示方位角与五个角度的关系，为后续准确地估计俯仰角 和偏航角 提供了基础。

[0085] 地磁方位角σM的实际测量值可以通过如下公式(2)求得：

[0086]

[0087] 公式(2)中，Tbx、Tby、Tbz分别为地磁强度矢量 在载体坐标系OX1Y1Z1的OX1、OY1轴、OZ1轴的投影分量，分别由安装在旋转飞行器上的地磁传感器的三个敏感轴Sx、Sy、Sz实际测得，为已知量。通过地磁传感器的三个敏感轴Sx、Sy、Sz测得Tbx、Tby、Tbz，并采用公式(1)计算σM的实际测量值，可以简单方便地获得比较准确的实际测量值。

[0088] 这样，式(1)中就只包含俯仰角 和偏航角 两个未知信息，可以根据飞行器的姿态运动规律估计出来。

[0089] 旋转飞行器的姿态运动规律是由绕质心运动规律来体现。假设飞行器为理想轴对称模型，不存在气动偏心和质量分布不均的情况，参考绕质心动力学运动方程组，结合实际问题建立新的动力学方程组如下，

[0090]

[0091] 其中，ωη、ωζ为旋转飞行器的两个径向角速率，ωξ为飞行器的轴向角速率，远大于两个径向角速率；也即，ωη、ωζ、ωξ分别为旋转飞行器的角速率在弹轴坐标系(A1系)的η、ζ、ξ轴的投影分量。

[0092] A为赤道转动惯量，C为极转动惯量，均为已知量。

[0093] Mη、Mζ分别为外力矩在A1系的η、ζ轴的投影分量。

[0094] 由旋转飞行器的受作用外力矩分析可知，稳定旋转的飞行器的受作用力矩主要有静力矩、赤道阻尼力矩、极阻尼力矩和马格努斯力矩。而极阻尼力矩是由飞行器旋转引起的，主要体现在飞行器纵轴上，横向分量可以不用考虑。因此，旋转稳定飞行器的外力矩在A1系中的横向分量Mη、Mζ(即η、ζ轴的投影分量)为：

[0095]

[0096] 其中，Mzη、Mzζ为静力矩在A1系的横向分量，Mzzη、Mzzζ为赤道阻尼力矩在A1系的横向分量，Myη、Myζ为马格努斯力矩在A1系的横向分量。

[0097] 静力矩在A1系的横向分量形式如下：

[0098]

[0099] 公式(5)式，ρ为空气密度，S为飞行器横截面积，l为飞行器长度；vr为考虑风时飞行器相对于风的相对速度；v为不考虑风时飞行器的速度；无风时，vr＝v；m′z为静力矩系数导数，根据飞行器的型号即可确定，vrη、vrζ为相对速度vr在A1系的横向分量(即η、ζ轴的投影分量)，且

[0100]

[0101] 式中，vrη2、vrζ2为vr在A2系的横向分量(即η2、ζ2轴的投影分量)。

[0102] 假设无风时，vr＝v，且根据V系和A2系的转动关系可以得到：

[0103]

[0104] δ1为高低攻角，δ2为方向攻角。

[0105] 则可以得到

[0106]

[0107] 将公式(8)代入公式(5)中，得到

[0108]

[0109] 而由于稳定飞行的飞行器具有特性 则上式(9)可以改写为

[0110]

[0111] 将载体参数、气象条件、力矩系数等与姿态角无直接联系的项统写为一项，即令则静力矩分量Mzη、Mzζ最终可以写为

[0112]

[0113] 同理，公式(4)中的赤道阻尼力矩在A1系的横向投影分量形式为：

[0114]

[0115] 上式中，d为飞行器直径，m′zz为赤道阻尼力矩系数导数，为已知量。令kzz＝ρSldm′zz/2A，且假设无风，vr＝v，代入公式(12)，则赤道阻尼力矩的分量Mzzη、Mzzζ最终可以写为

[0116]

[0117] 同理，公式(4)中的马格努斯力矩在A1系的横向投影分量形式为：

[0118]

[0119] 上式中，m″y为马格努斯力矩系数的二阶导数，为已知量。令ky＝ρSldm″y/2A，代入上式，可得

[0120]

[0121] 将式(11)、(13)、(15)代入式(4)，Mη、Mζ最终可以写为：

[0122]

[0123] 将式(16)代入式(3)，就得到包含俯仰角 和偏航角 的绕质心动力学方程组：

[0124]

[0125] ωξ约为飞行器转速，作为已知量。

[0126] 通过GNSS的数据计算出飞行器的飞行速度v，然后计算出飞行速度v在地面坐标系的三个轴的分量vx、vy和vz，再根据公式(18)计算速度高低角θa和速度方向角ψ2，计算公式如下：

[0127]

[0128] 然后就可以构建EKF滤波器来估计俯仰角 偏航角 以式(17)为状态驱动方程，以式(1)为观测方程，以地磁方位角σM的实际测量值为观测量，通过扩展卡尔曼滤波估计出飞行器的俯仰角 偏航角

[0129] 采用公式(17)作为驱动方程，可以解算出高精度的俯仰角、偏航角。

[0130] 根据姿态角与速度角的动力学约束方程组(17)取状态变量为

[0131]

[0132] 式(17)可以写成下式(20)：

[0133]

[0134] 非线性方程组(20)是对状态模型的近似描述，存在一定误差，特引入一个高斯白噪声W，且W～N(0,R)，其中

[0135]

[0136] 在公式(21)中，σ2是指均方差，

[0137] 分别表示ωη、ωζ、 kz、kzz、ky的均方差。

[0138] 将地磁方位角σM作为量测值，记量测变量为：y＝[σM]T。

[0139] 根据地磁方位角σM与飞行器姿态的几何关系，构建下述量测方程(22)

[0140] y＝h(x)+V

[0141]  ＝arccos(cos(I)cos(D‑αN)cos(x3)cos(x4)‑sin(I)sin(x3)+cos(I)sin(D‑αN)cos(x3)sin(x4))+V式中，V为量测噪声，V～N(0,Q)，其中 是指σM的均方差。

[0142] EKF的基本思想是将非线性系统线性化，然后进行卡尔曼线性滤波。在状态方程或测量方程为非线性时，EKF对非线性函数的Taylor展开式进行一阶线性化截断，忽略其余高阶项，从而将非线性问题转化为线性。

[0143] 对非线性状态方程(20)进行一阶线性化截断，取前两项为

[0144]

[0145] 其中，I为单位矩阵，Δt为采样间隔。

[0146] 由状态方程(20)可得状态方程的雅克比矩阵，为

[0147]

[0148] 其中，上述矩阵中的非零元素分别为

[0149] A11＝‑x6v+2x1tanx4； A13＝x7vωξ；

[0150] A15＝‑v2(x4‑ψ2)；A16＝‑x1v；A17＝vωξ(x3‑θa)；2 2

[0151] A22＝‑x6v+x1tanx4；A23＝x5v；A24＝x7vωξ+x1x2secx4；

[0152] A25＝v2(x3‑θa)；A26＝‑x2v；A27＝vωξ(x4‑ψ2)；

[0153] A41＝‑1。

[0154] 同理，由量测方程(22)可得

[0155]

[0156] 上式中，矩阵H中的非零元素为

[0157]

[0158]

[0159] a＝cos(I)cos(D‑αN)cos(x4)cos(x3)‑sin(I)sin(x3)+cos(I)sin(D‑αN)cos(x3)sin(x4)。

[0160] 至此，EKF滤波器构建完毕，就可以进行俯仰角 和偏航角 的估计。

[0161] 步骤S2、解算地磁矢量基准角γ0。

[0162] 如附图6所示，当旋转飞行器在地磁场中运动时，地磁强度矢量 在载体坐标系OX1Y1Z1(B系)上的三个轴的投影分量分别为Tbx、Tby和Tbz。其中，Tby和Tbz又构成横向投影分量TH,地磁强度矢量 与飞行器纵轴(飞行器中轴线)的夹角即为地磁方位角σM。B系相对于A1系转过的角度，即为滚转角γ。飞行器相对于地磁投影面转过的角度，即飞行器自转角φS。由于飞行器纵轴铅垂面(即图6、图7中的中轴铅垂面)与地磁投影面并不重合，因此飞行器自转角φS与滚转角γ之间相差一个地磁矢量基准角γ0。

[0163] 地磁矢量基准角γ0是飞行器纵轴铅垂面和地磁投影面之间的夹角，而地磁投影面的位置与旋转飞行器的瞬时姿态、当地的地磁矢量有关系。如附图7所示，地磁强度矢量在弹轴坐标系Oξηζ(A1系)的Oη轴和Oζ轴上的投影分量分别为Tη、Tζ，则地磁矢量基准角γ0为

[0164]

[0165] 由式(26)可知，计算地磁矢量基准角γ0需要先计算出投影分量Tη、Tζ，而计算Tη和Tζ需要两次坐标系转移：先由E系转移到N系，再由N系转移到A1系。

[0166] 则由E系到A1系的转移矩阵可以写为

[0167]

[0168] 则地磁强度矢量 在A1系的投影分量为

[0169]

[0170] 展开式(28)，其中Tη和Tζ分别为：

[0171]

[0172]

[0173] 航向角αN为实验已知量，三轴地磁分量TN、TE和TD可以通过GNSS提供的位置信息由地磁模型计算得到，俯仰角 和偏航角 已经通过上一步骤估计了出来，因此，可以计算出投影分量Tη、Tζ，进而计算出地磁矢量基准角γ0。

[0174] 步骤S3：解算滚转角γ。由附图7可以看出，滚转角γ、自转角φS和地磁矢量基准角γ0这三个角度之间存在关系式

[0175] γ＝γ0+φS        (30)

[0176] 式中，自转角φS是由安装在飞行器上的地磁传感器的两个横向敏感轴Sy、Sz测出的Tby、Tbz计算得到，计算公式如下：

[0177]

[0178] 结合(26)、(30)、(31)就可以得到滚转角γ的计算公式为：

[0179]

[0180] 本实施例的旋转飞行器滚转角解算方法，通过建立绕飞行器质心动力学方程组为驱动方程，以地磁方位角σM与俯仰角 偏航角 磁偏角D、磁倾角I和航向角αN的关系方程为观测方程，以地磁方位角σM的实际测量值为观测量，通过扩展卡尔曼滤波估计俯仰角和偏航角 再解算地磁矢量基准角γ0；最后解算出高精度的滚转角γ；本实施例的解算方法，提高了滚转角的解算精度。

[0181] 为了进一步提高滚转角的解算精度，本实施例的解算方法还包括步骤S4：对滚转角γ进行小波滤波，以进一步提高滚转角的解算精度。滚转角的解算精度与计算过程中俯仰角 和偏航角 的估计精度有直接关系，而俯仰角 和偏航角 的最大误差来源是估计值的残余噪声，具有高斯白噪声的特性，因此可以通过小波滤波等方法过滤掉残余噪声，进一步提高滚转角的解算精度。

[0182] 本实施例的基于GNSS/地磁传感器的旋转飞行器滚转角高精度解算方法，构建了三轴正交地磁传感器，通过分析旋转飞行器在飞行中的滚转姿态变化规律和地球磁场矢量在不同坐标系上的投影关系，推导出了旋转飞行器滚转角γ的解算公式；同时推导了滚转角解算所需要的地磁矢量基准角γ0的计算公式；而对于解算地磁矢量基准角γ0所需要的俯仰角 和偏航角 则以包含俯仰角 和偏航角 的绕质心动力学方程组为驱动方程，以地磁方位角σM的角度关系为观测方程，以测量得到的地磁方位角为观测量，通过扩展卡尔曼滤波估计出高精度的俯仰角 和偏航角 最后根据残余噪声的高斯白噪声特性，采用小波滤波器对滚转角γ进行小波滤波，进一步提高解算精度。

[0183] 下面进行轨迹仿真验证。由于转台试验无法模拟旋转飞行器全轨迹的绕质心运动，而真实飞行试验又无法获得姿态角真值作为参考，因此，验证阶段将采用六自由度刚体飞行器轨迹仿真验证的方法。仿真初始位置为120.4407°E，36.1358°N，海拔35m。仿真对象为某型号旋转飞行器，直径约为155mm，初速800m/s，初始俯仰角为45°，航向角为100°。通过六自由度刚体飞行器轨迹方程模拟出整条轨迹信息，包括：飞行速度v，飞行器在地面坐标系的位置x、y、z，速度高低角θa，速度方向角ψ2，飞行器在载体坐标系的三轴角速度ωx、ωy、ωz，俯仰角 偏航角 滚转角γ以及飞行器转速 为方便展示细节，部分仿真只取10s时间。同时根据位置信息通过IGRF13地磁模型计算出全轨迹的地磁信息，主要是北东地方向的地磁分量TN、TE和TD。

[0184] 附图8展示了仿真轨迹的速度角θa、ψ2分别与姿态角 的区别，可以看到，在轨迹初始段，姿态角出现了剧烈摆动，与速度角出现较大的偏差。

[0185] (1)无误差姿态角和速度角分别解算滚转角

[0186] 首先采用姿态角 和 按照式(29)计算出地磁强度矢量 在A1系Oη轴和Oζ轴上的投影分量Tη、Tζ，同时模拟的地磁信号计算出地磁强度矢量 在载体坐标系上的投影分量Tby和Tbz。最后，通过公式(32)计算出飞行器的滚转角，计算结果如附图9所示。由附图9可以看‑10出，采用姿态角 和 按照公式(32)解算的滚转角精度非常高，误差在10 度的量级。此时误差的主要产生原因应该是软件计算时的误差，这说明公式的计算结果是精确的。

[0187] 再将式(29)中的姿态角 用速度角θa、ψ2代替，按照同样的解算过程计算飞行器滚转角，计算结果如图10所示，由图中看出，解算误差的最大值约为2.5°，出现在飞行器摆动最大的初始段，然后不断随着摆动幅值的减小不断减小。因此可以得出结论，滚转角的解算精度与计算过程中采用的姿态角 和 精度有直接关系，俯仰角和偏航角的估计精度越高，滚转角的解算精度越高。

[0188] (2)带误差的姿态角估计值解算滚转角

[0189] 按照本实施例提出的方法，按照EKF滤波器估计出俯仰角和偏航角，与真值进行比较，如附图11所示。图11a、11b中可以看出，飞行器共飞行100多秒，全轨迹的飞行器俯仰角和偏航角的估计值与真值贴近，规律一致，估计效果较好。图11c、11d为全轨迹的俯仰角和偏航角估计误差，从这两幅图中可以看出，俯仰角的误差范围大约为[‑0.2°,0.25°]，偏航角的误差范围约为[‑0.25°,0.15°]。从图中也可以看出，最大的估计误差出现在初始阶段和轨迹顶点阶段。

[0190] 为了更好的验证本实施例解算方法的效果，为两个姿态角( 和 )的真值分别加入高斯白噪声d～N(0,0.2°)，模拟EKF滤波后的姿态角估计值，估计值的误差如图12所示，最大误差约为0.8°。同时为两个速度角(θa、ψ2)加入同样的噪声，以对比解算误差的精度效果，如图13所示。

[0191] 按照本实施例的方法分别采用速度角和姿态角信息解算滚转角，解算误差如附图14所示。由图中可以看出，采用姿态角解算滚转角的最大误差约为0.8°，采用速度角解算滚转角的误差从轨迹初始段的3°逐渐降为0.8°。

[0192] 从仿真结果可以看出，滚转角的直接计算误差与姿态角的估计误差存在大致相等的关系，可以通过小波滤波等方法进一步提高滚转角的解算精度。附图15展示的是采用了小波滤波之后的滚转角解算误差。采用速度角解算滚转角时，最大误差约为2.5°，变化不大，这是因为，采用速度角解算滚转角的最大误差来源是速度角与姿态角的真值差，无法通过小波滤波去除。而采用姿态角解算滚转角的最大误差来源是姿态角估计值的残余噪声，可以通过小波滤波等方法进一步过滤掉，滤波后精度可以达到0.1°以内，精度明显提高。

[0193] 通过完善的轨迹仿真验证，证明了本实施例的滚转角解算方法的解算精度可以达到较高水平，解算精度要高于传统方法，可以有效的提供准确的高精度滚转角，为旋转飞行器的测试、参数测量、导航制导与控制提供技术支持。

[0194] 实施例二、

[0195] 基于实施例一的旋转飞行器滚转角解算方法的设计，本实施例提出了一种旋转飞行器滚转角解算系统，包括俯仰角和偏航角估计模块、基准角解算模块、自转角解算模块、滚转角计算模块等，参见图17所示。

[0196] 俯仰角和偏航角估计模块，用于建立绕飞行器质心动力学方程组为驱动方程，以地磁方位角σM与俯仰角 偏航角 磁偏角D、磁倾角I和航向角αN的关系方程为观测方程，以地磁方位角σM的实际测量值为观测量，通过扩展卡尔曼滤波估计俯仰角 和偏航角[0197] 基准角解算模块，用于解算地磁矢量基准角γ0：

[0198]

[0199]

[0200]

[0201] 其中，Tη、Tζ分别为地磁强度矢量 在弹轴坐标系Oξηζ的Oη轴、Oζ轴的投影分量；在弹轴坐标系Oξηζ中，原点O为旋转飞行器的质心，Oξ轴沿旋转飞行器中轴线方向向前为正，Oη轴位于包含飞行器中轴线的铅垂面内且垂直于Oξ轴向上为正，按右手法则，Oζ轴与Oξη面垂直且向右为正；TN、TE、TD分别为地磁强度矢量 在北东地地理坐标系的三个分量。

[0202] 自转角解算模块，用于解算自转角φS；

[0203] Tby、Tbz分别为地磁强度矢量 在载体坐标系OX1Y1Z1的OY1轴、OZ1轴的投影分量；在载体坐标系OX1Y1Z1中，原点O为旋转飞行器的质心，OX1轴沿旋转飞行器中轴线方向向前为正，OY1轴和OZ1轴位于垂直于旋转飞行器中轴线的赤道面内并互相垂直。

[0204] 滚转角计算模块，用于解算滚转角γ；γ＝γ0+φS。

[0205] 滚转角解算系统还包括滤波模块，用于对滚转角γ进行小波滤波。

[0206] 在本实施例中，所述绕飞行器质心动力学方程组为：

[0207]

[0208] 其中，ωη、ωζ、ωξ分别为旋转飞行器的角速率在弹轴坐标系的η、ζ、ξ轴的投影分量；v为飞行器的飞行速度，vx、vy、vz为v在地面坐标系的三个分量；A为赤道转动惯量，C为极转动惯量；θa为速度高低角，ψ2为速度方向角；

[0209]

[0210] kzz＝ρSldm′zz/2A，ky＝ρSldm″y/2A；ρ为空气密度，S为飞行器横截面积，l为飞行器长度，d为飞行器直径；m′z为静力矩系数导数，m′zz为赤道阻尼力矩系数导数，m″y为马格努斯力矩系数的二阶导数。

[0211] 在本实施例中，所述地磁方位角σM与俯仰角 偏航角 磁偏角D、磁倾角I和航向角αN的关系方程为：

[0212]

[0213] 在本实施例中，地磁方位角σM的实际测量值通过如下公式求得：

[0214]

[0215] Tbx、Tby、Tbz分别由安装在旋转飞行器上的地磁传感器的三个敏感轴Sx、Sy、Sz实际测得；三个敏感轴Sx、Sy、Sz的方向分别对应载体坐标系OX1Y1Z1的OX1轴、OY1轴、OZ1轴的方向。

[0216] 具体的旋转飞行器滚转角解算系统的工作过程，已经在上述旋转飞行器滚转角解算方法中详述，此处不予赘述。

[0217] 本实施例的旋转飞行器滚转角解算系统，通过建立绕飞行器质心动力学方程组为驱动方程，以地磁方位角σM与俯仰角 偏航角 磁偏角D、磁倾角I和航向角αN的关系方程为观测方程，以地磁方位角σM的实际测量值为观测量，通过扩展卡尔曼滤波估计俯仰角和偏航角 然后解算地磁矢量基准角γ0；最后解算出高精度的滚转角γ；本实施例的解算系统，提高了滚转角解算精度。

[0218] 当然，上述说明并非是对本发明的限制，本发明也并不仅限于上述举例，本技术领域的普通技术人员在本发明的实质范围内所做出的变化、改型、添加或替换，也应属于本发明的保护范围。