第二章 基础数学回顾：向量与坐标系、矩阵运算、欧拉角与四元数、贝塞尔曲线基础

各位同学，欢迎来到第二章。

说实话，数学基础这块，很多人觉得枯燥。但我做了这么多年路径规划，可以负责任地告诉你——这些数学工具就是你的吃饭家伙。你想想看，机器人要平滑地走一条曲线，背后全是向量、矩阵、四元数这些东西在支撑。

这一章，我会把最常用的数学知识串一遍。不搞复杂的推导，只讲你写代码时真正用得上的东西。

2.1 向量与坐标系

先聊向量。说白了，向量就是带方向和大小的箭头。在路径规划里，我们用它表示位置、速度、加速度，甚至是机器人的朝向。

2.1.1 向量的基本运算

我个人习惯把向量运算分成三类：加减、点积、叉积。

• 加减法：对应分量相加减。比如位置更新：new_pos = old_pos + velocity * dt。就这么简单。
• 点积：a · b = |a||b|cosθ。用来判断两个方向是否一致。我在项目中常用它来检测机器人是否朝着目标点前进——如果点积大于0，方向对了；小于0，该调整了。
• 叉积：a × b 得到垂直于a和b的向量。在路径规划里，叉积常用来计算转向方向。嗯，这里要注意：叉积的结果是向量，不是标量。

2.1.2 坐标系变换

路径规划里，我们经常要在不同坐标系之间来回切换。最常见的是：

• 世界坐标系：全局固定，比如地图原点
• 机器人坐标系：以机器人自身为原点，前向为x轴
• 传感器坐标系：激光雷达、摄像头都有自己的坐标系

你想想看，如果传感器检测到一个障碍物在它前方1米，但机器人本身已经旋转了30度，那这个障碍物在世界坐标系下的位置是多少？这就需要用旋转矩阵来算了。

2.2 矩阵运算

矩阵这东西，刚开始学的时候觉得抽象。但说白了，它就是一组数字排成方阵，用来做线性变换的工具。

2.2.1 旋转矩阵

二维旋转矩阵长这样：

R(θ) = [cosθ  -sinθ]
       [sinθ   cosθ]

三维的稍微复杂一点，但原理一样。绕x轴、y轴、z轴旋转，各有各的矩阵。我在做AGV路径跟踪时，经常用旋转矩阵把机器人坐标系下的速度转换到世界坐标系下。

2.2.2 矩阵乘法与逆矩阵

矩阵乘法不满足交换律，这个坑我踩过。A×B和B×A结果完全不同。在路径规划里，变换顺序特别重要——先旋转再平移，和先平移再旋转，结果天差地别。

逆矩阵用来做反向变换。比如你知道世界坐标下的点，想求它在机器人坐标系下的位置，就用旋转矩阵的逆矩阵去乘。

2.3 欧拉角与四元数

说到姿态表示，就绕不开欧拉角和四元数。我刚开始做机器人时，只用欧拉角，觉得直观。后来吃了大亏。

2.3.1 欧拉角

欧拉角用三个角度表示旋转：偏航（yaw）、俯仰（pitch）、横滚（roll）。直观是直观，但有个致命问题——万向锁。

为什么会这样？当俯仰角接近±90度时，偏航和横滚会耦合在一起，丢失一个自由度。我曾经在无人机项目里遇到过，飞机在做大角度机动时姿态解算突然炸了，查了半天才发现是万向锁的问题。

2.3.2 四元数

四元数就是为了解决万向锁而生的。它用四个数表示旋转：q = w + xi + yj + zk。其中w是实部，x、y、z是虚部。

四元数的好处：

• 没有万向锁问题
• 插值平滑（比如用Slerp做姿态平滑过渡）
• 计算效率高

我建议你在路径规划中尽量用四元数。虽然写代码时不如欧拉角直观，但稳定性和平滑度好太多了。

2.4 贝塞尔曲线基础

贝塞尔曲线是路径平滑的利器。说白了，就是用几个控制点来定义一条平滑曲线。

2.4.1 一阶与二阶贝塞尔

一阶贝塞尔就是两点之间的直线：B(t) = (1-t)P0 + tP1。没什么好说的。

二阶贝塞尔有三个控制点：

B(t) = (1-t)²P0 + 2(1-t)tP1 + t²P2

这个在路径规划里很常用。比如机器人要从A点到C点，中间要绕过一个障碍物，就可以在障碍物旁边放一个控制点P1，生成一条平滑的绕行路径。

2.4.2 三阶贝塞尔与连续性

三阶贝塞尔有四个控制点，曲线更灵活。我在做AGV路径平滑时，经常用多段三阶贝塞尔拼接成一条完整路径。

这里有个关键点——连续性：

• C0连续：两段曲线端点重合
• C1连续：端点处切线方向一致（速度连续）
• C2连续：端点处曲率一致（加速度连续）

你想想看，如果机器人走一条路径，速度突然跳变，那肯定不行。所以至少要做到C1连续。我一般要求C2连续，这样机器人走起来才真正平滑。

2.5 本章小结

这一章的内容，说白了就是路径规划的数学工具箱：

• 向量帮你描述位置和方向
• 矩阵帮你做坐标变换
• 四元数帮你稳定地表示姿态
• 贝塞尔曲线帮你生成平滑路径

这些工具在后面的章节里会反复用到。我建议你花点时间把代码写一遍，尤其是四元数和贝塞尔曲线的实现。纸上得来终觉浅，绝知此事要躬行。

下一章，我们开始讲轨迹平滑的具体算法。到时候你会发现，今天学的这些数学知识，全都会派上用场。