四旋翼飞行器的悬停控制研究

摘 要旋翼式飞行器是近些年来无人机发展的重要方向，在多个领域发挥着重要的作用。本文介绍一种四旋翼飞行器定点悬停的控制策略，气压计与电子航向系统（AHRS）为获取关键参数的传感器，应用低通滤波与卡尔曼滤波。在保证控制精度的同时，笔者尽量使算法简单，不过高的依赖微处理器的运算速度。之后探讨了数据处理方面存在的题，希望可以对悬停控制的研究贡献微薄之力。

【关键词】四旋翼 定点悬停 气压 AHRS 低通滤波 卡尔曼滤波

四旋翼飞行器是一种结构简单的多旋翼形式的飞行器，其本身具有多重优势特征，主要表现为机体结构简单、造价低廉、陀螺效应小等多种优势。最重要的是，四旋翼飞行器可以做到垂直起降，使得起飞与降落的成本大大减小。因此，这种飞行器广泛的应用与空中拍摄、监视、侦查等方面。然而，四旋翼飞行器本身也存在一定的缺陷，飞行器本身在飞行的过程中容易受到噪声甚至是气流等多方面干扰，使控制变得困难。

众所周知，为了使四旋翼飞行器相比与固定翼飞行器发挥更大的优势，悬停控制是非常必要的，而现阶段大多数的悬停控制方案基于民用全球定位系统（Global Position System）或计算机视觉控制。但GPS对环境要求较大，在某些GPS信号不覆盖的地方不能使用，而计算机视觉控制对微处理器的功能要求比较高。但气压计的使用相对简单，只需要利用简单地机械结构设计避免气流造成的大噪声的数据，非常适合用于悬停控制

本文将注重对传感器数据的处理，从理论推导出发，并联系实际应用，将低通滤波与卡尔曼滤波相结合，并通过实体飞行器测试算法，提出并证明一种比较稳定的四旋翼悬停控制方案。

1 四旋翼飞行器悬停的意义与工作原理

四旋翼飞行器的悬停控制一直是国内外学者关注的一个重点课题。悬停，即四旋翼飞行器在某一时刻相对于地面保持一种静止的空间状态。

为了方便叙述，定义机体坐标系OXYZ：以飞行器重心为原点，纵轴为X轴，方向同机头方向；在飞行器对称面内且垂直于X轴为Z轴，方向向下；与X，Z轴垂直且向右为正方向为Y轴。根据右手定则由飞机重心为原点定义地面惯性坐标系OgXgYgZg。OX轴相对于OgXgYg平面的变化，即俯仰角（pitch）；OX轴在OgXgYg平面的投影与OgXg之间的夹角，即偏航角（yaw）；以及飞行器对称面绕X轴转动角度，即横滚角（roll）。

由于四轴飞行器在空间中有6个自由度，即在地面惯性坐标系中，沿三个坐标轴做平移和旋转运动。通过对四轴飞行器的运动分析，可以得出飞机的姿态变化可以分解成横滚角，俯仰角，航向角的变化，而OgZg轴的平移运动可以看做高度的变化（altitude），如图1所示。

可以看出四旋翼飞行器的运行方式主要包括四个方面的内容，有垂直起降和悬停（即高度的变化）、偏航运行、俯仰运动以及滚转运动四个，其中，基本的运动方式就是悬停运动以及垂直起降运动，但飞行器在空中飞行时会受到气流、气压等多种噪声干扰，飞行器的运动状态会发生改变。所以控制控制四旋翼飞行器的悬停，即控制横滚角，俯仰角，与航向角，以及高度。

2 四旋翼飞行器控制系统

经过反复多种方案的反复比较分析，本文决定采用电子航向系统，高精度气压计作为数据的采集，通过I2C+SPI方式向主控芯片传输数据，并对气压计做隔离处理，降低气流对气压计的干扰。为了方便采集飞行时的实时数据，利用串口方式+蓝牙方式，使飞行器与上位机进行通讯。

2.1 控制系统的模块化设计

通过AHRS（attitude heading reference system）可以获得实时的欧拉角，通过气压计可以获得实时高度数据，运用适当的微处理器（Microcontroller Unit）快速的控制，使得四旋翼飞行器可以快速的对误差进行反应。因此，本文描述的四旋翼飞行器呈现如下结构。

2.2 控制系统悬停的目标介绍

由于篇幅有限，本文只研究在手动飞行中的悬停过程。当飞行器在飞行时，当横滚角（pitch），俯仰角（roll）比较稳定时，对飞行器发射悬停信号。此时飞行器将进行自稳控制，并记录飞行器稳定后的姿态、航向、高度数据。在可以修正的范围内进行修正。由于文献（1）将已经将姿态的估计与控制做了详细的描述，使飞行器姿态很好的保持稳定，本文将不再描述姿态的控制，而将重点放在高度数据的处理与高度控制上，如图2所示。

3 基于AHRS与气压计的悬停控制系统的算法设计与实现

3.1 获得理想的控制量

3.1.1 低通滤波

低通滤波分为很多种，对于从模拟信号发展而来的最简单的低通滤波，分为一阶阻容滤波，和二阶阻容滤波，阻容滤波在频域上有阻挡高频信号的特点，在相位上存在滞后，阻容滤波也成为滞后滤波。而二阶低通滤波较一阶低通滤波

理想一阶低通滤波器的传递函数为

Z变换：

式中T为采样周期

Z反变换得到差分方程

称α为滤波系数，1>α>0

Y（n）=αX（n）+（1-α）Y（n-1）

其中Y（n）为本次计算结果，X（n）为得到的新数据， Y（n-1）为前一次计算的结果。滤波参数与截止频率和采样时间有关。

经过分析，滤波系数

ω为截止频率。

3.1.2 卡尔曼滤波

卡尔曼滤波系统中的信号和噪声状态方程如下：

其中，为系统状态转换矩阵，Qk-1为系统噪声方差阵。Hk为观测矩阵，Rk为观测噪声方差阵。移动物体的状态方程和输出方程，见式

式中：Q表示系统动态状态矩阵；R为噪声到系统状态的映射矩阵；O为系统输出矩阵；ξ为系统噪声；η为测量噪声。

基于状态观测方程，可以获得卡尔曼滤波器五大递推公式。其中，为校正前预测值；为校正后估计值；为校正前的预测均方差；为校正后的误差均方值，Kk为卡尔曼增益矩阵。

卡尔曼滤波的主要思想是“预测”与“修正”。在本飞行控制系统中，对于姿态结算和高度数据获取，卡尔曼滤波都是最重要的算法，（在文献中，）已经介绍了卡尔曼滤波在姿态解算中发挥的作用，本文将详细讨论卡尔曼滤波在高度数据获取中的应用。

3.1.3 高度数据的获得

本系统的加速度计与气压计的组合悬停系统的状态变量为

式中Px代表四旋翼飞行器的高度，Vx代表Z周速度。

加速度计长期数据可靠度较大，而实时采样数据受到机体震动等外界因素的干扰，导致毛刺较多。从频域上讲，加速度计的低频信号可用而高频信号大多为噪声。Z轴加速度的二次积分为Z轴移动的距离，而加速度计若存在误差，则误差也会被二次积分，但若高度发生变化，加速度计的反映非常灵敏。

气压计的读数受环境影响较大，漂移较为严重，但短期内较为可信。因此，先对加速度计做低通滤波，经测试研究，经一阶低通滤波处理过的数据在实时性与可靠性等方面都满足四轴飞行器控制要求，在将得到的数据与气压计数据通过卡尔曼滤波器，用处理过的加速度计的数据去处理气压计的数据。最后输出的高度数据我们认为是理想的。

3.2 高度控制算法

对四旋翼飞行器的物理模型进行分析后，可以知道Z轴的速度可以更好的体现高度是否稳定，因此，若能够直接对Z轴速度进行很好的闭环控制，必然会改善悬停的动态特性及稳定性。Z轴速度可以由Z轴高度的变化对时间做微分得到，本文将Z轴速度作为整个系统的增稳环节，而仅仅对Z轴速度进行控制则会使四旋翼控制的实时性变差，因此将Z轴速度作为内环控制，作为整个系统的增稳环节。而高度作为外环，其作用体现在对四旋翼飞行器高度的精确控制。

采用位置式数字PID控制：

上式中，u（t）为PID输出值，e（t）为期望值与实际值之差，Kp、Ki、Kd为比例、积分、微分系数，对应项为比例、积分、微分项，再将积分项、微分项离散化得到PID公式。

高度偏差量为：

Error=Current-Previous

其中，Current为当前高度，Previous期望角度。

Z轴速度为

则本系统控制算法可表示为

4 结束语

经过在实际四旋翼飞行器上的测试，在实际飞行中，使用AHRSc气压计的四旋翼飞行器，通过本文所述的方法，可以实现基本的悬停控制，如图4所示。

以下为试验用四轴飞行器的各项参数。

机身尺寸：50cm*50cm*20cm

整机质量：1500g

最大起飞重量：2kg

最大任务载荷：300g

电机：4208 直流无刷电机 kv980

桨：1047塑料正反桨

机架：DJIF450

脚架：塑料

电池： 5200mAh 锂电池

控制器：stm32

从实际测试数据可知，利用本文所描述的算法，可以的到较理想的飞行器高度数据并加以控制，观测到的结果与实验数据吻合。

参考文献

[1]汪绍华，杨莹.基于卡尔曼滤波的四旋翼飞行器姿态估计和控制算法研究[J].控制理论与应用，2013（10）：12.

作者单位

哈尔滨理工大学 黑龙江省哈尔滨市 150000