4. 机器人动力学建模
在机器人力控系统中，位置控制只关心“走到哪里”，而力控必须回答“需要施加多大力、关节要输出多大扭矩”。动力学建模正是连接运动指令与实际关节力矩的桥梁。若模型缺失或参数不准，重力补偿会残留偏差，摩擦力会造成低速爬行，力控环路就会出现持续振荡或响应迟滞。本节从刚体动力学出发，逐步建立关节力矩、重力补偿与摩擦力模型，为后续力控调试提供可计算的物理基础。
一、核心原理：从刚体动力学到关节力矩
机器人动力学描述关节运动与所需力矩之间的关系。对于n自由度串联机器人，标准形式为：
τ = M(q)·q̈ + C(q, q̇)·q̇ + G(q) + F(q̇) + τ_ext
其中：
τ 关节驱动力矩向量
M(q) 惯性矩阵，随位形变化
C(q,q̇) 科氏力与离心力项
G(q) 重力项
F(q̇) 摩擦力项
τ_ext 外部接触力映射到关节的等效力矩惯性项M(q)决定加速时的力矩需求，科氏项在高速多关节联动时不可忽略，重力项G(q)只与当前位形有关，摩擦力项F(q̇)与速度方向强相关。力控调试中，重力与摩擦力是最常被单独补偿的两项，因为它们直接决定“零力拖动”和“恒力接触”的手感与精度。
二、方法流程：参数辨识与补偿实现
工程上通常采用“先结构、后参数、再补偿”的流程：
- 建立结构模型：根据DH参数和连杆质量分布，写出M、C、G的解析形式，或使用牛顿-欧拉递推算法。
- 采集激励轨迹：让机器人执行傅里叶级数或五次多项式轨迹，覆盖各关节速度与加速度范围。
- 参数辨识：将动力学方程线性化为Y(q,q̇,q̈)·θ = τ，用最小二乘法估计惯性参数、质心位置和摩擦系数。
- 重力补偿：实时计算G(q)，以力矩前馈方式叠加到关节电流环，使机器人处于“失重”状态。
- 摩擦力补偿：根据关节速度方向，叠加库仑摩擦与粘滞摩擦项，低速段可加入Stribeck修正。
- 验证与迭代：用零力拖动和恒力接触实验评估残差，修正参数直至力矩误差进入允许范围。
下面给出一个典型的关节力矩前馈补偿代码片段：
// 重力与摩擦力前馈补偿（伪代码）
for each joint i:
tau_gravity[i] = G_i(q)
tau_friction[i] = fc[i] * sign(qd[i]) + fv[i] * qd[i]
tau_cmd[i] = tau_pid[i] + tau_gravity[i] + tau_friction[i]
send_joint_torque(i, tau_cmd[i])三、案例场景：零力拖动与恒力接触
假设一台六轴协作机器人进行恒力打磨。未开启重力补偿时，操作者拖动末端需要克服各连杆自重，手感沉重且松手后下坠。开启重力补偿后，G(q)被实时抵消，拖动只需克服摩擦力和惯性，手感明显轻快。进一步加入摩擦力补偿后，低速拖动不再出现“一顿一顿”的爬行现象。
在恒力接触阶段，机器人末端接触工件，力传感器反馈接触力。若重力补偿存在5%误差，在姿态变化较大的轨迹上，接触力会出现周期性波动。通过辨识得到的重力参数和摩擦参数，可将力波动从±3N降低到±0.5N以内，满足打磨工艺要求。
四、误区与复盘：常见建模陷阱
- 忽略负载变化：末端工具质量变化后未更新动力学参数，导致重力补偿失效。应在更换工具后重新辨识或至少修正负载质心。
- 摩擦力模型过于简单：仅用库仑摩擦无法描述低速Stribeck效应，容易在速度过零时产生极限环振荡。
- 参数辨识激励不足：轨迹速度过低或加速度变化单一，导致惯性参数不可观，辨识结果发散。
- 补偿周期不匹配：重力补偿计算频率低于控制周期，造成力矩更新滞后，力控环路相位裕度下降。
- 忽略关节柔性与传动间隙：高减速比关节中，柔性变形和间隙会引入额外力矩误差，需在模型中增加弹性项或通过实验标定补偿。
复盘时可用下表快速定位问题：
| 现象 | 可能原因 | 排查方向 |
|---|---|---|
| 零力拖动沉重 | 重力补偿未开启或参数错误 | 检查G(q)计算与负载质量 |
| 低速爬行 | 摩擦力补偿不足 | 检查库仑/粘滞系数与速度方向判断 |
| 力控低频振荡 | 补偿周期滞后或增益过高 | 提高补偿频率，降低力环增益 |
| 姿态变化时力波动 | 惯性参数或质心辨识不准 | 重新采集激励轨迹并辨识 |
| 高速联动时力矩偏差 | 科氏项未补偿 | 检查C(q,q̇)计算与实时性 |
动力学建模是力控调试的“地基”。刚体动力学给出力矩骨架，重力补偿消除静态负担，摩擦力建模改善低速品质。三者结合后，力控环路才能在一个可预测、可补偿的物理模型上运行。实际调试中，应坚持“先重力、后摩擦、再惯性”的递进策略，用实验数据反复修正参数，而不是一次性追求完美模型。