Robotics-从笛卡尔坐标到齐次变换，一次理清位置、姿态与位姿

机器人学基础：从笛卡尔坐标到齐次变换，一次理清位置、姿态与位姿

学习机器人运动学时，我们经常遇到几组看似相近、又不能互相替代的概念：笛卡尔坐标、欧拉角、旋转矩阵、齐次变换和四元数。

它们之间到底是什么关系？为什么一个三维空间中的刚体，会用到一个 4×4 矩阵？矩阵里有 16 个数，是否意味着机器人有 16 个自由度？

要理清这些问题，可以先记住一个基本区别：位置回答“在哪里”，姿态回答“朝向哪里”，位姿则同时回答这两个问题。

1. 笛卡尔坐标与笛卡尔位姿，不是同一个概念

严格地说，一个点的三维笛卡尔坐标

\[\mathbf p=\begin{bmatrix}x&y&z\end{bmatrix}^T \]

只描述位置，不包含姿态。

例如，把一个杯子放在桌面上，再保持杯子中心的位置不变，将杯子转动 90°。杯子中心的三个坐标没有改变，但杯子的朝向已经变了。

在机器人学中，常见的“笛卡尔位姿”或“笛卡尔空间控制”，通常会同时涉及位置和姿态。因此，人们会在三个位置坐标之外，再配上一种姿态表示。例如：

\[\mathbf X= \begin{bmatrix} x&y&z&\alpha&\beta&\gamma \end{bmatrix}^T. \]

这里的后三项可以是某种约定下的欧拉角，但它们不是额外的三个位置坐标。使用欧拉角或 RPY 角时，还必须明确旋转顺序，以及绕固定轴还是随动轴旋转。

名称	描述内容	常见表示
位置 Position	一个点在哪里	三维向量 \(\mathbf p\)
姿态 Orientation	一个坐标系朝向哪里	欧拉角、旋转矩阵、单位四元数
位姿 Pose	位置与姿态	\((\mathbf p,R)\)、齐次变换矩阵、位置加单位四元数

空间刚体有 6 个独立自由度，不等于必须用 6 个数表示。 三个位置坐标加一个旋转矩阵，需要存储 12 个数；加一个单位四元数，需要存储 7 个数。多出来的分量受到约束，并不是新增的自由度。

更严格地说，空间刚体的位姿属于 \(SE(3)\)。用位置加三个姿态角构成的六维向量，是它的一种局部参数化，不能把所有空间姿态都当作没有奇异性的普通三维角度坐标。

2. 为什么给刚体附上一个坐标系？

描述机械臂末端时，我们通常在夹爪或工具上固定一个坐标系。只要知道这个坐标系的原点在哪里、三根轴朝向哪里，就知道了工具的位姿。

设基座坐标系为 A，工具坐标系为 B：

• B 的原点在 A 中的位置，用平移向量 \({}^A\mathbf t_B\) 表示。
• B 的三根坐标轴在 A 中的方向，组成旋转矩阵 \({}^AR_B\)。

于是，同一个点 P 在两套坐标系中的坐标满足：

\[\boxed{{}^A\mathbf p={}^AR_B\,{}^B\mathbf p+{}^A\mathbf t_B}. \]

这条公式可以分成两个动作理解：先把 B 中的坐标分量换算成 A 的轴向表达，再加上 B 原点在 A 中的位置。

注意，左右两边描述的是同一个物理点。这里是在换算坐标，并不意味着点真的移动了。相同形式的矩阵也能表示主动运动，但必须先说明含义，不能混用。

3. 齐次坐标：把旋转和平移写进一次乘法

旋转可以写成矩阵乘法，平移却表现为加法：

\[\mathbf p'=R\mathbf p+\mathbf t. \]

这样的表达本身没有问题，也完全可以组合与求逆。不过，当机械臂存在多级连杆、工具偏移和相机外参时，用统一的矩阵乘法处理变换链会更方便。

做法是给点的坐标增加一个分量：

\[\tilde{\mathbf p}=\begin{bmatrix}x&y&z&1\end{bmatrix}^T. \]

再构造：

\[T=\begin{bmatrix} R&\mathbf t\\ \mathbf 0^T&1 \end{bmatrix}. \]

两者相乘：

\[\begin{bmatrix} R&\mathbf t\\ \mathbf 0^T&1 \end{bmatrix} \begin{bmatrix}\mathbf p\\1\end{bmatrix} = \begin{bmatrix}R\mathbf p+\mathbf t\\1\end{bmatrix}. \]

原来的“乘法加平移”被合并成了一次乘法。第四个分量是数学表示的扩展，并不是多出了一条物理空间轴。 这种 4×4 刚体变换表示也是机器人学中齐次变换的常用定义，参见 Modern Robotics：齐次变换矩阵。

“齐次”到底体现在哪里？

更一般地，齐次坐标写成 \([X,Y,Z,W]^T\)。当 \(W\ne0\) 时，对应的笛卡尔坐标是：

\[\left(\frac XW,\frac YW,\frac ZW\right). \]

将四个分量同时乘以任意非零数，表示的点不变。例如：

\[[1,2,3,1]^T\sim[2,4,6,2]^T. \]

这就是齐次坐标的比例等价性。在机器人刚体变换中，我们通常选取 \(W=1\) 的形式。

4. 为什么点的末位是 1，方向向量的末位是 0？

空间点有具体位置，平移坐标系后，它的坐标会改变。方向向量没有固定起点，因此不应该受平移影响。

将方向向量扩展为：

\[\tilde{\mathbf d}=\begin{bmatrix}d_x&d_y&d_z&0\end{bmatrix}^T, \]

便得到：

\[T\begin{bmatrix}\mathbf d\\0\end{bmatrix} =\begin{bmatrix}R\mathbf d\\0\end{bmatrix}. \]

平移项自动消失。

对象	扩展形式	变换结果
点	\([\mathbf p^T,1]^T\)	同时受到旋转和平移影响
方向或位移向量	\([\mathbf d^T,0]^T\)	只受到旋转影响

可以把末位的 1 和 0 理解为：这个对象是否需要计入原点的平移。

5. 读懂 4×4 矩阵中的每一块

将旋转矩阵的三列记为 \(\mathbf n,\mathbf o,\mathbf a\)，齐次变换可以展开成：

\[{}^AT_B= \begin{bmatrix} n_x&o_x&a_x&t_x\\ n_y&o_y&a_y&t_y\\ n_z&o_z&a_z&t_z\\ 0&0&0&1 \end{bmatrix}. \]

区域	几何意义
第一列前三项 \(\mathbf n\)	B 的 x 轴在 A 中的方向
第二列前三项 \(\mathbf o\)	B 的 y 轴在 A 中的方向
第三列前三项 \(\mathbf a\)	B 的 z 轴在 A 中的方向
第四列前三项 \(\mathbf t\)	B 原点在 A 中的位置
最后一行 \([0,0,0,1]\)	保持这种仿射变换的齐次结构

最后一行不是随意“凑成方阵”的填充。它让点变换后末位仍为 1，让方向向量末位仍为 0，并让这种矩阵在组合时保持相同结构。

同时，左上角也不是任意的 3×3 矩阵。合法旋转矩阵必须满足：

\[R^TR=I,\qquad\det R=1. \]

也就是三根轴彼此垂直、长度为 1，并组成右手系。这些约束解释了为什么矩阵虽然有 16 个元素，刚体位姿仍然只有 6 个独立自由度。

一个能直接算清楚的例子

设 B 相对 A 绕 z 轴旋转 90°，B 原点在 A 中的位置为 \([2,0,0]^T\)：

\[{}^AT_B= \begin{bmatrix} 0&-1&0&2\\ 1&0&0&0\\ 0&0&1&0\\ 0&0&0&1 \end{bmatrix}. \]

点 P 在 B 中的坐标为 \([1,0,0]^T\)，则：

\[{}^AT_B \begin{bmatrix}1\\0\\0\\1\end{bmatrix} =\begin{bmatrix}2\\1\\0\\1\end{bmatrix}. \]

因此 P 在 A 中的位置是 \([2,1,0]^T\)：它沿 B 的 x 轴前进 1，而 B 的 x 轴此时指向 A 的 y 轴方向。

6. 连乘与求逆：齐次变换为什么适合机械臂？

如果已知 B 相对 A 的变换，以及 C 相对 B 的变换，那么：

\[\boxed{{}^AT_C={}^AT_B\,{}^BT_C}. \]

一个实用记法是“中间坐标系对接”：A←B 与 B←C 相接，得到 A←C。这里采用列向量约定，最右侧的矩阵先作用于点坐标。

直接展开乘法还能得到：

\[\begin{bmatrix}R_1&\mathbf t_1\\0&1\end{bmatrix} \begin{bmatrix}R_2&\mathbf t_2\\0&1\end{bmatrix} = \begin{bmatrix} R_1R_2&R_1\mathbf t_2+\mathbf t_1\\0&1 \end{bmatrix}. \]

这提醒我们：平移向量必须先统一到同一个坐标系，不能直接把不同坐标系中的位移相加。

逆变换也有清晰的结构：

\[\boxed{ T^{-1}=\begin{bmatrix} R^T&-R^T\mathbf t\\0&1 \end{bmatrix}}. \]

逆变换的平移部分是 \(-R^T\mathbf t\)，而不只是 \(-\mathbf t\)。旋转和平移一般也不可交换，矩阵连乘的次序必须与坐标系关系一致。

7. 与 D-H 参数法的联系：每一行都是一个相邻坐标变换

D-H 参数法的作用，是通过有规则的建系方式，用四个参数描述相邻连杆坐标系之间的变换。

标准 D-H 常用形式为：

\[{}^{i-1}T_i= R_z(\theta_i)T_z(d_i)T_x(a_i)R_x(\alpha_i). \]

Craig 改进 D-H 常用形式为：

\[{}^{i-1}T_i= R_x(\alpha_{i-1})T_x(a_{i-1})R_z(\theta_i)T_z(d_i). \]

其中 \(R_x,R_z,T_x,T_z\) 均表示对应的 4×4 基本变换。两种约定的公式可对照 Robotics Toolbox 的 D-H 文档。

标准 D-H 与改进 D-H 不只是把同一张表里的四个矩阵换个顺序。 它们还涉及坐标系放置、关节轴编号以及参数下标的差异，必须使用各自配套的坐标图与参数表。

当各段变换确定后，正运动学就是沿运动链连乘：

\[{}^0T_E={}^0T_1\,{}^1T_2\cdots{}^{n-1}T_n\,{}^nT_E. \]

最后一项可以是固定工具变换，并不一定代表额外的运动关节。例如，一个四关节 RPRR 机构可以使用四个关节变换，再接一个固定的工具变换。

从结果矩阵中读取：

\[{}^0T_E=\begin{bmatrix}{}^0R_E&{}^0\mathbf p_E\\0&1\end{bmatrix}, \]

右上角是工具原点在基座中的位置，左上角是工具坐标系相对基座的姿态。

建系时应先确认真实关节轴，再根据采用的 D-H 约定选择公垂线、原点与轴向；连杆的几何偏移仍然要完整反映在参数中，不能只画对方向却漏掉距离。

8. 四元数放在哪里？它会包含位置吗？

用于表示旋转的单位四元数不包含位置。 它与旋转矩阵一样，描述的是姿态。

为避免与位置坐标混淆，将单位四元数写成：

\[\mathbf q_R=[q_w,q_x,q_y,q_z]^T, \qquad q_w^2+q_x^2+q_y^2+q_z^2=1. \]

若旋转轴的单位方向为 \(\mathbf u\)，旋转角为 \(\theta\)，则可以表示为：

\[\mathbf q_R= \begin{bmatrix} \cos(\theta/2)\\ \mathbf u\sin(\theta/2) \end{bmatrix}. \]

四个分量共同描述一个旋转；其中的 \(q_x,q_y,q_z\) 不是位置坐标。单位四元数避免了欧拉角的万向节锁，但有单位模约束，而且 \(\mathbf q_R\) 与 \(-\mathbf q_R\) 表示同一个姿态。进行插值时仍需选择合适的算法并处理符号一致性。

完整位姿可以用两种常见形式保存：

保存形式	组成	常见用途
位置加单位四元数	3 个位置分量 + 4 个四元数分量	位姿消息、姿态插值
齐次变换矩阵	旋转矩阵 + 位置向量	坐标换算、运动链连乘

ROS 的 geometry_msgs/Pose 就将位置和四元数姿态分别保存；其四元数字段顺序为 x, y, z, w，与本文公式使用的标量在前写法不同。使用接口时必须核对顺序，参见 Pose 消息定义 与 Quaternion 消息定义。

若采用双四元数，则可以统一表示旋转和平移，但那是另一种结构，不能与这里描述旋转的普通单位四元数混为一谈。

9. 再往前一步：为什么雅可比矩阵也会受表示方式影响？

对同一个末端位姿，可以选择不同的任务坐标：

任务坐标	分量数
只取笛卡尔位置	3
位置 + 三个姿态角	6
位置 + 单位四元数	7
位置 + 展开的旋转矩阵	12

若将选定的任务坐标记为 \(\mathbf X=f(\mathbf q)\)，则直接求导得到：

\[\dot{\mathbf X}=J_{\mathbf X}(\mathbf q)\dot{\mathbf q}, \qquad J_{\mathbf X}=\frac{\partial f}{\partial\mathbf q}. \]

任务坐标的表示改变，直接求导得到的雅可比矩阵也会改变。七维或十二维表示的分量之间存在约束，不意味着末端拥有七个或十二个独立自由度。

还要注意：欧拉角变化率、四元数变化率、旋转矩阵元素变化率，都不能直接等同于三维角速度向量 \(\boldsymbol\omega\)。

描述点的位置时使用笛卡尔坐标，描述工具朝向时选择一种姿态表示，需要在坐标系之间换算或连接多级运动链时使用齐次变换。把这三件事分清楚，后面的 D-H 建模、正运动学和雅可比推导就能衔接起来。