第1章：基础数学预备知识
各位同学，欢迎来到《路径跟踪核心算法与工程落地实战》的第一课。
做机器人控制，说白了就是跟数学打交道。我见过不少同学，一上来就啃PID、MPC，结果算到一半卡在坐标变换上，代码跑出来机器人原地打转。嗯，这种坑我自己也踩过。
所以这一章，咱们先把地基打牢。坐标系、向量、旋转矩阵、四元数——这些概念你搞明白了，后面写代码才有底气。
1.1 坐标系与坐标变换
先问个问题：你告诉机器人“往前走1米”，它怎么知道往哪走？
答案就是坐标系。没有坐标系，位置和方向都是废话。
1.1.1 世界坐标系与机器人坐标系
世界坐标系，你可以理解为整个场景的“绝对参考系”。比如我在地面上画个原点，X轴朝东，Y轴朝北，Z轴朝天。所有障碍物、目标点，都用这个坐标系来描述。
机器人坐标系呢？它跟着机器人走。原点在机器人中心，X轴朝前，Y轴朝左，Z轴朝天。机器人自己感知到的障碍物，都是在这个坐标系下的。
我在项目中遇到过一个问题：激光雷达扫到的障碍物坐标，直接拿来用，结果机器人撞墙了。为什么？因为雷达装在车头，坐标是相对于雷达的，不是相对于机器人中心的。你得先做一次坐标变换。
1.1.2 坐标变换的数学本质
说白了，坐标变换就是“平移+旋转”。
假设机器人坐标系原点在世界坐标系下的位置是 t，机器人坐标系相对于世界坐标系的旋转是 R。那么，一个在机器人坐标系下的点 p_robot，在世界坐标系下的坐标就是：
p_world = R * p_robot + t反过来，从世界坐标到机器人坐标：
p_robot = R^(-1) * (p_world - t)注意，旋转矩阵的逆就是它的转置，这个性质后面会经常用到。
1.2 向量与矩阵基础
向量和矩阵，是机器人学的“砖瓦”。你后面要学的雅可比矩阵、协方差矩阵，都从这里来。
1.2.1 向量的基本运算
向量就是一堆数字排成一列。比如位置向量 [x, y, z]^T，速度向量 [vx, vy, vz]^T。
- 加法： 对应分量相加。比如位置+位移=新位置。
- 点积： 结果是一个数。几何意义是投影长度。我常用它来判断两个方向是否一致。
- 叉积： 结果是一个向量。垂直于原来的两个向量。在机器人里，叉积常用来计算力矩。
举个例子，你要判断机器人前方有没有障碍物。把机器人的朝向向量和障碍物方向向量做点积，如果结果接近1，说明障碍物就在正前方。
1.2.2 矩阵与线性变换
矩阵可以看作一种“映射”。一个向量乘上一个矩阵，就变成了另一个向量。
旋转矩阵就是典型的例子。它把一个向量“转”到另一个方向，长度不变。
我记得刚学的时候，总觉得矩阵乘法很抽象。后来我把它理解为“对向量施加一系列操作”。比如：
v' = A * v意思就是：向量 v 经过矩阵 A 的变换，变成了 v'。
| 运算 | 数学表示 | 几何意义 |
|---|---|---|
| 旋转 | v' = R * v | 绕某个轴旋转 |
| 缩放 | v' = S * v | 沿坐标轴拉伸 |
| 平移 | v' = v + t | 移动位置 |
1.3 刚体运动与旋转矩阵
刚体运动，就是物体在空间里“又转又移”，但形状不变。机器人就是典型的刚体。
1.3.1 旋转矩阵的性质
旋转矩阵 R 有三个重要性质：
- 它是正交矩阵：R^T * R = I
- 行列式为 +1
- 每一列都是单位向量，且互相垂直
这些性质保证了旋转不会改变向量的长度。你想想看，如果旋转一下，物体变大了，那还叫旋转吗？
1.3.2 绕坐标轴的旋转
绕 X、Y、Z 轴的旋转矩阵，是基础中的基础。我建议你手推一遍，不要死记硬背。
绕Z轴旋转θ角：
R_z(θ) = [cosθ -sinθ 0]
[sinθ cosθ 0]
[0 0 1]绕X轴和Y轴的类似，只是把1和三角函数放在不同位置。
1.4 四元数与欧拉角
说到旋转表示法，就不得不提欧拉角和四元数。这两个东西，是很多初学者的噩梦。我当年也绕晕过。
1.4.1 欧拉角
欧拉角用三个角度表示旋转：偏航（Yaw）、俯仰（Pitch）、滚转（Roll）。
- 偏航： 绕Z轴转，就像你左右摇头。
- 俯仰： 绕Y轴转，就像你点头。
- 滚转： 绕X轴转，就像你歪头。
欧拉角很直观，但有个致命问题——万向锁。当俯仰角达到±90度时，偏航和滚转就分不清了。我在做无人机仿真时遇到过这个问题，姿态突然“卡住”，怎么调都调不回来。
1.4.2 四元数
四元数可以理解为“带约束的复数”。它用四个数表示旋转：一个实部，三个虚部。
q = w + xi + yj + zk其中 w 是实部，x、y、z 是虚部。约束条件是 w^2 + x^2 + y^2 + z^2 = 1。
四元数的好处是：没有万向锁，插值平滑，计算效率高。坏处是：不直观。你很难一眼看出 q = [0.707, 0, 0.707, 0] 表示绕Y轴转90度。
1.4.3 如何选择？
我个人习惯是：
- 人机交互： 用欧拉角。直观，好理解。
- 内部计算： 用四元数。稳定，无奇点。
- 存储和传输： 用旋转矩阵。方便，直接可用。
知识体系总览
下面这张图，是我自己梳理的本章知识结构。你可以把它当作一个“地图”，随时回来对照。
这张图把本章内容串起来了。你从左上角开始看：坐标系是基础，向量和矩阵是工具，旋转表示法是具体实现。三者结合，才能完成坐标变换。
好了，这一章就到这里。数学基础打牢了，后面讲PID、MPC、LQR的时候，你才不会觉得“天书”。
记住：坐标系是地图，向量是路标，矩阵是方向盘。三者缺一不可。