团队机器人系列（六）：微风四轴飞行器-STM32嵌入式开发-飞控算法理论

前言

前面我介绍了很多有关微型四轴飞行器嵌入式底层开发的知识，虽然对于很多初学者来说，其中有一些概念比较晦涩难懂，但是它们确实能够让大家对于嵌入式底层开发有更深入的理解与掌握。好了言归正传，在本篇文章中我会向大家介绍有关微型四轴飞行器飞控算法理论方面的相关内容，而至于什么是飞控，我想很多自己买过成品飞控板、组装过航模或四轴飞行器的同学都不会陌生，顾名思义，飞控指的是飞行控制算法，即通过获取IMU（惯性传感单元）数据，并采用滤波、PID等算法对飞行器当前的姿态进行实时解算，得到各电机的转速和转向，从而完成对飞行器的控制。由于飞控算法本身涉及到很多自动控制原理和线性代数方面的理论知识，所以推荐大家先把自控和线代好好复习一遍再去研究飞控算法，这样理解起来可能会容易一些。

研究现状

• 国内研究现状

  DJI

  

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

  Crazepony

  

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

  MiniFly

  

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

• 国外研究现状

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

  APM

  

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

  PX4

  

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

  CC3D

  

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

  MWC

  

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

  PPZ

  

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

  Crazeflie

  

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

基本概念

• 电机布局

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

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

  

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

• 动力学原理

  四轴飞行器在空间共有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)中，由于结构对称，所以侧向飞行的工作原理与上面讲到的前后运动完全一样。

• 飞行模式

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

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

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

软件框架

嵌入式系统软件实现部分没有使用实时操作系统（RTOS），而是直接通过编写裸机代码，依靠中断嵌套来完成整体功能的。在代码的初始化阶段，程序对STM32的定时器4进行了初始化，它可以中断while死循环，用于更新远程遥控数据、机身姿态融合、PID控制计算输出和电机PWM输出等操作。

总之，定时器4中断是核心中断，所有的飞控算法都是在这里实现的，机身的稳定也是依靠这个中断来实现的。接下来，本文章将结合程序执行流程图来详细地讲解定时器4中断内部的组织结构和实现的功能：

如上图所示，程序会在主循环中不断轮询各个定时器的标志位，从而产生三个大小不同的时间段。在这些时间段内，系统便可以完成像接收遥控器指令、更新传感器数据以及更新电机控制等任务。主程序通过划分不同频率的定时器循环，使得所有任务可以分时并发地执行，这有效地降低了函数串行执行时的延时问题，并且提高了主控对资源的利用率。

• 系统初始化

  主函数在开始执行阶段会先调用Hardware_Init()函数进行硬件相关设备和功能的初始化。初始化完成之后程序就会进入主循环while(1)中，主循环也是整个软件功能实现的关键，程序一旦进入便会在里面不断地运行，只有发生中断时程序才会暂时离开主循环，而去执行优先级高的中断服务程序。

• 主循环-100Hz循环

  主循环体内会首先判断timer_loop_flag_100hz的值是否为真，而timer_loop_flag_100hz标志位是在TIM4定时器中断中每10ms置位一次，即该循环会每10ms执行一次其中的工作。

  在这个循环之中，STM32会读取MPU6050和MS5611数据并进行融合。因为本论文采用的是软件解算MPU6050的姿态，所以读取的数据应为陀螺仪和加速度计的AD值，之后再将数据进行标定、滤波、校正后通过四元数融合得到三轴欧拉角，具体如下图所示：

  

  加速度传感器采集数据容易失真，造成姿态解算出来的欧拉角错误。只用角度单环的情况下，系统很难稳定运行，因此可以加入角速度作为内环。角速度由陀螺仪采集数据输出，采集值一般不存在受外界影响情况，抗干扰能力强，并且角速度变化灵敏，当受外界干扰时回复迅速增强了系统的鲁棒性。

  

  如上图所示，把角度作为外环，角速度作为内环，即通过将角度环的输出值作为角速度环的输入来进行姿态双环PID控制。

• 主循环-50Hz循环

  这个循环每20ms执行一次，其任务主要是解析通过无线通信发送过来的控制指令，并结合当前四轴飞行器的姿态，更新飞控核心程序，使得其可以按照指令进行移动。

• 主循环-10Hz循环

  这个循环会以每100ms的时间间隔执行一次，它的任务主要是当四轴飞行器出现异常情况时，可以控制飞行器平稳地降落。异常情况包括电池电量过低、飞行高度超出范围以及无线通信失败等。

算法讲解

• 姿态解算简介

  姿态解算也叫做姿态分析，姿态估计或姿态融合，它是指根据采集到的IMU数据（陀螺仪、加速度计、磁力计等）来求解得到四轴飞行器的空中姿态，姿态解算分为快速解算和深度解算两种。

  如果根据陀螺仪的三轴角速度对时间的积分得到了四轴飞行器的俯仰、翻滚和航向角，这是快速解算。快速解算得到的姿态是存在误差的，而且误差会伴随积分而累加，如果再结合三轴地磁和三轴加速度数据进行漂移补偿和校正，得到准确的姿态，这就是深度解算。不过由于通过快速解算得到的姿态误差往往较大，因此目前姿态解算通常指的就是深度解算。

• 姿态数学表示

  数学模型

  姿态是用来描述一个刚体的固连坐标系和参考坐标系之间的角位置关系。四轴飞行器使用的参考坐标系是当地水平坐标系，即地理坐标系，而其自身的固连坐标系叫做载体坐标系。地理坐标系有很多种，如下图所示，这里使用的是比较常用的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}$满足右手法则。

  表示方式

  • 欧拉角

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

    

    地理坐标系$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（万向锁）问题，因此欧拉角解算只适用于水平姿态变化不大的情况，而不适用于全姿态飞行器的姿态确定。

  • 四元数

    四元数是由爱尔兰数学家威廉·卢云·哈密顿在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个轴的陀螺仪角速度进行积分，得到3个方向上的旋转角度，也就是姿态数据。不过由于误差以及噪声的存在，对陀螺仪的积分并不能够得到完全准确的姿态，尤其是运转一段时间以后，积分误差的累加会让得到的姿态和实际的相差甚远。以下几种原因会导致陀螺仪得到的姿态结果不准确：

  • 零点漂移

    零点漂移指的是陀螺仪在零点处有误差，从而导致随着积分时间的增长，测量值会偏离真实值越来越远。因此，陀螺仪只有在短时间内才有较大的参考价值。

  • 白噪声

    电信号的测量中，一定会带有白噪声，陀螺仪数据的测量也不例外。所以获得的陀螺仪数据中也会带有白噪声，而且这种白噪声会随着积分而累加。

  • 温度/加速度

    陀螺仪是一个温度和加速度敏感的元器件。例如对于加速度，多轴飞行器中的马达一般会带来较强烈的振动，一旦减震控制不好，就会在飞行过程中产生很大的加速度，必会带来陀螺输出的变化，引入误差。

  • 积分误差

    对陀螺仪角速度的积分是离散的，长时间的积分会出现漂移的情况。所以要考虑积分误差的问题。

  由于陀螺仪测量姿态存在这么多的误差，所以我们必须要使用其它传感器辅助校正，其中最重要的就是下面要讲的加速度计。

  加速度计

  加速度计的低频特性好，可以测量低速的静态加速度。对于四轴飞行器来说，3轴加速度计输出重力加速度g（也就是前面说的静态加速度）在加速度计所在机体坐标系3个轴上的分量大小，由于重力加速度的方向和大小是固定的，所以通过这种关系，就可以得到加速度计所在平面与地面的角度关系。当然，加速度计若是绕着重力加速度的轴转动，则测量值不会改变，也就是说加速度计无法感知这种水平旋转。

  通过使用加速度计并结合陀螺仪的数据来做融合，可以矫正姿态的偏差，从而获取系统的最优解。但是由于加速度计无法测量出航向角，所以只用陀螺仪和加速度计的飞行器会存在航向漂移的情况。解决这个问题的方法是再使用一个磁力计传感器来测出当前飞行器的地磁角来修正漂移，不过由于飞行器自身装备有电机等对磁场影响较大的设备，因此由磁力计测出的地磁角往往是不准确的。目前，Breeze微型四轴飞行器未涉及与磁力计相关的内容。

• 姿态解算算法

  解算流程

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

  

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

  解算算法

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

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

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

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

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

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

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

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

• 姿态控制算法

  控制原理

  四轴飞行器的旋翼与空气发生相对运动，产生了向上的升力，当升力大于四轴的重力时四轴飞行器就可以飞起来了。理想情况下，只要四个电机的转速是完全相同，就可以使四轴飞行器在飞行过程中保持水平，但由于电机和旋翼本身制造上的差异，想要控制四个电机达到相同的转速是不可能的。因此，为了防止四轴飞行器发生侧翻的情况，需要使用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. 计算角度误差

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

  2. 计算比例项

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

  3. 计算积分项

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

  4. 计算微分项

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

  5. 整合结果输出

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

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

  串级PID控制

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

  

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

  1. 计算角度误差

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

  2. 计算外环比例项

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

  3. 计算外环积分项

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

  4. 整合外环输出

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

  5. 计算角速度误差

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

  6. 计算内环比例项

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

  7. 计算内环积分项

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

  8. 计算内环微分项

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

  9. 整合内环结果输出

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

  电机输出

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

• 自主悬停

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

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

  

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

总结

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

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