基础数学回顾：向量、坐标系变换、欧拉角与四元数

说实话，每次带新人做路径规划项目，我都要先问一句：「你数学底子怎么样？」

不是故意刁难。而是做动态避障，你绕不开向量、坐标系变换、姿态表达这些东西。我见过太多人代码写得飞起，结果一遇到旋转矩阵就懵了，最后调了三天bug，发现是坐标系搞反了。

所以这一章，咱们把基础打牢。别急，我不会扔一堆公式就跑。我会结合我实际踩过的坑来讲。

2.1 向量：路径规划的基本语言

向量是什么？说白了就是带方向的箭头。在路径规划里，它代表位置、速度、加速度，甚至代表障碍物相对于你的方向。

2.1.1 向量的基本运算

• 加法：两个向量首尾相连，结果就是合向量。比如机器人当前位置加上速度向量，就是下一时刻的位置。
• 点积：判断两个向量是否「同向」。点积为正，说明方向大致一致；为负，说明方向相反。我在做避障时常用它来判断障碍物是否在机器人前方。
• 叉积：得到垂直于两个向量的新向量。常用于计算法向量，或者判断转向方向。

// C++ 示例：向量点积判断障碍物方向
Eigen::Vector3d robotForward(1, 0, 0); // 机器人朝向
Eigen::Vector3d obstacleDir = obstaclePos - robotPos;
double dot = robotForward.dot(obstacleDir);
if (dot > 0) {
    // 障碍物在前方，需要避让
} else {
    // 障碍物在后方，暂时安全
}

2.1.2 向量归一化

把向量长度变成1，只保留方向。这在路径规划里太常用了。比如你要计算机器人指向目标点的单位方向，然后乘以速度，就得到了速度向量。

嗯，这里要注意：归一化前一定要检查向量长度是否为0。我曾经在调试时忘了这个，结果除零导致程序崩溃，查了半天才发现。

2.2 坐标系变换：从世界到机器人

你想想看，机器人感知到的障碍物是在「机器人坐标系」下的，但全局路径规划是在「世界坐标系」下做的。这两个坐标系之间怎么转换？

这就是坐标系变换要解决的问题。

2.2.1 平移与旋转

坐标系变换说白了就两步：先旋转，再平移。或者反过来也行，但顺序不能乱。

• 平移：把原点挪过去。比如机器人坐标系原点在机器人中心，世界坐标系原点在房间角落。两者相差一个平移向量。
• 旋转：把坐标轴转过去。比如机器人朝北，世界坐标系的Y轴朝北，那旋转角度就是0。

2.2.2 齐次坐标与变换矩阵

为了把平移和旋转统一成矩阵乘法，我们引入齐次坐标。说白了就是给二维向量加一个1，变成三维；给三维向量加一个1，变成四维。

// 齐次变换矩阵示例
Eigen::Matrix4d T = Eigen::Matrix4d::Identity();
T.block<3,3>(0,0) = rotationMatrix; // 旋转部分
T.block<3,1>(0,3) = translation;    // 平移部分

// 将机器人坐标系下的点转换到世界坐标系
Eigen::Vector4d p_robot(x, y, z, 1);
Eigen::Vector4d p_world = T * p_robot;

我个人建议：把变换矩阵写成4x4的形式，这样代码可读性高，而且不容易漏掉平移部分。

2.3 欧拉角：直观但容易踩坑

欧拉角用三个角度来描述旋转：偏航（Yaw）、俯仰（Pitch）、滚转（Roll）。很直观，对吧？

但我要说：欧拉角有坑。

2.3.1 万向锁问题

当俯仰角达到±90度时，偏航和滚转就分不清了。这叫万向锁。我在做无人机路径规划时就遇到过这个问题——无人机垂直俯冲时，姿态解算突然乱跳，就是因为万向锁。

2.3.2 欧拉角的顺序问题

不同的旋转顺序（比如ZYX、XYZ）会得到不同的结果。你想想看，先绕X轴转90度，再绕Y轴转90度；和先绕Y轴转90度，再绕X轴转90度，结果一样吗？

不一样。所以一定要明确你的旋转顺序。我习惯用ZYX顺序，也就是先偏航，再俯仰，最后滚转。

2.4 四元数：旋转的正确打开方式

四元数是什么？你可以把它理解成「带约束的复数」。它用四个数来表示旋转：一个实部，三个虚部。

为什么用四元数？因为它没有万向锁，而且插值平滑。说白了，就是比欧拉角靠谱。

2.4.1 四元数的基本形式

// 四元数表示绕单位轴 (x,y,z) 旋转角度 theta
double theta = M_PI / 2; // 90度
double x = 0, y = 0, z = 1; // 绕Z轴旋转
Eigen::Quaterniond q(cos(theta/2), x*sin(theta/2), y*sin(theta/2), z*sin(theta/2));
// q 的四个分量：w, x, y, z

2.4.2 四元数的乘法

两个四元数相乘，相当于两个旋转叠加。注意：顺序不能交换。这和矩阵乘法一样，不满足交换律。

2.4.3 四元数与欧拉角的转换

转换方向	方法	注意事项
欧拉角 → 四元数	按旋转顺序依次构造四元数，然后相乘	注意旋转顺序，我常用 ZYX
四元数 → 欧拉角	从四元数提取角度，注意奇点处理	当 w 接近 0 时，要特殊处理

// 四元数转欧拉角（ZYX顺序）
Eigen::Vector3d euler = q.toRotationMatrix().eulerAngles(2, 1, 0);
// 返回：yaw, pitch, roll

2.5 本章知识体系

下面这张图，是我自己总结的本章知识结构。你可以把它当成一张「地图」，随时回来查阅。

好了，这一章的内容就这些。向量、坐标系变换、欧拉角、四元数，这四个东西是路径规划的「四块基石」。你把它搞透了，后面做动态避障就会顺手很多。