一种用于救灾无人机姿态控制的互补滤波方法技术

本发明专利技术公开了一种用于救灾无人机姿态控制的互补滤波方法，所述方法包括如下步骤：(1)由于加速度计的输出中含有高平噪声，在数据融合之前，对加速度计的解算值进行低通滤波，滤除噪声干扰和累积增量；(2)由于陀螺仪在静态下，存在温度漂移现象，因此对陀螺仪的解算结果进行高通滤波；(3)加速度计解算姿态角具有较好的低频特性，而高频特性较差，而陀螺仪具有较好的高频动态特性，利用二者频率特性的互补性，进行数据的重构融合。利用本发明专利技术的方法，可以较为准确地计算姿态角的值，从而对无人机的姿态进行控制。

【技术实现步骤摘要】


本专利技术涉及防灾减灾
，特别是涉及一种用于救灾无人机姿态控制的互补滤波方法。

技术介绍

我国幅员辽阔，气候及自然环境复杂多变，各类极端天气与气候日益频发，地震、火山、台风、龙卷、山洪、泥石流等自然灾害已经对我国人民生命财产安全构成了极大的威胁，对基础设施建设、工农业生产、林牧养殖造成了严重破坏。灾害预警是指灾害发生前应急网络的建立和灾害信息的发布，以提前做出针对灾害的防范工作，尽最大可能降低灾害所带来的破坏。在与自然灾害的长期斗争过程中，我国已逐步形成了一系列基于观测数据的灾害预警机制，自然灾害的预警信息系统已在实际工作中得到应用。但传统的灾害预警方法信息化和智能化程度不高，预警不及时、信息发布缓慢，且覆盖范围有限，在恶劣的天气环境下信号的传输更是得不到保证。难以做好应对灾害的充分准备。传统的气象基于多终端的灾害预警及信息发布系统存在着严重缺陷。
无人机具有速度快、视野广、机动性好、不惧怕恶劣自然环境的优点，搭载智能探测设备的无人机在自然灾害预警信息采集中发挥着重要作用。传统的无人机姿态控制中，姿态角可以由陀螺仪积分得出，也可以通过加速度传感器测量重力加速度在载体系三轴上的向量分解坐标推导得出。其中陀螺仪解算姿态角具有如下优势：(1)高频动态响应特性良好，其输出能够迅速响应姿态角的变化；(2)无高频噪声干扰，输出值平滑；(3)陀螺仪的输出不受外部加速度的干扰，在载体处于剧烈震动的情况下仍能维持稳定的输出；但也具有如下缺陷：(1)陀螺仪的零点会随温度及其它外部环境因素的改变而产生漂移；(2)对陀螺仪输出值的积分会产生累计误差，长时间运行后解算误差较大。利用加速度计解算姿态角具有如下优势：(1)低频特性良好，静态输出稳定，没有漂移和累积误差；(2)姿态解算计算量小，在没有外部加速度时(仅有重力加速度)的情况下，能够迅速得出准确的瞬时姿态角数据；但也具有如下缺陷：(1)有高频噪声干扰；(2)高频动态特性较差，其输出不能迅速响应姿态角的快速变化；(3)容易受到外部加速度的干扰，如果存在除重力加速度之外的加速度，将无法得出准确的姿态角。

技术实现思路

本专利技术的目的是针对现有技术中存在的技术缺陷，而提供一种用于救灾无人机姿态控制的互补滤波方法。
为实现本专利技术的目的，本专利技术所采用的技术方案是：
一种用于救灾无人机姿态控制的互补滤波方法，所述方法包括如下步骤：
(1)由于加速度计的输出中含有高平噪声，在数据融合之前，对加速度计的解算值进行低通滤波，滤除噪声干扰和累积增量；
(2)由于陀螺仪在静态下，存在温度漂移现象，因此对陀螺仪的解算结果进行高通滤波；
(3)加速度计解算姿态角具有较好的低频特性，而高频特性较差，而陀螺仪具有较好的高频动态特性，利用二者频率特性的互补性，进行数据的重构融合。
所述方法包括如下步骤：将原始输入信号按傅立叶分解展开，即：
对于原始信号中的正弦分量，将会被低通滤波器极大的衰减，但能以很小的衰减通过高通滤波器；对于原始信号中的正弦分量，将会被高通滤波器极大的衰减，但能以很小的衰减通过低通滤波器，将高通滤波器和低通滤波器的输出进行整合，即可得到全部频域内的信号。
所述高通滤波算法如下：
设某一采样时刻，由陀螺仪解算直接得到的姿态角度为θgyro(k)(俯仰角、横滚角或偏航角)，前一次采样时刻由陀螺仪解算直接得到的结果为θgyro(k-1)，前一次采样时刻高通滤波之后的姿态角为θHighpass(k-1)，则当前采样时刻高通滤波之后的姿态角θHighpass(k)的表达式为：
θ H i g h p a s s ( k ) = R C R C + T [ θ g y r o ( k ) - θ g y r o ( k - 1 ) + θ H i g h p a s s ( k - 1 ) ] ]]>理想的高通滤波的输出量中，仅包含原始信号中频谱高于截止频率的动态部分信号，当原始信号处于低频或维持状态时，高通滤波的输出为0，由于高通滤波器滤掉了原始信号中的直流成分和低频成分，因此高通滤波的输出不能真实的反映原始信号相对于零点的实际值，但能够真实的反映原始信号在任一时刻的采样值相对于上一次采样值的高频改变量。
所述低通滤波算法如下：
设某一采样时刻，由加速度计解算直接得到的姿态角为θacc(k)，前一次采样低通滤波之后的姿态角为θLowpass(k-1)，则当前采样时刻低通滤波之后的姿态角θLowpass(k)的表达式为：
θ L o w p a s s ( k ) = T R C + T [ θ a c c ( k ) - θ L o w p a s s ( k - 1 ) ] + θ L o w p a s s ( k - 1 ) ]]>低通滤波的输出量中，仅包含原始信号中频谱低于截止频率的部分信号，原本文档来自技高网...

【技术保护点】
一种用于救灾无人机姿态控制的互补滤波方法，其特征在于，所述方法包括如下步骤：(1)由于加速度计的输出中含有高平噪声，在数据融合之前，对加速度计的解算值进行低通滤波，滤除噪声干扰和累积增量；(2)由于陀螺仪在静态下，存在温度漂移现象，因此对陀螺仪的解算结果进行高通滤波；(3)加速度计解算姿态角具有较好的低频特性，而高频特性较差，而陀螺仪具有较好的高频动态特性，利用二者频率特性的互补性，进行数据的重构融合。

【技术特征摘要】
1.一种用于救灾无人机姿态控制的互补滤波方法，其特征在于，所述方法
包括如下步骤：
(1)由于加速度计的输出中含有高平噪声，在数据融合之前，对加速度计
的解算值进行低通滤波，滤除噪声干扰和累积增量；
(2)由于陀螺仪在静态下，存在温度漂移现象，因此对陀螺仪的解算结果
进行高通滤波；
(3)加速度计解算姿态角具有较好的低频特性，而高频特性较差，而陀螺
仪具有较好的高频动态特性，利用二者频率特性的互补性，进行数据的重构融
合。
2.根据权利要求1所述的一种用于救灾无人机姿态控制的互补滤波方法，其
特征在于，
所述方法包括如下步骤：将原始输入信号按傅立叶分解展开，即：
对于原始信号中的正弦分量，将会被低通滤波器极大的衰减，但能
以很小的衰减通过高通滤波器；对于原始信号中的正弦分量，将会被高
通滤波器极大的衰减，但能以很小的衰减通过低通滤波器，将高通滤波器和低
通滤波器的输出进行整合，即可得到全部频域内的信号。
3.根据权利要求1所述的一种用于救灾无人机姿态控制的互补滤波方法，其
特征在于，所述高通滤波算法如下：
设某一采样时刻，由陀螺仪解算直接得到的姿态角度为θgyro(k)(俯仰角、横
滚角或偏航角)，前一次采样时刻由陀螺仪解算直接得到的结果为θgyro(k-1)，前
一次采样时刻高通滤波之后的姿态角为θHighpass(k-1)，则当前采样时刻高通滤波
之后的姿态角θHighpass(k)的表达式为：
θ H i g h p a s s ( k ) = R C R C + T [ θ g y r o ( k ) - θ g y r o ( k - 1 ) + θ H i g h p a s s ( k - 1 ) ] ]]>理想的高通滤波的输出量中，仅包含原始信号中频谱高于截止频率的动态部
分信号，当原始信号处于低频或维持状态时，高通滤波的输出为0，由于高通滤
波器滤掉了原始信号中的直流成分和低频成分，因此高通滤波的输出不能真实
的反映原始信号相对于零点的实际值，但能够真实的反映原始信号在任一时刻
的采样值相对于上一次采样值的高频改变量。
4.根据权利要求3所述的一种用于救灾无人机姿态控制的互补滤波方法，其
特征在于，所述低通滤波算法如下：
设某一采样时刻，由加速度计解算直接得到的姿态角为θacc(k)，前一次采样
低通滤波之后的姿态角为θLowpass(k-1)，则当前采样时刻低通滤波之后的姿态角
θLowpass(k)的表达式为：
θ Lowp a s s ( k ) = T RC + T [ θ acc ( k ) - θ Lowpass ( k - 1 ) + θ lowpa s s ( k - 1 ) ]]>低通滤波的输出量中，仅包含原始信号中频谱低于截止频率的部分信号，原
始信号中频率高于截止频率的信号和噪声都被衰减。因此低通滤波的输出能够
反映原始信号中的低频部分。
5.根据权利要求4所述的一种用于救灾无人机姿态控制的互补滤波方法，其
特征在于，所述高通滤波算法如下：
在没有外加速度干扰的情况下，互补滤波姿态解算数据融合的基本公式为：
θ(k)＝θHighpass(k)+θLowpass(k)
6.根据权利要求1所述的一种用于救灾无人机姿态控...

【专利技术属性】
技术研发人员：张德馨，薛文芳，孙哲南，
申请(专利权)人：天津中科智能识别产业技术研究院有限公司，
类型：发明
国别省市：天津;12