3. 数学基础（下）：样条曲线与优化理论

好，咱们接着聊数学基础。上一章讲了线性代数和微积分，那些是工具。这一章要讲的，是工具怎么用在轨迹规划上。

说白了，机器人要动，不能直愣愣地走直线。你得让它的路径平滑、连续、不抖。这就离不开样条曲线。另外，规划出来的轨迹好不好，得有个评判标准，然后去优化它。这就引出了优化理论。

我个人觉得，这一章是连接数学和实际算法的桥梁。你把它啃透了，后面看各种规划算法会轻松很多。

3.1 样条曲线：让轨迹变丝滑

先问个问题：为什么不用多项式直接拟合？

可以，但高次多项式容易震荡。你给十几个点，用一个高次多项式去穿过它们，曲线两端可能会甩到天上去。这就是所谓的“龙格现象”。我在做机械臂轨迹规划时吃过这个亏，末端执行器在路径中间抖得厉害，后来换成样条曲线才解决。

样条曲线的核心思想是“分段低次”。把路径切成一小段一小段，每段用一个低次多项式去拟合，段与段之间保证连续。这样既平滑，又稳定。

3.1.1 贝塞尔曲线

贝塞尔曲线是最基础的。它由一组控制点定义。曲线不穿过所有控制点，只穿过首尾两个点，中间的点用来“拉”曲线。

公式长这样（以三次贝塞尔为例）：

B(t) = (1-t)³P₀ + 3(1-t)²tP₁ + 3(1-t)t²P₂ + t³P₃,  t ∈ [0,1]

其中 P₀ 到 P₃ 是四个控制点。t 从 0 走到 1，曲线就从起点走到终点。

贝塞尔曲线有个特点：凸包性。整条曲线都被控制点围成的凸多边形包在里面。这意味着你调整控制点，曲线不会乱跑。我在做AGV路径微调时，就靠这个特性来保证调整后的路径不会撞到障碍物。

但它也有缺点：全局性。你动一个控制点，整条曲线都会变。这在局部调整时很麻烦。

3.1.2 B样条曲线

B样条曲线解决了贝塞尔的全局性问题。它引入了“节点向量”的概念，把曲线分成多个段。每个控制点只影响局部的一段曲线。

公式稍微复杂一点：

C(u) = Σᵢ Nᵢₚ(u) Pᵢ

其中 Nᵢₚ(u) 是B样条基函数，p 是次数，Pᵢ 是控制点。

基函数是通过递推定义的。嗯，这里不展开推导了，你记住几个关键点就行：

• 局部性：移动一个控制点，只影响附近几段曲线。这是它最大的优势。
• 连续性：p次B样条，在节点处有 Cᵖ⁻¹ 连续。三次B样条就是 C² 连续，速度和加速度都连续。
• 凸包性：和贝塞尔一样，曲线在控制点凸包内。

我在做自动驾驶路径规划时，B样条用得最多。因为道路是分段优化的，你调整某个路段的控制点，不会影响远处的路径。这在实际工程中太重要了。

3.2 多项式插值与拟合

样条曲线是分段拟合。那如果我就想用一个多项式穿过所有点呢？这就是插值。

插值要求曲线精确穿过每个数据点。拟合则允许有误差，只要整体趋势对就行。

举个例子。你给机器人标定，测了10个点的位置。如果这些点很精确，用插值。如果测量有噪声，用拟合。

常用的插值方法：

• 拉格朗日插值：公式直观，但计算量大，增加一个点要重算所有系数。
• 牛顿插值：可以递推，增加点方便。
• 三次样条插值：分段三次多项式，保证 C² 连续。这是工程中最常用的。

拟合的话，最小二乘法是基础。你想想看，给定一堆点，找一条直线或曲线，让所有点到曲线的距离平方和最小。这就是最小二乘。

// 最小二乘拟合直线
// y = ax + b
// 求解正规方程: (X^T X) β = X^T y

// 伪代码
Matrix X = [x₁ 1; x₂ 1; ...; xₙ 1];
Vector y = [y₁; y₂; ...; yₙ];
Vector β = (X^T * X)^(-1) * X^T * y;
// β[0] = a, β[1] = b

我曾经在一个项目中，用三次样条插值做路径平滑，结果发现路径在拐弯处有轻微过冲。后来换成B样条拟合，允许路径不精确穿过某些点，反而更平滑。嗯，这里要注意：不是所有点都必须经过的，有时候“放过”一些点，效果更好。

3.3 优化理论基础

轨迹规划的本质，是一个优化问题。你要在满足约束（比如速度限制、加速度限制、避障）的前提下，找到一条最优的轨迹。

优化问题分两类：凸优化和非凸优化。

3.3.1 凸优化

凸优化是优化里的“优等生”。它的目标函数是凸函数，约束集是凸集。凸优化有个好性质：局部最优就是全局最优。

为什么？因为凸函数只有一个“谷底”。你从任何起点出发，沿着梯度下降，最终都会走到同一个最低点。

常见的凸优化问题：

• 线性规划：目标函数和约束都是线性的。比如最短路径问题。
• 二次规划：目标函数是二次的，约束是线性的。比如最小加速度轨迹。
• 二阶锥规划：约束是二阶锥形式。比如考虑推力限制的轨迹规划。

我在做无人机轨迹规划时，经常把问题建模成二次规划。因为计算快，而且保证能找到最优解。你想想看，无人机在空中要实时调整路径，如果优化算法要算好几秒，那飞机早撞了。

3.3.2 非线性优化

现实中的问题，大多是非凸的。比如避障约束，障碍物是凹的，约束集就是非凸的。这时候凸优化就不适用了。

非线性优化没有“包治百病”的方法。常用的有：

• 梯度下降法：沿着负梯度方向走。简单，但容易陷入局部最优。
• 牛顿法：利用二阶导数（海森矩阵），收敛更快。但海森矩阵计算量大。
• 高斯-牛顿法：专门用于最小二乘问题。比如SLAM中的图优化。
• 序列二次规划：把非线性问题近似成一系列二次规划来求解。

说白了，非线性优化就是“摸着石头过河”。你没法保证找到全局最优，只能尽量找一个好的局部最优。

我的经验是：先尝试凸优化建模。如果实在不行，再用非线性优化。而且要给算法一个好的初始值。初始值越接近真实解，算法越容易收敛到好的结果。

3.4 本章知识体系

下面这张图，是我自己整理的。它把这一章的核心逻辑串起来了。

你看，这三块内容是环环相扣的。样条曲线提供了路径的数学表示，插值和拟合决定了路径的形状，优化理论则帮我们找到“最好”的那条路径。

我个人建议，学这一章时，不要只盯着公式看。拿笔在纸上画一画，或者用Python写个小程序，调几个控制点看看曲线怎么变。实践出真知。

好，这一章就到这里。数学基础打牢了，后面讲具体算法时，你会觉得顺风顺水。

---