CN105739510A - 一种用于救灾无人机姿态控制的互补滤波方法 - Google Patents

Abstract

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

Description

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

技术领域

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

背景技术

我国幅员辽阔，气候及自然环境复杂多变，各类极端天气与气候日益频发，地震、火山、台风、龙卷、山洪、泥石流等自然灾害已经对我国人民生命财产安全构成了极大的威胁，对基础设施建设、工农业生产、林牧养殖造成了严重破坏。灾害预警是指灾害发生前应急网络的建立和灾害信息的发布，以提前做出针对灾害的防范工作，尽最大可能降低灾害所带来的破坏。在与自然灾害的长期斗争过程中，我国已逐步形成了一系列基于观测数据的灾害预警机制，自然灾害的预警信息***已在实际工作中得到应用。但传统的灾害预警方法信息化和智能化程度不高，预警不及时、信息发布缓慢，且覆盖范围有限，在恶劣的天气环境下信号的传输更是得不到保证。难以做好应对灾害的充分准备。传统的气象基于多终端的灾害预警及信息发布***存在着严重缺陷。

无人机具有速度快、视野广、机动性好、不惧怕恶劣自然环境的优点，搭载智能探测设备的无人机在自然灾害预警信息采集中发挥着重要作用。传统的无人机姿态控制中，姿态角可以由陀螺仪积分得出，也可以通过加速度传感器测量重力加速度在载体系三轴上的向量分解坐标推导得出。其中陀螺仪解算姿态角具有如下优势：(1)高频动态响应特性良好，其输出能够迅速响应姿态角的变化；(2)无高频噪声干扰，输出值平滑；(3)陀螺仪的输出不受外部加速度的干扰，在载体处于剧烈震动的情况下仍能维持稳定的输出；但也具有如下缺陷：(1)陀螺仪的零点会随温度及其它外部环境因素的改变而产生漂移；(2)对陀螺仪输出值的积分会产生累计误差，长时间运行后解算误差较大。利用加速度计解算姿态角具有如下优势：(1)低频特性良好，静态输出稳定，没有漂移和累积误差；(2)姿态解算计算量小，在没有外部加速度时(仅有重力加速度)的情况下，能够迅速得出准确的瞬时姿态角数据；但也具有如下缺陷：(1)有高频噪声干扰；(2)高频动态特性较差，其输出不能迅速响应姿态角的快速变化；(3)容易受到外部加速度的干扰，如果存在除重力加速度之外的加速度，将无法得出准确的姿态角。

发明内容

本发明的目的是针对现有技术中存在的技术缺陷，而提供一种用于救灾无人机姿态控制的互补滤波方法。

为实现本发明的目的，本发明所采用的技术方案是：

一种用于救灾无人机姿态控制的互补滤波方法，所述方法包括如下步骤：

(1)由于加速度计的输出中含有高平噪声，在数据融合之前，对加速度计的解算值进行低通滤波，滤除噪声干扰和累积增量；

(2)由于陀螺仪在静态下，存在温度漂移现象，因此对陀螺仪的解算结果进行高通滤波；

(3)加速度计解算姿态角具有较好的低频特性，而高频特性较差，而陀螺仪具有较好的高频动态特性，利用二者频率特性的互补性，进行数据的重构融合。

所述方法包括如下步骤：将原始输入信号按傅立叶分解展开，即：

对于原始信号中的正弦分量，将会被低通滤波器极大的衰减，但能以很小的衰减通过高通滤波器；对于原始信号中的正弦分量，将会被高通滤波器极大的衰减，但能以很小的衰减通过低通滤波器，将高通滤波器和低通滤波器的输出进行整合，即可得到全部频域内的信号。

所述高通滤波算法如下：

设某一采样时刻，由陀螺仪解算直接得到的姿态角度为θ_gyro(k)(俯仰角、横滚角或偏航角)，前一次采样时刻由陀螺仪解算直接得到的结果为θ_gyro(k-1)，前一次采样时刻高通滤波之后的姿态角为θ_Highpass(k-1)，则当前采样时刻高通滤波之后的姿态角θ_Highpass(k)的表达式为：

${\mathrm{\θ}}_{Highpass}\left(k\right)=\frac{RC}{RC+T}\[{\mathrm{\θ}}_{gyro}\left(k\right)-{\mathrm{\θ}}_{gyro}(k-1)+{\mathrm{\θ}}_{Highpass}\left(k-1\right)\]$

理想的高通滤波的输出量中，仅包含原始信号中频谱高于截止频率的动态部分信号，当原始信号处于低频或维持状态时，高通滤波的输出为0，由于高通滤波器滤掉了原始信号中的直流成分和低频成分，因此高通滤波的输出不能真实的反映原始信号相对于零点的实际值，但能够真实的反映原始信号在任一时刻的采样值相对于上一次采样值的高频改变量。

所述低通滤波算法如下：

设某一采样时刻，由加速度计解算直接得到的姿态角为θ_acc(k)，前一次采样低通滤波之后的姿态角为θ_Lowpass(k-1)，则当前采样时刻低通滤波之后的姿态角θ_Lowpass(k)的表达式为：

${\mathrm{\θ}}_{Lowpass}\left(k\right)=\frac{T}{RC+T}\[{\mathrm{\θ}}_{acc}\left(k\right)-{\mathrm{\θ}}_{Lowpass}(k-1)\]+{\mathrm{\θ}}_{Lowpass}(k-1)$

低通滤波的输出量中，仅包含原始信号中频谱低于截止频率的部分信号，原始信号中频率高于截止频率的信号和噪声都被衰减。因此低通滤波的输出能够反映原始信号中的低频部分。

所述高通滤波算法如下：

在没有外加速度干扰的情况下，互补滤波姿态解算数据融合的基本公式为：

θ(k)＝θ_Highpass(k)+θ_Lowpass(k)

加速度计解算角度的准确度受外部加速度的影响很大，只有在没有外部加速度的情况下，加速度计的解算结果才是完全可信的，当存在外部加速度时，加速度计解算结果的准确性明显变差，且变差趋势随着外部加速度的增大而加剧，因此，在存在外加速度的情况下，就要降低互补滤波器高通和低通滤波器的截止频率，削弱加速度计解算结果在数据融合中所占的权重而加强陀螺仪解算结果在数据融合中所占的权重，a_e为外部加速度，f_c为截止频率，外加速度越大，截止频率越低，当外加速度大至一定程度时，使截止频率接近于0，完全由陀螺仪进行姿态的解算，

用加速度计在载体系XYZ三轴上测量值所构成的向量模值，与重力加速度进行比较，评估外加速度的影响度，设置参数：

$\mathrm{\ξ}=\frac{|\sqrt{a_x^2+a_y^2+a_z^2}-g|}{g}\∈(0,\mathrm{\∞})$

其中，a_x、a_y、a_z分别为加速度计在载体系XYZ三轴上的测量值；g为重力加速度的模值，ξ的值越大，则表示作用于载体的外部加速越大；ξ的值越接近于0，表示作用于载体的外部加速越小，

令互补滤波器中，高通滤波器和低通滤波器的传递函数分别为：

$G_H\left(s\right)=\frac{\[1+tan\left(4\mathrm{\π}\mathrm{\ξ}\right)\]\·RCs}{\[1+tan\left(4\mathrm{\π}\mathrm{\ξ}\right)\]\·RCs+1},\mathrm{\ξ}\∈\[0,0.125)$

$G_L\left(s\right)=\frac{1}{\[1+tan\left(4\mathrm{\π}\mathrm{\ξ}\right)\]\·RCs+1},\mathrm{\ξ}\∈\[0,0.125)$

则当ξ∈[0，0.125)时，互补滤波器的截止频率为：

$f_c=\frac{1}{2\mathrm{\π}RC\·\[1+tan\left(4\mathrm{\π}\mathrm{\ξ}\right)\]}$

可以发现，若ξ＝0，则当ξ逐渐增大至接近0.125时，f_c→0。当ξ≥0.125时，认为此时的外部加速度过大，完全由陀螺仪进行姿态角的解算。

与现有技术相比，本发明的有益效果是：

利用本发明的方法，可以较为准确地计算姿态角的值，从而对无人机的姿态进行控制。

附图说明

图1所示为本发明方法示意图；

具体实施方式

以下结合附图和具体实施例对本发明作进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

应当说明的是，本申请中所述的“连接”和用于表达“连接”的词语，如“相连接”、“相连”等，其既可以指代某一部件与另一部件直接连接，也可以指代某一部件通过其他部件与另一部件相连接。

如图1所示，由于加速度计的输出中含有高平噪声，在数据融合之前，对加速度计的解算值进行低通滤波，滤除噪声干扰和累积增量；由于陀螺仪在静态下，存在温度漂移现象，因此对陀螺仪的解算结果进行高通滤波。加速度计解算姿态角具有较好的低频特性，而高频特性较差，而陀螺仪具有较好的高频动态特性，利用二者频率特性的互补性，进行数据的重构融合。对于陀螺仪解算结果，认为其动态量是准确的。对于加速度计，认为在没有外部加速的情况下，其解算结果是准确的。加速度计解算结果的准确度受外部加速度的影响较大，需要根据外部加速度的大小实时的改变互补滤波其参数，消除外加速度对解算结果的影响。

互补滤波算法说明

互补虑波器的基本原理，将原始输入信号按傅立叶分解展开，即：

对于原始信号中的正弦分量，将会被低通滤波器极大的衰减，但能以很小的衰减通过高通滤波器；对于原始信号中的正弦分量，将会被高通滤波器极大的衰减，但能以很小的衰减通过低通滤波器。将高通滤波器和低通滤波器的输出进行整合，即可得到全部频域内的信号。

陀螺仪部分数据的高通滤波

为滤除陀螺仪输出结果中由于温漂等因素造成的低频干扰，要对陀螺仪的解算结果进行高通滤波。设某一采样时刻，由陀螺仪解算直接得到的姿态角度为θ_gyro(k)(俯仰角、横滚角或偏航角)，前一次采样时刻由陀螺仪解算直接得到的结果为θ_gyro(k-1)，前一次采样时刻高通滤波之后的姿态角为θ_Highpass(k-1)，则当前采样时刻高通滤波之后的姿态角θ_Highpass(k)的表达式为：

${\mathrm{\θ}}_{Highpass}\left(k\right)=\frac{RC}{RC+T}\[{\mathrm{\θ}}_{gyro}\left(k\right)-{\mathrm{\θ}}_{gyro}(k-1)+{\mathrm{\θ}}_{Highpass}(k-1)\]$

理想的高通滤波的输出量中，仅包含原始信号中频谱高于截止频率的动态部分信号，当原始信号处于低频或维持状态时，高通滤波的输出为0。由于高通滤波器滤掉了原始信号中的直流成分和低频成分，因此高通滤波的输出不能真实的反映原始信号相对于零点的实际值，但能够真实的反映原始信号在任一时刻的采样值相对于上一次采样值的高频改变量。

3.3.2加速度传感器部分数据的低通滤波

加速度计的解算结果中，存在高频噪声干扰和由于高频动态特性变差造成的存在较大误差的结果，但认为其静态值或低频动态值是准确的。因此，为同时消除加速度计解算结果中的高频噪声干扰和动态特性变差造成的误差，取加速计解算值低通滤波后的角度参与数据融合。

设某一采样时刻，由加速度计解算直接得到的姿态角为θ_acc(k)，前一次采样低通滤波之后的姿态角为θ_Lowpass(k-1)，则当前采样时刻低通滤波之后的姿态角θ_Lowpass(k)的表达式为：

${\mathrm{\θ}}_{Lowpass}\left(k\right)=\frac{T}{RC+T}\[{\mathrm{\θ}}_{acc}\left(k\right)-{\mathrm{\θ}}_{Lowpass}(k-1)\]+{\mathrm{\θ}}_{Lowpass}(k-1)$

低通滤波的输出量中，仅包含原始信号中频谱低于截止频率的部分信号，原始信号中频率高于截止频率的信号和噪声都被衰减。因此低通滤波的输出能够反映原始信号中的低频部分。

在没有外加速度干扰的情况下，互补滤波姿态解算数据融合的基本公式为：

θ(k)＝θ_Highpass(k)+θ_Lowpass(k)

加速度计解算角度的准确度受外部加速度的影响很大，只有在没有外部加速度的情况下，加速度计的解算结果才是完全可信的。当存在外部加速度时，加速度计解算结果的准确性明显变差，且变差趋势随着外部加速度的增大而加剧。因此，在存在外加速度的情况下，就要降低互补滤波器高通和低通滤波器的截止频率，削弱加速度计解算结果在数据融合中所占的权重而加强陀螺仪解算结果在数据融合中所占的权重。a_e为外部加速度，f_c为截止频率。外加速度越大，截止频率越低，当外加速度大至一定程度时，使截止频率接近于0，完全由陀螺仪进行姿态的解算。

用加速度计在载体系XYZ三轴上测量值所构成的向量模值，与重力加速度进行比较，评估外加速度的影响度。设置参数：

$\mathrm{\ξ}=\frac{|\sqrt{a_x^2+a_y^2+a_z^2}-g|}{g}\∈(0,\mathrm{\∞})$

其中，a_x、a_y、a_z分别为加速度计在载体系XYZ三轴上的测量值；g为重力加速度的模值。ξ的值越大，则表示作用于载体的外部加速越大；ξ的值越接近于0，表示作用于载体的外部加速越小。

令互补滤波器中，高通滤波器和低通滤波器的传递函数分别为：

$G_H\left(s\right)=\frac{\[1+tan\left(4\mathrm{\π}\mathrm{\ξ}\right)\]\·RCs}{\[1+tan\left(4\mathrm{\π}\mathrm{\ξ}\right)\]\·RCs+1},\mathrm{\ξ}\∈\[0,0.125)$

$G_L\left(s\right)=\frac{1}{\[1+tan\left(4\mathrm{\π}\mathrm{\ξ}\right)\]\·RCs+1},\mathrm{\ξ}\∈\[0,0.125)$

则当ξ∈[0，0.125)时，互补滤波器的截止频率为：

$f_c=\frac{1}{2\mathrm{\π}RC\·\[1+tan\left(4\mathrm{\π}\mathrm{\ξ}\right)\]}$

可以发现，若ξ＝0，则当ξ逐渐增大至接近0.125时，f_c→0。当ξ≥0.125时，认为此时的外部加速度过大，完全由陀螺仪进行姿态角的解算。

互补滤波算法总结：

(1)计算 $\mathrm{\ξ}=\frac{|\sqrt{a_x^2+a_y^2+a_z^2}-g|}{g}\∈\[0,\mathrm{\∞})$

(2)按以下两种情况进行数据融合

①当0≤ξ＜0.125时

${\mathrm{\θ}}_{Highpass}\left(k\right)=\frac{\[1+tan\left(4\mathrm{\π}\mathrm{\ξ}\right)\]RC}{\[1+tan\left(4\mathrm{\π}\mathrm{\ξ}\right)\]RC+T}\[{\mathrm{\θ}}_{gyro}\left(k\right)-{\mathrm{\θ}}_{gyro}(k-1)+{\mathrm{\θ}}_{Highpass}(k-1)\]$

${\mathrm{\θ}}_{Lowpass}\left(k\right)=\frac{T}{\[1+\tan \left(4\mathrm{\π}\mathrm{\ξ}\right)\]RC+T}\[{\mathrm{\θ}}_{acc}\left(k\right)-{\mathrm{\θ}}_{Lowpass}\left(k-1\right)\]+{\mathrm{\θ}}_{Lowpass}\left(k-1\right)$

θ(k)＝θ_Highpass(k)+θ_Lowpass(k)

②当ξ≥0.125时

此时仅使用陀螺仪进行姿态的解算θ(k)＝θ_gyro(k)-θ_gyro(k-1)+θ(k-1)

以上所述仅是本发明的优选实施方式，应当指出的是，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (6)

1.一种用于救灾无人机姿态控制的互补滤波方法，其特征在于，所述方法包括如下步骤：

(1)由于加速度计的输出中含有高平噪声，在数据融合之前，对加速度计的解算值进行低通滤波，滤除噪声干扰和累积增量；

(2)由于陀螺仪在静态下，存在温度漂移现象，因此对陀螺仪的解算结果进行高通滤波；

(3)加速度计解算姿态角具有较好的低频特性，而高频特性较差，而陀螺仪具有较好的高频动态特性，利用二者频率特性的互补性，进行数据的重构融合。

2.根据权利要求1所述的一种用于救灾无人机姿态控制的互补滤波方法，其特征在于，

所述方法包括如下步骤：将原始输入信号按傅立叶分解展开，即：

对于原始信号中的正弦分量，将会被低通滤波器极大的衰减，但能以很小的衰减通过高通滤波器；对于原始信号中的正弦分量，将会被高通滤波器极大的衰减，但能以很小的衰减通过低通滤波器，将高通滤波器和低通滤波器的输出进行整合，即可得到全部频域内的信号。

3.根据权利要求1所述的一种用于救灾无人机姿态控制的互补滤波方法，其特征在于，所述高通滤波算法如下：

设某一采样时刻，由陀螺仪解算直接得到的姿态角度为θ_gyro(k)(俯仰角、横滚角或偏航角)，前一次采样时刻由陀螺仪解算直接得到的结果为θ_gyro(k-1)，前一次采样时刻高通滤波之后的姿态角为θ_Highpass(k-1)，则当前采样时刻高通滤波之后的姿态角θ_Highpass(k)的表达式为：

${\mathrm{\θ}}_{Highpass}\left(k\right)=\frac{RC}{RC+T}\[{\mathrm{\θ}}_{gyro}\left(k\right)-{\mathrm{\θ}}_{gyro}(k-1)+{\mathrm{\θ}}_{Highpass}(k-1)\]$

理想的高通滤波的输出量中，仅包含原始信号中频谱高于截止频率的动态部分信号，当原始信号处于低频或维持状态时，高通滤波的输出为0，由于高通滤波器滤掉了原始信号中的直流成分和低频成分，因此高通滤波的输出不能真实的反映原始信号相对于零点的实际值，但能够真实的反映原始信号在任一时刻的采样值相对于上一次采样值的高频改变量。

4.根据权利要求3所述的一种用于救灾无人机姿态控制的互补滤波方法，其特征在于，所述低通滤波算法如下：

设某一采样时刻，由加速度计解算直接得到的姿态角为θ_acc(k)，前一次采样低通滤波之后的姿态角为θ_Lowpass(k-1)，则当前采样时刻低通滤波之后的姿态角θ_Lowpass(k)的表达式为：

${\mathrm{\θ}}_{\mathrm{Lowp}ass}\left(k\right)=\frac{T}{\mathrm{RC}+T}\[{\mathrm{\θ}}_{\mathrm{acc}}\left(k\right)-{\mathrm{\θ}}_{\mathrm{Lowpass}}(k-1)+{\mathrm{\θ}}_{\mathrm{lowpa}ss}(k-1)$

低通滤波的输出量中，仅包含原始信号中频谱低于截止频率的部分信号，原始信号中频率高于截止频率的信号和噪声都被衰减。因此低通滤波的输出能够反映原始信号中的低频部分。

5.根据权利要求4所述的一种用于救灾无人机姿态控制的互补滤波方法，其特征在于，所述高通滤波算法如下：

在没有外加速度干扰的情况下，互补滤波姿态解算数据融合的基本公式为：

θ(k)＝θ_Highpass(k)+θ_Lowpass(k)

6.根据权利要求1所述的一种用于救灾无人机姿态控制的互补滤波方法，其特征在于，加速度计解算角度的准确度受外部加速度的影响很大，只有在没有外部加速度的情况下，加速度计的解算结果才是完全可信的，当存在外部加速度时，加速度计解算结果的准确性明显变差，且变差趋势随着外部加速度的增大而加剧，因此，在存在外加速度的情况下，就要降低互补滤波器高通和低通滤波器的截止频率，削弱加速度计解算结果在数据融合中所占的权重而加强陀螺仪解算结果在数据融合中所占的权重，a_e为外部加速度，f_c为截止频率，外加速度越大，截止频率越低，当外加速度大至一定程度时，使截止频率接近于0，完全由陀螺仪进行姿态的解算，

用加速度计在载体系XYZ三轴上测量值所构成的向量模值，与重力加速度进行比较，评估外加速度的影响度，设置参数：

$\mathrm{\ξ}=\frac{\left|\sqrt{a_x^2+a_y^2+a_z^2}-g\right|}{g}\∈(0,\mathrm{\∞})$

其中，a_x、a_y、a_z分别为加速度计在载体系XYZ三轴上的测量值；g为重力加速度的模值，ξ的值越大，则表示作用于载体的外部加速越大；ξ的值越接近于0，表示作用于载体的外部加速越小，

令互补滤波器中，高通滤波器和低通滤波器的传递函数分别为：

$G_H\left(s\right)=\frac{\[1+tan\left(4\mathrm{\π}\mathrm{\ξ}\right)\]\·RCs}{\[1+tan\left(4\mathrm{\π}\mathrm{\ξ}\right)\]\·RCs+1},\mathrm{\ξ}\∈\[0,0.125)$

$G_L\left(s\right)=\frac{1}{\[1+tan\left(4\mathrm{\π}\mathrm{\ξ}\right)\]\·RCs+1},\mathrm{\ξ}\∈\[0,0.125)$

则当ξ∈[0，0.125)时，互补滤波器的截止频率为：

$f_c=\frac{1}{2\mathrm{\π}RC\·\[1+tan\left(4\mathrm{\π}\mathrm{\ξ}\right)\]}$

可以发现，若ξ＝0，则当ξ逐渐增大至接近0.125时，f_c→0。当ξ≥0.125时，认为此时的外部加速度过大，完全由陀螺仪进行姿态角的解算。