一种适用于垂直下视航空吊舱的姿态测量方法与流程

1.本发明涉及一种gnss/惯性组合导航定位领域，特别是一种适用于垂直下视航空吊舱的姿态测量方法。


背景技术：

2.垂直下视是航空吊舱目标导引的一种情况，该模式以光电转台自身的经度、纬度和高度实时作为目标引导的输入信息，然后基于空间中各个坐标系之间的变换理论建立测量方程，解算目标引导角度，最终通过实时控制光电转台方位框角度ψ和俯仰框角度θ的转动，以保持平台坐标系与地理坐标系的重合，使得光学镜头的视轴与地球椭球面垂直。
3.在进行目标的搜索、跟踪和定位过程中首先需要获得光电吊舱自身的偏航角、姿态角（俯仰角、横滚角）和位置（纬度、经度和高度）信息，然后根据自身的信息进行目标的定位和跟踪，因此光电吊舱自身的测姿、定位精度直接决定了目标的定位精度。为了消除减震器、传输时延等带来的误差，目前航空吊舱一般将惯性传感器与航空吊舱的光电转台捷联安装。
4.中国发明专利“基于机载光电吊舱位置和姿态的目标定位系统”（申请号：201510938847.9，公开日：2016.05.04）公开了一种基于gnss（global navigation satellite system,全球导航卫星系统）双天线、陀螺仪和加速度计的测姿定位方法，该方法将mems惯性测量模块与光电吊舱的转动轴固连，在姿态更新时先通过角增量求解等效旋转矢量，再利用等效旋转矢量更新四元数，然后利用四元数转换到姿态矩阵，最后通过姿态矩阵更新欧拉角。该方法应用于垂直下视航空调仓中存在以下两方面问题，其一，利用姿态矩阵更新欧拉角时，当俯仰角θ趋近于
±
90
°
时需要设置阈值做近似计算，导致俯仰角越趋近于
±
90
°
，计算的姿态角误差越大，导致求解失真；其二，该方法限定俯仰角范围为[-90
º
， 90
º
]，而垂直下视航空吊舱工作时，惯导俯仰角会越过-90
º
，甚至会在-90
ꢀº
左右摆动，导致俯仰角不连续、横滚角和偏航角突变180度，无法保持姿态角的连续性。
[0005]
因此，在垂直下视航空吊舱测姿应用领域，现有技术存在的问题是：俯仰角越趋近于
±
90
°
时，计算的姿态角误差越大；惯导俯仰角越过-90
º
时姿态角更新不连续。


技术实现要素：

[0006]
本发明的目的在于，提供一种适用于垂直下视航空吊舱的姿态测量方法。本发明当俯仰角越趋近
±
90
°
时，姿态角精度不损失，且惯导俯仰角越过-90
º
时姿态角更新保持连续性。
[0007]
本发明的技术方案：一种适用于垂直下视航空吊舱的姿态测量方法，包括以下步骤：a、传感器安装；将惯性测量模块与光电吊舱的光电转台固连，将gnss天线安装在飞机顶部；b、初始对准；
利用gnss天线的位置信息，惯导系统进行静态初始对准，得到惯性测量模块位姿初始值；c、惯性测量模块位姿信息更新；利用惯性测量模块输出的角增量和加速度增量数据，通过捷联惯导姿态、速度和位置更新算法计算并更新惯性测量模块的位置、速度和姿态矩阵；d、组合滤波；采用自适应扩展卡尔曼滤波器，利用惯性测量模块的速度、位置信息和gnss天线速度、位置进行卡尔曼滤波，估算惯性器件误差；e、全角度姿态角解算与更新：采用全角度双欧法进行正反欧拉角计算，并实时更新姿态角。
[0008]
前述的一种适用于垂直下视航空吊舱的姿态测量方法中，步骤a中，惯性测量模块的x、y和z轴分别与飞机的纵向轴、横向轴和天向轴保持平行。
[0009]
前述的一种适用于垂直下视航空吊舱的姿态测量方法中，惯性测量模块位姿初始值包括姿态、方位、速度和位置。
[0010]
前述的一种适用于垂直下视航空吊舱的姿态测量方法中，步骤e的具体过程为：e1）输入角速度和正、反欧拉角；e2）根据θ角判断正、反欧拉若为正欧拉，则对下面公式1）进行积分求解正欧拉角，并利用转换关系求解反欧拉角；1）；若为反欧拉，则对下面公式2）进行积分，求解反欧拉角，并利用转换关系求解正欧拉角；2）；式中，、和为惯性测量模块输出的x、y、z三个轴向的角速度；e3）对计算得到的正欧拉角和反欧拉角进行取舍，得到更新后的正欧拉角和反欧拉角；
e4）将更新后的正欧拉角和反欧拉角输入至步骤 e1）中。
[0011]
前述的一种适用于垂直下视航空吊舱的姿态测量方法中，步骤e2）中，转换关系通过设置转换矩阵实现：，其中，为航向角，为俯仰角，为横滚角。
[0012]
前述的一种适用于垂直下视航空吊舱的姿态测量方法中，通过正欧拉角求解反欧拉角的具体过程为：取，反欧拉角的计算方法如下：，以上各式中取0或1，对应两种结果。
[0013]
前述的一种适用于垂直下视航空吊舱的姿态测量方法中，由反欧拉角求解正欧拉角的具体过程如下：取，正欧拉角的计算方法如下：
ꢀ
；以上各式中取0或1，对应两种结果。
[0014]
前述的一种适用于垂直下视航空吊舱的姿态测量方法中，正、反欧拉角取舍的具体过程为：首先定义如下函数，在微分方程求解过程中，设某时刻各正欧拉角分别为：，反欧拉角分别为：，则下一时刻正欧拉角设为：，反欧拉角设为：，令，上式中，取0或1；由正欧拉角求解反欧拉角时，如果，则有，，并以该反欧拉角进行更新，得到更新后的反欧拉角；如
果，则有。
[0015]
由反欧拉角求解正欧拉角时，如果，则有，，并以该正欧拉角进行更新，得到更新后的正欧拉角；如果，则有。
[0016]
与现有技术相比，本发明的显著优点为：1、姿态角精度无损失：本发明利用正反欧拉方程奇异性呈现倒挂的关系，以二者解算准确的精华区进行分区接替运算，消除奇异区，发挥精华区解算姿态角最简洁、准确的有点，即使当俯仰角越趋近
±
90
°
或越过于-90
°
时，姿态角解算也不会降低精度。
[0017]
2、连续性好：在使用双欧拉角法进行姿态解算时，当俯仰角越过-90
°
时，通过定义一个函数自动进行判断，使得正反欧拉角能够一一对应，实现全角度解算，保证了姿态角解算的连续性。
附图说明
[0018]
图1为本发明一种适用于垂直下视航空吊舱的姿态测量方法的主流程图；图2为图1中全角度姿态解算与更新步骤中一个时间周期的运算流程图。
具体实施方式
[0019]
下面结合附图和实施例对本发明作进一步的说明，但并不作为对本发明限制的依据。
[0020]
实施例。一种适用于垂直下视航空吊舱的姿态测量方法，包括如下步骤（如图1所示）：（10）传感器安装：将惯性测量模块与光电吊舱的光电转台固连，gnss天线安装在飞机顶部。
[0021]
将惯性测量模块与光电吊舱的光电转台固连时，使得光电吊舱保持锁定时，惯性测量模块的x、y和z轴分别与飞机的纵向轴、横向轴和天向轴保持平行该步骤中，惯性测量模块与光电吊舱的光电转台固连，直接敏感吊舱的角运动和线运动，保证了测量信息的实时性和同步性。
[0022]
（20）初始对准：通过gnss天线接收gnss卫星信号，获得gnss天线位置、速度，并利用位置信息进行静态初始对准，得到惯性测量模块位姿初始值，包括姿态、方位、速度和位置。
[0023]
初始对准阶段要求光电吊舱处于锁定状态。
[0024]
（30）惯性测量模块位、姿信息更新：
利用惯性测量模块输出的角增量和加速度增量数据，通过捷联惯导姿态、速度和位置更新算法计算并更新惯性测量模块的位置、速度和姿态矩阵。
[0025]
姿态更新公式如下：姿态更新公式如下：，式中，为当前时刻的姿态矩阵，为上一时刻的姿态矩阵，为当前计算周期内通过角增量计算的等效旋转矢量，为当前计算周期内通过导航系相对于惯性系旋转角度计算的等效旋转矢量。
[0026]
速度更新公式如下：式中，为当前时刻的速度，为上一时刻的速度，为由惯性测量模块输出的加速度增量计算得到的速度增量，为由载体运动、和地球自转和地球重力引起的有害加速度增量。
[0027]
位置更新公式如下：式中，为当前时刻的位置，为上一时刻的位置，当前时刻的速度，上一时刻的速度，为计算周期。
[0028]
（40）组合滤波：采用自适应扩展卡尔曼滤波器，利用惯性测量模块的速度、位置信息和gnss天线速度、位置进行卡尔曼滤波，估算惯性器件误差x。
[0029]
式中，姿态角误差，为速度误差，为位置误差，为陀螺仪零偏误差，为加速度计偏值误差；为状态转移矩阵，为测量方程矩阵，为位置速度测量噪声，为陀螺仪和加速度计测量白噪声，为系统噪声分配矩阵。
[0030]
初始对准完成后要求光电吊舱继续处于锁定状态，等待飞机运动300秒，惯性器件误差估计收敛后，吊舱解除锁定，光学视轴可转动到垂直下视模式，此时惯性测量模块的俯仰角趋近或越过于-90
°
，或者越过-90
°
。
[0031]
（50）全角度姿态解算与更新：流程如图2所示，首先，输入角速度和正、反欧拉角；其次，根据θ角判断正、反欧拉；若为正欧拉则对下面公式进行积分求解正欧拉角，并利用转换关系求解反欧拉角；；式中，为惯性测量模块输出的x、y、z三个轴向的角速度。
[0032]
若为反欧拉则对下面公式进行积分，求解反欧拉角，并利用转换关系求解正欧拉角；；然后，对计算得到的正欧拉角和反欧拉角进行取舍，得到更新后的正欧拉角和反欧拉角；最后，将更新后的正欧拉角和反欧拉角输入至第一步中。
[0033]
转换关系通过设置转换矩阵实现：
，其中，为航向角，为俯仰角，为横滚角。
[0034]
由正欧拉角求解反欧拉角的具体步骤为：，即各元素由正欧拉角求得，反欧拉角的计算方法如下：，以上各式中取0或1，对应两种结果。
[0035]
由反欧拉角求解正欧拉角：，即各元素由反欧拉角求得，正欧拉角的计算方法如下：
以上各式中取0或1，对应两种结果。
[0036]
正反欧拉角取舍：定义如下函数：在微分方程求解过程中，设某时刻各正欧拉角分别为：，反欧拉角分别为：，则下一时刻正欧拉角设为：，反欧拉角设为：，令，上式中，取0或1。
[0037]
由正欧拉角求解反欧拉角时，如果，则有，，并以该反欧拉角进行更新，得到更新后的反欧拉角；如果，则有。
[0038]
由反欧拉角求解正欧拉角时，如果，则有，，并以该正欧拉角进行更新，得到更新后的正欧拉角；如果，则有。