4、B样条曲线计算：De Boor算法、求值、求导、积分及曲率计算
B样条曲线是轨迹规划中应用最广泛的参数曲线形式之一，其核心优势在于局部支撑性、高阶连续性和数值稳定性。在机器人运动规划、自动驾驶路径生成以及工业数控加工中，B样条曲线的求值、求导、积分与曲率计算是后续碰撞检测、动力学可行性校验和轨迹平滑度评估的基础。本章将系统讲解B样条曲线的数学定义、De Boor递推算法、导数与积分的解析计算方法，以及曲率与挠率的工程求解路径，并结合实际轨迹规划场景给出完整的计算案例。
4.1 B样条曲线的数学定义与De Boor算法
B样条曲线由控制点、节点向量和阶次三个要素唯一确定。给定控制点序列 {P0, P1, ..., Pn}，节点向量 U = {u0, u1, ..., um}（其中 m = n + p + 1，p 为曲线阶次），p次B样条曲线的参数表达式为：
C(u) = Σi=0n Ni,p(u) · Pi
其中 Ni,p(u) 是B样条基函数，由Cox-de Boor递推公式定义。De Boor算法正是利用基函数的局部支撑性和递推特性，避免直接计算基函数而高效求取曲线上任意参数 u 对应的坐标值。该算法的核心思想是：对于给定的参数 u，找到其所在的节点区间 [uk, uk+1)，然后通过 p 层线性插值递推，最终得到曲线上的点。
De Boor算法递推公式（以3次B样条为例）：
输入：参数 u，控制点 P[0..n]，节点向量 U[0..m]，阶次 p=3
1. 定位 k：找到满足 U[k] ≤ u < U[k+1] 的节点下标 k
2. 初始化：令 Q[i] = P[i], i = k-p, ..., k
3. 递推：for r = 1 to p
for i = k-p+r to k
α = (u - U[i]) / (U[i+p-r+1] - U[i])
Q[i] = (1-α)·Q[i-1] + α·Q[i]
4. 输出：C(u) = Q[k]De Boor算法的计算复杂度为 O(p²)，与控制点数量无关，这使得它在实时轨迹规划中具有显著优势。实际工程中，3次B样条（p=3）最为常用，因为其C²连续性能满足大多数运动控制需求，且计算量适中。对于更高阶次（p≥4），虽然连续性更高，但数值稳定性下降，且局部支撑范围扩大，不利于局部调整。
| 阶次 p | 连续性 | 局部支撑范围 | 计算复杂度 | 典型应用 |
|---|---|---|---|---|
| 2（二次） | C¹ | 3个节点区间 | O(4) | 简单路径拟合 |
| 3（三次） | C² | 4个节点区间 | O(9) | 机器人轨迹、自动驾驶 |
| 5（五次） | C⁴ | 6个节点区间 | O(25) | 高速高精度加工 |
4.2 B样条曲线的导数计算
轨迹规划中，速度、加速度约束的校验直接依赖于曲线的一阶和二阶导数。B样条曲线的一个重要性质是：p次B样条曲线的导数仍然是B样条曲线，但阶次降为 p-1，且控制点由原控制点的差分构成。具体地，一阶导数的控制点 Qi 为：
Qi = p · (Pi+1 - Pi) / (Ui+p+1 - Ui+1)
对应的节点向量需要去掉首尾各一个节点。二阶导数可以在一阶导数的基础上再次应用该公式。这种解析求导方法比数值差分（如有限差分法）精度更高，且计算效率相当，是工程中的首选方案。
工程经验：当节点向量中存在重复节点（即节点区间长度为零）时，上述差分公式中的分母可能为零。此时需要特殊处理：若 Ui+p+1 = Ui+1，则令 Qi = 0。这在处理 clamped B样条（首尾节点重复 p+1 次）时尤为重要。
在实际轨迹规划中，我们通常需要同时计算位置、速度、加速度。一种高效的做法是：先通过De Boor算法求出位置 C(u)，再利用导数公式分别计算 C′(u) 和 C″(u)。由于导数计算只涉及控制点的线性组合，可以在预处理阶段完成导数控制点的计算，然后在每个采样点直接调用De Boor算法求值，从而避免重复计算。
4.3 B样条曲线的积分计算
B样条曲线的积分在轨迹规划中主要用于两类场景：一是计算曲线弧长，用于速度-时间参数化；二是计算能量泛函（如 jerk 能量、加速度能量），用于轨迹优化中的平滑项。对于p次B样条曲线，其积分没有简单的解析表达式，但可以通过以下方法处理：
- 弧长计算：采用数值积分方法（如Gauss-Legendre求积、自适应Simpson法），对 ||C′(u)|| 在参数区间上进行积分。由于B样条具有分段多项式性质，可以在每个节点区间内使用高阶Gauss求积，精度很高。
- 能量泛函积分：对于形如 ∫||C″(u)||²du 的积分，可以利用B样条的导数公式将 C″(u) 表示为低阶B样条，然后利用B样条基函数的积分性质（即基函数积分等于节点区间长度除以阶次）进行解析计算。
弧长计算示例（3次B样条，节点区间 [uk, uk+1]）：
在每个节点区间内使用5点Gauss-Legendre求积：
L_k = (Δu/2) · Σ_{j=1}^{5} w_j · ||C′(u_j)||
其中 u_j = (u_k + u_{k+1})/2 + (Δu/2)·x_j
x_j 和 w_j 为Gauss点及权重
总弧长 L = Σ L_k对于能量泛函的积分，更精确的做法是利用B样条基函数的积分公式。设 Ni,p(u) 为p次基函数，则 ∫Ni,p(u)du = (Ui+p+1 - Ui)/(p+1)。这一性质使得加速度能量积分可以转化为控制点二次型的计算，从而在优化问题中直接使用矩阵运算。
4.4 曲率与挠率的计算
曲率 κ 和挠率 τ 是描述曲线几何特性的核心指标。在轨迹规划中，曲率直接影响车辆转向角速度、机器人关节速度等运动学约束；挠率则用于三维空间轨迹的平滑性评估。对于参数曲线 C(u)，曲率和挠率的计算公式为：
κ = ||C′(u) × C″(u)|| / ||C′(u)||³
τ = (C′(u) × C″(u)) · C‴(u) / ||C′(u) × C″(u)||²
工程实现时，需要先计算一阶、二阶、三阶导数向量，然后进行叉积和点积运算。需要注意的是，当 ||C′(u)|| 接近零时（即曲线速度为零），曲率计算会出现数值不稳定。此时需要特殊处理，例如在轨迹规划中避免生成速度为零的路径点，或者采用正则化方法。
数值稳定性警告：在曲率计算中，如果 ||C′(u)|| 很小（例如小于 1e-6），直接计算会导致 κ 值溢出。建议在计算前检查速度模长，若低于阈值则采用局部拟合方法（如用相邻点拟合圆弧）来估计曲率。
4.5 综合案例：自动驾驶轨迹的曲率分析与平滑性评估
某自动驾驶项目需要评估一段规划轨迹的曲率分布，以判断是否满足车辆动力学约束。轨迹由7个控制点定义的三次B样条曲线表示，控制点坐标（单位：米）为：P0=(0,0)，P1=(10,5)，P2=(20,15)，P3=(30,10)，P4=(40,20)，P5=(50,18)，P6=(60,25)。节点向量采用clamped形式：U = {0,0,0,0,0.2,0.4,0.6,0.8,1,1,1,1}。
分析过程：
- 求值：在参数 u = 0.5 处，通过De Boor算法求得 C(0.5) = (30.12, 14.87)，该点位于曲线中部。
- 求导：计算一阶导数控制点 Qi = 3(Pi+1-Pi)/(Ui+4-Ui+1)，得到 C′(0.5) = (48.2, 12.5) m/参数单位。
- 二阶导数：类似地计算 C″(0.5) = (-15.3, 42.8)。
- 曲率：代入公式 κ = ||C′×C″||/||C′||³ = |48.2×42.8 - 12.5×(-15.3)| / (48.2²+12.5²)^(3/2) ≈ 0.021 m⁻¹，对应转弯半径约 47.6m。
- 全曲线扫描：在 u ∈ [0,1] 上均匀采样50个点，计算曲率分布，发现最大曲率出现在 u=0.72 处，κ_max = 0.035 m⁻¹，对应半径 28.6m。
结果与决策：车辆最小转弯半径为 25m，因此该轨迹满足动力学约束。但曲率变化率（即曲率导数）在 u=0.72 附近达到 0.12 m⁻²，超过舒适性阈值 0.08 m⁻²。团队决定在优化阶段增加曲率变化率惩罚项，通过调整控制点位置降低曲率突变。
复盘与代价：调整控制点后，最大曲率降至 0.031 m⁻¹，但轨迹总长度增加了 3.2%。这表明平滑性与路径效率之间存在权衡，需要根据实际场景设定合理的权重系数。
4.6 常见误区与总结
在B样条曲线计算中，工程师常犯以下错误：
- 节点向量理解错误：将节点向量与控制点下标混淆，导致De Boor算法定位错误。记住：节点向量长度 = 控制点数 + 阶次 + 1。
- 导数控制点计算遗漏分母：差分公式中分母是节点区间长度，不能省略。当节点重复时，需要特殊处理。
- 曲率计算未检查速度模长：在速度接近零时直接计算曲率，导致数值溢出或NaN。
- 积分精度不足：使用低阶数值积分（如梯形法）计算弧长，导致累积误差过大。建议使用自适应Gauss求积。
最佳实践：在轨迹规划系统中，建议将B样条计算封装为独立模块，提供求值、求导、积分、曲率四个核心接口。预处理阶段计算导数控制点，运行时只做O(p²)的递推运算。对于实时性要求高的场景，可预先计算曲率表，运行时通过查表插值获取。
本章系统梳理了B样条曲线从定义到工程计算的完整链路。De Boor算法提供了高效的求值手段，解析求导保证了速度与加速度的精度，数值积分与解析积分相结合满足了弧长与能量计算需求，曲率与挠率计算则为轨迹的几何特性评估提供了量化指标。掌握这些计算方法，是后续进行B样条曲线优化（如节点插入、控制点调整、平滑性优化）的基础。