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\sim2,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 建模、正运动学和雅可比推导就能衔接起来。