采用惯导、GPS与气压计数据的飞控系统高度滤波算法

飞行高度是飞控系统研制过程中一个极其重要的参数。论文给出了一种采用GPS、惯导和气压计数据融

高度数据的准确获取是飞控系统研制过程中极其重要的一环， 是保证无人飞行器按照一定高程工作、平稳着陆的先决条件。但对于低成本惯性导航解算，位置漂移严重[1]，虽可通过加速度计姿态校正来抑制部分漂移，但解算出的速度与位置仍然不准确。因此需利用除惯导外的其它传感器测量值作为位置观测量参与滤波，在抑制位置漂移的情况下，修正速度与加速度，提高高程数据的精度。目前文献中大多是将惯性导航作为一个整体，对惯导的三维位置及速度进行滤波。如SINS/GPS 组合导航，通过组合导航对SINS 速度及位置漂移进行抑制[2] [3]。

但是当只需要高度方向上的数据时， 此种做法往往计算量大， 步骤繁琐， 且整体滤波兼顾经度、纬度、高程等多个因素， 反而影响了高度方向的滤波效果， 且当SINS/GPS组合导航中的GPS 信号较差时，得到的高度观测量误差也大。可见，当单一的高度传感器观测数据出现异常时，滤波后的高度也会出现异常。

针对单传感器无法适应复杂工作环境的缺点，本文结合GPS、气压计及惯导系统的优点，来抑制惯导高度方向上的发散。通过构建GPS 与气压计数据的权重模型获得高度方向观测量，使用互补滤波算法融合惯导数据与求得的观测量得到更为精确的高度观测值。算法简易，鲁棒性好，可在嵌入式飞控板中实时运行。

2. 高度滤波 下面分三部分阐述高度滤波，依次为姿态解算、GPS 与气压计观测量融合、高度滤波。

2.1. 纸型 四元数可以表征一个坐标系或一个矢量相对某一个坐标系的旋转，计算不存在“奇点”[4]，运算简单，所以本文采用四元数进行姿态解算。四元数微分方程采用一阶龙格-库塔进行解算，因为姿态解算不是本文研究方向，所以此处只列出公式，具体推导过程不再展示。

一阶龙格–库塔解算四元数[5]： ()( )( )( )( )( )( )( )001232xyzTqtTqtwt q twt qtwt qt+=+−−− (1) ()( )( )( )( )( )( )( )110232xzyTq tTq twt qtwt qtwt qt+=++− (2)