数学基础回顾（上）：向量与矩阵运算、坐标变换与齐次坐标、旋转矩阵与四元数

各位同学，欢迎来到《轨迹优化》的第二讲。

说实话，做轨迹规划这么多年，我最大的感触就是：数学基础不牢，后面全是坑。你想想看，无人机要飞得稳、机器人要抓得准，背后全是向量、矩阵、坐标变换这些东西在撑着。今天这一讲，咱们就把这些基本功彻底捋一遍。

2.1 向量与矩阵运算：轨迹优化的“加减乘除”

先聊向量。我个人习惯把向量理解成“带方向的一段距离”。在三维空间里，一个位置点、一个速度、一个力，都可以用向量表示。

向量的点积与叉积，这两个东西在轨迹规划里太常用了。

• 点积：a · b = |a||b|cosθ。说白了，就是衡量两个向量“有多像”。我在做路径平滑时，经常用点积判断两个路径段的方向是否一致。
• 叉积：a × b 得到一个新向量，垂直于 a 和 b 所在的平面。它的模长等于 |a||b|sinθ。嗯，这里要注意：叉积的方向遵循右手定则。我曾经在写避障算法时，因为叉积方向搞反了，无人机直接往墙上撞……

矩阵运算，说白了就是“批量处理向量”。

• 矩阵乘法：C = A × B。要求 A 的列数等于 B 的行数。我建议你把这个记牢，后面坐标变换全靠它。
• 转置：把矩阵的行和列互换。在优化问题里，转置经常用来调整数据维度。
• 逆矩阵：A⁻¹。只有方阵且行列式不为零时才有逆。逆矩阵在解线性方程组时特别有用。

2.2 坐标变换与齐次坐标：让机器人知道“我在哪”

做轨迹规划，最核心的问题之一就是：不同坐标系下的数据怎么统一？

举个例子。你有一个机械臂，它的末端执行器上装了一个摄像头。摄像头看到的物体位置，是在“摄像头坐标系”下的。但你要控制机械臂去抓它，就得把这个位置转换到“机械臂基座坐标系”下。这就是坐标变换。

齐次坐标，说白了就是给三维向量加一个维度，变成四维向量。为什么这么做？因为这样就能用矩阵乘法统一处理旋转和平移了。

一个三维点 (x, y, z) 的齐次坐标是 (x, y, z, 1)。一个方向向量的齐次坐标是 (x, y, z, 0)。区别在哪？点可以平移，方向不能平移。这个细节，很多初学者会忽略。

齐次变换矩阵长这样：

| R   t |
| 0   1 |

其中 R 是 3×3 的旋转矩阵，t 是 3×1 的平移向量。这个矩阵可以把一个点从一个坐标系变换到另一个坐标系。

2.3 旋转矩阵与四元数：旋转的两种“语言”

旋转矩阵和四元数，是描述旋转的两种主流方式。我两个都用过，各有优劣。

旋转矩阵：

• 优点：直观，容易理解。绕 X、Y、Z 轴的旋转矩阵有标准公式。
• 缺点：有 9 个参数，但自由度只有 3 个。存在冗余，而且容易产生“万向锁”问题。

绕 Z 轴旋转 θ 角的矩阵：

| cosθ  -sinθ  0 |
| sinθ   cosθ  0 |
| 0      0     1 |

四元数：

• 优点：只有 4 个参数，没有万向锁，插值平滑（球面线性插值 SLERP 特别好用）。
• 缺点：不够直观。你很难一眼看出一个四元数代表什么旋转。

四元数表示为 q = w + xi + yj + zk，其中 w 是实部，(x, y, z) 是虚部。单位四元数（模长为 1）才表示纯旋转。

旋转矩阵和四元数的转换：

• 旋转矩阵 → 四元数：通过矩阵的迹和元素值可以反算出四元数。注意数值稳定性，当迹接近 -1 时要特殊处理。
• 四元数 → 旋转矩阵：有标准公式，直接代入计算即可。

我个人建议：在轨迹规划中，用四元数做插值和优化，用旋转矩阵做坐标变换。两者结合，效率最高。

2.4 本章知识体系

下面这张图，是我自己梳理的本章核心逻辑。你把它看懂了，后面学轨迹优化会轻松很多。

这张图把今天的内容串起来了。你看，从最基础的向量运算开始，到矩阵运算，再到坐标变换和旋转表示，每一步都是为后面的轨迹优化做铺垫。

好了，今天就到这儿。把这些基础打牢，后面咱们才能放开手脚去搞优化。

---