数学基础（上）：向量与矩阵运算、坐标变换与齐次变换矩阵、旋转矩阵与欧拉角、四元数基础
各位同学，欢迎来到轨迹规划的第一站。说实话，很多初学者一上来就啃算法，结果被各种坐标系绕晕了。我当年刚入行时也踩过这个坑——明明代码逻辑没问题，机器人就是走不对位置。后来才发现，是数学基础没打牢。
这一章，咱们就把这些「地基」夯实。你想想看，轨迹规划说白了就是让机器人知道：我在哪、要去哪、怎么去。这三个问题，全得靠数学语言来描述。
核心观点：轨迹规划的本质，是在不同坐标系之间反复做「翻译」。向量、矩阵、齐次变换、旋转表示法，就是这套翻译系统的语法和词汇。
2.1 向量与矩阵运算——机器人的「语言」
先聊向量。我习惯把向量理解成「带箭头的一段位移」。在三维空间里，一个位置点可以用向量 [x, y, z]^T 表示。但注意，向量本身不绑定坐标系——它只是一个数学对象。
矩阵呢？说白了就是「批量处理向量的工具」。比如你要把空间里100个点同时旋转，用矩阵乘法一行代码搞定。我在做机械臂逆解时，经常用矩阵来组织多个关节的位姿数据。
这里有个关键运算，大家务必烂熟于心：
// 向量点积：判断两个方向是否一致
double dot = v1.x * v2.x + v1.y * v2.y + v1.z * v2.z;
// 向量叉积：求垂直于两个向量的新方向
Vector3 cross = new Vector3(
v1.y * v2.z - v1.z * v2.y,
v1.z * v2.x - v1.x * v2.z,
v1.x * v2.y - v1.y * v2.x
);
// 矩阵乘法：组合变换
Matrix4x4 result = matA * matB; // 注意顺序！先B后A我的习惯：写代码时，我会把向量运算封装成静态工具类。比如 VectorUtils.Dot()、MatrixUtils.Multiply()。这样主逻辑里看起来清爽，调试时也容易定位问题。
2.2 坐标变换与齐次变换矩阵——从A系到B系
好，现在进入重头戏。为什么需要坐标变换？举个例子：你让机械臂去抓桌上的杯子。杯子的坐标是在「世界坐标系」下定义的，但机械臂末端执行器是在「工具坐标系」下运动的。你得把杯子的坐标换算到工具坐标系里，机械臂才知道怎么动。
齐次变换矩阵 T 就是干这个的。它长这样：
T = | R t |
| 0 1 |其中 R 是3x3旋转矩阵，t 是3x1平移向量。用4x4矩阵的好处是：旋转和平移可以写在一个矩阵里，一次乘法搞定。
我曾经在AGV导航项目里吃过亏——忘了齐次矩阵的乘法顺序。记住：T_AB * v_B = v_A，意思是「把B系下的向量变换到A系」。如果你写反了，机器人会往反方向跑。
避坑指南：我曾经调试一个视觉抓取程序，发现机械臂总是差5厘米。查了两天，最后发现是齐次矩阵的平移向量单位搞错了——毫米和米混用了。所以，务必在代码里统一单位，我习惯在矩阵类里加一个 UnitType 枚举。
2.3 旋转矩阵与欧拉角——绕来绕去的学问
旋转矩阵 R 是正交矩阵，行列式为+1。它描述了一个坐标系相对于另一个坐标系的朝向。但直接用9个参数表示旋转，太冗余了。所以工程上常用欧拉角。
欧拉角就是三个角度：绕X轴转（滚转Roll）、绕Y轴转（俯仰Pitch）、绕Z轴转（偏航Yaw）。但这里有个大坑——万向锁。当Pitch接近±90°时，Roll和Yaw会变得无法区分。我在做无人机姿态控制时遇到过这个问题，飞控突然乱转，吓得我赶紧切了手动模式。
所以，我的建议是：
- 人机交互时用欧拉角（直观，好理解）
- 内部计算时用旋转矩阵或四元数（避免万向锁）
欧拉角转旋转矩阵的公式（ZYX顺序）：
R = Rz(yaw) * Ry(pitch) * Rx(roll)
其中：
Rz(yaw) = | cos(yaw) -sin(yaw) 0 |
| sin(yaw) cos(yaw) 0 |
| 0 0 1 |
Ry(pitch) = | cos(pitch) 0 sin(pitch) |
| 0 1 0 |
| -sin(pitch) 0 cos(pitch) |
Rx(roll) = | 1 0 0 |
| 0 cos(roll) -sin(roll)|
| 0 sin(roll) cos(roll) |重要：旋转矩阵的乘法顺序不可交换。先绕X转再绕Y转，和先绕Y转再绕X转，结果完全不同。我习惯在代码注释里写明「当前使用的是ZYX顺序，外旋」。这样半年后回来看代码，自己还能看懂。
2.4 四元数基础——旋转的「瑞士军刀」
四元数，听起来玄乎，其实就是一个四维向量 [w, x, y, z]。它表示绕某个轴旋转某个角度。为什么用它？三个理由：
- 无万向锁——这是最大的优势
- 插值平滑——做轨迹规划时，用四元数球面插值（Slerp）能得到非常平滑的旋转过渡
- 计算高效——比旋转矩阵少用4个浮点数
四元数乘法对应旋转的复合：
// 四元数乘法（哈密顿积）
Quaternion q = q1 * q2; // 先应用q2，再应用q1
// 用四元数旋转向量
Vector3 v_rotated = q * v * q.conjugate();
// 球面线性插值
Quaternion q_slerp = Quaternion.Slerp(q_start, q_end, t); // t从0到1我记得第一次用四元数做机械臂轨迹规划时，发现末端执行器的姿态变化特别丝滑。相比之下，用欧拉角做插值，中间会突然「跳一下」。嗯，这就是四元数的魅力。
实用技巧：调试四元数时，我习惯把它转成「轴角」形式打印出来——axis = [x, y, z] / sin(theta/2)，angle = 2 * acos(w)。这样一眼就能看出旋转轴和角度，比看四个数字直观多了。
知识体系总览
下面这张图，把本章的核心逻辑串起来了。你可以把它当作一个「导航图」——学完每个知识点后，回来看看它在整个体系中的位置。
好了，这一章的内容就到这里。向量和矩阵是基本功，齐次变换是核心工具，旋转表示法各有优劣。我个人建议：先吃透齐次变换矩阵，再根据场景选旋转表示法。做轨迹规划时，大部分时间你都在跟4x4矩阵打交道。
下一章，咱们会继续深入数学基础，聊聊微分运动学、雅可比矩阵这些更「动态」的东西。到时候你会发现，今天学的这些静态变换，是理解一切运动的基础。