STM32四旋翼无人机飞控系统设计与实现

1. 项目概述

四旋翼无人机作为当前最流行的飞行器平台之一，其核心在于飞行控制系统的设计与实现。基于STM32微控制器的飞控系统设计，既要考虑实时性要求，又要兼顾功耗和成本因素。我在过去三年中参与了多个无人机项目开发，发现很多初学者在飞控软件设计环节容易陷入各种误区。

这个项目主要解决的是如何基于STM32平台实现稳定、可靠的飞行控制算法。不同于市面上现成的飞控方案，我们从底层开始构建整个软件系统，包括传感器数据采集、姿态解算、PID控制算法实现、电机驱动等核心模块。这种自主开发方式虽然难度较大，但能让我们真正掌握无人机飞控的核心技术。

2. 硬件平台选型与搭建

2.1 STM32主控芯片选择

在STM32系列中，我们选择了STM32F405作为主控芯片。这款芯片具有以下优势：

• 168MHz主频，满足实时控制需求
• 自带FPU浮点运算单元，加速姿态解算
• 丰富的外设接口（6个USART、3个SPI、2个I2C）
• 充足的Flash（1MB）和SRAM（192KB）

注意：不要选择不带FPU的型号，否则姿态解算会占用大量CPU资源，影响控制周期。

2.2 传感器模块配置

飞控系统需要以下核心传感器：

1. MPU6050：三轴加速度计+陀螺仪（I2C接口）
2. BMP280：气压计（用于高度估计，I2C接口）
3. HMC5883L：磁力计（电子罗盘，I2C接口）
4. GPS模块（UART接口）

传感器布局要考虑电磁干扰问题：

• 磁力计应远离电机和电源线
• 加速度计尽量靠近无人机重心
• 气压计要有防气流干扰设计

3. 软件架构设计

3.1 实时操作系统选择

我们采用FreeRTOS作为实时操作系统，任务划分如下：

3.2 软件模块划分

飞控软件主要分为以下模块：

1. 驱动层：传感器驱动、PWM输出驱动
2. 算法层：姿态解算、控制算法
3. 通信层：遥控信号解析、数传接口
4. 应用层：飞行模式管理、安全保护

4. 核心算法实现

4.1 姿态解算实现

采用Mahony互补滤波算法，相比卡尔曼滤波计算量更小，适合STM32平台：

c复制void MahonyAHRSupdate(float gx, float gy, float gz, 
                     float ax, float ay, float az,
                     float mx, float my, float mz) {
    float recipNorm;
    float q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;  
    float hx, hy, bx, bz;
    float halfvx, halfvy, halfvz, halfwx, halfwy, halfwz;
    float halfex, halfey, halfez;
    float qa, qb, qc;

    // 使用磁力计数据时
    if(mx != 0.0f || my != 0.0f || mz != 0.0f) {
        // 归一化磁力计数据
        recipNorm = invSqrt(mx * mx + my * my + mz * mz);
        mx *= recipNorm;
        my *= recipNorm;
        mz *= recipNorm;

        // 计算参考磁场方向
        hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2));
        hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1));
        bx = sqrt(hx * hx + hy * hy);
        bz = 2.0f * (mx * (q1q3 - q0q2) + my * (q2q3 + q0q1) + mz * (0.5f - q1q1 - q2q2));
        
        // 计算磁场误差
        halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2);
        halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3);
        halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2); 
    }

    // 计算加速度计误差
    halfvx = q1q3 - q0q2;
    halfvy = q0q1 + q2q3;
    halfvz = q0q0 - 0.5f + q3q3;
    
    // 误差积分
    halfex += Ki * halfex * dt;
    halfey += Ki * halfey * dt;
    halfez += Ki * halfez * dt;
    
    // 应用反馈
    gx += Kp * halfex + halfwx;
    gy += Kp * halfey + halfwy;
    gz += Kp * halfez + halfwz;
    
    // 四元数积分
    gx *= (0.5f * dt);
    gy *= (0.5f * dt);
    gz *= (0.5f * dt);
    qa = q0;
    qb = q1;
    qc = q2;
    q0 += (-qb * gx - qc * gy - q3 * gz);
    q1 += (qa * gx + qc * gz - q3 * gy);
    q2 += (qa * gy - qb * gz + q3 * gx);
    q3 += (qa * gz + qb * gy - qc * gx);
    
    // 四元数归一化
    recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
    q0 *= recipNorm;
    q1 *= recipNorm;
    q2 *= recipNorm;
    q3 *= recipNorm;
}

4.2 PID控制算法实现

采用串级PID控制结构：

1. 外环：位置/角度控制（更新较慢）
2. 内环：角速率控制（更新较快）

c复制typedef struct {
    float kp;
    float ki;
    float kd;
    float integral;
    float prev_error;
    float limit;
} PID_Controller;

float PID_Update(PID_Controller* pid, float error, float dt) {
    float p_term = pid->kp * error;
    
    pid->integral += error * dt;
    // 积分限幅
    if(pid->integral > pid->limit) pid->integral = pid->limit;
    else if(pid->integral < -pid->limit) pid->integral = -pid->limit;
    float i_term = pid->ki * pid->integral;
    
    float d_term = pid->kd * (error - pid->prev_error) / dt;
    pid->prev_error = error;
    
    float output = p_term + i_term + d_term;
    // 输出限幅
    if(output > pid->limit) output = pid->limit;
    else if(output < -pid->limit) output = -pid->limit;
    
    return output;
}

5. 电机控制与混控算法

5.1 PWM信号生成

使用STM32的定时器产生4路PWM信号：

c复制void PWM_Init(TIM_HandleTypeDef* htim, uint32_t channel) {
    TIM_OC_InitTypeDef sConfigOC = {0};
    
    htim->Instance->ARR = 2000 - 1; // 50Hz PWM频率
    sConfigOC.OCMode = TIM_OCMODE_PWM1;
    sConfigOC.Pulse = 1000; // 初始化为1000us(油门最低)
    sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH;
    sConfigOC.OCFastMode = TIM_OCFAST_DISABLE;
    HAL_TIM_PWM_ConfigChannel(htim, &sConfigOC, channel);
    HAL_TIM_PWM_Start(htim, channel);
}

void PWM_SetDuty(TIM_HandleTypeDef* htim, uint32_t channel, uint16_t duty_us) {
    // duty_us范围通常为1000-2000us
    __HAL_TIM_SET_COMPARE(htim, channel, duty_us);
}

5.2 混控算法实现

将控制量分配到四个电机：

c复制void Mixer(float throttle, float roll, float pitch, float yaw) {
    // 电机混控公式
    float m1 = throttle - pitch + roll - yaw; // 前右
    float m2 = throttle - pitch - roll + yaw; // 前左
    float m3 = throttle + pitch - roll - yaw; // 后左
    float m4 = throttle + pitch + roll + yaw; // 后右
    
    // 限幅处理
    m1 = constrain(m1, 1000, 2000);
    m2 = constrain(m2, 1000, 2000);
    m3 = constrain(m3, 1000, 2000);
    m4 = constrain(m4, 1000, 2000);
    
    // 更新PWM输出
    PWM_SetDuty(&htim1, TIM_CHANNEL_1, m1);
    PWM_SetDuty(&htim1, TIM_CHANNEL_2, m2);
    PWM_SetDuty(&htim1, TIM_CHANNEL_3, m3);
    PWM_SetDuty(&htim1, TIM_CHANNEL_4, m4);
}

6. 系统调试与参数整定

6.1 传感器校准

1. 加速度计校准：

   • 将飞控板水平放置，采集100个样本求平均值
   • 旋转180度再次采集
   • 计算偏移量和比例因子

2. 陀螺仪校准：

   • 静止状态下采集数据，计算零偏
   • 通过旋转测试验证比例因子

3. 磁力计校准：

   • 执行"8字"校准法
   • 计算硬铁和软铁干扰补偿

6.2 PID参数整定

采用分步调试法：

1. 先调内环（角速率环）：

   • 从很小的P值开始（如0.01）
   • 逐渐增加直到出现轻微震荡
   • 然后加入D项抑制震荡
   • 最后加入少量I项消除静差

2. 再调外环（角度环）：

   • P值通常比内环小一个数量级
   • 一般不需要D项
   • I项用于消除稳态误差

典型参数范围：

• 角速率环：P(0.1-1.0), I(0-0.1), D(0.001-0.01)
• 角度环：P(1.0-5.0), I(0-0.5), D(0)

7. 飞行测试与问题排查

7.1 常见问题及解决方案

7.2 安全注意事项

1. 首次试飞时使用安全绳固定无人机
2. 远离人群和贵重物品
3. 逐步增加油门，避免突然全速
4. 准备紧急停止措施（如切断电源开关）
5. 飞行前检查螺旋桨紧固情况

8. 系统优化与扩展

8.1 性能优化技巧

1. 使用DMA传输传感器数据，减少CPU开销
2. 将频繁调用的数学函数优化为查表法
3. 合理使用STM32的硬件FPU
4. 关键代码放在ITCM内存提高执行速度
5. 使用RTOS的任务优先级确保关键任务及时执行

8.2 功能扩展方向

1. 添加GPS导航功能
2. 实现视觉定位系统
3. 开发地面站软件
4. 增加失控保护机制
5. 支持多种飞行模式（定高、定点、自主航线等）

在完成基础飞控系统后，我通常会建议开发者先专注于飞行稳定性调校，而不是急于添加复杂功能。一个响应灵敏、抗干扰能力强的底层控制系统，才是后续功能扩展的坚实基础。实际项目中，我遇到过很多因为底层控制不稳导致高级功能无法实现的情况，这时候往往需要返工重调PID参数，反而耽误更多时间。