四旋翼PID姿态控制实战指南-编程实验室

1. 无人机姿态控制系统架构

2. PID 控制器设计

PID 结构体与初始化

PID 更新函数

3. 电机混控（四旋翼 X 模式）

4. 姿态角到电机控制的完整流程

5. 示例代码（STM32 HAL 库）

6. 调试与优化建议

将 MPU-9250 解算出的欧拉角（Pitch、Roll、Yaw）作为反馈，输入到PID 控制器，输出控制信号到四个电机，实现姿态稳定控制。

我会给你一个模块化的实现方案，包括：

系统控制架构
PID 控制器设计
电机混控（Motor Mixing）
姿态数据与 PID 输出的结合
示例代码（STM32/CH32 + HAL 库）

1. 无人机姿态控制系统架构

典型四旋翼无人机的姿态控制是一个级联 PID 结构：

┌─────────────┐ ┌─────────────┐ ┌─────────────┐ │ 姿态环 PID │ │ 角速度环PID │ │ 电机混控 │ │ (Pitch/Roll/Yaw) │ │ (ωx/ωy/ωz) │ │ (Motor1~4) │ └──────┬──────┘ └──────┬──────┘ └──────┬──────┘ │ │ │ ▼ ▼ ▼ ┌─────────────┐ ┌─────────────┐ ┌─────────────┐ │ 期望姿态角 │ │ 期望角速度 │ │ PWM信号输出 │ └─────────────┘ └─────────────┘ └─────────────┘

姿态环：输入期望欧拉角（如悬停时为 0°）和实际欧拉角（来自 MPU-9250 DMP），输出期望角速度。
角速度环：输入期望角速度和实际角速度（来自陀螺仪），输出电机控制量。
电机混控：将姿态控制量映射到四个电机的 PWM 信号。

2. PID 控制器设计

PID 公式：u(t)=Kp⋅e(t)+Ki⋅∫0te(τ)dτ+Kd⋅dtde(t)其中：

e(t)=SetPoint−Feedback （误差）
Kp,Ki,Kd 分别为比例、积分、微分系数
输出限幅防止积分饱和和电机超调

PID 结构体与初始化

typedef struct { float Kp, Ki, Kd; float setpoint; float feedback; float error; float integral; float derivative; float prev_error; float output; float integral_limit; // 积分限幅 float output_limit; // 输出限幅 } PID_Controller; void PID_Init(PID_Controller *pid, float Kp, float Ki, float Kd, float integral_limit, float output_limit) { pid->Kp = Kp; pid->Ki = Ki; pid->Kd = Kd; pid->integral_limit = integral_limit; pid->output_limit = output_limit; pid->setpoint = 0.0f; pid->feedback = 0.0f; pid->error = 0.0f; pid->integral = 0.0f; pid->derivative = 0.0f; pid->prev_error = 0.0f; pid->output = 0.0f; }

PID 更新函数

void PID_Update(PID_Controller *pid, float dt) { pid->error = pid->setpoint - pid->feedback; // 比例项 float P = pid->Kp * pid->error; // 积分项（带限幅） pid->integral += pid->error * dt; if (pid->integral > pid->integral_limit) pid->integral = pid->integral_limit; if (pid->integral < -pid->integral_limit) pid->integral = -pid->integral_limit; float I = pid->Ki * pid->integral; // 微分项 pid->derivative = (pid->error - pid->prev_error) / dt; float D = pid->Kd * pid->derivative; // 总输出 pid->output = P + I + D; // 输出限幅 if (pid->output > pid->output_limit) pid->output = pid->output_limit; if (pid->output < -pid->output_limit) pid->output = -pid->output_limit; pid->prev_error = pid->error; }

3. 电机混控（四旋翼 X 模式）

四旋翼 X 模式的电机布局：

Motor2 (CCW) ↑ | Motor1 (CW) ←———→ Motor3 (CW) | ↓ Motor4 (CCW)

混控公式（简化版）：Motor1=Throttle+Roll−Pitch−YawMotor2=Throttle−Roll−Pitch+YawMotor3=Throttle−Roll+Pitch−YawMotor4=Throttle+Roll+Pitch+Yaw

其中：
Throttle：油门（总升力）
Roll、Pitch、Yaw：PID 输出的姿态控制量

4. 姿态角到电机控制的完整流程

读取 MPU-9250 姿态数据（Pitch, Roll, Yaw）和陀螺仪数据（ωx, ωy, ωz）。
姿态环 PID：输入期望姿态角（如悬停时为 0°）和实际姿态角，输出期望角速度。
角速度环 PID：输入期望角速度和实际角速度，输出姿态控制量（Roll, Pitch, Yaw）。
电机混控：结合油门和姿态控制量，计算四个电机的 PWM 值。
输出到电调：通过定时器 PWM 输出控制电机转速。

5. 示例代码（STM32 HAL 库）

#include "stm32f1xx_hal.h" #include "mpu9250.h" #include "pid.h" // 定义 PID 控制器 PID_Controller pid_pitch, pid_roll, pid_yaw; PID_Controller pid_rate_pitch, pid_rate_roll, pid_rate_yaw; // 姿态数据 EulerAngles euler; float gyro[3]; // ωx, ωy, ωz (°/s) // 电机 PWM 值（1000~2000us） uint16_t motor1, motor2, motor3, motor4; float throttle = 1000.0f; // 初始油门 // 初始化 void Drone_Init(void) { // 初始化 MPU-9250 mpu_init(); // 初始化姿态环 PID（角度控制） PID_Init(&pid_pitch, 5.0f, 0.1f, 0.2f, 100.0f, 500.0f); PID_Init(&pid_roll, 5.0f, 0.1f, 0.2f, 100.0f, 500.0f); PID_Init(&pid_yaw, 4.0f, 0.05f, 0.1f, 100.0f, 500.0f); // 初始化角速度环 PID（速率控制） PID_Init(&pid_rate_pitch, 2.0f, 0.05f, 0.1f, 100.0f, 500.0f); PID_Init(&pid_rate_roll, 2.0f, 0.05f, 0.1f, 100.0f, 500.0f); PID_Init(&pid_rate_yaw, 1.5f, 0.05f, 0.1f, 100.0f, 500.0f); // 初始化定时器 PWM（如 TIM1 CH1~CH4） HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1); HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_2); HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_3); HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_4); } // 控制循环（100Hz） void Drone_Control_Loop(void) { float dt = 0.01f; // 10ms // 读取姿态和陀螺仪数据 read_dmp_data(&euler, gyro); // 姿态环：输入期望角度（此处为 0° 悬停） pid_pitch.setpoint = 0.0f; pid_roll.setpoint = 0.0f; pid_yaw.setpoint = 0.0f; pid_pitch.feedback = euler.pitch; pid_roll.feedback = euler.roll; pid_yaw.feedback = euler.yaw; PID_Update(&pid_pitch, dt); PID_Update(&pid_roll, dt); PID_Update(&pid_yaw, dt); // 角速度环：输入姿态环输出作为期望角速度 pid_rate_pitch.setpoint = pid_pitch.output; pid_rate_roll.setpoint = pid_roll.output; pid_rate_yaw.setpoint = pid_yaw.output; pid_rate_pitch.feedback = gyro[0]; pid_rate_roll.feedback = gyro[1]; pid_rate_yaw.feedback = gyro[2]; PID_Update(&pid_rate_pitch, dt); PID_Update(&pid_rate_roll, dt); PID_Update(&pid_rate_yaw, dt); // 电机混控 motor1 = throttle + pid_rate_roll.output - pid_rate_pitch.output - pid_rate_yaw.output; motor2 = throttle - pid_rate_roll.output - pid_rate_pitch.output + pid_rate_yaw.output; motor3 = throttle - pid_rate_roll.output + pid_rate_pitch.output - pid_rate_yaw.output; motor4 = throttle + pid_rate_roll.output + pid_rate_pitch.output + pid_rate_yaw.output; // 限幅 motor1 = (motor1 < 1000) ? 1000 : (motor1 > 2000) ? 2000 : motor1; motor2 = (motor2 < 1000) ? 1000 : (motor2 > 2000) ? 2000 : motor2; motor3 = (motor3 < 1000) ? 1000 : (motor3 > 2000) ? 2000 : motor3; motor4 = (motor4 < 1000) ? 1000 : (motor4 > 2000) ? 2000 : motor4; // 输出到电机 __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, motor1); __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, motor2); __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, motor3); __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_4, motor4); }