数学基础（三）：插值与拟合——让离散点“说话”
做轨迹规划，你手里拿到的往往是一堆离散点。要么是传感器采的路径点，要么是运动学解出来的关节位置。但机器人要走的是一条连续、平滑的曲线，不是跳来跳去的散点。怎么从这些点里“猜”出整条路？这就是插值与拟合要干的事。
我个人习惯把插值和拟合分得很清：插值是“必须穿过每个点”，适合高精度路径；拟合是“整体趋势对就行”，适合有噪声的数据。你想想看，如果传感器带噪声你还硬要它穿过每个点，那曲线反而会扭得很难看。
多项式插值：最朴素的想法
给你 n+1 个点，找一个 n 次多项式穿过它们。这就是多项式插值的核心思路。拉格朗日和牛顿两种方法，本质上算的是同一个多项式，只是构造方式不同。
拉格朗日插值
公式长这样：
L(x) = Σ yᵢ · lᵢ(x)
其中 lᵢ(x) = Π (x - xⱼ) / (xᵢ - xⱼ) (j ≠ i)说白了，每个点都有一个“权重函数” lᵢ(x)，在 xᵢ 处等于 1，在其他点处等于 0。把所有点加权求和，就得到了插值曲线。
牛顿插值
牛顿用了一个技巧：差商。它把多项式写成嵌套形式：
N(x) = f[x₀] + f[x₀,x₁](x-x₀) + f[x₀,x₁,x₂](x-x₀)(x-x₁) + ...好处是，新增一个点只需要在末尾加一项，前面的系数不用动。我在做实时轨迹更新时特别喜欢用牛顿插值——传感器每来一个新点，我就在多项式后面“挂”一项，计算量只增加 O(n)。
| 对比项 | 拉格朗日 | 牛顿 |
|---|---|---|
| 新增点代价 | 全部重算 O(n²) | 追加一项 O(n) |
| 数值稳定性 | 高次时抖动明显 | 高次时抖动明显 |
| 适用场景 | 教学、点少且固定 | 在线增量插值 |
样条插值：分段低次，全局平滑
既然高次多项式会抖，那干脆分段插值，每段用低次多项式，再保证连接处平滑。这就是样条的思想。
三次样条
每两个相邻点之间用一个三次多项式，整条曲线满足：
- 穿过所有数据点
- 一阶导数连续（速度连续）
- 二阶导数连续（加速度连续）
这正好是轨迹规划最想要的性质——机器人运动时速度不能突变，加速度也不能突变，否则关节会“咯噔”一下。
// 三次样条核心：三弯矩方程
// 每个区间 [xᵢ, xᵢ₊₁] 上的多项式：
Sᵢ(x) = aᵢ + bᵢ(x-xᵢ) + cᵢ(x-xᵢ)² + dᵢ(x-xᵢ)³
// 系数由连续性条件和边界条件联立求解B样条
B样条比三次样条更“灵活”。它不要求曲线穿过所有控制点，而是“逼近”控制点。每个控制点只影响局部一段曲线，移动一个点不会牵动全局。
我记得有一次做移动机器人路径平滑，全局路径有 200 多个点。如果用三次样条，改一个点就要重新解整个方程组。换成 B 样条后，我只改了局部几个控制点，曲线就调整好了，效率提升很明显。
| 特性 | 三次样条 | B样条 |
|---|---|---|
| 过点性 | 必须穿过所有点 | 逼近控制点 |
| 局部性 | 全局耦合 | 局部控制 |
| 连续性 | C² 连续 | 可调（C⁰~C²） |
| 计算复杂度 | O(n) | O(n) |
最小二乘拟合：不纠结于过点
如果数据本身有噪声，硬要插值反而会“过拟合”。这时候就该最小二乘出场了。
它的目标很简单：找一条曲线，让所有数据点到曲线的误差平方和最小。公式：
min Σ (yᵢ - f(xᵢ))²
// 线性最小二乘（多项式基函数）：
f(x) = a₀ + a₁x + a₂x² + ... + aₘxᵐ
// 正规方程解：
a = (XᵀX)⁻¹ Xᵀ y最小二乘在轨迹规划里最常见的用途是：
- 路径平滑： 对带噪声的路径点做低次拟合，得到一条平滑的参考路径
- 参数辨识： 从实验数据中拟合出机器人动力学模型的系数
- 速度/加速度估计： 对位置数据做局部多项式拟合，再求导得到速度和加速度
嗯，数学基础这块就讲到这里。这些方法在后面的轨迹规划章节里会反复用到，尤其是三次样条和 B 样条，几乎每章都会出现。建议你动手写代码跑一跑，把拉格朗日、牛顿、三次样条、最小二乘都实现一遍，感受一下不同方法在相同数据上的表现差异——相信我，亲手跑出来的结果比看十遍公式都管用。