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 矩阵是什么？

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

两种描述曲线的“语言”

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

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

• 公式：

• 变量：

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

（基函数）非常缓慢，因为它是一个复杂的递归函数。

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

• 公式：

• 变量：

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

M 矩阵的定义

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

或者用向量形式表示：

因此，$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$ 处的点时：

旧的慢方法：费力地计算

新的快方法（代码 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$ 满足：

推导方法：泰勒展开

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

我们以三次B样条 (k=3, Order=4) 为例（比代码中的5次简单，但原理一致）。

第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)$ 为：

(即：第 $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}})^{-1}$：

这正是 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);）

附录

转换矩阵

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)**

换算关系：
我们可以将这两个公式合并，得到所有四个量之间的关系：

举个例子（$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$ = `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$。
代入上面的公式：

这正是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 \approx w \cdot P_i^T P_i - 2w \cdot P_i^T P_{\text{ref}, i}$

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

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

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，加加加速度），减少“抖动感”。

相关矩阵的获得

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;
}

应用差分：将差分矩阵乘以原始的基础矩阵，即可得到下一阶的导数基础矩阵：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^{(r)}_{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$。

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

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$ 的线性组合：

heading约束

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

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

曲率约束

曲率公式：$\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$：

此时约束变为线性的