机器人空间描述与坐标变换：旋转矩阵、齐次变换与位姿计算入门

1. 从机器人为什么会撞到自己的胳膊说起很多人第一次接触机器人学是从一个看似很蠢的问题开始的为什么机械臂抓东西的时候明明目标就在眼前它却经常摆出一个拧巴到不行的姿势甚至差点撞到自己我当年在实验室里调一台六轴机械臂就遇到过这种尴尬——手动示教时它能很顺畅地走到位一切换成程序下发位姿末端就直奔一个诡异的角度去把旁边的人吓得够呛。问题的根子几乎都出在空间描述和坐标变换上。这两个词听起来很学术但说白了就是一件事机器人得先搞清楚东西在哪自己朝哪才谈得上动。你告诉它把杯子拿起来这句话在人脑子里对应的是一个三维位置加一个姿态而机器人控制器里真正接收到的是一串数。这串数怎么来的、怎么换算、参考的是哪个坐标系、旋转变换有没有搞反——全在这一层决定。这篇内容适合谁如果你是刚翻开机器人学教材、被旋转矩阵和齐次变换绕晕的学生或者是转行做机器人应用、天天跟示教器和位姿数据打交道的工程师再或者只是想搞明白机器人到底是怎么知道自己在哪的好奇者这一篇都能用得上。我不打算把教材原样复述一遍——那些公式书上有。我要做的是把为什么要有这些数学工具、每个量到底代表什么、实操中哪里最容易翻车讲清楚让你看完能自己动手把一个位姿算出来而不是只会背公式。整篇内容会围绕几条主线展开空间里一个点、一个刚体到底该怎么描述旋转矩阵为什么长这样、怎么用它齐次变换矩阵是怎么把旋转平移打包在一起的以及多坐标系串联时为什么顺序一错结果就面目全非。每一条我都会配上一个能直接算、能直接验证的例子你看完可以自己拿笔或者拿代码跑一遍。先说一个反直觉的结论机器人学里最难的往往不是动力学、不是控制算法而是最开头这几页几何。后面再复杂的规划、再花哨的强化学习最后都要落到一个末端相对于基座的位姿是多少上。这一层要是含糊后面全是空中楼阁。2. 位置和姿态把在哪和朝哪拆开看2.1 位置描述只需要三个数但有前提描述空间中一个点的位置最直观的办法就是在某个坐标系里给出三个坐标。比如以机器人基座为原点建立坐标系 {A}那么末端执行器上某个点的位置可以写成一个 3×1 的列向量$$^A P \begin{bmatrix} p_x \ p_y \ p_z \end{bmatrix}$$这里的左上标 A 非常关键它表示这个位置是在坐标系 A 里量的。我在带新人的时候发现大部分人最开始会忽略这个上标觉得是多余的装饰。等做到两个、三个坐标系串联位姿突然算不对回头一看全是上标写错、参考系搞混惹的祸。一个位置向量单独存在是没有意义的必须绑定一个参考坐标系。你可以这样理解我说离我三步远你得先知道我是谁、面朝哪。坐标系就是那个我。位置描述的本质是在一个已知的参照系里用三个正交方向上的分量锁定一个点。这里有个常见误解要提前澄清三个数并不总是长度。在笛卡尔坐标系里它们是长度分量但换成柱坐标、球坐标三个数的含义就变了。机器人学里绝大部分场景默认用笛卡尔坐标所以位置就是三个长度分量。但你要清楚这只是一种约定不是天然如此。2.2 姿态描述才是真正的难点所在位置好办三个数搞定。姿态就麻烦了——刚体在空间里可以任意朝向怎么用数字把它转了多少、往哪转描述清楚这是机器人学里最容易让人掉进去的坑。先建立直觉一个刚体的姿态本质上是固连在它身上的坐标系相对于参考坐标系的朝向关系。所以描述姿态等价于描述一个坐标系的三个轴分别指向参考坐标系的什么方向。设有一个固连在刚体上的坐标系 {B}它的三个单位轴向量在参考坐标系 {A} 中的表示分别是 $^A X_B$、$^A Y_B$、$^A Z_B$每个都是 3×1 的列向量。把这三大列并排放在一起就得到了旋转矩阵$$^A_B R \begin{bmatrix} ^A X_B ^A Y_B ^A Z_B \end{bmatrix} \begin{bmatrix} r_{11} r_{12} r_{13} \ r_{21} r_{22} r_{23} \ r_{31} r_{32} r_{33} \end{bmatrix}$$这个 3×3 矩阵是整个机器人学的地基之一。它有三个必须记住的性质我把它整理成一张表因为这三条直接决定了后面所有推导成不成立性质数学表达实际含义正交性各列两两点积为 0三个轴互相垂直坐标系不变形单位性每列的模长为 1轴向量是单位向量只表方向不表长度行列式为 1$\det(R) 1$排除镜像只表示纯旋转最后那条要特别强调。行列式等于 1 而不是 -1意味着这个变换是正常旋转不会把左手系变成右手系、不会产生镜像。很多调试时的诡异现象——比如机器人明明该往左转却看起来像照镜子——追到根上就是某个旋转矩阵行列式算成了负的或者某处不小心用了转置。2.3 一个能立刻上手的数值例子光说抽象性质没感觉来一个具体的。为了让例子不至于太琐碎我打算在下面几节里用同一个场景贯穿始终机器人基座坐标系 {A}末端工具坐标系 {B}我们要求出从 A 到 B 的变换。这样你读起来能一直挂在同一个语境里。假设 {B} 相对于 {A} 只绕 Z 轴转了 90 度位置没动。那么 {B} 的 X 轴在 {A} 里指向原来的 Y 方向即 $(0,1,0)^T${B} 的 Y 轴指向 $-X$ 方向即 $(-1,0,0)^T${B} 的 Z 轴和 {A} 的 Z 轴重合即 $(0,0,1)^T$。并起来$$^A_B R \begin{bmatrix} 0 -1 0 \ 1 0 0 \ 0 0 1 \end{bmatrix}$$这正好是绕 Z 轴转 90 度的标准旋转矩阵形式。你可以拿它验算把 {B} 里的点 $(1,0,0)^T$ 变换到 {A}得到 $^A_B R \cdot (1,0,0)^T (0,1,0)^T$也就是转到 A 的 Y 方向上——和绕 Z 转 90 度完全吻合。这种自己代进去验一遍的习惯比死记矩阵格式可靠得多。3. 旋转矩阵为什么长这样怎么用它3.1 从轴角到矩阵的构造逻辑旋转矩阵不是凭空写出来的它来自一个很朴素的想法旋转不改变长度也不改变夹角。任何满足这两条的变换都是旋转或者旋转加反射被行列式条件排除掉。正交矩阵恰好刻画了保持长度和夹角这件事所以旋转的数学语言自然就是正交矩阵。具体构造上最常用的是基本旋转——绕 X、Y、Z 轴分别转一个角度得到三个基础矩阵任何复杂旋转都可以拆成它们的组合。绕 Z 轴转 $\theta$ 的矩阵是$$R_z(\theta) \begin{bmatrix} \cos\theta -\sin\theta 0 \ \sin\theta \cos\theta 0 \ 0 0 1 \end{bmatrix}$$绕 Y 轴转 $\theta$注意符号习惯:$$R_y(\theta) \begin{bmatrix} \cos\theta 0 \sin\theta \ 0 1 0 \ -\sin\theta 0 \cos\theta \end{bmatrix}$$绕 X 轴转 $\theta$$$R_x(\theta) \begin{bmatrix} 1 0 0 \ 0 \cos\theta -\sin\theta \ 0 \sin\theta \cos\theta \end{bmatrix}$$这三个矩阵我建议你不要背而是记住构造方法确定哪个轴不动那个轴对应的行列上就是 1 和 0剩下两个轴构成的平面里就是普通的二维旋转。绕 Z 轴转时 Z 轴不动所以第三行第三列是 1左上角 2×2 是二维旋转矩阵。这个方法一记三个矩阵永远不用查表。注意绕 Y 轴的矩阵里$\sin\theta$ 的符号位置和另外两个不一样。这不是笔误而是右手系下 Y 轴旋转的固有形式。很多人在这一步栽跟头把三个矩阵统一成同一种模式结果算出的姿态总是差一个镜像。3.2 复合旋转顺序决定结果单个轴旋转好办麻烦的是先绕 X 转、再绕 Y 转这种复合。这里有一条铁律旋转矩阵的乘法不可交换$R_1 R_2 \neq R_2 R_1$。这不是数学上的小瑕疵而是物理现实——你拿一本书先绕竖直轴转 90 度再绕水平轴转 90 度和反过来做书的朝向完全不同。不信就用手边的手机试一下两秒钟就能验证。所以在写代码或做推导时顺序信息必须显式保留。常见的约定有两类固定轴旋转如 XYZ 欧拉角每次都绕参考系固定的轴和动轴旋转如 ZYZ 欧拉角每次都绕刚体自己的当前轴转。这两类即使角度数值一样结果也完全不同。机器人学教材里那个著名的万向节死锁问题根源就在某些欧拉角组合会在特定姿态下丢失一个自由度。我用一个简化例子说明为什么顺序重要。设 $R_1 R_x(90°)$$R_2 R_z(90°)$。先算 $R_1 R_2$表示先做 $R_2$ 再做 $R_1$因为矩阵作用在列向量上是从右往左$$R_x(90°) R_z(90°) \begin{bmatrix} 1 0 0 \ 0 0 -1 \ 0 1 0 \end{bmatrix} \begin{bmatrix} 0 -1 0 \ 1 0 0 \ 0 0 1 \end{bmatrix} \begin{bmatrix} 0 -1 0 \ 0 0 -1 \ 1 0 0 \end{bmatrix}$$再算 $R_2 R_1$$$R_z(90°) R_x(90°) \begin{bmatrix} 0 -1 0 \ 1 0 0 \ 0 0 1 \end{bmatrix} \begin{bmatrix} 1 0 0 \ 0 0 -1 \ 0 1 0 \end{bmatrix} \begin{bmatrix} 0 0 1 \ 1 0 0 \ 0 1 0 \end{bmatrix}$$两个结果明显不同。这不是巧合是必然。所以凡是看到旋转矩阵相乘第一步永远是确认乘法顺序第二步才是算数。3.3 旋转矩阵的逆转置就够但方向别搞反旋转矩阵有一