第二章：数学基础回顾——向量与坐标系，导数与微分，曲线参数化表示

各位同学，欢迎来到《轨迹平滑与曲率约束精讲》的第二讲。

说实话，很多做路径规划的朋友，最后都卡在了数学上。不是算法多难，而是基础概念没吃透。我见过不少项目，代码写了一大堆，结果轨迹一跑就飞，回头一看，坐标系都搞反了。嗯，今天我们就来把这些地基夯实。

做轨迹规划，第一件事就是搞清楚：你的点在哪个坐标系下说话。

我个人习惯把坐标系分成两类：

向量呢？说白了就是带方向的箭头。从A点到B点，就是一个向量。在代码里，我们通常用一个三元组 (x, y, z) 或者 (x, y) 来表示。

核心概念： 向量只有方向和大小，没有位置。你把它放在哪里，它都是那个向量。

我在项目中遇到过一个问题：两个传感器数据融合，一个用的是毫米波雷达（极坐标系），一个用的是激光雷达（笛卡尔坐标系）。如果不做坐标系转换，直接算距离，结果就是灾难。你想想看，角度和长度混在一起算，能对吗？

常见的坐标系转换公式（二维）：

// 极坐标 -> 笛卡尔坐标
x = r * cos(θ)
y = r * sin(θ)

// 笛卡尔坐标 -> 极坐标
r = sqrt(x² + y²)
θ = atan2(y, x)

小技巧： 实际工程中，我建议所有中间计算都统一到笛卡尔坐标系下。极坐标只在传感器原始数据读取时使用，避免来回转换带来的精度损失。

导数，说白了就是变化率。你开车时，速度是位置对时间的导数，加速度是速度对时间的导数。

在轨迹规划里，我们关心的是：

为什么会这样？因为机器人或者车辆是有物理限制的。你不能让它瞬间转个90度的弯，那会翻车。所以我们要用导数来约束轨迹的平滑性。

微分呢？微分的核心思想是“以直代曲”。在一个足够小的区间内，曲线可以近似为直线。这个思想在后面的曲率计算中会反复用到。

注意： 数值微分在实际代码中很容易引入噪声。我曾经因为直接用差分法算导数，导致轨迹抖动得跟筛子一样。后来改用Savitzky-Golay滤波，才把问题压下去。

数值微分的基本公式（前向差分）：

dx/dt ≈ (x(t+Δt) - x(t)) / Δt

这个公式简单，但噪声敏感。如果数据有毛刺，导数会被放大。建议配合平滑滤波使用。

一条曲线怎么表示？最常见的有两种：

我个人强烈推荐参数化表示。为什么？因为轨迹规划里，我们不仅要关心位置，还要关心速度、加速度，甚至加加速度。参数化表示天然地把时间维度带进来了。

举个例子，一个匀速圆周运动：

x(t) = R * cos(ωt)
y(t) = R * sin(ωt)

这里t是时间，R是半径，ω是角速度。你看，位置、速度、加速度都可以通过对t求导得到。

还有一种常用的参数化方式——弧长参数化。用s表示从起点沿曲线走过的距离。这样做的好处是，参数s与速度无关，纯粹描述曲线的几何形状。

关键点： 弧长参数化下的导数，直接对应曲线的几何属性。比如dx/ds就是切向量，d²x/ds²与曲率直接相关。

我在做AGV路径跟踪时，就吃过亏。一开始用时间参数化，结果车速一变化，轨迹点就乱跳。后来换成弧长参数化，跟踪稳定多了。

下面这张图，是我自己总结的本章知识结构。你可以把它当作一张地图，随时回来查阅。

最后，分享几个我踩过的坑：

我的习惯： 每次拿到新的轨迹数据，第一件事就是画出来看看。肉眼扫一遍，坐标系对不对、曲线有没有毛刺，一目了然。别急着调算法，先看数据。

好了，数学基础就聊到这里。这些东西看着简单，但真到工程里，每一个细节都可能让你加班到深夜。希望今天的梳理能帮你少走弯路。