1. 三次 B 样条曲线及其导数
三次 B 样条曲线定义为：
C(u) = Σᵢ₌₀ⁿ Pᵢ · Nᵢ,₃(u), u ∈ [uₘᵢₙ, uₘₐₓ]
其中：
- Pᵢ 为第 i 个控制点（二维或三维向量）；
- Nᵢ,₃(u) 为三次 B 样条基函数；
- u 为曲线参数。
通过递推或解析方法，可计算一阶导数 C′(u) 和二阶导数 C″(u)。
2. 曲率计算（平面或空间曲线）
对于平面曲线，曲率 κ(u) 可表示为：
κ(u) = |C′ₓ(u)·C″ᵧ(u) − C′ᵧ(u)·C″ₓ(u)| / (C′ₓ(u)² + C′ᵧ(u)²)^(3/2)
对于空间曲线，通用形式为：
κ(u) = ‖C′(u) × C″(u)‖ / ‖C′(u)‖³
其中：
- "×" 表示向量叉积；
- ‖·‖ 表示欧几里得范数；
- 分母不能为零（即 C′(u) ≠ 0）。
3. 曲率极值求解
对整条曲线在参数区间内进行采样或数值优化，找出局部和全局曲率极值点：
- 在每个 B 样条段 [uⱼ, uⱼ₊₁] 内，求解 dκ(u)/du = 0 的根；
- 同时检查端点 uⱼ 和 uⱼ₊₁ 处的曲率；
- 记录该段最大曲率 κₘₐₓ,ⱼ = max{κ(u) | u ∈ [uⱼ, uⱼ₊₁]}。
可采用黄金分割法、牛顿迭代法或自适应采样（如曲率变化率大时加密采样）提高精度。
4. 基于曲率的限速模型
假设系统允许的最大向心加速度为 aₘₐₓ，则在曲率 κ 处对应的最大安全速度 v 满足：
aₙ = v² · κ ≤ aₘₐₓ
⇒ vₘₐₓ = √(aₘₐₓ / κ) （当 κ > 0）
特别地：
- 若 κ = 0（直线段），则不限速，取 vₘₐₓ = vₗᵢₘ（系统最大线速度）；
- 若 κ → ∞（尖点或奇异点），则 vₘₐₓ → 0，需特殊处理或路径重规划。
因此，对每一段 j，其速度上限为：
vⱼ = { √(aₘₐₓ / κₘₐₓ,ⱼ), 若 κₘₐₓ,ⱼ > 0;
vₗᵢₘ, 若 κₘₐₓ,ⱼ = 0 }
5. 速度平滑与前瞻规划
为避免速度突变，需对分段限速结果进行平滑处理：
- S 型速度规划：在相邻段之间插入加减速过渡段，满足加速度 a 和加加速度 j（jerk）约束；
- 前瞻窗口机制：在当前位置向前预览若干段（如 3~5 段），取其中最小 vₘₐₓ 作为当前实际限速，防止"看到弯才刹车"；
- 最终速度曲线 v(u) 应满足：
- v(u) ≤ vⱼ, ∀u ∈ 第 j 段；
- v(u) 连续可导（通常 C¹ 连续）；
- 加速度 |dv/dt| ≤ aₘₐₓ；
- 加加速度 |d²v/dt²| ≤ jₘₐₓ（可选）。
6. 实施步骤总结
- 输入三次 B 样条控制点与节点矢量；
- 对每段参数区间 [uⱼ, uⱼ₊₁] 计算曲率 κ(u)；
- 求取每段最大曲率 κₘₐₓ,ⱼ；
- 根据 vⱼ = √(aₘₐₓ / κₘₐₓ,ⱼ) 计算各段速度上限；
- 应用前瞻策略和平滑算法生成全局连续速度曲线 v(u)；
- 输出时间参数化轨迹：C(t) = C(u(t))，其中 t 由 v(u) 积分得到。