B样条曲线根据曲率极值进行分段速度规划的方法介绍

在 B 样条曲线轨迹上，已经找到曲率极值点并划分了段落，也做了 S 型速度规划，但极值点附近和段内仍出现规划速度超过曲率允许值的情况。如何调整？

核心思路：
“极值点速度合规”只是必要条件，不是充分条件；必须保证整条速度曲线在曲率约束曲线下方。下面给出可落地的三步法。

1. 先把“曲率约束速度”变成连续函数

• 把整条曲线离散成足够密的点（Δu ≤ 0.001），算出每个点的曲率 κ(u)。
• 用 κ(u) 算出每个点对应的最大安全速度
  v_max(u) = √(a_max / κ(u)) (κ>0)
  得到一条“曲率限速曲线” v_max(u)。
• 这条曲线就是后续速度规划的硬上限。

2. 速度规划时“先限速、再平滑”

(1) 分段限速

• 以极值点为界，把曲线分成若干段。
• 每段取该段内 v_max(u) 的最小值作为该段的段上限v_seg。
• 这样每段的匀速区速度 ≤ v_seg，天然不会越界。

(2) S 型过渡

• 在相邻段之间做 S 型加减速，但加减速过程必须满足
  v(t) ≤ min{ v_seg_prev, v_seg_curr }
  即过渡段速度不能冲破前后两段的限速墙。
• 若仍超限，可把过渡时间拉长，或降低加加速度 j_max，让速度曲线更“扁”。

(3) 前瞻窗口

• 在每段开始前，向前看 3–5 段，取其中最小的 v_seg 作为当前段的前瞻限速。
• 这样极值点还没到，速度已经提前降下来，避免“急刹”。

3. 事后检查与自适应调整

(1) 逐点检查

• 生成完整速度曲线 v(u) 后，与 v_max(u) 逐点比较。
• 若某点 v(u) > v_max(u)，记录超限量 Δv。

(2) 局部重规划

• 对超限区间 [u_a, u_b]，把该段的 v_seg 直接设为 min(v_max(u))，再重新做 S 型过渡。
• 若仍不收敛，可加密极值点：在 [u_a, u_b] 内再找局部曲率极大值，把段落切得更细，重复步骤 2。

(3) 控制点微调（可选）

• 若超限区间集中在某段且幅度较大，可微调该段对应的 2–3 个控制点，降低曲率峰值，再重新计算 v_max(u)。
• 由于 B 样条具有局部支撑性，只影响局部速度曲线，不会“牵一发而动全身”。

一句话总结

先把“曲率→限速”做成连续上限曲线，再用“分段限速 + 前瞻 + 局部重规划”三板斧，保证整条速度曲线始终贴着上限走，既安全又平滑。