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

📂 示例代码：09_pid_motor

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_ctrl_t;

/* 返回 int（pulse_us），内部已加 neutral 并钳位到 [1000, 2000] */
int pid_update(pid_ctrl_t *p, float target, float current,
               int neutral, float dt_sec)
{
    float err = target - current;
    if (fabsf(err) < p->deadband) err = 0.0f;

    p->integral += err * dt_sec;
    if (p->integral >  p->max_integral) p->integral =  p->max_integral;
    if (p->integral < -p->max_integral) p->integral = -p->max_integral;

    float derivative = (err - p->last_error) / dt_sec;
    p->last_error = err;

    float output = p->kp * err + p->ki * p->integral + p->kd * derivative;
    int pulse = neutral + (int)output;
    if (pulse < 1000) pulse = 1000;
    if (pulse > 2000) pulse = 2000;
    return pulse;  // 直接可用的 ESC 脉宽（µs）
}

void pid_reset(pid_ctrl_t *p)
{
    p->integral   = 0.0f;
    p->last_error = 0.0f;
}

pid_update 返回值是可以直接传给 esc_set() 的脉宽（µs），neutral 加法和钳位都在函数内完成，调用方不需要额外处理。目标速度接近零时调用 pid_reset 清除积分，防止重新启动时出现积分饱和冲击。

控制任务（20 ms 周期）：

static void task_control(void *arg)
{
    pid_ctrl_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 target = s_target_speed;

        /* 目标为零时重置积分，防止重启冲击 */
        if (fabsf(target) < 0.05f)
            pid_reset(&pid);

        /* pid_update 直接返回 pulse_us，内含 neutral+clamp */
        int pulse = pid_update(&pid, target, filtered_speed,
                               THROTTLE_NEUTRAL, 0.020f);
        esc_set((uint32_t)pulse);
    }
}

9.5 课程总结

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

---