1.3 坐标系与齐次变换
1.3 坐标系与齐次变换
本节摘要：位姿必须挂在参考系上才有意义。本节讲清坐标系、位姿、齐次变换矩阵三件事，给出"左乘右乘决定先后次序"的判断规则，并用一段 Python 代码把笔算结果验算一遍。
为什么这句话什么都没说
"物料块在 (0.5, 0.3, 0.2) 米处。"——这句话在机器人学里等于没说。是相对于哪个参考系？机器人底座？车间地面？还是抓取它的上一台设备？三个答案对应三个不同的物理位置，差之毫厘的参考系混淆，到了末端执行器那里就是实打实的撞击。
所有机器人学讨论都建立在同一个约定上：位姿 = 位置 + 姿态，且必须声明参考系。位置是三维平移向量，姿态是相对参考系的旋转。本节的任务是给"位姿的运算"立规矩，此后全册的公式都沿用这套记号。
齐次变换矩阵：一套记号吃下旋转与平移
平移好办，向量加法即可。旋转麻烦一些，二维旋转角 θ 的矩阵是熟悉的正余弦排布，三维则绕不同轴的旋转矩阵形状不同。麻烦在于实际变换几乎总是"先转再移"或"先移再转"的复合，分别维护矩阵和向量两套运算很啰嗦。齐次变换矩阵的思路是给向量添一个维度，把两种运算统一成一个 4×4 矩阵：
T = | R p | R 为 3x3 旋转矩阵，p 为 3x1 平移向量 | 0 1 | 底行补 0 0 0 1，凑成 4x4
复合变换就是矩阵连乘。已知机器人底座系 B 到相机系 C 的变换、相机系到物料系的变换，物料在底座系下的位姿直接是两个矩阵相乘。
图：基座系、相机系与物体系之间的变换链
左乘还是右乘：唯一容易翻车的点
矩阵乘法不满足交换律，于是"先绕 X 转 90 度再平移"与"先平移再绕 X 转 90 度"结果不同。规则只需记一句：
- 右乘（T · A）：A 在当前坐标系自身上发生——"就地"转一下、挪一下；
- 左乘（A · T）：A 在固定参考系里发生——从"外面"看它转一下、挪一下。
两关节平面机械臂是最小案例。关节一在基座上转 θ1，连杆一长 L1；关节二在连杆末端就地转 θ2，连杆二长 L2。末端位姿对基座系是：
T(B,末端) = Rot(z, θ1) · Trans(L1, 0, 0) · Rot(z, θ2) · Trans(L2, 0, 0)
从左往右读就是"站在基座看"的运动序列：先整体转 θ1（左乘视角的固定系旋转），再沿臂长挪，再就地转 θ2……这正是第 2 章 DH 参数法要系统化的东西。此处先混个脸熟即可。
用代码验算一遍
笔算容易错，所以工程师的习惯是手推一次、代码验一次。下面用 Python 的矩阵运算把两关节例子跑出数值（约定 z 轴竖直向上，角度单位弧度）：
import numpy as np def rot_z(theta): """绕 z 轴旋转 theta 弧度的 3x3 矩阵""" c, s = np.cos(theta), np.sin(theta) return np.array([[c, -s, 0], [s, c, 0], [0, 0, 1]]) def homo(R, p): """旋转 + 平移 拼 4x4 齐次矩阵""" T = np.eye(4) T[:3, :3] = R T[:3, 3] = p return T # 两关节臂：L1 = 0.4 m, L2 = 0.3 m, theta1 = 30 度, theta2 = 45 度 L1, L2 = 0.4, 0.3 t1, t2 = np.radians([30, 45]) T = homo(rot_z(t1), [0, 0, 0]) @ homo(np.eye(3), [L1, 0, 0]) \ @ homo(rot_z(t2), [0, 0, 0]) @ homo(np.eye(3), [L2, 0, 0]) print(T[:2, 3]) # 取 x, y # 输出: [0.42395 0.48989]
用解析式核一遍：x = L1·cosθ1 + L2·cos(θ1+θ2) = 0.4×0.8660 + 0.3×0.2588 = 0.4240，y = L1·sinθ1 + L2·sin(θ1+θ2) = 0.2 + 0.2898 = 0.4898。与代码输出一致——注意解析式里 cos(θ1+θ2) 的"总角度"，正是矩阵链里 Rot(z,θ2) 右乘在前面变换之后的效果：就地旋转的叠加，在固定系看来就是角度相加。笔算与代码互相咬合，这套记号就可以放心交给第 2 章用了。
调试变换链有个通用办法：把链拆开，每乘一段打印一次中间矩阵。错误必然现形于某一段——要么是旋转轴写错，要么是平移向量忘了"跟着转"。
本节要点
- 位姿 = 位置 + 姿态，必须声明参考系，否则无意义。
- 齐次变换矩阵用一套乘法统一旋转与平移；链条上依次相乘即复合变换。
- 右乘是"就地"变换，左乘是"固定参考系"变换；写公式前先想清楚视角。
- 变换链调试的万能招：逐段相乘、逐段打印，错误必然现形于某一段。
逆变换：链条可以倒着走
既然 T 把 A 系下的位姿翻译到 B 系，逆变换 T 的逆就完成反向翻译，且有一个免于求逆矩阵的漂亮性质：齐次变换矩阵的逆等于旋转矩阵转置加"平移取反后再旋转"。用式子写：若 T = [R p; 0 1]，则 T 的逆 = [Rᵀ, −Rᵀp; 0 1]。物理含义直白：从 B 系看 A 系，就是把 B 系到 A 系的平移向量反向、再按 A 系的姿态转回去。数值验证一行代码：用 1.3 节正解代码输出的 T 乘上按这个公式算出的逆，应得到单位矩阵。这个性质让手眼标定等工程问题（已知两段链求第三段）变成矩阵的加减消元：T(BC) = T(BM)·T(MC)⁻¹，已知末端与相机的测量，就能解出安装变换。
变换的两种视角再练一遍
把 1.3 节的两关节臂改造一下：在末端再装一段长 0.2 米、绕自身 z 轴转 θ3 的第三连杆。请分别写出"站在基座看"与"站在末端看"两种视角的变换链，并验证两者结果一致。做完这道练习你会发现，视角只影响"链的书写顺序"，不影响最终矩阵——这正是记号体系的全部意义：把争论从"谁对谁错"变成"按哪条链写"。
问题：欧拉角与四元数什么时候登场
齐次矩阵回答"在哪、朝哪"，但工程日志里直接打印 4×4 矩阵不便阅读，于是有了紧凑表示。欧拉角（偏航、俯仰、横滚）直观但有万向锁陷阱——俯仰 90 度时两个转轴重合，姿态表示退化；四元数用四个数表示姿态、无奇异性、插值平滑，是状态估计与 SLAM 内部的通行表示。工程约定：人读的日志用欧拉角，算法内部与存储用四元数或矩阵，两者转换交给成熟的几何库，不要手搓。