2. 运动学基础：刚体在空间中的位姿描述、齐次变换矩阵、正逆运动学基础概念

各位同学，欢迎来到《轨迹规划算法实战解析》的第二讲。

今天咱们聊点硬核的——运动学基础。说实话，我刚开始做机器人那会儿，觉得运动学就是背公式。后来踩了不少坑才明白，这玩意儿是机器人一切动作的「地基」。你想想看，如果连机器人胳膊在哪儿、怎么动都描述不清楚，后面的轨迹规划、避障、力控，全都是空中楼阁。

2.1 刚体位姿描述：位置与姿态

先问个问题：怎么告诉别人「机器人手抓在哪儿」？

光说坐标不够。比如我说「手抓在 (1, 2, 3) 这个点」，但它是朝上抓还是朝下抓？是水平抓还是斜着抓？所以，我们得同时描述两件事：位置和姿态。合起来，就叫位姿。

位置好办，用三维坐标 (x, y, z) 就行。姿态呢？我习惯用旋转矩阵 R 来描述。它是一个 3x3 的矩阵，每一列代表刚体坐标系的一个轴在参考坐标系下的投影。

举个例子：

# 绕Z轴旋转θ角的旋转矩阵
R_z(θ) = [[cosθ, -sinθ, 0],
          [sinθ,  cosθ, 0],
          [0,      0,    1]]

嗯，这里要注意：旋转矩阵有个硬性要求——必须是正交矩阵，且行列式为+1。我在项目中遇到过有人手算旋转矩阵，结果行列式算出来是0.98，导致后续逆解全错。所以，一定要检查矩阵的正交性。

2.2 齐次变换矩阵：把旋转和平移打包

光有旋转矩阵还不够。你想啊，机器人关节既要转又要平移，总不能每次分开算吧？太麻烦了。

所以，我们引入齐次变换矩阵 T。它把旋转和平移打包成一个 4x4 的矩阵：

T = [[R, t],
     [0, 1]]

其中 R 是 3x3 旋转矩阵，t 是 3x1 平移向量。最后一行是 [0, 0, 0, 1]。

为什么叫「齐次」？说白了，就是把三维坐标扩展成四维齐次坐标 (x, y, z, 1)。这样，一次矩阵乘法就能同时完成旋转和平移。我个人觉得，这是机器人学里最优雅的设计之一。

2.3 正运动学：从关节空间到笛卡尔空间

正运动学，说白了就是：已知每个关节的角度，求末端执行器的位姿。

这就像你看着机器人的每个关节转了多少度，然后算出它的手在哪儿、朝哪个方向。这是个「顺推」的过程，通常有唯一解。

以六轴工业机器人为例，正运动学公式长这样：

T_06 = T_01(q1) * T_12(q2) * T_23(q3) * T_34(q4) * T_45(q5) * T_56(q6)

每个 T_(i-1)i 都是关于关节角度 qi 的齐次变换矩阵。连乘之后，得到的就是末端相对于基座的位姿。

我记得第一次手算六轴正运动学时，算到第三个矩阵就晕了。后来我学乖了——用符号计算工具（比如 SymPy）来推导，既快又准。你想想看，人工算错一个符号，后面全白费。

2.4 逆运动学：从笛卡尔空间到关节空间

逆运动学就反过来了：已知末端执行器的目标位姿，求各个关节的角度。

这是轨迹规划里最头疼的部分。为什么？因为逆解通常不唯一，甚至可能无解。

比如，你想让机器人把手伸到桌子底下，但关节角度限制可能让这个位姿根本达不到。又或者，同一个末端位姿，对应着好几组关节角度（比如肘部朝上还是朝下）。

逆运动学的求解方法主要有两种：

• 解析法：针对特定构型（比如有球形腕的六轴机器人），可以推导出封闭解。速度快，但通用性差。
• 数值法：比如牛顿-拉夫森迭代法。通用性强，但可能陷入局部最优，且计算量大。

我个人建议：能用解析法就用解析法。实在不行，再用数值法，但一定要加多组初始值尝试。

2.5 知识体系总览

说了这么多，咱们用一张图来梳理一下本章的核心逻辑：

2.6 本章小结

好，咱们捋一捋今天的内容：

• 位姿描述：位置 + 姿态，6个自由度，一个不能少。
• 齐次变换矩阵：把旋转和平移打包成 4x4 矩阵，优雅又实用。
• 正运动学：从关节角到位姿，顺推，唯一解。
• 逆运动学：从位姿到关节角，反推，多解或无解，需要小心处理。

说实话，这些概念看起来简单，但真正用起来，坑不少。我建议你动手写个小程序，实现一个两连杆机器人的正逆运动学。代码跑通了，才算真懂了。