无人机双环 PID 悬停控制方案详解

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;
    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(&current_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. 调试与优化建议

1. 参数整定顺序：
   • 先调姿态环，再调位置环。
   • 位置环的 Kp 从小开始，逐步增加，避免振荡。
2. 传感器数据滤波：
   • GPS 数据噪声大，建议使用卡尔曼滤波或滑动平均。
   • 光流数据需结合高度信息进行缩放。
3. 安全保护：
   • 加入位置超范围保护（如偏离悬停点过远时自动返航）。
   • 低电压保护，避免失控。
4. 仿真验证：
   • 先用 MATLAB/Simulink 搭建仿真模型，验证位置控制逻辑。
5. 实际测试：
   • 先在室内用光流 + 超声波测试悬停，再到室外用 GPS 测试。