机器人动力学控制：关键步骤指南

面向有一定机器人基础（了解关节、正运动学、URDF 即可），准备开展动力学参数辨识与基于模型的力矩控制的读者。
本文按工程流水线说明“要做什么、为什么、常见坑、完成标志”，不展开公式推导。

0. 先明确目标：动力学要服务于什么？

目标不同，建模粒度与工程量差很多：

核心公式：

[
\tau = M(q)\ddot q + C(q,\dot q)\dot q + G(q) + \tau_{\text{fric}}
]

等价的对参数线性形式（辨识与实现都围绕它）：

[
\tau = Y(q,\dot q,\ddot q)\,\pi
]

其中 (Y) 只依赖运动，(\pi) 是惯性、摩擦等动力学参数。

工程上真正可稳定辨识的，通常不是完整 (\pi)，而是最小参数集 (\pi_b)（见第 3 步）。

总览：推荐流水线

URDF 模型
  → MDH（修正 DH）参数 + 运动学校验
  → 动力学方程 / 最小参数集 / 回归矩阵 Yb
  → 激励轨迹设计
  → 辨识算法开发与仿真验证
  → 真机数据采集（含滤波与标定）
  → 参数辨识（含约束 / 正则）
  → 动力学参数验证（开环预测 / 闭环控制）
  → 实时动力学计算工程落地
  → （可选）基于模型的控制器集成

1. URDF：几何与惯性的源数据

做什么

• 明确拓扑：连杆、关节、父子关系、关节类型与限位。
• 从 CAD 或厂家数据填写各 link 的质量、质心、惯性张量。
• 统一单位：米、千克、千克·米²；关节角用弧度。

为什么重要

URDF 是运动学、可视化、仿真、DH 提取的共同源头。
动力学辨识不必完全信任其中的 inertial（CAD 常有误差，线缆、电机转子等也未必计入），但它仍然是：

1. 运动学几何的权威描述；
2. 辨识初值与仿真对比的参考；
3. 真机联调与可视化的接口。

常见坑

• 惯性坐标系与 link 坐标系不一致（origin 写错）。
• 串联链中夹杂过多 fixed joint，增加后续 DH 提取难度。
• CAD 惯性与真机差距大——这正是后续辨识存在的理由。
• 未建模电机转子惯量、传动比、减速器损耗。

产出物

• 可复现的机器人描述文件（含关节限位与 inertial）
• 明确的串联链定义：base → tip，以及实际要辨识/控制的自由度

2. MDH 参数：把树状模型变成标准串联链

做什么

从 URDF 提取或手写修正 DH（Craig / MDH）表：

[
T_{i-1}^{i} = R_x(\alpha_{i-1})\,T_x(a_{i-1})\,R_z(\theta_i)\,T_z(d_i)
]

每个关节一行：(\alpha, a, \theta, d)（其中一项为关节变量）。

为什么重要

• 经典动力学（递归牛顿–欧拉、回归矩阵）多建立在 DH/MDH 串联模型上。
• MDH 错了，后面的 (Y)、(\pi_b)、控制都会错，且很难靠辨识“补回来”。

必须做的校验

1. 正运动学对齐：同一组 (q)，MDH 与 URDF（或实测）的末端位姿误差应接近数值噪声量级。
2. 多姿态抽查：零位、限位附近、随机姿态。
3. 可视化对照：关节坐标系与工具点是否一致。

产出物

• 固化的 MDH 参数表
• 校验报告（位置误差、姿态误差）
• 明确记录：关节顺序、重力方向、base 坐标系约定

3. 动力学方程、最小参数集、最小参数矩阵

3.1 完整动力学与回归形式

用递归牛顿–欧拉（RNE）或拉格朗日法得到：

[
\tau = Y(q,\dot q,\ddot q)\,\pi
]

每连杆常见的一组参数（重心参数 / barycentric parameters）大致为：

[
(L_{xx}, L_{xy}, L_{xz}, L_{yy}, L_{yz}, L_{zz},\; l_x, l_y, l_z,\; m)
]

再按需加入摩擦：粘滞 (f_v)、库仑 (f_c)、偏置 (f_o) 等。

3.2 为什么需要最小参数集

完整 (\pi) 中部分参数：

• 对输出力矩不可独立观测（只以组合形式出现）；
• 或彼此线性相关。

通过符号重组或数值 QR 等方法，得到：

[
\tau = Y_b(q,\dot q,\ddot q)\,\pi_b
]

• (\pi_b)：最小（基）参数集——真正要辨识的量
• (Y_b)：最小参数回归矩阵——激励设计与辨识都围绕它

3.3 工程含义

• 辨识目标是 (\pi_b)，不是 CAD 里“看起来完整”的 (m, I, r)。
• 控制侧也优先使用 (\tau = Y_b\pi_b)，或与之等价的最小参数逆动力学实现。
• URDF / CAD 惯性适合做初值，不宜直接当最终真值。

建议的子步骤

1. 自由度裁剪：先用 2R 或 3～4 关节打通全流程，再扩展到完整臂。
2. 线性性自检：随机采样验证 (\tau \approx Y\pi)。
3. 最小集自检：验证 (\tau \approx Y_b\pi_b)。
4. 传动侧建模决策：关节侧惯量 vs 电机侧惯量、齿轮比是否进入模型。

产出物

• (Y_b) 与 (\pi_b) 的定义（符号表达式或可调用代码）
• 可选的 (M(q), C, G) 分解（便于控制设计）
• 摩擦模型选型说明

4. 激励轨迹设计

做什么

设计关节空间轨迹 (q(t))，使观测矩阵中的 (Y_b) 信息丰富、条件数良好，同时满足：

• 关节位置 / 速度 / 加速度限位
• 力矩或电流限位
• 工作空间与防碰撞约束
• 足够平滑，便于滤波（常用傅里叶级数或多项式样条）

常用做法

• 周期傅里叶激励轨迹（工业机器人辨识中很常见）
  优化目标：最小化观测矩阵条件数，或最大化最小奇异值。

• 多组不同幅值、频率的轨迹，覆盖：

  • 慢速：重力、摩擦
  • 快速：惯性、科氏 / 离心项

优化时常见约束

• (q_{\min} \le q(t) \le q_{\max})
• (|\dot q| \le \dot q_{\max})，(|\ddot q| \le \ddot q_{\max})
• 可选：避开桌面碰撞、奇异附近降权

常见坑

• 轨迹过“保守” → (Y_b) 病态 → 参数方差大、不稳定。
• 轨迹过猛 → 饱和、撞限位、触发保护，数据作废。
• 只充分激励部分关节 → 对应 (\pi_b) 分量估不准。
• 忽略低速区 → 静摩擦 / 库仑摩擦偏置。

产出物

• 可下发的 (q_d(t),\dot q_d,\ddot q_d)（含周期、采样频率）
• 激励质量指标：条件数、奇异值分布、仿真力矩峰值

5. 参数辨识算法开发（先仿真，再上真机）

做什么

在拿到真机数据前，先把“估计 (\pi_b)”的软件链路跑通。

基本问题

堆叠多时刻数据：

[
\begin{bmatrix}\tau_1\ \vdots\ \tau_N\end{bmatrix}
=
\begin{bmatrix}Y_{b,1}\ \vdots\ Y_{b,N}\end{bmatrix}
\pi_b
\quad\Rightarrow\quad
\tau_{\text{stack}} = W\,\pi_b
]

常见求解方式：

• 普通最小二乘（OLS）
• 加权 / 正则最小二乘：抑制噪声与过拟合
• 带约束优化：质量为正、惯性张量物理可行、摩擦符号合理等
• 递推或分批辨识：在线或分段处理

算法开发清单

1. 用已知真值 (\pi_b^{\text{true}}) 生成理想仿真数据。
2. 验证 OLS 能近乎完美回收。
3. 加入测量噪声与滤波误差，评估偏差与方差。
4. 引入“由 (q) 数值微分得到 (\dot q,\ddot q)”这一真实误差源。
5. 对比有无摩擦项、有无重力项时的结果差异。

产出物

• 辨识程序：输入 ((q,\dot q,\ddot q,\tau)) → 输出 (\hat\pi_b) 与残差统计
• 仿真回归用例，防止后续改动引入隐性错误

6. 数据采集

采什么

关节空间辨识的最低配置：

• (q)：编码器或电机位置
• (\tau)：关节力矩传感器，或由电流经力矩常数 / 传动模型换算
• 严格同步的时间戳

通常还需后处理得到：

• (\dot q, \ddot q)：对 (q) 滤波微分，或采用指令侧 (\dot q_d,\ddot q_d)（各有利弊）

采集前必须完成

1. 力矩标定：电流→力矩的比例、偏置、温度漂移。
2. 零位与坐标系：与 MDH 模型使用同一套 (q=0) 约定。
3. 控制模式：位置环跟踪激励轨迹，同步记录实际 (q) 与 (\tau)。
4. 采样率：建议不低于轨迹最高频率的 10～20 倍；写清控制周期与记录周期。
5. 安全：软限位、力矩限制、急停、碰撞检测。

后处理（常被低估）

• 时间对齐、丢包处理
• 低通或零相位滤波后再微分
• 剔除启动加速段、饱和段、碰撞段
• 若基座不水平，修正重力方向 (g)

产出物

• 原始数据与元数据（机器人、日期、负载、温度、控制增益等）
• 清洗后的 ({q, \dot q, \ddot q, \tau}) 数据集

7. 参数辨识

做什么

用采集数据估计 (\hat\pi_b)。

推荐流程

1. 组装观测矩阵 (W)，检查条件数。
2. 先做无约束 OLS，检查残差与参数数量级是否合理。
3. 再引入物理约束或正则（可用 CAD/URDF 映射得到 (\pi_b) 初值）。
4. 交叉验证：轨迹 A 辨识、轨迹 B 预测。
5. 记录 (\hat\pi_b)、不确定性（协方差或置信区间）、残差 RMS。

实用技巧

• 分阶段：慢速先抓重力相关组合，快速再抓惯性与科氏项。
• 摩擦可分段：高速偏粘滞，低速补库仑。
• 有负载 / 无负载分开辨识，或把负载当作附加参数。
• 若某参数方差极大，多半是激励不足，应回到第 4 步。

产出物

• 版本化的 (\hat\pi_b)
• 辨识报告：条件数、残差、与 CAD/URDF 差异

8. 动力学参数验证

验证不能只看训练集残差，建议分层确认。

8.1 开环力矩预测（基础）

在未参与辨识的轨迹上计算：

[
\hat\tau = Y_b(q,\dot q,\ddot q)\,\hat\pi_b
]

与实测 (\tau) 对比：

• 每关节 RMS / 峰值误差
• 分别看重力主导段与高速段

8.2 能量 / 功率一致性（可选）

按所选方法检查模型是否违背基本物理关系。

8.3 控制侧验证（更接近最终目标）

• 重力补偿拖动：手感是否接近失重、漂移是否可接受
• 逆动力学前馈 + PD：跟踪误差是否明显下降
• 不同负载、不同速度下是否仍稳定

通过标准（需按项目自定）

• 验证集力矩预测误差低于额定力矩的约定比例
• 相对纯 PD，跟踪误差有明确改善
• 重复实验得到的参数不过度发散

产出物

• 验证曲线与指标表
• 明确结论：可用于控制 / 仅可用于重力补偿 / 需重新采集

9. 实时动力学计算工程

做什么

把离线模型变成控制周期内可稳定算完的模块。

常见实现路径

1. 符号代码生成后移植到 C/C++ 或嵌入式平台。
2. 数值 RNE（如 Pinocchio、RBDL 等）：通用、易维护，参数用辨识结果更新。
3. 混合：离线辨识用 (Y_b\pi_b)，在线用 RNE，并由 (\pi_b) 映射回可用的惯性参数。

工程要点

• 算时：在典型 1 kHz 控制下，逆动力学通常应远小于 1 ms（视自由度与硬件而定）。
• 确定性：实时路径避免动态内存抖动与解释型语言 GC；预分配缓冲。
• 接口：输入 (q,\dot q,\ddot q)（或 (q,\dot q) + 期望 (\ddot q_d)），输出前馈力矩 (\tau_{\text{ff}})。
• 参数管理：(\pi_b) 可配置加载，带版本号。
• 单位与符号：电机侧 / 关节侧、减速比、力矩方向与驱动器一致。
• 保护：NaN 检测、力矩限幅、模型切换时的平滑过渡。

与控制的衔接

常见结构：

[
\tau_{\text{cmd}}
= \underbrace{Y_b(q,\dot q,\ddot q_d)\,\hat\pi_b}_{\text{逆动力学前馈}}

• K_p e + K_d \dot e
  ]

也可采用计算力矩形式：先用 (M,C,G) 近似线性化，再套外环控制器。

产出物

• 实时计算库或控制节点
• 周期耗时测试，以及离线实现与在线实现的数值一致性测试
• 控制联调说明

容易被漏掉、却常决定成败的步骤

最小可行路径（MVP）

人手有限时，建议“先打穿，再加深”：

1. 用平面 2R，或真机前 3～4 个关节做试点。
2. URDF → MDH，并通过正运动学校验。
3. 建立动力学模型并导出最小参数形式。
4. 仿真激励 + 最小二乘，确认能回收真值。
5. 真机慢速轨迹，先验证重力相关项。
6. 完整激励 → 辨识 → 开环力矩预测。
7. 把重力补偿或逆动力学前馈接入控制。
8. 再扩展到全自由度与更细的摩擦模型。

每一步的完成定义

延伸阅读（自学关键词）

教材与理论

• Modified DH（Craig）
• Recursive Newton–Euler（Khalil）
• Base / minimal parameters（Gautier）
• Dynamic regressor (Y(q,\dot q,\ddot q))
• Exciting trajectories（Fourier）、观测矩阵条件数
• Computed-torque control、inverse-dynamics feedforward

推荐精读方向

• Khalil & Dombre，《Modeling, Identification and Control of Robots》
• Gautier & Khalil 关于最小参数与激励轨迹的经典论文
• Featherstone，《Rigid Body Dynamics Algorithms》（高效动力学算法）

开源工具生态（按需选用）

• 数值动力学：Pinocchio、RBDL
• 符号动力学与最小参数：SymPyBotics 等
• 辨识工具链：FIGAROH、FloBaRoID 等

一句话总结

几何用 URDF/MDH 做对 → 用最小参数把可辨识动力学写对 → 用激励与数据把 (\pi_b) 估准 → 用验证与实时工程把模型嵌进控制环。

几何错了，后面全错；参数集不最小，辨识不稳；不验证就进环，控制容易反噬。