Part I · 第 5 章刚体运动与李群 · 5.1

坐标系与位姿

机器人身上有世界系、底盘系、相机系、夹爪系，还有被抓的物体系。同一个点在每个坐标系里的数字都不一样；一个位姿 $T_{WC}$ 说清楚的，就是"相机系相对世界系摆在哪里"。把名字写规范，位姿连乘就变成消去相邻下标。

约 20 分钟前置：1.1 向量 · 1.3 矩阵即变换 · 1.10 正交矩阵与旋转 4 个交互

1为什么机器人需要它

相机看到杯子，说它在"前方 0.6 米、偏左 0.1 米"。机械臂却要知道杯子在底座的哪里，才能伸手去抓。这两句话描述的是同一个杯子，数字却完全不同，因为它们量的是不同的坐标系。从相机的读数到手臂的目标，中间隔着一串"坐标系 A 相对坐标系 B 在哪里"的信息：相机装在头上，头在底盘上，底盘在世界里，手臂又装在底盘上。

机器人软件里相当多的 bug，说穿了都是坐标系的 bug：手臂朝相反方向伸出去，地图和激光对不上，抓取总是偏一个固定的位移。这一节只做一件事：把"位姿"这个概念讲清楚，再立一条命名纪律，让你写下的每一个变换都能被检查。

2直觉：点只有一个，读数有很多

一个坐标系（frame）由一个原点和三根互相垂直的单位坐标轴组成。本课程一律用右手系，$z$ 轴向上（地面是 $xy$ 平面），机器人自己的坐标系里 $x$ 朝前、$y$ 朝左、$z$ 朝上（ROS 的 REP-103 约定）。$x$、$y$、$z$ 轴依次用红、绿、蓝画。本节先看平面（$xy$ 平面，从上往下看），三维只是多一根轴。

桌上的杯子是物理上的一个点，它不属于任何坐标系。坐标是"把它投影到某个坐标系的各根轴上得到的读数"：$\mathbf p^{W}$ 是世界系 W 的读数，$\mathbf p^{B}$ 是机器人系 B 的读数。换坐标系，读数就变，点不动。

反过来也成立：机器人动了，杯子在世界里没动，杯子在机器人系里的读数却变了。下面拖动机器人和杯子，对照两组读数。

FIG 1同一个点，两个坐标系

拖动机器人原点、车头方向和杯子

大坐标系是世界系 W，三角形车身带着机器人系 B。琥珀色的点是杯子。虚线折线是"读数"：沿坐标轴先走 $p_x$，再走 $p_y$，就到了杯子。选"世界"看 W 的读数，选"机器人"看 B 的读数。

试一试

让杯子正好在机器人正前方 1 米：$\mathbf p^{B} \approx (1, 0)$。
再让杯子在机器人正左方 1 米：$\mathbf p^{B} \approx (0, 1)$。这时车头指向世界系的哪个方向？
只拖动机器人，不碰杯子，把机器人挪到离出发点 2 米以外。注意 $\mathbf p^{W}$ 一动不动，$\mathbf p^{B}$ 却变了。

核心洞见

位姿就是"坐标系 B 相对坐标系 W 摆在哪里"：B 的原点在 W 里的坐标 $\mathbf t_{WB}$，加上 B 的三根轴在 W 里的方向 $R_{WB}$。有了这对 $(R_{WB}, \mathbf t_{WB})$，就能把任何点的 B 读数翻译成 W 读数：$\mathbf p^{W} = R_{WB}\,\mathbf p^{B} + \mathbf t_{WB}$。

3动手玩：连乘与顺序

命名纪律：T_WB 读作"B 在 W 里的位姿"

规则只有一条，写在变量名里：$T_{WB}$ 是 B 系在 W 系里的位姿，它把 B 系的读数变成 W 系的读数，$\mathbf p^{W} = T_{WB}\,\mathbf p^{B}$。代码里写 T_world_base，读作"world ← base"：右边的下标和被乘的点的上标相同，才能消掉。相机装在底盘上、底盘在世界里，则相机在世界里的位姿是

$$ T_{W\!C} \;=\; T_{W\!B}\;T_{B C}, \qquad \mathbf p^{W} = T_{W\!B}\,T_{B C}\,\mathbf p^{C}. $$

中间的 B 相邻，就消掉：$W{\cdot}\!B \,\cdot\, B{\cdot}\!C \to W{\cdot}\!C$。下面的实验里，机器人在世界中移动（$T_{WB}$ 变），相机固定在车上（$T_{BC}$ 不变，可以用滑块调安装位置）。杯子固定在世界里。

FIG 2连乘 T_WC = T_WB · T_BC

拖动机器人和杯子

原点处的小坐标系是世界系 W（标签为灰色），三角形是底盘系 B，青色扇形是相机系 C 的视野（±35°，6 米）。杯子在视野内时扇形变绿。位姿用 $(x, y, \theta)$ 表示：平面里的 $R_{WB}$ 只由一个角 $\theta$ 决定。选"错误顺序"会多画一个红色的 $T_{BC}\,T_{WB}$：乘反了，相机就跑到了别处。

试一试

移动机器人，让杯子出现在相机的正前方（偏角小于 5°，距离 1 到 6 米）。读一读 $\mathbf p^{C}$：它的 $y$ 分量应该接近 0。
切换到"错误顺序"，让红色的错误相机和真实相机相距超过 0.5 米。乘法顺序为什么不能随便换？

左乘还是右乘：在谁的坐标系里运动

机器人已经在位姿 $A = T_{WB}$，现在要再做一个相对运动 $\Delta$（例如"前进 1 米"）。有两种乘法，结果不同：

右乘：$A\,\Delta$。$\Delta$ 的读数是相对机器人自己的坐标系 B：前进 1 米，就是沿车头方向走 1 米。
左乘：$\Delta\,A$。$\Delta$ 的读数是相对世界系 W：先把整个机器人当成世界里的一个物体，再把它整体平移、转动。"前进 1 米"变成沿世界 $x$ 轴走 1 米，转 90° 则是绕世界原点转。

FIG 3右乘 A·Δ vs. 左乘 Δ·A

拖动机器人 A

灰色是起点 A。琥珀色是右乘的结果 $A\Delta$（在 A 自己的坐标系里动），紫色是左乘的结果 $\Delta A$（在世界系里动）。用三个滑块设定 $\Delta = (\Delta x, \Delta y, \Delta\theta)$。当 A 是单位位姿（在世界原点、车头朝世界 $+x$）时两者重合；A 离原点越远、或车头偏得越多，差别越大。

试一试

先把机器人转到车头偏离世界 $x$ 轴超过 30°，再设 $\Delta = (1, 0, 0)$。琥珀色和紫色各自往哪个方向走了？
设 $\Delta$ 是纯旋转 90°（$\Delta x = \Delta y = 0$），把机器人放在离原点 1.5 米以外。一个是原地转弯，另一个是绕着世界原点画圆弧，哪个是哪个？

4再看数学

提示：把鼠标悬停在公式中带底色的符号上，FIG 1 中对应的量会被高亮。

位姿 = 旋转 + 平移

设 B 系的原点在 W 里是 $\mathbf t_{WB}$，B 的三根轴在 W 里的坐标依次是 $\mathbf r_1, \mathbf r_2, \mathbf r_3$。把它们并排成矩阵 $R_{WB} = [\mathbf r_1\ \mathbf r_2\ \mathbf r_3]$，它是一个旋转矩阵（1.10 节）。点的两种读数满足

$$ \hl{pW}{\mathbf p^{W}} \;=\; \hl{R}{R_{WB}}\,\hl{pB}{\mathbf p^{B}} \;+\; \hl{t}{\mathbf t_{WB}}. $$

读法：$\mathbf p^{B}$ 是"沿 B 的各轴走多少"，$R_{WB}\mathbf p^{B} = p^B_x\mathbf r_1 + p^B_y\mathbf r_2 + p^B_z\mathbf r_3$ 把这些步子换成 W 里的方向，再从 B 的原点 $\mathbf t_{WB}$ 出发走过去。这也是 1.3 节的"矩阵的列是新基向量"。

逆与连乘

把上式反解 $\mathbf p^{B}$，用到 $R^{-1} = R\T$（1.10 节）：

$$ \mathbf p^{B} = R_{WB}\T\,\mathbf p^{W} - R_{WB}\T\,\mathbf t_{WB} \quad\Longrightarrow\quad R_{BW} = R_{WB}\T, \qquad \mathbf t_{BW} = -R_{WB}\T\,\mathbf t_{WB}. $$

注意 $\mathbf t_{BW} \neq -\mathbf t_{WB}$：W 的原点在 B 里的坐标，要把 $-\mathbf t_{WB}$ 也换到 B 的方向上才对。再看两次变换的连乘：先 $C \to B$，再 $B \to A$，

$$ \begin{aligned} \mathbf p^{A} &= R_{AB}\big(R_{BC}\mathbf p^{C} + \mathbf t_{BC}\big) + \mathbf t_{AB} \\ &\Longrightarrow\; T_{AC} = T_{AB}T_{BC}: \quad R_{AC} = R_{AB}R_{BC}, \quad \mathbf t_{AC} = R_{AB}\,\mathbf t_{BC} + \mathbf t_{AB}. \end{aligned} $$

于是有一套可以"目检"的记号规则，写代码时对着变量名核对：

相邻下标消去：$T_{AB}\,T_{BC} = T_{AC}$，里面的 B 必须相同，且紧挨着。
求逆交换下标：$T_{AB}^{-1} = T_{BA}$。
作用在点上：$T_{AB}\,\mathbf p^{B} = \mathbf p^{A}$，点的上标和 $T$ 的右下标相同。

主动与被动：同一个 $T$ 的两种读法

被动读法：$T_{WB}$ 把同一个点的 B 读数换成 W 读数，点没动，只是换了尺子。主动读法：$T_{WB}$ 是一个动作，把 W 系本身搬到 B 系的位置（先转 $R_{WB}$，再平移 $\mathbf t_{WB}$），或者把 B 系里的点搬到 W 系里同样的位置。数字一样，说法不同。FIG 3 里的"左乘 / 右乘"也出自这里：$T\Delta$（右乘）是"在 $T$ 的自身坐标系里再动 $\Delta$"，$\Delta T$（左乘）是"在外面的坐标系里整体再动 $\Delta$"。

坐标系组成树

一个机器人系统里坐标系很多，但不需要存"每一对之间的位姿"。只要每个坐标系记录它相对父坐标系的位姿，整个系统就是一棵树。任意两个坐标系 A、B 之间的位姿，都是沿树上唯一的一条路径，把每一段边的位姿连乘（逆着边走就取逆）：$T_{AB} = T_{WA}^{-1}\,T_{WB}$（W 是它们的共同祖先）。树里不能有环，否则同一对坐标系会有两个互相矛盾的答案。ROS 的 tf2 就是这样的一个带时间戳的位姿树。

5机器人中的它：移动操作机器人的 tf 树

下面是一台简化的移动操作机器人：底盘 B 在世界 W 里，头上装着相机 C，肩部 S、肘部 E、夹爪 G 组成一条手臂，桌上有一只杯子 O。树里每条边存一个相对父坐标系的位姿，其中底盘位姿来自里程计 / 定位，肩、肘位姿由关节角算出，杯子是（相机检测、世界系里固定的）一个静态位姿。

从下面选两个坐标系 A 和 B，页面会沿树找出路径，把边上的位姿连乘（或求逆）出 $T_{AB}$。抓取的核心是 $T_{GO}$：杯子在夹爪系里的位姿。它的平移部分为 0 时，手就在杯子上了。

FIG 4tf 树：任意两个坐标系之间的位姿

拖动画面旋转视角

从 A（左下标）

到 B（右下标）

每个坐标系画成三色小坐标轴，灰线连着父子；琥珀色粗线是 A 到 B 的路径。位姿读数是 $T_{AB} = (R_{AB}, \mathbf t_{AB})$：B 的原点在 A 里的坐标，以及 B 的三根轴在 A 里的方向。关节角正方向：$q_2$、$q_3$ 和相机俯仰都绕各自的 $y$ 轴，正值使 $x$ 轴向下低头。

试一试

选 A = 相机 C，B = 杯子 O。这就是"相机看到杯子"的读数 $T_{CO}$，它的平移部分就是相机坐标系里的杯子位置。
驱动底盘移动超过 0.8 米。哪些位姿变了，哪些没变？（提示：$T_{WO}$ 是不是该变？$T_{BC}$ 呢？）
选 A = 夹爪 G，B = 杯子 O，用底盘和三个关节把夹爪送到杯子上，使 $\|\mathbf t_{GO}\| < 0.12$ 米。

真实系统里

ROS 的 tf2 正是这个结构：每个节点广播"子坐标系在父坐标系里的位姿（带时间戳）"，lookupTransform 沿树查询任意两个坐标系之间的位姿。手眼标定求出的就是树里某一条固定的边，比如相机相对夹爪的 $T_{GC}$，求出来之后写进 URDF，往后每一帧的 $T_{BO} = T_{BG}\,T_{GC}\,T_{CO}$ 都靠它。SLAM / 定位负责维护 $T_{\text{map},\text{base}}$ 这条边，里程计负责 $T_{\text{odom},\text{base}}$，两者之差就是 map → odom 的修正。

6常见误区

误区 1：把 T_WC 和 T_CW 弄反

两者互为逆矩阵，都是"合法"的位姿，所以弄反时程序不会报错，只会让机器人动错。习惯：变量名写全两个坐标系（T_world_cam，不要写 T_cam、pose），读的时候念"W ← C"。tf 的 API 也容易反：lookupTransform(target, source) 返回的是"source 在 target 里的位姿"，把 source 系里的点变换到 target 系。

误区 2：左乘右乘随便用

"向前走 1 米"是在机器人自己的坐标系里说的，要右乘；"把整个机器人在世界里向东平移 1 米"是在世界系里说的，要左乘。只有机器人的位姿恰好是单位位姿（在世界原点、车头朝世界 $+x$），或 $\Delta$ 与 $A$ 可交换时，两者才相等（FIG 3）。判断办法：问一句，这个增量是谁的坐标系里的读数？

误区 3：以为所有"相机坐标系"都一样

机器人系（ROS 的 base_link、相机的 camera_link）是 $x$ 朝前、$y$ 朝左、$z$ 朝上。计算机视觉的相机系（OpenCV、ROS 的 camera_optical_frame）是 $x$ 朝右、$y$ 朝下、$z$ 朝前。两者之间差一个固定的旋转，需要显式写进 tf 里。把像素算出来的点直接当成 camera_link 里的点用，是新手最常见的"整个世界歪了 90°"的来源。

误区 4：忘了位姿是随时间变化的

相机在 $t_1$ 拍到杯子，机器人到 $t_2$ 才开始动手，这期间底盘动过了。要用 $t_1$ 时刻的 $T_{WC}(t_1)$ 把杯子变到世界系里，再用 $t_2$ 时刻的 $T_{WG}(t_2)$ 去抓它。tf 树里每个位姿都带时间戳，混用不同时刻的位姿，会得到一个"既不是过去也不是现在"的机器人。

7小练习

1. 相机在世界里的位姿是 $T_{WC}$，相机看到某点的读数是 $\mathbf p^{C}$。这个点的世界坐标是？

$T_{WC}$ 的右下标 C 和点的上标 C 相同，相乘后得到上标 W 的读数。$T_{CW}\,\mathbf p^{C}$ 下标对不上，是常见的"乘错逆"。

2. 已知 $T_{WB}$、$T_{BC}$、$T_{CG}$（夹爪相对相机）。夹爪在世界里的位姿 $T_{WG}$ 是？

相邻下标要能消去：$W\!B \cdot B C \cdot C G \to W G$。其余几种的下标都对不上，第一种是整条链倒着乘。

3. 平面里机器人在世界 $(2, 1)$，车头朝世界 $+y$ 方向（$\theta = 90°$）。杯子在机器人系里是 $(1, 0)$。它的世界坐标是？

"正前方 1 米"，车头朝 $+y$，所以往 $+y$ 走 1 米：$R(90°)(1,0)^\top = (0,1)$，加上 $\mathbf t = (2,1)$ 得 $(2,2)$。$(3,1)$ 是忘了旋转，直接把 $(1,0)$ 加到了平移上。

4. 同一个机器人（位于 $(2,1)$，$\theta = 90°$）。世界原点在机器人系里的坐标是？

$\mathbf t_{BW} = -R_{WB}\T\mathbf t_{WB}$，其中 $R_{WB}\T = \begin{bmatrix}0&1\\-1&0\end{bmatrix}$，$R_{WB}\T(2,1)^\top = (1,-2)$，取负得 $(-1, 2)$：原点在机器人身后 1 米、左边 2 米。直接取 $-\mathbf t_{WB} = (-2,-1)$ 是忘了把它换到机器人的方向上。

5. 想让机器人"沿自己车头前进 1 米"，已知它当前位姿 $A$。应该算哪个？

增量是机器人自己坐标系里的读数，所以右乘：$A\Delta$。$\Delta A$ 会让机器人沿世界 $x$ 轴走 1 米，不管车头朝哪。

8延伸阅读

K. Lynch & F. Park, Modern Robotics, Ch. 3.3 — 刚体运动、坐标系的下标记法（$T_{sb}$）与左右乘的含义
T. Barfoot, State Estimation for Robotics, Ch. 6 — 三维几何、坐标系与位姿的记号
B. Siciliano, L. Sciavicco, L. Villani, G. Oriolo, Robotics: Modelling, Planning and Control, Ch. 2 — 刚体位姿与坐标变换
ROS 文档：REP-103（单位与坐标系约定）、REP-105（移动平台的坐标系：map / odom / base_link）与 tf2 教程