3. 坐标系与变换：世界坐标系、机器人坐标系、坐标变换矩阵、欧拉角与四元数

做全向轮底盘，绕不开坐标系。

说实话，我早年刚入行时，觉得坐标系这东西太简单了——不就是X、Y、Z嘛。结果第一次做多传感器融合，机器人原地转圈，定位数据却飞到了天边。查了一整天，最后发现是坐标变换矩阵写反了。嗯，从那以后，我再也不敢小看坐标系了。

3.1 世界坐标系 vs 机器人坐标系

先搞清楚两个最基础的概念。

世界坐标系，也叫全局坐标系。你可以把它想象成房间里的固定参考点——比如墙角那个点就是原点，X轴指向东，Y轴指向北，Z轴指向天。不管机器人怎么动，这个坐标系纹丝不动。

机器人坐标系，也叫本体坐标系。它绑在机器人身上，原点在机器人中心，X轴朝前，Y轴朝左，Z轴朝上。机器人一转，这个坐标系也跟着转。

为什么要区分这两个？

举个例子。你让机器人「向前走1米」。这个「前」是机器人自己的前，还是世界的前？

• 如果是机器人坐标系下的「前」——那机器人朝哪个方向，就往哪走。
• 如果是世界坐标系下的「前」——那不管机器人脸朝哪，都往世界坐标的X正方向走。

我做过一个项目，客户说「机器人往北走」，结果机器人原地打转。后来发现，他们把世界坐标系的北当成了机器人坐标系的「前」。说白了，就是坐标系没对齐。

3.2 坐标变换矩阵

坐标变换，说白了就是「把点从一个坐标系搬到另一个坐标系」。数学上，我们用4x4的齐次变换矩阵来完成。

// 齐次变换矩阵 T 的结构
// [ R   t ]
// [ 0   1 ]

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

举个例子。机器人在世界坐标系的位置是 (x, y, θ)，那么从机器人坐标系到世界坐标系的变换矩阵就是：

T = [ cosθ  -sinθ  0  x ]
    [ sinθ   cosθ  0  y ]
    [ 0      0     1  0 ]
    [ 0      0     0  1 ]

我个人习惯用齐次矩阵，因为一次乘法就能搞定旋转+平移，代码写起来也干净。

3.3 欧拉角

欧拉角，就是用三个角度来描述旋转：绕X轴转（roll）、绕Y轴转（pitch）、绕Z轴转（yaw）。

在全向轮底盘里，我们最常用的是 yaw（偏航角），也就是绕Z轴的旋转。因为机器人基本在平面上跑，roll和pitch变化不大。

但欧拉角有个大坑——万向锁。

我曾经在调试机械臂时遇到过。当pitch接近90度时，roll和yaw就分不清了，机器人突然抽搐了一下。嗯，这就是万向锁。

3.4 四元数

四元数，听起来玄乎，其实就是四个数：w, x, y, z。它没有万向锁问题，插值平滑，计算效率高。

// 四元数表示绕单位向量 (nx, ny, nz) 旋转 θ 角度
q = [cos(θ/2), nx*sin(θ/2), ny*sin(θ/2), nz*sin(θ/2)]

// 在 ROS 中，四元数常用格式
geometry_msgs::Quaternion q;
q.w = 0.707;
q.x = 0.0;
q.y = 0.0;
q.z = 0.707;  // 表示绕Z轴旋转90度

你想想看，四元数为什么好？因为它用四个数，避免了三个数带来的奇异性。而且两个旋转之间做插值，用四元数的球面线性插值（SLERP），平滑得不得了。

我记得有一次做全向轮底盘的轨迹跟踪，用欧拉角做插值，机器人走出来的路径像锯齿。换成四元数SLERP后，路径丝滑得像德芙巧克力。

3.5 知识体系总览

下面这张图，是我自己总结的坐标系与变换的知识结构。你一看就明白各个概念之间的关系。

3.6 实战中的选择建议

场景	推荐方式	原因
平面全向轮底盘	欧拉角（仅yaw）	简单直观，无万向锁风险
带倾斜的底盘	四元数	避免万向锁，插值平滑
多传感器融合	四元数 + 齐次矩阵	计算稳定，适合EKF/UKF
人机交互调试	欧拉角	人类更容易理解角度值

最后说一句。坐标系和变换，是机器人学的「语法」。语法不对，句子就错了。我见过太多项目，代码写得飞起，最后定位不准，查来查去，就是坐标系搞反了。

所以，花点时间把这块吃透，后面做速度解算、定位融合，你会感谢自己的。