第 1 章 空间描述与刚体变换¶
位姿（pose）是机器人学的第一等对象：一个刚体在空间中的"在哪里、朝哪里"。描述它的前提，是先约定"相对于谁来说"。本章建立参考坐标系、点的坐标、旋转与平移算子，并把二者合并成一个 4×4 的齐次变换矩阵 \(T \in SE(3)\)——后续运动学、动力学与规划里几乎所有矩阵运算都以它为原子。符号以 Lynch & Park《Modern Robotics》体系为主，Craig 记号在对照处标注。
多个坐标系可以串联复合，下标顺序决定矩阵相乘的次序：
flowchart LR
A["{A}"] -->|"T_AB = [R p; 0 1]"| B["{B}"]
B -->|"T_BC"| C["{C}"]
A -.->|"T_AC = T_AB · T_BC<br/>复合按右乘串联"| C1. 参考坐标系与点的坐标¶
坐标系（frame）\(\{s\}\) 由一个原点 \(O_s\) 与三个两两正交的单位轴 \(\hat{x}_s,\hat{y}_s,\hat{z}_s\) 构成，右手系满足 \(\hat{z}_s = \hat{x}_s \times \hat{y}_s\)。脱离坐标系谈"坐标"没有意义：同一个点 \(p\) 相对 \(\{s\}\) 与相对另一个坐标系 \(\{b\}\) 有两组不同的数。
点 \(p\) 相对 \(\{s\}\) 的坐标记作 \({}^{s}p \in \mathbb{R}^3\)，它是三个带符号的投影。约定上标写"在哪个坐标系里看"，下标写"这是哪个坐标系"（活动坐标系本身用 \(\{b\}\) 标记）。刚体位姿由两部分组成：本体固连坐标系 \(\{b\}\) 的原点相对 \(\{s\}\) 的位置 \({}^{s}p \in \mathbb{R}^3\)，加上 \(\{b\}\) 三轴相对 \(\{s\}\) 三轴的朝向（orientation）——朝向用旋转矩阵表示，见「平移与旋转算子」一节。
位置（position）与向量（vector）是两类量：位置是"离原点多远"的点，向量是"有大小无作用点"的自由量（如速度、力）。二者在给定坐标系下都用 \(\mathbb{R}^3\) 三元组表示，但在齐次坐标下用末位 \(1\) 与末位 \(0\) 区分（见「齐次变换与 SE(3)」）。
2. 平移与旋转算子¶
旋转矩阵（rotation matrix）\(R \in SO(3)\) 是"朝向"的代数载体。它把 \(\{b\}\) 下的坐标搬到 \(\{s\}\) 下：
其中 \(d \in \mathbb{R}^3\) 是 \(\{b\}\) 原点相对 \(\{s\}\) 的位置，\(R\) 的三列正是 \(\hat{x}_b,\hat{y}_b,\hat{z}_b\) 在 \(\{s\}\) 下的坐标。绕基本轴的三个旋转矩阵为
\(SO(3)\) 的定义是满足 \(R^\top R = I\)、\(\det R = +1\) 的 \(3\times3\) 矩阵集合：正交且不退化、保手性。旋转的四种表示与互转见第 2 章。
平移与旋转各自是线性算子，但"先转后移"这类刚体运动是二者的组合，用 \(3\times3\) 的 \(R\) 单独表达不下平移分量。
3. 齐次变换与 SE(3)¶
把旋转与平移打包进一个 \(4\times4\) 矩阵，就得到齐次变换（homogeneous transformation）：
分块 \(R\in SO(3)\) 管朝向、\(d\in\mathbb{R}^3\) 管位置，末行 \((0,0,0,1)\) 保证变换后再齐次化。点的末位取 \(1\)、向量的末位取 \(0\)，于是同一个 \(T\) 作用于向量时只旋转不平移——这正是用齐次坐标区分两类的动机。\(SE(3)\) 指全体此类 \(4\times4\) 齐次变换矩阵在矩阵乘法下构成的群（special Euclidean group）。
import numpy as np
def rot_z(t): # 绕 z 轴旋转 θ
c, s = np.cos(t), np.sin(t)
return np.array([[c, -s, 0.], [s, c, 0.], [0., 0., 1.]])
def homog(R, d): # 把 R、d 组装成 4x4 齐次变换
T = np.eye(4)
T[:3, :3] = R
T[:3, 3] = d
return T
T = homog(rot_z(np.pi/2), np.array([1., 2., 3.]))
p_b = np.array([1., 0., 0.])
p_s = (T @ np.append(p_b, 1.))[:3] # 齐次坐标下作用于点
print(p_s) # [1. 3. 3.]：先绕 z 转 90° 再平移 (1,2,3)
对 \(p_b=(1,0,0)\) 施加"绕 \(z\) 转 \(90^\circ\)、再平移 \((1,2,3)\)"，齐次作用给出 \((1,3,3)\)，与分块公式 \(Rp_b+d=(0,1,0)+(1,2,3)\) 逐元素一致。
4. 变换的求逆与分块形式¶
齐次变换的逆有闭式分块解，无需调用通用 \(4\times4\) 求逆：
理由：旋转部分逆即转置（\(R^{-1}=R^\top\)），而平移要先把原点搬回、再反向旋转，故为 \(-R^\top d\) 而非 \(-d\)。这是最常见的符号坑——忘记转置会把逆算错。
R = rot_z(np.pi/2); d = np.array([1., 2., 3.])
T = homog(R, d)
T_inv = np.eye(4)
T_inv[:3, :3] = R.T
T_inv[:3, 3] = -R.T @ d # 关键：-R^T d，不是 -d
assert np.allclose(T_inv, np.linalg.inv(T)) # 与通用求逆差 0.0
上例的分块逆为
[[ 0. 1. 0. -2.]
[-1. 0. 0. 1.]
[ 0. 0. 1. -3.]
[ 0. 0. 0. 1.]]
它与 numpy.linalg.inv 的结果差为 \(0.0\)，但计算量小得多、且天然保持 \(SE(3)\) 结构（数值积分后不必再投影回流形）。
5. 帧的复合与相对描述¶
多个坐标系之间的变换可串联。若已知 \(\{s\}\) 相对 \(\{c\}\) 的 \(T_{sc}\)、\(\{c\}\) 相对 \(\{b\}\) 的 \(T_{cb}\)，则 \(\{s\}\) 相对 \(\{b\}\) 为
下标像分数一样"约掉"中间的 \(\{c\}\)，这个记法便于检查链式变换是否接对。矩阵乘法不可交换，\(T_{sc}T_{cb}\neq T_{cb}T_{sc}\)，因此书写顺序即复合顺序，不能随意调换。
由 \(T_{sb}=T_{sc}T_{cb}\) 可解出任意"相对变换"：\(T_{cb}=T_{sc}^{-1}T_{sb}\)、\(T_{sc}=T_{sb}T_{bc}\)。给定两个坐标系在第三坐标系下的描述，就能求二者之间的相对位姿，这是多传感器外参、手眼标定的公共骨架。
6. 固定轴与活动轴旋转¶
对一个当前位姿 \(T\) 再叠加一次旋转，左乘与右乘对应两种物理意义：
- 左乘（固定轴 / 外旋，extrinsic）：\(T' = R_{\text{fixed}}\,T\)，新旋转绕固定的基坐标系 \(\{s\}\) 轴施加。
- 右乘（活动轴 / 内旋，intrinsic）：\(T' = T\,R_{\text{current}}\)，新旋转绕已经转动过、随之移动的当前轴施加。
同一组角度，两种约定的复合矩阵不同。绕 \(x,y,z\) 各转 \(90^\circ\) 时：
A = rot_z(r) @ rot_x(r) # 先绕 x，再绕固定 z（后施加者左乘）
B = rot_x(r) @ rot_z(r) # 先绕 z，再绕固定 x
## A = [[0,0,1],[1,0,0],[0,1,0]]，B = [[0,-1,0],[0,0,-1],[1,0,0]]
## np.linalg.norm(A - B) = 2.449489742783178
二者的 Frobenius 差为 \(2.449\ldots\)，直观说明旋转次序不可交换。把"依次绕哪些轴"与"角度"整体反序，内旋与外旋则严格等价：绕活动轴 \(X(\alpha)\!\to\!Y(\beta)\!\to\!Z(\gamma)\) 与绕固定轴 \(Z(\gamma)\!\to\!Y(\beta)\!\to\!X(\alpha)\) 给出同一矩阵。这条等价关系是欧拉角内外旋约定混乱的根源，第 2 章展开。
7. 要点¶
- 位姿 = 位置 \({}^{s}p\) + 朝向 \(R\)；一切"坐标"都相对于某个坐标系而言，上标注"在哪看"。
- 齐次变换 \(T=\begin{bmatrix}R&d\\0&1\end{bmatrix}\in SE(3)\) 把旋转与平移合成一次矩阵乘法；点末位取 \(1\)、向量末位取 \(0\)。
- 逆有分块闭式 \(T^{-1}=\begin{bmatrix}R^\top&-R^\top d\\0&1\end{bmatrix}\)，平移是 \(-R^\top d\) 不是 \(-d\)，且比通用求逆更省、更保结构。
- 复合 \(T_{sb}=T_{sc}T_{cb}\) 下标可约，但矩阵乘法不可交换，顺序即语义。
- 左乘绕固定轴、右乘绕活动轴；同角度下两约定矩阵不同，轴序与角度整体反序时内旋等价于外旋。