虽然踩了一些坑，但是4轴的电机总算可以转了，当四个电机已经可以独立控制转速时，四轴飞行器在物理上已经具备了飞行的基本条件，只要四个电机同时提高转速，使总推力大于自身重力，飞行器就可以离地上升；通过改变前后或左右电机之间的转速差，可以产生俯仰（Pitch）或横滚（Roll）力矩，从而改变飞行姿态。
从原理上看，似乎只要按照“前低后高”“左低右高”的规则调整电机转速，飞行器就可以实现前进或侧移，但这只是理想情况下的分析，实际上四轴飞行器是一个典型的开环不稳定系统。即使存在极小的姿态偏差、重心误差、电机响应不一致或外界气流扰动，都会导致姿态迅速发散，最终失控坠落。
它更像一个倒立摆系统：

理论上只要“向偏移方向反向施力”就能保持平衡，但如果没有持续、快速、精确的反馈控制，人几乎无法凭感觉维持稳定。因此，仅仅能够控制电机转速远远不够。
飞控系统必须实时感知当前姿态，并根据姿态误差动态调整电机输出，构成一个闭环控制系统，飞行器才能稳定悬停。

MPU6050与惯性测量单元（IMU）

要感知飞控系统的姿态，实际上就是飞控系统必须实时知道

当前是否倾斜
倾斜了多少
正在以多快的速度旋转

这些信息无法通过“推算电机转速”得到，必须通过传感器测量。在四轴飞控中，这类传感器被称为 IMU（Inertial Measurement Unit，惯性测量单元）

什么是 IMU？

IMU 是一种用于测量刚体运动状态的传感器模块，通常包含：

三轴加速度计
三轴陀螺仪
有些高端 IMU 还会包含磁力计，但在基础四轴飞控中，仅使用 6 轴 IMU 已经足够完成姿态解算。
IMU 并不会直接告诉我们“当前是 10° 俯仰”，它只提供两类基础物理量：

线性加速度（ax, ay, az）
角速度（gx, gy, gz）
飞控系统需要通过算法，将这些原始数据转换为可理解的姿态角

MPU6050 简介

MPU6050 是 InvenSense 推出的一款 6 轴 IMU 芯片，内部集成：

3 轴 MEMS 加速度计
3 轴 MEMS 陀螺仪
数字运动处理器（DMP）
I2C 通信接口

其主要特点包括：

可配置加速度量程（±2g / ±4g / ±8g / ±16g）
可配置角速度量程（±250 / ±500 / ±1000 / ±2000 °/s）
内置数字低通滤波器（DLPF）
支持最高 1kHz 采样率

对于 DIY 四轴飞控而言，MPU6050 成本低、资料丰富，是非常经典的入门选择，它在整个飞控架构中，MPU6050 位于最底层的数据输入端：

MPU6050 采集原始数据
        ↓
零偏校准 + 单位转换
        ↓
姿态解算算法（Mahony / 互补滤波）
        ↓
输出 roll / pitch / yaw
        ↓
PID 控制
        ↓
电机 PWM 输出

采集原始数据

我是在拼多多6.5元购买的MPU6050模块，支持I2C协议读取三轴加速度与三轴陀螺仪数据，代码就不放了，基本找AI写的，唯一需要注意的是，正常的MPU6050模块初始化完成读取设备ID时，读到的一般是0x68，但是我始终读到的是0x70，一开始我以为有问题，后来才发现我买到的是国产芯片，有一些区别，有些商家会在商品详情页说明，有些则没说，不过也没关系，经过测试读到的各轴数据都是正常的。

零偏校准

在读取 MPU6050 数据时，很多人会发现一个现象：
即使飞行器静止不动，陀螺仪的输出也不是 0，加速度计在水平放置时，也并不完全是 (0, 0, 1g)，这种“静止状态下的非零输出”，就是零偏（Bias）。所谓零偏就是当输入物理量为 0 时，传感器输出却不为 0 的偏移误差。
对于低成本 IMU（如 MPU6050），零偏是普遍存在的现象，而不是异常情况。那如果处理这种误差问题呢，这里就需要做零偏校准。
具体做法就是：

上电后保持飞行器静止
连续采集 N 次数据
取平均值
将平均值作为偏移量
后续数据减去该偏移量

经过这样处理，在静止状态下，可以让陀螺仪输出应接近 0，代码如下：

#include "imu.h"  
#include "mpu6050.h"  
#include "math.h"  
  
#define IMU_CALIB_SAMPLES   1000  
  
/* MPU6050 量程对应比例 */#define ACC_SENS_2G        16384.0f // LSB/g  
#define GYRO_SENS_2000DPS  16.4f // LSB/(°/s)  
  
/* 零偏 */static float ax_off = 0, ay_off = 0, az_off = 0;  
static float gx_off = 0, gy_off = 0, gz_off = 0;  
  
void IMU_Init(void)  
{  
    MPU6050_RawData_t raw;  
    int32_t ax_sum = 0, ay_sum = 0, az_sum = 0;  
    int32_t gx_sum = 0, gy_sum = 0, gz_sum = 0;  
  
    /* 上电后保持静止！ */  for (int i = 0; i < IMU_CALIB_SAMPLES; i++)  
    {  
        MPU6050_ReadRawData(&raw);  
  
        ax_sum += raw.ax;  
        ay_sum += raw.ay;  
        az_sum += raw.az;  
  
        gx_sum += raw.gx;  
        gy_sum += raw.gy;  
        gz_sum += raw.gz;  
  
        HAL_Delay(2);   // ~500Hz 采样  
  }  
  
    ax_off = ax_sum / (float)IMU_CALIB_SAMPLES;  
    ay_off = ay_sum / (float)IMU_CALIB_SAMPLES;  
    az_off = az_sum / (float)IMU_CALIB_SAMPLES - ACC_SENS_2G;  
    // Z 轴默认朝上，减去 1g  
  gx_off = gx_sum / (float)IMU_CALIB_SAMPLES;  
    gy_off = gy_sum / (float)IMU_CALIB_SAMPLES;  
    gz_off = gz_sum / (float)IMU_CALIB_SAMPLES;  
}  
  
void IMU_Update(IMU_Data_t *imu)  
{  
    MPU6050_RawData_t raw;  
  
    MPU6050_ReadRawData(&raw);  
  
    /* 去零偏 */  float ax = raw.ax - ax_off;  
    float ay = raw.ay - ay_off;  
    float az = raw.az - az_off;  
  
    float gx = raw.gx - gx_off;  
    float gy = raw.gy - gy_off;  
    float gz = raw.gz - gz_off;  
  
    /* 单位换算 */  imu->ax = ax / ACC_SENS_2G;  
    imu->ay = ay / ACC_SENS_2G;  
    imu->az = az / ACC_SENS_2G;  
  
    imu->gx = gx / GYRO_SENS_2000DPS;  
    imu->gy = gy / GYRO_SENS_2000DPS;  
    imu->gz = gz / GYRO_SENS_2000DPS;  
}

经过零偏校准顺便单位换算后，可以得到较为准确的6轴数据

姿态解算

经过零偏校准和单位转换之后，我们已经能够稳定获得：

加速度：ax, ay, az
角速度：gx, gy, gz

但飞控真正需要的不是“加速度”和“角速度”，而是：横滚角(roll)、俯仰角(pitch)、偏航角(pitch)
这里需要用到一套姿态解算算法，常用的有：互补滤波、Mahony 算法等，其实这一步的算法我也没搞懂，反正就是通过输入零偏校准后的数据到一个算法里，算出来欧拉角，所以这一步交给AI来给我提供算法我直接用吧，让ChatGPT写了一套Mahony 算法，代码如下：

#include "mahony.h"  
#include <math.h>  
  
// 四元数表示姿态  
static float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;  
  
// Mahony 参数  
static float Kp = 1.0f;  
static float Ki = 0.0f;  
static float integralFBx = 0, integralFBy = 0, integralFBz = 0;  
  
/**  
 * @brief 初始化 Mahony 滤波器  
 */void Mahony_Init(float kp, float ki)  
{  
    Kp = kp;  
    Ki = ki;  
    q0 = 1.0f; q1 = 0.0f; q2 = 0.0f; q3 = 0.0f;  
    integralFBx = 0; integralFBy = 0; integralFBz = 0;  
}  
  
/**  
 * @brief Mahony 滤波器更新  
 */void Mahony_Update(IMU_Data_t *imu, float dt, Attitude_t *att)  
{  
    float ax = imu->ax;  
    float ay = imu->ay;  
    float az = imu->az;  
    float gx = imu->gx * (M_PI / 180.0f); // °/s -> rad/s  
  float gy = imu->gy * (M_PI / 180.0f);  
    float gz = imu->gz * (M_PI / 180.0f);  
  
    // 1. 归一化加速度向量  
  float norm = sqrtf(ax*ax + ay*ay + az*az);  
    if (norm == 0.0f) return; // 避免除零  
  ax /= norm;  
    ay /= norm;  
    az /= norm;  
  
    // 2. 估计重力方向  
  float vx = 2*(q1*q3 - q0*q2);  
    float vy = 2*(q0*q1 + q2*q3);  
    float vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;  
  
    // 3. 误差计算 (加速度 - 重力向量)  
  float ex = (ay*vz - az*vy);  
    float ey = (az*vx - ax*vz);  
    float ez = (ax*vy - ay*vx);  
  
    // 4. 积分反馈  
  if(Ki > 0.0f)  
    {  
        integralFBx += Ki*ex*dt;  
        integralFBy += Ki*ey*dt;  
        integralFBz += Ki*ez*dt;  
  
        gx += integralFBx;  
        gy += integralFBy;  
        gz += integralFBz;  
    }  
  
    // 5. 比例反馈  
  gx += Kp*ex;  
    gy += Kp*ey;  
    gz += Kp*ez;  
  
    // 6. 四元数微分  
  float qDot0 = 0.5f * (-q1*gx - q2*gy - q3*gz);  
    float qDot1 = 0.5f * ( q0*gx + q2*gz - q3*gy);  
    float qDot2 = 0.5f * ( q0*gy - q1*gz + q3*gx);  
    float qDot3 = 0.5f * ( q0*gz + q1*gy - q2*gx);  
  
    // 7. 积分更新四元数  
  q0 += qDot0 * dt;  
    q1 += qDot1 * dt;  
    q2 += qDot2 * dt;  
    q3 += qDot3 * dt;  
  
    // 8. 四元数归一化  
  norm = sqrtf(q0*q0 + q1*q1 + q2*q2 + q3*q3);  
    q0 /= norm;  
    q1 /= norm;  
    q2 /= norm;  
    q3 /= norm;  
  
    // 9. 输出 Roll / Pitch / Yaw（单位：度）  
  att->roll = atan2f(2*(q0*q1 + q2*q3), 1 - 2*(q1*q1 + q2*q2)) * (180.0f / M_PI);  
    att->pitch = asinf(2*(q0*q2 - q3*q1)) * (180.0f / M_PI);  
    att->yaw = atan2f(2*(q0*q3 + q1*q2), 1 - 2*(q2*q2 + q3*q3)) * (180.0f / M_PI);  
}

PID

在完成姿态解算之后，下一步就是进入 控制层（Control Layer），也就是 PID 控制，前面主要解决的是：

我现在“是什么姿态”？

而下面要解决的是：

我要变成“什么姿态”，以及如何通过调节电机，让当前姿态逼近目标姿态。

上面通过姿态解算获得的欧拉角仅仅只是知道了当前的姿态，飞控系统还必须根据目标姿态，动态调整四个电机的转速，使飞行器保持平衡或完成指定动作，这个过程，就是姿态控制。
所谓PID 是最经典、最实用的闭环控制算法：

1️⃣ P（比例项）

根据当前误差立即调整。

误差越大，修正越强
是最主要的控制力量

P 太小 → 反应慢
P 太大 → 振荡

2️⃣ I（积分项）

累计长期误差。
用于消除：

长时间轻微倾斜
重心偏移
推力不一致

没有 I 项，飞行器可能始终带有微小角度偏差。

3️⃣ D（微分项）

预测误差变化趋势。
当姿态变化过快时：

提前抑制过冲
增强稳定性

D 太大 → 抖动明显

以上部分又涉及算法，这里还是交给AI吧，AI还是太好用了。。。

#include "pid.h"  
  
void PID_Init(PID_t *pid, float Kp, float Ki, float Kd, float out_min, float out_max)  
{  
    pid->Kp = Kp;  
    pid->Ki = Ki;  
    pid->Kd = Kd;  
    pid->integral = 0.0f;  
    pid->last_error = 0.0f;  
    pid->output_min = out_min;  
    pid->output_max = out_max;  
}  
  
float PID_Calc(PID_t *pid, float target, float current, float dt)  
{  
    float error = target - current;  
    pid->integral += error * dt;  
    float derivative = (error - pid->last_error) / dt;  
    pid->last_error = error;  
  
    float output = pid->Kp * error + pid->Ki * pid->integral + pid->Kd * derivative;  
  
    // 限幅，避免 PWM 越界  
  if (output > pid->output_max) output = pid->output_max;  
    if (output < pid->output_min) output = pid->output_min;  
  
    return output;  
}

总结：

PID 是一种基于误差反馈的控制算法，通过比例、积分、微分三种调节方式，使系统输出逼近目标值，是四轴飞控实现稳定姿态控制的核心算法。

目录CONTENT

基于STM32的四轴飞行器-姿态感知篇