从旋转矩阵到齐次变换：机器人空间运动的数学语言-平芜编程栈

1. 刚体位姿描述：从坐标系到旋转矩阵

想象你桌上放着一部手机。要精确描述它的摆放状态，我们需要两个关键信息：位置（放在桌子的哪个位置）和姿态（是横放、竖放还是斜着放）。这就是机器人学中的位姿描述问题。

最常用的方法是在手机（运动刚体）上建立一个坐标系{B}，在桌子（固定参考系）上建立坐标系{A}。通过描述{B}相对于{A}的状态，就能完整表达手机的位姿：

位置描述：用坐标系{B}的原点在{A}中的坐标表示，比如(2,3,0)表示手机中心距离桌子参考点向右2单位、向前3单位。
姿态描述：用旋转矩阵表示。这个3×3矩阵的每一列都是{B}的X/Y/Z轴在{A}中的方向向量。例如手机竖放时，它的Y轴可能指向桌子的Z轴方向。

绕单轴旋转的矩阵特别简单：

# 绕X轴旋转θ角的旋转矩阵 Rx = [[1, 0, 0], [0, cosθ, -sinθ], [0, sinθ, cosθ]]

这种矩阵有个重要特性：正交性。意味着它的逆矩阵等于转置矩阵，这在计算坐标变换时非常方便。

2. 坐标变换：当位置和姿态同时变化

实际场景中，物体往往既有平移又有旋转。假设手机先旋转了一定角度，又被平移到了桌面另一个位置，这时如何计算手机表面某点在桌面坐标系中的坐标？

2.1 分步处理法

传统方法是分两步计算：

旋转：用旋转矩阵处理方向变化
```
P_{rotated} = R \cdot P_{local}
```
平移：加上位置偏移量
```
P_{world} = P_{rotated} + P
```

但这种方法在连续变换时会变得繁琐。比如机械臂有多个关节时，每增加一个关节就需要多一次旋转和平移运算。

2.2 齐次坐标的魔法

数学家们想出了个聪明办法：齐次坐标。通过增加一个维度，把旋转和平移合并成一个4×4矩阵：

T = [[r11, r12, r13, px], [r21, r22, r23, py], [r31, r32, r33, pz], [0, 0, 0, 1]]

其中左上角3×3是旋转矩阵，右上角3×1是平移向量。这样，复杂的变换可以用一次矩阵乘法完成：

P_{world} = T \cdot P_{local}

实测案例：假设手机旋转90度后平移到(5,4,0)，屏幕上某点局部坐标(3,2,0)的世界坐标计算：
import numpy as np T = np.array([[0,-1,0,5], [1,0,0,4], [0,0,1,0], [0,0,0,1]]) P_local = np.array([3,2,0,1]) # 齐次坐标 P_world = T @ P_local # 输出：[3,7,0,1]

3. 齐次变换的进阶应用

3.1 运动合成的两种方式

齐次变换矩阵相乘时，顺序不同会导致完全不同的物理意义：

相对固定坐标系：新运动是相对于原始参考系的。这时要从右往左乘矩阵，就像穿衣服时先内衣再外套。
```
T_{total} = T_{move2} \cdot T_{move1}
```
相对运动坐标系：新运动是相对于物体当前位姿的。这时要从左往右乘矩阵，就像先戴眼镜再调整眼镜腿。
```
T_{total} = T_{move1} \cdot T_{move2}
```

3.2 逆变换：换个视角看问题

已知{B}相对于{A}的变换矩阵，如何求{A}相对于{B}的变换？不需要复杂求逆，利用旋转矩阵的正交性可以直接构造：

T_B^A = \begin{bmatrix} R_A^B & -R_A^B \cdot P \\ 0 & 1 \end{bmatrix}

这在机器人视觉中特别有用。比如已知相机相对于机械臂的位姿，就能快速计算机械臂在相机视角中的位置。

4. 欧拉角：直观但危险的姿态描述

虽然旋转矩阵很强大，但人类更习惯用角度思考。欧拉角用三个绕轴旋转的角度来描述姿态，比如无人机控制中的：

偏航(Yaw)：左右转头
俯仰(Pitch)：上下点头
滚转(Roll)：左右歪头

# ZYX欧拉角转旋转矩阵 def euler_to_matrix(yaw, pitch, roll): Rz = [[cos(yaw), -sin(yaw), 0], [sin(yaw), cos(yaw), 0], [0, 0, 1]] Ry = [[cos(pitch), 0, sin(pitch)], [0, 1, 0], [-sin(pitch), 0, cos(pitch)]] Rx = [[1, 0, 0], [0, cos(roll), -sin(roll)], [0, sin(roll), cos(roll)]] return Rz @ Ry @ Rx

但欧拉角有个致命缺陷——万向节死锁。当俯仰角为±90度时，偏航和滚转会失去独立性。这就是为什么航天器姿态控制要慎用欧拉角。