基础数学工具：向量、矩阵、坐标系变换与姿态表示
做轨迹规划这些年，我最大的感触就是——数学基础不牢，后面全是坑。今天咱们就把这些基础工具捋一遍。别怕，我会用最接地气的方式讲。
向量：轨迹规划的最小单元
向量是什么？说白了就是带方向和大小的箭头。在轨迹规划里，每个位置点、每个速度方向，本质上都是向量。
向量的基本运算
- 加法：两个向量首尾相连，结果就是合向量。比如机器人同时有前进速度和侧移速度，合起来就是实际运动方向。
- 点积：
a·b = |a||b|cosθ。我常用它来判断两个方向是否一致——点积为正说明同向，为负说明反向。 - 叉积：
a×b得到垂直于a和b的向量。在3D轨迹里，叉积用来计算法向量，比如判断障碍物在路径的哪一侧。
避坑指南：我曾经在项目中直接用向量减法算相对位置，结果忘了考虑坐标系原点不同，算出来的方向全是反的。记住：向量运算一定要在同一坐标系下进行。
矩阵：坐标变换的瑞士军刀
矩阵这东西，刚学时觉得抽象，用多了就知道它有多香。在轨迹规划里，矩阵主要干两件事：旋转和平移。
2D旋转矩阵
R(θ) = [cosθ -sinθ]
[sinθ cosθ]这个矩阵能把一个点绕原点旋转θ角度。我习惯把它记成「cos在左上，sin在左下，负sin在右上」。
3D旋转矩阵就复杂一些，绕X、Y、Z轴各有各的矩阵。不过别慌，实际项目中我们很少手写这些矩阵，都是用现成库。但理解原理很重要——你想想看，如果连旋转顺序都搞不清，后面算欧拉角时肯定翻车。
我的小技巧：调试矩阵变换时，先用单位向量(1,0,0)和(0,1,0)测试一下，看旋转后对不对。这招帮我省了不少排查时间。
坐标系变换：从世界到局部
轨迹规划里最绕的就是坐标系变换。一个机器人身上可能有十几个坐标系：世界坐标系、车体坐标系、传感器坐标系、工具坐标系...
齐次坐标是解决这个问题的利器。把3D点变成4D向量(x,y,z,1)，旋转和平移就能统一用4×4矩阵表示了：
T = [R t]
[0 1]其中R是3×3旋转矩阵，t是3×1平移向量。这个T矩阵能把一个点从局部坐标系变换到世界坐标系。
举个例子：激光雷达检测到障碍物在(1,0,0)处（雷达坐标系），但雷达装在机器人前方0.5米处，机器人又在世界坐标(10,5)处。这一串变换，用齐次矩阵连乘就能搞定。
注意：矩阵乘法不满足交换律！先旋转再平移，和先平移再旋转，结果完全不同。我见过有人在这上面栽跟头，调试了一整天。
欧拉角与四元数：姿态表示的两种哲学
说到姿态表示，就不得不提欧拉角和四元数。这两个东西，我刚开始学的时候也是一头雾水。
欧拉角：用三个角度表示旋转，比如偏航角(yaw)、俯仰角(pitch)、滚转角(roll)。直观好理解，但有个致命问题——万向锁。当俯仰角达到±90°时，偏航和滚转就分不清了。
四元数：用四个数(q0,q1,q2,q3)表示旋转，没有万向锁问题，插值也平滑。但说实话，它不够直观，你很难直接看出它代表什么姿态。
| 特性 | 欧拉角 | 四元数 |
|---|---|---|
| 直观性 | ★★★★★ | ★★ |
| 无万向锁 | ✗ | ✓ |
| 插值平滑 | ★★ | ★★★★★ |
| 计算效率 | ★★★ | ★★★★ |
我的建议：人机交互时用欧拉角（直观），内部计算时用四元数（稳定）。我在做无人机轨迹规划时，就是先用欧拉角设定目标姿态，然后转成四元数做插值和避障计算。
知识体系总览
下面这张图，是我梳理的本章知识结构。你看一遍，心里就有谱了。
实战中的避坑经验
最后分享几个我踩过的坑：
- 坐标系方向搞反：有一次我把Z轴向上记成了向下，结果规划的轨迹全在地底下。后来我养成了习惯，每次定义坐标系都在代码里写注释说明方向。
- 角度单位混淆：弧度制和角度制混用，是新手最容易犯的错。我建议统一用弧度，只在显示给用户看时转成角度。
- 四元数归一化：四元数如果不归一化，旋转结果会变形。我每次构造完四元数都会做一次归一化，这已经成了肌肉记忆。
一句话总结：向量是基础，矩阵是工具，坐标系是框架，欧拉角和四元数是表达方式。把这四样吃透了，轨迹规划的数学地基就算打牢了。
可可精品资料库