

9.1 飞控算法理论与设计

前言

这里会向读者介绍有关 Breeze Mini 飞控算法理论与程序设计的相关知识。先简单介绍一下飞控是什么，飞控全称为 飞行控制算法(Flight Control Algorithm)，即通过获取 惯性传感单元(Inertial Measurement Unit, i.e. IMU) 数据，并采用噪声滤波、数据融合等算法对飞行器当前姿态进行实时解算，并结合给定的电机转速的参考值，利用PID等控制算法得到控制输出，从而完成对飞行器的控制。由于飞控算法本身涉及到很多自动控制原理和线性代数方面的理论知识，所以推荐读者先把自控和线代这两部分的基础知识好好学习一下，推荐的书籍有：

1. 《自动控制原理》程鹏, 高等教育出版社

研究现状

1. 国内研究现状

DJI

官方网站：www.dji.com

目前国内做四轴飞行器的公司有很多，其中做飞控系统最好的当属大疆创新（DJI）。大疆创新（DJI）目前拥有NAZE、WooKong以及A系列的多种商业飞控系统，其中作为NAZA飞控系列的最新一代产品，N3多旋翼飞控系统采用DJI最新的控制导航算法，新增内置的双IMU冗余设计，可实现数据实时互为备份，并结合全新内减震结构设计，赋予了飞行器更高的可靠性，为无人机爱好者及行业应用探索者提供了稳定而全面的系统级解决方案。

Crazepony

项目网站：www.crazepony.com

Crazepony微型四轴飞行器由深圳创客火（Maker Fire）公司设计、制造并发行，其内置的飞控算法可以实现飞行器的定高悬停、有头（X模式）和无头模式飞行等功能。相比于商业飞控来说，Crazepony的飞控开源（包括源代码、原理图、工作原理、系统框架、设计思路等），且核心算法部分有详细的教程文档，既方便航模爱好者和各高校的学生学习，也便于对其进行二次开发。

MiniFly

项目网站：www.alientek.com

MiniFly是广州市星翼电子科技有限公司（ALIENTEK）最新推出的一款开源微型四轴飞行器，它所使用的飞控算法可以支持：定高和手动飞行、4D翻飞、抛飞、有头（X模式）和无头模式飞行、一键起飞和降落等功能。由于MiniFly微型四轴的硬件电路设计主要参考了国外的Crazeflie项目，所以其硬件性能在国内同等规格的四轴飞行器里绝对算是一流的，当然强大的硬件性能也为MiniFly实现更为复杂的飞控算法提供了一个绝佳的平台。

2. 国外研究现状

国外高校和科研机构对飞行器控制系统的研究已经有很多年了，技术相对来说比较成熟，而且基本都是以开源飞控为主。以下简要介绍几个目前比较流行的开源飞控系统：

APM

项目网站：www.ardupilot.org

APM(ArduPilot Mega)是在2007年由DIY无人机社区（DIY Drones）推出的飞控产品，是当今最为成熟的开源硬件项目。APM基于Arduino开源平台，对多处硬件做出了改进，包括加速度计、陀螺仪和磁力计组合惯性测量单元（IMU）。由于APM良好的可定制性，APM在全球航模爱好者范围内迅速传播开来。通过开源地面站软件Mission Planner，开发者可以对APM进行相应的配置、接收并显示传感器数据和使用Google Map完成自动驾驶等功能。除此之外，APM在连接外置GPS传感器后还能够增强飞行的稳定性，并能实现自主起降、自主航线规划、回家、定高、定点等丰富的飞行模式。APM还可以使用外置的超声波传感器和光流传感器，在室内实现定高和定点悬停。

PIXHawk & PX4

项目网站：www.px4.io

PX4是一个软硬件开源项目（遵守BSD协议），旨在为学术、爱好和工业团体提供一款低成本、高性能的高端自驾仪。这个项目源于苏黎世联邦理工大学的计算机视觉与几何实验室、自主系统实验室和自动控制实验室的PIXHawk项目。于2004年推出的PIXHawk飞控是PX4飞控的升级版本，它拥有PX4和APM两套固件和相应的地面站软件。该飞控是目前全世界飞控产品中硬件规格最高的，也是当前爱好者手中最炙手可热的产品。PIXHawk拥有168MHz的主频，并突破性地采用了整合硬件浮点运算核心的Cortex-M4的单片机作为主控芯片，其内置了两套陀螺和加速度计传感器，互为补充矫正，其还内置了三轴磁场传感器并可以外接一个三轴磁场传感器，同时可外接一主一备两个GPS传感器，在故障时自动切换。

CC3D

项目网站：www.github.com/openpilot/OpenPilot

CC3D是Openpilot旗下最流行的飞控系统，此飞控板只采用一颗72MHz的32位STM32单片机和一颗MPU6000就能够完成四旋翼、固定翼、直升机的姿态控制飞行。与所有开源飞控不同，它不需要GPS融合或者磁场传感器参与修正，就能保持长时间的姿态控制，而且通过设置就可以更改飞机种类、飞行模式、支持云台增稳等功能。此外，CC3D飞控编译完的固件容量只有大约100KB，代码效率令人惊叹，而且其地面站软件集成了完整的电子地图，可以通过电台实时监测飞机状态。

MWC

项目网站：www.multiwii.com

MultiWiiCopter(MWC)飞控是一款典型的Arduino衍生产品，是专为多旋翼开发的低成本飞控，其固件最早由法国的Alex开发，它完整地保留了Arduino IDE开发和Arduino设备升级和使用的方法。由于成本低、架构简单、固件比较成熟，因此该飞控在国内外拥有大量爱好者。除了支持常见的四、六、八旋翼以外，该飞控的最大特点是支持很多奇特的飞行器类型，比如三旋翼、Y4型多旋翼（其中两轴为上下对置）等。

Paparazzi

项目网站：www.paparazziuav.org

Paparazzi(PPZ)是一个软硬件全开源的项目，它始于2003年，开发目标是建立一个配置灵活且性能强大的开源飞控项目。PPZ的一大特点是，该开源飞控方案中除了常见的飞控硬件、飞控软件和地面站软件之外，还包含地面站硬件，包括各种调制解调器、天线等设备。从功能上讲，PPZ已经接近一个小型的无人机系统了。该开源项目的另一个特点是采用Ubuntu操作系统，它将全部地面站软件和开发环境集成于该系统下，官方称之为Live CD。一张CD加飞控硬件就可完成从开发到使用的全部工作。PPZ目前最流行的硬件版本拥有大量的扩展接口，方便开发者进行DIY。

Crazeflie

项目网站：www.bitcraze.io

Crazeflie是国外最著名的微型四轴飞行器开源项目，它由三个来自瑞典的嵌入式工程师所创建，目的是使用尽可能少的零件来构造一款可以在室内使用的小型飞行机器人。得益于其出色、严谨的硬件电路设计、稳定且可靠的飞控系统以及模块化的扩展能力，Crazeflie四轴飞行器可以完成很多同类型四轴飞行器无法做到的高难度实验任务。除此之外，Bitcraze团队还为Crazeflie微型四轴飞行器项目编写了大量的使用和发教程，方便全世界的四轴爱好者们进行学习和开发（国内Crazepony和MiniFly两大开源微型四轴项目就在很大程度上参考了Crazeflie中的一些设计理念和内容）。

基础知识

1. 基本结构及飞行原理

1. 坐标系定义

姿态是用来描述一个刚体的固连坐标系和参考坐标系之间的角位置关系。四轴飞行器使用的参考坐标系是当地水平坐标系，即 地理坐标系，而其自身的固连坐标系叫做 载体坐标系。地理坐标系有很多种，这里使用的是比较常用的 NED（即“东北地”）坐标系，如下图所示：

四轴飞行器在空中飞行时，我们使用下图中的姿态角来描述其在空间里的角度关系。其中姿态角包含有翻滚角 Roll（记作$\phi$）、俯仰角 Pitch（记作$\theta$）和航向角 Yaw（记作$\psi$）。

通常我们一般选择把$X$轴作为四轴飞行器的正前方，那么俯仰角$\theta$则为载体绕$Y$轴旋转的角度，指向水平面以下为正，指向水平面以上为负，角度范围从−90°至90°；翻滚角$\phi$为载体绕$X$轴旋转的角度，坐标$Y$指向水平面以上为正，指向水平面以下为负，角度范围从−180°至180°；而航向角$\psi$为机体绕$Z$轴旋转的角度，俯视图逆时针为正，顺时针为负。

如地理坐标系图所示，定义导航坐标系为$OX_{n}Y_{n}Z_{n}$，坐标原点为载体的转动中心，地理坐标系在载体运动时作为基准坐标系，所以求解载体航行姿态时，需要先将载体坐标系内测量到的数据转换到地理坐标系中，再进行姿态解算。

如机体坐标系图所示，定义载体坐标系为$OX_{b}Y_{b}Z_{b}$，其坐标系原点$O_b$在载体的质心或者中心，$X_b$轴沿载体水平方向向前，$Y_b$轴指向朝着载体正前方看的左侧方向，$Z_b$轴则垂直于$OX_{b}Y_{b}Z_{b}$平面沿载体竖轴向上，且载体坐标系$OX_{b}Y_{b}Z_{b}$满足右手法则。

2. 电机布局

四轴飞行器的四个电机呈十字形排列，驱动四片桨旋转产生向上的推力。由于四个电机的轴距几何中心的距离相等，所以当对角两个轴产生的升力相同时能够保证力矩的平衡，四轴不会向任何一个方向倾转。而当四个电机一对正转一对反转时，可使得绕竖直轴方向旋转的反扭矩平衡，保证了四轴航向的稳定。

与传统的直升机相比，四轴飞行器有着下列的优势：各个旋翼对机身所施加的反扭矩与旋翼的旋转方向相反，因此当处于同一对角线的两个电机向相同方向旋转时（不同对角线上的电机转向相反），就可以平衡旋翼对机身的反扭矩。

如上图所示，根据用户自定义的机头的位置不同，四轴飞行器可以分为×模式和+模式。×模式的机头方向位于两个电机之间，而+模式的机头方向位于某一个电机上。×和+就是表示正对机头方向时飞行器的形状。相对而言，×模式稳定一些，但动作更灵活。但如果要完成翻跟头等特技动作，需要用+模式。

3. 动力学原理

四轴飞行器在空间共有6个自由度（分别沿3个坐标轴作平移和旋转动作），这6个自由度的控制都可以通过调节不同电机的转速来实现。基本运动状态分别为：垂直运动、俯仰运动、滚转运动、偏航运动、前后运动和侧向运动。

垂直运动

垂直运动相对来说比较容易。在上图a中，因有两对电机转向相反，可以平衡其对机身的反扭矩，当同时增加四个电机的输出功率，旋翼转速增加使得总的拉力增大，当总拉力足以克服整机的重量时，四轴飞行器便离地垂直上升。反之，同时减小四个电机的输出功率，四轴飞行器则垂直下降，直至平衡落地，实现了沿Z轴的垂直运动。当外界扰动量为零时，在旋翼产生的升力等于飞行器的自重时，飞行器便保持悬停状态。保证四个旋翼转速同步增加或减小是垂直运动的关键。

俯仰运动

在上图b中，电机1的转速上升，电机3的转速下降，电机2、电机4的转速保持不变。为了不因为旋翼转速的改变引起四轴飞行器整体扭矩及总拉力改变，旋翼1与旋翼3转速该变量的大小应相等。由于旋翼1的升力上升，旋翼3的升力下降，产生的不平衡力矩使机身绕Y轴旋转（方向如图所示），同理，当电机1的转速下降，电机3的转速上升，机身便绕y轴向另一个方向旋转，实现飞行器的俯仰运动。

滚转运动

与上图b原理相同，在图c中，改变电机2和电机4的转速，保持电机1和电机3的转速不变，则可使机身绕X轴旋转（正向和反向），实现四轴飞行器的滚转运动。

偏航运动

四轴飞行器偏航运动可以借助旋翼产生的反扭矩来实现。旋翼转动过程中由于空气阻力作用会形成与转动方向相反的反扭矩，为了克服反扭矩影响，可使四个旋翼中的两个正转两个反转，且对角线上的各个旋翼转动方向相同。反扭矩的大小与旋翼转速有关，当四个电机转速相同时，四个旋翼产生的反扭矩相互平衡，四轴飞行器不发生转动；当四个电机转速不完全相同时，不平衡的反扭矩会引起四轴飞行器转动。在图d中，当电机1和电机3的转速上升，电机2和电机4的转速下降时，旋翼1和旋翼3对机身的反扭矩大于旋翼2和旋翼4对机身的反扭矩，机身便在富余反扭矩的作用下绕Z轴转动，实现飞行器的偏航运动，转向与电机1、电机3的转向相反。

前后运动

要想实现飞行器在水平面内前后、左右的运动，必须在水平面内对飞行器施加一定的力。在图e中，增加电机3转速，使拉力增大，相应减小电机1转速，使拉力减小，同时保持其他两个电机转速不变，反扭矩仍然要保持平衡。按图b的理论，飞行器首先发生一定程度的倾斜，从而使旋翼拉力产生水平分量，因此可以实现飞行器的前飞运动。向后飞行与向前飞行正好相反。当然在图b和图c中，飞行器在产生俯仰、翻滚运动的同时也会产生沿X、Y轴的水平运动。

侧向运动

在图f中，由于结构对称，所以侧向飞行的工作原理与上面讲到的前后运动完全一样。

4. 飞行模式

飞行模式分为 有头模式 和 无头模式 两种。其中飞行器在飞行的过程中，其运动的前后左右以地理坐标系为参考坐标系，则为无头模式飞行；而有头模式则是飞行器运动的前后左右以自身的坐标系为参考坐标系。

这里对有头和无头模式进行展开讲解。任何飞行器都一定有个自身的坐标系，也就是飞行器的头和尾，这也就是前面说的飞行器的自身坐标系。如果推动遥控器向前运动，飞行器总是向它头的方向飞行，那么这个飞行器就是运行在有头模式。如果推动遥控器的向前飞行，飞行器还是向它起飞时头指示的方向飞行，即使这个时候飞行器在飞行的过程中改变了机头方向（操纵了遥控的航向角），那么这个飞行器依旧运行在无头模式下。

因为飞行器的无头模式是以地理坐标系为参考，而有头模式则是以飞行器自身的坐标系为参考，所以对于不同的飞行模式，姿态解算算法是不太一样的。对于无头模式，姿态解算部分可以使用磁力计来测量飞行器相对于地球磁场的角度，从而算出机头在磁场中的方向。另外，也可以直接对航向角进行积分，算出飞行器相对于起飞时机头旋转的角度，目前，Breeze微型四轴飞行器使用的就是这种实现方式。

2. 飞行姿态表示方法

1. 欧拉角法

欧拉角由莱昂哈德·欧拉创立，用来描述刚体在三维欧几里得空间的取向。对于在三维空间里的一个参考系，任何坐标系的取向，都可以用三个欧拉角来表现。参考系又称为实验室参考系，是静止不动的，而坐标系则固定于刚体，随着刚体的旋转而旋转。

地理坐标系$OX_{n}Y_{n}Z_{n}$与载体坐标系$OX_{b}Y_{b}Z_{b}$之间的坐标变换矩阵即方向余弦矩阵可以通过三次基本旋转得到。对应的旋转变换矩阵为：

$$\begin{align} C_n^b&= \begin{bmatrix} {\cos{\theta}} & {0} & {-\sin{\theta}} \\ {0} & {1} & {0} \\ {\sin{\theta}} & {0} & {\cos{\theta}} \\ \end{bmatrix} \begin{bmatrix} {1} & {0} & {0} \\ {0} & {\cos{\phi}} & {\sin{\phi}} \\ {0} & {-\sin{\phi}} & {\cos{\phi}} \\ \end{bmatrix} \begin{bmatrix} {\cos{\psi}} & {-\sin{\psi}} & {0} \\ {\sin{\psi}} & {\cos{\psi}} & {0} \\ {0} & {0} & {1} \\ \end{bmatrix} \\&= \begin{bmatrix} {\cos{\theta}\cos{\psi}+\sin{\theta}\sin{\phi}\sin{\psi}} & {-\cos{\theta}\sin{\psi}+\sin{\theta}\sin{\phi}\cos{\psi}} & {-\sin{\theta}\cos{\phi}} \\ {\cos{\phi}\sin{\psi}} & {\cos{\phi}\cos{\psi}} & {\sin{\phi}} \\ {\sin{\theta}\cos{\psi}-\cos{\theta}\sin{\phi}\sin{\psi}} & {-\sin{\theta}\sin{\psi}-\cos{\theta}\sin{\phi}\cos{\psi}} & {\cos{\theta}\cos{\phi}} \\ \end{bmatrix} \end{align}$$

使用下面的欧拉角微分方程可以方便地对四轴飞行器的姿态进行解算：

$$\begin{bmatrix} {\dot\theta} \\ {\dot\phi} \\ {\dot\psi} \\ \end{bmatrix} = \frac{1}{\cos{\theta}} \begin{bmatrix} {\cos{\phi}} & {\sin{\theta}\sin{\phi}} & {\cos{\theta}\sin{\phi}} \\ {0} & {\cos{\phi}\cos{\theta}} & {-\sin{\theta}\cos{\phi}} \\ {0} & {\sin{\theta}} & {\cos{\theta}\cos{\phi}} \\ \end{bmatrix}^{-1} \begin{bmatrix} {\omega_{EbY}^b} \\ {\omega_{EbX}^b} \\ {\omega_{EbZ}^b} \\ \end{bmatrix}$$

上面公式中左侧是更新后的欧拉角，右侧是上个周期测算出来的角度，而三个角速度则由安装在四轴飞行器上的三轴陀螺仪测得。因此，求解这个微分方程就能解算出当前的欧拉角。

不过，由于欧拉微分方程中包含了大量的三角函数运算，这给实时解算带来了一定的难度，而且当俯仰角为正负90°时，方程式会出现神奇的 GimbalLock（万向锁）问题，因此欧拉角解算只适用于水平姿态变化不大的情况，而不适用于全姿态飞行器的姿态确定。

2. 四元数法

四元数是由爱尔兰数学家威廉·卢云·哈密顿在1843年发现的数学概念。明确地讲，四元数是复数的不可交换延伸，如把四元数的集合考虑成多维实数空间的话，四元数就代表着一个四维空间，相对于复数为二维空间。

四元数是简单的超复数。复数是由实数加上虚数单位i组成，其中$i^2=1$。相似地，四元数都是由实数加上三个虚数单位$i、j、k$组成，而且它们有如下的关系: $i^2=j^2=k^2=-1$和$i^0=j^0=k^0=-1$。每个四元数都是$i 、j、k$的线性组合，即四元数可用下面的公式进行表示，其中$q_0、q_1、q_2$和$q_3$是实数，$\omega$是转动的角度，$n$为旋转轴。

$$Q=q_0+q_1i+q_2j+q_3k=q_0+n\sin({\frac{\omega}{2})}$$

采用四元数表示姿态变换时，由四元数的运算法则，将其中的四元数按照元素展开并按照运算符法则进行计算可以得到四元数表示的方向余弦矩阵：

$$C_n^b= \begin{bmatrix} {q_1^2+q_0^2-q_3^2-q_2^2} & {2(q_1q_2-q_0q_3)} & {2(q_1q_3+q_0q_2)} \\ {2(q_1q_2+q_0q_3)} & {q_2^2-q_3^2+q_0^2-q_1^2} & {2(q_2q_3-q_0q_1)} \\ {2(q_1q_3-q_0q_2)} & {2(q_2q_3+q_0q_1)} & {q_3^2-q_2^2-q_1^2+q_0^2} \\ \end{bmatrix}$$

最后综合以上公式可得欧拉角和四元数之间的转换关系：

$$\begin{align} \theta&=-\sin^{-1}\left[{2(q_1q_2-q_0q_3)}\right] \\ \phi&=\tan^{-1}\left[\frac{2(q_1q_3+q_0q_1)}{q_3^2+q_2^2-q_1^2+1_0^2}\right] \\ \psi&=\tan^{-1}\left[\frac{2(q_1q_2+q_0q_3)}{q_1^2+q_0^2-q_3^2-q_2^2}\right] \\ \end{align}$$

相对于另几种旋转表示法（矩阵，欧拉角，轴角），四元数具有某些方面的优势，如速度更快、提供平滑插值、有效避免万向锁问题、存储空间较小等等。

3. 动力学模型建立方法

1. 机理建模法

2. 辨识建模法

姿态信号分析

四轴飞行器的姿态信号主要是指其姿态角，即俯仰角、偏航角和滚转角。四轴飞行器姿态测量主要器件是陀螺仪，加速度计和磁力计。加速度计输出重力在机体坐标系X、Y和Z三个轴上的分量信号，陀螺仪输出机体绕三轴旋转的角速度信号，电子罗盘输出磁场强度在三个轴上的分量信号。将上述三个传感器的输出信号经过滤波、融合等处理后便可以得到俯仰角、滚转角和偏航角信息。

在姿态传感器中，陀螺仪和加速度计是主要的敏感器件，电子罗盘则是作为辅助器件对陀螺仪在偏航角上的积分进行长时矫正。因此本小节主要对陀螺仪和加速度计的姿态信号进行分析和研究。

1. 陀螺仪信号分析

现在设置MPU6050的陀螺仪量程为2000dps，在四轴飞行器水平静止在地面上(电机不转)时，通过IIC总线读取出陀螺仪的三个姿态角速度，并通过串口发送到PC端，功能函数为void MPU6050_RawDataOutput(void)。可以在main.c中定义并实现该函数，然后在主程序硬件初始化完成后调用(注意此时需要使MPU6050工作在软件解算状态下，即需要注释掉User/config.h中的宏定义MPU6050_DMP)，以下是完整代码：

void MPU6050_RawDataOutput(void)
{
    u8 i;
    s16 main_acc_adc[3];
    s16 main_gyr_adc[3];
    float main_acc_raw[3];
    float main_gyr_raw[3];

     while (1) // Software Calc
    {
        MPU6050_ReadAcc(main_acc_adc);
        MPU6050_ReadGyr(main_gyr_adc);
        for (i = 0; i < 3; i++)
        {
            main_acc_raw[i] = (float) main_acc_adc[i] * IMU_ACC_SCALE *
                IMU_CONSTANTS_ONE_G;
            main_gyr_raw[i] = (float) main_gyr_adc[i] * IMU_GYR_SCALE *
                M_PI / 180.0F;
//            printf("acc_adc: %.8f gyro_adc: %.8f\r\n",
//            main_acc_adc[i], main_gyro_adc[i]);
//            printf("acc_raw: %.8f gyro_raw: %.8f\r\n",
//            main_acc_raw[i], main_gyr_raw[i]);
        }

        printf("%.8f %.8f %.8f %.8f\r\n", Delay_GetRuntimeMs() / 1000.0F,
                      main_gyr_raw[0], main_gyr_raw[1], main_gyr_raw[2]);
    }
}

考虑到MPU6050的ADC建立稳定的采样需要时间，所以这里只截取使用3.86s~7.79s共616组数据，最后调用Matlab绘图得到原始姿态角数据随时间变化的曲线。源代码和测试数据可以从breeze_experimental_research中获得，文件名为MPU6050_RawDataOutput.m和MPU6050_RawGyroData.txt，以下是Matlab绘图程序：

clear all;
clc;

data2=load('./MPU6050_RawGyroData.txt'); 
x=data2(:,1);
y1=data2(:,2);
y2=data2(:,3);
y3=data2(:,4);

subplot(3, 1, 1);
plot(x,y1);
xlabel('t/s');
ylabel('Amplitude/LSB');
title('GYRO X Axis');

subplot(3, 1, 2);
plot(x,y2);

xlabel('t/s');
ylabel('Amplitude/LSB');
title('GYRO Y Axis');

subplot(3, 1, 3);
plot(x,y3);

xlabel('t/s');
ylabel('Amplitude/LSB');
title('GYRO Z Axis');

最后得到的姿态角曲线如下所示：

分析上述曲线可发现四轴飞行器水平静止在地面上(电机不转)时，各轴输出的波动较小，均小于正负0.03LSB。但是不难发现X、和Z轴数据均为正值，且分别在2*10-3LSB和6.5*10-3LSB附近波动，而Y轴数据均为负值，且-0.0255LSB附近波动，然而正常状态下输出的数据应该在0附近波动。出现这个问题实际上是因为MPU6050在输出数据时没有经过初始偏差矫正，存在一个常值误差，发生了 “角速率零点漂移” 现象。此时只要每次在采样数据时减去这个常值误差即可。如果读者细心的话还会发现各个轴信号波峰和波谷在波形中所占比例是不同的，比如Y轴波峰所占比例就更大一些，这样对该角速率的积分会随着时间的增加而偏向于一个方向增加。这就是陀螺仪的 “积分漂移” 现象，而波形中的波峰和波谷所占比例不同是硬件存在的固有问题，没法通过软件消除。所以陀螺仪数据在未经过其他数据补偿校正的情况下只能在短时间内使用，否则最终计算出的误差会很大。

以上是在四轴飞行器没有电机振动干扰的情况下得到的结果，但在实际的飞行过程中，高速转动的电机所带来的机体振动会给陀螺仪带来严重的噪声干扰。为了分析电机振动对姿态数据的影响，在保持采样配置相同的情况下，使四轴飞行器静止在水平面，但电机处于怠速运转状态。使用前面介绍过的void MPU6050_RawDataOutput(void)函数串口输出数据并调用Matlab程序绘图。这里截取3.87s~12.68s之间的1354组数据进行处理，为了分析信号的频域特性，对采样的信号进行FFT变换，绘制出频谱图。源代码和测试数据可以从breeze_experimental_research中获得，文件名为MPU6050_RawDataNoiseOutput.m和MPU6050_RawGyroNoiseData.txt，最后得到的波形如下：

分析上述时域波形确实可以看到电机的高速转动所产生的振动使得陀螺仪输出数据的波动幅度相比电机不转时的更大，大约有0.2LSB。由频域波形可以看到在500Hz~900Hz之间存在高频振动，而四轴飞行器是静止在水平面上的，不可能产生高频的信号，所以可知该信号只可能对应于电机和桨叶的高速旋转所带来的高频振动。

2. 加速度计信号分析

设置MPU6050加速度的量程为8g(4096LSB/g)，在四轴飞行器水平静止在地面上(电机不转)时，调用void MPU6050_RawDataOutput(void)串口输出加速度在三个轴上的分量值，这里截取3.87s~7.89s间的587组数据并调用Matlab绘图。最后得到的加速度分量波形如下所示：

分析上述波形后可发现，由于四轴飞行器没有完全水平放置在地面上，所以X和Y轴的输出没有在0附近波动，Z轴也没有在1g(9.8m/s^2)附近波动。而且加速度计对微小振动非常敏感，即使没有电机振动，输出也会有一定的波动，但是波动幅值小于0.05g，对姿态的解算影响不大。然而当四轴飞行器水平放置地面并同时开启电机怠速转动时，截取串口从3.86s~12.68s之间发来的共1310组数据并调用Matlab绘图处理，源代码和测试数据可以从breeze_experimental_research中获得，文件名为MPU6050_RawDataNoiseOutput.m和MPU6050_RawAccNoiseData.txt，最后得到的波形如下：

由上述波形可见加速度输出信号中存在大量的干扰噪声，其幅值最高可达1.5g/(m/s^2)，已经将真实信号完全掩盖了，这样的信号是没法直接使用的。

姿态数据滤波

上面对姿态传感器(陀螺仪和加速度计)数据的分析表明：四轴飞行器在实际飞行过程中电机和桨叶高速旋转所带来的扰动会对姿态数据的精度产生很大的影响。但是噪声信号大多集中在高频段，此时考虑设计一个滤波器将高频噪声信号滤除掉来 提高有用信号在总信号中所占的比例。

1. 数字滤波器设计

1. 常用数字滤波器与IIR滤波器简介

数字滤波器可以分为经典滤波器和现代滤波器。

• 经典滤波器：假定输入信号中的有用成分和希望去除的成分各自占有不同的频带。如果信号和噪声的频谱相互重迭，经典滤波器则无能为力。常见的经典滤波器有FIR和IIR滤波器等。
• 现代滤波器：从含有噪声的时间序列中估计出信号的某些特征或信号本身。现代滤波器将信号和噪声都视为随机信号。常见的包括Wiener滤波器、Kalman滤波器、线性预测器、自适应滤波器等。

IIR数字滤波器是一类递归型的线性时不变因果系统，其差分方程可以写为：

$$y(n) = \sum_{i=1}^N a_{i}y(n-i) + \sum_{i=0}^M b_{i}x(n-i)$$

对上式进行Z变换得：

$$Y(z) = \sum_{i=1}^N a_{i}z^{-i}Y(z) + \sum_{i=0}^M b_{i}z^{-i}X(z)$$

对上式等式左右两边合并同类项后得传递函数$H(z)$：

$$H(z) = \frac{Y(z)}{X(z)} = \frac{\sum_{i=0}^M b_{i}z^{-i}}{1-\sum_{i=1}^N a_{i}z^{-i}} = b_{0}\frac{\prod_{i=1}^M (1-c_{i}z^{-1})}{\prod_{i=1}^N (1-d_{i}z^{-1})}$$

其中$c_{i}$为零点而$d_{i}$为极点。$H(z)$的设计目标就是要确定其系数和零极点，以使滤波器满足给定的性能指标。

数字滤波器的功能本质上是将一组输入数字序列通过一定的运算后转变为另一组输出数字序列。滤波器系统函数可以表达为多种不同的形式，每一种对应着不同的算法，也就对应着不同的实现结构。例如：

$$H(z)=\frac{1}{1-0.3z^{-1}-0.4z^{-2}}$$

可以分解为：

$$H(z)=\frac{1}{1-0.8z^{-1}}\cdot \frac{1}{1+0.5z^{-1}}$$

或：

$$H(z)=\frac{0.6154}{1-0.8z^{-1}}+\frac{0.3846}{1+0.5z^{-1}}$$

上述同一系统的三种不同描述形式就对应了不同的实现结构，或者说不同的滤波器结构可以实现相同的传递函数。IIR滤波器常见的结构形式有直接Ⅰ型、直接Ⅱ型（典范型）、级联型、并联型。通过差分方程能够画出包含反馈结构的数字网络称为直接型。

直接Ⅰ型滤波器的网络结构可以根据差分方程很直观地画出（The Direct-Form I structure implements the feed-forward terms first followed by the feedback terms）：

可以看出直接Ⅰ型滤波器需要N+M个延时单元（N≥M）。直接Ⅱ型结构是对直接Ⅰ型的变型，将上面网络的零点与极点的级联次序互换，并将延时单元合并。实现N阶滤波器只需要N个延时单元（The Direct-Form II structure uses fewer delay blocks than Direct-Form I），故称为典范型。

直接Ⅱ型看上去不那么直观，可以通过下图进行理解。我们可以将整个滤波器系统看成A、B两个子系统串联而成，由于为线性系统因此交换顺序不影响最终输出结果，传递函数可写为：

$$H(z)=B(z)\cdot \frac{1}{A(z)}=\frac{1}{A(z)}\cdot B(z)$$

• 直接型

  优点：直接型结构简单，用的延迟器较少（N和M中较大者的个数）

  缺点：系数$a_{k}$，$b_{k}$对滤波器性能的控制关系不直接，因此调整不方便；具体实现滤波器时$a_{k}$，$b_{k}$的量化误差将使滤波器的频率响应产生很大的改变，甚至影响系统的稳定性。直接型结构一般用于实现低阶系统。

• 级联型

  将系统传递函数$H(z)$因式分解为多个二阶子系统，系统函数就可以表示为这些二阶子系统传递函数的乘积。实现时将每个二阶子系统用直接型实现，整个系统函数用二阶环节的级联实现。

• 并联型

  与级联型类似，用部分分式展开法将系统函数表示为二阶子系统传递函数的和。每个二阶子系统仍然用直接型实现，整个系统函数用二阶环节的并联实现。

2. 使用Matlab辅助设计IIR滤波器

在IIR滤波器设计过程中，通常利用模拟滤波器来设计数字滤波器。即先根据滤波器的性能指标设计出相应的模拟滤波器的系统函数$H(s)$，然后再由$H(s)$经变换得到所需要的数字滤波器的系统函数$H(z)$。常用的变换方法有冲激响应不变法和双线性变换法。下面使用Matlab自带的滤波器工具箱来可视化生成数字滤波器和所需要的相关系数：

在Matlab命令行界面中输入 fdatool 打开滤波器设计工具箱。Design Method 用于选择IIR滤波器还是FIR滤波器，这里我们选择IIR滤波器，类型选择 Butterworth，当然也可以选择其他类型，不同类型的频率响应不同，选择后默认的滤波器结构是直接II型。ResponseType 用于选择低通、高通、带通、带阻等类型，选择低通滤波“Lowpass”。Frequency Specifications 用于设置采样频率以及截止频率，这里填入200以及30，即采样率为200Hz，30Hz以上的频率将被滤除掉。Fiter Order 选择滤波器阶数，为了简单起见，先选择一个2阶滤波器做实验。

参数设置好后点击 Design filter 按钮，将按要求设计滤波器。默认生成的IIR滤波器类型是 Direct-Form II,Second-Order Sections（直接Ⅱ型，每个Section是一个二阶滤波器），在工具栏上点击 Filter Coefficients 图标或菜单栏上选择 Analysis→Filter Coefficients 可以查看生成的滤波器系数。具体操作如下图所示

由Matlab工具箱依据给定参数设计出的滤波器系数如下图所示：

其表示的系统函数为：

$$H(z) = \frac{0.131106439(1 + 2z^{-1} + z^{-2})}{1 - 0.747789178z^{-1} + 0.272214937z^{-2}}$$

高阶IIR滤波器是采用二阶滤波器级联的方式来实现的，其系统函数为多个二阶滤波器系统函数的乘积。默认情况下，Filter Coefficients 把结果分成多个二阶Section显示，其中还包括有增益，增益的目的是为了保证计算的精度和系统的稳定性。 选择 [edit]→[Convert to Single Section]，这时候系数就变成我们所熟悉的形式了，由于这里设计的就是二阶滤波器，所以转换后的参数就是将增益乘到括号中的各项后展开得到的值。

其表示的系统函数为：

$$H(z) = \frac{0.131106439 + 0.262212879z^{-1} + 0.131106439z^{-2}}{1 - 0.747789178z^{-1} + 0.272214937z^{-2}}$$

Matlab中二阶滤波器差分方程公式如下（注意反馈项符号为负号）：

$$y(n) = b_0 \cdot x(n) + b_1 \cdot x(n-1) + b_2 \cdot x(n-2) -a_1 \cdot y(n-1) - a_2 \cdot y(n-2)$$

按照上面的公式，滤波器差分方程为：

$$y(n) = 0.131106439 \cdot x(n) + 0.262212879 \cdot x(n-1) + 0.131106439 \cdot x(n-2) \\ - (- 0.747789178) \cdot y(n-1) - 0.272214937 \cdot y(n-2)$$

为了能够在微控制器上实现所设计的数字滤波器，将上面得到的差分方程用C语言改写后得到：

const int maxn  = 16;

double w_x[maxn], w_y[maxn];

const double B[] = {0.13110643991662596, 0.26221287983325192, 0.13110643991662596};
const double A[] = {1, -0.74778917825850344,  0.27221493792500728};

void Init() {
    memset(w_x, 0, sizeof(w_x));
    memset(w_y, 0, sizeof(w_y));    
}

double Filter(double data) {
    double out;

    w_x[0] = data;
    w_y[0] = B[0] * w_x[0] + B[1] * w_x[1] + B[2] * w_x[2] - w_y[1] * A[1]
    - w_y[2] * A[2];
    out    = w_y[0] / A[0];

    w_x[2] = w_x[1];
    w_x[1] = w_x[0];

    w_y[2] = w_y[1];
    w_y[1] = w_y[0];

    return out;
}

2. 数字滤波器性能测试

用上面设计出的滤波器对前面陀螺仪的噪声数据进行滤波处理，即先将噪声数据读入，然后再调用double Filter(double data)函数依次处理x、y和z轴的数据。注意每次处理一组数据时要先调用void Init()函数初始化。源代码和测试数据可以从breeze_experimental_research中获得，文件名为IIR_digital_filter_second_order.cpp和MPU6050_RawGyroNoiseData.txt，完整代码如下所示：

#include <cstdio>
#include <cstring>
#include <iostream>
using namespace std;

const int maxn  = 16;
const int maxnn = 6666;

double w_x[maxn], w_y[maxn];

const double B[] = {0.13110643991662596, 0.26221287983325192, 0.13110643991662596};
const double A[] = {1, -0.74778917825850344,  0.27221493792500728};

void Init() {
    memset(w_x, 0, sizeof(w_x));
    memset(w_y, 0, sizeof(w_y));    
}

double Filter(double data) {
    double out;

    w_x[0] = data;
    w_y[0] = B[0] * w_x[0] + B[1] * w_x[1] + B[2] * w_x[2] - w_y[1] * A[1]
    - w_y[2] * A[2];
    out    = w_y[0] / A[0];

    w_x[2] = w_x[1];
    w_x[1] = w_x[0];

    w_y[2] = w_y[1];
    w_y[1] = w_y[0];

    return out;
}

int len = 0;
double va[maxnn], vx[maxnn], vy[maxnn], vz[maxnn];
double vxx[maxnn], vyy[maxnn], vzz[maxnn];

int main()
{
    freopen("MPU6050_RawGyroNoiseData.txt", "r", stdin);
    freopen("MPU6050_FilteredGyroData_second_order.txt", "w", stdout);

    double data;
    while(scanf("%f", &data) != EOF) {
        va[len] = data;
        scanf("%f %f %f", &vx[i], &vy[i], &vz[i]);
        len++;
    }

    Init();
    for(int i = 0; i < len; i++)
        vxx[i] = Filter(vx[i]);

    Init();
    for(int i = 0; i < len; i++)
        vyy[i] = Filter(vy[i]);

    Init();
    for(int i = 0; i < len; i++)
        vzz[i] = Filter(vz[i]);


    for(int i = 0; i < len; i++)
        printf("%f %f %f %f\n", va[i], vxx[i], vyy[i], vzz[i]);

    return 0;
}

滤波后的陀螺仪数据存储在MPU6050_FilteredGyroData_second_order.txt中，对其调用Matlab绘图，绘图程序的完整代码如下：

clear all;
clc;

is_gyro = 1;

if is_gyro == 1
    data1 = load('./MPU6050_RawGyroNoiseData.txt');
    data2 = load('./MPU6050_FilteredGyroData_second_order.txt');
%     data2 = load('./MPU6050_FilteredGyroData_fifth_order.txt');
    x1  = data1(:, 1);
    y11 = data1(:, 2);
    y12 = data1(:, 3);
    y13 = data1(:, 4);

    x2  = data2(:, 1);
    y21 = data2(:, 2);
    y22 = data2(:, 3);
    y23 = data2(:, 4);

    subplot(3, 2, 1);
    plot(x1, y11, x2, y21, 'r');
    axis([3 13 -0.2 0.2]);
    xlabel('t/s');
    ylabel('Amplitude/LSB');
    title('GYRO X Axis');
    legend('Raw Noise Data','Filtered Data')

    subplot(3, 2, 2);
    plot(abs(fft(y11))); hold on;
    plot(abs(fft(y21)), 'r'); hold on;
    axis([0 1400 0 20]);
    xlabel('f/Hz');
    ylabel('Amplitude/LSB');
    legend('Raw Noise Data','Filtered Data')

    subplot(3, 2, 3);
    plot(x1, y12, x2, y22, 'r');
    axis([3 13 -0.15 0.1]);
    xlabel('t/s');
    ylabel('Amplitude/LSB');
    title('GYRO Y Axis');
    legend('Raw Noise Data','Filtered Data')

    subplot(3, 2, 4);
    plot(abs(fft(y12))); hold on;
    plot(abs(fft(y22)), 'r'); hold on;
    axis([0 1400 0 40]);
    xlabel('f/Hz');
    ylabel('Amplitude/LSB');
    legend('Raw Noise Data','Filtered Data')

    subplot(3, 2, 5);
    plot(x1, y13, x2, y23, 'r');
    axis([3 13 -0.05 0.15]);
    xlabel('t/s');
    ylabel('Amplitude/LSB');
    title('GYRO Z Axis');
    legend('Raw Noise Data','Filtered Data')

    subplot(3, 2, 6);
    plot(abs(fft(y13))); hold on;
    plot(abs(fft(y23)), 'r'); hold on;
    axis([0 1400 0 25]);
    xlabel('f/Hz');
    ylabel('Amplitude/LSB');

    legend('Raw Noise Data','Filtered Data')
else
    data = load('./MPU6050_FilterAccData.txt'); 
    x  = data(:, 1);
    y1 = data(:, 2);
    y2 = data(:, 3);
    y3 = data(:, 4);

    subplot(3, 2, 1);
    plot(x, y1);
    xlabel('t/s');
    ylabel('g/(m/s^2)');
    title('ACC X Axis');

    subplot(3, 2, 2);
    plot(abs(fft(y1)));
    xlabel('f/Hz');
    ylabel('Amplitude/LSB');

    subplot(3, 2, 3);
    plot(x, y2);
    xlabel('t/s');
    ylabel('g/(m/s^2)');
    title('ACC Y Axis');

    subplot(3, 2, 4);
    plot(abs(fft(y2)));
    xlabel('f/Hz');
    ylabel('Amplitude/LSB');

    subplot(3, 2, 5);
    plot(x, y3);
    xlabel('t/s');
    ylabel('g/(m/s^2)');
    title('ACC Z Axis');

    subplot(3, 2, 6);
    plot(abs(fft(y3)));
    xlabel('f/Hz');
    ylabel('Amplitude/LSB');
end

源代码和测试数据可以从breeze_experimental_research中获得，文件名为MPU6050_FilteredDataOutput.m和MPU6050_FilteredGyroData_second_order.txt，最后得到的波形如下：

从图中可以看到，红色波形所表示的滤波后数据的噪声比原始采集数据要小得多，而且从频谱图中可以明显看到高频信号的幅值响应被有效抑制。这说明设计的IIR低通滤波器是有效的，将其应用到陀螺仪信号的实时滤波是合理的，这样对滤波后的数据进行姿态解算得到的飞行器的姿态才是准确的。加速度数据滤波器设计和陀螺仪的类似，这里就不再赘述。

姿态解算

姿态解算指的是利用陀螺仪、加速度计和磁力计等姿态传感器数据通过数学计算得到飞行器三个姿态角(即俯仰、偏航和滚转角)的过程。为了能够更加深刻地理解姿态解算算法，现先对各姿态传感器的特性和测量原理进行简要介绍。

• 加速度计

  加速度直接测量的是重力矢量在机体坐标系三个轴上的分量，由于有绝对的参照物即重力轴，因此在 无外力的加速的情况下，能够准确测量出俯仰角和滚转角，并且不会存在累积误差。虽然经过低通滤波器滤除了大部分由于电机振动而引入的噪声信号，但是当飞行器在三维空间做变速运动时，加速度传感器同样会检测到变速运动的加速度信号，从而导致姿态角的解算出现偏差。

• 陀螺仪

  陀螺仪测量飞行器绕机体坐标系三个轴的旋转角速度，通过对角速度的积分进而得到旋转角。在给定初始旋转角的前提下，可以测量出飞行器的俯仰、偏航和滚转角。其工作原理决定了其测量值是相对某个惯性空间的，不需要外部的参照物。因此陀螺仪数据只在较短的时间内是有意义的。即使经过滤波后的陀螺仪信号噪声很小，但随着时间的增加，其积分误差也将逐渐增大，导致测量出的姿态角出现误差。

• 磁强计

  磁力计直接测量的是地磁场强度矢量在机体坐标系三个轴上的分量，与加速度计一样也有绝对的参照物地磁场，因此在水平面检测时可以准确检测地磁场方向用于飞行器偏航角的解算，但当飞行器倾斜时，其计算结果就需要根据倾斜角进行相应的补偿。

姿态解算分为 快速解算 和 深度解算 两种。

• 快速解算

  快速解算指的是根据陀螺仪的三轴角速度对时间的积分直接得到四轴飞行器的俯仰、偏航和滚转角。所以快速解算得到的姿态是存在误差的，而且这种误差还会随着时间而累积。这种解算方法在实际中较少使用。

• 深度解算

  深度解算指的是在陀螺仪数据基础上再结合三轴地磁和三轴加速度数据进行漂移补偿和校正。从上面对传感器数据的特性进行分析后可以得到：无论只使用哪一个传感器都无法准确测量飞行器的三个姿态角信息，此时需要综合使用每个传感器的数据，使其各自的优缺点得到互补。这种解算方法被广泛使用，下面就来介绍3种常用的深度解算算法。

1. 互补滤波

2. 卡尔曼滤波

3. 四元数引入

最小二乘辨识得到系统动力学模型(被控对象)

1. 辨识目的和先验知识

2. 实验设计

3. 模型结构辨识

4. 模型参数辨识

串联PID控制算法设计与整定

1. Matlab仿真

2. 真机调试

高度串联PID控制算法与整定

1. Matlab仿真

2. 真机调试

总体调试与经验总结

算法讲解

姿态解算算法

解算流程

姿态解算的核心在于旋转，旋转共有4种表示方式：矩阵、欧拉角、轴角和四元数。其中矩阵适合变换向量，欧拉角最直观，轴角则适合几何推导，而在组合旋转方面，四元数表示最佳。因为姿态解算需要频繁组合旋转并用旋转变换向量，所以应采用四元数来保存飞行器的姿态（地理坐标系中的俯仰/翻滚/航向角）。下图是姿态解算的整个流程：

如上图所示，STM32会通过IIC总线采集MPU6050中陀螺仪和加速度计的AD值，之后再通过姿态解算算法得到飞行器当前的姿态（姿态使用四元数表示），然后将四元数转化为欧拉角，用于后面的姿态PID控制。

解算算法

姿态解算算法会通过巧妙的方法来使用加速度计数据去修正由陀螺仪数据经过快速解算所产生的姿态误差，并最终得到准确的飞行器姿态。目前常用的软件姿态解算算法为：非线性互补滤波算法、Kalman滤波算法、Mahony互补滤波算法等。下面对其中的Mahony互补滤波算法进行介绍（Breeze微型四轴飞行器使用的就是这种）。

Mahony互补滤波算法的原理是根据加速度计和磁力计的数据，与转换到载体坐标系的参考重力向量和地磁向量进行求误差，用这个误差来矫正陀螺仪的输出，然后用它来更新四元数，最后再将四元数转换为欧拉角。以下是算法执行的具体步骤：

1. 对加速度数据进行归一化，得到单位加速度。

2. 将飞行器上次计算得到的姿态（四元数）换算成方向余弦矩阵中第三列的三个元素。根据余弦矩阵和欧拉角的定义，地理坐标系的重力向量转换到载体坐标系下，正好是方向余弦矩阵第三列的三个元素。

3. 计算叉乘误差。在载体坐标系上，加速度计测出来的重力向量和根据上次姿态解算的姿态所推算出的重力向量之间的误差向量，就是上次姿态解算（可以认为是陀螺仪积分）后的姿态和加速度计测出来的姿态之间的误差。向量间的误差，可以用向量积来表示。因为叉积和陀螺仪积分误差都处于载体坐标系下而且成正比，所以可以使用叉积向量来修正陀螺仪积分误差。由于陀螺仪是对载体直接进行积分的，所以对陀螺仪的纠正量会直接体现在对载体坐标系的纠正。

4. 对叉乘误差对时间进行积分。

5. 使用叉乘误差来做PI修正陀螺仪零点漂移，通过调节参数，可以控制加速度计修正陀螺仪积分姿态的速度。

6. 使用四元数微分方程，得到修正后的陀螺仪数据，再对其进行时间积分，得到使用四元数表示的飞行器当前姿态，最后对四元数进行单位化处理就完成整个算法执行流程。

7. 姿态控制算法

控制原理

四轴飞行器的旋翼与空气发生相对运动，产生了向上的升力，当升力大于四轴的重力时四轴飞行器就可以飞起来了。理想情况下，只要四个电机的转速是完全相同，就可以使四轴飞行器在飞行过程中保持水平，但由于电机和旋翼本身制造上的差异，想要控制四个电机达到相同的转速是不可能的。因此，为了防止四轴飞行器发生侧翻的情况，需要使用PID自动反馈控制系统来完成对四轴飞行器的自稳定。

PID控制理论

PID控制是最常见，应用最为广泛的自动反馈系统。PID控制器由偏差的比例（P: Proportional）、积分（I: Integral）和微分（D: Derivative）来对被控对象进行控制。这里的积分或微分都是偏差对时间的积分或微分。

对于一个自动反馈控制系统来说，共有以下几个基本指标：

稳定性（P和I降低系统稳定性，D提高系统稳定性）：在平衡状态下，系统受到某个干扰后，经过一段时间其被控量可以达到某一稳定状态。

准确性（P和I提高稳态精度，D无作用）：系统处于稳态时，其稳态误差。

快速性（P和D提高响应速度，I降低响应速度）：系统对动态响应的要求。一般由过渡时间的长短来衡量。

• 比例控制

比例控制是一种最简单的控制方式，其控制器的输出与输入误差信号成比例关系。当仅有比例控制时系统输出存在稳态误差。比例项输出：

$$P_{out}=K_pe(t)$$

• 积分控制

在积分控制中，控制器的输出与输入误差信号的积分成正比关系。对于只有比例控制的系统存在稳态误差，为了消除稳态误差，在控制器中必须引入积分项。积分项是误差对时间的积分，随着时间的增加，积分项会增大，这样，即便误差很小，积分项也会随着时间的增加而加大，它推动控制器的输出增大使稳态误差进一步减小，直到等于零。因此，比例积分（PI）控制器可以使系统在进入稳态后无稳态误差。积分项输出：

$$I_{out}=K_i\int_0^te(\tau)d\tau$$

• 微分控制

在微分控制中，控制器的输出与输入误差信号的微分成正比关系。微分调节的是偏差值的变化率，使用其能够实现系统的超前控制。如果输入偏差值线性变化，则需要在调节器输出侧叠加一个恒定的调节量。大部分控制系统不需要调节微分时间，因为只有时间滞后的系统才需要附加这个参数。微分项输出：

$$D_{out}=K_d\frac{de(t)}{dt}$$

综上所述，PID控制的数学公式为：

$$\mu(t)=K_pe(t)+K_i\int_0^te(\tau)d\tau+K_d\frac{de(t)}{dt}$$

单环PID控制

如上图所示，输入的期望角度就是远程遥控端控制飞行器的角度值，反馈当前角度就是由传感器测得的飞行器角度，这里的角度值分别指的是翻滚角、俯仰角和航向角，在做PID控制计算的时候，它们是相互独立的。这里以翻滚角为例，介绍一下PID计算的全过程：

1. 计算角度误差

角度误差=期望角度-当前角度

1. 计算比例项

比例项=比例系数角×度误差

1. 计算积分项

微分项=微分项系数×角度误差积分

1. 计算微分项

微分项=微分系数×角速度的微分

1. 整合结果输出

结果输出=比例项+积分项+微分项

以上步骤适用于翻滚角和俯仰角的PID计算，但航向角比较特殊，因为航向角法线方向刚好和地球重力平行，因此这个方向的角度无法由加速度计直接测得。由于Breeze Mini微型四轴飞行器并没有使用磁力计，所以使用PID计算航向角时候，就不存在比例项，只能使用微分项来调节。

串级PID控制

角度单环PID控制算法仅仅考虑了四轴飞行器的角度信息，如果要增加四轴飞行器的稳定性并提高控制质量，可以使用下面的角度/角速度串级PID控制算法：

如上图所示，串级PID控制算法其实就是将角度环PID和角速度环PID控制算法串联了起来，因为两个控制器比单个能控制更多的变量，因此它增强了系统的抗干扰性，使得四轴飞行器的适应能力更强。它的计算过程如下：

1. 计算角度误差

角度误差=期望角度-当前角度

1. 计算外环比例项

外环比例项=外环比例系数×角度误差

1. 计算外环积分项

外环积分项=外环积分系数×角度误差积分

1. 整合外环输出

外环结果输出=外环比例项+外环积分项

1. 计算角速度误差

角速度误差=外环结果输出-当前角速度

1. 计算内环比例项

内环比例项=内环比例系数×角速度误差

1. 计算内环积分项

内环积分项=内环积分系数×角速度误差积分

1. 计算内环微分项

内环微分项=内环微分系数×当前角速度微分

1. 整合内环结果输出

内环结果输出=内环比例项+内环积分项+内环微分项

电机输出

电机输出是姿态控制算法的最后一步，它在油门基准值的基础之上整合上面通过PID控制算法得到的翻滚角、俯仰角和航向角进行电机控制。

• 自主悬停

自主悬停指的是四轴飞行器能够悬停在某个位置上，在发生了偏移之后依然能够自动校正并回到原来悬停的位置。由于四轴飞行器在空中的位置是用一个三维坐标来表示的，对于自主悬停，其中涉及了两个维度。第一是水平面上的自主悬停，飞行器不能够发生左右或前后的位置漂移。第二个是在垂直方向上，飞行器不能够发生太大的高度变化。针对这两个不同的维度，有不同的解决办法。在水平面上，为了确定飞行器的位置，可以使用GPS或光流传感器进行定位。而在垂直方向上，一般使用气压计或超声波模块进行定高。

目前，Breeze Mini微型四轴飞行器拥有一个板载的高精度气压计MS5611，所以可以实现高度的自主悬停。在水平面上，虽然该飞行器还装备有一个小型的摄像头图传模块，但由于视觉算法部分还存在着一些问题，所以目前仅靠3轴加速度计和3轴陀螺仪数据来做的双环PID控制是无法使其在水平面上做到真正意义上的自主悬停的。以下简单地讲解一下目前Breeze微型四轴飞行器所使用的高度双环PID控制算法：

如上图所示，由于MS5611气压计的精度为10cm，所以需要融合加速度计互补滤波得到较为准确的高度值。用高度作为外环，速度作为内环形成高度双环PID控制，最后调节输出油门以实现Z轴的自主悬停。

总结

到此为止，Breeze微型四轴飞行器飞控算法方面的内容就全部介绍完了，我想有很多同学看完之后还是会觉得云里雾里。的确，飞控这一部分在整个四轴飞行器的开发过程中算是最具挑战性，同时也是最难以理解的，所以不要指望仅仅只看几篇网上的博客教程就能入门飞控。如果你的研究方向是基于四轴飞行器平台来做上层研究的话（比如视觉避障、视觉SLAM、自主悬停和运动规划等），那么目前你所了解到的飞控知识应该已经足够，但是如果你想研究四轴飞行器的底层飞控算法实现，甚至是想自己实现一整套飞控算法的话，那么你还需要阅读更多飞控方面的专业书籍、论文，并要对国内外优秀的开源飞控代码进行深入的研究，我相信只要你足够的努力，你也可以成为别人眼里的飞控大神！

最后，简单预告一下下一篇文章所要讲的内容。下一篇文章我主要会介绍Breeze微型四轴飞行器上层软件开发的相关内容，除此之外，由于下一篇文章是Breeze微型四轴系列的最后一篇，所以我还会在文章中对之前整个Breeze微型四轴项目所讲的内容进行总结，敬请期待。

参考

• IIR数字滤波器的实现(C语言)

• 四轴飞行器姿态解算——互补滤波算法及理论推导