基于STM32 的室内四旋翼飞行器的设计与实现

张锦瑞，赵华磊，李试森，吴星华，马益清

（阳光学院人工智能学院，福州350015）

0 引言

四旋翼飞行器具有可垂直升降、成本低、结构简单等特点。其可应用于农药喷洒、环境探测、高空拍摄、探索未知环境等功能。因此四旋翼飞行器的研究是近年来的热点。当前有不少的团队对四旋翼飞行器进行了相关的研究。其中王伟团队设计基于微小飞行器姿态控制系统成本高，控制性能不稳定的特点，提出了采用廉价的角速度率陀螺仪和加速度计并结合姿态角模型，利用卡尔曼滤波器推测姿态角信息。来解决微小飞行器控制系统成本搞得问题。但该方案一般只能作为新手的试炼使用。因为其采集精度和频率够不高，会产生较大的温漂和零漂。且其使用寿命周期短。王皓毅的团队设计基于STM32F103C8T6 为核心采用MPU6050 传感器设计飞行控制板，完成四旋翼飞行器控制软硬的设计与开发，实现四旋翼左右、上下、前后的飞行要求。但该方案采用的MPU6050 传感器相比于ICM20602 传感器使得四旋翼飞行器噪音较大，无法适用于一些场合。 且STM32F103C8T6 相比于STM32F103RCT6 性能较差。针对以上情况，团队提出应用STM32F103RCT6 微控制器，采用ICM20602 传感器配合外设设施实现四旋翼飞行器的飞行和后期各种外设功能扩展，它能实现成品无人机，无法实现的扩展功能，并相比于大疆、小米等品牌四旋翼飞行器，成品机没有开源，无法二次开发。应用STM32F103RCT6 微控制器的四旋翼飞行器，拥有更低的价格并且可以进行二次开发。

1 系统总体设计

整个四轴飞行器系统由七大模块组成，分别为STM32F103RCT6 控制器、电源模块、位置传感器模块（光流、OpenMV）、超声波模块、ICM20602 模块、遥控接收模块和电机。四翼飞行器主要通过ICM20602 和磁力计获取四轴飞行器的此时的倾仰角、翻滚角、偏航角等参数，融合位置传感器和超声波的数据来辅助ICM20602 进行位置估计，最后通过实时获取OpenMV对图像进行二值化线性回归计算黑线的角度和位置来进行巡线任务等。如图1 所示。

图1 系统总体框图

2 系统硬件设计

2.1 微控制器的设计

该四轴飞行器选择STM32 作为核心控制模块。STM32F103RCT6 是以ARM Cortex-M3 处理器为内核设计的。它提供出色的计算性能和卓越的系统中断响应能力。如图2 所示。

图2 STM32F103RCT6电路配置图

STM32 控制器包含八个完全可编程的16C550 型UART。UART 能够根据RX、TX、调制解调器流量控制、和错误条件产生独立可屏蔽的中断。如果任何中断发生并且未被屏蔽，那么模块将生成一个组合的中断。并且通过I2C 总线获取ICM20602 和IST8310 中的九轴数据再通过内部算法进行姿态解算。

2.2 电源模块设计

该四轴飞行器选用AMS1117 降压芯片来给MCU和板载传感器供电，AMS1117 降压芯片最大输出电流为1A，完全足够供电，并且在电源输入端我们还加装了两个分压电阻来对电池电压进行采集，以保证无人机工作在正常的电量下。电路分别如图3 所示。

图3 电源电路图

2.3 无刷电机模块设计

该四轴飞行器选择无刷电机来为其提供飞行动力。其无刷电机的型号是KV1400 朗宇A2212。如图4 所示。

图4 电机实物图

电机作为四轴飞行器整个驱动系统中最重要的一环，要能为飞行器提供强大的飞行动力，确保四轴飞行器飞行的稳定性。一般无刷电机需要搭配电调使用，因无刷电机的转速主要是通过电调来控制的，所以四轴飞行器选用20A 的电调，最大可以提供20A 的电流来控制电机的转速。该电调原理如图5 所示。

图5 电调原理图

2.4 光流传感器模块

光流就是利用飞行器底部的摄像头通过采集图像数据检测地面的状态，监测飞机的移动，然后通过PID控制算法实现飞行器稳定的定点悬停。这种定位手段配合GPS 可以在室外实现对飞行器的精准控制，并且在室内没有GPS 信号的时候，也能对飞行器实现高精度的定位，实现更加稳定的控制。光流名字很专业，但道理很简单，光流就是在这个世界中明显感觉到的视觉运动。例如，坐在动车上，看外面发现树木、物体在往后运动，这就是光流。

2.5 OpenMV模块

在视觉模块上，团队采用OpenMV4 作为主要视觉传感器。OpenMV4 可以满足人脸识别、颜色追踪、录制视频等额外扩展等功能。OpenMV 是一个开源、低成本、功能强大的机器视觉模块。以STM32F427MCU为核心，集成了OV7725 摄像头芯片，在小巧的硬件模块上，用C 语言高效地实现了核心机器视觉算法，提供Python 编程接口可以用Python 语言使用OpenMV 提供的机器视觉功能，使用其内部集成的算法，对所示图像进行图像滤波、二值化、边缘检测等，通过不同的方式运用这些算法，可以提取出需要的图像细节，然后上传飞控交由飞控决策，例如说通过对图像的二值化提取出图像黑线，然后对黑线进行线性回归，就能实现无人机的巡线。

3 系统软件设计

3.1 总体程序设计

软件设计中主要通过姿态计算和PID 控制算法，以实现对整个系统的稳定性。系统再接收遥控信号解出姿态的同时读取自身携带姿态传感器的数据并计算出飞行姿态。进行数据融合后。PID 控制器利用差值来计算出合适的电机输出量，最终实现四轴飞行器的稳定飞行，再配合OpenMV 可以实现循迹功能。整个控制系统的流程图如图6 所示。

3.2 姿态解算

姿态其实就是指飞行器的翻滚、倾仰以及航向情况。姿态解算就是指利用自身携带的传感器来检测飞行器飞行数据，如线性加速度、角速度、磁场强度等，然后计算得出飞行器的飞行姿态的信息，利用PID 控制器计算出多个电机的输出，使四轴飞行器飞行稳定。

3.3 坐标系转换

因为飞行器在飞行过程中它的姿态是随意变化的，所以必须建立坐标系来对姿态进行解算。坐标系可以分为N 系参考坐标和B 系载体坐标。

图6 控制系统流程图

N 系坐标和B 系坐标之间任何角度的关系都可看成有限的基本旋转组合，绕X 轴旋转，绕Y 轴旋转，绕Z 轴旋转，得出的基本旋转矩阵分别为：

由旋转矩阵求欧拉角。设三个轴x,y,z 的欧拉角分别为θx,θy,θz，正弦值、余弦值分别为sx,cx,sy,cy,sz,cz 那么旋转矩阵为：

解方程可得：

（1）四元数求姿态角

四元数应用在姿态解算中，配合PID 算法让飞行稳定，单位向量（x,y,z）旋转θ角度后的四元数：其中R代表四元素旋转矩阵，θ表示俯仰角。

可以转化成矩阵的形式：

通过式4 和式7 比较，可以得出姿态角：

（2）PID 控制器

PID 控制器结构图如图7 所示。

图7 PID控制器结构图

括号内第一项是比例项，第二项是积分项，第三项是微分项。其输入e（t）与输出u（t）的关系为：u（t）=kp（e（t）+1/TI∫e（t）dt+TD*de（t）/dt）式中积分的上下限分别是0 和t。因此传递函数为：G（s）=U（s）/E（s）=kp（1+1/（TI*s）+TD*s），其中kp 为比例系数；TI 为积分时间常数；TD 为微分时间常数。

（3）梯度下降算法

梯度下降法其实是一种迭代求极值的方法在数据融合时，融合姿态角加速度等数据得到最佳姿态角。公式的实现流程图如图8 所示。

图8

3.4 算法实现

飞行器通过PID 控制器计算出电机输出量，最终是以PWM 占空比形式输出进而驱动电机。

y（k）=y（k-1）+△y（增量），△y 是pwm 输出变化量，y（k-1）是上一次输出量，y（k）这次输出量。具体过程如图9 所示。

图9 电机动力分配图

在电机输出计算中，需要用到油门信息量和偏航信息量。油门信息主要通过对遥控命令来实现。其中横滚量参与左右电机输出量的计算，俯仰量参与前后电机输出量的计算。偏航信息、横滚量以及俯仰量通过PID 计算获取。利用主控芯片驱动OpenMV 从而实现对物体的识别与循迹。

4 实验结果分析

4.1 软硬件联调

在完成飞行器的硬件设计和软件设计之后，进行了飞行器的拼装和飞行测试。主要测试了其飞行器的基本功能并且初步确立了飞行器的各项控制参数。最佳的控制参数需要进一步的研究，需要进行反复的实验与调试，还需对飞行器姿态融合算法与PID 控制参数进行更细微的调整。

该四轴飞行器在如下条件和环境中进行了测试：

（1）采用4200mah 25c 3s 锂电池对系统进行供电；

（2）在室内进行简单的飞行运动，完成升降、定高悬停等基本动作；

（3）配合OpenMV 完成对物体的识别以及寻黑线的功能；飞行器实物如图10 所示。

图10 飞行器飞行图

通过初步试飞实验表明，该飞行器能够完成基本的飞行动作，但是飞行会晃动，通过调节PID 来提高稳定性，提高P 和I 的值，飞行器趋于稳定。重新整定PID 的参数，飞行器基本的飞行动作都能很好地完成，飞行器能很好地平稳飞行配合OpenMV 四旋翼飞行器实现了物体识别、寻黑线等功能。

4.2 数据融合分析

数据融合主要包括了传感器的数据处理和梯度下降算法。因为信号传输时和信号测量时都会产生巨大的干扰，同时也存在着外部磁干扰，所以需要对传感器的数据进行处理。这样也可以消除各芯片的静态误差。梯度下降算法的作用主要是为了三轴陀螺仪解算姿态角，利用磁力计和加速度计来补偿陀螺仪积分的漂移误差量。因为飞行器携带的传感器都为三轴，所以选取其中单个轴向进行分析。在飞行器传感器中，加速度计的初始数据非常最要。数据处理前后的对例如图11 所示。

图11 加速度计数据图

由图11 可知，要消除静态误差，必须对加速度计进行数据处理。以减少数据的高频噪声同时实现其数据的低通滤波功能。

ICM20602 姿态传感器中加速度计可以对横滚角和俯仰角进行修正，而磁力计可以对偏航角进行修正。因为数据融合主要围绕消除陀螺仪数据的积分误差展开，所以为了减少信号在传输过程中的干扰提高姿态数据的准确性，需对陀螺仪进行数据处理来消除积分误差。

虽然陀螺仪数据可以直接进行姿态角解算，但是如果只用陀螺仪进行姿态解算，姿态角的漂移误差会很大，得不到准确的姿态角，进行数据融合后，可以有效减少姿态角的漂移误差，其效果非常明显。如图12为数据融合前后的波形对比。

图12 加速度计修正图

由图12 可知，融合后的数据减小了漂移倾向，波形更加平稳。不仅降低了姿态角的漂移误差，也增强了系统的抗干扰能力。

偏航角的修正主要通过磁力计来实现，如图13所示。

图13 磁力计计修正图

由图13 可知，融合前的偏航数据存在明显的角度漂移且噪声较大。经梯度下降算法融合后都得到了很好的控制。如图14 为融合后的完整姿态数据波形。

4.3 总结

通过初步试飞实验，进行融合数据分析和调节PID 算法，能进一步提高四旋翼飞行器的飞行稳定性，应用STM32F103RCT6 控制器，配合外设设备可以实现四旋翼飞行器的实地飞行，后期在配合外设OpenMV机器视觉模块可以进行物体识别、巡线等功能。本作品参加2019 年全国大学生电子设计竞赛，最终获得全国二等奖。

图14 姿态角数据波形图