一、欧拉角与旋转矩阵：从入门到避坑
做机器人规划调试，绕不开坐标变换。而坐标变换里，最让人头疼的，往往就是欧拉角和旋转矩阵。
我刚开始做规划时，就被这玩意儿坑过。明明路径规划得好好的，机器人一到某个姿态就突然“抽风”，原地打转。后来才发现，是欧拉角搞的鬼。今天咱们就来把这层窗户纸捅破。
1.1 欧拉角的定义：RPY 到底在说啥？
欧拉角，说白了就是用三个角度来描述一个物体的朝向。你想想看，一个刚体在三维空间里，要描述它“脸朝哪、头顶哪”，三个数就够了。
最常用的就是 RPY 角：
- Roll（滚转）：绕 X 轴旋转。就像飞机翻身，机翼上下摆动。
- Pitch（俯仰）：绕 Y 轴旋转。就像飞机抬头低头。
- Yaw（偏航）：绕 Z 轴旋转。就像飞机左右转弯。
我个人习惯把 RPY 记成“先偏航，再俯仰，最后滚转”。这个顺序很重要，后面会讲为什么。
重要提醒：欧拉角的定义顺序不是唯一的。有的系统用 ZYX，有的用 XYZ。ROS 里默认是 ZYX（即先 Yaw，再 Pitch，最后 Roll）。调试时一定要先确认你用的哪个约定。
1.2 旋转矩阵的构建：从角度到矩阵
旋转矩阵，就是把欧拉角变成数学上能算的东西。每个轴对应一个基本旋转矩阵：
// 绕 X 轴旋转 Roll 角
R_x(φ) = [1, 0, 0 ]
[0, cos(φ), -sin(φ)]
[0, sin(φ), cos(φ)]
// 绕 Y 轴旋转 Pitch 角
R_y(θ) = [ cos(θ), 0, sin(θ)]
[ 0, 1, 0 ]
[-sin(θ), 0, cos(θ)]
// 绕 Z 轴旋转 Yaw 角
R_z(ψ) = [cos(ψ), -sin(ψ), 0]
[sin(ψ), cos(ψ), 0]
[ 0, 0, 1]
完整的旋转矩阵就是这三个矩阵的乘积。按 ZYX 顺序：
R = R_z(Yaw) * R_y(Pitch) * R_x(Roll)
嗯，这里要注意：矩阵乘法不满足交换律。顺序错了，结果就完全不对。我在项目中遇到过，同事把乘法顺序写反了，结果机器人末端执行器直接怼到工件上去了……还好速度不快。
1.3 万向锁问题：规划中的隐形杀手
万向锁，是欧拉角最臭名昭著的缺陷。为什么会这样？
简单说，当 Pitch 角接近 ±90° 时，Roll 和 Yaw 的旋转轴会重合。这时候，你丢失了一个自由度。本来三个角度控制三个轴，现在两个角度控制同一个轴，你就没法独立控制所有方向了。
举个例子：
- 假设机器人当前 Pitch = 90°（手臂竖直向上）
- 你想让它绕某个轴转 10°
- 结果发现，无论你调 Roll 还是 Yaw，机器人都在同一个方向转
- 你永远到不了目标姿态
这就是万向锁。在规划中，它会导致：
- 路径插值出现奇异点
- 机器人突然加速或反向运动
- 轨迹跟踪失败
避坑指南：我曾经调试一个六轴机械臂的喷涂轨迹，每到工件侧面（Pitch 接近 90°）机器人就剧烈抖动。查了两天，最后发现是欧拉角插值导致的万向锁。后来改用四元数插值，问题立刻解决。
1.4 知识体系总览
下面这张图，是我自己总结的欧拉角与旋转矩阵的知识脉络。建议你收藏，调试时对照着看：
1.5 实战建议
说了这么多理论，来点实际的。我总结了几条调试经验：
| 场景 | 推荐做法 | 不推荐做法 |
|---|---|---|
| 人机交互界面 | 用欧拉角显示姿态（直观） | 用四元数显示（反人类） |
| 轨迹插值 | 用四元数 SLERP | 用欧拉角线性插值 |
| 运动学计算 | 用旋转矩阵或四元数 | 用欧拉角做微分运算 |
| 调试打印 | 欧拉角 + 四元数同时打印 | 只打印一种 |
小技巧：调试时，我习惯在代码里同时打印欧拉角和对应的四元数。一旦发现欧拉角跳变（比如从 179° 跳到 -179°），就知道快到万向锁区域了。这时候赶紧检查插值逻辑。
最后说一句：欧拉角不是不能用，但要清楚它的局限。你想想看，如果每个规划任务都只用欧拉角，迟早会踩坑。我的建议是——显示用欧拉角，计算用四元数，调试两者都看。
嗯，这一章就到这里。记住今天讲的三个核心：RPY 定义、矩阵构建、万向锁。下一章咱们聊聊四元数，那个东西虽然看着抽象，但用起来是真香。