当前位置：首页 > news >正文

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

news 2026/5/28 3:52:56

一、系统概述

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

二、飞行程序核心架构

2.1 分层设计

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 TDA[系统初始化] --> 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);  // 读取IMUgps_read(&lat, &lon, &alt, &vx, &vy, &vz); // 读取GPSbaro_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/contentcnt/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); // 落地后关闭电机
}