2. 空间描述与变换：位置与姿态描述、旋转矩阵、齐次变换矩阵、坐标变换基础

各位同学，欢迎来到第二章。上一章我们聊了机器人的自由度、连杆和关节，算是打了个底。但光知道结构还不够——你得能描述它。怎么描述？说白了，就是回答两个问题：“它在哪？” 和 “它朝哪？”

我刚开始做机器人项目时，觉得这玩意儿不就是坐标嘛，初中就学过。结果第一次调试机械臂，发现末端执行器死活对不准目标点。折腾了两天，最后发现是姿态描述搞反了。嗯，从那以后我再也不敢小看这一章的内容了。

先来最简单的。在三维空间里，一个点的位置用三个数就能搞定：

P = [x, y, z]ᵀ

这就是位置矢量。你可能会问：“就这么简单？” 对，就这么简单。但有个坑——这个坐标是相对于哪个坐标系说的？

举个例子。我让你去拿桌上的杯子。你说“杯子在我前方30厘米”。这个“前方”是相对于你的身体而言的。如果换个人站在你对面，他的“前方”就是你的“后方”。所以，描述位置必须指明参考系。

我的习惯： 在代码里给每个坐标变量加个后缀，比如 pos_A 表示“在坐标系A下的位置”。别嫌麻烦，这能省掉你80%的调试时间。

位置搞定了，但机器人不光要“到那儿”，还得“摆好姿势”。比如你拿螺丝刀拧螺丝，螺丝刀尖到了螺丝位置，但刀头方向不对，还是拧不进去。

描述姿态，最直观的方法是旋转矩阵。它长这样：

R = [r₁₁  r₁₂  r₁₃]
    [r₂₁  r₂₂  r₂₃]
    [r₃₁  r₃₂  r₃₃]

这个3×3的矩阵，每一列代表被旋转坐标系的一个轴在参考系中的投影。说白了，它就是告诉你：“我把坐标系转了这么多角度，每个轴指向哪了。”

旋转矩阵有个重要性质：它是正交矩阵。什么意思？

这两个性质保证了旋转矩阵不会改变物体的形状和大小，只是让它转了个方向。

  核心理解： 旋转矩阵的9个数不是独立的，它们之间受6个约束条件限制（正交性3个 + 单位长度3个）。所以实际上只有3个自由度——这就是为什么我们后面会学欧拉角、四元数这些只用3个或4个参数的方法。

任何复杂的旋转，都可以拆成绕三个坐标轴的简单旋转。我建议你把这三种矩阵记牢，后面会反复用到：

你想想看，这三个矩阵其实长得挺像的。我个人的记忆方法是：绕哪个轴转，那个轴对应的行和列就不动，剩下的2×2块就是标准的二维旋转矩阵。

在实际编程中，我们经常需要同时处理位置和姿态。总不能每次都写一个旋转矩阵加一个位置矢量吧？太麻烦了。所以就有了齐次变换矩阵：

T = [R   p]
    [0ᵀ 1]

其中R是3×3旋转矩阵，p是3×1位置矢量，0ᵀ是1×3零行向量。这个4×4的矩阵，把位置和姿态打包成了一个整体。

为什么要加一行[0 0 0 1]？这其实是为了让矩阵乘法能同时处理旋转和平移。你想想看，如果只用3×3矩阵，平移就没法用乘法表示。加了这一行，我们就可以用统一的矩阵乘法来做坐标变换了。

我曾经踩过的坑： 齐次变换矩阵的最后一列是位置，但不是直接等于你想要的坐标！它表示的是“从当前坐标系原点指向目标坐标系原点的矢量，在当前坐标系下的表示”。翻译成人话就是：T矩阵告诉你，如果你站在坐标系A的原点，坐标系B的原点在你前方多远、朝哪个方向。

好了，现在我们有工具了。假设你在世界坐标系下知道了一个点的坐标，但机器人用的是它自己基座上的坐标系。怎么转换？

分两步走：

用齐次变换矩阵写出来就是：

ᵇP = ᵇTₐ · ᵃP

其中ᵃP是点在坐标系A下的坐标，ᵇTₐ是从A到B的变换矩阵，ᵇP是变换后的坐标。

这里有个容易搞混的地方：变换矩阵的下标顺序。我习惯这样记：ᵇTₐ 读作“从A到B的变换”，它把A系下的点变到B系下。你写代码的时候，下标顺序一定要保持一致，不然算出来的结果天差地别。

下面这张图帮你理清本章的核心逻辑：

最后，分享几个我在项目中总结的经验：

一个实用的检查方法： 算完坐标变换后，把变换后的点代回去，看能不能还原。如果能还原，说明你的变换矩阵和计算过程都没问题。这叫“闭环验证”，我每次写运动学代码都会做这一步。

好了，这一章的内容就到这里。位置和姿态的描述是机器人运动学的基石，后面的正运动学、逆运动学、轨迹规划全都建立在这些概念之上。你如果现在觉得有点绕，没关系——多画图、多手算几遍，自然就熟了。