机器人如何描述空间：坐标系、位置、姿态与参考系

《机器人软件与具身智能：原理、架构与工程实践》 · 第一部分：机器人基础知识 · 第 02 篇

本篇承接第一篇的系统全景，从一个二维例子出发，逐步理解三维旋转、位姿变换和 TF。数值例子只需 Python 与 NumPy，不需要安装 ROS 或连接机器人。

相机告诉机器人：“杯子在前方 80 厘米。”机械臂却不能立刻向前伸出 80 厘米。

因为相机的“前方”是沿着镜头看的方向，机械臂的“前方”来自它自己的安装方式；相机与手臂也不在同一个位置。如果底盘已经移动，这条观测还可能属于过去的时刻。

上一期提到，机器人数据必须带上坐标系和时间。本篇把这件事展开：一个空间数值究竟描述了什么，我们怎样把同一个目标从相机的语言翻译成机械臂的语言？

图 1：底盘、相机、工具和物体各自拥有位置与朝向，软件需要把它们联系起来。本图依据八界 URDF 的比例参考和实拍外观生成，为场景示意，非实机拍摄或尺寸测量图。

1. 坐标不是物体的属性，而是相对于一套基准的描述

说“杯子在 (0.8, 0.2, 0.6)”，信息还不完整。首先要确定三个问题：原点在哪里，轴朝哪个方向，数值用什么单位。

一套三维直角坐标系，可以看作空间中的一个原点，以及从该原点出发的三根相互垂直的单位轴。把它固定在地面上，就可以描述机器人如何移动；把它固定在相机上，就可以描述目标相对镜头的位置。

物体没有移动，换一套坐标系，坐标值也会改变。反过来，坐标值没有变化，如果参考系本身动了，物体在世界中的位置仍可能已经改变。

这里先区分四个术语：

术语	本篇采用的含义	例子
坐标系 / 参考系 frame	用来表达空间关系的原点和轴，本篇不讨论惯性系等动力学区别	地图、机器人基座、相机光学坐标系
位置 position	某个点相对参考系原点的坐标	杯柄上的一个点在基座系中为 (x, y, z)
姿态 / 朝向 orientation	一组物体轴相对参考轴的方向关系	夹爪朝下还是朝前
位姿 pose	刚体坐标系的位置与朝向共同构成的空间状态	夹爪原点的位置，加上夹爪三根轴的朝向

中文资料有时会把 pose 也简称为“姿态”，阅读接口时应检查它是否同时包含平移与旋转。一个三维点不等于一个六自由度位姿。 即使末端位置完全相同，夹爪从上方接近杯子和从侧面接近杯子，也不是同一个抓取目标。

三维刚体位姿有三个平移自由度、三个旋转自由度。使用四元数或矩阵后，存储的数字可以多于六个，但这些数字受到约束，并不意味着刚体多出了自由度。

2. 用一个能手算的例子，理解“换参考系”

先暂时不看三维机械臂，只看平面上的 A、B 两套坐标系。

• A 的原点位于 (0, 0)，x 轴向右、y 轴向上。
• B 的原点在 A 中位于 (1.0, 0.5) m。
• B 的轴相对 A 逆时针转了 90°，因此 B 的 x 轴朝上，B 的 y 轴朝左。
• 同一个点 P，在 B 中的坐标是 (0.4, 0.2) m。
  

图 2：从 B 的原点沿 x_B 走 0.4 米，再沿 y_B 走 0.2 米，到达点 P。把这段路改用 A 的轴描述，就是向上 0.4 米、向左 0.2 米。

从图上直接算：先找到 B 的原点，再加上向上和向左的位移。

p_A = (1.0, 0.5) + (-0.2, 0.4)
    = (0.8, 0.9) m

这不是把杯子从一个地方搬到了另一个地方，而是为同一个点换了一套描述。

如果忽略 B 的朝向，只做平移，就会算成 (1.4, 0.7)；如果只做旋转、忘了原点不重合，就会算成 (-0.2, 0.4)。这两类错误都能产生数值上看起来正常的结果，程序也未必会报错。

把上面的操作写成通用形式，就是：

p_A = R_A_B × p_B + t_A_B

本文统一约定：点使用列向量；R_A_B 把 B 中表达的方向换成 A 中的表达；t_A_B 是 B 原点在 A 中的位置。下标从右向左读：B → A。

这个约定会一直用到文末。读其他库时不能默认它使用相同记号，更不能只看到 transform 这个名字就猜方向。

3. 旋转矩阵为什么长这样：每一列都有空间含义

3.1 从三根轴构造矩阵

把上一节扩展到三维：B 相对 A 绕 z 轴正向转 90°，z 轴保持向上。B 的三根单位轴，在 A 中分别是：

x_B 在 A 中 = (0, 1, 0)
y_B 在 A 中 = (-1, 0, 0)
z_B 在 A 中 = (0, 0, 1)

把三个向量按列排好，得到：

          x_B  y_B  z_B
R_A_B = [[ 0,  -1,   0],
         [ 1,   0,   0],
         [ 0,   0,   1]]

这里最值得记住的是：旋转矩阵的第 i 列，描述源坐标系第 i 根轴在目标坐标系中的方向。 这为矩阵中的每个数字提供了几何解释。[1]

图 3：本图把两个原点重合，只观察朝向。彩色轴不是另一个位置，而是 B 的轴方向在 A 中的表示。

再看矩阵乘法，就不必把它当作抽象口诀。p_B = (0.4, 0.2, 0) 意味着取 0.4 份 x_B、0.2 份 y_B、0 份 z_B，相加后得到相对 B 原点的位移：

R_A_B × p_B
= 0.4 × (0, 1, 0) + 0.2 × (-1, 0, 0)
= (-0.2, 0.4, 0)

最后再加上 B 原点的位置，才是 P 在 A 中的完整位置。

3.2 位置和方向为什么不能使用同一套平移规则

假设一个相机给出了物体表面的法向量，或者任务指定了夹爪的接近方向。它表达的是方向，没有“这个向量自己的空间原点”需要一起搬过去。

因此，刚体变换下的两条规则是：

点的位置：p_A = R_A_B × p_B + t_A_B
自由方向：v_A = R_A_B × v_B

给方向向量加上平移，会改变它的方向和长度，产生错误。这里讲的是自由方向向量；机器人速度、角速度、力和力矩还有各自的变换规则，不能把这一行扩展成“所有带 vector 的量都只乘 R”。

3.3 如何检查一个矩阵像不像合法旋转

纯旋转保持长度和夹角，因此三根轴应互相垂直、各自长度为 1。对右手坐标系，还需要满足：

R.T × R = I
det(R) = +1

如果行列式是 -1，它包含镜像翻转，不是本文使用的正常三维旋转。浮点计算中应使用误差容限检查，而不是要求每项精确相等。

这两个检查能发现非正交、镜像、数值损坏等问题，却不能判断变换方向是否用反：R 与 R.T 都可能是合法旋转。它们也不能证明外参标定准确，一个完全合法的旋转矩阵，仍可能描述了错误的安装角度。

4. 欧拉角和四元数：两种表达，同一个朝向

4.1 “转 90°”还没有把事情说完整

绕哪根轴？这根轴是固定在世界上，还是跟着机器人转？多个旋转按什么顺序应用？

三维旋转通常不能交换顺序。取一个初始沿 x 轴的向量 e_x = (1, 0, 0)，两次操作都绕固定参考系 A 的轴，按右手规则转正 90°：

先绕 z，再绕 x：
(1, 0, 0) → (0, 1, 0) → (0, 0, 1)

先绕 x，再绕 z：
(1, 0, 0) → (1, 0, 0) → (0, 1, 0)

图 4：两个结果的原点相同，朝向不同。这里是在同一参考系内主动转动物体；前面几节是在物体不动的情况下改变坐标表达，要区分这两种用途。

对列向量，先作用的矩阵写在右边，所以第一种顺序是 R_x @ R_z @ e_x，第二种是 R_z @ R_x @ e_x。@ 是 Python / NumPy 的矩阵乘法符号。

4.2 roll、pitch、yaw 必须连同约定一起读

RPY 通常分别指绕 x、y、z 方向的转角，但仅有三个数还不够。本篇采用固定轴依次 x、y、z 的约定：

R = R_z(yaw) × R_y(pitch) × R_x(roll)

这里描述的是先绕固定 x、再绕固定 y、最后绕固定 z。另一些接口使用随物体一起变化的轴，或者采用另一种次序。比较角度前，必须先核对约定和角度单位。ROS 的单位约定使用弧度，90° = π/2 rad。[2]

这种 RPY 表达在 pitch = ±90° 附近存在奇异性：roll 与 yaw 对朝向的影响不再能被独立区分。这是参数化方式出现退化，不是机器人在那个姿态下必然丢失一个机械自由度。机械臂本身的运动学奇异位形，需要另外分析。

4.3 四元数的四个数，不是四个独立角度

对于单位旋转轴 n = (nx, ny, nz) 和转角 θ，单位四元数可以写为：

q_xyzw = (nx sin(θ/2), ny sin(θ/2), nz sin(θ/2), cos(θ/2))

绕 z 轴正向转 90°，得到：

q_xyzw = (0, 0, sin(π/4), cos(π/4))
≈ (0, 0, 0.7071, 0.7071)

三个向量分量和一个标量分量共同编码朝向，并且满足单位长度约束。ROS 的 geometry_msgs/Quaternion 按字段 x、y、z、w 表达；其他库的构造参数或数组存储顺序可能不同，应分别检查，不能只凭“都叫 quaternion”直接复制四个数。[3]

工程中先抓住三件事：

• 单位化与有效性：表示旋转的四元数应归一化；(0, 0, 0, 0) 无法归一化，不表示“没有旋转”。在 xyzw 顺序下，单位旋转是 (0, 0, 0, 1)。
• 符号等价：q 和 -q 表示同一朝向，不能把分量突然整体变号直接当作物体跳转。比较旋转或插值时应处理这种等价性。
• 仍需参考系：四元数避免了欧拉角那种参数奇异性，但不会替你解决变换方向、乘法顺序和时刻选择问题。转回欧拉角显示时，也仍会遇到欧拉角自身的边界。

ROS 的四元数教程给出了归一化、固定轴 RPY 与旋转组合的用法。阅读时要把数学概念和具体库的存储顺序分开。[4]

5. 把旋转和平移合起来：位姿、逆变换与变换链

5.1 为什么需要一个 4 × 4 矩阵

前面的变换是 p_A = R_A_B × p_B + t_A_B。把点补上一维 1，就能把乘法与加法合并：

T_A_B = [ R_A_B  t_A_B ]       p̄_B = [x, y, z, 1].T
        [ 0 0 0    1   ]

p̄_A = T_A_B × p̄_B

这个 4 × 4 矩阵称为齐次变换矩阵。新增的一维用于统一运算，不是多出了一维物理空间。方向向量补的是 0，所以它不会引入平移项。

同一个 T_A_B 也表示 B 坐标系相对于 A 的位姿：它的旋转块给出 B 的朝向，平移块给出 B 的原点位置。这样就把“位姿描述”和“坐标转换”连接起来了。[5]

5.2 逆变换为什么不是简单把平移改成负号

从 p_A = R × p_B + t 出发，先减掉 t，再换回原来的轴：

p_B = R.T × (p_A - t)
    = R.T × p_A - R.T × t

T_B_A = inverse(T_A_B)
      = [ R.T  -R.T × t ]
        [0 0 0     1    ]

纯旋转的逆等于转置，因此这里不需要对旋转块做一般矩阵求逆。但平移的逆是 -R.T × t，不是 -t，因为反向描述时坐标轴也已经改变了。

用第二节的数值检查：

t_A_B = (1.0, 0.5, 0)
t_B_A = (-0.5, 1.0, 0)

R_A_B.T × (0.8, 0.9, 0) + t_B_A
= (0.9, -0.8, 0) + (-0.5, 1.0, 0)
= (0.4, 0.2, 0)

把同一点变过去、再变回来，是成本很低的检查。但如果正反向都使用同一份错误外参，它也可能通过，因此还要与真实轴方向、原点和标定数据核对。

5.3 从相机走到机械臂，不必为每对坐标系单独标定

若已知 B 相对 A 的位姿，以及 C 相对 B 的位姿：

T_A_C = T_A_B × T_B_C

右边先把 C 中的数据换到 B，再换到 A。中间下标相接，可以帮助检查顺序；不能因为矩阵尺寸都为 4×4，就认为任意相乘都有需要的语义。

例如，相机和机械臂都固定在机器人本体上，已知 T_base_camera 与 T_base_arm，那么：

T_arm_camera = inverse(T_base_arm) × T_base_camera

这条链先从相机到本体，再从本体到手臂基座。这里的 base、arm、camera 只是教学简称，不能直接当作某个真实机器人的 frame 名称。

图 5：箭头表示父子关系，箭头旁的文字说明关系可能来自哪里。树中的“父 → 子”箭头不是点转换的方向；T_父_子 用来把子坐标中的点转换到父坐标。

ROS 的移动机器人约定常使用 map → odom → base_link 这条链。odom 提供连续的局部参考但允许漂移，map 承载全局修正；TF 树中一个子坐标系只能有一个父坐标系，不能把多条相互冲突的父子关系同时发布进去。[6]

TF2 维护这类关系及其时间历史，提供查询能力。固定安装关系来自机构模型或标定；关节相关关系来自关节状态与运动学；全局关系来自里程计、定位等模块。TF2 本身不会从坐标系名称中计算出真实安装位置。

以 ROS 2 Python Buffer.lookup_transform 为例，前两个参数的语义是目标坐标系、源坐标系：想把相机点换到手臂基座，就查询 target 为手臂基座、source 为相机的关系，同时提供所需时刻。这里的“目标”指目标参考系，不是机器人要去的目标位置。[7]

6. 为什么同一条变换，还必须带上时间

给出 T_map_camera 仍然不够。机器人移动时，这个矩阵随时间改变，真正需要查询的是 T_map_camera(t)。

用一维平移把问题单独取出来：采图时，相机在 map 中的 x 位置是 0.20 m，静止杯子在 1.00 m，所以相机测得杯子在前方 0.80 m。过了 0.20 s，相机以 0.50 m/s 前进到 0.30 m，算法此时才处理完旧图像。

图 6：旧观测的 0.80 米属于采集时刻。把它与当前相机位置相加，会人为制造 10 厘米的位置偏移。数值为教学设定。

正确还原到固定参考系，应使用采集时刻。下式中的点采用前面补上一维 1 的齐次形式：

p_map = T_map_camera(t_capture) × p_camera(t_capture)

正确：0.20 + 0.80 = 1.00 m
错误：0.30 + 0.80 = 1.10 m

如果想得到静止目标相对于当前机器人基座的位置，需要再用当前基座的逆变换：

p_base_now = inverse(T_map_base(t_now)) × p_map

这不是把所有东西都换成“最新时刻”，而是先还原旧测量的空间含义，再表达在当前参考系中。若物体本身也在运动，还需要跟踪、预测或重新观测，不能只靠机器人位姿补偿。

另一个细节是，固定相机到本体的外参可以不变，但本体到地图的关系会变。固定外参不代表整条坐标链固定。时间戳还必须处在可比较的时钟基准下；把设备时钟、系统时钟和仿真时钟混用，也会破坏这条推理。

因此，空间数据进入应用层时，至少要一起看：数值、单位、参考系、采样时间、有效性。只留下一个 (x, y, z)，会把决定其含义的信息丢掉。

7. 一个可运行实验：变过去、变回来，再故意交换顺序

下面的代码使用 NumPy。若环境中尚未安装，可先执行 python -m pip install numpy。把两段 Python 代码按顺序保存到同一个文件运行即可。

先构造第二节的变换。注意这里用的是 @；NumPy 中的 * 是逐元素乘法，不等于矩阵乘法。

import numpy as np

R = np.array([[0., -1., 0.],
              [1.,  0., 0.],
              [0.,  0., 1.]])
t = np.array([1.0, 0.5, 0.0])
p_B = np.array([0.4, 0.2, 0.0])

p_A = R @ p_B + t
p_B_back = R.T @ (p_A - t)
print("p_A:", np.round(p_A, 4))
print("p_B_back:", np.round(p_B_back, 4))

assert np.allclose(R.T @ R, np.eye(3))
assert np.isclose(np.linalg.det(R), 1.0)
assert np.allclose(p_B_back, p_B)

输出为 p_A = [0.8, 0.9, 0.0]，p_B_back = [0.4, 0.2, 0.0]。然后检查两个固定轴旋转的顺序：

Rz = R  # 绕固定 z 轴正转 90°
Rx = np.array([[1., 0.,  0.],
               [0., 0., -1.],
               [0., 1.,  0.]])
e_x = np.array([1.0, 0.0, 0.0])

print("z then x:", Rx @ Rz @ e_x)
print("x then z:", Rz @ Rx @ e_x)
assert not np.allclose(Rx @ Rz, Rz @ Rx)

两行结果分别为 [0, 0, 1] 和 [0, 1, 0]，与图 4 一致。

再延续上一篇的相机例子做一道变换链练习：相机点为 (0.08, 0.04, 0.80) m，相机水平朝前，安装在本体前方 0.20 m、上方 0.90 m。手臂基座与本体轴平行，原点在本体中的位置是 (0.10, 0, 0.50) m。

这里采用相机光学系 x 向右、y 向下、z 向前，本体系 x 向前、y 向左、z 向上，轴约定见参考资料 [2]。因此，相机中的 (x, y, z) 先旋转为本体方向下的 (z, -x, -y)，再加安装位置：

p_base = (0.80, -0.08, -0.04) + (0.20, 0, 0.90)
       = (1.00, -0.08, 0.86) m

p_arm  = (1.00, -0.08, 0.86) - (0.10, 0, 0.50)
       = (0.90, -0.08, 0.36) m

本例第二步恰好可以直接减平移，是因为手臂基座与本体轴平行；一旦安装角度不同，就要使用完整逆变换。

这个练习有意重复使用同一批数字：如果手算、矩阵链和图上的方向不能相互印证，应先停下来检查约定，而不是继续把结果交给运动模块。

8. 落到抓取代码时，还需要补齐哪些条件

完成坐标转换，只说明“知道该在哪里表达一个点或位姿”。它不自动等于完整抓取目标。

首先，物体位姿与抓取位姿有偏移。 夹爪可能需要对准杯柄，而不是杯子几何中心。若知道物体相对基座的位姿，以及期望工具中心点 TCP 相对物体的位姿，可以写成：

T_base_tcp_goal = T_base_object × T_object_tcp_goal

其次，接口控制的点可能不是 TCP。 若控制接口接收的是工具安装法兰位姿，而工具配置给出了 T_flange_tcp，还需换算：

T_base_flange_goal = T_base_tcp_goal × inverse(T_flange_tcp)

最后仍要检查可达性、碰撞、观测是否有效和执行结果。把 frame 名填对，只解决了其中一部分。

定位问题时，可以先用下面的检查缩小范围：

现象	先检查什么	对应的原理
目标左右颠倒，或高度像深度一样变化	光学轴与本体轴是否混用，是否出现镜像	轴方向与右手系
位置固定偏一段距离	原点、工具偏置、平移单位	位姿中的平移与 TCP 配置
原点附近正确，离原点越远偏差越大	旋转外参、角度单位、旋转顺序	小角度误差会随距离放大横向偏差
正向转换正常，反向转换不对	是否只对 t 取负，是否把整个 4 × 4 矩阵当作旋转转置	t_inverse = -R.T @ t
静止时正常，移动时偏差明显	采集时间、变换时间、时钟基准	空间关系随时间变化
四元数分量跳变，但显示朝向没变	是否只是 q 与 -q 的等价表示	四元数的双重表示

这些是检查方向，不是单凭现象就能下结论的故障分类。例如，某个目标静止时也可能因外参和时间问题同时偏移，仍需结合原始观测和实际机构验证。

涉及八界的接口对接与工程实现时，欢迎关注科沃斯八界 AtomGit，仓库入口见文末参考链接 [8]。相关代码将逐步释放，实际开放内容与使用说明以仓库为准。 本篇的 NumPy 代码是独立教学示例，不依赖八界 SDK，也不表示相应代码已经上传到该仓库。

掌握本篇之后，再看到一份目标位姿，可以先读出五件事：它描述哪个刚体或点、相对于哪个参考系、采用哪种朝向表示、对应什么时刻，以及转换到调用方需要的参考系时要经过哪条链。这样，位置与姿态就不再是一组只能照抄的数字。

---

本文演算参数及图 2—6 的机构关系为教学设定，不是八界标定参数或实测数据。具体接口以所用版本为准。

参考链接

1. Modern Robotics：旋转矩阵 —— 从坐标轴理解矩阵的列与合法旋转约束。
2. REP-103：标准单位与坐标约定 —— ROS 单位、右手系、本体轴与相机光学轴约定。
3. ROS 2 Jazzy：Quaternion 消息定义 —— geometry_msgs/Quaternion 的字段及排列顺序。
4. ROS 2 Humble：Quaternion Fundamentals —— 四元数归一化、固定轴 RPY 与旋转组合。
5. Modern Robotics：齐次变换 —— 位姿、坐标转换、求逆与组合。
6. REP-105：移动机器人参考系约定——map、odom、base_link 的职责与父子关系。
7. ROS 2 Jazzy：tf2 Buffer 接口定义——lookup_transform 的目标参考系、源参考系与查询时刻。
8. 科沃斯八界 AtomGit——八界代码仓库入口；相关代码逐步释放，实际开放内容以仓库为准。