第四节：B样条曲线与路径平滑
做AGV运动控制这些年，我越来越觉得路径平滑是个被低估的活。很多人觉得能走就行，结果车一拐弯就抖，一加速就偏。说白了，路径不光要“对”，还得“顺”。B样条曲线，就是解决这个问题的利器。
4.1 为什么是B样条？
先说说我踩过的坑。最早做路径规划时，我用的是多项式插值。结果呢？控制点一多，曲线就开始乱抖——这就是所谓的“龙格现象”。后来换Bezier曲线，好一些，但改一个控制点整条曲线都变，调参调到崩溃。
B样条不一样。它最大的特点是局部性——你动一个控制点，只影响附近一小段曲线。这在AGV路径调整时太实用了。比如你发现某个拐弯太急，只需要微调那附近的几个点，其他路段纹丝不动。
核心优势总结：
- 局部可控：修改一个点不影响全局
- 连续性好：天然满足C²连续（加速度连续）
- 计算稳定：数值上比多项式插值靠谱得多
4.2 B样条的基本原理
B样条的数学定义其实不复杂。给定n+1个控制点P₀, P₁, ..., Pₙ，以及一个节点向量U = {u₀, u₁, ..., uₘ}，k阶B样条曲线定义为：
C(u) = Σᵢ₌₀ⁿ Nᵢₖ(u) · Pᵢ其中Nᵢₖ(u)是基函数，通过Cox-de Boor递推公式计算：
Nᵢ₀(u) = 1 if uᵢ ≤ u < uᵢ₊₁, else 0
Nᵢₖ(u) = (u - uᵢ)/(uᵢ₊ₖ - uᵢ) · Nᵢₖ₋₁(u)
+ (uᵢ₊ₖ₊₁ - u)/(uᵢ₊ₖ₊₁ - uᵢ₊₁) · Nᵢ₊₁ₖ₋₁(u)嗯，看着有点吓人。但实际用起来，你只需要记住几个关键点：
- 阶数k：一般用3阶（三次B样条），保证C²连续
- 节点向量：决定了曲线在控制点间的“过渡方式”
- 控制点：你真正要调的东西
我的经验：刚开始别纠结数学推导。先跑通一个三次均匀B样条的例子，感受一下“调控制点→曲线变化”的直观效果。数学是后补的。
4.3 控制点选取策略
控制点怎么选？这问题我当年琢磨了很久。后来总结出一套实用方法：
4.3.1 从路径点反算控制点
大多数时候，我们已经有了一串路径点（比如A*规划出来的）。需要反算出B样条的控制点，让曲线通过这些点。
假设有m个路径点Q₀, Q₁, ..., Qₘ₋₁，想用三次B样条插值。需要解一个线性方程组：
对于每个路径点Qⱼ：
Qⱼ = (1/6)·Pᵢ₋₁ + (2/3)·Pᵢ + (1/6)·Pᵢ₊₁写成矩阵形式：
[Q₀] [2/3 1/6 0 ... 0 ] [P₀]
[Q₁] = [1/6 2/3 1/6 ... 0 ] [P₁]
[Q₂] [ 0 1/6 2/3 ... 0 ] [P₂]
... ... ...
[Qₘ₋₁] [ 0 0 0 ... 2/3 ] [Pₘ₋₁]解这个三对角矩阵，就能得到控制点。我在项目里直接用Eigen库的稀疏矩阵求解，几毫秒就搞定。
注意：路径点数量不能太少。少于4个点，三次B样条插值会出问题。我建议至少6-8个点起步。
4.3.2 控制点的手动微调
自动反算出来的控制点，往往还需要手动调一调。我一般遵循几个原则：
- 拐弯处加密：曲率大的地方多放几个控制点
- 直道处稀疏：直线段两个点就够了
- 端点处理：起点和终点用重复节点，让曲线严格经过
举个例子。有一次做仓储AGV，货架间的通道很窄。自动生成的路径在拐弯处离货架太近。我手动把拐弯处的控制点往通道中心拉了拉，曲线就顺过去了。前后花了不到10分钟。
4.4 路径插值与平滑
有了控制点，接下来就是生成平滑路径。说白了，就是在控制点之间“插值”出足够密的点，让AGV能跟着走。
4.4.1 均匀采样 vs 弧长参数化
最简单的做法是均匀采样u参数。但问题来了——曲线上的点分布不均匀。u变化均匀，不代表弧长变化均匀。结果就是：AGV在直道上跑得快，拐弯处突然减速，不平滑。
我建议用弧长参数化。大致思路：
- 先均匀采样一批点，计算累计弧长
- 建立u到弧长的映射关系
- 按目标弧长间隔反查u值
// 伪代码示例
function resampleByArcLength(controlPoints, numSamples):
// 1. 粗采样
rawPoints = sampleUniform(controlPoints, numSamples * 10)
// 2. 计算累计弧长
arcLengths = [0]
for i = 1 to len(rawPoints)-1:
dist = distance(rawPoints[i], rawPoints[i-1])
arcLengths.append(arcLengths[-1] + dist)
// 3. 按目标弧长插值
totalLength = arcLengths[-1]
step = totalLength / (numSamples - 1)
result = []
for i = 0 to numSamples-1:
targetLength = i * step
// 二分查找对应区间，线性插值
idx = binarySearch(arcLengths, targetLength)
t = (targetLength - arcLengths[idx]) / (arcLengths[idx+1] - arcLengths[idx])
point = lerp(rawPoints[idx], rawPoints[idx+1], t)
result.append(point)
return result避坑指南：我曾经直接用均匀采样给AGV下路径，结果车在弯道里一卡一卡的。换成弧长参数化后，问题立刻解决。这个坑我替你们踩过了。
4.4.2 曲率约束检查
路径平滑不光要“看着顺”，还得“跑得顺”。AGV有最小转弯半径限制。所以插值完后，一定要检查曲率：
// 计算三点确定的曲率
function computeCurvature(p0, p1, p2):
// 向量
v1 = p1 - p0
v2 = p2 - p1
// 叉积的模 / 向量模的乘积
cross = v1.x * v2.y - v1.y * v2.x
len1 = length(v1)
len2 = length(v2)
if len1 * len2 < 1e-6:
return 0 // 近似直线
curvature = 2 * abs(cross) / (len1 * len2 * (len1 + len2))
return curvature如果某段曲率超过AGV的极限，就需要调整控制点。我一般设一个安全系数，比如极限曲率的80%作为报警阈值。
4.5 知识体系总览
下面这张图是我自己整理的B样条路径平滑流程，你可以照着这个思路搭系统：
4.6 实战中的几个要点
最后分享几个我在项目里总结的要点：
- 节点向量的选择：均匀节点向量最简单，但非均匀节点向量在拐弯处效果更好。我一般用弦长参数化法生成节点向量。
- 实时性考虑：如果路径需要在线更新（比如动态避障），控制点数量控制在20个以内，计算量可以接受。
- 与轨迹规划的衔接：B样条输出的是几何路径，还需要加上速度规划（梯形/S形速度曲线）才能变成可执行的轨迹。
一句话总结：B样条路径平滑，核心就三步——反算控制点、手动微调、弧长参数化插值。每一步都有坑，但踩过去之后，你会发现AGV跑起来顺滑得像在冰面上滑行。
公众号:蓝海资料掘金营，微信deep3321