3、坐标变换与刚体运动：坐标系基础、旋转矩阵、四元数、齐次变换矩阵、刚体运动学
各位同学，欢迎来到第三章。这一章，我们聊聊机器人抓取中最核心的数学基础——坐标变换与刚体运动。
说实话，我刚开始做机器人抓取那会儿，觉得这东西不就是算算坐标嘛，有啥难的？结果第一次调试机械臂，抓一个放在桌上的杯子，愣是抓歪了半米。后来一查，原来是相机坐标系和机械臂基坐标系之间的变换矩阵写反了。嗯，从那以后，我再也不敢小看坐标变换了。
核心观点：传感器融合的本质，就是把不同坐标系下的数据统一到同一个参考系下。没有坐标变换，融合就是空谈。
3.1 坐标系基础
先说说坐标系。你想想看，一个机器人身上有多少个坐标系？相机有自己的坐标系，机械臂每个关节都有一个坐标系，末端执行器也有一个坐标系，甚至被抓取的物体也有自己的坐标系。
我个人习惯把坐标系分成三类：
- 世界坐标系：固定不变的参考系，通常设在地面或机器人基座
- 传感器坐标系：相机、激光雷达等传感器自身的坐标系
- 工具/物体坐标系：末端夹爪、被抓物体自身的坐标系
坐标系由三个要素构成：原点位置、三个坐标轴的方向。在三维空间中，我们通常用右手定则来确定坐标轴方向。食指指向X轴，中指指向Y轴，拇指指向Z轴——这个大家应该都熟悉。
我的小技巧：在实际项目中，我习惯在每一个传感器和关键部件上都贴一个标签，写上它的坐标系名称。调试的时候，看一眼标签就知道当前在哪个坐标系下操作，避免搞混。
3.2 旋转矩阵
旋转矩阵，说白了就是描述一个坐标系相对于另一个坐标系怎么转的。它是一个3x3的正交矩阵，行列式为+1。
我记得有一次做视觉引导抓取，相机检测到物体的位姿，需要告诉机械臂怎么转过去抓。这里就用到了旋转矩阵。
绕X轴旋转θ角的旋转矩阵：
R_x(θ) = [1 0 0 ]
[0 cosθ -sinθ ]
[0 sinθ cosθ ]绕Y轴旋转θ角：
R_y(θ) = [cosθ 0 sinθ ]
[0 1 0 ]
[-sinθ 0 cosθ ]绕Z轴旋转θ角：
R_z(θ) = [cosθ -sinθ 0 ]
[sinθ cosθ 0 ]
[0 0 1 ]注意：我曾经踩过一个坑——旋转矩阵的乘法顺序。先绕X轴转再绕Y轴转，和先绕Y轴转再绕X轴转，结果完全不同。旋转矩阵乘法不满足交换律，这一点一定要牢记。
3.3 四元数
说到四元数，很多人第一反应是「好难」。其实没那么可怕。四元数就是用一个四维向量来表示三维空间中的旋转。
为什么不用旋转矩阵？因为旋转矩阵有9个参数，但只有3个自由度，存在冗余。而且连续旋转时会出现万向锁问题。四元数只有4个参数，没有冗余，也不会出现万向锁。
四元数的形式：
q = w + xi + yj + zk其中w是实部，x、y、z是虚部，满足i² = j² = k² = ijk = -1。
单位四元数（w² + x² + y² + z² = 1）表示纯旋转。w = cos(θ/2)，(x,y,z) = sin(θ/2) * 旋转轴方向向量。
实用建议：在传感器融合中，我强烈推荐使用四元数。卡尔曼滤波、姿态估计这些算法，用四元数比用欧拉角稳定得多。我在做IMU和视觉融合时，全部统一用四元数表示姿态，省去了很多麻烦。
3.4 齐次变换矩阵
齐次变换矩阵，就是把旋转和平移统一到一个4x4的矩阵里。形式如下：
T = [R t]
[0 1]其中R是3x3旋转矩阵，t是3x1平移向量。
为什么要用齐次变换？因为这样可以把连续的坐标变换写成矩阵乘法，非常简洁。比如从相机坐标系到机械臂基坐标系的变换：
T_base_cam = T_base_end * T_end_cam在实际的抓取系统中，我们经常需要做这样的链式变换：
- 物体在相机坐标系下的位姿 → 相机在机械臂末端下的位姿 → 末端在基座下的位姿
- 最终得到物体在基座坐标系下的位姿
- 然后机械臂才能规划路径去抓取
我的经验：写代码的时候，我习惯把所有变换矩阵都存成一个4x4的numpy数组。调试时打印出来，一眼就能看出旋转部分和平移部分是否正确。我曾经见过有人把旋转矩阵和平移向量分开存，结果对不齐，排查了半天。
3.5 刚体运动学
刚体运动学，研究的是物体在空间中的位置和姿态随时间的变化，不考虑力和质量。
一个刚体在三维空间中有6个自由度：3个平移自由度（x, y, z）和3个旋转自由度（roll, pitch, yaw 或 四元数）。
刚体运动的速度描述：
- 线速度：v = [vx, vy, vz]ᵀ
- 角速度：ω = [ωx, ωy, ωz]ᵀ
这两者合起来就是6维的「空间速度」或「旋量」。
在抓取控制中，我们经常需要计算机械臂末端的运动速度。比如视觉伺服控制中，根据图像特征点的运动，反推出机械臂末端需要的速度指令。
关键公式：刚体上一点P的速度 v_p = v_o + ω × r，其中v_o是刚体上参考点的速度，r是从参考点到P点的向量，ω是刚体的角速度。
3.6 知识体系总览
下面这张图，是我自己总结的本章知识体系。你可以把它当作一个思维导图来看：
3.7 实战中的避坑指南
最后，分享几个我在项目中踩过的坑：
- 坐标系方向搞反：我曾经把相机坐标系的Z轴方向搞反了，结果抓取时机械臂一直往桌子底下钻。后来加了一个可视化工具，实时显示各个坐标系的方向，问题就解决了。
- 四元数归一化：四元数必须归一化才能表示旋转。我见过有人直接拿未归一化的四元数做插值，结果姿态完全乱套。
- 变换矩阵的逆：求齐次变换矩阵的逆时，不能简单地对每个元素取倒数。正确做法是：R的逆是Rᵀ，t的逆是 -Rᵀt。
我的调试习惯：每次写完坐标变换的代码，我都会用一个已知点做测试。比如取物体坐标系下的原点(0,0,0)，变换到基座坐标系下，看看结果是否合理。这个小习惯帮我避免了很多低级错误。
好了，这一章的内容就到这里。坐标变换是传感器融合的基石，一定要熟练掌握。下一章我们会把这些知识用到实际的传感器融合算法中。
可可精品资料库