第1章：齐次坐标与变换矩阵

做机器人规划调试这些年，我见过太多人在坐标变换上栽跟头。说白了，这玩意儿就是机器人学的"地基"。地基不稳，上层算法再漂亮也是空中楼阁。今天咱们就来啃这块硬骨头。

先问个问题：在三维空间中，一个点用(x,y,z)表示，这很自然对吧？但当你需要同时表示旋转和平移时，麻烦就来了。

我记得刚入行那会儿，用3x3矩阵做旋转，平移还得单独加个向量。每次变换都要写两套公式，代码里全是if-else判断。有一次调试机械臂的抓取程序，就是因为旋转和平移没处理好，眼睁睁看着夹爪从目标旁边擦过去...嗯，从那以后我就再也不敢轻视坐标变换了。

齐次坐标的引入，说白了就是给三维点加一个维度。把(x,y,z)变成(x,y,z,w)，其中w通常取1。这样一来，旋转和平移就能统一用4x4矩阵表示了。

核心思想：用n+1维向量表示n维空间中的点，让线性变换能统一处理旋转和平移。

咱们先看平移。假设要把点P沿x轴移动dx，沿y轴移动dy，沿z轴移动dz。用齐次坐标写出来就是：

| 1  0  0  dx |   | x |   | x + dx |
| 0  1  0  dy | * | y | = | y + dy |
| 0  0  1  dz |   | z |   | z + dz |
| 0  0  0  1  |   | 1 |   |   1    |

再看旋转。绕z轴旋转θ角，矩阵长这样：

| cosθ  -sinθ  0  0 |
| sinθ   cosθ  0  0 |
|  0      0    1  0 |
|  0      0    0  1 |

绕x轴和y轴的旋转矩阵类似，只是把sin和cos放到对应位置。我个人习惯把这三个基本旋转矩阵记作Rx(θ)、Ry(θ)、Rz(θ)。

我的小技巧：写代码时，我习惯把旋转矩阵和平移矩阵分开定义，最后再乘到一起。这样调试时能单独验证每个部分是否正确。

实际项目中，很少只用一次变换。比如机械臂从基座到末端，中间要经过好几个关节。每个关节都有自己的坐标系，变换就得一层层叠加上去。

链式法则说白了就是：多个变换矩阵按顺序相乘。假设有变换T1、T2、T3，从原始点P到最终点P'的变换就是：

P' = T3 * T2 * T1 * P

注意顺序！矩阵乘法不满足交换律。先旋转再平移，和先平移再旋转，结果完全不同。我曾经在调试AGV导航时，就是因为把旋转和平移的顺序搞反了，车子在原地转了好几圈才找到方向...后来我养成了一个习惯：每次写变换代码，都会在注释里标明"先旋转后平移"还是"先平移后旋转"。

下面这张图展示了复合变换的完整流程：

讲几个我踩过的坑，希望对你有帮助。

坑1：坐标系方向搞反

我曾经在调试视觉引导抓取时，相机坐标系和机器人基坐标系的方向没对齐。结果机器人每次抓取都偏了10厘米。后来发现是旋转矩阵的符号写反了。建议：每次定义坐标系时，先用右手定则确认方向。

坑2：齐次坐标的w分量忘记归一化

连续做几次变换后，w分量可能不再是1。如果不做归一化，点坐标会莫名其妙地缩放。我习惯在每个变换链的末尾加一步归一化处理。

我的调试习惯：

齐次坐标和变换矩阵，是机器人规划调试的基石。记住三点：

这些概念看着简单，但真正用好需要大量实践。我建议你找个开源机器人仿真环境，亲手写几段变换代码试试。纸上得来终觉浅，绝知此事要躬行。