4、计算力矩控制：基本原理、前馈+反馈结构、PD+前馈控制器设计
各位同学，今天我们来聊聊力矩模式下的轨迹跟踪。说实话，这个主题在机器人控制里算是「硬骨头」，但也是最能体现控制功底的地方。我个人习惯把计算力矩控制看作「给机器人装一个智能弹簧」——既要让它有力气，又要让它听话。
4.1 基本原理：从动力学说起
先问大家一个问题：为什么PID控制搞不定高速高精度的轨迹跟踪？
原因很简单——机器人是个强耦合、非线性的系统。你动一下肩关节，肘关节的受力就变了。传统的PID是「头痛医头，脚痛医脚」，它不知道关节之间的相互影响。
计算力矩控制的核心思想，就是利用机器人的动力学模型，把非线性系统「假装」成线性系统。说白了，就是先算出来「需要多大的力才能让机器人动」，然后把这个力作为前馈加上去。
动力学方程长这样：
M(q) * q̈ + C(q, q̇) * q̇ + G(q) + τ_friction = τ其中：
- M(q) —— 惯性矩阵，跟关节位置有关
- C(q, q̇) —— 科里奥利力和离心力项
- G(q) —— 重力项
- τ_friction —— 摩擦力（这个最烦人）
我在项目中遇到过一个问题：明明动力学模型算得很准，但实际跑起来就是抖。后来发现是摩擦力模型没建好。嗯，这里要注意，摩擦力是「非线性中的非线性」，千万别小看它。
4.2 前馈+反馈结构：两条腿走路
计算力矩控制的结构，我习惯用一张图来说明：
你看，这个结构其实很直观：
- 前馈通道（红色）：根据期望轨迹提前算出力，让机器人「知道该往哪走」
- 反馈通道（绿色）：根据实际误差修正，让机器人「走得更准」
为什么要两条腿走路？我打个比方：前馈就像你开车时看路标提前打方向盘，反馈就像你发现车偏了再微调。光看路标不看路，你会撞；光看路不看路标，你永远在纠正。
我的经验：前馈能解决80%的跟踪误差，反馈解决剩下的20%。但千万别把反馈增益调太大，否则系统会抖。我曾经在一个六轴机器人上把PD增益调高了30%，结果末端抖得像筛糠一样。
4.3 PD+前馈控制器设计
好了，理论讲完了，咱们来点实际的。PD+前馈控制器，说白了就是：
τ = M(q) * (q̈_d + Kp * e + Kd * ė) + C(q, q̇) * q̇ + G(q)其中：
- q̈_d —— 期望加速度（前馈项）
- e = q_d - q —— 位置误差
- ė = q̇_d - q̇ —— 速度误差
- Kp, Kd —— PD增益矩阵
你想想看，这个公式其实很聪明：前馈项让机器人「知道该用多大力」，PD项让机器人「知道该怎么修正」。
设计步骤
- 辨识动力学参数：M(q)、C(q,q̇)、G(q) 这些参数得先测出来。我一般用最小二乘法做离线辨识。
- 设计PD增益：先让Kp和Kd小一点，慢慢往上调。Kp决定响应速度，Kd决定阻尼。
- 加入前馈：把期望加速度项加进去，观察跟踪误差是否减小。
- 调试摩擦补偿：这是最磨人的一步。我建议用Stribeck模型，比简单的库仑+粘滞模型准得多。
关键参数调优建议：
| 参数 | 作用 | 调大后果 | 调小后果 |
|---|---|---|---|
| Kp | 位置刚度 | 响应快，但可能震荡 | 跟踪慢，误差大 |
| Kd | 阻尼 | 系统稳定，但响应变慢 | 容易震荡 |
| 前馈增益 | 模型补偿 | 过补偿导致超调 | 欠补偿，跟踪差 |
代码示例（伪代码）
// PD+前馈控制器的核心实现
void computeTorqueControl() {
// 1. 获取当前状态
q = getJointPosition();
q_dot = getJointVelocity();
// 2. 计算误差
e = q_desired - q;
e_dot = q_dot_desired - q_dot;
// 3. 计算PD项
tau_pd = Kp * e + Kd * e_dot;
// 4. 计算前馈项（动力学模型）
M = computeInertiaMatrix(q);
C = computeCoriolisMatrix(q, q_dot);
G = computeGravityVector(q);
tau_ff = M * q_ddot_desired + C * q_dot + G;
// 5. 总力矩 = 前馈 + 反馈
tau = tau_ff + tau_pd;
// 6. 限幅保护
tau = saturate(tau, TAU_MIN, TAU_MAX);
// 7. 输出到电机
setMotorTorque(tau);
}
注意：千万不要在真实机器人上直接跑未经仿真的代码！我曾经有一次把动力学参数搞反了，结果机器人一启动就「飞」了出去，差点撞到人。先在仿真环境里跑三天，确认没问题再上真机。
4.4 避坑指南
做计算力矩控制，有几个坑我踩过，跟大家分享一下：
- 模型不准怎么办？—— 别追求完美模型，用鲁棒控制的思想，让反馈去弥补模型误差。
- 摩擦力怎么处理？—— 我建议用前馈补偿+反馈修正的组合。前馈补偿掉80%的摩擦力，剩下的让PD去处理。
- 采样频率要多少？—— 至少1kHz，我习惯用2kHz。频率太低，前馈就失去了意义。
- 关节柔性怎么办？—— 如果机器人有减速器（谐波减速器尤其明显），要考虑关节柔性。这时候需要加一个陷波滤波器。
说实话，计算力矩控制是个「越做越有意思」的领域。刚开始你可能觉得动力学模型好复杂，但当你看到机器人真的按照期望轨迹平滑运动时，那种成就感是PID给不了的。
好了，这一章的内容就到这里。记住：前馈让你走得快，反馈让你走得稳，两者缺一不可。