第二章：运动学与动力学基础

各位同学，欢迎来到第二章。说实话，这一章是整个轨迹规划的「地基」。你想想看，如果连机器人怎么动、为什么这么动都搞不清楚，后面谈最优控制就是空中楼阁。

我在做第一个工业机器人项目时，就吃过这个亏。当时急着调轨迹，结果机器人末端抖得像筛糠一样。后来才发现，是运动学模型里一个旋转矩阵的方向搞反了。嗯，从那以后，我每次建模型都会老老实实把基础再过一遍。

2.1 刚体运动的数学描述

刚体运动，说白了就是描述一个物体在空间里「又转又移」。我们得用数学语言把它说清楚。

2.1.1 旋转矩阵

旋转矩阵是描述姿态的核心工具。它是一个 3×3 的正交矩阵，行列式为 +1。我个人习惯把它理解成「三个坐标轴的方向余弦」。

举个例子，绕 Z 轴旋转 θ 角：

R_z(θ) = [cosθ  -sinθ  0]
         [sinθ   cosθ  0]
         [0      0     1]

这里有个坑，我提醒一下：旋转矩阵的乘法不满足交换律。也就是说，先绕 X 轴转再绕 Y 轴转，和反过来转，结果完全不同。我在项目里见过有人搞混这个顺序，结果机械臂直接撞到工件上去了。

2.1.2 齐次变换矩阵

齐次变换矩阵把旋转和平移统一到一个 4×4 的矩阵里。形式是这样的：

T = [R   p]
    [0   1]

其中 R 是 3×3 旋转矩阵，p 是 3×1 平移向量。为什么用齐次坐标？说白了就是为了把多次变换写成矩阵连乘的形式，方便计算机计算。

我记得有一次调试视觉引导抓取，相机坐标系到机器人基坐标系的变换，就是用齐次变换矩阵串起来的。如果不用这个，你得手动算一堆三角函数，容易出错不说，还慢。

2.2 机器人正运动学

正运动学，就是已知关节角度，求末端位姿。说白了，你给每个电机发一个角度值，我想知道机器人手在哪里、朝哪个方向。

最经典的方法是 Denavit-Hartenberg 参数法，也就是 DH 参数法。每个关节用四个参数描述：

参数	含义
θ	绕 Z 轴的旋转角
d	沿 Z 轴的平移
a	沿 X 轴的平移（连杆长度）
α	绕 X 轴的旋转角（连杆扭角）

然后每个关节的变换矩阵为：

T_i = Rot(z, θ_i) * Trans(z, d_i) * Trans(x, a_i) * Rot(x, α_i)

把所有关节的变换矩阵乘起来，就得到末端位姿：

T_end = T_1 * T_2 * ... * T_n

这里我建议你写代码时，先把每个关节的 DH 参数列成表格，再逐层相乘。我在项目中就是这么做的，调试起来一目了然。

2.3 机器人逆运动学

逆运动学正好反过来：已知末端位姿，求关节角度。这是轨迹规划中最头疼的部分，因为解不唯一，甚至可能无解。

为什么难？你想想看，一个六轴机器人，末端在某个位置，可能有 8 种不同的关节角度组合都能达到。怎么选？

常用的方法有两种：

• 解析法： 通过几何关系直接推导出角度公式。速度快，但只适用于特定构型的机器人。
• 数值法： 比如牛顿-拉夫森迭代法。通用性强，但可能收敛到局部解。

我在做焊接机器人项目时，用的是解析法。因为焊接路径对姿态要求很严格，数值法有时候会给出一个「看起来对但实际不能用」的解。解析法虽然推导麻烦，但每个解都有明确的物理意义。

2.4 拉格朗日动力学方程

运动学只关心「怎么动」，动力学关心「为什么这么动」。说白了，就是力和运动之间的关系。

拉格朗日法是我个人最喜欢的方法。它基于能量，不用考虑复杂的约束力。公式很简单：

L = T - V

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

其中 L 是拉格朗日量，T 是动能，V 是势能，q 是广义坐标，τ 是广义力。

具体到机器人上，动能 T 可以写成：

T = 0.5 * q̇^T * M(q) * q̇

M(q) 是惯性矩阵，它跟关节角度有关。势能 V 通常就是重力势能。

展开后得到：

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

这里 C 是科里奥利力和离心力项，G 是重力项。这个方程就是机器人动力学控制的基础。

我记得有一次做高速拾取机器人，一开始没考虑科里奥利力，结果速度一上去，轨迹就偏了。后来加上这一项，精度立马就上来了。所以，别小看这些「小项」。

2.5 本章知识体系

下面这张图是我自己整理的，把这一章的核心逻辑串起来了：

从这张图可以看得很清楚：刚体运动描述是基础，正/逆运动学是桥梁，动力学方程是最终要用的工具。每一步都环环相扣。

好了，这一章的内容就到这里。记住，基础打牢了，后面讲最优轨迹规划时你才能游刃有余。下一章我们会进入轨迹规划的核心——路径点插值与时间参数化。

---

公众号:蓝海资料掘金营，微信deep3321