基于APM的模块化飞行控制器的设计

Design of Modular Flight Controller Based on APM

摘要: 本文以开源的APM飞控板的硬件原理、固件原理为基础，模块化搭建和研究飞行控制系统。文章对四旋翼无人机飞行控制系统的架构进行分析，硬件部分，采用模块化单元设计，替换更改部分元器件和接口，主要由32位的STM32F4处理器芯片，替换APM的Mega2560、Mega32u2的8位双处理器芯片，替换部分传感器，删除或添加了部分外部接口；软件部分，底层的驱动库源码程序，采用父子类的形式编写源码，实现源码的模块化，方便后期的软件升级和维护。该系统方案具实用性高、后期开发方便、经济性好等优点。

Abstract: Based on the hardware principle and firmware principle of the open source APM flight control board, this paper constructs and studies the flight control system in a modular manner. The article analyzes the architecture of the quad-rotor UAV control system. The hardware part adopts modular unit design and replaces and changes some components. The 32-bit STM32F4 processor chip is mainly used to replace the 8-bit dual processing of APM’s Mega2560 and Mega32u2. The chip is replaced with some sensors, and some external interfaces have been deleted or added; the software part, the underlying driver library source code file, uses the parent-child class to write the source code, realizes the modularization of the source code, and facilitates later software upgrades and maintenance. The system scheme has the advantages of high reliability and high cost performance.

1. 引言

随着科技的不断发展，无人机在各个行业的应用也越来越广泛。飞行控制系统是无人机的核心控制系统，飞控板是飞行控制系统的硬件载体，目前市面上已出现多种类型的飞控板，如：Pixhawk、MWC、KK等。这些飞控板存在一些问题，硬件电路都采用了多层PCB板组成集成电路，电路复杂高度集成，不利于后期单元模块的更换和升级；软件源码的编译大部分采用Linux操作系统下的GCC工具链编译，编译环境难搭建，源码思路复杂层次不够清晰。本文探索研究的是在Windows操作系统下使用MDK开发环境编译源码，硬件电路采用模块化搭建设计，软件源码采用C++语言，以创建父子类的形式模块化编写。解决开发环境难搭建的问题，同时也有硬件电路后期更换升级方便，软件源码维护升级方便，后期软硬件开发升级维护简单等优点。

本文是基于开源APM飞行控制系统的硬件和软件为基础，针对四旋翼无人机飞行控制系统的硬件和软件进行模块化设计，以供参考 [1]。

2. 四旋翼无人机简介

无人机是一种由电动力驱动、无人驾驶、可重复使用的航空器的简称 [2]。无人机在军用领域和民用领域都有广泛的应用 [3]，军用领域可用于：跟踪定位、特种作战、精确制导等；民用领域可用于：气象监测、电力巡检、快递送货等。无人机按结构有多种分类：固定翼无人机、多旋翼无人机、无人直升机等多种形式。

四旋翼无人机，是一种由电动力驱动，可垂直起降的飞行器，具有结构简单、稳定性好，对起降场地要求低等优点，得到了广泛的应运 [4]。四旋翼无人机在结构上有“十”型结构和“X”型结构，“X”型结构较为常见，本文以“X”型结构的四旋翼无人机为载体研究模块化飞行控制系统，四旋翼无人机具有体积小、重量轻、携带方便、能轻易进入人不易进入的各种恶劣环境等优点 [5]，在民用领域和军用领域都有极广泛的应用。

四旋翼无人机由机械结构系统、动力系统、控制系统三大系统组成。机械结构系统主要由机架组成，用于连接支撑无刷电机、飞控板、动力电池等器件；动力系统主要由动力电池、电子调速器、直流无刷电机组成；控制系统主要由飞控板、GPS模块、无线数传模块等器件组成。本文是以“X”型结构的四旋翼无人机为载体，研究模块化飞行控制系统，以供参考。

3. 四旋翼无人机工作原理

如图1所示，“X”型结构四旋翼无人机。四旋翼无人机一般采用4个正反桨的机械结构，4个无刷直流电机作为其核心的动力源，M1、M3电机顺时针旋转，M2、M4电机逆时针旋转，用于抵消四旋翼无人机的陀螺效应。

Figure 1. “X” type four-rotor UAV structure diagram

图1. “X”型四旋翼无人机结构图

四旋翼无人机的控制系统是欠驱动控制系统 [6]，由垂直、滚转、俯仰、偏航4个输入，向上、向下、向前、向后、向左、向右6个输出。根据四旋翼无人机特殊的结构模式，有四种飞行控制模式：垂直控制、滚转控制、俯仰控制、偏航控制。

1) 垂直控制。垂直控制用于四旋翼无人机的上升、下降、悬停飞行，如图1所示，在Z轴方向向，同时增加M1、M2、M3、M4电机的转速，4个旋翼产生升力的合力大于四旋翼无人机的重力时，实现上升运动；同时减小M1、M2、M3、M4电机的转速，4个旋翼产生升力的合力小于四旋翼无人机的重力时，实现下降运动；在M1、M2、M3、M4电机转速在某一值时，4个旋翼产生升力的合力等于四旋翼无人机的重力时，并且此时的俯仰、偏航、滚转三个姿态角为零，实现悬停状态。

如图2所示，由牛顿第二定律知：

Figure 2. Vertical control

图2. 垂直控制

${\displaystyle \sum F}=m\ddot{z}=F_1+F_2+F_3+F_4-mg$

$\ddot{z}=\frac{F_1+F_2+F_3+F_4-mg}{m}$

其中：F为合外力，F₁-F₄为电机M1-M4产生的向上升力，m为无人机的总质量， $\ddot{z}$ 为轴方向的加速度。

如上所示：当 $\ddot{z}>0$ 时，上升运动；当 $\ddot{z}<0$ 时，下降运动；当 $\ddot{z}=0$ 时，悬停运动。

2) 滚转控制。滚转控制用于四旋翼无人机的向左、向右飞行，如图1所示，同时增大M1、M4电机的转速，减少M2、M3电机的转速，左边的升力大于右边的升力，无人机向右倾斜，产生向右的分力，无人机向右飞行，分力越大，向右飞行的速度越快；同时减小大M1、M4电机的转速，增大M2、M3电机的转速，右边的升力大于左边的升力，无人机向左倾斜，产生向左的分力，无人机向左飞行，分力越大，向左飞行的速度越快。

如图3所示：

${\displaystyle \sum M}=I_x\ddot{\alpha }=L\left(F_{23}-F_{14}\right)$

$\ddot{\alpha }=\frac{L\left(F_{23}-F_{14}\right)}{I_x}$

其中：M为合外力矩，F14为电机M1、M4产生的向上合升力，F23为电机M2、M3产生的向上合升力，I_x为无人机X轴上的转动惯量， $\ddot{\alpha }$ 为X轴方向的角加速度。

Figure 3. Roll control

图3. 滚转控制

如上所示：当 $\ddot{\alpha }>0$ 时，向左加速翻转运动；当 $\ddot{\alpha }<0$ 时，向右加速翻转运动。对 $\ddot{\alpha }$ 一次积分运算，可得旋转角速度，对 $\ddot{\alpha }$ 二次积分运算，可得旋转角度，既滚转角。

3) 俯仰控制。俯仰控制用于四旋翼无人机的向前、向后飞行，如图1所示，同时增大M3、M4电机的转速，减少M1、M2电机的转速，后边的升力大于前边的升力，无人机向前倾斜，产生向前的分力，无人机向前飞行，分力越大，向前飞行的速度越快；同时减小大M3、M4电机的转速，增大M1、M2电机的转速，前边的升力大于后边的升力，无人机向后倾斜，产生向后的分力，无人机向后飞行，分力越大，向后飞行的速度越快。

如图4所示：

${\displaystyle \sum M}=I_y\ddot{\beta }=L\left(F_{12}-F_{34}\right)$

$\ddot{\beta }=\frac{L\left(F_{12}-F_{34}\right)}{I_y}$

其中：M为合外力矩，F₁₂为电机M1、M2产生的向上合升力，F₃₄为电机M3、M4产生的向上合升力， $I_y$ 为无人机Y轴上的转动惯量， $\ddot{\beta }$ 为Y轴方向的角加速度。

Figure 4. Pitch control

图4. 俯仰控制

如上所示：当 $\ddot{\beta }>0$ 时，向前加速翻转运动；当 $\ddot{\beta }<0$ 时，向后加速翻转运动。对 $\ddot{\beta }$ 一次积分运算，可得旋转角速度，对 $\ddot{\beta }$ 二次积分运算，可得旋转角度，既俯仰角。

4) 偏航控制。偏航控制用于四旋翼无人机在滚转、俯仰角度为零时，控制无人机向左、向右转向控制，如图1所示，M1、M3与M2、M4电机转速相同、方向相反，目的是克服自身的牛扭矩力，当M1、M3与M2、M4电机转速不相同，会产生一定的扭矩力，从而对四旋翼无人机的偏航进行控制。当同增大M1、M3的转速，减少M2、M4的转速时，四旋翼无人机向左偏转，同减少M1、M3的转速，增大M2、M4的转速时，四旋翼无人机向右偏转。

如图5所示：

Figure 5. Yaw control

图5. 偏航控制

${\displaystyle \sum M}=I_z\ddot{\gamma }=M_1-M_2+M_3-M_4$

$M_1=H{F}_1$

$M_2=H{F}_2$

$M_3=H{F}_3$

$M_4=H{F}_4$

$\ddot{\gamma }=\frac{H\left(F_1-F_2+F_3-F_4\right)}{I_z}$

其中：M为合外力矩，F₁~F₄为电机M1~M4产生的向上合升力，I_z为无人机Z轴上的转动惯量， $\ddot{\gamma }$ 为绕Z轴方向的角加速度，H为电机到机体中心的距离。

如上所示：当 $\ddot{\gamma }>0$ 时，机体逆时针加速旋转运动；当 $\ddot{\beta }<0$ 时，机体顺时针加速旋转运动。对 $\ddot{\gamma }$ 一次积分运算，可得旋转角速度，对 $\ddot{\gamma }$ 二次积分运算，可得旋转角度，既偏航角。

4. 飞行控制系统的硬件模块化设计

飞行控制系统是四旋翼无人机的控制核心系统，飞行控制系统的好坏，决定了四旋翼无人机的飞行品质。

本文以开源的APM飞行控制系统的硬件原理图为基础，根据实际工程的需要，并考虑因素，删除或添加部分硬件电路或接口。本文设计搭建的模块化飞行控制系统与APM飞行控制系统，在硬件方面具有较多的创新点：

1) 采用32位ARM结构的STM32F407做为主控芯片，替换APM原有的8位Mega2560、Mega32u2双处理器，在处理性能、功耗和经济性有很大的优势。

2) 采用SPI通讯的双MPU6500传感器，代替APM原有的IIC通讯的MPU6050，在数据采集速度有较大的提升，双传感器的方式也提高了数据精度和可靠性。

3) 采用低端的BMP180气压计代替APM原有的MS5611气压计，可以较大程度上节约经济成本。

4) PWM信号读的方式，本文保留了APM的PPM信号解析读取的方式读取各路PWM信号，取消了APM直接读取各路PWM信号的方式读取PWM信号，大量节约了电气连线。

5) PWM输出，本文采用的是通过IIC通讯的16路PWM模块输出PWM信号，代替APM通过主控芯片的引脚直接输出PWM信号的方式，节约了电气连线和自控芯片的引脚接口，也便于后期扩展PWM信号通道。

6) 增加OLED显示屏，用与显示飞行控制系统的相关信息状态，代替了APM通过不同的LED状态显示的方式，读取信息状态更加直观明了。

7) 所有传感器单元模块、GPS、数传电台等器件采用杜邦线与主控板连接，代替APM高度集成的PCB板，便于后期器件更换和升级。

本文模块化设计搭建的硬件结构图如图6所示，其中：

1) STM32F407处理器模块，用于读取传感器数据、接收机信号、地面站信号，做姿态解算、位置解算、处理数据等，通过16路PWM模块输出PWM信号，控制电机转速实现不同的飞行姿态。

2) EEPROM存储器模块，采用AT24C256模块，32KB存储容量，模拟APM处理器Mega2560中的EEPROM，IIC的通讯协议，存储程序源码中的一些静态数据、传感器校正值、航点数据等数据。

3) IMU惯性测量单元，包含两个MPU6500模块，一个HMC5883L模块。通过读取加速度值、角速度值、磁场强度值，做姿态解算，用于得出飞行姿态角。

4) BMP180模块。气压计传感器，计算出当前四旋翼无人机的飞行高度，做微分运算可得上升下降速度，用于位置解算。

5) GPS模块，采用的U-BLOXNEO-6M模组，串口的通讯协议，用于获取当前的经纬度、海拔高度、航速等信息，用于位置解算。

Figure 6. Modular hardware structure diagram

图6. 模块化的硬件结构图

6) 接收机模块，主控芯片以解析接收机PPM信号的方式读取各个通道的PWM信号，用于手动模式下控制四旋翼无人机飞行的姿态和飞行模式切换。

7) 16路PWM模块，IIC的通讯协议，用于输出4路PWM信号至电子调速器，控制电机转速。

8) OLED显示屏，显示飞行控制性的相关信息状态，如故障信息等。

5. 飞行控制系统的控制流程

算法部分，是飞行控制系统的核心部分，主要有：整体控制流程、姿态解算流程、位置解算流程、姿态控制流程、PID控制器。

a) 整体控制流程

整体控制流程如图7所示，是四旋翼无人机飞行控制器的核心算法部分，主要包括：姿态控制、位置控制两大算法。加速度计、磁罗盘、陀螺仪实时采集9轴数据，通过姿态解算，实时计算四旋翼无人机当前的姿态角，作为闭环控制的反馈部分。GPS模块单元，实时采集四旋翼无人机当前的位置信息，与地面站输入的航点位置作比较，通过位置控制器实时计算目标姿态角，并与当前的姿态角做对比，通过电机混控输出不同数值的PWM信号至电子调速器，进而控制四旋翼无人机的输出电机转速，控制四旋翼无人机的飞行姿态。

Figure 7. Overall control flow chart

图7. 整体控制流程图

b) 姿态解算流程

姿态解算流程如图8所示，加速度计易受机体震动影响，磁力计易受机体周围金属物体影响，陀螺仪易受温度变化产生积分漂移。同时，机体坐标系(北东地坐标系)下，加速度计测量Z轴数据误差较大，磁罗盘测量Y轴数据误差较大，因此，本文读取3轴角速度的采用的方法是：首先通过一些简单算法直接读取加速度计、磁力计的姿态角，做均值滤波；然后通过PD控制器后计算三轴角速度；最后再与陀螺仪所读出的3轴角速度做权值滤波，以便提高3轴角速度准确性和可靠性。最后通过DCM矩阵，解算四旋翼无人机的姿态角。

Figure 8. Posture calculation flowchart

图8. 姿态解算流程图

c) 位置解算流程

位置解算流程如图9所示，期望位置由地面站输入设置，当前位置由机载GPS模块单元实时测量，位置误差作为输入信号，通过位置控制器计算期望飞行速度，期望速度与四旋翼无人机实时测量速度的数值误差作为速度控制器的输入信号，计算期望姿态角。

Figure 9. Position calculation flowchart

图9. 位置解算流程图

d) 姿态控制流程

姿态控制流程如图10所示，期望姿态角由位置解算流程得出，测量姿态角由姿态解算流程得出，其误差数值作为角速度控制器的输入信号，计算四旋翼无人机的期望角速度，期望角速度与传感器测量的角速度做对比，通过算法计算PWM信号数值，通过电子调速器控制电气转速，进而控制四旋翼无人机飞行姿态。

e) PID控制器

PID控制器如图11所示，是实际工程中最为常见的控制器，它具有结构简单，参数调整方便等优势得到了广泛的应用。PID控制器包括三大部分：比例控制器、积分控制器、微分控制器，是根据系统的输入和输出相比较的误差，利用比例、积分、微分计算出控制量进行控制的 [7]。在实际应用中，PID控制器也包括PID控制器、PI控制器、PD控制器，其中PID控制器是控制效果最为理想的控制器，既有比例P的响应速度，又有积分I的消除静态误差，还有微分D的超前调节控制。因此，在飞行控制系统中有大量的运用，主要用在四旋翼无人机的姿态控制和位置控制中。

Figure 10. Attitude control flow chart

图10. 姿态控制流程图

Figure 11. PID controller

图11. PID控制器

6. 飞行控制系统的源码模块化设计

本文以开源的APM飞行控制系统的源码为基础，根据所采用的硬件模块和接口，删减或添加相关源码程序。本文采用编译方式、源码编写思路与开源的APM源码有很多的优势之处：

1) 本文是在Window系统下MDK开发环境下编译源码程序，替换APM原有的在Linux系统下GCC工具链编译的方式，Windows系统下的界面化编译环境，具有开发环境易搭建、编译调试程序方便等优点，便于后期源码开发。

2) 以STM32F407的标准库为工程模板，二次分装部分标准库函数接口，便于移植APM源码。传感器模块的源码程序采用父子类的形式编写，实现源码模块化，父类中创建应用层所调用的函数接口，子类集成父类函数接口，针对不同型号的传感器编写相对应的驱动程序，后期更新升级传感器模块，只需更新编写子类驱动源码，便于后期源码升级维护。

7. 实验结果

如图12~15所示，分别为：垂直控制、俯仰控制、滚转控制、偏航控制结果。

Figure 12. Vertical control

图12. 垂直控制

Figure 13. Pitch control

图13. 俯仰控制

Figure 14. Roll control

图14. 滚转控制

Figure 15. Yaw control

图15. 偏航控制

8. 结束语

本文研究的是在MDK编译环境下，基于APM飞行控制系统的模块化研发，与APM飞行控制系统相比，具有很多的优势。在编译环境上，具有开发环境易搭建、成本低、效率高、可视化界面、易于上手等优势；在硬件上，各个功能模块采用的是模块化设计，后期升级替换模块，仅需要针对性替换升级即可，在经济上具有很多的优势；在软件源码上，底层的驱动源码由于是采用的父子类的形式，后期替换模块，仅需要在子类中对模块实现即可，而不需要修改上层应用层源码，源码思路清晰，开发周期短，具有很大的经济效益。