03｜工程增强 PID：前馈、滤波、抗饱和与级联控制

基础 PID 能让速度跟随目标，但面对大阶跃、执行器饱和和位置控制时还不够稳健。本节进一步加入前馈、反馈滤波、抗积分饱和和位置—速度级联，并为两个电机分别保存控制状态。

本篇目标与整体结构

目标位置不会直接变成 PWM。位置环先生成目标转速，速度环再根据编码器转速生成受限输出：

目标位置 - 实际位置 → 位置环 → 目标 RPM
目标 RPM - 实际 RPM → 前馈 + PID → 受限电机输出

这种分层让调试更可控：先让速度环稳定跟踪，再让较慢的位置环改变速度目标。当前代码没有给出两个环的实测带宽，因此工程上必须按“先内环、后外环”的顺序验证。

参数与状态如何组织

PID_Param_Init() 写入位置环和速度环参数。速度环还通过 speed_pid[MOTOR_COUNT] 为每个电机保存积分、滤波反馈、上次目标、上次误差和初始化标志。

参数表中的数值依赖 PID.h 里的周期、限幅和死区宏。若缺少这些定义，就无法复现采样周期和执行器范围，因此不能直接照搬参数。

SpeedPID_Reset() 会从全局参数重新装载 Kp/Ki/Kd 并清空动态状态。SpeedPID_ResetAll() 对每个电机执行同样操作，适合上电、故障恢复或控制模式切换。

位置环：把距离误差变成速度目标

LocationRing_PID_Realize() 计算位置误差、积分和误差差分，然后把输出限制在 MOTOR_SPEED_MAX。

位置环输出的物理意义被设计为目标速度，而不是直接 PWM。要让这个意义成立，位置反馈和速度上限必须使用一致单位，并先完成编码器脉冲到位置的换算。

位置环没有展示条件积分或输出饱和回算。若目标距离很大，输出长期被速度上限夹住，积分仍可能继续累计。工程接入时应单独验证外环的积分行为。

速度环前馈

速度环输出首先包含目标相关的前馈：

output = Kf * target_rpm
       + Kp * error
       + Ki * integral
       + Kd * derivative;

前馈根据目标直接给出基础驱动力，反馈负责修正电池电压、负载和模型误差。它能减少“必须先产生误差才能推动车轮”的迟滞。

Kf 不是通用常量。应通过多组稳态目标记录所需输出，再拟合目标转速与基础输出的关系；换电机、供电或负载后需要重新验证。

反馈低通与微分先行

首次调用时，代码直接用当前转速初始化滤波状态。后续采用：

filtered += alpha * (actual_rpm - filtered);

微分项不是对误差求差分，而是对测量值求负导数：

derivative = -(filtered_actual - last_actual) / dt;

这属于微分先行。设定值突然变化时不会直接造成微分冲击，但编码器噪声、滤波滞后和任务周期抖动仍会影响微分结果。

dt 来自 PID_COMPUTATION_PERIOD。这比把周期完全隐含在参数中更清晰，但前提是实际调度周期确实与宏一致。

死区与方向切换

目标从明显正值跨到明显负值，或反向切换时，代码会清空积分和输出，防止旧方向状态继续推动电机。

当目标接近零且滤波后的实际转速也落入死区，函数直接清零并返回。误差本身也有死区，小误差会被改为零，以避免输出在零点附近来回抖动。

死区能改善静止稳定性，也可能留下稳态误差。阈值应从编码器量化噪声、机械静摩擦和真实控制目标中确定。

条件积分与抗饱和

代码先生成候选积分，只在误差位于活动范围内时更新，并对积分状态限幅：

if (fabs(error) <= integral_active_range) {
    candidate_integral += error * dt;
    candidate_integral = clamp(candidate_integral,
                               -integral_limit,
                                integral_limit);
}

计算完整输出并限幅后，只有以下情况才接受候选积分：输出未饱和，或当前误差方向有助于把输出拉回允许范围。

这比“无条件积分 + 最后夹输出”更完整。仍需记录饱和标志和积分状态，确认长时间堵转或大阶跃下不会积累异常状态。

双电机状态隔离

motor_index 用于选择每个电机自己的 SpeedPidState_t。索引越界时函数返回零，避免访问数组外内存。

但 VelocityRing_PID_Realize() 只调用左电机索引。要构成双轮速度闭环，调用层必须分别对左右轮执行 SpeedPID_Update()，并传入各自目标和反馈。

接入真实小车前还要确认什么

检查项	当前代码表现	接入要求
速度环结构	包含前馈、低通和测量微分	分别记录各分量，避免只观察最终输出
积分保护	使用条件积分和饱和方向判断	用堵转与大阶跃测试积分是否能及时释放
双电机状态	`speed_pid[]` 隔离两个实例	调用层必须分别传入左右目标与反馈
级联运行	代码给出位置环和速度环	先验证内环，再接外环并控制带宽比例
参数移植	数值依赖周期、限幅和机械负载	在当前车体上重新标定前馈并整定反馈参数

本篇总结

级联控制把位置目标转换为速度目标，再由速度环驱动电机。
前馈提供基础输出，反馈修正模型误差和扰动。
测量微分避免设定值突变引起的微分冲击。
条件积分、积分限幅和饱和方向判断共同构成更可靠的抗饱和逻辑。
这仍是一份待集成源码；完整双轮调用、宏参数和上板性能需要真实工程补证。

管理入口