📚 课程目录第2章刚体与位姿描述

2. 刚体与位姿描述：位置矢量、旋转矩阵、齐次变换矩阵

各位，咱们正式开始啃机器人学里最基础、也最绕不开的一块硬骨头——刚体位姿描述。

说白了，就是回答两个问题：“它在哪里？” 和 “它朝哪边？”。

你想想看，一个机械臂要抓东西，首先得知道自己的手（末端执行器）在空间里什么位置、什么姿态。这个“位置+姿态”，我们叫它位姿。今天我就把描述位姿的三种数学工具掰开揉碎了讲清楚。

最简单的情况。空间里一个点，怎么描述？

建个坐标系，用三个坐标值就行了。比如点 P 在坐标系 {A} 下的位置，写成：

^A P = [p_x, p_y, p_z]^T

这就是位置矢量。一个 3×1 的列向量，干净利落。

我在项目里见过不少新手，上来就写坐标，却忘了标参考坐标系。嗯，这里要注意：没有参考系的位置是没有意义的。你说“点 P 在 (1,2,3)”，我问“相对于哪个坐标系？”——答不上来，这数据就是废的。

我的习惯： 所有位置矢量，左上角标清楚参考坐标系。比如 ^A P 表示在 {A} 系下的坐标。这个习惯能帮你省掉后面一大堆坐标系变换时的混乱。

位置搞定了，但刚体不光有位置，还有朝向。比如一个扳手，你把它放在桌上，可以正着放，也可以斜着放——位置没变，但姿态变了。

描述姿态，最常用的工具就是旋转矩阵。

旋转矩阵 R 是一个 3×3 的正交矩阵，满足 R^T R = I，且 det(R) = +1。它的每一列，其实就是被旋转坐标系 {B} 的三个主轴在参考坐标系 {A} 下的方向余弦。

^A_B R = [ ^A X_B, ^A Y_B, ^A Z_B ]

举个例子，绕 Z 轴转 θ 角，旋转矩阵长这样：

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

看着眼熟吧？就是平面旋转加了个 Z 轴不变。

  核心性质： 旋转矩阵的逆等于它的转置。也就是说，R^{-1} = R^T。这个性质在后续做坐标变换时特别有用，不用真的去求逆矩阵，转置一下就行。

我曾经在调试一个六轴机器人时，发现末端姿态总是不对。查了半天，原来是旋转矩阵写反了——把 ^A_B R 和 ^B_A R 搞混了。记住：左上标是参考系，左下标是被描述系。这个约定别搞反。

位置用矢量，姿态用矩阵。但实际中，我们经常需要同时描述位置和姿态，还要做连续的坐标变换。这时候，齐次变换矩阵就登场了。

它是一个 4×4 的矩阵：

^A_B T = [ ^A_B R    ^A P_Borg ]
         [ 0 0 0       1       ]

左上角 3×3 是旋转矩阵，右上角 3×1 是 {B} 系原点在 {A} 系下的位置矢量。最后一行是 [0 0 0 1]，固定不变。

为什么叫“齐次”？因为把 3D 的点 (x,y,z) 扩展成齐次坐标 (x,y,z,1)，就可以用矩阵乘法一次性完成旋转和平移：

^A P = ^A_B T · ^B P

你看，一个式子，既转了方向，又挪了位置。干净！

注意： 齐次变换矩阵的乘法顺序不能乱。先旋转后平移，和先平移后旋转，结果完全不同。我见过有人把 T1 · T2 和 T2 · T1 搞反，结果机器人直接撞到工作台上——还好当时速度慢，不然就是事故了。

我把它们放在一起对比一下，方便你理解各自的应用场景：

我个人习惯是：做理论推导时用齐次矩阵，写代码时拆成位置矢量和旋转矩阵。为什么？因为 4×4 矩阵乘起来虽然方便，但占内存、计算慢。嵌入式控制器里，能省则省。

下面这张图，把本章的核心逻辑串起来了。你看一遍，应该能对“刚体位姿描述”有个整体印象。

讲完理论，说点实际的。

我在做机器人标定时，经常需要把测量设备（比如激光跟踪仪）的数据转换到机器人基坐标系下。这时候，齐次变换矩阵就是我的首选工具。把测量坐标系和机器人坐标系之间的变换关系用一个 T 矩阵表示，后面所有测量点，乘一下 T 就完事了。

但要注意：T 矩阵的精度直接决定了标定精度。我曾经因为一个旋转矩阵的数值误差（小数点后第四位），导致标定后机器人抓取偏差了 0.5mm。对于精密装配来说，这已经算事故了。

避坑指南： 计算旋转矩阵时，尽量用解析公式，别用数值迭代。如果非要用数值方法，记得做正交化处理——保证 R^T R = I 严格成立。我一般用 SVD 分解来做正交化，稳定可靠。

好了，这一章的内容就这些。位置矢量、旋转矩阵、齐次变换矩阵——三个工具，层层递进。你把这套东西吃透了，后面学连杆坐标系定义、正逆运动学，就会顺畅很多。