Part I · 第 5 章 刚体运动与李群 · 5.1Part I · Ch. 5 Rigid-Body Motion & Lie Groups · 5.1

坐标系与位姿Frames & Poses

机器人身上有世界系、底盘系、相机系、夹爪系，还有被抓的物体系。同一个点在每个坐标系里的数字都不一样；一个位姿 \(T_{WC}\) 说清楚的，就是"相机系相对世界系摆在哪里"。把名字写规范，位姿连乘就变成消去相邻下标。 A robot carries a world frame, a base frame, a camera frame, a gripper frame and the frame of the object it is holding. The same point has different numbers in each of them; a pose \(T_{WC}\) says where the camera frame sits relative to the world frame. Name them carefully and chaining poses becomes cancelling adjacent indices.

约 20 分钟~20 min 前置：1.1 向量 · 1.3 矩阵即变换 · 1.10 正交矩阵与旋转Prereqs: 1.1 Vectors · 1.3 Matrices as transformations · 1.10 Orthogonal matrices & rotations 4 个交互4 interactives

    1为什么机器人需要它Why robots need this

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

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

    A camera sees a cup "0.6 m ahead and 0.1 m to the left". The arm, however, needs to know where the cup is relative to its base before it can reach for it. These two sentences describe the same cup with completely different numbers, because they measure it in different frames. Between the camera reading and the arm's target lies a chain of "where is frame A relative to frame B" facts: the camera sits on the head, the head on the base, the base in the world, and the arm is also mounted on the base.

    A surprising share of robot bugs are frame bugs: the arm reaches the opposite way, the map and the laser scan do not line up, every grasp misses by the same fixed offset. This lesson does one job: make the notion of a "pose" precise, and adopt a naming discipline so that every transform you write down can be checked.

    2直觉：点只有一个，读数有很多Intuition: one point, many readings

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

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

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

    A frame is an origin together with three mutually perpendicular unit axes. This course is always right-handed and z-up (the ground is the \(xy\) plane); in a robot's own frame, \(x\) points forward, \(y\) left and \(z\) up (the ROS REP-103 convention). The \(x\), \(y\), \(z\) axes are drawn in red, green and blue. We start in the plane (the \(xy\) plane seen from above); 3D just adds one more axis.

    The cup on the table is one physical point; it belongs to no frame. Its coordinates are readings obtained by projecting it onto the axes of some frame: \(\mathbf p^{W}\) is the reading in the world frame W and \(\mathbf p^{B}\) the reading in the robot frame B. Change the frame and the reading changes while the point stays put.

    The converse holds as well: when the robot moves, the cup does not move in the world, yet its reading in the robot frame changes. Drag the robot and the cup below and compare the two sets of readings.

    FIG 1同一个点，两个坐标系One point, two frames
    拖动机器人原点、车头方向和杯子drag the robot, its heading handle and the cup
    大坐标系是世界系 W，三角形车身带着机器人系 B。琥珀色的点是杯子。虚线折线是"读数"：沿坐标轴先走 \(p_x\)，再走 \(p_y\)，就到了杯子。选"世界"看 W 的读数，选"机器人"看 B 的读数。 The big frame is the world frame W; the triangular body carries the robot frame B. The amber dot is the cup. The dashed polylines are the "readings": walk \(p_x\) along the first axis and then \(p_y\) along the second to reach the cup. Choose "world" for W's reading, "robot" for B's.
    试一试Try this
    • 让杯子正好在机器人正前方 1 米：\(\mathbf p^{B} \approx (1, 0)\)。Put the cup exactly 1 m straight ahead of the robot: \(\mathbf p^{B} \approx (1, 0)\).
    • 再让杯子在机器人正左方 1 米：\(\mathbf p^{B} \approx (0, 1)\)。这时车头指向世界系的哪个方向？Now put it exactly 1 m to the robot's left: \(\mathbf p^{B} \approx (0, 1)\). Which way does the robot face in the world?
    • 只拖动机器人，不碰杯子，把机器人挪到离出发点 2 米以外。注意 \(\mathbf p^{W}\) 一动不动，\(\mathbf p^{B}\) 却变了。Move only the robot (leave the cup alone) more than 2 m from its start. \(\mathbf p^{W}\) never changes, but \(\mathbf p^{B}\) does.
    核心洞见Key insight

    位姿就是"坐标系 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}\)。

    A pose says where frame B sits relative to frame W: the coordinates \(\mathbf t_{WB}\) of B's origin in W, plus the directions \(R_{WB}\) of B's three axes in W. With this pair \((R_{WB}, \mathbf t_{WB})\) you can translate any point's B reading into its W reading: \(\mathbf p^{W} = R_{WB}\,\mathbf p^{B} + \mathbf t_{WB}\).

    3动手玩：连乘与顺序Play: chaining and order

    命名纪律：T_WB 读作"B 在 W 里的位姿"The naming discipline: T_WB reads "pose of B in W"

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

    There is a single rule, and it lives in the variable name: \(T_{WB}\) is the pose of frame B in frame W, and it turns B readings into W readings, \(\mathbf p^{W} = T_{WB}\,\mathbf p^{B}\). In code, write T_world_base and read it "world ← base": the right-hand subscript must match the superscript of the point it multiplies, so that they cancel. A camera mounted on the base, which sits in the world, has the world pose

    $$ 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}\) 不变，可以用滑块调安装位置）。杯子固定在世界里。

    Adjacent middle indices B cancel: \(W{\cdot}\!B \,\cdot\, B{\cdot}\!C \to W{\cdot}\!C\). In the lab below the robot moves in the world (\(T_{WB}\) changes) while the camera is bolted to it (\(T_{BC}\) is fixed; the sliders adjust the mount). The cup is fixed in the world.

    FIG 2连乘 T_WC = T_WB · T_BCChaining T_WC = T_WB · T_BC
    拖动机器人和杯子drag the robot and the cup
    原点处的小坐标系是世界系 W（标签为灰色），三角形是底盘系 B，青色扇形是相机系 C 的视野（±35°，6 米）。杯子在视野内时扇形变绿。位姿用 \((x, y, \theta)\) 表示：平面里的 \(R_{WB}\) 只由一个角 \(\theta\) 决定。选"错误顺序"会多画一个红色的 \(T_{BC}\,T_{WB}\)：乘反了，相机就跑到了别处。 The small frame at the origin (grey label) is the world frame W, the triangle is the base frame B, and the cyan wedge is the field of view of the camera frame C (±35°, 6 m); it turns green when the cup is inside. A pose is written \((x, y, \theta)\): in the plane \(R_{WB}\) is fixed by the single angle \(\theta\). "Wrong order" also draws a red \(T_{BC}\,T_{WB}\): multiply the wrong way round and the camera ends up somewhere else.
    试一试Try this
    • 移动机器人，让杯子出现在相机的正前方（偏角小于 5°，距离 1 到 6 米）。读一读 \(\mathbf p^{C}\)：它的 \(y\) 分量应该接近 0。Move the robot until the cup is dead ahead of the camera (bearing under 5°, distance 1 to 6 m). Read \(\mathbf p^{C}\): its \(y\) component should be close to 0.
    • 切换到"错误顺序"，让红色的错误相机和真实相机相距超过 0.5 米。乘法顺序为什么不能随便换？Switch to "wrong order" and get the red wrong camera more than 0.5 m away from the real one. Why can't the order of the product be swapped?

    左乘还是右乘：在谁的坐标系里运动Left or right: motion in whose frame?

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

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

    The robot is already at pose \(A = T_{WB}\) and must now apply a relative motion \(\Delta\) (say "drive forward 1 m"). There are two ways to multiply, with different results:

    • Right-multiply: \(A\,\Delta\). \(\Delta\) is read in the robot's own frame B: "forward 1 m" means 1 m along the heading.
    • Left-multiply: \(\Delta\,A\). \(\Delta\) is read in the world frame W: treat the whole robot as an object in the world and move it. "Forward 1 m" becomes 1 m along the world \(x\) axis, and a 90° turn swings it about the world origin.
    FIG 3右乘 A·Δ vs. 左乘 Δ·ARight A·Δ vs. left Δ·A
    拖动机器人 Adrag the robot A
    灰色是起点 A。琥珀色是右乘的结果 \(A\Delta\)（在 A 自己的坐标系里动），紫色是左乘的结果 \(\Delta A\)（在世界系里动）。用三个滑块设定 \(\Delta = (\Delta x, \Delta y, \Delta\theta)\)。当 A 是单位位姿（在世界原点、车头朝世界 \(+x\)）时两者重合；A 离原点越远、或车头偏得越多，差别越大。 Grey is the start A. Amber is the right-multiplied result \(A\Delta\) (moving in A's own frame) and purple the left-multiplied result \(\Delta A\) (moving in the world frame). Set \(\Delta = (\Delta x, \Delta y, \Delta\theta)\) with the three sliders. When A is the identity (at the world origin, facing world \(+x\)) the two coincide; the farther A is from the origin, or the more it is turned, the more they differ.
    试一试Try this
    • 先把机器人转到车头偏离世界 \(x\) 轴超过 30°，再设 \(\Delta = (1, 0, 0)\)。琥珀色和紫色各自往哪个方向走了？Turn the robot so its heading is more than 30° off the world \(x\) axis, then set \(\Delta = (1, 0, 0)\). Which way did amber and purple each go?
    • 设 \(\Delta\) 是纯旋转 90°（\(\Delta x = \Delta y = 0\)），把机器人放在离原点 1.5 米以外。一个是原地转弯，另一个是绕着世界原点画圆弧，哪个是哪个？Set \(\Delta\) to a pure 90° turn (\(\Delta x = \Delta y = 0\)) with the robot more than 1.5 m from the origin. One result spins in place, the other swings around the world origin. Which is which?

    4再看数学The math

    提示：把鼠标悬停在公式中带底色的符号上，FIG 1 中对应的量会被高亮。Tip: hover the highlighted symbols in the formulas; the matching quantity in FIG 1 lights up.

    位姿 = 旋转 + 平移A pose = rotation + translation

    设 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 节）。点的两种读数满足

    Let B's origin be \(\mathbf t_{WB}\) in W, and let B's three axes have W-coordinates \(\mathbf r_1, \mathbf r_2, \mathbf r_3\). Stack them as \(R_{WB} = [\mathbf r_1\ \mathbf r_2\ \mathbf r_3]\), a rotation matrix (Lesson 1.10). The two readings of a point satisfy

    $$ \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 节的"矩阵的列是新基向量"。

    Reading: \(\mathbf p^{B}\) says "how far to go along each of B's axes". \(R_{WB}\mathbf p^{B} = p^B_x\mathbf r_1 + p^B_y\mathbf r_2 + p^B_z\mathbf r_3\) converts those steps to directions in W, and you start walking from B's origin \(\mathbf t_{WB}\). This is the "columns are the new basis vectors" of Lesson 1.3.

    逆与连乘Inverse and composition

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

    Solving the formula for \(\mathbf p^{B}\) with \(R^{-1} = R\T\) (Lesson 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\)，

    Note that \(\mathbf t_{BW} \neq -\mathbf t_{WB}\): the world origin's coordinates in B require re-expressing \(-\mathbf t_{WB}\) along B's axes. Composing two transforms, first \(C \to B\) and then \(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} $$

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

    Hence a notation rule you can check by eye against your variable names:

    • 相邻下标消去：\(T_{AB}\,T_{BC} = T_{AC}\)，里面的 B 必须相同，且紧挨着。
    • Adjacent indices cancel: \(T_{AB}\,T_{BC} = T_{AC}\); the inner B's must be equal and adjacent.
    • 求逆交换下标：\(T_{AB}^{-1} = T_{BA}\)。
    • Inverting swaps the indices: \(T_{AB}^{-1} = T_{BA}\).
    • 作用在点上：\(T_{AB}\,\mathbf p^{B} = \mathbf p^{A}\)，点的上标和 \(T\) 的右下标相同。
    • Acting on a point: \(T_{AB}\,\mathbf p^{B} = \mathbf p^{A}\); the point's superscript matches \(T\)'s right subscript.

    主动与被动：同一个 \(T\) 的两种读法Active vs. passive: two readings of the same \(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\)"。

    Passive reading: \(T_{WB}\) converts the B reading of the same point into its W reading; the point does not move, you just swap rulers. Active reading: \(T_{WB}\) is an action that carries the W frame onto the B frame (rotate by \(R_{WB}\), then translate by \(\mathbf t_{WB}\)), or carries a point in B's coordinates to the same-numbered place in W. The numbers are the same; only the story differs. Left vs. right multiplication comes from here too: \(T\Delta\) (right) means "move by \(\Delta\) in \(T\)'s own frame", \(\Delta T\) (left) means "move the whole thing by \(\Delta\) in the outer frame".

    坐标系组成树Frames form a tree

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

    A robot system has many frames, but you do not store "the pose between every pair". Let each frame record its pose relative to its parent frame and the whole system becomes a tree. The pose between any two frames A and B comes from the unique path through the tree, multiplying each edge's pose (and inverting those traversed against the edge): \(T_{AB} = T_{WA}^{-1}\,T_{WB}\), where W is a common ancestor. The tree may contain no loops, or the same pair of frames would get two contradictory answers. ROS's tf2 is precisely a time-stamped pose tree like this.

    5机器人中的它：移动操作机器人的 tf 树In robotics: the tf tree of a mobile manipulator

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

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

    Here is a simplified mobile manipulator: the base B in the world W, a camera C on its head, a shoulder S, elbow E and gripper G forming an arm, and a cup O on the table. Each edge of the tree stores a pose relative to the parent frame. The base pose comes from odometry or localisation, the shoulder and elbow poses are computed from the joint angles, and the cup is a static pose fixed in the world (as detected by the camera).

    Pick any two frames A and B below; the page walks the tree to find the path and chains (or inverts) the edge poses to produce \(T_{AB}\). The heart of grasping is \(T_{GO}\): the pose of the cup in the gripper frame. When its translation reaches 0, the hand is on the cup.

    FIG 4tf 树：任意两个坐标系之间的位姿The tf tree: the pose between any two frames
    拖动画面旋转视角drag to orbit
    从 A（左下标）From A (left subscript)
    到 B（右下标）To B (right subscript)
    每个坐标系画成三色小坐标轴，灰线连着父子；琥珀色粗线是 A 到 B 的路径。位姿读数是 \(T_{AB} = (R_{AB}, \mathbf t_{AB})\)：B 的原点在 A 里的坐标，以及 B 的三根轴在 A 里的方向。关节角正方向：\(q_2\)、\(q_3\) 和相机俯仰都绕各自的 \(y\) 轴，正值使 \(x\) 轴向下低头。 Every frame is drawn as a small tri-colour axis triad, with grey lines joining parent and child; the thick amber line is the path from A to B. The readout is \(T_{AB} = (R_{AB}, \mathbf t_{AB})\): the coordinates of B's origin in A, and the directions of B's three axes in A. Joint sign: \(q_2\), \(q_3\) and the camera tilt rotate about their own \(y\) axis, and a positive angle tips the \(x\) axis downward.
    试一试Try this
    • 选 A = 相机 C，B = 杯子 O。这就是"相机看到杯子"的读数 \(T_{CO}\)，它的平移部分就是相机坐标系里的杯子位置。Pick A = camera C and B = cup O. This is what the camera "sees", \(T_{CO}\); its translation is the cup's position in the camera frame.
    • 驱动底盘移动超过 0.8 米。哪些位姿变了，哪些没变？（提示：\(T_{WO}\) 是不是该变？\(T_{BC}\) 呢？）Drive the base more than 0.8 m. Which poses changed and which did not? (Hint: should \(T_{WO}\) change? What about \(T_{BC}\)?)
    • 选 A = 夹爪 G，B = 杯子 O，用底盘和三个关节把夹爪送到杯子上，使 \(\|\mathbf t_{GO}\| < 0.12\) 米。Pick A = gripper G and B = cup O, then use the base and the three joints to bring the gripper onto the cup so that \(\|\mathbf t_{GO}\| < 0.12\) m.
    真实系统里In real systems

    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 的修正。

    ROS tf2 is exactly this structure: each node broadcasts "the pose of a child frame in its parent frame (with a timestamp)", and lookupTransform walks the tree for any pair. Hand–eye calibration finds one fixed edge of the tree, such as the camera's pose in the gripper frame \(T_{GC}\); once it is stored in the URDF, every frame's \(T_{BO} = T_{BG}\,T_{GC}\,T_{CO}\) relies on it. SLAM / localisation maintains the \(T_{\text{map},\text{base}}\) edge, odometry maintains \(T_{\text{odom},\text{base}}\), and their difference is the map → odom correction.

    6常见误区Common pitfalls

    误区 1：把 T_WC 和 T_CW 弄反Pitfall 1: confusing T_WC with T_CW

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

    The two are inverses of each other and both are "legal" poses, so mixing them up raises no error; the robot simply moves wrongly. Habit: spell out both frames in the variable name (T_world_cam, never T_cam or pose) and read it as "W ← C". Even tf's API invites the slip: lookupTransform(target, source) returns the pose of source in target, mapping points from the source frame to the target frame.

    误区 2：左乘右乘随便用Pitfall 2: using left and right multiplication interchangeably

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

    "Drive forward 1 m" is stated in the robot's own frame and needs a right multiplication; "shift the whole robot 1 m east in the world" is stated in the world frame and needs a left one. They agree only if the robot's pose happens to be the identity (at the world origin, facing world \(+x\)), or \(\Delta\) commutes with \(A\) (FIG 3). To decide, ask: in whose frame is this increment expressed?

    误区 3：以为所有"相机坐标系"都一样Pitfall 3: assuming all "camera frames" are the same

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

    Robot frames (ROS base_link, a camera's camera_link) have \(x\) forward, \(y\) left, \(z\) up. Computer-vision camera frames (OpenCV, ROS's camera_optical_frame) have \(x\) right, \(y\) down, \(z\) forward. They differ by a fixed rotation that must be written into the tf tree explicitly. Using a point computed from pixels as if it were in camera_link is the classic cause of "the whole world is rotated by 90°".

    误区 4：忘了位姿是随时间变化的Pitfall 4: forgetting that poses change with time

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

    A camera sees the cup at \(t_1\) but the robot only starts reaching at \(t_2\), and the base has moved in between. Use \(T_{WC}(t_1)\) to put the cup into the world frame, then \(T_{WG}(t_2)\) to reach for it. Every pose in a tf tree is time-stamped; mixing poses from different instants gives a robot that is neither in the past nor in the present.

    7小练习Check yourself

    1. 相机在世界里的位姿是 \(T_{WC}\)，相机看到某点的读数是 \(\mathbf p^{C}\)。这个点的世界坐标是？1. The camera's pose in the world is \(T_{WC}\) and it reads a point as \(\mathbf p^{C}\). What are the point's world coordinates?
    \(T_{WC}\) 的右下标 C 和点的上标 C 相同，相乘后得到上标 W 的读数。\(T_{CW}\,\mathbf p^{C}\) 下标对不上，是常见的"乘错逆"。The right subscript C of \(T_{WC}\) matches the superscript C of the point, and the product is a reading with superscript W. \(T_{CW}\,\mathbf p^{C}\) does not match and is the classic "multiplied by the wrong inverse".
    2. 已知 \(T_{WB}\)、\(T_{BC}\)、\(T_{CG}\)（夹爪相对相机）。夹爪在世界里的位姿 \(T_{WG}\) 是？2. Given \(T_{WB}\), \(T_{BC}\) and \(T_{CG}\) (gripper in the camera frame), what is the gripper's world pose \(T_{WG}\)?
    相邻下标要能消去：\(W\!B \cdot B C \cdot C G \to W G\)。其余几种的下标都对不上，第一种是整条链倒着乘。Adjacent indices must cancel: \(W\!B \cdot B C \cdot C G \to W G\). The other orders have mismatched indices; the first is the chain multiplied backwards.
    3. 平面里机器人在世界 \((2, 1)\)，车头朝世界 \(+y\) 方向（\(\theta = 90°\)）。杯子在机器人系里是 \((1, 0)\)。它的世界坐标是？3. In the plane, the robot is at \((2, 1)\) in the world and faces world \(+y\) (\(\theta = 90°\)). The cup is at \((1, 0)\) in the robot frame. Its world coordinates?
    "正前方 1 米"，车头朝 \(+y\)，所以往 \(+y\) 走 1 米：\(R(90°)(1,0)^\top = (0,1)\)，加上 \(\mathbf t = (2,1)\) 得 \((2,2)\)。\((3,1)\) 是忘了旋转，直接把 \((1,0)\) 加到了平移上。"1 m straight ahead" with the nose along \(+y\) means 1 m in \(+y\): \(R(90°)(1,0)^\top = (0,1)\); adding \(\mathbf t = (2,1)\) gives \((2,2)\). \((3,1)\) forgets the rotation and adds \((1,0)\) straight onto the translation.
    4. 同一个机器人（位于 \((2,1)\)，\(\theta = 90°\)）。世界原点在机器人系里的坐标是？4. The same robot (at \((2,1)\), \(\theta = 90°\)). What are the world origin's coordinates in the robot frame?
    \(\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)\) 是忘了把它换到机器人的方向上。\(\mathbf t_{BW} = -R_{WB}\T\mathbf t_{WB}\) with \(R_{WB}\T = \begin{bmatrix}0&1\\-1&0\end{bmatrix}\), so \(R_{WB}\T(2,1)^\top = (1,-2)\) and the negative is \((-1, 2)\): the origin is 1 m behind and 2 m to the left of the robot. Taking \(-\mathbf t_{WB} = (-2,-1)\) forgets to re-express it along the robot's axes.
    5. 想让机器人"沿自己车头前进 1 米"，已知它当前位姿 \(A\)。应该算哪个？5. To make the robot "drive 1 m along its own heading" from its current pose \(A\), which should you compute?
    增量是机器人自己坐标系里的读数，所以右乘：\(A\Delta\)。\(\Delta A\) 会让机器人沿世界 \(x\) 轴走 1 米，不管车头朝哪。The increment is stated in the robot's own frame, so right-multiply: \(A\Delta\). \(\Delta A\) would move the robot 1 m along the world \(x\) axis, whichever way it faces.

    8延伸阅读Further reading

    • K. Lynch & F. Park, Modern Robotics, Ch. 3.3 — 刚体运动、坐标系的下标记法（\(T_{sb}\)）与左右乘的含义rigid-body motions, the subscript notation (\(T_{sb}\)) and the meaning of left vs. right multiplication
    • T. Barfoot, State Estimation for Robotics, Ch. 6 — 三维几何、坐标系与位姿的记号3D geometry, frames and pose notation
    • B. Siciliano, L. Sciavicco, L. Villani, G. Oriolo, Robotics: Modelling, Planning and Control, Ch. 2 — 刚体位姿与坐标变换rigid-body pose and coordinate transformations
    • ROS 文档：REP-103（单位与坐标系约定）、REP-105（移动平台的坐标系：map / odom / base_link）与 tf2 教程ROS documentation: REP-103 (units and coordinate conventions), REP-105 (coordinate frames for mobile platforms: map / odom / base_link) and the tf2 tutorials