四旋翼无人机飞行程序设计（基于STM32的嵌入式实现）

一、系统概述

四旋翼无人机飞行程序是多传感器融合、实时控制算法与嵌入式系统的综合应用，核心目标是实现稳定飞行、自主导航与任务执行。本设计基于STM32F427VIH6（Cortex-M4，180MHz，带FPU）为主控，融合IMU（惯性测量单元）、GPS、气压计、磁力计等传感器，通过串级PID控制实现姿态稳定，结合路径规划完成自主飞行，支持手动遥控、半自主悬停、全自主导航三种模式。

二、飞行程序核心架构

2.1 分层设计

graph TD
    A[应用层] -->|任务指令| B[导航与控制层]
    B -->|姿态/位置误差| C[算法层]
    C -->|控制量| D[执行器层]
    D -->|PWM/DShot信号| E[电机/舵机]
    F[传感器层] -->|原始数据| C
    G[通信层] -->|遥控/数传| A
    H[电源与安全层] -->|状态监控| A

2.2 核心模块功能

模块	功能描述
传感器层	IMU（加速度计/陀螺仪）、GPS（位置/速度）、气压计（高度）、磁力计（航向）
算法层	姿态解算（Mahony滤波）、位置估计（卡尔曼滤波）、控制算法（串级PID）
控制层	姿态控制（Roll/Pitch/Yaw）、高度控制、位置控制（X/Y/Z）、导航逻辑
执行器层	电机混控（X型布局）、电调驱动（DShot协议）、舵机控制（可选）
通信层	遥控器（SBUS）、数传（SiK Radio）、地面站（Mavlink协议）

三、核心算法与实现

3.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 q0 = q[0], q1 = q[1], q2 = q[2], q3 = q[3]; // 四元数
    float norm;
    float hx, hy, bx, bz;
    float vx, vy, vz, wx, wy, wz;
    float ex, ey, ez;
    float integralFBx = 0.0f, integralFBy = 0.0f, integralFBz = 0.0f; // 积分项

    // 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. 计算误差向量（传感器测量值与参考向量的叉积）
    wx = 2*bx*(0.5 - q2*q2 - q3*q3) + 2*bz*(q1*q3 - q0*q2);
    wy = 2*bx*(q1*q3 + q0*q2) + 2*bz*(0.5 - q1*q1 - q3*q3);
    wz = 2*bx*(q2*q3 - q0*q1) + 2*bz*(q0*q2 + q1*q3);
    ex = (ay*vz - az*vy) + (my*wz - mz*wy);
    ey = (az*vx - ax*vz) + (mz*wx - mx*wz);
    ez = (ax*vy - ay*vx) + (mx*wy - my*wx);

    // 4. PI控制器修正陀螺仪漂移
    float ki = 0.01f, kp = 0.1f; // 积分/比例系数
    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;
}

3.2 串级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;
    // 姿态调整量（差速控制）
    float m1 = base - roll_out + pitch_out + yaw_out;  // 前右电机
    float m2 = base + roll_out + pitch_out - yaw_out;  // 前左电机
    float m3 = base + roll_out - pitch_out + yaw_out;  // 后左电机
    float m4 = base - 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（DShot600协议）
    esc_write(0, m1 * 20.47);  // DShot值=油门*20.47（0~2047）
    esc_write(1, m2 * 20.47);
    esc_write(2, m3 * 20.47);
    esc_write(3, m4 * 20.47);
}

3.3 导航与路径规划

功能：实现自主飞行（如定点悬停、航点飞行、跟随模式）。

核心逻辑：

• 定点悬停：通过GPS/气压计获取位置/高度，与目标值比较，输出姿态角调整量；
• 航点飞行：按预设坐标点（Waypoints）飞行，通过PID控制位置误差；
• 跟随模式：基于UWB/视觉模块跟踪目标，动态调整飞行方向。

航点飞行示例代码：

// 航点结构体
typedef struct {
    float x, y, z;  // 目标位置（ENU坐标系）
    float yaw;      // 目标航向
} Waypoint;

// 航点飞行控制
void waypoint_navigation(Waypoint *wp, float current_x, float current_y, float current_z) {
    // 位置误差
    float err_x = wp->x - current_x;
    float err_y = wp->y - current_y;
    float err_z = wp->z - current_z;
    // 位置环PID（输出期望姿态角）
    float target_roll = pid_update(&pid_pos_x, 0, err_x, dt);
    float target_pitch = pid_update(&pid_pos_y, 0, err_y, dt);
    float target_yaw = pid_update(&pid_yaw, wp->yaw, current_yaw, dt);
    // 高度环PID（输出期望油门）
    float target_throttle = pid_update(&pid_alt, wp->z, current_z, dt);
    // 调用姿态控制
    attitude_control(target_roll, target_pitch, target_yaw, target_throttle);
}

四、主程序流程（FreeRTOS任务调度）

graph TD
    A[系统初始化] --> B[传感器校准]
    B --> C[创建FreeRTOS任务]
    C --> D[任务1: 传感器读取 100Hz]
    D --> E[任务2: 姿态解算 100Hz]
    E --> F[任务3: 导航控制 200Hz]
    F --> G[任务4: PID计算 200Hz]
    G --> H[任务5: 电机控制 400Hz]
    H --> I[任务6: 通信与监控 50Hz]
    I -->|异常| J[安全保护 紧急降落/返航]

关键任务代码示例：

// 传感器读取任务（100Hz）
void sensor_task(void *pvParameters) {
    while (1) {
        imu_read(&ax, &ay, &az, &gx, &gy, &gz);  // 读取IMU
        gps_read(&lat, &lon, &alt, &vx, &vy, &vz); // 读取GPS
        baro_read(&pressure, &temperature, &alt_baro); // 读取气压计
        vTaskDelay(10); // 10ms周期（100Hz）
    }
}

// 电机控制任务（400Hz）
void motor_task(void *pvParameters) {
    while (1) {
        // 1. 获取姿态/位置误差
        float roll_err = target_roll - roll;
        float pitch_err = target_pitch - pitch;
        // 2. 串级PID计算
        float roll_rate = pid_update(&pid_roll_angle, roll_err, roll, dt);
        float roll_out = pid_update(&pid_roll_rate, roll_rate, gx, dt);
        // 同理计算pitch_out, yaw_out, throttle
        // 3. 电机混控
        motor_mixing(roll_out, pitch_out, yaw_out, throttle);
        vTaskDelay(2.5); // 2.5ms周期（400Hz）
    }
}

参考代码 四旋翼无人机飞行程序设计 www.youwenfan.com/contentcss/56673.html

五、安全机制与异常处理

5.1 关键保护功能

异常类型	处理措施
低电压	触发报警，自动返航或降落（电压<10.5V时切断非必要负载）
失控	检测遥控器信号丢失（>1s），触发自动返航（RTH）或悬停
传感器故障	磁力计失效时切换至航向角速率模式，IMU故障时紧急降落
碰撞预警	超声波/视觉避障模块检测到障碍物，自动悬停或绕行

5.2 紧急降落代码

void emergency_landing(void) {
    float throttle = current_throttle;
    while (alt_baro > 0.5f) { // 高度>0.5m时持续降落
        throttle -= 5.0f; // 缓慢减小油门
        motor_mixing(0, 0, 0, throttle); // 保持姿态水平
        HAL_Delay(100);
    }
    motor_mixing(0, 0, 0, 0); // 落地后关闭电机
}

六、测试与调试

6.1 地面测试

• 传感器校准：水平静止校准加速度计，旋转360°校准磁力计，静态测试陀螺仪零偏；
• 电机测试：逐个解锁电机，检查转向（X型布局：前右/后左顺时针，前左/后右逆时针）；
• PID调参：手动模式下逐步调整P/I/D系数（如先调角度环P，再调角速度环P/I）。

6.2 试飞调试

• 悬停测试：半自主模式下观察高度/位置稳定性，调整高度环PID；
• 航线测试：上传航点，测试航向/速度跟踪精度，优化位置环参数；
• 抗风测试：在微风环境下验证姿态恢复能力，必要时增加前馈补偿。

七、扩展功能

1. 视觉导航：添加OV5640摄像头，通过光流算法实现室内定位；
2. 集群控制：多机通过数传组网，实现编队飞行（TDMA协议避免冲突）；
3. 自动充电：集成红外定位充电桩，低电量时自主返航充电；

八、总结

四旋翼无人机飞行程序的核心是“感知-决策-控制”闭环，通过多传感器融合获取状态，串级PID实现精准控制，结合导航算法完成自主飞行。本设计基于STM32和FreeRTOS，模块化架构便于扩展，关键算法（Mahony滤波、串级PID）已在嵌入式平台验证可行。