自动驾驶中的B样条轨迹及B样条＜-＞贝塞尔转换实现避障

github 原文地址

B样条（B-Spline）笔记

概述

1. 目标：B样条用来做什么？

在最基础的层面上，B样条（B-Spline，即 Basis Spline）是一种通过一组“控制点”来定义平滑曲线的数学方法。

想象一下：

• 问题：你需要在计算机上绘制一条复杂的、非常平滑的曲线，比如汽车的流线型车身、一个漂亮的字体，或者一条机器人的运动轨迹。
• B样条的解决方案：你不需要定义曲线上成千上万个点的坐标，你只需要在曲线“大致”经过的地方放几个控制点，B样条算法就会自动帮你“脑补”出一条穿梭于这些点之间的、数学上完美的平滑曲线。

2. 核心思想：一个直观的比喻

理解B样条最简单的方法是“磁铁”和“弹性尺”的比喻：

1. 控制点 (Control Points)：想象它们是一块块磁铁，你把它们放在桌子上。
2. B样条曲线：想象你有一根弹性的、灵活的尺子（“Spline”这个词的本意就是“细木条”或“样条”，是造船时用来画船体曲线的工具）。
3. 生成曲线：你把这根弹性尺“扔”到磁铁附近。尺子会因为磁铁的吸引力而弯曲，朝着磁铁的方向靠近，但不一定会碰到每一块磁铁（除非磁铁吸力特别强）。

最终，这根弹性尺形成的那个平滑的、被磁力吸引而弯曲的形状，就是B样条曲线。

• 你移动一块磁铁（控制点），尺子（曲线）的形状就会局部发生平滑的变化。

3. B样条的几个概念

a. 控制点 (Control Points)

• 这就是上面比喻中的“磁铁”。
• 它们是B样条的**“骨架”。将所有控制点按顺序“连连看”形成的折线，被称为控制多边形 (Control Polygon)**。
• B样条曲线总是被完全包含在它的控制多边形的“凸包”内（Convex Hull Property）。简单说，曲线永远不会“飞出”它的控制点所框定的大致范围。

b. 次数 (Degree) / 阶数 (Order)

这是一个非常重要的设置项，它决定了曲线的**“平滑等级”**。

• 关系：阶数 (Order) = 次数 (Degree) + 1。（在你的代码中，B_spline_order 5 指的是阶数，所以它是一个4次样条。注：请再次核对你的代码，B_spline_order 5 通常指k=5，即5次，Order=6。我们假设你的代码中 B_spline_order 指的是次数 (Degree=k)，即5次样条，Order=6。）

  • （为避免混淆，我们下面统一使用“次数”）

• 次数 (k=1), 线性：曲线就是控制多边形本身（一堆直线）。非常“硬”。

• 次数 (k=2), 二次：曲线由抛物线段构成。开始变得平滑。

• 次数 (k=3), 三次 (Cubic)：这是最常用的！ 比如 Adobe Illustrator、CAD软件、字体设计默认都是它。它在“平滑度”和“控制性”之间达到了完美的平衡。

• 次数 (k=5), 五次 (Quintic)：（这就是你代码中用的） 这是一个非常高等级的平滑。为什么用这么高？因为五次样条（六阶）能保证它的**“加加速度（Jerk）”和“加加加速度（Snap）”也是平滑的。在机器人或自动驾驶路径规划中，这意味着极高的乘坐舒适度**，机器或车辆在执行轨迹时不会有突然的“抖动”。

c. 节点向量 (Knot Vector)

• 这是B样条最强大、也最难理解的概念。
• 比喻：如果说控制点是“磁铁”，那么节点向量就是一本**“规则手册”，它详细规定了每一个控制点从哪里开始生效，到哪里结束生效**。
• 它是一个数字列表（必须是非递减的），例如：[0, 0, 0, 0, 1, 2, 3, 4, 4, 4, 4] (对于一个3次样条)。
• 它的作用：
  1. 定义曲线的“分段点”：[...0, 1, 2, 3, 4...] 这些变化的值，定义了曲线在参数 t 上的分段。
  2. 控制“端点”行为：
     • 你注意到开头有4个 0，结尾有4个 4 吗？
     • 规则：如果一个节点值（如0）重复了 k 次（k = 次数），那么曲线将精确地穿过它对应的那个控制点。
     • 所以，[0, 0, 0, 0, ...]（开头重复 k+1 次，这里是4次） $⟹$ 曲线必须从第一个控制点开始。
     • [... 4, 4, 4, 4]（结尾重复 k+1 次） $⟹$ 曲线必须在最后一个控制点结束。
     • 这种两端“锁死”的B样条，也称为**“Clamped” B-spline（钳位B样条）**。

d. 局部控制性 (Local Control)

这是B样条相对贝塞尔曲线（Bézier）的“杀手锏”功能。

• 贝塞尔曲线的问题：在一条（单一的）贝塞尔曲线中，你移动 任何一个 控制点，整条曲线（从头到尾）都会发生变化。这叫“全局修改”。
• B样条的优势：由于“节点向量”这本规则手册的存在，每个控制点$$C_i$$的“磁力”只在曲线的一小段上有效。
• 结果：如果你有一条由100个控制点定义的曲线，你移动第50号控制点，只有它附近（比如48号到52号）那一小段曲线会跟着变形。曲线的开头和结尾（比如1号点和100号点）完全不会动。

这对于精确调整一个复杂曲线的局部细节（比如在规划路径时躲避一个小障碍物）是至关重要的。

4. B样条 vs. 贝塞尔曲线 (Bézier)

特性	贝塞尔曲线 (Bézier Curve)	B样条 (B-Spline)
控制性	全局控制 (移动一个点，整条线都变)	局部控制 (移动一个点，只变一小段)
端点	总是穿过第一个和最后一个控制点	不一定穿过 (除非用节点向量“钳住”它)
复杂度	更简单，不需要“节点向量”	更复杂，但功能更强大
关系	贝塞尔曲线是B样条的一种特例	B样条是贝塞尔曲线的推广

5. 总结：为什么B样条这么重要？

1. 局部控制：最核心的优势。允许你精细、安全地修改复杂曲线。
2. 平滑度可控：你可以通过“次数”来精确定义你需要的平滑等级（例如，自动驾驶需要“Jerk平滑”，就用5次）。
3. 通用性：它是一种“统一”的数学表达。它可以完美地表示一条直线、一个圆、一条贝塞尔曲线，或者一条极其复杂的有机曲线。

B样条和贝塞尔的转换、递推公司与矩阵的转换（实际代码需要用到的）

B样条核心：M 基础矩阵（Basis Matrix）

Part 1: M 矩阵是什么？

$M$矩阵是一个**“翻译官”，它的唯一工作就是“基变换” (Change of Basis)**。

具体来说，它负责将B样条的**“控制点语言”翻译成计算机最喜欢的“多项式语言”**。

---

两种描述曲线的“语言”

想象我们有两种方式来描述同一条平滑曲线：

语言 1：B样条基（设计师的语言：用控制点描述曲线）

• 公式：

$$P(t)=C_0\cdot N_{0,k}(t)+C_1\cdot N_{1,k}(t)+\cdots +C_k\cdot N_{k,k}(t)$$

• 变量：

$$C$$ (一个包含k+1个控制点的向量)。

• 优点：非常直观，具有“局部控制性”。
• 缺点：计算

$$N_{i,k}(t)$$
（基函数）非常缓慢，因为它是一个复杂的递归函数。

语言 2：多项式基（计算机的语言：用多项式描述曲线）

• 公式：

$$P(t)=A_0+A_1\cdot t+A_2\cdot t^2+\cdots +A_k\cdot t^k$$

• 变量：

$$A$$ (一个包含 $k+1$ 个多项式系数的向量)。

• 优点：计算极其快速，计算机硬件天生就擅长这个。
• 缺点：非常不直观，改一个系数 $A_2$，整条曲线都会变。

---

M 矩阵的定义

$M$ 矩阵就是连接这两种语言的**“翻译矩阵”**。

B样条理论证明，对于同一条曲线段，它的多项式系数 $A$ 和它的B样条控制点 $C$ 之间，存在一个固定的线性关系：

$$A=M\cdot C$$

或者用向量形式表示：

$$\left[\begin{array}{c}{A}_0\\ A_1\\ ⋮\\ A_k\end{array}\right]=\left[\begin{array}{cccc}{M}_{0,0}& M_{0,1}& \dots & M_{0,k}\\ M_{1,0}& M_{1,1}& \dots & M_{1,k}\\ ⋮& ⋮& \ddots & ⋮\\ M_{k,0}& M_{k,1}& \dots & M_{k,k}\end{array}\right]\cdot \left[\begin{array}{c}{C}_0\\ C_1\\ ⋮\\ C_k\end{array}\right]$$

因此，$M$ 矩阵是一个常量矩阵，它将B样条控制点向量 $C$ 转换为等效的多项式系数向量 $A$。

在代码中，M_pos_、this->M 就是这个 $M$ 矩阵。cal_M 函数 就是用来在程序启动时，根据数学公式计算出这个矩阵。

Part 2: 为什么要得到 M 矩阵？

答案：为了追求极致的计算速度。

有了 $M$ 矩阵，我们就把一个“递归”问题（慢）转换成了一个“多项式求值”问题（快）。

在自动驾驶或机器人中，我们每秒可能需要重新规划几十次路径。如果每次计算一个点的位置，都需要从B样条最基础的递归公式（$N(t)$）开始，计算量会大到无法接受。

现在的工作流程变得飞快：

1. 程序启动时：调用一次 cal_M()，计算这个“翻译官” $M$。
2. 优化器运行时：当需要计算 $t=0.5$ 处的点时：

旧的慢方法：费力地计算

$$N_{0,5}(0.5),N_{1,5}(0.5),\dots$$

新的快方法（代码 Get_line_position 正是这么做的）：
1. A = M * C (一次矩阵乘法，得到多项式系数)
2. P(0.5) = A_0 + A_1 \cdot 0.5 + A_2 \cdot (0.5)^2 + \dots (几次乘法和加法)

结论：$M$ 矩阵是B样条在工程中得以高速应用（尤其是在优化问题中）的关键。

Part 3: 怎么得到 M 矩阵？

$M$ 矩阵是一个“基变换矩阵” (Change of Basis Matrix)。 它的推导是纯粹的数学。

我们再回到这两个公式：

1. $P(t)=T(t)\cdot A$ (多项式形式， $T(t)=[1,t,t^2,\dots ,t^k]$)
2. $P(t)=N(t)\cdot C$ (B样条形式， $N(t)=[N_{0,k}(t),N_{1,k}(t),\dots ]$)

我们想找到 $M$ 使得 $A=M\cdot C$。
将 $A$ 替换掉： $P(t)=T(t)\cdot (M\cdot C)$
所以，我们必须找到一个 $M$ 满足：

$$N(t)=T(t)\cdot M$$

---

推导方法：泰勒展开

我们可以通过在 $t=0$ 处对 $P(t)$ 连续求导来“剥离”出多项式系数 $A_i$。

我们以三次B样条 (k=3, Order=4) 为例（比代码中的5次简单，但原理一致）。
$P(t)=A_0+A_{1}t+A_2{t}^2+A_3{t}^3$

第1步：求 $A_0$ (M的第一行)

• 在 $t=0$ 处求值：
  $P(0)=A_0$
• 同时，在B样条世界中：
  $P(0)=C_0{N}_0(0)+C_1{N}_1(0)+C_2{N}_2(0)+C_3{N}_3(0)$
• 所以：$A_0=C_0{N}_0(0)+C_1{N}_1(0)+C_2{N}_2(0)+C_3{N}_3(0)$
• 结论：$M$ 矩阵的第一行就是B样条基函数在 $t=0$ 处的值：
  $M_{\text{row }0}=[N_0(0),N_1(0),N_2(0),N_3(0)]$

第2步：求 $A_1$ (M的第二行)

• 对 $P(t)$ 求一阶导数：
  $P^{\prime }(t)=A_1+2A_{2}t+3A_3{t}^2$
• 在 $t=0$ 处求值：
  $P^{\prime }(0)=A_1$
• 同时，在B样条世界中求导：
  $P^{\prime }(0)=C_0{N}_0^{\prime }(0)+C_1{N}_1^{\prime }(0)+C_2{N}_2^{\prime }(0)+C_3{N}_3^{\prime }(0)$
• 结论：$M$ 矩阵的第二行就是B样条基函数的一阶导数在 $t=0$ 处的值：
  $M_{\text{row }1}=[N_0^{\prime }(0),N_1^{\prime }(0),N_2^{\prime }(0),N_3^{\prime }(0)]$

第3步：求 $A_2$ (M的第三行)

• 对 $P(t)$ 求二阶导数：
  $P^{\prime \prime }(t)=2A_2+6A_{3}t$
• 在 $t=0$ 处求值：
  $P^{\prime \prime }(0)=2A_2⟹A_2=\frac{P^{\prime \prime }(0)}{2}$
• 结论：$M$ 矩阵的第三行是基函数二阶导数值除以 $2$ (即 $2!$)：
  $M_{\text{row }2}=\frac{1}{2!}[N_0^{\prime \prime }(0),N_1^{\prime \prime }(0),N_2^{\prime \prime }(0),N_3^{\prime \prime }(0)]$

第4步：求 $A_k$ (M的第k+1行)

• 通用公式（泰勒展开）：$A_k=\frac{P^{(k)}(0)}{k!}$
• M 矩阵的通用定义： $M$ 矩阵第 $i$ 行、第 $j$ 列的元素 $M(i,j)$ 为：

$$M(i,j)=\frac{N_{j,k}^{(i)}(0)}{i!}$$

(即：第 $j$ 个基函数的 $i$ 阶导数在 $t=0$ 处的值，再除以 $i$ 的阶乘)

cal_M 函数在做什么？

cal_M 函数中的那个复杂的、带有组合数 C_a_b 和 pow 的公式，就是 $M(i,j)=\frac{N_{j,k}^{(i)}(0)}{i!}$ 这个公式的最终解析解（Analytic Solution）。

• temp_k 就是 $k$ (次数)。
• factor 就是 $k!$。
• temp1 和 temp2 里的组合数和循环，就是数学家们通过符号计算（比如用Mathematica）推导出的 $N_{j,k}^{(i)}(0)$ 的通用表达式。

B样条与贝塞尔的转换

Step 1: 需要解决的问题：

一个在路径规划中非常经典的问题：

“我想要B样条的平滑性（Jerk/Snap连续），但又想要贝塞尔曲线的严格约束性（凸包特性）。我如何能两者兼得？”

本架构使用的方案是：“用B样条的控制点作为优化变量，但在构建约束时，把它们‘假装’成贝塞尔控制点来用。”

• B样条控制点（钝器）：

  • 优点：非常适合做平滑度优化。B样条的数学结构保证了曲线在节点处是连续的（C2, C3, C4…连续）。
  • 缺点：不适合做边界约束。B样条的“凸包性”很弱，控制点 $C_i$ 对曲线的影响是“柔和”且“宽泛”的。你把 $C_i$ 放在路沿内侧，并不能保证曲线本身不会“鼓出去”撞上路沿。

• 贝塞尔控制点（利器）：

  • 优点：非常适合做边界约束。贝塞尔曲线有严格的**“凸包性”（Convex Hull Property）。只要你保证所有贝塞尔控制点 $B_i$ 都在路沿内侧，数学上就100%保证**整条曲线段都在路沿内侧。
  • 缺点：不适合做平滑拼接。要把很多段贝塞尔曲线平滑地（例如曲率连续）拼在一起，需要对控制点施加额外的、复杂的等式约束，这会把优化问题搞得很复杂。

设计决策：
我们选择B样条控制点 $C$ 作为我们的最终优化变量 $x$，因为它们天生就能保证平滑。
但是，在施加边界约束时，我们希望能利用贝塞尔控制点 $B$ 的“凸包性”。

Step 2: 明确目标（建立“约束翻译”）

我们的优化问题是：

• 求解：$x=C_{\text{B-spline}}$ (B样条控制点)
• 最小化：Cost(C) (B样条的Jerk、Snap等平滑成本)
• 约束：…
• 我们真正想要的约束是施加在贝塞尔控制点 $B$ 上的：
  $\text{Boundary}\cdot B\le \text{Limit}$
• 但我们的优化变量是 $C$。
• 所以，我们必须找到一个**“翻译矩阵” $M_{\text{trans}}$**，它能告诉我们 $B$ 和 $C$ 之间的关系：
  $B=M_{\text{trans}}\cdot C$
• 一旦找到了 $M_{\text{trans}}$，我们就可以把“愿望”代入，得到一个OSQP能看懂的、关于 $C$ 的新约束：
  $(\text{Boundary}\cdot M_{\text{trans}})\cdot C\le \text{Limit}$

Step 3: 推导“翻译矩阵” (你问的 A_trans_temp)

1. 多项式形式：$P(t)=T\cdot A$
2. B样条形式：$P(t)=T\cdot (M_{\text{B-spline}}\cdot C)$
3. 贝塞尔形式：$P(t)=T\cdot (M_{\text{Beˊzier}}\cdot B)$

从 (2) 和 (3) 可知：
$M_{\text{Beˊzier}}\cdot B=M_{\text{B-spline}}\cdot C$

两边左乘 $(M_{\text{Beˊzier}}{)}^{-1}$：
$B=(M_{\text{Beˊzier}}{)}^{-1}\cdot M_{\text{B-spline}}\cdot C$

这正是 new_osqp_interface.cc 中这行代码的数学原理：

Eigen::MatrixXd A_trans_temp = A_bezier_inv * M_bspline;

• M_bspline $⟹$ 就是 $M_{\text{B-spline}}$，即B样条基础矩阵（来自 cal_M）。
• A_trans_temp $⟹$ 这就是我们的“翻译矩阵” $M_{\text{trans}}$！

1. B样条的“段”由“节点”划分：
   “5米”（length_between_knots_） 是**节点（Knot）的间距。B样条曲线是由节点划分成段（Segment）**的。

2. 一个“段”由 $k+1$ 个控制点决定：
   代码是 $k=5$ 次（B_spline_order 5）。
   这意味着，任意一个曲线段（比如节点 $t_i$ 到 $t_{i+1}$ 这一段），它的形状是由6个（即 $k+1$）B样条控制点 共同决定的。

所以，这个“翻译”是“6个B样条控制点 $\to$ 6个贝塞尔控制点”。

• A_trans_temp 是一个 $6\times 6$ 的矩阵。

输入：6个B样条控制点 $[C_i,C_{i+1},\dots ,C_{i+5}]$

输出：6个等效的贝塞尔控制点 $[B_0,B_1,\dots ,B_5]$（这6个点定义了同一段曲线）

Step 4: 完整的设计流程（代码实现）

现在，我们来看 AddAugmentConstraintToAmatrix 或 AutomateCalculateAffineConstraint 这样的函数是如何工作的：

1. 遍历所有曲线“段”：
   代码通过一个循环 for (int i = 1; i < num_of_knots_; i++) 来遍历每一段曲线。

2. 获取该段的“B样条控制点组”：
   代码知道第 i 段曲线（从节点 i-1 到 i）是由 spline_order_ + 1（即6）个B样条控制点决定的，这6个控制点的起始索引是 i-1。

3. 获取该段的“路沿约束”：
   代码获取第 i 段对应的路沿边界（比如一条直线 $ax+by\le c$）。这个约束用一个矩阵 A_vertice 来表示，它代表 $a$ 和 $b$。

4. 执行“约束翻译”：
   我们的“愿望”是：A_vertice * B_seg <= c (其中 $B_{seg}$ 是这一段的6个贝塞尔控制点)

我们的“翻译”是：$B_{seg}=\text{A\_trans\_temp}\cdot C_{seg}$ (其中 $C_{seg}$ 是这一段的6个B样条控制点)
代码计算这个“翻译后”的约束矩阵：
Eigen::MatrixXd A_vertice_bound = A_vertice * A_trans;
（这里的 A_trans 就是 A_trans_temp）

1. “喂”给求解器：
   A_vertice_bound 现在是一个（例如） $6\times 12$ 的矩阵，它代表了对6个B样条控制点（$C_i,\dots ,C_{i+5}$）的 $x$ 和 $y$ 坐标施加的等效约束。

代码将这个 A_vertice_bound 矩阵，填充到OSQP总约束矩阵 $A$ 的正确位置——即对应B样条控制点 i-1 到 i+4 的那几列中。
（例如 A_matrix(row_index, k + i - 1) = A_vertice_bound(j, k);）

附录

转换矩阵

$$A_{trans\_temp}=(M_{\text{Beˊzier}}{)}^{-1}\ast M_{bspline}$$

$$(M_{\text{Beˊzier}}{)}^{-1}=\left[\begin{array}{cccccc}1.0& 0.0& 0.0& 0.0& 0.0& 0.0\\ 1.0& 0.2& 0.0& 0.0& 0.0& 0.0\\ 1.0& 0.4& 0.1& 0.0& 0.0& 0.0\\ 1.0& 0.6& 0.3& 0.1& 0.0& 0.0\\ 1.0& 0.8& 0.6& 0.4& 0.2& 0.0\\ 1.0& 1.0& 1.0& 1.0& 1.0& 1.0\end{array}\right]$$

$$M_{\text{B-spline}}=\frac{1}{120}\left[\begin{array}{cccccc}1& 26& 66& 26& 1& 0\\ -5& -50& 0& 50& 5& 0\\ 10& 20& -60& 20& 10& 0\\ -10& 20& 60& -20& -10& 0\\ 5& -20& 30& -20& 5& 0\\ -1& 5& -10& 10& -5& 1\end{array}\right]$$

B样条核心：节点、控制点、曲线段数与次数的关系

在B样条的数学理论中，节点（Knots）、控制点（Control Points）、**曲线段数（Segments）**和次数（Degree）之间的关系是固定的。

有两个核心公式：

1. 经典教科书公式（关于节点向量总长度）：
   $$m=n+p$$
   或者
   $$m=n+k+1$$

   • $m$ = 节点(Knots)的数量（即节点向量的总长度）
   • $n$ = 控制点(Control Points)的数量
   • $p$ = 阶数 (Order)
   • $k$ = 次数 (Degree) (并且 $p=k+1$)

2. 工程“黄金公式”（关于曲线段数量）：
   $$n=N_{\text{seg}}+k$$

   • $N_{\text{seg}}$ = 曲线段(Segments)的数量
   • $n$ = 控制点(Control Points)的数量
   • $k$ = 次数 (Degree)

换算关系：
我们可以将这两个公式合并，得到所有四个量之间的关系：
$m=(N_{\text{seg}}+k)+k+1$
$$m=N_{\text{seg}}+2k+1$$

举个例子（$k=3$）：

• 假设我们有 5段 曲线 ($N_{\text{seg}}=5$)。
• 根据公式2，我们需要 $n=5+3=8$ 个控制点。
• 根据公式1，节点向量总长度 $m=8+3+1=12$ 个节点。
  • (例如：[0, 0, 0, 0, 1, 2, 3, 4, 5, 5, 5, 5])

---

这套代码中的实现（更直观）

你的这套代码没有使用上面的公式，而是用了一个更符合工程直觉的、等效的公式。

在 new_osqp_interface.cc 的 Update_refpoint_Bspline 函数中，关系是这样定义的：

1. num_of_knots_

   • 在代码中，这个变量不是指节点向量的总长度。
   • 它指的是你输入的参考点（Ref Points）的数量。
   • size_t num_of_knots = resample_points.size();

2. num_of_knots_interval_

   • 这个变量被定义为 num_of_knots_ - 1。
   • 它的真正含义是**“B样条的曲线段（Segment）的数量”**。
   • 例如：你有10个参考点，你就在它们之间创建 $10-1=9$ 段B样条曲线。

3. spline_order_

   • 在 B_spline.h 中 B_spline_order 被定义为5。
   • 在 B_spline.cpp 中，M 矩阵被resize为 (a+1, a+1)，其中 a = B_spline_order。
   • 这证明了 spline_order_ 变量存储的是次数 (Degree, k)，即 $k=5$。
   • （因此，这是一个 5次、6阶 的B样条）。

核心公式（代码中的）

在 Update_refpoint_Bspline 中，关键的赋值是：

set_num_of_control_points(num_of_knots + spline_order_ - 1);

我们来翻译一下这个公式：

$$n=N_{\text{ref}}+k-1$$

• $n$ = num_of_control_points_
• $N_{\text{ref}}$ = num_of_knots_ （参考点的数量）
• $k$ = spline_order_ （次数，为5）

为什么这个公式是正确的？

我们把上面这个公式换一种写法。
我们知道 $N_{\text{seg}}=N_{\text{ref}}-1$，所以 $N_{\text{ref}}=N_{\text{seg}}+1$。
代入上面的公式：
$n=(N_{\text{seg}}+1)+k-1$
$n=N_{\text{seg}}+k$

这正是B样条中关于“分段”的黄金公式：

控制点的数量 = 曲线段的数量 + 曲线的次数
$n=N_{\text{seg}}+k$

直观理解一下这个公式（以 $k=5$ 为例）：

• 第1段 (N_seg=1)：你需要 $k+1$ 个控制点来定义第一段曲线（即6个CPs）。
  • 公式：$n=1+5=6$。 (正确)
• 第2段 (N_seg=2)：为了和第一段平滑拼接，你只需要增加1个新的控制点。
  • 总控制点数 = 7。
  • 公式：$n=2+5=7$。 (正确)
• 第3段 (N_seg=3)：你再增加1个新的控制点。
  • 总控制点数 = 8。
  • 公式：$n=3+5=8$。 (正确)

以此类推…

第 $N_{\text{seg}}$ 段：你需要 $N_{\text{seg}}+k$ 个控制点。

总结：代码中如何定义这个关系

理论术语	代码中的变量	解释
参考点数量 ($N_{\text{ref}}$)	num_of_knots_	resample_points.size()
曲线段数量 ($N_{\text{seg}}$)	num_of_knots_interval_	等于 num_of_knots_ - 1
B样条次数 ($k$)	spline_order_	固定为 5
控制点数量 ($n$)	num_of_control_points_	由公式 $n=N_{\text{seg}}+k$ 算出

代码中的核心关系在 Update_refpoint_Bspline 中：
num_of_control_points_ = (num_of_knots_ - 1) + spline_order_
即：
控制点数量 = (参考点数量 - 1) + 次数

github 原文地址