STM32四旋翼无人机飞控系统开发：从姿态解算到PID控制

简介：一套基于STM32单片机的四轴无人机控制系统完整代码包，面向嵌入式开发学习者、无人机爱好者、电子设计竞赛队伍及本科毕业设计人群。方案覆盖硬件结构搭建、系统建模、硬件模块设计、传感器数据采集、姿态检测融合算法、控制算法设计以及环境下的程序编写与调试，完整度较高。资源共400个文件，压缩包约10.91MB，主体为105个C源文件和80个头文件，另含汇编启动文件、工程配置文件、编译中间文件以及少量调试辅助文件，可直接导入工程查看或重新编译。目前已有999人浏览学习，说明该飞控方案在同类资源中具有一定参考热度。通过代码可了解姿态融合与PID调参的具体实现，结合设计任务分解，能帮助读者梳理无人机系统从传感器读取到电机控制的完整软件链路；针对干扰下的稳定性、快速性与准确性分析，也为相关课题研究和竞赛调试提供了实用思路。

1. 项目整体设计与方案选型

用 STM32 做无人机控制系统，是很多入门飞控开发的人都会经历的一步。哪怕现在市面上有 PX4、Betaflight 这类成熟开源飞控，STM32 依然是理解无人机底层原理的最佳选择：传感器怎么读、姿态怎么算、电机怎么转、遥控信号怎么解析，这些核心逻辑在 STM32 上都能看得清清楚楚。

这个项目的定位很明确：用一块主控芯片（STM32F4 或 F1 系列）从零搭建一套可运行的四旋翼无人机控制系统，包含姿态解算、PID 控制、电机输出、遥控解析等完整代码框架。它适合两类人：一类是准备做毕业设计或电赛的学生，另一类是想深入理解飞控原理、不想只当开源代码“调参侠”的嵌入式开发者。

1.1 为什么用 STM32 做无人机主控

无人机主控本质上是一个实时控制系统：它必须以固定频率读取传感器数据，计算姿态偏差，然后输出 PWM 控制电机转速。这个“传感器读取—姿态解算—PID 计算—PWM 输出”的闭环周期通常在 1ms 到 4ms 之间，对 MCU 的实时计算能力、中断响应速度和外设资源都有明确要求。

STM32 在这一层面的优势非常突出：

• 主频够用：F103 的 72MHz 已经能流畅跑通姿态解算和 PID 控制，F405/F427 的 168MHz 更是可以直接上 FreeRTOS 做任务调度，不会出现计算资源不足的窘境。
• 硬件外设齐全：I2C/SPI 接口接 IMU 传感器，定时器输出多路 PWM 驱动电调，USART 接遥控接收机和地面站，ADC 采集电池电压，这些需求 STM32 全都能满足。
• 生态成熟：无论是寄存器操作还是 HAL 库、标准库，网上都有海量参考代码，遇到问题也容易找到答案。

值得说明的是，如果你打算做真正能稳定飞行的无人机，传感器选型比 MCU 选型更关键。STM32 只是大脑，而“前庭系统”——也就是陀螺仪和加速度计——决定了大脑能不能准确感知身体姿态。本项目选用 MPU6050 作为姿态传感器，它是六轴（三轴陀螺仪 + 三轴加速度计），价格便宜、资料多，入门完全够用。预算允许的话可以直接上 ICM-20602 或 BMI088，性能和温漂表现更好，代码迁移成本也不高。

1.2 系统架构与硬件选型清单

在做系统设计时，我习惯先把整个无人机的信号链路画出来：遥控器 → 接收机 → STM32（解析 PPM/SBUS 信号，读取 IMU 数据）→ 姿态解算 → PID 控制 → 电机驱动 → 螺旋桨 → 飞行响应。每一个环节都对应具体的硬件选型。

以一套 250mm 轴距的入门四轴为例，硬件清单大致如下：

电机选型上给一个实用公式：整机重量（含电池）大约是单电机最大推力的 4 倍左右，悬停油门约在 50%-60% 区间。如果一架飞机的起飞重量是 800g，那么单电机最大推力建议在 200g-250g 以上，否则悬停油门会过高，PID 余量不足，飞起来会感觉很“肉”甚至失控。

2. 核心代码框架与关键实现

很多人拿到飞控代码的第一反应是“从 main 函数开始读”，但我建议反过来：先看数据流和控制流。飞控代码的核心链路其实非常清晰，想清楚这条链路，代码就只是这个逻辑的具象化表达。

2.1 传感器数据采集与姿态解算

姿态解算是整个飞控最基础也最重要的一环。它的任务是把 IMU 输出的角速度和加速度数据，转换成飞行器的横滚角（Roll）、俯仰角（Pitch）和偏航角（Yaw）。

我用的方案是Mahony 互补滤波算法（四元数解算），它的计算量比 EKF 小很多，在 F103 上也能以 1kHz 频率运行，而且效果足够稳定。核心思路是：陀螺仪短时间精度高但会积分漂移，加速度计长期稳定但有噪声，所以用互补滤波把两者融合，各取所长。

以下是基于 HAL 库的 MPU6050 数据读取和姿态解算核心代码：

// imu.h typedef struct { float roll; // 横滚角 float pitch; // 俯仰角 float yaw; // 偏航角 } attitude_t; extern attitude_t attitude; // imu.c #include "imu.h" #include "mpu6050.h" #include "mahony.h" void IMU_Update(void) { int16_t accel_raw[3], gyro_raw[3]; float accel[3], gyro[3]; // 1. 从 MPU6050 读取原始数据（I2C 或 SPI） MPU6050_ReadAccel(accel_raw); MPU6050_ReadGyro(gyro_raw); // 2. 根据量程转换为物理量 // 加速度计量程默认为 ±8g，陀螺仪量程默认为 ±2000°/s accel[0] = accel_raw[0] * 8.0f / 32768.0f; accel[1] = accel_raw[1] * 8.0f / 32768.0f; accel[2] = accel_raw[2] * 8.0f / 32768.0f; gyro[0] = gyro_raw[0] * 2000.0f / 32768.0f * 0.0174533f; // 转为 rad/s gyro[1] = gyro_raw[1] * 2000.0f / 32768.0f * 0.0174533f; gyro[2] = gyro_raw[2] * 2000.0f / 32768.0f * 0.0174533f; // 3. 输入 Mahony 滤波更新四元数 Mahony_Update(gyro[0], gyro[1], gyro[2], accel[0], accel[1], accel[2]); // 4. 四元数转欧拉角，得到当前姿态 Quaternion_ToEuler(&attitude.roll, &attitude.pitch, &attitude.yaw); }

这里有一个容易被忽略的坑：陀螺仪数据必须转成弧度每秒（rad/s）再进行姿态解算，否则四元数更新的积分步长会出错，导致姿态角严重漂移。上面代码里乘以 0.0174533f 就是做角度转弧度，这个细节我当时调试时折腾了很久才发现。

2.2 姿态控制环：串级 PID 设计

姿态解算解决“飞机现在是什么姿态”的问题，控制环解决“怎么让飞机回到目标姿态”的问题。四旋翼姿态控制通常使用串级 PID：外环是角度环（控制姿态角），内环是角速度环（控制角速度）。

为什么要用串级结构？因为单纯用角度环控制无人机，响应慢且容易震荡；加入角速度内环后，相当于先抑制掉角速度的突变，飞机姿态变化会更平滑。打个比方：角度环是“老板”，负责定目标；角速度环是“执行者”，负责让姿态变化速率符合预期。老板只看方向对不对，执行者管步子迈多大。

角度环和角速度环的 PID 计算代码如下：

// pid.h typedef struct { float kp; float ki; float kd; float integral; float last_error; float output; float integral_limit; // 积分限幅 float output_limit; // 输出限幅 } pid_t; void PID_Init(pid_t *pid, float kp, float ki, float kd, float ilim, float olim); float PID_Update(pid_t *pid, float target, float feedback, float dt); // pid.c float PID_Update(pid_t *pid, float target, float feedback, float dt) { float error = target - feedback; // 比例项 float p_out = pid->kp * error; // 积分项，带限幅防积分饱和 pid->integral += error * dt; if (pid->integral > pid->integral_limit) pid->integral = pid->integral_limit; else if (pid->integral < -pid->integral_limit) pid->integral = -pid->integral_limit; float i_out = pid->ki * pid->integral; // 微分项 float derivative = (error - pid->last_error) / dt; float d_out = pid->kd * derivative; pid->last_error = error; pid->output = p_out + i_out + d_out; // 输出限幅 if (pid->output > pid->output_limit) pid->output = pid->output_limit; else if (pid->output < -pid->output_limit) pid->output = -pid->output_limit; return pid->output; }

串级控制的完整流程是：遥控器或地面站给出目标姿态角（比如 Roll=0°），角度环 PID 根据当前姿态角误差输出目标角速度，角速度环 PID 再根据目标角速度和当前角速度误差输出电机修正量。这个修正量最终和油门基本值叠加，形成四路电机的最终 PWM 占空比。

PID 参数整定最有效的办法是先内环后外环、先 P 后 D 后 I：先把内环的 P 从小往大调，找到飞机“变硬”但不振的临界点；再加入 D 抑制超调；最后用 I 消除稳态误差。外环参数同理，只是外环的 P 值要保守很多，因为外环输出是内环的目标值，太激进会导致角速度超限。

2.3 电机 PWM 输出与油门映射

STM32 输出 PWM 驱动电调，在代码上其实是最简单的一步，但也是炸机风险最高的一步。这里我使用定时器的四路通道输出 50Hz-500Hz 的 PWM 信号，其中 50Hz 传统 PWM 电调居多，500Hz 多用于刷了 BLHeli 固件的电调，能获得更快的响应速度。

// motor.c - 以定时器 TIM1 的四路通道为例 void Motor_Init(void) { TIM_HandleTypeDef htim; // 配置 TIM1 的四路 PWM 通道，PWM 频率设为 500Hz // 这里省略 HAL_TIM_PWM_Init 的具体结构体配置 // 注意电调上电时需要输出油门最低值（1000us）保持几秒，等待电调初始化 } void Motor_Write(int16_t m1, int16_t m2, int16_t m3, int16_t m4) { // 假设 PWM 比较值范围为 1000~2000（对应 1ms~2ms 高电平时间） __HAL_TIM_SET_COMPARE(&htim, TIM_CHANNEL_1, m1); __HAL_TIM_SET_COMPARE(&htim, TIM_CHANNEL_2, m2); __HAL_TIM_SET_COMPARE(&htim, TIM_CHANNEL_3, m3); __HAL_TIM_SET_COMPARE(&htim, TIM_CHANNEL_4, m4); }

油门映射的逻辑是：遥控器油门通道输出往往在 1000~2000 之间，你需要在控制代码里把它归一化到 0~1，再根据 PID 输出加权分配到四个电机。以十字型四轴为例，电机 1 和电机 3 对横滚响应，电机 2 和电机 4 对俯仰响应，四个电机共同影响偏航。分配代码本质上就是“控制量到执行器的混控矩阵”，这部分逻辑不需要很复杂，但方向一定要检查清楚——万一反了，飞机起飞瞬间就会直接翻转。

3. 从裸机到 RTOS：任务调度与实时性优化

很多 STM32 飞控初学者一开始都是在主循环里顺序执行“读传感器→算姿态→跑 PID→更新 PWM”，这种裸机写法在 1kHz 控制频率下勉强可行，但随着功能增加（比如加光流定高、加避障传感器），主循环会越来越臃肿，实时性越来越难保证。

3.1 FreeRTOS 任务划分方案

当控制系统复杂度上来之后，建议直接上 FreeRTOS。我在项目中把飞控任务划分为四个独立任务：控制任务、传感器任务、通信任务、地面站处理任务。其中控制任务优先级最高，因为它直接决定飞行安全；传感器任务次之，负责以固定频率更新姿态数据；通信任务处理遥控信号接收和指令解析。

以下是一个简单的 FreeRTOS 任务创建示例：

// freertos_tasks.c #include "FreeRTOS.h" #include "task.h" void vControlTask(void *argument) { // 控制任务： 1kHz 频率运行 const TickType_t xFrequency = pdMS_TO_TICKS(1); TickType_t xLastWakeTime = xTaskGetTickCount(); for (;;) { // 等待下一个控制周期 vTaskDelayUntil(&xLastWakeTime, xFrequency); IMU_Update(); Attitude_PID_Update(); Motor_Update(); } } void vComTask(void *argument) { // 通信任务： 100Hz 频率解析遥控信号 const TickType_t xFrequency = pdMS_TO_TICKS(10); TickType_t xLastWakeTime = xTaskGetTickCount(); for (;;) { vTaskDelayUntil(&xLastWakeTime, xFrequency); RC_Parse(); // 更新目标姿态角 FlightMode_Update(); } }

这里的关键点在于：控制任务的优先级必须最高，通信任务、地面站任务都不能打断它。你可以用 FreeRTOS 的configMAX_PRIORITIES和任务创建时的优先级参数来控制这一点。

3.2 中断、DMA 与数据同步

使用 RTOS 之后，一个容易踩坑的地方是：传感器数据可能在控制任务读取到一半时，被中断或另一个任务修改，导致数据竞争。MPU6050 读取建议用硬件 I2C/SPI + DMA，减少 CPU 中断占用；同时，传感器数据和姿态数据要定义成全局变量时，最好加上volatile关键字，或者用portENTER_CRITICAL()/portEXIT_CRITICAL()临界区保护关键数据。

另一个实践心得是：遥控信号解析放到中断里做会很容易丢失数据，尤其是 SBUS 这种 100Hz、每帧 25 字节的数据流。建议用空闲中断 + DMA 接收整包数据，再由通信任务解析帧内容，这样能做到不丢帧、不卡控制任务。

4. 常见问题与调试心得

这部分是全文最实在的地方。项目做完之后的调试阶段，花费的时间往往比写代码还多。把我在这个项目里踩过的坑和最终解决方案整理出来，希望帮你少走弯路。

4.1 上电炸机：解锁逻辑与安全保护

我非常不建议在没有任何保护措施的情况下直接上螺旋桨试飞。正确做法是：先拆桨，用手拿着机架，小幅推油门测试电机转向和控制反应，确认无误后再装桨做悬停测试。

代码层面必须加上解锁（Arm）和锁定（Disarm）逻辑：

// 解锁条件：遥控器油门最低 + 偏航通道拉到最右（或按特定组合） if (rc_roll < -0.9f && rc_throttle < 0.05f) { armed = 1; } // 输出 PWM 前必须检查 armed 状态 if (!armed) { Motor_Write(1000, 1000, 1000, 1000); // 输出最低油门 }

这个安全逻辑用最简单的方式实现，效果却非常直接：没有解锁前提，电机永远不可能输出高 PWM，从源头上防止了上电突然满油门导致的伤人事故。

4.2 姿态漂移：传感器校准与温漂问题

飞控常见的故障是：飞机静止放在桌面上，地面站显示的姿态角却在慢慢变化，或者加速计数据有明显偏移。这通常是两个原因造成的：一是传感器零偏没有校准，二是加速度计没有做水平校准。

校准方法：在飞机完全静止水平的状态下，采集 100 次陀螺仪和加速度计数据取平均，作为零偏值在代码中扣除。对于 MPU6050，还可以直接读取其寄存器中的XG_OFFSET_H等校准寄存器写入校准值。如果换了一个环境温度，最好重新校准一次。

4.3 调试工具思维：日志比波形更重要

刚开始调 PID 时，我习惯用串口把姿态角、角速度、PID 输出实时打印出来，然后用串口助手看波形。这个方法能看趋势，但数据刷新太快反而看不过来。后来我在工程里加了一个黑匣子（Blackbox）的思路：把关键数据按固定格式存储到 SD 卡或通过蓝牙低功耗传输到手机，飞行结束后统一分析。

哪怕是用最简单的 UART 日志，也比什么都没有强得多。记录数据时建议包含时间戳、姿态角目标值和反馈值、三轴角速度、油门值和 PID 输出。飞行结束后把 CSV 导入 Excel 或 MATLAB，画图看曲线，PID 参数该怎么调就会非常直观。

下面是我常用的一条调试日志格式，你可以直接参考：

// log.c - 在控制循环里调用 void Log_Record(float target_roll, float target_pitch, float target_yaw, float actual_roll, float actual_pitch, float actual_yaw, uint16_t throttle, float pid_rout, float pid_pout) { // 格式：时间戳, 目标横滚, 实际横滚, 目标俯仰, 实际俯仰, 油门, 横滚PID输出 printf("%lu,%0.2f,%0.2f,%0.2f,%0.2f,%u,%0.2f\r\n", (unsigned long)HAL_GetTick(), target_roll, actual_roll, target_pitch, actual_pitch, throttle, pid_rout); }

如果是新手，我强烈建议花两个晚上把日志功能搭好，再开始调参。没日志，调 PID 就像闭着眼拧旋钮，全凭运气。

5. 完整代码框架与后续扩展方向

最后把整体代码框架串一遍。一个完整的 STM32 无人机控制系统，目录结构大致如下：

Drone_Project/ ├── Core/ // 启动文件、系统时钟配置 │ ├── Inc/ │ └── Src/ │ ├── main.c // 主函数，初始化 RTOS 并启动任务 │ └── stm32f4xx_it.c // 中断服务 ├── Drivers/ │ ├── BSP/ // IMU、电调、遥控接收机等板级驱动 │ │ ├── mpu6050.c │ │ ├── motor.c │ │ └── rc_sbus.c ├── Middlewares/ │ └── FreeRTOS/ // RTOS 内核 ├── App/ │ ├── attitude.c // 姿态解算 │ ├── pid.c // PID 控制器 │ ├── control.c // 串级控制主逻辑 │ └── blackbox.c // 日志记录 └── Project/ └── MDK-ARM/ // Keil 工程文件

5.1 从基础飞行到功能扩展

当你把基础的手动飞行跑通之后，这个项目可以朝几个方向继续扩展：

• 定高飞行：加 BMP280 气压计，用油门通道作为高度目标，外环改成高度 PID，内环保持姿态控制不变。
• 光流悬停/避障：加 I2C 光流传感器或超声波模块，让无人机在无 GPS 环境下保持位置稳定，这对室内飞行非常实用。
• 地面站通信：把 STM32 的 UART 数据统一转发到 2.4G 数传模块，连接 QGroundControl 或自写地面站软件，可以看到实时姿态、航向和电量信息。
• 视觉跟随：预算允许的话，在机身加 K210 或 OpenMV 摄像头模块，通过 UART/SPI 与 STM32 通信，把目标物体的像素坐标转换成姿态角指令，实现简单的目标跟随。

5.2 国产芯片替代与生态迁移

如果你用的是 APM32、GD32 这类国产兼容芯片，代码迁移非常简单——它们大部分引脚和寄存器都能直接兼容 STM32 的 HAL 库，只需要注意启动文件和时钟配置差异。做产品化时要提前评估芯片供应商的长期稳定性，这一点样品阶段就要留意。

扩展方向里，我个人最推荐优先做定高 + 光流悬停。一方面这两个功能代码复杂度适中，能显著提升飞行体验；另一方面它们会让你接触到更多控制理论的实际应用，比如串级高度环、惯性滤波、数据融合等，技术含金量比单纯做手动飞行高不少。

最后再分享一个我个人的实操体会：想飞好一架无人机，代码能力只是及格线，真正拉开差距的是调试方法和安全习惯。写飞控代码之前，先把传感器校准流程、解锁保护逻辑、日志系统和机械安全检查做好，再开始谈算法。每一步都求稳，飞机自然稳。

本文还有配套的精品资源，点击获取