2. 坐标系与位姿表示

好，咱们正式开始搭建路径跟踪系统的第一块基石——坐标系与位姿表示。

说实话，我刚开始做机器人那会儿，觉得坐标系这东西太简单了，不就是X、Y、Z三个轴嘛。结果第一次调试AGV小车，发现它死活不走直线，折腾了两天才意识到是坐标系搞反了。嗯，从那以后，我再也不敢小看这个基础了。

2.1 世界坐标系与机器人坐标系

先说说两个最常用的坐标系。

世界坐标系，你可以理解为整个场景的“绝对参考系”。比如在工厂里，我们以某个墙角为原点，东为X轴，北为Y轴，天花板方向为Z轴。所有静态物体——货架、工位、充电桩——都在这套坐标系下有固定的坐标。

机器人坐标系，也叫本体坐标系，是贴在机器人身上的。通常以机器人的几何中心为原点，前进方向为X轴，左侧为Y轴，垂直向上为Z轴。这个坐标系会随着机器人移动而移动。

我个人的习惯是：所有传感器数据先转换到机器人坐标系，再通过位姿变换映射到世界坐标系。这样做的好处是，传感器标定一次就够了，不用每次换场地都重新标。

2.2 齐次变换矩阵

怎么把一个坐标系下的点，转换到另一个坐标系下？

答案就是齐次变换矩阵。它长这样：

| R   t |
| 0   1 |

其中R是3x3的旋转矩阵，t是3x1的平移向量。这个4x4的矩阵，可以同时描述旋转和平移。

举个例子，如果机器人坐标系原点在世界坐标系中的位置是(1, 2, 0)，并且机器人没有旋转，那么齐次变换矩阵就是：

| 1 0 0 1 |
| 0 1 0 2 |
| 0 0 1 0 |
| 0 0 0 1 |

把机器人坐标系下的点(0.5, 0.3, 0)乘以这个矩阵，就得到世界坐标(1.5, 2.3, 0)。

2.3 欧拉角

旋转矩阵有9个元素，但自由度只有3个。能不能用更少的参数表示旋转？

欧拉角就是答案。它用三个角度来描述旋转：绕X轴转（横滚角roll）、绕Y轴转（俯仰角pitch）、绕Z轴转（偏航角yaw）。

在路径跟踪中，我们最关心的是偏航角yaw。因为机器人在地面上跑，横滚和俯仰基本为0。说白了，就是看机器人“脸朝哪边”。

但欧拉角有个大坑——万向锁。当俯仰角接近±90度时，横滚和偏航会变得无法区分。我在做无人机项目时遇到过这个问题，飞控突然失控，就是因为俯仰角过大触发了万向锁。

2.4 四元数

四元数，听起来很玄乎，其实就是一个四维向量(qw, qx, qy, qz)。它没有万向锁问题，插值平滑，计算效率高。

我刚开始学四元数时，觉得它反直觉。后来发现一个简单的理解方式：四元数描述的是“绕某个轴转多少度”。比如绕Z轴转90度，对应的四元数是：

qw = cos(45°) = 0.707
qx = 0
qy = 0
qz = sin(45°) = 0.707

你看，比旋转矩阵直观多了吧？

在实际代码中，我通常这样用：

// C++示例：四元数转旋转矩阵
Eigen::Quaterniond q(0.707, 0, 0, 0.707);  // w, x, y, z
Eigen::Matrix3d R = q.toRotationMatrix();

// 四元数插值
Eigen::Quaterniond q_start(1, 0, 0, 0);
Eigen::Quaterniond q_end(0, 0.707, 0, 0.707);
Eigen::Quaterniond q_interp = q_start.slerp(0.5, q_end);

2.5 知识体系总览

下面这张图，是我做课程时画的。它把本章的核心逻辑串起来了：

2.6 实际项目中的选择建议

说了这么多理论，到底该用哪种表示方式？我根据经验总结了一张表：

表示方式	优点	缺点	推荐场景
旋转矩阵	直观，容易理解	9个参数，冗余大	坐标变换计算
欧拉角	参数少，物理意义明确	万向锁，不适合插值	人机交互界面显示
四元数	无奇点，插值平滑	不够直观	姿态控制、轨迹插值

好了，坐标系和位姿表示就讲到这里。这些东西看似基础，但真的会直接影响整个系统的成败。我见过太多项目，最后查bug发现是坐标系搞混了——嗯，希望你不会踩这个坑。