第 9 章：PID 电机速度闭环控制

9.1 知识要点

• 离散 PID 算法的实现（增量式积分，积分限幅，死区）
• 一阶低通滤波器消除编码器速度噪声
• PID 输出到 ESC 脉宽的映射与钳位
• 串口命令设置目标速度的实时调试方法

9.2 课程内容

本章将编码器测速（第 6 章）和 ESC PWM 控制（第 3 章）结合，实现电机速度的 PID 闭环控制。控制周期为 20 ms，目标速度通过 USB CDC 串口命令设置，实际速度经一阶低通滤波后作为 PID 反馈。

9.3 基础学习

离散 PID 算法

位置式 PID 公式：

e(k) = target - measured
u(k) = Kp × e(k) + Ki × Σe(k)×dt + Kd × (e(k) - e(k-1)) / dt

实现要点：

• 积分限幅：防止积分饱和（windup），限制 integral 的绝对值
• 死区：误差绝对值小于 deadband 时不累积积分，消除静态抖动
• 输出钳位：将 PID 输出限制在 ESC 可接受范围内

一阶低通滤波

编码器速度在低速时噪声较大，使用一阶低通滤波平滑：

filtered = α × raw + (1 - α) × filtered_prev

α = 0.15（OSRCORE 默认值），α 越小滤波越强但响应越慢。

PID 参数（OSRCORE 默认值）

Kp = 447.0   Ki = 4.7   Kd = 47.0
积分限幅 = 1000.0   死区 = 0.05 m/s

9.4 程序学习

PID 结构体与计算函数：

typedef struct {
    float kp, ki, kd;
    float max_integral, deadband;
    float integral, last_error;
} pid_t;

float pid_calc(pid_t *p, float target, float measured, float dt)
{
    float err = target - measured;
    if (fabsf(err) > p->deadband) {
        p->integral += err * dt;
        if (p->integral >  p->max_integral) p->integral =  p->max_integral;
        if (p->integral < -p->max_integral) p->integral = -p->max_integral;
    }
    float deriv = (err - p->last_error) / dt;
    p->last_error = err;
    return p->kp * err + p->ki * p->integral + p->kd * deriv;
}

控制任务（20 ms 周期）：

static void task_control(void *arg)
{
    pid_t pid;
    pid_init(&pid, 447.0f, 4.7f, 47.0f, 1000.0f, 0.05f);
    int32_t last_count = encoder_count();
    float filtered_speed = 0.0f;

    while (1) {
        vTaskDelay(pdMS_TO_TICKS(20));

        int32_t delta = encoder_count() - last_count;
        last_count = encoder_count();

        float raw_speed = delta * DIST_PER_PULSE / 0.020f;
        filtered_speed  = 0.15f * raw_speed + 0.85f * filtered_speed;

        float out = pid_calc(&pid, s_target_speed, filtered_speed, 0.020f);
        int32_t pulse = 1500 + (int32_t)out;
        // 钳位到 [1000, 2000]
        if (pulse < 1000) pulse = 1000;
        if (pulse > 2000) pulse = 2000;
        esc_set((uint32_t)pulse);
    }
}

9.5 课程总结

本章实现了完整的速度 PID 闭环控制，掌握了积分限幅、死区和低通滤波等工程实践技巧。PID 参数调整方法：先增大 Kp 直到出现轻微振荡，再加 Ki 消除稳态误差，最后加 Kd 抑制超调。

---