082 运动控制中的坐标系变换：齐次变换矩阵
从一次现场调试说起
去年在调试一台六轴协作机器人时，遇到了一个让我熬夜到凌晨三点的问题。机械臂末端执行器在抓取工件时，明明示教点坐标都正确，但一运行到特定姿态，抓取位置就偏了2毫米。当时我盯着示波器上的编码器数据，百思不得其解。
后来发现，问题出在坐标系变换上——我在计算工具中心点（TCP）时，把旋转矩阵和平移向量拼错了顺序。这个教训让我意识到，齐次变换矩阵不是数学课本上的抽象概念，而是嵌入式运动控制中每天都要面对的“硬通货”。
为什么需要齐次变换矩阵
在运动控制系统中，我们至少会面对三个坐标系：世界坐标系（基座标系）、关节坐标系、工具坐标系。如果你做AGV或者移动机器人，还得加上车体坐标系和导航坐标系。
传统做法是把旋转和平移分开处理：先旋转，再平移。但这样在串联多个变换时，代码会变得极其混乱。比如你要计算“工具末端相对于基座标系的位置”，中间可能经过关节1到关节6的六次变换，每次都要处理旋转矩阵乘法和向量加法，代码里全是嵌套的for循环，调试时根本分不清哪一步算错了。
齐次变换矩阵的妙处在于，它把旋转和平移统一成一个4x4矩阵。这样，多次变换就变成了矩阵乘法，代码结构清晰，调试时只需要检查矩阵乘法的结果是否正确。
齐次变换矩阵的数学本质
别被4x4矩阵吓到。本质上，它就是把3x3旋转矩阵和3x1平移向量塞进一个4x4的壳子里。
| R11 R12 R13 Tx | | R21 R22 R23 Ty |
