ABSTRACT

Flight altitude is a very important parameter in the development of flight control system. This paper presents a height filtering algorithm fusing GPS, INS and barometer data. First, the altitude fusion model of GPS and barometer is constructed, and then combined with inertial navigation data, the algorithm uses complementary filtering to filter altitude, velocity and position in the navigation coordinate system. The static and simulated dynamic tests whose data is collected by flight control board in laboratory reveal that the algorithm can give accurate relatively height, and also velocity and acceleration. The algorithm eliminates the adverse effects of the filtering result when the signal of single sensor is weak or the data is poor.

Keywords:Inertial Navigation, Altitude Filter, Measurement Value, GPS, Barometer

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

田博显

北京航空航天大学，北京

收稿日期：2018年11月18日；录用日期：2018年12月5日；发布日期：2018年12月12日

摘 要

飞行高度是飞控系统研制过程中一个极其重要的参数。论文给出了一种采用GPS、惯导和气压计数据融合的飞控系统高度滤波算法。算法构建了GPS与气压计高度融合模型，并结合惯导数据，使用互补滤波对导航坐标系下的高度、速度及位置进行滤波。利用实验室飞控平台，通过静态及模拟动态测试，验证滤波算法能够得到较为准确的高度、速度与加速度。算法解决了单一传感器信号较弱或数据较差时对滤波效果产生的不利影响。

关键词 :惯性导航，高度滤波，观测量，GPS，气压计

Copyright © 2018 by author and Hans Publishers Inc.

This work is licensed under the Creative Commons Attribution International License (CC BY).

http://creativecommons.org/licenses/by/4.0/

1. 引言

高度数据的准确获取是飞控系统研制过程中极其重要的一环，是保证无人飞行器按照一定高程工作、平稳着陆的先决条件。但对于低成本惯性导航解算，位置漂移严重 [1] ，虽可通过加速度计姿态校正来抑制部分漂移，但解算出的速度与位置仍然不准确。因此需利用除惯导外的其它传感器测量值作为位置观测量参与滤波，在抑制位置漂移的情况下，修正速度与加速度，提高高程数据的精度。目前文献中大多是将惯性导航作为一个整体，对惯导的三维位置及速度进行滤波。如SINS/GPS组合导航，通过组合导航对SINS速度及位置漂移进行抑制 [2] [3] 。但是当只需要高度方向上的数据时，此种做法往往计算量大，步骤繁琐，且整体滤波兼顾经度、纬度、高程等多个因素，反而影响了高度方向的滤波效果，且当SINS/GPS组合导航中的GPS信号较差时，得到的高度观测量误差也大。可见，当单一的高度传感器观测数据出现异常时，滤波后的高度也会出现异常。

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

2. 高度滤波

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

2.1. 纸型

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

一阶龙格–库塔解算四元数 [5] ：

$q_0\left(t+T\right)=q_0\left(t\right)+\frac{T}{2}\left[-w_x\left(t\right)q_1\left(t\right)-w_y\left(t\right)q_2\left(t\right)-w_z\left(t\right)q_3\left(t\right)\right]$ (1)

$q_1\left(t+T\right)=q_1\left(t\right)+\frac{T}{2}\left[w_x\left(t\right)q_0\left(t\right)+w_z\left(t\right)q_2\left(t\right)-w_y\left(t\right)q_3\left(t\right)\right]$ (2)

$q_2\left(t+T\right)=q_2\left(t\right)+\frac{T}{2}\left[w_y\left(t\right)q_0\left(t\right)-w_z\left(t\right)q_1\left(t\right)+w_x\left(t\right)q_3\left(t\right)\right]$ (3)

$q_3\left(t+T\right)=q_3\left(t\right)+\frac{T}{2}\left[w_z\left(t\right)q_0\left(t\right)+w_y\left(t\right)q_1\left(t\right)-w_x\left(t\right)q_2\left(t\right)\right]$ (4)

其中 $q_0\left(t\right)$ ， $q_1\left(t\right)$ ， $q_2\left(t\right)$ ， $q_3\left(t\right)$ 为t时刻的四元数， $\overrightarrow{q}\left(t\right)={\left[q_0\left(t\right),q_1\left(t\right),q_2\left(t\right),q_3\left(t\right)\right]}^T$ ， $w_x\left(t\right)$ ， $w_y\left(t\right)$ ， $w_z\left(t\right)$ 分别为机体坐标系下 $x$ ， $y$ ， $z$ 轴的角速度值，亦即陀螺仪的输出。

解算出四元数后，可以从四元数中提取由机体坐标系到导航系的旋转矩阵：

$Cbn=\left[\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 2\left(q_1{q}_2-q_0{q}_3\right)& 2\left(q_1{q}_3+q_0{q}_2\right)\\ 2\left(q_1{q}_2+q_0{q}_3\right)& q_0^2+q_2^2-q_1^2-q_3^2& 2\left(q_2{q}_3-q_0{q}_1\right)\\ 2\left(q_1{q}_3-q_0{q}_2\right)& 2\left(q_2{q}_3+q_0{q}_1\right)& q_0^2+q_3^2-q_1^2-q_2^2\end{array}\right]$ (5)

同时可得由导航系到机体坐标系的旋转矩阵

$Cnb={\left(Cbn\right)}^T$ (6)

由于陀螺仪漂移，若没有其它传感器数据进行校正，单纯用陀螺仪数据解算得到的姿态会慢慢漂移，此时通过姿态矩阵转换至导航系的加速度积分得到的速度和位置漂移会越来越快。所以，需要进行姿态校正。目前文献普遍用加速度计对姿态进行水平校正，用磁力计进行偏航角校正，滤波算法有互补滤波 [6] [7] 、卡尔曼滤波 [8] [9] 等。因为加速度计测量加速度由重力加速度及运动加速度组成，所以当采用加速度计对水平姿态进行校正时，解算得到的姿态会被运动加速度污染。综上，无论单纯使用陀螺仪解算姿态或是采用加速度计进行校正，积分得到的高度均需要进行滤波。本文采用互补滤波算法 [6] 用加速度计及磁力计对姿态进行校正。

然后将加速度计测量载体坐标系下的加速度转换至导航系下，得到导航系下的加速度。

$\overrightarrow{a_n}=Cbn\ast \overrightarrow{a_b}$ (7)

其中 $Cbn$ 为姿态解算得到的旋转矩阵， $\overrightarrow{a_b}$ 为载体坐标系下的加速度值，亦即加速度计输出值， $\overrightarrow{a_n}$ 为导航系下的加速度值。

2.2. GPS与气压计高度测量值融合

将GPS高度测量值与气压计高度值融合后的高度作为观测量。

$\widehat{H}=K_H{H}_1+\left(1-K_H\right)H_2$ (8)

上式中 $H_1$ 为GPS观测高度， $H_2$ 为气压计观测高度， $\widehat{H}$ 为GPS与气压计观测高度融合后的高度值，作为滤波抑制惯导高度发散的观测量。其中 $K_H$ 为比例因子，表征GPS高度测量值与气压计测量值在观测量中所占的比例大小。

GPS卫星数与三维位置精度因子pDOP是决定GPS定位精度高低的关键因素。可用GPS卫星为4个时，能够实现初步定位，但是此时精度较差，具体表现为，当载体静止不动时，测量得到的高度值在几十米甚至是一百多米的范围内变动，此时的GPS数据只能确定载体在地球中的粗略位置，无法实现精确定位。特别是对于多旋翼无人机这样活动范围较小的载体，需要更为精确的定为。GPS卫星相对于用户的几何分布对GPS解算得到的位置精度有很大影响，在GPS导航和定位中，使用几何精度因子来衡量可用卫星的空间几何分布对定位精度的影响。其中pDOP三维位置精度因子为GPS几何精度因子中的重要一个，其值越小，代表GPS解算得到的位置信息精度越高，当小于3时表明有效定位。GPS卫星数与pDOP也存在一定关联，因为可用的卫星数越多，往往卫星几何位置越能够满足要求，即GPS卫星数越多，pDOP越小。

根据上述分析，设GPS搜到的卫星数与其定位精度成正比，pDOP与其定位精度成反比，因此可将比例系数 $K_H$ 的计算公式设为：

$K_H=\frac{n}{14}\ast \frac{1.1}{x}$ (9)

其中， $n$ 为GPS搜到的卫星数， $x$ 为pDOP三维位置精度因子。

由 $K_H$ 值计算公式知当搜到的卫星数越多，且pDOP三维位置精度因子越小时， $K_H$ 值越大，越接近1，即作为观测量GPS高度值所占比例越大。因为当GPS信号良好时，其高度测量值不受大风等环境影响，数据稳定性比气压计要高，所以此时更加信任GPS数据；当搜到的卫星数较少，且pDOP三维位置精度因子较大时， $K_H$ 越小，越接近0，此时GPS信号较差，误差太大，观测量更加信任气压计数据。因此，加入比例因子 $K_H$ 后，能够在GPS高度与气压计高度任何一方出现问题时，保证滤波后的高度稳定收敛不发散。

因为GPS输出高度值为WGS-84坐标系下高度值，气压计计算高度值为气压高度，两者为不同基准，但都表示垂直方向上的距离变化，所以需要转换至同一基准下的高度值，然后再进行高度融合。

2.3. GPS与气压计高度测量值融合

采用互补滤波对惯导垂直方向位置、速度及加速度进行滤波。

首先计算高度偏差：

$\Delta H\left(k\right)=\widehat{H}\left(k\right)-H^{\ast }\left(k-n\right)$ (10)

其中， $H^{\ast }\left(k-n\right)$ 为距当前 $k$ 时刻 $n$ 个滤波周期前的滤波高度历史值。因为GPS与气压计传感器观测高度存在延迟，所以为了在时间上保持计算同步，将当前的观测高度与 $n$ 个滤波周期前的滤波高度作差值求取高度偏差。 $\Delta H\left(k\right)$ 为观测高度与滤波高度的偏差值。

加速度，速度及高度修正值计算：

$a_c\left(k\right)=a_c\left(k-1\right)+\Delta H\left(k\right)K_a\Delta T\left(k\right)$ (11)

$v_c\left(k\right)=v_c\left(k-1\right)+\Delta H\left(k\right)K_v\Delta T\left(k\right)$ (12)

$p_c\left(k\right)=p_c\left(k-1\right)+\Delta H\left(k\right)K_p\Delta T\left(k\right)$ (13)

其中 $a_c\left(k\right)$ 、 $v_c\left(k\right)$ 及 $p_c\left(k\right)$ 分别为第 $k$ 时刻的加速度、速度和高度修正值。 $\Delta T\left(k\right)$ 为第 $k$ 个滤波周期与第 $k-1$ 个滤波周期之间的间隔时间。 $K_a$ 、 $K_v$ 及 $K_p$ 分别为加速度、速度及高度修正值的计算比例系数，其计算方式为：

$K_a=1/K^3$ (14)

$K_v=1/K^2$ (15)

$K_p=1/K$ (16)

其中 $K$ 可取2~10之间的整数值，可以根据滤波效果具体调整。

然后，进行加速度校正：

$a^{\ast }\left(k\right)=a_n\left(k\right)+a_c\left(k\right)$ (17)

其中 $a^{\ast }\left(k\right)$ 为校正后的加速度值， $a_n\left(k\right)$ 为加速度计输出转换至导航系下的垂直方向加速度值。

进行高度计算及高度校正：

$\Delta v\left(k\right)=\left(a^{\ast }\left(k\right)+a^{\ast }\left(k-1\right)\right)\Delta T\left(k\right)/2$ (18)

$H_o\left(k\right)=H_o\left(k-1\right)+\left[v^{\ast }\left(k-1\right)+\left(v^{\ast }\left(k-1\right)+\Delta v\left(k\right)/2\right)\right]\Delta T\left(k\right)$ (19)

$H^{\ast }\left(k\right)=H_o\left(k\right)+p_c\left(k\right)$ (20)

其中 $\Delta v\left(k\right)$ 为第 $k-1$ 时刻至 $k$ 时刻的速度增量， $H^{\ast }\left(k\right)$ 为最终滤波后的高度， $H_o\left(k\right)$ 为中间计算未经滤波后的高度， $v^{\ast }\left(k-1\right)$ 为 $k-1$ 时刻的滤波速度。

计算速度及进行速度校正：

$v_o\left(k\right)=v_o\left(k-1\right)+\Delta v\left(k\right)$ (21)

$v^{\ast }\left(k\right)=v_o\left(k\right)+v_c\left(k\right)$ (22)

其中 $v^{\ast }\left(k\right)$ 为滤波后的速度， $v_o\left(k\right)$ 为第 $k$ 时刻未经校正的速度计算值。

3. 实验

为了验证算法在实际工作环境中的有效性及稳定性，利用实验室飞控板平台对数据采集处理(图1)。因为高度滤波时姿态解算周期设为5 ms，所以设置PC机对飞控板数据获取周期为5 ms。在5 ms的时间内，飞控板通过串口将获取的加速度计、陀螺仪、磁力计、GPS数据、气压计数据传输到PC机中。

图1. 飞控板-陀螺仪数据采集平台

飞控板的IMU传感器采用ADIS16448，此IMU集成三轴加速度计、陀螺仪及磁力计，数据更新频率高达1000 Hz，高达1 MHz的SPI总线保证数据传输速率。GPS采用ublox NEO-7N，更新频率设置为元器件最大，10 Hz。数据处理芯片DSP采用浮点处理器TMS320C6722，主频高达250 MHz。GPS与IMU通过一套SPI总线连接至DSP，高速SPI总线保证GPS与IMU数据的实时获取。气压计采用MS5611，考虑到气压测量精度，将气压计更新频率设置为50 Hz。

当在空旷的室外测试时，GPS接收机搜到的卫星数均能够大于9，且pDOP三维位置精度因子在1.5左右，此时定位精度较高。当在有高楼遮挡的室外时，如家属小区内，GPS接收机搜到的卫星数约4颗上下，pDOP三维精度因子在3到8之间变化，此时定位精度较差。

采集数据时，在三种情况下对数据进行采集。在开阔无高大遮挡物的室外，且无风的环境中，此时GPS信号强，气压计数据良好；在开阔无高大遮挡物以及有大风的室外，此时GPS信号强，气压计数据差；有高大建筑物遮挡且无风的室外，如家属小区内，此时GPS信号差，气压计数据良好。数据采集后，在PC机中用matlab对数据进行处理，观察滤波效果。

1) 首先测试在静态情况下，当GPS信号强，且气压计数据较好，即漂移不是很严重时的滤波效果。GPS信号较好，是指搜到的卫星数大于等于9，pDOP三维位置精度因子小于3。处理数据为静态情况下10分钟数据，时间间隔5 ms (图2)。

图2. GPS与气压计数据良好时静态滤波数据曲线

图(a)中的观测高度为GPS高度值与气压计高度值公式(10)融合后的高度值。由图可知，滤波后的高度值能够稳定在一定范围内，且滤波后的速度与加速度值在短时间内收敛后都能够稳定在零值附近。

2) 测试静态情况下，当GPS信号强，气压计数据差时的滤波效果。处理数据为十分钟数据，数据时间间隔为5 ms。

由图3中曲线知，气压计高度值在静态情况下，上下滑动范围在7 m左右，在有大风的环境中使得气压计数据不够精确。滤波后的高度在气压计数据较差时依然能够稳定在小范围内，且滤波后的速度与加速度值能够稳定在零值附近。

3) 测试静态情况下，当GPS信号弱，但是气压计数据较好时的情景。处理数据为十分钟数据，数据时间间隔为5 ms (图4)。

图3. 气压计数据差时静态滤波数据曲线

图4. GPS信号弱时静态滤波数据曲线

因为在小区内进行数据采集，所以GPS信号较差，由图知GPS测量高度变动范围较大。经过滤波后的高度能够稳定在小范围内，且滤波后速度与加速度在短暂时间内均能收敛至零，在零值上下浮动。

因为试验条件限制，为了测试动态环境下的滤波效果，在3.1试验数据的基础上进行模拟动态测试，在导航系下，分别模拟匀速垂直向上运动与匀加速垂直向上运动，进而观察滤波效果。

4) 测试当GPS信号较好，且气压计数据良好，但是飞行器处于动态情况下的滤波效果。在(1)测试数据的基础上，在导航系下给机体加入一个定值向上速度，模拟机体向上匀速直线运动，然后观测滤波结果，设定速率值大小为0.1 m/s。数据为10分钟数据，数据时间间隔为5 ms。

由图5可知，在动态情况下，滤波高度保持稳定，没有发散。滤波后的速度也稳定在设定速度值，滤波后加速度值稳定在零值。高度值、速度值及加速度值偏差均能稳定在零值固定范围内，说明了算法稳定性。

5) 观察匀加速直线运动下的滤波效果。在(1)数据的基础上模拟飞行器处于匀加速下的滤波效果。在导航系下给机体加入一个定值向上加速度，模拟机体向上匀加速直线运动。设定加速度值为0.01。数据为10分钟数据，数据时间间隔为5 ms。

图5. 匀速运动滤波数据曲线

由图6中曲线知滤波后高度收敛不发散，滤波后的速度及加速度均稳定在理论值。高度、速度及加速度误差能够稳定在零值小范围内。

图6. 匀加速运动滤波数据曲线

综上，在静态及模拟动态环境中对滤波效果进行了验证，试验结果表明，在静态与动态中滤波都能够有效发挥作用，得到较为准确且稳定的高度、速度与加速度。

4. 结论

本文提出了一种融合GPS高度与气压计数据的飞控系统高度滤波算法，并在静态下的各种情况对滤波效果进行了测试，且模拟了动态环境，测试算法在动态环境下的鲁棒性。试验结果表明，滤波算法能够很好适应观测量信号的强弱性，且能够在动态情况下保持稳定。因为试验条件限制，只能模拟动态情况进行测试。但是惯导、GPS及气压计的作业环境往往多变且复杂，特别是对于无人机，如机体晃动时的姿态变化是否对高度滤波产生影响等，还需要进一步进行测试。本质上，抑制惯导高度漂移的观测量仍然是决定算法是否漂移的决定因素，因为算法解算的高度精度依然依赖观测量，所以当GPS信号较差且气压计观测高度较差时，滤波解算得到的高度也会较差，在这方面还有待改进。

基金项目

北京市自然科学基金(4162034)；青海省科技计划项目(2016-NK-138)。

文章引用

田博显. 采用惯导、GPS与气压计数据的飞控系统高度滤波算法
Altitude Filter Algorithm Based on Fused Data of INS, GPS and Barometer[J]. 国际航空航天科学, 2018, 06(04): 77-87. https://doi.org/10.12677/JAST.2018.64009

参考文献