一种抗大气参数漂移的无人机飞行导航系统高度信息融合方法与流程

本发明涉及一种抗大气参数漂移的无人机飞行导航系统高度信息融合方法，适用于低成本气压高度传感器和MEMS IMU的无人机飞行导航系统的高度通道估计。

背景技术：

微小型无人机采用MEMS级别的IMU及低成本的气压高度计等作为测量传感器。IMU包含加速度、陀螺仪等传感器，与磁罗盘可构成无人机系统的AHRS系统，为无人机提供可靠的姿态信息。导航系统准确的高度、速度估计结果为无人机的高度控制提供有力的保障。微小型无人机系统集成的高度通道测量传感器受体积、质量、成本的影响，存在着漂移、延迟、随机噪声大等问题。本发明针对传统的气压高度传感器存在的问题，提出了一种利用差分气压高度观测量抗大气参数漂移的高度信息融合方法。

传统的高度通道估计一般采用多传感器、单一融合方法进行信息融合,无法民气压高度计的大气漂移及延迟等问题。

技术实现要素：

本发明的技术解决问题是：克服传统低成本气压高度计的漂移、延迟等问题，提供一种抗大气参数漂移的无人机飞行导航系统高度信息融合方法，在多传感器信息融合的基础上，采用互补滤波器及卡尔曼滤波器双滤波器，根据无人机的运动特性自适应分配双滤波器的权重，充分发挥两种滤波器各自的优势，具有更好的可靠性，为无人机高度通道提供较优的高度、速度估计。

本发明的技术解决方案为：一种抗大气参数漂移的无人机飞行导航系统高度信息融合方法，采用基站气压传感器及机上气压传感器构建气压高度差分观测量，抑制气压高度传感器的大气环境漂移误差；使用惯性测量单元IMU及磁传感器构成的AHRS(Attitude and Heading Reference System，航姿参考系统)进行姿态更新，获得机上加速度在导航系统的天向加速度分量，同时与气压高度差分量测量进行信息融合，获得高度通道的高度、速度估计值；使用加计辅助气压高度的互补滤波器CF(Complementary Filter)+气压高度辅助加计的卡尔曼滤波器KF(Kalman Filter)构成的双滤波器实现高度通道的信息融合，实现自适应无人机动态特性的较优融合。

具体包括以下步骤：

(1)采用基站气压传感器与机上气压传感器构造气压差分观测量。基站气压传感器安装在地面系统中，保持静止状态。基站与机上气压传感器在小于5公里的飞行范围内的大气漂移量近乎相同，大气漂移为缓变量，故基站气压传感器经过预处理后以1HZ的频率发送到机上处理器，与机上气压传感器构造气压差分量测量以减小气压高度的漂移误差。惯性测量单元IMU为MEMS(Micro-electromechanical Systems)工艺惯性测量单元，满足小型无人机机载电子设备低功耗、小体积、轻质量的限制。

(2)无人机的天向加速度由载体系的加速度通过阵分解得到。阵则由IMU及磁传感器组成的AHRS系统实时更新的姿态信息计算得到。

(3)子滤波器1(CF)为加计辅助气压高度的互补滤波器，利用加速度计算高度与气压高度量测量噪声的互补特性实现高度信息估计。加速度计算高度噪声具有低频统计特性，气压高度量测量具有高频统计特性，采用互补滤波器进行信息融合得到较为平滑的估计结果。

(4)子滤波器2(KF)为气压高度辅助加速度的卡尔曼滤波器，具有较好的动态响应。子滤波器2分为动态递推和滤波更新两个过程。在非滤波时刻利用加速度与高度的动力学方程对高度通道的状态量进行递推解算，在滤波时刻利用气压高度量测量校正高度估计的状态量。子滤波器2具有较好的动态响应，利用加速度与高度的动力学方程在非滤波时刻对高度通道的状态量进行递推解算，滤波时刻利用气压高度量测量校正高度估计的状态量。

(5)在每个计算周期，均采取加计辅助气压高度的互补滤波器+气压高度辅助加计的卡尔曼滤波器双滤波器进行滤波更新，根据无人机的运动特性自适应分配双滤波器的权重，进行高度通道的信息融合。

(6)使用加计辅助气压高度的互补滤波器CF+气压高度辅助加计的卡尔曼滤波器KF构成的双滤波器实现高度通道的信息融合时，根据无人机的运动特性自适应分配双滤波器的权重，进行信息融合；当无人机处于运动状态时，因互补滤波器CF的高度、速度估计结果受气压高度延迟影响，故加大卡尔曼滤波器KF的融合权重，提高整个滤波器的动态响应能力；当无人机处于静止或平稳运动状态时，互补滤波器CF的高度、速度估计结果的平滑性优于卡尔曼滤波器KF，加大互补滤波器的融合权重。

(7)当天向加速度的均值、方差以及速度均小于相应的门限值时，则判断无人机在高度方向上处于静止或平稳运动状态，加大互补滤波器的权重，获得更平滑的滤波结果；反之则判断无人机在高度方向上有机动运动，处于运动状态，加大卡尔曼滤波器的权重，获得更好的动态特性。

(8)传感器量测数据更新，上述步骤重复计算。

本发明的原理是：气压高度传感器为高度状态量的估计提供直接观测量，采用气压差分观测量抑制高度观测量的大气漂移误差。利用机上的IMU及磁传感器组成的AHRS系统，计算天向加速度观测量，实现多传感器在高度通道上的信息融合。使用互补滤波器/卡尔曼滤波器双滤波器的信息融合方法，根据无人机的运动特性自适应分配双滤波器的权重，充分发挥两种滤波器各自最大的优势，得到无人机高度、速度的较优估计。

本发明与现有技术相比的优点在于：

(1)本发明针对微小型无人机的低成本气压高度量测量存在的漂移、延迟、随机噪声大等问题，提出了一种抗大气参数漂移的信息融合方法。采用基站与机上气压计的差分量作为子滤波器的观测量抑制大气参数的漂移误差。相对于传统的高度通道信息融合技术，本发明采用多传感器、多融合方法进行高度通道的信息融合。用互补滤波器及卡尔曼滤波器双滤波器，根据无人机的运动特性自适应分配双滤波器的权重，充分发挥两种滤波器各自的优势，具有更好的可靠性，为无人机高度通道提供较优的高度、速度估计。

(2)本发明以低成本气压高度计和MEMS IMU等为测量传感器，采用基站与机上的气压差分观测量，以互补滤波器和卡尔曼滤波器双滤波器进行信息融合，改善气压高度计的大气漂移及延迟等问题，为无人机提供较优的高度、速度估计值。本发明适用于低成本气压高度传感器和MEMS IMU的无人机飞行导航系统的高度通道估计。

附图说明

图1为本发明的抗大气参数漂移的无人机飞行导航系统高度信息融合方法流程图。

具体实施方式

如图1所示，本发明的具体方法如下：

(1)基站气压传感器经过预处理后以一定的频率发送到机上处理器，与机上气压传感器构造气压差分量测量h_diff＝h_d-h_b。h_d为机上气压测量值，h_b为基站气压测量值，h_diff为气压差分量测量。

(2)选择东北天坐标系为无人机的导航坐标系，由机上的IMU及磁传感器组成的AHRS系统得到姿态角(φθψ)，φ、θ、ψ分别为滚转、俯仰和航向姿态角，计算方向余弦矩阵由方向余弦矩阵将机体系下的加速度转换到导航系下，获得天向加速度观测量a_U。分别为机体系下的加速度，a_E、a_N、a_U分别东、北、天向加速度。

(3)将步骤(1)、(2)计算得到的气压差分观测量h_diff和天向加速度a_U送入子滤波器1(加计辅助气压高度的互补滤波器)和子滤波器2(气压高度辅助加计的卡尔曼滤波器)进行滤波估计分别得到k时刻子滤波器1和子滤波器2的高度、速度估计值和

(4)子滤波器1的计算方法如下，由天向加速度a_U对前一滤波时刻得到的高度、速度估计进行状态更新得利用气压高度与状态更新的高度作为误差校正量闭环校正高度、速度估计。得到子滤波器1的高度、速度估计量

(5)子滤波器2的计算方法如下，在非滤波时刻t时刻，由天向加速度a_U对前一时刻的高度、速度估计进行状态更新。达到滤波时刻k时刻，利用气压高度观测量作为高度量测量对子滤波器2进行量测更新获得子滤波器2的高度、速度估计子滤波器2在非滤波时刻充分信赖加速度传感器的数据，具有较好的动态响应能力。

(6)对子滤波器1和子滤波器2的高度、速度估计值和进行信息融合，融合权重分别为β₁和β₂。β₁和β₂均小于1，且β₁+β₂＝1。当β₁＞β₂时，对CF滤波器信赖的权重更大；当β₁＜β₂时，对KF滤波器信赖的权重更大。

(7)根据无人机当前的运动特性自适应分配双滤波器的融合权重β₁、β₂，β₁+β₂＝1。当1s内天向加速度的均值a_m、方差a_std以及天向速度v_u均小于相应的门限值时，则判断无人机在高度方向上处于静止或平稳运动状态。天向加速度的均值门限可取0.2m/s²，加速度方差门限值与加速度传感器的噪声统计特性有关，对于RMS为的MEMS加速度传感器，阈值可取0.05，速度门限可取2-5m/s。

当无人机处于静止或平稳运动状态，此时应加大互补滤波器的权重，β₁＞β₂，获得更平滑的滤波结果。当无人机在高度方向上有机动运动，处于运动状态，则加大卡尔曼滤波器的权重，β₁＜β₂，获得更好的动态特性。具体的权重计算公式由上述阈值可设计如下所示。

(8)在测量值更新时迭代重复上述过程。

总之，本发明使用加计辅助气压高度的CF+气压高度辅助加计的KF双滤波器实现高度通道的信息融合，改善气压高度的延迟问题，实现自适应无人机动态特性的高度通道的较优融合。本发明适用于低成本气压高度传感器和MEMS IMU的无人机飞行导航系统的高度通道估计。

本发明说明书中未作详细描述的内容属于本领域专业技术人员公知的现有技术。

提供以上实施例仅仅是为了描述本发明的目的，而并非要限制本发明的范围。本发明的范围由所附权利要求限定。不脱离本发明的精神和原理而做出的各种等同替换和修改，均应涵盖在本发明的范围之内。