3. 数学基础（二）：运动学与动力学

各位同学，欢迎来到第三章。上一章我们把向量、旋转矩阵、齐次变换这些数学工具梳理了一遍。这一章，我们要真正开始用这些工具去描述机器人的运动与受力。

运动学与动力学，说白了就是回答两个问题：“机器人动起来是什么样子？” 和 “让它这样动需要多大的力？”。前者是几何问题，后者是物理问题。我在做第一个机械臂项目时，把运动学算得滚瓜烂熟，结果一上电机就抖成筛子——因为完全没考虑动力学。所以这两块内容，缺一不可。

本章核心脉络： 正运动学（关节→末端）→ 逆运动学（末端→关节）→ 雅可比矩阵（速度映射）→ 拉格朗日动力学（力与运动的关系）。
第三章 知识体系：运动学与动力学 正运动学 关节角 → 末端位姿 逆运动学 末端位姿 → 关节角 雅可比矩阵 关节速度 → 末端速度 拉格朗日动力学 力 → 运动 应用：轨迹规划、力控制、奇异点分析、运动学标定 💡 学习路径：先掌握正运动学（DH参数），再攻克逆运动学（解析法/数值法）， 然后用雅可比矩阵连接速度，最后用拉格朗日方程搞定动力学。

3.1 正运动学：从关节空间到操作空间

正运动学，就是已知每个关节的角度（或位移），求末端执行器在笛卡尔空间的位置和姿态。你想想看，这其实就是一个坐标变换的链式反应。

我个人习惯用 DH参数法（Denavit-Hartenberg） 来建立正运动学模型。这个方法的核心思想是：在每个关节上固连一个坐标系，然后用四个参数（a, α, d, θ）来描述相邻坐标系之间的变换关系。

DH参数 符号 物理含义
连杆长度 ai-1 沿 Xi-1 轴，从 Zi-1 到 Zi 的距离
连杆扭角 αi-1 绕 Xi-1 轴，从 Zi-1 到 Zi 的转角
连杆偏距 di 沿 Zi 轴，从 Xi-1 到 Xi 的距离
关节角 θi 绕 Zi 轴，从 Xi-1 到 Xi 的转角

有了这四个参数，相邻坐标系间的齐次变换矩阵就是标准形式：

# 标准DH变换矩阵（从坐标系 i-1 到 i）
T_i = [
    [cos(θ_i),  -sin(θ_i)*cos(α_i),   sin(θ_i)*sin(α_i),   a_i*cos(θ_i)],
    [sin(θ_i),   cos(θ_i)*cos(α_i),  -cos(θ_i)*sin(α_i),   a_i*sin(θ_i)],
    [0,          sin(α_i),             cos(α_i),             d_i        ],
    [0,          0,                    0,                    1          ]
]

然后，从基座到末端，把所有变换矩阵乘起来：

T_0_n = T_1 * T_2 * ... * T_n

这个 T_0_n 就是末端位姿。嗯，这里要注意：矩阵乘法顺序不能错，左乘还是右乘取决于你用的DH约定。我在项目中吃过这个亏，用标准DH和改良DH混着算，结果末端位置差了十几厘米。

我的习惯： 统一使用 Craig 版（改良）DH参数。它的坐标系固连在连杆前端，推导逆运动学时更直观。一旦选定，整个项目就别换了。

3.2 逆运动学：从操作空间回到关节空间

逆运动学正好反过来：已知末端位姿，求关节角。这比正运动学难得多，因为：

  • 多解性： 同一个末端位姿，可能有多个关节构型（比如肘部朝上或朝下）。
  • 奇异性： 某些位姿下，关节速度会趋于无穷大。
  • 无解： 末端位姿超出了机器人的工作空间。

求解方法主要有两类：

方法 优点 缺点 适用场景
解析法（封闭解） 速度快、精度高、所有解可枚举 只适用于特定构型（如6轴有相邻轴相交） 工业机器人（PUMA、KUKA等）
数值法（迭代法） 通用性强，任意构型都能用 速度慢、可能陷入局部最优、需要初值 冗余机器人、特殊构型

我曾经在一个7自由度协作机器人项目里，死活找不到解析解。后来改用数值法（Levenberg-Marquardt迭代），配合一个合理的初始猜测，才把问题搞定。所以我的建议是：能解析就解析，不行再上数值。

避坑指南： 数值法迭代时，一定要检查雅可比矩阵是否奇异。如果接近奇异，迭代会发散。我遇到过迭代步长突然跳变到几万度的情况——吓得我赶紧加了奇异值阈值判断。

3.3 雅可比矩阵：速度的桥梁

雅可比矩阵 J 把关节空间的速度映射到操作空间的速度：

ẋ = J(q) · q̇

其中 ẋ 是末端速度（线速度 + 角速度，共6维），q̇ 是关节速度（n维），J 是 6×n 的矩阵。

雅可比矩阵的求法有两种：

  1. 微分法： 对正运动学方程直接求偏导。适合符号推导。
  2. 矢量积法： 利用运动学链的几何关系，逐列构造。适合编程实现。

我个人更推荐矢量积法，因为它物理意义清晰：

# 对于旋转关节 i，雅可比矩阵的第 i 列：
J_i = [
    [z_{i-1} × (p_n - p_{i-1})],  # 线速度部分
    [z_{i-1}]                       # 角速度部分
]
# 其中 z_{i-1} 是关节 i 的旋转轴方向，p_n 是末端位置，p_{i-1} 是关节 i 的原点位置

雅可比矩阵的用途远不止速度映射。它还用于：

  • 奇异性分析： 当 det(J·Jᵀ) = 0 时，机器人处于奇异位形。
  • 静力映射： τ = Jᵀ · F，关节力矩与末端力的关系。
  • 可操作度： w = sqrt(det(J·Jᵀ))，衡量机器人运动的灵活性。
关键认知： 雅可比矩阵是连接运动学与动力学的桥梁。没有它，你无法做速度规划，也无法做力控制。我每次调试机器人前，都会先打印出雅可比矩阵的条件数——条件数太大，说明快接近奇异了。

3.4 拉格朗日动力学：从能量角度看运动

动力学研究的是力与运动的关系。拉格朗日方法从能量角度出发，避开了复杂的受力分析，非常适合多体系统。

拉格朗日方程的标准形式：

d/dt (∂L/∂q̇) - ∂L/∂q = τ

其中 L = T - V 是拉格朗日函数
T 是系统总动能
V 是系统总势能
q 是广义坐标（关节角）
τ 是广义力（关节力矩）

推导步骤其实很机械：

  1. 写出每个连杆的动能 T_i（平动动能 + 转动动能）
  2. 写出每个连杆的势能 V_i（重力势能）
  3. 求和得到总动能 T 和总势能 V
  4. 代入拉格朗日方程，求偏导

最终得到的动力学方程可以写成标准形式：

M(q) · q̈ + C(q, q̇) · q̇ + G(q) = τ

M(q)：惯性矩阵（与加速度有关）
C(q, q̇)：科里奥利力和离心力项（与速度有关）
G(q)：重力项（与位置有关）

我记得第一次手动推导6轴机器人的拉格朗日方程，整整花了两天。符号展开后，M矩阵里全是 sin 和 cos 的乘积，看得人眼花缭乱。后来我学乖了，用符号计算工具（SymPy）来辅助推导。

工程建议： 实际项目中，很少有人手推完整的拉格朗日方程。更常见的做法是：
  • 用牛顿-欧拉递推法（计算效率高，适合实时控制）
  • 用参数辨识方法（实验数据拟合动力学参数）
  • 用符号工具自动生成代码
但理解拉格朗日法的物理意义，能帮你更好地理解动力学补偿、前馈控制等高级话题。

3.5 本章小结

这一章我们走完了从几何到物理的完整链条：

  • 正运动学 让你知道机器人长什么样
  • 逆运动学 让你能指挥机器人去指定位置
  • 雅可比矩阵 让你理解速度与力的传递
  • 拉格朗日动力学 让你算清楚需要多大的力
← 上一章 📖 返回目录 下一章 →