障碍环境下B样条轨迹问题解析

以下为对论文 Zhou, B., Gao, F., Wang, L., Liu, C., & Shen, S. (2019). Robust and efficient quadrotor trajectory generation for fast autonomous flight. IEEE Robotics and Automation Letters, 4(4), 3529-3536. 提出的B样条避障轨迹方法的综合分析

控制点的生成过程

在这篇论文中，B 样条（degree = 3）的控制点是从“动力学路径搜索”得到的初始离散路径直接生成的，具体过程可概括如下：

  1. 先用动力学路径搜索生成一条离散的初始轨迹 在论文第 III 节，作者通过一种基于混合状态 A* 的“动力学路径搜索”算法，在体素网格中搜索出一系列符合安全性和动力学可行性的运动基元（motion primitives）所连成的路径。这条初始路径由多个离散状态（包括位置和速度）构成，每个状态对应一个离散的空间点，以及从该点出发一段固定持续时间的运动基元。

  2. 将初始离散路径映射为三次均匀B 样条的控制点序列 在论文第 IV-A 节，作者说明了如何用 “三次（pb = 3）均匀 B 样条” 来表示一条连续轨迹：

    • 假设有 $N+1$ 个控制点 $\{Q_0, Q_1, \dots, Q_N\}$，以及一个节点向量 $[t_0, t_1, \dots, t_{M}]$，其中 $M = N + p_b + 1$。

    • 对于均匀 B 样条，所有相邻结点间的间隔 $\Delta t = t_{m+1} - t_m$ 都相等。

    • 样条曲线在时间 $t \in [t_{p_b},\,t_{M-p_b}]$ 内定义，利用矩阵形式：

      $$ p(s(t)) \;=\; s(t)^\top\,M_{p_b+1}\,q_m, \quad s(t) = \bigl[1,\,s(t),\,s(t)^2,\,s(t)^3\bigr]^\top, \quad q_m = \bigl[Q_{m-p_b},\,Q_{m-p_b+1},\,\dots,\,Q_m\bigr]^\top, $$

      其中 $s(t) = (\,t - t_m\,)/\Delta t$，而 $M_{p_b+1}$ 是由样条阶数 $p_b$ 决定的常数矩阵。

    论文中并没有专门用一段文字来描述“如何把搜索到的离散状态直接转换为控制点”，但可以从如下几处结合理解：

    • 图 5（论文第 5 页）里，就展示了“红色曲线”为由路径搜索得到的初始离散轨迹，而“黄色点”便是初始设置好的B 样条控制点。也就是说，作者直接将那条初始离散路径上的各个节点（通常是路径搜索中生成的关键状态的空间坐标）当作 $Q_i$；并将端点及其最近的 $p_b=3$ 个控制点固定不动，以保证边界条件（起点、终点的位置和速度）不变，然后用“均匀结点分布”去构造这 $N+1$ 个控制点的节点向量，从而得到一条初始的三次均匀B 样条曲线。

    • 在第 IV-C 节中，作者提到：

      “对于由 $N+1$ 个控制点定义的 $p_b$ 次 B 样条轨迹 $\{Q_0, Q_1, \dots, Q_N\}$，我们优化 $N+1 - 2p_b$ 个控制点中的子集 $\{Q_{p_b},\,\dots,\,Q_{N-p_b}\}$。第一个和最后一个 $p_b$ 个控制点不予改变，因为它们决定了边界状态。” 由此可知，这些控制点本身就是从初始路径（路径搜索结果）中选出来的状态坐标，并按照均匀间隔的结点构造 B 样条后就成为了 $Q_i$ 的初始值，方便后续基于凸包性质和距离场梯度的优化。

  3. 端点和首尾各 3 个控制点保持固定，内部控制点参与优化

    • 由于用的是三次 B 样条 ($p_b = 3$)，所以共有 $N+1$ 个控制点，其中 $\{Q_0, Q_1, Q_2\}$ 和 $\{Q_{N-2}, Q_{N-1}, Q_N\}$（共 $2\,p_b = 6$ 个）用于确保起点和终点的边界条件（位置、速度等）不被改变。
    • 剩下的 $\{Q_3, Q_4, \dots, Q_{N-3}\}$ 则是根据初始离散路径直接对号入座、等距安置后生成的，作为可优化的内部控制点，在 “弹性带 + 碰撞成本 + 速度/加速度软限制” 的混合目标函数下反复迭代调整，使得最终的轨迹既平滑又远离障碍且满足动态可行性。

总结：

  • 初始控制点 就是“动力学路径搜索”阶段输出的离散路径上的各个关键状态（主要是位置坐标）——直接当作 B 样条的 $\{Q_i\}$。
  • 结点向量 按 “三次均匀 B 样条” 的标准方式构造，所有相邻结点等间隔；由于样条阶数 $p_b=3$，节点数满足 $M = N + p_b + 1$。
  • 首尾各 $p_b=3$ 个控制点保持固定（以保证边界条件不变），剩余的内部控制点在后端优化中迭代更新，以获得更平滑、更安全且满足速度/加速度约束的最终轨迹。

B样条控制点的处理

在论文中的确是先将动力学路径搜索得到的离散轨迹上的关键状态（即位置坐标）直接用作 B 样条的初始控制点，但这仅仅是“控制点的空间位置来源”；为了保证所有凸包都无碰撞，还需要对这些控制点之间的几何关系（即相邻控制点的连线长度）进行额外约束。具体来说：

  1. $r_{j,j+1}$ 的含义 记连续四个控制点为 $Q_1, Q_2, Q_3, Q_4$，任意位于它们对应凸包内的一点记为 $Q_h$。通过三角不等式可知，对于某个障碍物所在体素，与 $Q_h$ 之间的最小距离满足

    $$ d_h \;>\; d_c \;-\; r_h, $$

    其中

    • $d_c$ 是该体素与“任意一个控制点”之间的距离（取四个控制点中最近的那个距离），
    • $r_h$ 则是从那个控制点到凸包内任意一点 $Q_h$ 的最大可能距离。 由于点 $Q_h$ 一定在四个顶点的凸包内，可得到
    $$ r_h \;\le\; r_{12} \;+\; r_{23} \;+\; r_{34}, $$

    其中 $r_{12}=\lVert Q_1 - Q_2\rVert$，$r_{23}=\lVert Q_2 - Q_3\rVert$，$r_{34}=\lVert Q_3 - Q_4\rVert$。 因此，只要满足

    $$ d_h \;>\; d_c \;-\;(r_{12}+r_{23}+r_{34}), $$

    就能保证整个凸包与障碍物无碰撞。在实际推导中，为了让 $d_h > 0$ 恒成立，论文进一步取保守条件

    $$ d_c > 0,\quad r_{j,j+1} < \frac{d_c}{3}\quad(j\in\{1,2,3\}), $$

    这样就可以确保

    $$ r_{12} + r_{23} + r_{34} \;<\; d_c, $$

    进而 $d_h > d_c - (r_{12}+r_{23}+r_{34}) > 0$ ，使得该段凸包里的每一点都离障碍有正距离，保证了安全性。

  2. “动力学路径搜索”得到的离散点为何可以作为控制点

    • 动力学路径搜索输出的离散轨迹之所以能被直接用作 B 样条的初始控制点，是因为路径搜索阶段已经考虑了最低限度的碰撞检测——每个离散状态都至少在体素网格的中心且与周围障碍保持一定距离（也就是初始 $d_c>0$）。

    • 然而，这些离散状态之间的距离（也就是相邻控制点之间的 $r_{j,j+1}$）可能比较大。若直接用作控制点而不作任何处理，可能会出现

      $$ r_{12}+r_{23}+r_{34}\;\ge\;d_c， $$

      导致对应凸包和障碍物发生碰撞。因此，论文中提到“在实践中，只要我们选择 $r_{j,j+1}\,(j\in\{0,1,\dots,N\})$ 非常小（作者实现时取 $r_{j,j+1}<0.2$），轨迹在大多数情况下是安全的”（第 5 页倒数第 2 段）。

  3. 如何让相邻控制点之间的 $r_{j,j+1}$ 足够小 由于 B 样条是按均匀时间节点（$\Delta t$）来生成的，公式（7）中：

    $$ V_i = \frac{Q_{i+1}-Q_i}{\Delta t},\quad A_i = \frac{V_{i+1} - V_i}{\Delta t}, $$

    与速度、加速度的约束一同隐式决定了“在相邻两个控制点之间采样的时间间隔 $\Delta t$”以及它们之间的空间距离——$\lVert Q_{i+1} - Q_i\rVert$。如果直接沿用路径搜索给出的“稀疏”离散点，$\Delta t$ 会偏大，导致 $\lVert Q_{i+1} - Q_i\rVert$ 过大，从而可能违反 $r_{j,j+1}

    1. 保持“初始控制点位置”与离散路径一致，只要路径搜索输出的相邻离散点本身就满足一定距离（一般情况下搜索时就留有空间留白）。
    2. 如果该距离仍然太大，可以通过“缩小$\Delta t$”或“对离散路径作等距插值”，在原有两点之间插入若干补偿点，使得新产生的每对相邻控制点之间的欧氏距离都满足 $r_{j,j+1}
    3. 论文中提到 “即便在极端情况下，如果环境非常复杂，我们也可以重新参数化 B 样条来选择更小的 $r_{j,j+1}$，从而仍然满足式 (8)”（第 5 页倒数第 2 段）。换言之，唯一的要求是“相邻控制点之间不能隔得太远”，而并不是“绝对不能使用动力学路径搜索给出的原始离散点”。原始离散点能否直接作为控制点，一方面要看它们与障碍物之间的最小距离 $d_c$；另一方面要看相邻点之间的距离是否足够小。

  4. 最后如何保证所有凸包都无碰撞

    • 一旦在“每个控制点与障碍物”的距离 $d_c$ 恒为正，并且通过调节采样（或插值）使得所有相邻控制点间的距离都满足

      $$ r_{j,j+1} < \frac{d_c}{3}, $$

      那么就能确保每一个 $\{Q_i, Q_{i+1}, Q_{i+2}, Q_{i+3}\}$ 这四个连续控制点所张成的凸包（对应一个 B 样条片段）中任意一点，都与障碍物有正距离，从而“所有凸包均无碰撞”。

    • 在后端优化时，若任意一段凸包存在碰撞风险（比如中途某个凸包的四个顶点太靠近障碍物，导致后续插值点也可能进入障碍），碰撞代价项 $f_c$ 会把这些控制点沿梯度方向往远离障碍物的方向“拉”出来，同时维持相邻点间的间距。经过几轮迭代，必然能最终让整个轨迹的所有凸包满足无碰撞条件。

总结：

  • 控制点本身可以直接来源于动力学路径搜索的离散状态。 初始阶段只要保证它们与障碍物之间有正的最小距离 $d_c>0$，即可满足凸包安全的第一条件。
  • 为了满足式 (8) 中的 “$r_{j,j+1} < d_c/3$”，需要保证每对相邻控制点之间的欧式距离都足够小。这通常是通过“选择较小的时间间隔 $\Delta t$”或“对离散路径做等间距插值”来完成，而并不是放弃动力学路径搜索输出的原始点，而是对它们进行重新参数化或加密采样。
  • 最终效果： 在满足上述条件后，再通过后端的凸包约束式优化，便可确保“所有 B 样条凸包”都与环境中的障碍物保持正距离，从而得到一条既平滑又安全的连续轨迹。

最终轨迹与离散路径点的关系

不是完全“经过”那些原始离散路径点。原因在于：

  1. B 样条本身的性质 对于三次（degree = 3）均匀 B 样条来说，除非采用“节点打结”或特意设置插值条件，否则曲线并不会在每个控制点处通过（只有在两端端点／打结点时才会保证路径经过）。换句话说，即使你把动力学路径搜索得到的离散点 $P_0,P_1,\dots,P_N$ 直接作为初始控制点 $\{Q_i\}$，曲线也只是“包围”这些点，而不是一定在它们上面。

  2. 优化会修改内部控制点 论文中为了满足安全性，需要满足

    $$ r_{j, j+1}=\left\|Q_j-Q_{j+1}\right\|<\frac{d_c}{3}, $$

    并且在优化过程中，通过凸包无碰撞约束和代价函数，会迭代地调整内部控制点的位置。

    • 首尾各 3 个控制点（对应起点和终点的位姿边界）通常会被“锚定”在原始路径的起始位置和速度／加速度条件上，但不保证曲线在这些锚点 “插值” 经过。
    • 中间的控制点会因为要远离障碍并且保持平滑，向更优的位置移动。所以最终的控制点序列 $\{Q_i\}$ 与初始从路径搜索得到的点集合就已经不完全相同了。

  3. 最终轨迹与离散路径点的关系

    • （a）起点与终点：论文会通过打结（clamped knot）或者直接固定端点控制点，保证 B 样条曲线在时间 $t_0$ 处“起点”位置和时间 $t_f$ 处“终点”位置与动力学路径搜索的起终状态一致。这样才能保证飞行器从搜索得到的起点出发、到达搜索得到的终点。
    • （b）中间状态：除非刻意对某个中间离散点打上插值结点，否则中间点并不会被强制经过，反而会被视为“参考”——在后端优化时，系统会拉扯它们，让它们既满足“远离障碍”、又满足“平滑连续”的要求，这样一来，中间很多原始离散点就变成了“优化变量”而非“必须通过的点”。

  4. 一句话概括 最终生成的 B 样条轨迹，仅保证：

    • 在起点和终点处匹配搜索得到的起/终状态（位置、速度等边界条件），

    • 中间控制点则会因为凸包碰撞约束和各种代价项而被移动、插值或加密，确保整个曲线既平滑又安全。 因此，除了起点和终点外，B 样条曲线不会逐点“穿过”原来动力学路径搜索的那些离散点。它们只是作为初始“参考”给出了一个可行的安全通道，后续会被优化推得一个更优的平滑路径。

      不是完全“经过”那些原始离散路径点。原因在于：

  5. B 样条本身的性质 对于三次（degree = 3）均匀 B 样条来说，除非采用“节点打结”或特意设置插值条件，否则曲线并不会在每个控制点处通过（只有在两端端点／打结点时才会保证路径经过）。换句话说，即使你把动力学路径搜索得到的离散点 $P_0,P_1,\dots,P_N$ 直接作为初始控制点 $\{Q_i\}$，曲线也只是“包围”这些点，而不是一定在它们上面。

  6. 优化会修改内部控制点 论文中为了满足安全性，需要满足

    $$ r_{j,j+1} = \lVert Q_j - Q_{j+1}\rVert < \frac{d_c}{3}\,, $$

    并且在优化过程中，通过凸包无碰撞约束和代价函数，会迭代地调整内部控制点的位置。

    • 首尾各 3 个控制点（对应起点和终点的位姿边界）通常会被“锚定”在原始路径的起始位置和速度／加速度条件上，但不保证曲线在这些锚点 “插值” 经过。
    • 中间的控制点会因为要远离障碍并且保持平滑，向更优的位置移动。所以最终的控制点序列 $\{Q_i\}$ 与初始从路径搜索得到的点集合就已经不完全相同了。

  7. 最终轨迹与离散路径点的关系

    • （a）起点与终点：论文会通过打结（clamped knot）或者直接固定端点控制点，保证 B 样条曲线在时间 $t_0$ 处“起点”位置和时间 $t_f$ 处“终点”位置与动力学路径搜索的起终状态一致。这样才能保证飞行器从搜索得到的起点出发、到达搜索得到的终点。
    • （b）中间状态：除非刻意对某个中间离散点打上插值结点，否则中间点并不会被强制经过，反而会被视为“参考”——在后端优化时，系统会拉扯它们，让它们既满足“远离障碍”、又满足“平滑连续”的要求，这样一来，中间很多原始离散点就变成了“优化变量”而非“必须通过的点”。

  8. 一句话概括 最终生成的 B 样条轨迹，仅保证：

    • 在起点和终点处匹配搜索得到的起/终状态（位置、速度等边界条件），
    • 中间控制点则会因为凸包碰撞约束和各种代价项而被移动、插值或加密，确保整个曲线既平滑又安全。 因此，除了起点和终点外，B 样条曲线不会逐点“穿过”原来动力学路径搜索的那些离散点。它们只是作为初始“参考”给出了一个可行的安全通道，后续会被优化推得一个更优的平滑路径。

关于插值出来的控制点

中间的那些控制点并不是凭空“新生成”的，也不是完全沿用原始路径搜索得到的稀疏点，而是从原始离散路径出发，经过以下两个步骤得到并不断被“拉扯”优化的：

  1. 初始阶段：由离散路径点决定或插值得到的一系列控制点

    • 如果动力学路径搜索得到的离散点本身相邻距离就足够小（满足 $r_{j,j+1}
    • 如果相邻两点距离过大，就需要在它们之间插入若干“辅助点”（等距插值），使得每一对相邻控制点之间的距离都不超过阈值。插入完成后，这条由原始点＋新插值点组成的序列，就构成了最开始用于 B 样条的“控制点集”。

    换句话说，初始控制点要么就是“原始路径点”，要么是在两原始路径点之间新增的插值点，目的都是为了保证相邻两点的间距足够小，从而让每一个四点凸包都有足够的安全裕度。

  2. 优化阶段：对这些初始控制点进行“拉扯”

    • 在保证首尾各三个控制点固定（以匹配起终点位置／速度边界）的前提下，剩下的中间控制点就是自由变量。
    • 优化算法会在每次迭代里，根据“凸包无碰撞”约束和“平滑性＋动力学可行性”代价函数，把这些中间控制点沿梯度方向往更安全、更平滑的地方“拉扯”。
    • 因此，即便最开始某个中间控制点就是原始路径搜索的输出点（或插值得到的点），在优化完成后，这个控制点的位置通常已经被移动到一个新的位置上，以满足碰撞距离和速度／加速度约束。

举例说明

  1. 路径搜索输出： 假设路径搜索给出了 $\{P_0,P_1,P_2,P_3\}$ 四个离散点，其中 $P_0,P_3$ 分别是起点和终点，二者与障碍物保持了最小距离 $d_c>0$，但中间的 $P_1,P_2$ 离得太远，令 $\lVert P_1 - P_2 \rVert > d_c/3$。

  2. 插值（如有必要）： 为了满足 $r_{12} < d_c/3$，我们在 $P_1$ 和 $P_2$ 之间插入一个或多个均匀分布的点（比如在它们中点再插入一个 $P_{1.5}$），使得新的序列可能变成 $\{P_0,P_1,P_{1.5},P_2,P_3\}$，并且每对相邻两点的距离都满足阈值。

  3. 确定初始控制点集： 最终把 $\{P_0,P_1,P_{1.5},P_2,P_3\}$ 作为 B 样条的初始控制点（clamped），其中首尾各三个端点 $P_0,P_1,P_{1.5}$（起点）和 $P_{1.5},P_2,P_3$（终点）的边界会被固定或者用打结使得曲线通过；中间的一个点 $\{P_{1.5}\}$ 是自由的可优化点。

  4. 迭代优化：

    • 在每次迭代里，优化器会检测每个四点凸包与障碍物的距离，如果某个凸包有碰撞风险，就根据凸包中离障碍最近的距离梯度，把这四个控制点里的中间点向更远离障碍的方向“拉”。
    • 同时，也要兼顾曲线的平滑性（高阶导数代价）以及速度、加速度限制，这些都会让原先插入的点不断微调位置，而不再严格位于原始插值的地方。

结论

  • 中间控制点一开始是从离散路径点衍生而来：

    • 直接取路径搜索输出的点，或在相邻输出点之间等距插值生成若干点。

  • 最终它们被不断“拉扯”到新的位置：

    • 优化完后，这些中间控制点往往已经不在原始路径点或插值点的位置，而是被调整到满足凸包安全和轨迹平滑的最优位置。

因此，中间控制点既不是凭空新生成的，也不是一直固定在原有路径点上，而是“基于原始路径点（加插值点）”得到初始值，再经由优化“拉扯”到最终位置。

原始离散路径点本身就在一条直线的情况

如果原始离散路径点本身就在一条直线上，那么把它们（或在它们之间等距插值得到的点）作为三次均匀 B 样条的控制点，生成的曲线在理论上仍然会“紧贴”那条直线，不会莫名其妙地偏离出弯来。原因可以从以下几点来理解：

  1. B 样条在控制点共线时会共线
    • 任何一个三次（degree = 3）的均匀 B 样条，都满足“曲线完全落在它所有控制点凸包之内”这一性质。
    • 如果所有控制点都严格共线，那么这些点的凸包实际上就是那条直线段本身（或者更确切地说，是一段闭合的线段）。只要每个控制点都在同一条直线上，B 样条曲线在几何上就不可能“跑到”凸包之外去——也就必然“躺”在这条直线上。
    • 换句话说，当你把一系列严格共线且等距分布的点当作控制点时，均匀三次 B 样条得到的曲线就恰好是那条直线段（如果首尾打结，始终会从第一个控制点开始，沿着直线到最后一个结束；如果不打结，对内间段也是严格共线，不会出现弯曲）。
  2. 哪怕 B 样条“不插值”中间点，依然不弯曲
    • 有人会担心：“B 样条本身并不保证经过所有中间控制点，只保证落在凸包内；那不会拐弯吗？”
    • 但如果所有控制点都在同一条直线上，那么“落在凸包内”就意味着“落在那条直线上”。B 样条虽然不一定穿过每个中间点，但在控制点共线的情况下，它只是“沿着直线”去逼近控制点之间的连线，不会出现弯曲。
    • 事实上，你可以把三次 B 样条想象成对控制点多次做线性组合：当所有控制点共线时，无论怎么线性组合，结果都落在同一条线上。
  3. 优化阶段如果空间足够宽松，也不会把点“拉”离直线太远
    • 在宽松自由空间里，只要原始离散点与障碍物距离 $d_c$ 远远大于相邻两点距离之和，就没必要对点做额外插值，也没必要再“拉扯”它们远离障碍。
    • 此时凸包碰撞代价几乎为零，优化目标里与“碰撞”相关的梯度项也几乎为零。唯一剩下的主要代价是“高阶导数平滑项”（即让轨迹更圆滑、让加速度／抖动更小）。而如果控制点本身已经等距共线，那么任何想要让它“再圆滑一点”的微调，最多也就是让中间控制点在那条直线上做非常微小的平移（比如为了保持加速度连续，可能会把它们在线上不同位置略微移动），但并不会把它们拉出直线。整个曲线仍然会是一条几乎与原始直线重合的直线段，仅仅在数值上做微小调整以保证速度／加速度的连续性和平滑性而已。
    • 简而言之，当你在完全开阔、无遮挡的长直通道里用 B 样条表示飞行轨迹：
      1. 如果原始路径离障碍很远，就直接用那些共线点生成 B 样条；
      2. B 样条会原封不动地“躺”在那条直线上（首尾端点相同，中间绝不会往外拐）；
      3. 后端优化只会在那条直线方向上做极小的位移，目的是让速度/加速度曲线更平滑，但仍然保持共线状态。
  4. 举个直观的例子
    • 假设自由空间是一条长走廊，路径搜索给你的一系列点是 $(0,0), (1,0), (2,0), (3,0)$。它们都在 $x$-轴上等距分布。
    • 如果把这四个点当作 B 样条的控制点，而且不做任何与障碍物相关的拉扯，B 样条曲线就会从 $(0,0)$ 开始，一直沿着 $x$-轴到 $(3,0)$ 结束，中间不会偏离到 $y\neq0$ 的方向。即使后端为了让速度／加速度更平滑而对中间两个点做轻微调整，也绝对不会把它们从 $(1,0)$、$(2,0)$ 往 $y\neq0$ 的位置“拽”掉去——因为这样反而会引入不必要的曲率成本，也违背了“只要空间宽松就不拉向障碍”这一原则。
    • 因此，得到的最终轨迹看起来依然跟一条直线几乎没有区别，只是数值上可能在 $x$-方向微微摆动，确保飞行器的速度、加速度在换段时不会瞬间跳变。

总结

  • 控制点若共线，那么三次均匀 B 样条生成的曲线也必然共线，因此“不会改变直线的本质”。
  • 在宽松自由空间里，如果原始路径点是一条直线，且它们与障碍物的距离都很大，就完全不需要再额外插值或调整来满足凸包安全条件；此时 B 样条就会“乖乖地”保持与那条直线重合，仅在数值上为了平滑性做极小的同线调整。
  • 因此，你不必担心水平不平直——只要控制点一开始在一条直线上，最终得到的 B 样条轨迹也会是一条接近甚至等同于那条直线的轨迹。

触发插值的条件

触发插值的条件（何时需要在原有离散路径点之间生成新的控制点）

  1. 凸包安全性约束不足

    • 对于任意连续四个控制点构成的三次 B 样条片段，需要满足

      $$ r_{12} + r_{23} + r_{34} \;<\; d_c， \quad d_c>0， $$

      其中 $d_c$ 是该片段中任意一个控制点到最近障碍物的最小距离，$r_{jk}=\lVert Q_j - Q_k\rVert$。

    • 若原始离散路径点（作为初始控制点）中，存在相邻点对 $(P_i,P_{i+1})$ 使得

      $$ \|P_i - P_{i+1}\| \;\ge\; \frac{d_c}{3}， $$

      则会导致某些四点凸包之内可能与障碍物发生碰撞，这时就需要在它们之间插入新的点，使得对于任意相邻控制点序列都满足

      $$ r_{j,j+1} < \frac{d_c}{3}\,. $$

  2. 动力学可行性与平滑性需求

    • 当相邻离散路径点之间的空间距离过大时，直接用它们作为控制点生成的 B 样条会出现速度或加速度突变，导致轨迹不够平滑或超出飞行器的动力学约束。即使在宽松空间中与障碍物距离充裕，也常常会因“速度／加速度上限”而要求对原始路径点进行重新采样。

    • 若某段两个离散点间的距离 $\lVert P_i - P_{i+1}\rVert$ 高于预设的最大允许控制点间距（由速度限值 $\bar v$、时间间隔 $\Delta t$ 共同决定，通常有

      $$ \lVert P_i - P_{i+1}\rVert \;>\;\bar v \times \Delta t， $$

      或者对应加速度峰值过大），也需要插值得到更密集的控制点，以确保 B 样条曲线在计算速度和加速度时不会出现突变。

如何在原有离散路径点之间生成新的控制点（位置、数量与参数设定）

  1. 确定插值阈值

    • 距离阈值 $r_{\text{max}}$：通常取为 $\min\bigl\{\tfrac{d_c}{3},\,\bar v\,\Delta t\bigr\}$。其中

      • $d_c$：在该段路径上离障碍物的最小距离（由体素距离查询或距离场获得）。
      • $\bar v\Delta t$：根据允许的最大速度 $\bar v$ 与 B 样条均匀节点间隔 $\Delta t$ 确定的最大空间步长。

    • 若相邻离散路径点 $(P_i,P_{i+1})$ 的欧氏距离 $\ell = \lVert P_{i+1} - P_i\rVert$ 满足

      $$ \ell \;\le\; r_{\text{max}}， $$

      则无需插值；否则必须补充若干插值点。

  2. 计算插值点数量

    • 令 $\ell = \lVert P_{i+1} - P_i\rVert$。若 $\ell > r_{\text{max}}$，则需要在这段直线段上等距离插入：

      $$ N_{\text{ins}} \;=\; \Bigl\lceil \frac{\ell}{r_{\text{max}}} \Bigr\rceil \;-\; 1， $$

      其中 $\lceil\cdot\rceil$ 表示向上取整。这样可以保证插完后，任何两相邻控制点之间的距离都不超过 $r_{\text{max}}$。

    • 插值总数 $N_{\text{ins}}$ 可能为 0（无需插值），也可能为 1、2、3…，直至将区段分割成若干长度均不大于 $r_{\text{max}}$ 的子段。

  3. 确定插值点位置

    • 若需要插值 $N_{\text{ins}}$ 个点，则第 $k$（从 1 到 $N_{\text{ins}}$）个插值点位置为

      $$ P_i^{(k)} \;=\; P_i \;+\; \frac{k}{\,N_{\text{ins}}+1\,}\,(P_{i+1} - P_i)\,, \quad k=1,2,\dots,N_{\text{ins}}. $$

    • 这些新点都在线段 $[P_i,P_{i+1}]$ 上等距分布，保证每对相邻点的距离约为 $\dfrac{\ell}{\,N_{\text{ins}}+1\,}$，从而不超过 $r_{\text{max}}$。

  4. 更新控制点序列

    • 原始离散路径序列为 $\{P_0,P_1,\dots,P_N\}$。插值完成后，将每对 $(P_i,P_{i+1})$ 之间生成的插值点按顺序插入，得到新的控制点序列

      $$ \{\,P_0,\;[\,P_0^{(1)},\,\dots,\,P_0^{(N_{\text{ins},0})}\,],\;P_1,\;[\,P_1^{(1)},\,\dots\,],\;P_2,\dots,P_N\}\,, $$

      其中 $N_{\text{ins},i}$ 表示在 $(P_i,P_{i+1})$ 这对点之间插入的数量。

    • 如果某段不需要插值，则对应的 $N_{\text{ins},i}=0$，序列里直接连 $P_i,P_{i+1}$。

  5. 时间间隔 $\Delta t$ 与节点向量设置

    • B 样条是“均匀”节点的，所以在插值后，通常对整个序列规定一个常数时间间隔 $\Delta t$，即每个控制点对应时刻 $t_i = t_0 + i\,\Delta t$。
    • 如果对不同区间想要采用不同步长，也可在节点向量 $\{t_0,t_1,\dots,t_{M}\}$ 中人为调节某几段间隔，但一般为了实现速度、加速度连续，都选用相同的 $\Delta t$。

  6. 边界与固定约束

    • 起点与终点对应的前 3 个与后 3 个（当 B 样条 degree = 3 时）需要被锚定或打结，以确保轨迹从给定初始位置和初始速度出发，并到达最终位置和速度。
    • 在插值后，如果原始起终离散点不够连续（相邻距离过大），通常也会在起点与第一个插值点之间“插入”或调整，以确保第 0→1、1→2、2→3 这三点段的安全与动力学约束同样得到满足。

具体示例流程

  1. 计算每对相邻原始路径点的 $d_c$ 与间距 $\ell$

    • 先对每个原始离散点 $P_i$ 查询距离场，得到该点到最近障碍物的距离 $d_c^{(i)}$。
    • 对每一对 $(P_i,P_{i+1})$ 计算 $\ell_i = \lVert P_{i+1} - P_i\rVert$。
    • 对应凸包安全和动力学约束，定义阈值 $r_{\text{max}}^{(i)} = \min\bigl\{\tfrac{d_c^{(i)}}{3},\,\bar v\,\Delta t\bigr\}$。

  2. 判断并插值

    • 若 $\ell_i \le r_{\text{max}}^{(i)}$，则无需插值；新的控制点按 $\dots,P_i,P_{i+1},\dots$ 直接排序。
    • 否则，计算 $N_{\text{ins},i} = \lceil \ell_i / r_{\text{max}}^{(i)}\rceil - 1$。
    • 在 $[P_i,P_{i+1}]$ 上均匀插入 $N_{\text{ins},i}$ 个点，位置公式如上。

  3. 整合生成完整控制点序列

    • 比如有三段：

      • $\ell_0$ 满足阈值，无需插值；
      • $\ell_1$ 过大，需要 2 个插值；
      • $\ell_2$ 也过大，需要 1 个插值。

    • 则最终序列为

      $$ \{\,P_0,\;P_1,\;P_{1}^{(1)},\,P_{1}^{(2)},\;P_2,\;P_{2}^{(1)},\;P_3\}. $$

  4. 节点时间与打结

    • 令首尾 3 个控制点分别是 $Q_0,Q_1,Q_2$ 和 $Q_{N-2},Q_{N-1},Q_{N}$，它们被“打结”在节点向量最前与最后，或直接在优化时固定位置与速度边界。
    • 中间其余 $Q_i$ 都作为可优化的内部控制点。插值后，每个 $Q_i$ 对应一个离散时刻 $t_0 + i\,\Delta t$。

  5. 后续优化

    • 将插值后得到的控制点序列输入后端优化，优化目标包括：

      1. 凸包无碰撞约束（保证任意四点凸包与障碍物距离 > 0）；
      2. 平滑度代价（高阶导数连续）；
      3. 速度／加速度软约束（不超出飞行器动态极限）。

    • 优化过程中，如果插值后某些点依然太靠近障碍或速度/加速度不满足要求，会继续沿梯度方向微调控制点，最终得到一个既安全又平滑的 B 样条轨迹。

关键要点总结

  • 触发插值的最核心条件：

    1. 凸包安全性：相邻两点距离 $\ell > d_c/3$ 时，需要插值。
    2. 动力学平滑性：相邻两点距离 $\ell$ 若导致速度 $\ell/\Delta t$ 或加速度跃变超限，也要插值。

  • 如何插值生成新控制点：

    1. 计算每对 $(P_i,P_{i+1})$ 的实际距离 $\ell$ 与阈值 $r_{\text{max}}=\min\{d_c/3,\bar v\,\Delta t\}$。
    2. 当 $\ell > r_{\text{max}}$ 时，令 $N_{\text{ins}}=\lceil \ell/r_{\text{max}}\rceil-1$。
    3. 在直线段 $[P_i,P_{i+1}]$ 中等距插入 $N_{\text{ins}}$ 个点，位置按 $\displaystyle P_i^{(k)}=P_i+\tfrac{k}{N_{\text{ins}}+1}(P_{i+1}-P_i)$。
    4. 将这些插值点与原始路径点按顺序合并，形成新的控制点序列。
    5. 设定统一时间间隔 $\Delta t$ 并打结起终点（前后三个点固定）以满足边界条件。

  • 最终效果：

    • 保证生成的控制点序列中，任意相邻两点距离都不超过 $\min\{d_c/3,\bar v\,\Delta t\}$，从而使得每个四点凸包都与障碍物保持正距离；
    • 同时满足飞行器的速度、加速度限制，使得后续的凸包／成本优化能迅速收敛到一条平滑、安全且动力学可行的 B 样条轨迹。
    • 在自由空间足够宽松、原始点本身已共线且距离较小的情况下，可选择跳过插值直接构造 B 样条，否则按照上述流程插值并再优化。

触发插值条件（含参数定义）

触发插值的条件（何时需要在原有离散路径点之间生成新的控制点）

  1. 凸包安全性约束不足

    • 对于三次 B 样条而言，任意连续四个控制点 $\{Q_j, Q_{j+1}, Q_{j+2}, Q_{j+3}\}$ 构成一个样条片段，对应的凸包必须与障碍物保持正距离。具体地：

      • 记任意一个控制点 $Q_j$ 到最近障碍物（所在体素）的最小距离为

        $$ d_c := \min_{h\in\{\text{该体素中所有点}\}} \|Q_j - x_h\| > 0, $$

        其中 $x_h$ 表示体素内某一点的坐标，$\|\cdot\|$ 表示欧氏距离。

      • 记相邻两个控制点 $Q_j$ 与 $Q_{j+1}$ 之间的欧氏距离为

        $$ r_{j,j+1} := \|Q_{j} - Q_{j+1}\|. $$

      • 为保证凸包内部每一点到障碍物的最小距离 $d_h>0$，推导出只要满足

        $$ r_{j,j+1} < \frac{d_c}{3} \quad\bigl(j=1,2,3\bigr) $$

        则 $r_{12} + r_{23} + r_{34} < d_c$，从而有 $d_h > d_c - (r_{12} + r_{23} + r_{34}) > 0$。

    • 若原始离散路径点 $\{P_i\}$（作为初始控制点）中，存在某对相邻点 $(P_i, P_{i+1})$ 使得它们之间的距离

      $$ \ell := \|P_{i+1} - P_i\| \;\ge\; \frac{d_c}{3}, $$

      则这对点无法直接作为安全的相邻控制点，因为可能导致某个四点凸包与障碍物碰撞，此时就必须在 $P_i$ 与 $P_{i+1}$ 之间插入新的控制点，使得新的相邻距离均满足 $r_{j,j+1} < d_c/3$。

  2. 动力学可行性与平滑性需求

    • 记飞行器允许的最大速度为 $\bar v$，样条使用的均匀时间间隔为

      $$ \Delta t := \text{相邻控制点对应时间戳之差} > 0. $$

    • 若直接将原始路径点 $(P_i, P_{i+1})$ 作为相邻控制点，则对应的最大速度约为

      $$ \frac{\ell}{\Delta t}. $$

      如果 $\ell > \bar v \,\Delta t$，则这段轨迹会超出速度限制，或导致加速度突变、轨迹不够平滑。在这种情况下，也需要在它们之间插入新的控制点，使得每段时间内的速度与加速度都在允许范围内。

如何在原有离散路径点之间生成新的控制点（位置、数量与参数设定）

  1. 定义关键变量

    • $P_i$：原始离散路径点的第 $i$ 个点，$i=0,1,\dots,N$。
    • $\ell_i := \|P_{i+1} - P_i\|$：第 $i$ 对相邻离散点之间的欧氏距离。
    • $d_c^{(i)}$：在区间 $[P_i, P_{i+1}]$ 上，控制点最靠近障碍物的最小距离（通过距离场查询得到）。
    • $\bar v$：飞行器允许的最大速度。
    • $\Delta t$：B 样条使用的均匀时间间隔，所有控制点对应时刻之差均为 $\Delta t$。
    • $r_{\text{max}}^{(i)} := \min\Bigl\{\frac{d_c^{(i)}}{3},\,\bar v\,\Delta t\Bigr\}$：第 $i$ 段允许的最大控制点间距。
    • $N_{\text{ins},i}$：在 $(P_i, P_{i+1})$ 之间需要插入的新控制点数量。

  2. 判断是否需要插值

    • 对于每一对相邻原始路径点 $(P_i, P_{i+1})$：

      1. 计算 $\ell_i = \|P_{i+1} - P_i\|$。

      2. 通过距离场或体素距离查询，得到区间上最小的障碍物距离 $d_c^{(i)}$。

      3. 计算允许的最大间距：

        $$ r_{\text{max}}^{(i)} := \min\Bigl\{\tfrac{d_c^{(i)}}{3},\;\bar v\,\Delta t\Bigr\}. $$

      4. 若 $\ell_i \le r_{\text{max}}^{(i)}$，则直接保留 $P_i$ 与 $P_{i+1}$ 作为相邻控制点，无需插值；否则进入下一步插值计算。

  3. 计算插值数量

    • 当 $\ell_i > r_{\text{max}}^{(i)}$ 时，需要在直线段 $\overline{P_iP_{i+1}}$ 上等距插入若干点，使得任意两相邻点距离不超过 $r_{\text{max}}^{(i)}$。具体插值数量取：

      $$ N_{\text{ins},i} \;=\; \Bigl\lceil \frac{\ell_i}{r_{\text{max}}^{(i)}} \Bigr\rceil \;-\; 1, $$

      其中 $\lceil \cdot \rceil$ 表示向上取整。这样分割后，每段长度约为 $\ell_i / (N_{\text{ins},i}+1)\le r_{\text{max}}^{(i)}$。

  4. 确定插值点位置

    • 若需要插入 $N_{\text{ins},i}$ 个点，则对 $k=1,2,\dots,N_{\text{ins},i}$，第 $k$ 个插值点的位置为：

      $$ P_i^{(k)} \;=\; P_i + \frac{k}{\,N_{\text{ins},i}+1\,}\,(P_{i+1} - P_i). $$

    • 这些点都在 $\overline{P_iP_{i+1}}$ 这条线段上等距分布，保证相邻间距为 $\displaystyle \frac{\ell_i}{\,N_{\text{ins},i}+1\,}\le r_{\text{max}}^{(i)}$。

  5. 构建新的控制点序列

    • 将原始序列 $\{P_0,P_1,\dots,P_N\}$ 按顺序遍历：对于每对 $(P_i,P_{i+1})$，

      1. 如果 $N_{\text{ins},i}=0$，则序列中直接添加 $P_i$（或在上一轮已添加过，就只在最后一次时添加 $P_{i+1}$）。
      2. 若 $N_{\text{ins},i}>0$，则先添加 $P_i$，再依次添加插值点 $\{P_i^{(1)},\dots,P_i^{(N_{\text{ins},i})}$，最后在处理到下一段时再添加 $P_{i+1}$。

    • 最终得到一个新的控制点序列：

      $$ \{\,P_0,\;[\text{插值点}],\;P_1,\;[\text{插值点}],\;\dots,\;P_N\}. $$

  6. 设置时间节点与打结（固定边界）

    • 对插值后得到的控制点序列 $\{Q_0,Q_1,\dots,Q_M\}$（其中 $M = N + \sum_i N_{\text{ins},i}$），采用均匀时间间隔 $\Delta t$，即让第 $j$ 个控制点对应时刻

      $$ t_j = t_0 + j\,\Delta t,\quad j=0,1,\dots,M. $$

    • 当 B 样条次数为 3 时，需要对首尾各 3 个控制点做“打结”（clamped）或在优化阶段固定其位置与速度边界，以保证轨迹在起点 $Q_0$ 处具有给定初始位置与初始速度，在终点 $Q_M$ 处具有给定目标位置与终止速度。

      • 具体地，令 $\{Q_0,Q_1,Q_2\}$ 为首端打结点，$\{Q_{M-2},Q_{M-1},Q_M\}$ 为末端打结点，这些点在后端优化时会被固定或受“端点约束”强制保持与原始边界状态一致。

  7. 后端优化与微调

    • 插值完成后，将控制点序列输入凸包＋成本优化：

      1. 凸包无碰撞约束：确保任意连续四个控制点 $\{Q_j,Q_{j+1},Q_{j+2},Q_{j+3}\}$ 构成的凸包与障碍物距离 $>0$。
      2. 平滑代价（高阶导数）：通过惩罚三次导数或更高阶导数，确保轨迹曲率／加速度变化平滑。
      3. 动力学软约束：对速度 $\|Q_{j+1}-Q_j\|/\Delta t$ 和加速度 $\bigl(\tfrac{Q_{j+2}-2Q_{j+1}+Q_j}{\Delta t^2}\bigr)$ 进行惩罚，确保不超过飞行器的最大速度 $\bar v$ 与最大加速度 $\bar a$。

    • 若插值后某些控制点离障碍物过近，优化会根据凸包碰撞项梯度沿“远离障碍”的方向对中间控制点做微调；若动态约束未满足，则会调整 $\Delta t$ 或对控制点位置做进一步细化。

    • 最终输出一条既满足“每个四点凸包无碰撞”又满足“速度／加速度限制”和“高阶导数平滑性”的连续 B 样条轨迹。

关键变量定义汇总

符号含义
$P_i$原始离散路径点的第 $i$ 个点，$i=0,1,\dots,N$。
$\ell_i$$\displaystyle \|P_{i+1}-P_i\|$，第 $i$ 对相邻离散点之间的欧氏距离。
$d_c^{(i)}$在 $[P_i,P_{i+1}]$ 区间上，任意控制点到最近障碍物的最小距离。
$\bar v$飞行器允许的最大速度。
$\Delta t$B 样条使用的均匀时间间隔，所有控制点对应时刻之差均为 $\Delta t$。
$r_{j,j+1}$$\displaystyle \|Q_j - Q_{j+1}\|$，相邻两控制点之间的欧氏距离。
$r_{\text{max}}^{(i)}$$\displaystyle \min\Bigl\{\tfrac{d_c^{(i)}}{3},\,\bar v\,\Delta t\Bigr\}$，第 $i$ 段允许的最大控制点间距。
$N_{\text{ins},i}$在 $(P_i,P_{i+1})$ 之间需要插入的新控制点数量。
$Q_j$插值及后续优化后得到的 B 样条控制点，第 $j$ 个，$j=0,1,\dots,M$。
$M$插值后控制点总数，$M = N + \sum_i N_{\text{ins},i}$。
$t_j$控制点 $Q_j$ 对应的时间戳，$t_j = t_0 + j\,\Delta t$。

整体流程示例

  1. 计算每段原始路径点到障碍的最小距离与相邻距离

    • 对每对 $(P_i,P_{i+1})$：

      • $\ell_i = \|P_{i+1}-P_i\|$。
      • 通过距离场查询，得到 $d_c^{(i)}$。
      • 计算允许间距 $r_{\text{max}}^{(i)} = \min\bigl\{d_c^{(i)}/3,\;\bar v\,\Delta t\bigr\}$。

  2. 判断并插值

    • 若 $\ell_i \le r_{\text{max}}^{(i)}$，则无需插值；

    • 否则计算

      $$ N_{\text{ins},i} = \Bigl\lceil \frac{\ell_i}{r_{\text{max}}^{(i)}}\Bigr\rceil - 1, $$

      并在直线段 $\overline{P_iP_{i+1}}$ 上等距插入 $N_{\text{ins},i}$ 个点，生成位置为 $\displaystyle P_i^{(k)} = P_i + \tfrac{k}{N_{\text{ins},i}+1}(P_{i+1} - P_i)$，$k=1,\dots,N_{\text{ins},i}$。

  3. 构建新的控制点序列与时间戳

    • 按顺序将 $\{P_0,P_1,\dots,P_N\}$ 及各段生成的插值点合并，得到 $\{Q_0,Q_1,\dots,Q_M\}$。
    • 设定统一时间间隔 $\Delta t$，让第 $j$ 个控制点对应时刻 $t_j = t_0 + j\,\Delta t$。

  4. 打结与固定首尾边界

    • 对于 B 样条次数为 3，将 $\{Q_0,Q_1,Q_2\}$ 和 $\{Q_{M-2},Q_{M-1},Q_M\}$ 作为打结点，以确保起点/终点位置与速度边界满足原始离散路径的要求。

  5. 后端优化

    • 以新的控制点序列为初始值，进行凸包无碰撞约束与成本函数（平滑项、动力学软约束）优化。
    • 若有控制点仍过近障碍，靠凸包碰撞项梯度对中间控制点做“拉扯”；若速度／加速度超限，则调整 $\Delta t$ 或对控制点做适当微调。
    • 最终得到满足所有安全与动力学约束的光滑 B 样条轨迹。

通过以上流程与变量定义，便能清晰地了解：

  • 何时插值：基于凸包安全（$r_{j,j+1}
  • 如何插值：按各段最大允许间距 $r_{\text{max}} = \min\{d_c/3,\bar v\,\Delta t\}$，计算所需插值点数量，并在直线段上等距分布；
  • 最终控制点：原始离散点与插值点合并后，经统一时间间隔打结，再通过后端凸包／代价优化，实现平滑、安全的 B 样条轨迹。

问题的完整定义

问题描述 在无人机自主飞行规划中，我们需要由一个粗糙的离散路径（由动力学路径搜索算法生成）和平面障碍物栅格地图，进一步生成一条既满足安全性又平滑连续的三次 B 样条轨迹。该轨迹最后要被离散化为一系列时空状态点，供无人机按顺序跟随执行。

1. 问题背景与目标

  • 动力学路径搜索（Kinodynamic Path Search）输出：

    • 算法在二维障碍物栅格地图（Occupancy Grid Map）上，采用基于运动原语（Motion Primitives）的混合状态 A* 或类似方法，得到一条可行的离散状态序列。
    • 每个状态包含至少位置 $(x,y)$ 及速度/加速度信息，保证粗略满足动力学可行性并避开障碍。
    • 该序列往往较为稀疏、折线化，且仅在栅格中心或运动原语末端处做了安全检查，无法直接用于飞行器平滑轨迹跟踪。

  • 平面障碍物栅格地图（2D Occupancy Grid）：

    • 将飞行环境离散化为大小为 $M\times N$ 的栅格，每个栅格单元对应实际空间中的一个矩形区域（如 $0.2\times0.2$,m²）。
    • 每个单元由二值标记：占用（Occupied）表示该处有障碍物，不可进入；空闲（Free）表示可通行。
    • 同时可预先计算得到距离场（Distance Field），用于快速查询任意点到最近障碍物的欧氏距离。

  • 生成 B 样条轨迹的需求：

    1. 安全性（Collision‐Free）：

      • 轨迹在任何时刻都不得与栅格地图中标记为占用的单元交叉；等价于在每个三次 B 样条片段的四点凸包内部与任何障碍保持正距离。

    2. 平滑性（Smoothness）与动力学可行性（Dynamical Feasibility）：

      • 保证无人机按照轨迹飞行时，速度和加速度不要出现突变、保持在预设的最大速度 $\bar v$ 和最大加速度 $\bar a$ 范围内；同时让曲线的高阶导数（如角加速度、抖动）尽可能小。

    3. 实时性与简洁性：

      • 在在线规划或近实时重规划场景下，须在有限计算资源和允许的时间窗口内完成 B 样条构建与优化。

    4. 离散化输出（Trajectory Re‐Sampling）：

      • 将连续的 B 样条曲线离散化为固定时间间隔 $\Delta t$ 下的一系列状态点 $(x(t_k),\,y(t_k),\,v(t_k),\,a(t_k))$，供无人机低层控制器逐步跟踪。

2. 问题的输入定义

  1. 初始离散路径 $\mathcal{P}$

    • $\displaystyle \mathcal{P}=\{P_0,P_1,\dots,P_N\}$，其中每个离散点

      $$ P_i = \bigl(x_i,\;y_i,\;v_i,\;a_i\bigr),\quad i=0,1,\dots,N. $$

    • $\{P_i\}$ 来源于“动力学路径搜索”阶段，满足：

      1. 避障可行性：每个 $P_i$ 对应的栅格单元为“Free”；
      2. 动力学约束：相邻状态点间由运动原语连接，保证速度/加速度初步可行。

    • 该离散路径一般为折线连接 $\overline{P_iP_{i+1}}$，轨迹在这些点间并不平滑，且没有凸包安全保证。

  2. 二维障碍物栅格地图 $\mathcal{G}$

    • 大小为 $M\times N$ 的矩阵 $\mathrm{Grid}[u,v]$，其中

      $$ \mathrm{Grid}[u,v] = \begin{cases} 1, & \text{障碍（Occupied）},\\ 0, & \text{空闲（Free）}, \end{cases} \quad u=1,2,\dots,M;\;v=1,2,\dots,N. $$

    • 地图分辨率：每个栅格对应实际长度 $r_{\text{res}}$（如 $0.1$ m）。因此，离散点 $(x_i,y_i)$ 可映射到栅格索引 $\bigl\lfloor x_i/r_{\text{res}}\bigr\rfloor,\;\bigl\lfloor y_i/r_{\text{res}}\bigr\rfloor$。

    • 距离场（Distance Field）：预先计算得到一个同尺寸的矩阵 $\mathrm{DF}[u,v]$，表示该栅格中心点到最近障碍点（标记为 1 的格子）的欧氏距离。实际算法中常用八连通或更精细算法来近似欧氏距离（例如离散欧氏距离变换）。

    • 快速查询接口：给定任意连续空间点 $(x,y)$，可通过双线性插值或最近邻，从 $\mathrm{DF}$ 中得到最近障碍物距离 $d_c = \mathrm{DF}\bigl(\lfloor x/r_{\text{res}}\rfloor,\lfloor y/r_{\text{res}}\rfloor\bigr)$。

  3. 飞行器动力学与约束参数

    • 最大线速度：$\bar v >0$。
    • 最大加速度：$\bar a >0$。
    • B 样条次数（阶数）： $p=3$（三次 B 样条）。
    • 时间间隔：$\displaystyle \Delta t > 0$，用于将最终连续轨迹离散化为等步长时间戳 $\{t_k\}$。

3. 问题的输出定义

  1. 连续三次 B 样条轨迹 $\mathcal{S}(t)$

    • 轨迹由控制点序列 $\{Q_0,Q_1,\dots,Q_M\}$ 及均匀节点向量 $\{t_0,t_1,\dots,t_{M+p+1}\}$ 构成，其中：

      1. 控制点

        $$ Q_j = \bigl(x_j^Q,\;y_j^Q\bigr),\quad j=0,1,\dots,M. $$

      2. 节点向量（均匀打结）

        $$ t_k = \begin{cases} t_0, & k=0,1,\dots,p,\\ t_0 + (k-p)\,\Delta t, & k=p+1,\dots,M,\\ t_M, & k=M+1,\dots,M+p+1, \end{cases} $$

        其中 $p=3$，即打结节点在首尾各重复 $p+1=4$ 次；中央均匀分布，使相邻点时间差为 $\Delta t$。

    • 对应参数化形式：

      $$ \mathcal{S}(t) \;=\; \bigl(x(t),\,y(t)\bigr) \;=\; \sum_{j=0}^{M} N_{j,p}(t)\,Q_j, \quad t\in\bigl[t_0,\,t_M\bigr], $$

      其中 $N_{j,p}(t)$ 为三次 B 样条基函数。

    • 轨迹在物理空间上应满足：

      1. 对任意 $t\in [t_0,t_M]$，点 $\bigl(x(t),y(t)\bigr)$ 必须落在空闲区（对应的栅格值为 0）。
      2. 由控制点构成的任意连续四点凸包 $\mathrm{Conv}\bigl\{Q_j,Q_{j+1},Q_{j+2},Q_{j+3}\bigr\}$ 与障碍物距离 $\mathrm{dist}>0$。

  2. 离散化后用于跟踪的时序状态序列

    • 令 $t_k = t_0 + k\,\Delta t$，这里 $k=0,1,\dots,K$，其中 $K = \bigl\lfloor (t_M - t_0)/\Delta t\bigr\rfloor$。

    • 离散状态包含位置、速度、加速度等：

      $$ S_k = \Bigl(x(t_k),\,y(t_k),\;v_x(t_k),\,v_y(t_k),\;a_x(t_k),\,a_y(t_k)\Bigr). $$

      其中

      $$ v_x(t_k) \;=\; \frac{dx}{dt}\Bigl|_{t=t_k}, \quad v_y(t_k) \;=\; \frac{dy}{dt}\Bigl|_{t=t_k}, $$$$ a_x(t_k) \;=\; \frac{d^2x}{dt^2}\Bigl|_{t=t_k}, \quad a_y(t_k) \;=\; \frac{d^2y}{dt^2}\Bigl|_{t=t_k}. $$

    • 该离散序列应满足：

      1. 速度约束：$\sqrt{v_x(t_k)^2 + v_y(t_k)^2} \le \bar v$。
      2. 加速度约束：$\sqrt{a_x(t_k)^2 + a_y(t_k)^2} \le \bar a$。
      3. 碰撞检测：每个 $\bigl(x(t_k),y(t_k)\bigr)$ 对应的栅格单元必须为“Free”。

  3. 输出格式示例

    • 控制点列表

      $$ \bigl\{\,Q_0 = (x_0^Q,y_0^Q),\;Q_1 = (x_1^Q,y_1^Q),\;\dots,\;Q_M = (x_M^Q,y_M^Q)\bigr\}. $$

    • 节点时间$/$打结信息

      $$ t_0 = 0,\;t_1 = 0,\;t_2 = 0,\;t_3 = 0,\;t_4 = \Delta t,\;t_5 = 2\Delta t,\;\dots,\;t_{M+1}=t_M,\;t_{M+2}=t_M,\;t_{M+3}=t_M,\;t_{M+4}=t_M. $$

    • 离散跟踪点列

      $$ S_0,\,S_1,\,\dots,\,S_K, \quad S_k = \Bigl(x(t_k),\,y(t_k),\,v_x(t_k),\,v_y(t_k),\,a_x(t_k),\,a_y(t_k)\Bigr). $$

    • 通常将上述数据按时间顺序存储为一个二维数组或消息队列，供飞控系统按帧发送给无人机底层控制器。

4. 整体流程概述

  1. 输入

    • 初始离散轨迹 $\mathcal{P} = \{P_0,\dots,P_N\}$，每个 $P_i=(x_i,y_i,v_i,a_i)$。
    • 障碍物栅格地图 $\mathcal{G}\in\{0,1\}^{M\times N}$、分辨率 $r_{\text{res}}$，以及对应的距离场 $\mathrm{DF}[u,v]$。
    • 飞机动力学参数 $\bar v,\bar a$，B 样条阶数 $p=3$，时间步长 $\Delta t$。

  2. 初步检查与插值

    • 对每对相邻 $(P_i,P_{i+1})$：

      1. 计算间距 $\ell_i = \|P_{i+1}-P_i\|$。

      2. 查询距离场，得到该段的最小障碍距离 $d_c^{(i)}$。

      3. 设定允许最大间距 $r_{\text{max}}^{(i)} = \min\bigl\{\tfrac{d_c^{(i)}}{3},\,\bar v\,\Delta t\bigr\}$。

      4. 若 $\ell_i > r_{\text{max}}^{(i)}$，则根据

        $$ N_{\text{ins},i} = \left\lceil \frac{\ell_i}{r_{\text{max}}^{(i)}}\right\rceil - 1 $$

        在直线段上等距插入 $N_{\text{ins},i}$ 个点 $\bigl\{P_i^{(1)},\dots,P_i^{(N_{\text{ins},i})}\bigr\}$。

    • 合并所有 $P_i$ 与插值点，得到新的初始控制点序列 $\{Q_0,\dots,Q_M\}$。

  3. 构造三次 B 样条轨迹

    • 对 $\{Q_j\}$ 设定均匀节点向量 $\{t_k\}$，前后各打结 4 次。
    • 表示出连续轨迹 $\displaystyle \mathcal{S}(t) = \sum_{j=0}^{M} N_{j,3}(t)\,Q_j$，并对任意 $t\in[t_0,t_M]$ 计算位置、速度、加速度表达式。

  4. 后端凸包安全与优化

    • 对每个连续四点 $\{Q_j,Q_{j+1},Q_{j+2},Q_{j+3}\}$ 构成的凸包，保证与障碍物距离 $>0$。如果存在冲突，利用距离场梯度对中间控制点做微调。
    • 同时最小化“高阶导数平滑项” 与“速度/加速度软约束”，保证轨迹动力学可行。
    • 迭代优化直至收敛，输出最终控制点 $\{Q_j^*\}$。

  5. 离散化输出

    • 令 $t_k = t_0 + k\,\Delta t$，$k=0,\dots,K$，计算

      $$ S_k = \Bigl(x(t_k),\,y(t_k),\;v_x(t_k),\,v_y(t_k),\;a_x(t_k),\,a_y(t_k)\Bigr). $$

    • 在每个时刻 $t_k$ 进行碰撞检查，确保 $\bigl(x(t_k),y(t_k)\bigr)$ 所在栅格为“Free”；若越界或碰撞异常，则需要退回第 3 步重新微调。

  6. 最终输出

    • 控制点列表 $\{Q_0^*,\,Q_1^*,\,\dots,\,Q_M^*\}$ 及对应节点向量。
    • 离散跟踪点序列 $\{\,S_0,\,S_1,\,\dots,\,S_K\}$，其中每个 $S_k=(x(t_k),\,y(t_k),\,v_x(t_k),\,v_y(t_k),\,a_x(t_k),\,a_y(t_k))$。
    • 该序列即可作为无人机轨迹跟踪器的输入，使无人机能够按时序精准执行并完成避障。

总结

  • 输入：

    1. 初始离散路径 $\mathcal{P}=\{P_i\}$，包含 $(x_i,y_i,v_i,a_i)$。
    2. 二维障碍物栅格地图 $\mathcal{G}$（占用/空闲信息）及其距离场 $\mathrm{DF}$。
    3. 飞机动力学限值 $\bar v,\bar a$、B 样条阶数 $p=3$、时间步长 $\Delta t$。

  • 输出：

    1. 连续三次 B 样条轨迹 $\mathcal{S}(t)$，由控制点 $\{Q_j^*\}$（共 $M+1$ 个）和均匀打结节点构成。
    2. 离散化后供跟踪的时序状态序列 $\{S_k\}$，满足速度/加速度不超限且与障碍无碰撞。

  • 核心步骤：

    1. 从原始离散路径点出发，基于凸包安全$\bigl(r_{j,j+1} < d_c/3\bigr)$与动力学 $\bigl(\ell_i \le \bar v\,\Delta t\bigr)$ 判断插值；
    2. 在必要处等距插值，生成新的初始控制点；
    3. 构造三次 B 样条并进行凸包无碰撞与动力学平滑性优化；
    4. 对最终轨迹按 $\Delta t$ 离散化，输出位置、速度、加速度等状态。

以上即为从“初始离散路径 + 障碍物栅格地图”到“平滑安全的 B 样条轨迹及其离散化跟踪点序列”的完整问题定义、输入与输出说明。

固定翼无人机的附加约束

以上是针对四旋翼无人机的B样条轨迹生成问题的解析，但针对固定翼无人机以上算法需要做一些针对固定翼飞行器动力学的改动。具体来说：

  1. 最低速度与不可悬停

    • 固定翼无人机必须保持一定的前向速度 $\bar v_{\min}>0$，不能像多旋翼那样“停住”或原地悬停。因此，B 样条轨迹中不应出现速度为零的点。

    • 在插值与优化时，要保证对任意时刻 $t$，输出的速度 $v(t)=\sqrt{\dot x(t)^2 + \dot y(t)^2}$ 满足

      $$ \bar v_{\min} \;\le\; v(t)\;\le\; \bar v_{\max} $$

      而不是单纯地 $\|v(t)\|\le\bar v$（多旋翼常用的双向速度约束）。

    • 实现方法：

      1. 对于控制点 $\{Q_j\}$，除了约束 $\|Q_{j+1}-Q_j\|/\Delta t \le \bar v_{\max}$ 外，还需要约束 $\|Q_{j+1}-Q_j\|/\Delta t \ge \bar v_{\min}$。
      2. 在速度较低（接近 $\bar v_{\min}$）的区段，尽量不要让相邻控制点距离过小，否则 $\|Q_{j+1}-Q_j\|/\Delta t$ 会降到零甚至小于 $\bar v_{\min}$。这会触发插值时“不要插太密”，反而要保证每段曲线都有足够“拉长”才能维持最低速度。

  2. 曲率（转弯半径）约束

    • 固定翼无人机存在最小转弯半径 $R_{\min}$，对应轨迹的最大曲率 $\kappa_{\max} = 1/R_{\min}$。

    • 对于三次 B 样条，曲率在 $t$ 处可通过

      $$ \kappa(t) \;=\; \frac{|\,\dot x(t)\ddot y(t) - \ddot x(t)\dot y(t)\,|}{\bigl(\dot x(t)^2 + \dot y(t)^2\bigr)^{3/2}} $$

      计算。需要在优化时附加不等式约束

      $$ \kappa(t) \;\le\; \kappa_{\max},\quad \forall\,t\in[t_0,t_M]. $$

    • 离散化约束也可近似为对每个三点（或五点）段的曲率做限值：

      • 比如对每个连续三点 $\{Q_j,Q_{j+1},Q_{j+2}\}$ 计算离散曲率 $\kappa_j$，确保 $\kappa_j \le \kappa_{\max}$。
      • 或在优化时加入“最小转弯半径”惩罚项，让相邻控制点在几何上不要形成过大曲率。

  3. 不可侧向滑行（非完整可控）

    • 固定翼通常是非完整可控系统，不能像多旋翼那样直接朝任意方向加速/减速。B 样条优化里要保证：

      1. 轨迹切线方向要始终与飞行器机头方向基本一致；
      2. 若使用更精确模型，则在求解中考虑航向角 $\psi(t)$ 与横摆角速率限制。

    • 简化做法：在平面路径规划阶段，可先忽略横摆约束，仅从几何上保证曲率与速度约束，随后再做航向角调度，让机头逐渐跟踪 B 样条的切线方向。

  4. 插值条件的调整

    • 对于多旋翼，我们在相邻两点距离 $\ell$ 大于 $\bar v\,\Delta t$ 或 $\tfrac{d_c}{3}$ 时插值；

    • 固定翼还需保证相邻两控制点间的距离 $\ell$ 同时满足

      $$ \bar v_{\min}\,\Delta t \;\le\; \ell \;\le\; \bar v_{\max}\,\Delta t, \quad \ell < \frac{d_c}{3}. $$

    • 也就是说，当 $\ell < \bar v_{\min}\,\Delta t$ 时，反而 不应 插值或应适当拉长节点间距，以免速度降到 $\bar v_{\min}$ 以下。最好的做法是选择 $\Delta t$ 或插值密度，使得对于所有相邻点都有

      $$ \bar v_{\min}\,\Delta t \;\le\; \|Q_{j+1}-Q_j\|\;\le\;\bar v_{\max}\,\Delta t, \quad \|Q_{j+1}-Q_j\| < \frac{d_c}{3}. $$

  5. 优化目标的变化 除了原有的“凸包无碰撞”、“高阶导数平滑”、“速度/加速度软约束”之外，还要加上：

    • 最低速度惩罚：对任何时刻速度 $v(t)$ 若低于 $\bar v_{\min}$ 就产生较大惩罚，使优化尽量将控制点往外“拉长”，保证不低于最小速度；
    • 曲率惩罚或约束：如果局部曲率 $\kappa(t)$ 超过最大值，就要加大代价或直接设为不可行；
    • 航向一致性（可选）：若需要考虑机头机尾方向，可在优化中引入航向差 $\psi(t) - \atan2\bigl(\dot y(t),\dot x(t)\bigr)$ 的惩罚。

  6. 输出离散化供跟踪

    • 离散时序点仍然按固定 $\Delta t$ 取样，但是要确保每个时刻的速度都在区间 $[\bar v_{\min},\,\bar v_{\max}]$，加速度在 $[-\bar a,\,\bar a]$ 内，曲率不超过 $\kappa_{\max}$。
    • 如果某个时刻因为曲率过大或速度过低而不符合固定翼动态，就需要回到优化阶段修改控制点或适当增加插值点，让轨迹变得更平缓。

结论

  • 以上这套基于三次 B 样条的路径平滑与凸包安全优化方法，逻辑上可以直接移植到固定翼，它的核心仍然是“从离散路径点插值＋生成 B 样条控制点＋凸包碰撞检查＋动力学约束优化”。

  • 但是，针对固定翼的特点，必须在：

    1. 速度下限 $\bar v_{\min}$、
    2. 曲率上限 $\kappa_{\max}=1/R_{\min}$、
    3. 机头（航向）一致性 等方面做专门的约束与惩罚；同时在插值时避免产生过小的段长，以免速度降到无法维持“前飞”。

只要将上述固定翼特有的最低速度和最小转弯半径约束加入到原本多旋翼版本的“插值判断”和“后端优化目标”中，生成出来的 B 样条轨迹就能满足固定翼飞行器的动力学要求，也能在二维障碍物栅格地图上实现安全避障和平滑跟踪。