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

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

一、系统概述

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

核心特点

二、系统总体架构

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 主控最小系统

3.2.2 电机驱动电路

四、核心软件设计

4.1 开发环境与工具

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）

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

结构：

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

// 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 地面测试

5.2 试飞调试

5.3 常见问题与解决

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

六、扩展功能

  1. 视觉避障：添加OV2640摄像头，用OpenMV实现前方障碍物检测（TOF或光流）；
  2. 跟随模式：通过UWB模块（如Decawave）实现“主人跟随”；
  3. 自动返航（RTH）：GPS失联时，按预设高度返回起飞点；
  4. 集群控制：多机通过数传模块组网，实现编队飞行（需TDMA协议）。

七、总结

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

专注于matlab/simulink,电子电路,编程

关于我们湘ICP备19023172号-4