4、控制参数整定：PID参数对抖动的影响、陷波滤波器设计与应用、加速度前馈补偿技术
控制参数整定，说白了就是给机器人“调手感”。
我见过太多团队，硬件做得挺扎实，一上电就开始抖。不是高频啸叫，就是低频晃悠。最后查来查去，问题出在参数上。这一章，咱们就把PID、陷波滤波器和前馈补偿这三板斧讲透。
4.1 PID参数对抖动的影响
PID三个字母，每个都跟抖动有关系。我习惯把它们拆开看：
- P（比例）：反应快，但太大会让系统“硬碰硬”，产生高频抖动。
- I（积分）：消除静差，但积分饱和会让响应变慢，甚至出现低频振荡。
- D（微分）：预测未来，但噪声敏感。微分项放太大，电机端子能听到滋滋声。
为什么会这样？
你想想看，P增益高了，相当于弹簧变硬。硬弹簧一推就弹回来，来回几次就抖起来了。D增益高了，相当于阻尼器太敏感，连传感器噪声都被放大，反而引入高频振动。
核心原则：先调P，再调I，最后微调D。P给到系统开始轻微振荡，然后退一半。I给到静差消除即可，别贪多。D能不用就不用，实在要用，先加低通滤波。
我在项目中遇到过一台双足机器人，走路时髋关节一直在15Hz左右抖动。一开始以为是结构共振，后来发现是P增益设到了80，降到45之后，抖动立刻消失。嗯，有时候问题就是这么简单。
4.2 陷波滤波器设计与应用
当机械结构存在固有频率，PID怎么调都压不住的时候，就该陷波滤波器上场了。
陷波滤波器，说白了就是一个“定向消除器”。它只干掉某个特定频率的信号，其他频率不受影响。对于机器人关节控制来说，这是处理结构共振的利器。
我常用的二阶数字陷波滤波器传递函数长这样：
H(z) = (1 - 2*cos(ω0)*z^(-1) + z^(-2)) / (1 - 2*r*cos(ω0)*z^(-1) + r^2*z^(-2))
其中：
ω0 = 2*π*f0/fs // 陷波中心频率
r = 0.95~0.99 // 陷波深度控制，越接近1越窄
fs = 采样频率
设计步骤我一般这么走：
- 扫频找共振点：给关节扫频信号，看哪个频率振幅最大。
- 确定陷波频率：f0 就设在这个共振频率上。
- 选择陷波深度：r 从0.98开始试。太深会损失相位裕度，太浅压不住。
- 验证效果：再扫一次频，看共振峰是否被削平。
我的经验：陷波滤波器不要串联太多，一般1~2个就够了。超过3个，相位滞后会明显影响系统稳定性。我曾经在一个六轴机器人上串了4个陷波器，结果低频响应变得一塌糊涂，最后拆掉两个才恢复正常。
注意：陷波滤波器会引入相位滞后。如果陷波频率刚好在系统穿越频率附近，可能导致稳定性下降。建议在加入陷波后重新测量一次开环伯德图。
4.3 加速度前馈补偿技术
前馈补偿，是让机器人“提前知道”要干什么。
PID是事后纠偏——等误差出来了才去调整。前馈是事前预测——根据目标轨迹直接给一个补偿量。两者配合，效果最好。
加速度前馈，专门用来补偿惯性力。人形机器人跑步、跳跃时，关节需要克服巨大的惯性力矩。如果只靠PID，误差会很大，抖动也会跟着来。
公式很简单：
τ_ff = J * q_ddot_des
其中：
τ_ff = 前馈力矩
J = 关节等效惯量（包含电机转子惯量+负载惯量）
q_ddot_des = 目标角加速度
实际实现时，我习惯把前馈加到电流环的输入端：
// 伪代码示例
float computeTorqueCommand(float q_des, float q_dot_des, float q_ddot_des,
float q_cur, float q_dot_cur) {
// PID反馈项
float err_pos = q_des - q_cur;
float err_vel = q_dot_des - q_dot_cur;
float tau_fb = Kp * err_pos + Kd * err_vel;
// 加速度前馈项
float tau_ff = J_eff * q_ddot_des;
// 总力矩指令
return tau_fb + tau_ff;
}
这里有个坑——惯量辨识。J_eff 如果给不准，前馈反而会添乱。我一般用两种方法获取：
| 方法 | 原理 | 精度 | 适用场景 |
|---|---|---|---|
| CAD估算 | 从三维模型直接读取惯量 | 中等 | 静态结构，负载变化小 |
| 系统辨识 | 给激励信号，用最小二乘法拟合 | 高 | 变负载、变姿态场景 |
我曾经在一个人形机器人膝关节上试过，不加前馈时，跑步动作的跟踪误差有5度，加上加速度前馈后，误差降到了0.8度。抖动也明显减轻了。
4.4 三者协同：一个完整的整定流程
PID、陷波滤波器、加速度前馈，不是各自为战，而是协同工作。我总结了一套流程：
- 先做机械检查：确保没有松动、间隙过大等硬件问题。
- 粗调PID：让系统能稳定运行，不追求极致性能。
- 扫频找共振：确定需要抑制的频率点。
- 设计陷波滤波器：削掉共振峰。
- 精调PID：在陷波基础上，适当提高增益。
- 加入加速度前馈：补偿惯性力，减少跟踪误差。
- 整体验证：跑典型工况，确认抖动消除。
下面这张图，是我习惯用的整定逻辑框架：
这套流程我用了很多年，基本没出过大问题。当然，每个机器人都有自己的脾气，参数不可能一次到位。多试几次，多记录数据，慢慢就能找到手感。
一个小技巧：每次改参数之前，先把当前参数组保存下来。我习惯用时间戳命名，比如 pid_20241015_1423.yaml。这样改坏了还能退回去，不至于从头再来。
控制参数整定，说到底就是跟机器人“对话”。你给它一个参数，它回你一个动作。抖了，就松一点；慢了，就紧一点。多听、多看、多试，总能找到那个平衡点。