CN106249744A - 一种基于二级互补滤波的小型旋翼飞行器高度控制方法 - Google Patents

Abstract

该发明公开了一种基于二级互补滤波的小型旋翼飞行器高度控制方法，属于小型旋翼飞行器控制技术领域。该方法首先测量小型旋翼飞机的高度和速度参数，根据测量的高度和速度新型，通过改变飞行器输出动力，测量改变动力输出后飞行器的飞行姿态的变化来，反馈给动力输出，从而根据反馈回来的姿态信息重新实时调整动力输出，从而实时控制飞行器的高度。本发明二级互补滤波的测量方法在室内室外都能适用，精度高，成本低；而串级PID的控制方法不仅能稳定跟踪设定的高度，而且鲁棒性强，超调小。

Description

一种基于二级互补滤波的小型旋翼飞行器高度控制方法

技术领域

本发明属于小型旋翼飞行器控制技术领域。

背景技术

由于小型旋翼飞行器的体积小，重量轻，良好的机动灵活性，使得小型旋翼飞行器作为飞行平台在军用和民用领域得到了广泛应用。比如军用四旋翼侦察机，大疆公司的航拍无人机等。高度作为水平二维位置空间的拓展，是飞行器实现其他功能的基础。要实现对小型旋翼飞行器在高度维度上的功能，不仅需要飞行器准确的高度信息，还需要对飞行器的高度进行有效控制。

对于飞行器高度的测量，国内外提出了一些可行有效的测量方法。华南理工大学提出了一种由气压计提供高度信息，再经过11阶拓展卡尔曼滤波的估计，实现对飞行器的高度测量(期刊：自动化与仪表；著者：李洪辉，裴海龙；出版年月：2012年；文章题目：无人直升机高度测量的设计与实现；页码：11-15)。南京航空航天大学提出了通过组合气压计、GPS、和加速度计三种传感器数据的高度测量系统。(期刊：兵工自动化；著者：谢勇，陈照梅，陈万法；出版年月：2010年；文章题目：某无人直升机高度测量系统融合方法；页码：85-86)。苏黎世联邦理工学院采用单目摄像机，再运用同步定位和见图算法实现对高度和位置的测量(会议：the 2010IEEE International Conference on Robotics and Automatic；著者：Michael Blosch D Sea,Stephan Weiss；出版年月：2010年；文章题目：Vision Based MAVNavigation in Unknown and Unstructured Environments；页码：21-28)。以上的方法在一定条件下效果显著，但是也存在典型不足。例如，11阶卡尔曼滤波器的计算和单目摄像机图像的计算对飞行器芯片要求过高，普通的处理器无法胜任。同时，在城市和室内等环境中，GPS无法接收到可用信息，所以南京航空航天大学的高度测量方法适用性不足。

发明内容

本发明基于对处理器要求过高以及单级PID控制的不足之处，改进设计一种基于二级互补滤波的小型旋翼飞行器高度控制方法。该发明技术方案为一种基于二级互补滤波的小型旋翼飞行器高度控制方法，该方法包括：

步骤1：测量飞行器所在位置的气压计值baro和飞行器的垂直加速度值acc；

步骤2：设定截止频率其中a是权重，T是采样周期；

步骤3：计算垂直方向速度高度h：计算垂直方向速度v时，其中a_v表示计算速度时的权重，v[k+1]为估计的垂直方向速度，baro[k]为上一采样时刻气压器测量值，acc[k]为上一时刻加速度垂直方向测量值；计算垂直高度时，h[k+1]＝a_h ^*baro[k]+(1-a_h)^*∫∫acc[k]dt，其中a_h表示计算高度时的权重，h[k+1]为估计的垂直方向高度，baro[k]为上一采样时刻气压器测量值，acc[k]为上一时刻加速度垂直方向测量值；

步骤4：采用方法来控制小型旋翼飞行器的高度；

其中，其中h为高度，h_d为期望的高度，v为垂直方向上的速度，v_d为垂直方向上的期望速度，k_p>0、k_v>0为常数，u为控制输入，m为飞行器质量，g为重力加速度，θ和φ分别为飞行器的俯仰角和滚转角；

进一步的，在计算垂直方向速度时，取T_v＝20ms，a_v＝0.02，故f_rv＝0.16HZ，T_v表示计算速度时的采样周期，a_v表示计算速度时的权重，f_rv表示计算速度时的截止频率；在计算垂直高度时，取T_h＝60ms，a_h＝0.05，故f_rh＝0.13HZ，T_h表示计算高度时的采样周期，a_h表示计算高度时的权重，f_rh表示计算高度时的截止频率；

进一步的，海拔高度h的具体测量方法；

$Altitude=44330*{(1-\frac{{\mathrm{Presure}}_{Current}}{{\mathrm{Presure}}_{Start}})}^{0.1903}$

其中Altitude为计算出的相对地面的实际高度，单位为米，Presure_Current为当前的气压值，单位为Pa，Presure_Start为标准的气压值，单位为Pa。

进一步的，垂直加速度acc测量方法：根据固连的加速度计可测得机体加速度，然后通过转换矩阵，即可得到导航系下垂直方向的加速度。

本发明提出了一种基于二级互补滤波的小型旋翼飞行器高度控制方法，其优点和功效是：与当前高度的测量和控制方法相比，这种二级互补滤波的测量方法在室内室外都能适用，精度高，成本低；而串级PID的控制方法不仅能稳定跟踪设定的高度，而且鲁棒性强，超调小。

附图说明

图1：气压计高频特性，由于融合方法为互补滤波，故在融合算法中会给气压计的数据加上一个低通滤波器。

图2：气压计所得测量值频谱。

图3：互补滤波示意图，为需要得到的信号，其测量值分别为y₁和y₂，其测量噪声分别为高频噪声u₁，低频噪声u₂，故需要对u₁施加一个低通滤波，对u₂施加一个高通滤波。

图4：本发明二级互补滤波示意图，这是本发明的到高度的融合算法示意图。

图5：单级PID结构示意图，其控制对象为双积分。

图6：串级PID结构示意图，其控制对象为双积分。

图7：仿真条件下：单级PID结构和串级PID结构抗干扰能力对比，误差越小，说明抗干扰能力越强，显然，串级PID忧于单级PID。在仿真时，需要加上传感器噪声。

图8：真实条件下：分别在单级PID结构和串级PID结构高度控制结构下四旋翼跟踪5m的效果图。静差越小，其抗干扰能力越强，显然，串级PID忧于单级PID。

图9：四旋翼飞行器跟踪5m效果示意图。

图10：四旋翼飞行器跟踪10m效果示意图。

具体实施方式

设计目标包括两个方面：其一，得到有效准确的高度；其二，对高度进行有效控制。

本发明的设计方案是：利用气压计采集计算飞行器当前的海拔高度信息，利用加速度计采集飞行器的在垂直方向的加速度信息，通过二级互补滤波融合算法，得到飞行器的高度估计值，和垂直方向速度的估计值。然后采用串联PID的控制结构，内外环分别反馈高度估计值和垂直方向速度的估计值，最终来控制小型旋翼飞行器的高度。在本发明中采用的气压传感器为MS5611。这是MEAS公司推出的一款SPI和IIC总线接口的新一代高分辨率气压传感器，分辨率可达10cm。

对于高度的测量和控制，其具体的技术步骤如下：

1.气压计数测量值的分析处理

气压计可以得到飞行器相对于水平面的海拔高度，其原理是利用大气压值与温度和高度的数学关系。气压传感器采集的大气压值受温度影响较大，需要经过二阶的温度补偿，得到准确的大气压值，再通过气压转换高度公式得到高度。由于飞行器运动时，气流的不稳定，造成读取的气压值有高频噪声存在(如图1)。频谱如图2。此外，要得到垂直方向的速度，需要对气压计所得高度进行微分处理。微分处理不可避免地会出现毛刺现象，从而需要在估计垂直方向速度时加上低通滤波器，也需要在估计高度时加上低通滤波器。对于选择的低通滤波器其参数特征在技术步骤3中阐述。

气压计中，由气压转换高度公式可以得到高度测量值。其公式如下：

$Altitude=44330*{(1-\frac{{\mathrm{Presure}}_{Current}}{{\mathrm{Presure}}_{Start}})}^{0.1903}$

公式中Altitude(单位为米)为计算出的相对地面的实际高度，Presure_Current(单位为Pa)为当前的气压值，Presure_Start(单位为Pa)为标准的气压值。

在本发明中采用的气压传感器为MS5611。这是MEAS公司推出的一款SPI和IIC总线接口的新一代高分辨率气压传感器，分辨率可达10cm。

2.加速度计测量值分析处理

因为加速度计固联在飞行器的机体上，所以得到的加速度计为机体系下的加速度值。要通过加速度计值得到高度，必须通过旋转矩阵将其转换到导航坐标系下。

设机体系到导航系的转换矩阵为：机体系下加速度计测量向量为[a_bx,a_by,a_bz]^T，导航系下三周加速度为[a_nx,a_ny,a_nz]^T，可以得到导航系下三轴加速度向量为：

$\left[\begin{array}{c}{a}_{nx}\\ a_{ny}\\ a_{nz}\end{array}\right]=C_b^n\left[\begin{array}{c}{a}_{bx}\\ a_{by}\\ a_{bz}\end{array}\right]$

当飞行器启动时，由于电机和螺旋桨震动产生的机器噪声，使得传感器数据有一个高频干扰，对通过旋转矩阵计算得到的加速度值有较大的影响。但是，我们是通过对垂直方向的加速度一次积分得到垂直方向的速度，二次积分得到高度信息。

假设有用信号叠加了一个正弦噪声y＝Asinwt，对于由加速度计一次积分得到的垂直方向的速度误差有:

可知w出现在分母上，可得结论：低频噪声超声的影响超过高频噪声产生的影响。若对噪声进行二次积分，可得其高度误差：

$dh=\∫dvdt=\∫\frac{A}{w}\cos wtdt=\frac{A}{w^2}\sin wt$

可知低频噪声对二次积分的到的高度影响更大。经上述面分析，需要对加速度计的一次积分和二次积分加上高通滤波器。对于选择的高通滤波器其参数特征在技术步骤3中阐述。

3.设计二级互补滤波算法

多个传感器对同一信息进行独立的测量，且各自存在的噪声不同时，互补滤波算法提供了一种高效的数据融合方法。如图2。

设A、B传感器对某一信息进行测量：

y₁＝x+u₁

y₂＝x+u₂

x为状态，其测量值分别为y₁和y₂，其测量噪声分别为高频噪声u₁，低频噪声u₂。

采用一对互补的传递函数：F₁(s)+F₂(s)＝1，设F₁(s)是低通滤波器的传递函数，F₂(s)是高通滤波器的传递函数。要去除A传感器测量数据中的高频噪声，可采用低通滤波器；而B传感器测量数据中的低频噪声，则采用高通滤波器。其结构如图3所示。可以得到状态x的估计传递函数为：

$\hat{X}\left(s\right)=F_1\left(s\right)Y_1+F_2\left(s\right)Y_2=X\left(s\right)+F_1\left(s\right)U_1\left(s\right)+F_2\left(s\right)U_2\left(s\right)$

低通滤波器F₁(s)的传递函数为：根据数据频率特征选择C(s)＝k_p，高通滤波器F₂(s)的传递函数为：

故可以得到：

气压计和加速度计对于高度的测量各有优缺点。具体而言，气压计测量的是大气压强，虽然干扰源较多，精度有限，但其测量值在真实值左右浮动，漂移不大，故具有参考性。加速度计测量值有静差，而且经过二次积分得到的高度信息长时间飘移大，但短时间内是精准的。

根据两种高度传感器的特性，本发明采用二级互补滤波算法。一级互补滤波得到垂直方向的速度信息，另外一级互补滤波得到高度信息。如图4。

当式中k_p取为固定常数，则设计的低通滤波器和高通滤波器的截止频率为：对于垂直方向速度信息的估计，在大于f_rv(下标v表示计算速度时的参数)的高频段，加速度计一次积分所得值对估计结果起主要的作用，小于f_rv的低频段，气压计微分所得值起主要的作用。对于高度信息的估计，在大于f_rh(下标h表示计算高度时的参数)的高频段，加速度计二次积分所得值对估计结果起主要的作用，小于f_rh的低频段，气压计测量值起主要的作用。

在实际的编码中，对于垂直方向速度的估计：

$v\[k+1\]=a_v*\frac{d}{dt}baro\[k\]+(1-a_v)*\∫acc\[k\]dt$

其中v[k+1]为估计的垂直方向速度，baro[k]为上一采样时刻气压器测量值，acc[k]为上一时刻加速度垂直方向测量值，a_v为权重，a_v与截止频率的关系为：T为采样周期。在本发明中，取T_v＝20ms，a_v＝0.02，故f_rv＝0.16HZ。

对于垂直方向速度的估计：

h[k+1]＝a_h ^*baro[k]+(1-a_h)^*∫∫acc[k]dt

其中h[k+1]为估计的垂直方向速度，baro[k]为上一采样时刻气压器测量值，acc[k]为上一时刻加速度垂直方向测量值，a_h为权重，a_h与截止频率的关系为：T为采样周期。在本发明中，取T_h＝60ms，a_h＝0.05，故f_rh＝0.13HZ。

4.利用滤波算法处理后的数据设计高度调节器

由简化后的四旋翼飞行器的运动学模型有：其中h为高度，高度的二次微分，高度的一阶微分为垂直速度，二阶微分为垂直加速度，u为控制输入，m为飞行器质量，g为重力加速度，θ和φ分别为飞行器的俯仰角和滚转角。经反馈线性化有：

$\stackrel{\·\·}{h}=input=\frac{ucos\mathrm{\θ}cos\mathrm{\φ}-mg}{m}$

其中为典型的二阶线性系统，对其设计控制律为：

$input=-k_p(h-h_d)-k_v(\stackrel{\·}{h}-{\stackrel{\·}{h}}_d)$

从而，可以得到：

$u=\frac{m\[-k_p(h-h_d)-k_v(\stackrel{\·}{h}-{\stackrel{\·}{h}}_d)+g\]}{\cos \mathrm{\θ}\cos \mathrm{\φ}}$

从输入可以看出，对于高度控制在不考虑外界影响的情况下，采用PD控制即能达到稳定和跟踪。然而对于实际的系统，考虑到室外条件下的各种不确定的干扰，如风阻等，选择串级PID的控制结构。内环反馈量为估计的速度，外环反馈量为估计的高度。关于单级PID和串级PID的控制结构如图5和图6。在有垂直方向速度信息噪声的情况下，单级PID和串级PID仿真控制效果如图7，真是控制效果如图8。

5.高度调节器参数的整定和优化

由步骤4可知，本发明采用的控制结构是串级PID，PID控制不可避免地需要对PID参数进行整定和优化才能取得较好的控制效果。针对选定的串级PID的控制结构，先整定内环的参数，然后整定外环。

在内环的参数能保证系统的稳定性的前提下，有针对性的优化外环的参数，使其稳态误差更小。

最终取内环参数为外环参数为

控制效果如图9和如图10。

6.设计结束

整个技术步骤重点考虑了三个方面的问题，分别是传感器处理、高度信息融合、高度控制。围绕这三个方面，首先在上述第一步和第二步中确定了高度传感器和传感器数据的滤波处理；然后在第三步中由两种传感器的特性设计了一种基于二级互补滤波的融合方法；第四步中主要从数学上分析了高度的控制模型，选择了串级PID的控制结构。第五步是根据第四步的控制结构对PID参数进行整定和优化，给出了四旋翼飞行器跟踪5m和10m的效果图。经上述各步骤后，设计结束。

Claims (4)

1.一种基于二级互补滤波的小型旋翼飞行器高度控制方法，该方法包括：

步骤1：测量飞行器所在位置的气压计值baro和飞行器的垂直加速度值acc；

步骤2：设定截止频率其中a是权重，T是采样周期；

步骤3：计算垂直方向速度高度h：计算垂直方向速度v时，其中a_v表示计算速度时的权重，v[k+1]为估计的垂直方向速度，baro[k]为上一采样时刻气压器测量值，acc[k]为上一时刻加速度垂直方向测量值；计算垂直高度时，h[k+1]＝a_h*baro[k]+(1-a_h)*∫∫acc[k]dt，其中a_h表示计算高度时的权重，h[k+1]为估计的垂直方向高度，baro[k]为上一采样时刻气压器测量值，acc[k]为上一时刻加速度垂直方向测量值；

步骤4：采用方法来控制小型旋翼飞行器的高度；

其中，其中h为高度，h_d为期望的高度，v为垂直方向上的速度，v_d为垂直方向上的期望速度，k_p>0、k_v>0为常数，u为控制输入，m为飞行器质量，g为重力加速度，θ和φ分别为飞行器的俯仰角和滚转角。

2.如权利要求1所述的一种基于二级互补滤波的小型旋翼飞行器高度控制方法，其特征在于在计算垂直方向速度时，取T_v＝20ms，a_v＝0.02，故f_rv＝0.16HZ，T_v表示计算速度时的采样周期，a_v表示计算速度时的权重，f_rv表示计算速度时的截止频率；在计算垂直高度时，取T_h＝60ms，a_h＝0.05，故f_rh＝0.13HZ，T_h表示计算高度时的采样周期，a_h表示计算高度时的权重，f_rh表示计算高度时的截止频率。

3.如权利要求1所述的一种基于二级互补滤波的小型旋翼飞行器高度控制方法，其特征在于海拔高度h的具体测量方法；

$Altitude=44330*{(1-\frac{{\mathrm{Presure}}_{Current}}{{\mathrm{Presure}}_{Start}})}^{0.1903}$

其中Altitude为计算出的相对地面的实际高度，单位为米，Presure_Current为当前的气压值，单位为Pa，Presure_Start为标准的气压值，单位为Pa。

4.如权利要求1所述的一种基于二级互补滤波的小型旋翼飞行器高度控制方法，其特征在于垂直加速度acc测量方法：根据固连的加速度计可测得机体加速度，然后通过转换矩阵，即可得到导航系下垂直方向的加速度。