3、坐标变换与空间描述：刚体运动描述

各位，咱们今天聊聊机器人学里最基础、也最绕不开的一块——坐标变换。说白了，就是怎么描述一个物体在空间里“动了”。

我刚开始做机器人那会儿，总觉得这东西太理论，不就是个旋转平移嘛。直到有一次，我调试一个六轴机械臂的抓取程序，明明数学算出来位置是对的，可机械臂就是抓不到工件。折腾了两天，最后发现是坐标变换的顺序搞反了。嗯，从那以后，我再也不敢小看这个“基础”了。

3.1 刚体运动的三种“语言”

描述一个刚体在三维空间里的姿态，说白了就是描述它“转了多少”。常用的有三种方式：旋转矩阵、欧拉角、四元数。我个人的习惯是，根据场景选工具，别死磕一种。

3.1.1 旋转矩阵

旋转矩阵是最直观的。它是一个3x3的矩阵，每一列代表旋转后的新坐标轴在原坐标系下的投影。

举个例子，绕Z轴转θ角，旋转矩阵长这样：

R_z(θ) = | cosθ  -sinθ  0 |
         | sinθ   cosθ  0 |
         | 0      0     1 |

它的好处是：可以直接跟向量相乘，得到旋转后的新坐标。而且多个旋转可以连乘，形成复合旋转。

但缺点也很明显——冗余。9个元素，其实只有3个自由度。而且你想想看，每次更新姿态都得维护一个3x3矩阵，计算量不小。

3.1.2 欧拉角

欧拉角就亲民多了。它用三个角度来描述旋转：比如绕Z轴转ψ（偏航），绕Y轴转θ（俯仰），绕X轴转φ（滚转）。

我见过很多做无人机飞控的朋友，特别喜欢用欧拉角。为什么？因为直观啊！你告诉飞控“俯仰5度”，它就知道怎么动。

但欧拉角有个大坑——万向锁。当俯仰角接近±90度时，偏航和滚转就分不清了，会丢失一个自由度。

3.1.3 四元数

四元数，听起来玄乎，其实它就是四个数：一个实部加三个虚部。写成 q = w + xi + yj + zk。

它的好处太多了：

• 没有万向锁
• 插值平滑（做动画、轨迹规划时特别好用）
• 计算效率高（比矩阵快）

我个人的习惯是：在IMU（惯性测量单元）数据处理、姿态解算、以及需要频繁插值的场景里，一律用四元数。只有在需要给人看、或者做可视化界面时，才转成欧拉角。

四元数旋转向量的公式也很简洁：

v' = q * v * q_conj

其中q_conj是q的共轭。你看，一行代码就搞定了。

3.2 齐次变换矩阵

光有旋转还不够，刚体还会平移。所以我们需要一个工具，把旋转和平移打包在一起。这就是齐次变换矩阵。

它是一个4x4的矩阵：

T = | R   t |
    | 0   1 |

其中R是3x3旋转矩阵，t是3x1平移向量。

为什么叫“齐次”？因为我们在三维坐标后面加了一个1，变成齐次坐标。这样，旋转和平移就可以用一个矩阵乘法搞定。

举个例子，一个点p在坐标系A中的坐标是p_A，要转换到坐标系B：

p_B = T_B_A * p_A

这里的T_B_A表示“从A到B的变换”。

3.3 坐标系转换与链式法则

机器人身上有好多坐标系：基座坐标系、关节坐标系、工具坐标系、相机坐标系……它们之间怎么转换？

答案是：链式法则。

假设我们有一个机械臂，从基座到末端执行器，经过三个关节：

• T_1_0：从基座到关节1
• T_2_1：从关节1到关节2
• T_3_2：从关节2到关节3（末端）

那么从基座到末端的变换就是：

T_3_0 = T_3_2 * T_2_1 * T_1_0

注意顺序！是右乘。因为后面的变换是在前面的坐标系下定义的。

我刚开始学的时候，总搞不清左乘右乘。后来我想了个笨办法：你从基座出发，每经过一个关节，就把这个关节的变换矩阵乘到右边。这样一步步“走”过去，就不会错了。

3.4 实战：一个简单的坐标变换例子

假设你有一个移动机器人，上面装了一个激光雷达。激光雷达检测到一个障碍物，在雷达坐标系下的坐标是 (1, 0, 0) 米。雷达安装在机器人前方0.5米处，且朝上倾斜了30度。

问：这个障碍物在机器人坐标系下的坐标是多少？

步骤：

1. 先做旋转：绕X轴转-30度（因为雷达朝上，所以是负的）
2. 再做平移：沿X轴平移0.5米

写成齐次变换矩阵：

T_robot_radar = | 1      0         0     0.5 |
                | 0   cos30°  -sin30°   0  |
                | 0   sin30°   cos30°   0  |
                | 0      0         0     1  |

然后：

p_robot = T_robot_radar * p_radar

算出来，障碍物在机器人坐标系下的位置大约是 (1.5, 0, 0.5) 米。

你看，就这么简单。但实际项目中，往往有七八个坐标系串在一起，这时候就得靠链式法则一步步推了。

3.5 小结

这一章的内容，说白了就是三件事：

• 怎么描述旋转——矩阵、欧拉角、四元数，各有各的脾气
• 怎么打包旋转+平移——齐次变换矩阵
• 怎么串起来——链式法则

我个人觉得，学坐标变换最好的方法就是动手算。拿张纸，画个坐标系树，标清楚每个变换的方向，然后一步步乘。算多了，自然就熟了。

下一章，咱们聊聊传感器数据的融合方法。嗯，那又是另一个有意思的话题了。