B样条

• B样条（B-Spline）笔记
  • 概述
    • 1. 目标：B样条用来做什么？
    • 2. 核心思想：一个直观的比喻
    • 3. B样条的几个概念
      • a. 控制点 (Control Points)
      • b. 次数 (Degree) / 阶数 (Order)
      • c. 节点向量 (Knot Vector)
      • d. 局部控制性 (Local Control)
    • 4. B样条 vs. 贝塞尔曲线 (Bézier)
    • 5. 总结：为什么B样条这么重要？
• B样条和贝塞尔的转换、递推公式与矩阵的转换（实际代码需要用到的）
• B样条核心：M 基础矩阵（Basis Matrix）
  • Part 1: M 矩阵是什么？
    • 两种描述曲线的“语言”
      • 语言 1：B样条基（设计师的语言：用控制点描述曲线）
      • 语言 2：多项式基（计算机的语言：用多项式描述曲线）
    • M 矩阵的定义
  • Part 2: 为什么要得到 M 矩阵？
  • Part 3: 怎么得到 M 矩阵？
    • 推导方法：泰勒展开
    • cal_M 函数在做什么？
• B样条与贝塞尔的转换
  • Step 1: 需要解决的问题：
  • Step 2: 明确目标（建立“约束翻译”）
  • Step 3: 推导“翻译矩阵” (你问的 A_trans_temp)
    • Step 4: 完整的设计流程（代码实现）
• 附录
  • 转换矩阵
• B样条核心：节点、控制点、曲线段数与次数的关系
  • 这套代码中的实现（更直观）
    • 核心公式（代码中的）
    • 为什么这个公式是正确的？
  • 代码中如何定义这个关系
• 本项目如何构建hessian和约束
  • 位置成本
  • 航向成本 (PointCost_v_)
  • 加速度/曲率成本 (PointCost_a_)
  • Jerk 和 Snap 成本 (PointCost_j_, PointCost_s_)
    • 相关矩阵的获得
    • B样条的求导和差分
    • N矩阵构成
  • 约束构建
    • 位置约束
    • heading约束
    • 曲率约束

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次） $\implies$ 曲线必须从第一个控制点开始。
     • [... 4, 4, 4, 4]（结尾重复 k+1 次） $\implies$ 曲线必须在最后一个控制点结束。
     • 这种两端“锁死”的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)

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) + \dots + 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 + \dots + A_k \cdot t^k$$

• 变量：

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

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

M 矩阵的定义

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

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

$$A = M \cdot C$$

或者用向量形式表示：

$$\begin{bmatrix} A_0 \\ A_1 \\ \vdots \\ A_k \end{bmatrix} = \begin{bmatrix} M_{0,0} & M_{0,1} & \dots & M_{0,k} \\ M_{1,0} & M_{1,1} & \dots & M_{1,k} \\ \vdots & \vdots & \ddots & \vdots \\ M_{k,0} & M_{k,1} & \dots & M_{k,k} \end{bmatrix} \cdot \begin{bmatrix} C_0 \\ C_1 \\ \vdots \\ C_k \end{bmatrix}$$

因此，$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’(t) = A_1 + 2 A_2 t + 3 A_3 t^2$
• 在 $t=0$ 处求值： $P’(0) = A_1$
• 同时，在B样条世界中求导： $P’(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 } 1} = [N’_0(0), N’_1(0), N’_2(0), N’_3(0)]$

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

• 对 $P(t)$ 求二阶导数： $P’'(t) = 2 A_2 + 6 A_3 t$
• 在 $t=0$ 处求值： $P’‘(0) = 2 A_2 \implies A_2 = \frac{P’'(0)}{2}$
• 结论：$M$ 矩阵的第三行是基函数二阶导数值除以 $2$ (即 $2!$)： $M_{\text{row } 2} = \frac{1}{2!} [N’‘_0(0), N’‘_1(0), N’‘_2(0), N’'_3(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{Bézier}} \cdot B)$

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

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

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

1

Eigen::MatrixXd A_trans_temp = A_bezier_inv * M_bspline;

• M_bspline $\implies$ 就是 $M_{\text{B-spline}}$，即B样条基础矩阵（来自 cal_M）。
• A_trans_temp $\implies$ 这就是我们的“翻译矩阵” $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{Bézier}})^{-1} * M_{bspline}$$

$$(M_{\text{Bézier}})^{-1} = \begin{bmatrix} 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{bmatrix}$$ $$M_{\text{B-spline}} = \frac{1}{120} \begin{bmatrix} 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{bmatrix}$$

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$ 个控制点。

代码中如何定义这个关系

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

本项目如何构建hessian和约束

位置成本

$J = w \cdot || P_i - P_{\text{ref}, i} ||^2$

成本函数可简化为：$J \approx w \cdot P_i^T P_i - 2w \cdot P_i^T P_{\text{ref}, i}$

B样条的一个核心特性是，曲线上任何一点的任何属性（位置、速度、加速度…）都可以表示为B样条控制点 $C$ 的线性组合。

$P_i = N_{p, i} \cdot C$ （其中 $N_{p, i}$ 是一个行向量，它来自B样条基础矩阵 Np 的第 $i$ 行，代表了在第 $i$ 个节点上，各个控制点对“位置”的“影响力”权重）

$J \approx w \cdot C^T (N_{p, i}^T N_{p, i}) C - (2w \cdot N_{p, i}^T P_{\text{ref}, i})^T C$

Hessian $P$ 的部分： $P_i = 2 \cdot w \cdot (N_{p, i}^T N_{p, i})$梯度 $q$ 的部分： $q_i = -2w \cdot N_{p, i}^T P_{\text{ref}, i}$

航向成本 (PointCost_v_)

成本函数：$J_v = w_v \cdot || V_i - V_{\text{ref}, i} ||^2$B

样条形式：$V_i = N_{v, i} \cdot C$ (其中 $N_{v, i}$ 是 Nv 矩阵的第 $i$ 行)。

Hessian $P$ 的部分：$P_v = 2 \cdot w_v \cdot (N_{v, i}^T N_{v, i})$

加速度/曲率成本 (PointCost_a_)

成本函数：$J_a = w_a \cdot || A_i - A_{\text{ref}, i} ||^2$B

样条形式：$A_i = N_{a, i} \cdot C$ (其中 $N_{a, i}$ 是 Na 矩阵的第 $i$ 行)。

Hessian $P$ 的部分：$P_a = 2 \cdot w_a \cdot (N_{a, i}^T N_{a, i})$

Jerk 和 Snap 成本 (PointCost_j_, PointCost_s_)

PointCost_j_：最小化三阶导数（Jerk，加加速度），减少“闯动感”。

PointCost_s_：最小化四阶导数（Snap，加加加速度），减少“抖动感”。

$J_j = w_j \cdot || J_i - 0 ||^2 = w_j \cdot || J_i ||^2$

相关矩阵的获得

1. M 矩阵（位置基础矩阵）的获得 $M$ 矩阵（即位置基础矩阵，对应于 0 次导数）是通过一个复杂的解析公式一次性计算出来的。

   初始化：在 B_spline 构造函数中，首先根据样条的阶数（$a=5$，即6阶）来调整矩阵大小：this->M.resize(a + 1, a + 1);。

   计算：核心的计算逻辑封装在 cal_M(Eigen::MatrixXd &M) 函数中。

   推导原理：cal_M 函数通过多层循环和组合数函数 C_a_b，实现了将B样条定义直接转换到**多项式基（Monomial Basis）**的解析公式。这个公式是事先推导好的，用于快速计算 $M$ 矩阵的每一个元素 $M(i, j)$。 计算方式前文已述。

2. 导数矩阵（M_{diff_1}, M_{diff_2}, …）的获得导数矩阵是通过对已有的 $M$ 矩阵施加 差分算子（Difference Operator） 来获得的，而不是重新计算。这个过程利用了B样条的导数特性。

   核心原理：差分矩阵对于B样条，下一阶导数的控制点 $C’$ 与当前阶的控制点 $C$ 之间存在一个线性关系，这个关系可以表示为一个差分矩阵。例如，速度（1次导数）的控制点 $C’$ 是由位置控制点 $C$ 的相邻点相减得到的。

   构造差分矩阵：代码首先构造了一个 $k \times (k+1)$ 的差分矩阵 v_mat（用于1阶导数）。这个矩阵的元素是 [-1, 1] 的对角块形式。

1
2
3
4

for (int i = 0; i < a; i++) {
    v_mat(i, i) = -1;
    v_mat(i, i + 1) = 1;
}

$$v_{\text{mat}} = \begin{bmatrix} -1 & 1 & 0 & 0 & 0 & 0 \\ 0 & -1 & 1 & 0 & 0 & 0 \\ 0 & 0 & -1 & 1 & 0 & 0 \\ 0 & 0 & 0 & -1 & 1 & 0 \\ 0 & 0 & 0 & 0 & -1 & 1 \end{bmatrix}$$ 应用差分：将差分矩阵乘以原始的基础矩阵，即可得到下一阶的导数基础矩阵：this->M_diff_1 = this->M_diff_1 * v_mat;

3. 链式应用（2, 3, 4 阶导数）对于更高的阶数（加速度 $M_{\text{diff\\_2}}$、Jerk $M_{\text{diff\\_3}}$、Snap $M_{\text{diff\\_4}}$），代码继续构造新的差分矩阵 (a_mat, jerk_mat, snap_mat)，并以链式法则的形式将它们连续应用到原始的基础矩阵上： 2阶导数： this->M_diff_2 = this->M_diff_2 * a_mat * v_mat; 4阶导数： this->M_diff_4 = this->M_diff_4 * snap_mat * jerk_mat * a_mat * v_mat;

B样条的求导和差分

对于一个 $k$ 次B样条，它的第 $r$ 阶导数是一条 $k-r$ 次的B样条。第 $r$ 阶导数的第 $i$ 个控制点 $C^{®}_{i}$ 由以下公式给出：

$$C^{®}_{i} = \frac{k - r + 1}{t_{i+k+1} - t_{i+r}} (C^{(r-1)}_{i+1} - C^{(r-1)}_{i})$$

对于均匀B样条，即我们假设节点间距是恒定的（通常为1）。在这种情况下，上式中的系数项（被称为缩放因子）可以简化为一个常数：

$$\frac{k - r + 1}{t_{i+k+1} - t_{i+r}} \quad \xrightarrow{\text{Uniform Knots}} \quad \frac{k - r + 1}{k - r + 1} = 1$$

我们的目标是构造一个矩阵 $D$ (即 $v_{mat}$, $a_{mat}$, 等)，它能将 $N_{\text{in}}$ 个控制点（输入向量）转换为 $N_{\text{out}}$ 个控制点（输出向量）。假设我们要计算第 $r$ 阶导数，即从 degree $k-r+1$ 降到 $k-r$。

$$D_{r}(i, j) = \begin{cases} -1 & \text{if } j = i \\ +1 & \text{if } j = i+1 \\ 0 & \text{otherwise} \end{cases}$$

要从原始的 $C_{\text{position}}$ 向量直接计算第 $r$ 阶导数 $C^{®}$，我们需要连续应用 $r$ 个差分矩阵。

$$C^{®} = D_{\text{total}} \cdot C_{\text{position}}$$

$D_1$ (即 $v_{\text{mat}}$)：作用于原始 $k$ 次控制点，生成 $k-1$ 次控制点。

$D_2$ (即 $a_{\text{mat}}$)：作用于 $D_1$ 的结果，生成 $k-2$ 次控制点。

$D_r$：作用于第 $r-1$ 阶导数的控制点，生成最终的 $r$ 阶导数控制点。

$v_{\text{mat}}$ ($D_1$)：$5 \times 6$ $a_{\text{mat}}$ ($D_2$)：$4 \times 5$ $jerk_{\text{mat}}$ ($D_3$)：$3 \times 4$ $snap_{\text{mat}}$ ($D_4$)：$2 \times 3$

N矩阵构成

以Np为例 对于位置（0次导数），局部构建块是 $1 \times 6$ 的 $row\\_0$ 向量：这个 $row\\_0$ 是从 $6 \times 6$ 的 $M$ 矩阵（位置基础矩阵）中提取出来的。它代表了 $k+1=6$ 个控制点对单个节点位置的影响权重。

通过循环将 $row\\_0$ 堆叠到 $Np$ 中：

1. 循环：代码遍历所有节点（for (int row_index = 0; …)），每一行对应曲线上一个要被约束的位置。
2. 稀疏性：由于局部控制性，每一行只在对应影响它的6个控制点（列）上包含非零值（即 $row\\_0$）。
3. 平移：代码计算出当前节点对应的 6个控制点的起始列索引（interval_index），然后将 $row\\_0$ 填充到 $Np$ 矩阵的相应位置。通过这种方式， $Np$ 矩阵被构建成一个带有稀疏对角块的结构，从而实现了全局映射。

约束构建

位置约束

我们将位置 $P(t_i)$ 表示为控制点 $C$ 的线性组合：

$$P(t_i) = Np_i \cdot C$$

heading约束

由于QP求解器只能处理线性约束（$Ax \le u$），因此必须将非线性的角度约束 ($\theta = \arctan(V_y/V_x)$) 转化为关于向量分量 ($V_x, V_y$) 的线性不等式。

首先计算出角度边界 ($\theta_{\text{min}}$ 和 $\theta_{\text{max}}$) 对应的 $\cos$ 和 $\sin$ 值：

$\theta_{\text{min}}$ 边界：由 $(dx_{\text{min}}, dy_{\text{min}})$ 定义。

$\theta_{\text{max}}$ 边界：由 $(dx_{\text{max}}, dy_{\text{max}})$ 定义。

曲率约束

曲率公式：$\kappa = \frac{|V_x A_y - V_y A_x|}{(V_x^2 + V_y^2)^{3/2}}$ ($\vec{A}$是二阶导数) 为了在QP求解器中使用，必须将曲率约束简化为一个线性不等式。

1. 分母： 在路径规划的局部优化中，车辆的切线速度（分母项 $V_x^2 + V_y^2$) 被假定为非零常数或已知值。
2. 分子： 曲率约束被简化为约束分子中的法向加速度分量（即 $V_x A_y - V_y A_x$）。
3. 近似航向： 为了得到 $V_x$ 和 $V_y$ 的系数，我们用已知的参考航向 $\theta_{\text{ref}}$ （或上一次迭代的航向）来近似 $V_x$ 和 $V_y$：

$$V_x \approx \cos(\theta_{\text{ref}})$$ $$V_y \approx \sin(\theta_{\text{ref}})$$

此时约束变为线性的 $$\text{Lower} \le [-\sin(\theta_{\text{ref}}) \cdot A_x + \cos(\theta_{\text{ref}}) \cdot A_y] \le \text{Upper}$$