机器人运动的语言：坐标系与刚体运动学基础

如果你问一个机器人“你现在在哪？”，它大概率不会直接回答“我在客厅沙发旁边”，而是会甩给你一串数字：$x=1.2\mathrm{m}$，$y=0.8\mathrm{m}$，航向角$45^\circ$——这串数字就是机器人世界的“坐标”，而所有坐标的参考基准，就是本章要讲的坐标系。如果说机器人的感知是“眼睛”，控制是“手脚”，那坐标系就是机器人的“大脑语言”：激光雷达扫到的点云、摄像头拍到的图像、IMU测到的加速度，最终都要翻译成这套坐标系的数值才能被理解；而机器人要执行的移动、抓取动作，也要先从这套语言翻译成电机的转速、关节的角度才能落地。可以说，搞懂了坐标系和刚体运动学，就拿到了进入机器人领域的“入场券”。

---

1.1 机器人的“坐标系搬家”游戏：从世界到末端的坐标转换逻辑

我们可以把机器人的所有坐标系想象成一个个“家”，每个“家”都有自己的原点、坐标轴方向，同一个物体在不同“家”里的地址（坐标）是不一样的。比如你手里拿着的手机，在你自己的“家”（以你的腰部为原点，x轴朝前、y轴朝左、z轴朝天）里的坐标是$(0.3, 0, 0.5)$，也就是身前30厘米、腰以上50厘米；但如果放到客厅的“家”（以墙角为原点，x轴朝东、y轴朝北、z轴朝天）里，坐标就变成了$(2.1, 1.5, 1.2)$，完全不一样。机器人要做的，本质上就是把这些不同“家”里的地址互相翻译。

在机器人系统里，我们最常接触的坐标系有以下5种，每种都有明确的定义和使用场景： 1. 世界坐标系（World Frame）：是整个机器人系统的“总基准”，固定在物理环境中不动，比如实验室的墙角、仓储AGV作业区域的一个固定标记点。所有机器人的位姿、地图里的障碍物位置，最终都要统一到世界坐标系下才有意义。 2. 机器人基坐标系（Base Frame）：是每个机器人自己的“家”，原点固定在机器人的底座中心，坐标轴方向一般和机器人的运动方向对齐：比如差速机器人的x轴朝前、y轴朝左、z轴朝天；六轴机械臂的z轴朝天、x轴朝前。这个坐标系会跟着机器人整体移动/转动，相对于世界坐标系的位姿就是机器人自身的“全局地址”。 3. 传感器坐标系（Sensor Frame）：是每个板载传感器的“小家”，原点在传感器的测量中心，坐标轴方向和传感器的安装方向一致：比如激光雷达一般x轴朝前、z轴朝天，IMU的z轴朝上，摄像头的z轴朝光轴方向。传感器测到的所有数据（点云、像素、加速度）都是在这个坐标系下的。 4. 末端执行器坐标系（End-Effector Frame）：是机器人“手”的坐标系，原点在手爪的中心，坐标轴方向一般和抓取方向对齐：比如机械臂的z轴朝手爪朝向，x轴朝手爪的“上方”。这个坐标系用来表示抓取物体的位置、焊接枪的姿态等。 5. 目标坐标系（Target Frame）：是机器人要交互的物体的坐标系，比如要抓的杯子、要避障的障碍物，原点在物体的中心，坐标轴方向一般和物体的朝向对齐。

> 📌 电子版侧边栏：坐标系选择的小技巧 > 设计机器人系统时，尽量让每个坐标系的轴和机器人的运动方向对齐：比如差速机器人的基坐标系x轴朝前，那么机器人前进的速度就是x方向的速度，转弯的角速度就是z方向的角速度，不用做额外的坐标转换，能减少大量计算量，也避免出错。

这些坐标系之间的关系，可以用下面的转换逻辑串起来：传感器测到的点云在传感器坐标系下，要先转到机器人基坐标系，再转到世界坐标系，才能和全局地图匹配；而全局地图里的目标物体位姿在世界坐标系下，要先转到机器人基坐标系，再转到末端执行器坐标系，才能计算机械臂的关节角度。整个过程就像“坐标系搬家”：把物体从一个坐标系里“搬”到另一个坐标系里，需要的工具就是齐次变换矩阵。

---

1.2 齐次变换：机器人坐标转换的“通用翻译器”

刚体运动包含两个核心要素：位置（平移）和姿态（旋转）。位置是3个标量$(x,y,z)$，很好表示；但姿态的表示就麻烦得多：你可以说“手机绕x轴转了90度”，也可以说“绕y轴转了270度”，两者得到的朝向完全一样，这就是姿态表示的非唯一性。而齐次变换矩阵就是用来同时表示平移和旋转、且能通过矩阵乘法直接完成连续变换的工具。

1.2.1 齐次坐标：把3维点变成4维向量

普通的3维坐标$(x,y,z)$只能表示位置，没法把平移和旋转统一成矩阵运算：旋转是矩阵乘法，平移是向量加法，两者没法直接合并。而齐次坐标就是把3维点扩展成4维向量$\boldsymbol{P} = [x, y, z, 1]^T$，多出来的第4个分量固定为1，这样平移和旋转就都可以表示成4×4的矩阵乘法了。

比如你要把一个点先沿x轴平移$t$米，再绕z轴旋转$\theta$角，普通3维坐标需要先算平移：$(x+t, y, z)$，再算旋转：$x' = (x+t)\cos\theta - y\sin\theta$，$y' = (x+t)\sin\theta + y\cos\theta$，步骤繁琐还容易错；而用齐次坐标的话，只需要构造两个4×4矩阵，直接相乘就行： $$\boldsymbol{P}' = \boldsymbol{T}_{\text{trans}} \boldsymbol{T}_{\text{rot}} \boldsymbol{P}$$ 其中平移矩阵和旋转矩阵的形式非常固定：

• 沿x轴平移$t$的变换矩阵：

$$\boldsymbol{T}_{\text{trans}}(x,t) = \begin{bmatrix} 1 & 0 & 0 & t \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}$$

• 绕z轴旋转$\theta$的变换矩阵：

$$\boldsymbol{T}_{\text{rot}}(z,\theta) = \begin{bmatrix} \cos\theta & -\sin\theta & 0 & 0 \\ \sin\theta & \cos\theta & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}$$

1.2.2 齐次变换矩阵的结构：平移+旋转的“打包格式”

任意两个刚体坐标系之间的齐次变换矩阵都是4×4的方阵，结构高度统一： $$\boldsymbol{T} = \begin{bmatrix} \boldsymbol{R} & \boldsymbol{t} \\ \boldsymbol{0}^T & 1 \end{bmatrix}$$ 其中：

• 左上角的3×3矩阵$\boldsymbol{R}$是**旋转矩阵**，表示两个坐标系的姿态差异：$\boldsymbol{R}$是正交矩阵，满足$\boldsymbol{R}^T\boldsymbol{R} = \boldsymbol{I}$，因此$\boldsymbol{R}^{-1} = \boldsymbol{R}^T$，这个性质在坐标逆变换时非常有用。
• 右上角的3×1向量$\boldsymbol{t}$是**平移向量**，表示两个坐标系原点的位置差异。
• 最后一行$[0,0,0,1]$是固定格式，保证矩阵乘法时齐次坐标的第4个分量始终为1。

比如你站在世界坐标系的原点，面朝x轴方向，往前走1米，再左转90度，此时你的基坐标系相对于世界坐标系的变换矩阵就是： $$\boldsymbol{T}_{WB} = \begin{bmatrix} 0 & -1 & 0 & 1 \\ 1 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}$$ 其中旋转矩阵$\begin{bmatrix}0&-1&0\\1&0&0\\0&0&1\end{bmatrix}$是绕z轴旋转90度的结果，平移向量$[1,0,0]^T$是沿x轴走了1米。

1.2.3 坐标变换的核心逻辑：不要搞反“左乘右乘”

齐次变换最常踩的坑就是变换矩阵的乘法顺序，这里可以用“坐标系搬家”的类比彻底讲清楚：假设你要把一个放在机器人基坐标系B里的点$\boldsymbol{P}_B$，转到世界坐标系W里，相当于要把这个点从B“搬”到W，需要先经过B到W的变换$\boldsymbol{T}_{WB}$，因此直接左乘就行： $$\boldsymbol{P}_W = \boldsymbol{T}_{WB} \boldsymbol{P}_B$$ 反过来，如果要把世界坐标系里的点$\boldsymbol{P}_W$转到B坐标系里，就是“反向搬家”，需要乘$\boldsymbol{T}_{WB}$的逆（也就是$\boldsymbol{T}_{BW}$）： $$\boldsymbol{P}_B = \boldsymbol{T}_{WB}^{-1} \boldsymbol{P}_W = \boldsymbol{T}_{BW} \boldsymbol{P}_W$$

如果是多级坐标转换，比如传感器坐标系S→基坐标系B→世界坐标系W，那么总变换矩阵就是各级变换矩阵依次左乘： $$\boldsymbol{T}_{WS} = \boldsymbol{T}_{WB} \boldsymbol{T}_{BS}$$ 这个顺序绝对不能反：$\boldsymbol{T}_{BS}$是传感器到基坐标系的变换，$\boldsymbol{T}_{WB}$是基坐标系到世界坐标系的变换，先转S到B，再转B到W，顺序错了结果完全错误。

```mermaid graph LR A[世界坐标系W] -->|T_WB| B[机器人基坐标系B] B -->|T_BS| C[传感器坐标系S] C -->|测量得到

```mermaid graph LR A[世界坐标系W] -->|T_WB| B[机器人基坐标系B] B -->|T_BS| C[传感器坐标系S] C -->|测量得到点云/像素/IMU数据| D[传感器原始数据] D -->|坐标转换| E[基坐标系下统一数据] E -->|坐标转换| F[世界坐标系下全局数据] F -->|匹配地图/目标识别| G[机器人全局位姿与场景理解] ```

补全这个流程后，我们再回到坐标变换的核心细节：很多初学者容易混淆变换矩阵的“方向”，这里给一个具体的数值验证：假设激光雷达坐标系S相对于机器人基坐标系B的变换是沿z轴向上平移0.5米，没有旋转，那么$\boldsymbol{T}_{BS}$为： $$\boldsymbol{T}_{BS} = \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0.5 \\ 0 & 0 & 0 & 1 \end{bmatrix}$$ 如果机器人的基坐标系B相对于世界坐标系W的位姿是$x=2\mathrm{m}, y=3\mathrm{m}, \theta=45^\circ$（航向角，即x轴和世界x轴的夹角），那么$\boldsymbol{T}_{WB}$为： $$\boldsymbol{T}_{WB} = \begin{bmatrix} \cos45^\circ & -\sin45^\circ & 0 & 2 \\ \sin45^\circ & \cos45^\circ & 0 & 3 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \approx \begin{bmatrix} 0.707 & -0.707 & 0 & 2 \\ 0.707 & 0.707 & 0 & 3 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}$$ 现在激光雷达测到一个前方1米、高度0米的障碍物，在激光雷达坐标系下的齐次坐标是$\boldsymbol{P}_S = [1, 0, 0, 1]^T$，转到世界坐标系下的坐标就是： $$\boldsymbol{P}_W = \boldsymbol{T}_{WB} \boldsymbol{T}_{BS} \boldsymbol{P}_S \approx \begin{bmatrix} 0.707 & -0.707 & 0 & 2 \\ 0.707 & 0.707 & 0 & 3 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0.5 \\ 0 & 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} 1 \\ 0 \\ 0 \\ 1 \end{bmatrix} = \begin{bmatrix} 2.707 \\ 3.707 \\ 0 \\ 1 \end{bmatrix}$$ 也就是障碍物在世界坐标系下的坐标是$(2.707, 3.707, 0)$，和我们的直观理解一致：机器人站在(2,3)的位置，面朝东北方向（45度），前方1米的障碍物自然在东北方向1米的位置，也就是世界坐标系的(2+0.707, 3+0.707, 0)。

---

1.3 姿态表示的进阶：从欧拉角到李群李代数

齐次变换矩阵虽然能统一表示平移和旋转，但它存在两个明显的痛点：一是冗余存储，3个自由度的旋转用9个元素表示，多了一倍的内存占用；二是不适合优化和插值，旋转矩阵必须满足正交约束（$\boldsymbol{R}^T\boldsymbol{R}=\boldsymbol{I}$且$\det(\boldsymbol{R})=1$），如果直接对两个旋转矩阵做线性插值，中间结果的矩阵会破坏正交性，得到的是无意义的“旋转”。为了解决这些问题，机器人领域发展出了多种姿态表示方法，其中最常用的是欧拉角、四元数和李代数。

1.3.1 欧拉角：最直观的姿态表示，却藏着“万向节死锁”的坑

欧拉角是用三个绕不同轴旋转的角度来表示姿态，是最符合人类直观理解的表示方法：比如我们说飞机的“偏航角Yaw（绕z轴转）、俯仰角Pitch（绕y轴转）、滚转角Roll（绕x轴转）”，就是典型的欧拉角， aerospace领域和机器人领域普遍采用这种ZYX顺序的欧拉角。

但欧拉角有一个致命的缺陷：奇点问题（万向节死锁）。当俯仰角Pitch达到±90°时，偏航轴和滚转轴会重合，这时候无论怎么转滚转角，都只能改变偏航和俯仰的组合，相当于少了一个自由度，这时候欧拉角无法唯一表示姿态。

> 📌 电子版侧边栏：直观体验万向节死锁 > 你可以拿三个互相垂直的指环代表三个旋转轴，先绕最外侧的环（z轴）转90度，让中间的环（y轴）和最内侧的环（x轴）平行，这时候你再转中间的环，最内侧的环会跟着一起转，相当于两个旋转轴“锁死”在了一起，这就是万向节死锁的直观表现。实际工程中，无人机的飞控、机械臂的位姿表示都会刻意避开这个奇点区域，或者直接使用无奇点的四元数/李代数表示。

欧拉角的另一个问题是非唯一性：同一个姿态可以对应多组欧拉角，比如绕z轴转360°和转0°的姿态完全一样，但欧拉角的数值完全不同，这在姿态优化、插值的时候会造成大量麻烦。

1.3.2 李群与李代数：机器人位姿优化的“核心工具”

欧拉角和四元数虽然解决了奇点问题，但在做位姿估计、SLAM优化的时候，依然存在求导困难、约束难处理的问题。这时候就需要引入李群（Lie Group）和李代数（Lie Algebra）这套数学工具，它是当前机器人位姿估计、运动规划领域的核心基础。

简单来说：

• **特殊欧氏群SE(3)** 是所有三维空间刚体变换（平移+旋转）的集合，是一个李群，每个元素就是一个4×4的齐次变换矩阵，对应一个位姿。SE(3)满足群的四个性质：封闭性、结合律、单位元、逆元，因此可以直接对两个位姿做“乘法”（即连续变换）。
• **特殊欧氏李代数se(3)** 是SE(3)在单位元处的切空间，是一个6维向量$\boldsymbol{\xi} = [v_x, v_y, v_z, \omega_x, \omega_y, \omega_z]^T$，前3个元素是平移分量，后3个元素是旋转的角速度轴（和角速度方向一致，模长是旋转角度），刚好对应刚体运动的6个自由度。

李群和李代数之间通过指数映射和对数映射相互转换：

• 指数映射：把李代数的6维向量$\boldsymbol{\xi}$映射到李群的齐次变换矩阵$\boldsymbol{T} \in SE(3)$，公式为：

$$\boldsymbol{T} = \exp(\boldsymbol{\xi}^\wedge)$$ 其中$\boldsymbol{\xi}^\wedge$是李代数的“hat”算子，把6维向量变成4×4的斜对称矩阵： $$\boldsymbol{\xi}^\wedge = \begin{bmatrix} \omega^\wedge & v \\ 0 & 0 \end{bmatrix}, \quad \omega^\wedge = \begin{bmatrix} 0 & -\omega_z & \omega_y \\ \omega_z & 0 & -\omega_x \\ -\omega_y & \omega_x & 0 \end{bmatrix}$$

• 对数映射：反过来把齐次变换矩阵$\boldsymbol{T}$映射回李代数的6维向量$\boldsymbol{\xi}$，公式为：

$$\boldsymbol{\xi} = \ln(\boldsymbol{T})^\vee$$ 其中$\vee$是“vee”算子，把斜对称矩阵变回6维向量。

李代数最大的优势是：它是一个无约束的6维向量，可以直接用梯度下降等优化方法处理，不需要考虑旋转矩阵的正交约束。比如在SLAM的位姿图优化中，我们需要最小化两个位姿之间的误差，用李代数表示位姿的话，误差项可以直接对$\boldsymbol{\xi}$求导，优化过程非常顺畅；而如果用旋转矩阵表示，还需要额外加正交约束，优化难度会大很多。

> 📌 电子版侧边栏：李代数的直观理解 > 你可以把李代数想象成“位姿的增量”：比如机器人当前位姿是$\boldsymbol{T}$，要走一小步，位姿增量是$\Delta\boldsymbol{\xi}$，那么新的位姿就是$\boldsymbol{T}' = \boldsymbol{T} \exp(\Delta\boldsymbol{\xi}^\wedge)$，这个增量就是李代数表示的，非常适合做运动规划、位姿修正。

1.3.3 姿态表示方法的选型对比

实际工程中，不同的场景需要选择不同的姿态表示方法，下表是常用的选型参考：

| 表示方法