位姿描述与坐标变换¶

要控制机器人，首先必须能精确描述物体在空间中的位置和姿态——这是机器人学的数学基础。

一、刚体的位置和姿态描述¶

1.1 位置描述¶

在三维空间中，一个点的位置用位置向量描述：

\[ \mathbf{p} = \begin{bmatrix} p_x \\ p_y \\ p_z \end{bmatrix} \]

但这只说明了"在哪里"，没有说"朝哪个方向"——描述刚体还需要姿态。

1.2 姿态描述——旋转矩阵¶

用一个与刚体固连的坐标系 \(\{B\}\) 相对于参考坐标系 \(\{A\}\) 的方向来描述姿态。将 \(\{B\}\) 的三个坐标轴单位向量 \(\hat{x}_B, \hat{y}_B, \hat{z}_B\) 在 \(\{A\}\) 中表达，构成旋转矩阵：

\[ {}^A_B R = \begin{bmatrix} \hat{x}_B \cdot \hat{x}_A & \hat{y}_B \cdot \hat{x}_A & \hat{z}_B \cdot \hat{x}_A \\ \hat{x}_B \cdot \hat{y}_A & \hat{y}_B \cdot \hat{y}_A & \hat{z}_B \cdot \hat{y}_A \\ \hat{x}_B \cdot \hat{z}_A & \hat{y}_B \cdot \hat{z}_A & \hat{z}_B \cdot \hat{z}_A \end{bmatrix} \]

旋转矩阵的每一列就是 \(\{B\}\) 的一个坐标轴在 \(\{A\}\) 中的单位向量表达。

旋转矩阵的关键性质

\(R \in SO(3)\)：\(R^T R = I\)，\(\det(R) = 1\)
逆 = 转置：\(R^{-1} = R^T\)（这使得旋转的"撤销"计算非常高效）
列向量两两正交，且均为单位向量

1.3 基本旋转矩阵¶

绕三个坐标轴旋转角度 \(\theta\) 的旋转矩阵：

绕 \(x\) 轴旋转：

\[ R_x(\theta) = \begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos\theta & -\sin\theta \\ 0 & \sin\theta & \cos\theta \end{bmatrix} \]

绕 \(y\) 轴旋转：

\[ R_y(\theta) = \begin{bmatrix} \cos\theta & 0 & \sin\theta \\ 0 & 1 & 0 \\ -\sin\theta & 0 & \cos\theta \end{bmatrix} \]

绕 \(z\) 轴旋转：

\[ R_z(\theta) = \begin{bmatrix} \cos\theta & -\sin\theta & 0 \\ \sin\theta & \cos\theta & 0 \\ 0 & 0 & 1 \end{bmatrix} \]

旋转的非交换性

矩阵乘法不满足交换律：\(R_x(\alpha) R_z(\beta) \neq R_z(\beta) R_x(\alpha)\)。旋转的顺序非常重要！

1.4 组合旋转：固定轴 vs 当前轴¶

对同一最终姿态，旋转矩阵的乘法顺序取决于旋转轴的参考系：

绕固定轴（外旋 / Extrinsic）旋转：先 \(X\)，再 \(Y\)，再 \(Z\)

\[ R = R_z \cdot R_y \cdot R_x \quad \text{（后做的放左边）} \]

绕当前轴（内旋 / Intrinsic）旋转：先 \(Z'\)，再 \(Y'\)，再 \(X'\)

\[ R = R_z \cdot R_y \cdot R_x \quad \text{（先做的放左边）} \]

记忆方法

固定轴旋转：按操作顺序从右往左写（左乘）
当前轴旋转：按操作顺序从左往右写（右乘）
注意：固定轴 XYZ 旋转和当前轴 ZYX 旋转的结果完全相同

1.5 位姿 = 位置 + 姿态¶

一个刚体在空间中的位姿（Pose）由位置和姿态共同决定，可以用一个坐标系 \(\{B\}\) 完整描述：

\[ \{B\} = \{{}^A_B R, \; {}^A p_B\} \]

其中 \({}^A_B R\) 描述姿态，\({}^A p_B\) 描述坐标系原点的位置。

二、坐标变换¶

2.1 问题：同一个点，不同坐标系下的坐标是什么？¶

已知点 \(P\) 在坐标系 \(\{B\}\) 中的坐标为 \({}^B p\)，求 \(P\) 在坐标系 \(\{A\}\) 中的坐标 \({}^A p\)。

旋转变换（只有姿态差异，无平移）：

\[ {}^A p = {}^A_B R \; {}^B p \]

一般变换（既有旋转又有平移）：

\[ {}^A p = {}^A_B R \; {}^B p + {}^A p_{B_{\text{org}}} \]

其中 \({}^A p_{B_{\text{org}}}\) 是 \(\{B\}\) 原点在 \(\{A\}\) 中的位置。

直觉理解

旋转矩阵把"\(\{B\}\) 的语言"翻译成"\(\{A\}\) 的语言"，平移向量再做一次位移校正。

2.2 组合变换¶

对于多次连续变换（经过多个坐标系），变换关系可以链式相乘：

\[ {}^A p = {}^A_B R \cdot {}^B_C R \; {}^C p + {}^A_B R \; {}^B p_{C_{\text{org}}} + {}^A p_{B_{\text{org}}} \]

这个表达式比较复杂——于是引入了齐次坐标来统一旋转和平移。

三、齐次坐标与齐次变换¶

3.1 齐次坐标¶

将三维坐标扩展为四维，在末尾添加一个 1：

\[ \tilde{p} = \begin{bmatrix} p_x \\ p_y \\ p_z \\ 1 \end{bmatrix} \]

这样就能用一个矩阵同时表达旋转和平移。

3.2 齐次变换矩阵¶

\[ {}^A_B T = \begin{bmatrix} {}^A_B R & {}^A p_{B_{\text{org}}} \\ \mathbf{0}^T & 1 \end{bmatrix} = \begin{bmatrix} r_{11} & r_{12} & r_{13} & p_x \\ r_{21} & r_{22} & r_{23} & p_y \\ r_{31} & r_{32} & r_{33} & p_z \\ 0 & 0 & 0 & 1 \end{bmatrix} \]

其中：

左上 3×3：旋转矩阵 \(R\)（描述姿态）
右上 3×1：平移向量 \(p\)（描述位置）
最后一行：固定为 \([0 \; 0 \; 0 \; 1]\)

坐标变换变得极其简洁：

\[ {}^A \tilde{p} = {}^A_B T \; {}^B \tilde{p} \]

3.3 齐次变换的性质¶

链式组合：

\[ {}^A_C T = {}^A_B T \cdot {}^B_C T \]

这就是为什么齐次变换如此重要

不管要经过多少个坐标系，只需把变换矩阵依次左乘即可。这正是机器人正运动学的基础。

求逆：

\[ {}^B_A T = ({}^A_B T)^{-1} = \begin{bmatrix} R^T & -R^T p \\ \mathbf{0}^T & 1 \end{bmatrix} \]

注意：不需要通用的 \(4 \times 4\) 矩阵求逆算法，利用旋转矩阵的特殊性质（逆 = 转置）即可高效计算。

3.4 纯旋转与纯平移的齐次矩阵¶

纯平移（无旋转）：

\[ T_{\text{trans}}(d_x, d_y, d_z) = \begin{bmatrix} 1 & 0 & 0 & d_x \\ 0 & 1 & 0 & d_y \\ 0 & 0 & 1 & d_z \\ 0 & 0 & 0 & 1 \end{bmatrix} \]

纯旋转（无平移），例如绕 \(z\) 轴旋转 \(\theta\)：

\[ T_{R_z}(\theta) = \begin{bmatrix} \cos\theta & -\sin\theta & 0 & 0 \\ \sin\theta & \cos\theta & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \]

四、其他姿态表示方法¶

旋转矩阵虽然完整，但参数多（9 个，实际上只有 3 个自由度）。以下是其他常用表示：

4.1 欧拉角（Euler Angles）¶

用三次绕坐标轴的旋转来描述姿态。最常见的约定：

ZYX 欧拉角（RPY 角）：

\[ R = R_z(\alpha) \cdot R_y(\beta) \cdot R_x(\gamma) \]

其中 \(\alpha\) = Yaw（偏航），\(\beta\) = Pitch（俯仰），\(\gamma\) = Roll（翻滚）。

万向节死锁（Gimbal Lock）

当 \(\beta = \pm 90°\) 时，两个旋转轴重合，丧失一个自由度——无法区分 \(\alpha\) 和 \(\gamma\) 的单独效果。这是所有三参数姿态表示的固有问题。

4.2 轴角表示（Axis-Angle）¶

用一个旋转轴 \(\hat{k}\) 和一个旋转角度 \(\theta\) 表示：任意旋转都等价于绕某个轴旋转一个角度。

\[ R = I + \sin\theta \; [\hat{k}]_\times + (1 - \cos\theta) \; [\hat{k}]_\times^2 \]

这就是 Rodrigues 旋转公式，其中 \([\hat{k}]_\times\) 是 \(\hat{k}\) 的反对称矩阵：

\[ [\hat{k}]_\times = \begin{bmatrix} 0 & -k_z & k_y \\ k_z & 0 & -k_x \\ -k_y & k_x & 0 \end{bmatrix} \]

4.3 四元数（Quaternion）¶

用 4 个参数（1 个标量 + 3 个向量分量）表示旋转：

\[ q = w + xi + yj + zk = (w, \mathbf{v}) \]

其中 \(\|q\| = 1\)（单位四元数）。

表示方法	参数个数	优点	缺点
旋转矩阵	9（3 自由度 + 6 约束）	直接用于变换计算	冗余，需保持正交性
欧拉角	3	直觉性强	万向节死锁
轴角	4（3 轴 + 1 角）	几何意义清晰	不便于组合
四元数	4（1 约束）	无万向节死锁，插值平滑，计算高效	不直观

实际工程中的选择

理论推导和运动学建模：旋转矩阵 / 齐次变换
姿态控制和轨迹插值：四元数（避免万向节死锁，球面线性插值 SLERP）
人机交互和直觉设定：欧拉角（容易理解）

五、总结¶

理解坐标变换的核心思路：

位置 → 向量描述
姿态 → 旋转矩阵（或欧拉角、四元数等）
位姿 → 齐次变换矩阵，将旋转 + 平移统一为一个 \(4 \times 4\) 矩阵
多坐标系链 → 齐次变换矩阵连乘

\[ {}^0 T_n = {}^0 T_1 \cdot {}^1 T_2 \cdots {}^{n-1} T_n \]

这个公式就是机器人正运动学的核心工具。