基于STM32的四轴飞行器控制系统设计

一、系统概述

四轴飞行器（Quadcopter）是一种垂直起降（VTOL）多旋翼无人机，通过四个无刷电机的转速差实现姿态控制与稳定飞行。本系统以STM32高性能微控制器为核心，融合传感器融合、姿态解算、PID控制、电机驱动等技术，实现自主悬停、姿态稳定、航线飞行等功能，适用于航拍、测绘、物流配送等场景。

核心特点

• 高实时性：基于STM32F4/H7系列（Cortex-M4/M7内核，带FPU），满足姿态解算与控制算法的实时性需求；

• 模块化设计：传感器、执行器、通信模块独立封装，便于维护与扩展；

• 多模式控制：支持手动遥控、半自主悬停、全自主导航（GPS/视觉）；

• 安全冗余：集成低电压保护、失控保护、传感器故障检测机制。

二、系统总体架构

2.1 硬件架构

graph TD A[主控模块<br>STM32F427/STM32H750] --> B[传感器模块<br>IMU+气压计+GPS+磁力计] A --> C[执行器模块<br>无刷电机+电调] A --> D[通信模块<br>遥控器+数传+蓝牙] A --> E[电源模块<br>锂电池+稳压电路] B -->|I2C/SPI| A C -->|PWM| A D -->|UART/SBUS| A E -->|3.3V/5V| A&B&C&D

2.2 软件架构

graph TD A[底层驱动层<br>传感器/电机/通信驱动] --> B[中层算法层<br>姿态解算+PID控制+导航] B --> C[上层应用层<br>模式管理+安全保护+数据交互] C --> D[用户接口<br>遥控器+地面站]

三、核心硬件设计

3.1 核心组件选型

模块	型号/参数	功能说明
主控	STM32F427VIT6（Cortex-M4，180MHz，FPU）	姿态解算、PID控制、任务调度（或STM32H750，更高性能）
IMU	ICM-20602（6轴，陀螺仪+加速度计，SPI）	测量角速度（°/s）与加速度（m/s²）
磁力计	HMC5883L（3轴，I2C）	航向角测量（罗盘校准）
气压计	BMP388（高精度，I2C）	海拔高度测量（定高用）
GPS	NEO-M8N（UBlox，5Hz，UART）	位置与速度测量（自主导航用）
电机	朗宇X2212（980KV，无刷）	提供升力（搭配4个电调）
电调	好盈XRotor 20A（BLHeli_S，支持DShot）	将PWM信号转换为电机驱动电流
通信	SBUS接收机（FrSky，UART）+ 数传模块（SiK Radio）	遥控器信号接收、地面站数据交互
电源	3S锂电池（11.1V，2200mAh）+ LM2596（5V）+ AMS1117（3.3V）	为各模块供电（电机单独供电）

3.2 硬件电路设计要点

3.2.1 主控最小系统

• 时钟：8MHz外部晶振+PLL倍频至180MHz（STM32F427）；

• 调试接口：SWD（SWDIO+SWCLK）+ 串口（用于日志输出）；

• 引脚分配：

  • I2C1：连接IMU（ICM-20602）、磁力计（HMC5883L）、气压计（BMP388）；

  • SPI1：备用IMU或GPS接口；

  • UART1：SBUS接收机（波特率100000，8E2）；

  • UART2：数传模块（波特率57600）；

  • TIM1/TIM8：PWM输出（4路，控制4个电调，频率400Hz，DShot600协议）。

3.2.2 电机驱动电路

• 电调连接：每个电调信号线接STM32的PWM输出引脚，电源端接锂电池（11.1V），GND共地；

• 保护电路：在电调电源端并联1000μF电容，抑制电压波动；

• 螺旋桨安装：X型布局（前右/左后/后左/右前），注意旋转方向（相邻电机转向相反）。

四、核心软件设计

4.1 开发环境与工具

• IDE：STM32CubeIDE（基于Eclipse，集成HAL库）；

• RTOS：FreeRTOS（多任务调度，优先级：传感器读取>姿态解算>PID控制>通信）；

• 算法库：ARM CMSIS-DSP（浮点运算加速）、Eigen（矩阵运算，用于扩展卡尔曼滤波）；

• 地面站：Mission Planner（参数配置、数据监控）、QGroundControl（航线规划）。

4.2 核心算法实现

4.2.1 姿态解算（Mahony滤波算法）

功能：融合IMU（加速度计+陀螺仪）与磁力计数据，输出飞行器姿态角（ roll/pitch/yaw ）。

原理：基于四元数表示姿态，通过梯度下降法修正陀螺仪漂移，结合加速度计（重力方向）和磁力计（地磁方向）校准。

代码实现（STM32 HAL库）：

// Mahony滤波核心函数
void mahony_update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
    float norm;
    float hx, hy, bx, bz;
    float vx, vy, vz, wx, wy, wz;
    float ex, ey, ez;
    float q0 = q[0], q1 = q[1], q2 = q[2], q3 = q[3];  // 四元数

    // 1. 归一化加速度计和磁力计数据
    norm = sqrt(ax*ax + ay*ay + az*az);
    ax /= norm; ay /= norm; az /= norm;
    norm = sqrt(mx*mx + my*my + mz*mz);
    mx /= norm; my /= norm; mz /= norm;

    // 2. 计算参考向量（地球坐标系）
    // 重力向量（加速度计）
    vx = 2*(q1*q3 - q0*q2);
    vy = 2*(q0*q1 + q2*q3);
    vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;
    // 地磁向量（磁力计）
    hx = 2*mx*(0.5 - q2*q2 - q3*q3) + 2*my*(q1*q3 - q0*q2) + 2*mz*(q0*q1 + q2*q3);
    hy = 2*mx*(q1*q3 + q0*q2) + 2*my*(0.5 - q1*q1 - q3*q3) + 2*mz*(q2*q3 - q0*q1);
    bx = sqrt(hx*hx + hy*hy);
    bz = 2*mx*(q2*q3 - q0*q1) + 2*my*(q0*q2 + q1*q3) + 2*mz*(0.5 - q1*q1 - q2*q2);

    // 3. 计算误差向量（传感器测量值与参考向量的叉积）
    ex = (ay*vz - az*vy) + (my*bz - mz*by);
    ey = (az*vx - ax*vz) + (mz*bx - mx*bz);
    ez = (ax*vy - ay*vx) + (mx*by - my*bx);

    // 4. 用PI控制器修正陀螺仪漂移
    ki = 0.01;  // 积分系数
    kp = 0.1;   // 比例系数
    integralFBx += ki * ex * dt;
    integralFBy += ki * ey * dt;
    integralFBz += ki * ez * dt;
    gx += kp*ex + integralFBx;
    gy += kp*ey + integralFBy;
    gz += kp*ez + integralFBz;

    // 5. 更新四元数（陀螺仪积分）
    q0 += (-q1*gx - q2*gy - q3*gz) * 0.5f * dt;
    q1 += (q0*gx + q2*gz - q3*gy) * 0.5f * dt;
    q2 += (q0*gy - q1*gz + q3*gx) * 0.5f * dt;
    q3 += (q0*gz + q1*gy - q2*gx) * 0.5f * dt;

    // 6. 四元数归一化
    norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
    q0 /= norm; q1 /= norm; q2 /= norm; q3 /= norm;

    // 7. 四元数转欧拉角（roll, pitch, yaw）
    roll = atan2(2*(q0*q1 + q2*q3), 1-2*(q1*q1 + q2*q2)) * RAD2DEG;
    pitch = asin(2*(q0*q2 - q3*q1)) * RAD2DEG;
    yaw = atan2(2*(q0*q3 + q1*q2), 1-2*(q2*q2 + q3*q3)) * RAD2DEG;
}

4.2.2 PID控制（串级PID）

功能：根据姿态角误差计算电机转速调整量，实现稳定飞行。

结构：

• 内环（角速度环）：输入陀螺仪测量的角速度，输出期望角速度（比例P控制，快速响应）；

• 外环（角度环）：输入姿态角误差（目标角-实际角），输出期望角速度（PI控制，消除静差）；

• 位置环（可选）：输入GPS/气压计的位置误差，输出期望姿态角（用于自主导航）。

代码实现（角度环+角速度环）：

// PID控制器结构体
typedef struct {
    float Kp, Ki, Kd;  // 比例、积分、微分系数
    float integral;    // 积分项
    float prev_error;  // 上次误差
} PID_Controller;

// PID计算函数
float pid_update(PID_Controller *pid, float setpoint, float measured, float dt) {
    float error = setpoint - measured;
    pid->integral += error * dt;
    float derivative = (error - pid->prev_error) / dt;
    float output = pid->Kp*error + pid->Ki*pid->integral + pid->Kd*derivative;
    pid->prev_error = error;
    return output;
}

// 四轴电机转速分配（X型布局）
void motor_mixing(float roll_out, float pitch_out, float yaw_out, float throttle) {
    // 基础油门（四个电机相同）
    float base_throttle = throttle;
    // 姿态调整量（差速控制）
    float m1 = base_throttle - roll_out + pitch_out + yaw_out;  // 前右电机
    float m2 = base_throttle + roll_out + pitch_out - yaw_out;  // 前左电机
    float m3 = base_throttle + roll_out - pitch_out + yaw_out;  // 后左电机
    float m4 = base_throttle - roll_out - pitch_out - yaw_out;  // 后右电机
    // 限幅（0~100%油门）
    m1 = constrain(m1, 0, 100); m2 = constrain(m2, 0, 100);
    m3 = constrain(m3, 0, 100); m4 = constrain(m4, 0, 100);
    // 输出PWM（DShot协议）
    esc_write(0, m1); esc_write(1, m2); esc_write(2, m3); esc_write(3, m4);
}

4.2.3 电机控制（DShot协议）

功能：通过数字信号（DShot600）控制电调，比传统PWM更抗干扰、精度更高。

实现：STM32通过定时器输出DShot信号（每 bit 1.67μs，600kbps），包含油门值（0~2047）和校验位。

关键代码（DShot600发送）：

// 发送DShot信号（单路）
void dshot_send(uint8_t motor_id, uint16_t value) {
    // 计算校验位（低11位求和的最低3位）
    uint16_t checksum = (value ^ (value >> 4) ^ (value >> 8)) & 0x0F;
    value = (value << 4) | checksum;  // 12位数据（11位油门+1位校验）
    
    // 配置定时器为输出比较模式（每1.67μs翻转一次）
    TIM_HandleTypeDef *htim = &htim1;  // 假设用TIM1输出
    htim->Instance->ARR = 1;  // 1.67μs @ 600MHz/360=1.67MHz（需根据主频调整）
    for (int i=0; i<16; i++) {  // 16位（含2位起始位0）
        uint16_t bit = (value >> (15-i)) & 0x01;
        HAL_GPIO_WritePin(GPIOA, GPIO_PIN_0, bit);  // 假设用PA0输出
        __HAL_TIM_SET_COMPARE(htim, TIM_CHANNEL_1, 1);  // 触发翻转
        HAL_Delay_us(1.67);  // 精确延时（或用定时器中断）
    }
}

4.3 主程序流程（FreeRTOS任务调度）

graph TD A[系统初始化<br>时钟/外设/GPIO] --> B[创建FreeRTOS任务] B --> C[传感器读取任务<br>IMU/气压计/GPS，100Hz] C --> D[姿态解算任务<br>Mahony滤波，100Hz] D --> E[PID控制任务<br>姿态环+位置环，200Hz] E --> F[电机控制任务<br>DShot输出，400Hz] F --> G[通信任务<br>遥控器/数传，50Hz] G --> H[安全监控任务<br>低电压/失控保护，10Hz] H -->|异常| I[紧急降落/返航]

参考代码 基于STM32 四轴飞行器控制系统（硬件、源码、设计报告） www.youwenfan.com/contentcns/124192.html

五、系统测试与调试

5.1 地面测试

• 传感器校准：

  • 加速度计：水平静止放置，校准零偏；

  • 磁力计：旋转飞行器360°，绘制椭球校准；

  • 陀螺仪：静态下观察角速度漂移，调整积分系数。

• 电机测试：逐个解锁电机，检查转向是否正确（X型布局：前右/后左顺时针，前左/后右逆时针）。

5.2 试飞调试

• 手动模式：通过遥控器控制姿态，观察PID参数是否合适（如震荡则减小Kp，静差则增大Ki）；

• 半自主模式：开启气压计定高，测试悬停稳定性（调整高度环PID）；

• 全自主模式：上传航线（Waypoints），测试GPS导航精度（需开阔场地）。

5.3 常见问题与解决

问题现象	可能原因	解决方案
飞行器自旋	陀螺仪零偏未校准/电机转向错误	重新校准IMU，检查电机转向（交换任意两相线）
悬停时上下波动	高度环PID参数不当/气压计噪声	减小高度环Kp，增加气压计滤波（移动平均）
遥控器信号丢失	SBUS协议解析错误/天线接触不良	检查UART配置（8E2），更换接收机天线

六、扩展功能

1. 视觉避障：添加OV2640摄像头，用OpenMV实现前方障碍物检测（TOF或光流）；

2. 跟随模式：通过UWB模块（如Decawave）实现“主人跟随”；

3. 自动返航（RTH）：GPS失联时，按预设高度返回起飞点；

4. 集群控制：多机通过数传模块组网，实现编队飞行（需TDMA协议）。

七、总结

本系统基于STM32实现了四轴飞行器的全流程控制，从传感器数据采集、姿态解算到PID控制与电机驱动，形成了完整的闭环控制链路。核心优势在于模块化设计（便于替换传感器/算法）、实时性（FreeRTOS任务调度）和安全性（多重保护机制）。