目录
1. 无人机悬停控制系统架构
2. 位置 PID 控制器设计
PID 结构体（扩展到位置控制）
3. 位置控制与姿态控制结合
位置数据结构
位置环 PID 初始化
4. 位置控制循环
5. 完整控制流程（位置 + 姿态 + 电机）
6. 传感器数据融合（GPS / 光流）
GPS 数据读取示例
光流数据读取示例
7. 调试与优化建议
我们在之前的姿态控制 PID基础上，增加位置控制 PID层，这样无人机就可以根据GPS或光流模块提供的位置信息，实现精准悬停。
我会给你一个完整的位置 + 姿态双环 PID 控制方案，包括：
- 系统架构（位置环 → 姿态环 → 电机混控）
- 位置 PID 设计
- 传感器数据融合（GPS / 光流 → 位置估计）
- 代码实现（STM32 HAL 库）
- 调试与优化建议
1. 无人机悬停控制系统架构
悬停控制是一个双环 PID 结构：
┌─────────────┐ ┌─────────────┐ ┌─────────────┐ ┌─────────────┐ │ 位置环 PID │ │ 姿态环 PID │ │ 角速度环PID │ │ 电机混控 │ │ (X/Y/Z) │ │ (Roll/Pitch/Yaw) │ │ (ωx/ωy/ωz) │ │ (Motor1~4) │ └──────┬──────┘ └──────┬──────┘ └──────┬──────┘ └──────┬──────┘ │ │ │ │ ▼ ▼ ▼ ▼ ┌─────────────┐ ┌─────────────┐ ┌─────────────┐ ┌─────────────┐ │ 期望位置 │ │ 期望姿态角 │ │ 期望角速度 │ │ PWM信号输出 │ └─────────────┘ └─────────────┘ └─────────────┘ └─────────────┘- 位置环：输入期望位置（如悬停点）和实际位置（来自 GPS / 光流），输出期望姿态角（Roll/Pitch）和期望高度（油门修正）。
- 姿态环：输入期望姿态角和实际姿态角（来自 MPU-9250），输出期望角速度。
- 角速度环：输入期望角速度和实际角速度（来自陀螺仪），输出电机控制量。
- 电机混控：结合油门和姿态控制量，计算四个电机的 PWM 值。
2. 位置 PID 控制器设计
位置环需要对X、Y、Z三个方向分别控制：
- X/Y 轴：控制无人机在水平面上的位置，输出期望的横滚角（Roll）和俯仰角（Pitch）。
- Z 轴：控制无人机的高度，输出油门修正量。
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; } 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. 位置控制与姿态控制结合
位置数据结构
typedef struct { float x; float y; float z; } Position; Position target_pos; // 期望位置 Position current_pos; // 当前位置（来自GPS/光流）位置环 PID 初始化
PID_Controller pid_pos_x, pid_pos_y, pid_pos_z; void Position_PID_Init(void) { // X轴位置PID（输出期望Roll角） PID_Init(&pid_pos_x, 2.0f, 0.1f, 0.05f, 50.0f, 15.0f); // 输出限幅±15° // Y轴位置PID（输出期望Pitch角） PID_Init(&pid_pos_y, 2.0f, 0.1f, 0.05f, 50.0f, 15.0f); // Z轴位置PID（输出油门修正量） PID_Init(&pid_pos_z, 5.0f, 0.2f, 0.1f, 100.0f, 200.0f); // 输出限幅±200us }4. 位置控制循环
void Position_Control_Loop(float dt) { // 更新位置环PID pid_pos_x.setpoint = target_pos.x; pid_pos_x.feedback = current_pos.x; PID_Update(&pid_pos_x, dt); pid_pos_y.setpoint = target_pos.y; pid_pos_y.feedback = current_pos.y; PID_Update(&pid_pos_y, dt); pid_pos_z.setpoint = target_pos.z; pid_pos_z.feedback = current_pos.z; PID_Update(&pid_pos_z, dt); // 位置环输出作为姿态环输入 pid_roll.setpoint = pid_pos_y.output; // Y轴位置误差→Roll角 pid_pitch.setpoint = pid_pos_x.output; // X轴位置误差→Pitch角 pid_yaw.setpoint = 0.0f; // 悬停时航向角保持0° // 油门修正 throttle = 1500.0f + pid_pos_z.output; // 基础油门1500us + Z轴修正 throttle = (throttle < 1000) ? 1000 : (throttle > 2000) ? 2000 : throttle; }5. 完整控制流程（位置 + 姿态 + 电机）
void Drone_Hover_Loop(void) { float dt = 0.01f; // 100Hz控制频率 // 1. 读取传感器数据 read_mpu9250_data(&euler, gyro); // 姿态角和角速度 read_position_sensor(¤t_pos); // GPS/光流位置 // 2. 位置环控制 Position_Control_Loop(dt); // 3. 姿态环控制 pid_roll.feedback = euler.roll; pid_pitch.feedback = euler.pitch; pid_yaw.feedback = euler.yaw; PID_Update(&pid_roll, dt); PID_Update(&pid_pitch, dt); PID_Update(&pid_yaw, dt); // 4. 角速度环控制 pid_rate_roll.setpoint = pid_roll.output; pid_rate_pitch.setpoint = pid_pitch.output; pid_rate_yaw.setpoint = pid_yaw.output; pid_rate_roll.feedback = gyro[0]; pid_rate_pitch.feedback = gyro[1]; pid_rate_yaw.feedback = gyro[2]; PID_Update(&pid_rate_roll, dt); PID_Update(&pid_rate_pitch, dt); PID_Update(&pid_rate_yaw, dt); // 5. 电机混控 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; // 6. 输出到电机 __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); }6. 传感器数据融合（GPS / 光流）
GPS 数据读取示例
void read_gps_data(Position *pos) { // 假设GPS模块通过串口输出NMEA数据 // 解析$GPGGA或$GPRMC帧，提取经纬度和高度 // 转换为局部坐标系（如ENU坐标系） pos->x = gps_lon_to_x(gps_lon); pos->y = gps_lat_to_y(gps_lat); pos->z = gps_alt; }光流数据读取示例
void read_optical_flow_data(Position *pos) { // 读取光流模块的X/Y位移和高度数据 pos->x += optical_flow_x * dt; pos->y += optical_flow_y * dt; pos->z = sonar_alt; // 超声波或气压计高度 }7. 调试与优化建议
- 参数整定顺序：
- 先调姿态环，再调位置环。
- 位置环的 Kp 从小开始，逐步增加，避免振荡。
- 传感器数据滤波：
- GPS 数据噪声大，建议使用卡尔曼滤波或滑动平均。
- 光流数据需结合高度信息进行缩放。
- 安全保护：
- 加入位置超范围保护（如偏离悬停点过远时自动返航）。
- 低电压保护，避免失控。
- 仿真验证：
- 先用 MATLAB/Simulink 搭建仿真模型，验证位置控制逻辑。
- 实际测试：
- 先在室内用光流 + 超声波测试悬停，再到室外用 GPS 测试。
✅ 我已经给你一个完整的位置 + 姿态双环 PID 悬停控制方案，可以直接在 STM32 上实现，支持 GPS 或光流模块。
如果你需要的话，我可以帮你加上卡尔曼滤波（EKF）融合 GPS 和光流数据，这样位置估计会更稳定，尤其在 GPS 信号弱的环境下。
