【学习笔记007】Fast-Planner | LIU Xiao的读博日志

FrameWork

整体流程：先找一条可行的路线，再把它优化成适合无人机飞行的轨迹。

前端：Hybrid A* 搜索

根据起点、终点和障碍物，搜索一条初始路线。它在搜索时考虑运动状态及运动约束，为后续优化提供初始解。

后端：轨迹优化（Trajectory Optimization）

初始轨迹还不够理想，需要反复执行三个步骤：

计算代价和梯度：评价轨迹是否平滑、是否靠近障碍物等，并计算调整方向。

梯度下降：沿着减小代价的方向调整轨迹参数。

更新曲线：得到新轨迹，再继续计算和优化。

图中回到左侧的箭头，表示这个过程会循环迭代。

后端：迭代时间调整（Iterative Time Adjustment）

除了确定“怎么走”，还要确定“走多快”。调整各段轨迹的时间，使速度、加速度等满足无人机的运动限制。

可以类比开车：先选路 → 把转弯调整得更顺畅 → 安排各路段的车速。

前端

把普通 A* 的“扩展相邻格子”，换成了“按运动模型扩展可行轨迹段”。

对比	普通栅格 A*	图中的动力学搜索
节点记录什么	主要是位置	位置、速度等运动状态
如何扩展	找周围相邻格子	尝试不同加速度，生成一小段运动轨迹
检查什么	是否碰撞	是否碰撞，以及速度、加速度是否满足限制
得到什么	通常是折线路径	考虑运动约束的初始轨迹

运动基元生成

从无人机当前的位置和速度出发，尝试不同的加速度，生成一组短轨迹，作为 A* 搜索的候选分支。这些短轨迹就叫运动基元（motion primitives）。

① 这部分在整个算法里负责什么？

三个关键步骤：

Expand：生成候选运动段，决定“下一步有哪些飞法”。

EdgeCost：计算这一段的代价，评价“这样飞需要付出多少代价”。

Heuristic：估计到终点的剩余代价，帮助搜索选择更有希望的方向。

后面四页主要解释第一个步骤：Expand 如何生成图中的红色分叉曲线。

② 用数学表达轨迹和运动状态

无人机的三维位置写成：

每个方向的位置都可以用关于时间的多项式表示。例如：

对它求导，就得到速度和加速度：

所以，一条位置曲线也包含了对应的速度、加速度信息。这里的是多项式系数。

PPT 接着给出一般形式：

意思是：把位置及其前 阶导数作为状态，把第 n 阶导数作为控制输入。

例如，取 n=2，状态就是“位置＋速度”，输入就是加速度。注意，表示 n··· 阶导数，不是 n 次方。

③ 给定当前状态和控制输入，怎么算未来状态？

PPT 用状态方程表达运动规律：

对 n=2 的情况，它其实就是我们熟悉的：

写成矩阵形式：

其中是三维单位矩阵。这些矩阵只是在表达：位置的变化由速度决定，速度的变化由加速度决定。

下面这个看起来复杂的公式：

就是上述运动方程的解：

第一项：初始状态在零输入下演化到什么状态。

第二项：控制输入在这段时间里带来的额外影响。

这里采用的是用于规划的简化积分链模型，不是包含姿态、电机等因素的完整四旋翼动力学模型。

④ 控制输入有无穷多种，怎么搜索？

加速度可以连续取值，搜索时不可能全部尝试，因此要进行离散采样。

每个轴的输入范围为：

按 PPT 的方式，每个轴取个值，相邻值间隔为。

例如取 r=1，每个轴只尝试：三个轴组合起来，就有：种控制输入。

对每种输入保持一小段固定时间，就能生成一条候选运动段。所以，一个固定持续时间下有：

图中红色分叉就是这种扩展的示意。它们生成后，还要检查整段轨迹是否碰撞、是否满足运动限制，并进行剪枝。

⑤ Fast-Planner 具体用什么状态和输入？

这里明确选择了 n=2：即 六维状态：三个位置分量＋三个速度分量。

控制输入为：

在一个运动段内令加速度保持不变，通用解就简化为：

这就是生成这些短轨迹时最需要掌握的公式。

例如，只看水平面：无人机从 (0,0) 出发，初始速度为，向前预测 1 秒：

尝试的加速度	1 秒后的位置（m）	1 秒后的速度（m/s）
(0,0)	(1,0)	(1,0)
(0,1)	(1,0.5)	(1,1)
(0,-1)	(1,-0.5)	(1,-1)

于是，同一个节点就扩展出了“继续直行、向一侧弯曲、向另一侧弯曲”等候选运动段。每段的末端位置和速度，又成为下一轮搜索的初始状态。

Cost

已经知道“从当前节点可以怎么飞”，接着解决 “这些候选飞法，应该优先选哪个”。

选择依据仍然是 A* 的评价函数：

其中，g(n) 是从起点到该节点的累计代价，h(n) 是从该节点到终点的预计代价。

① 实际代价 g 怎么算？

这里希望无人机既不要加减速过猛，也不要飞得太慢，因此定义：

在 Fast-Planner 的这个模型中，就是加速度：

：惩罚过大的加速度，衡量控制用力程度，不等于实际耗电量。

T：飞行时间。

：时间权重，越大，越倾向于缩短飞行时间。

前面生成的每个运动基元，在持续时间内采用恒定加速度，所以积分变成：

这就是伪代码中的 EdgeCost：新增这一段的代价。

例如，，则：

多段轨迹的代价直接相加。若各段持续时间不同，更一般地写成：

对应伪代码：

意思是：到父节点已经付出的代价，加上飞到新节点这一段的代价。

PPT 将描述为最优轨迹代价；在搜索进行过程中，更准确地说，它是目前找到并记录的到达该节点的最好代价。

② 剩余代价 h 为什么要用三次多项式估计？

普通栅格 A* 常用距离估计剩余代价，但无人机还具有速度。

例如，两架无人机离终点同样远，一架正朝终点飞，另一架正背离终点飞，它们到达终点需要付出的代价通常不同。

因此，这里要同时考虑：

计算思路是：先暂时忽略障碍物，并放宽控制幅值等约束，求一条连接这两个状态的理想轨迹，用它的代价估计剩余代价。

先假设到达时间 T 已知。对任意一个坐标轴，要求：

这就是图中矩阵表达的内容：用起点和终点的位置、速度，确定多项式系数。

对于这个双积分模型，在固定 T、最小化加速度平方积分的条件下，最优解具有如下形式：

求导得到：

也就是说，理想最优连接的加速度随时间线性变化，位置则是三次多项式。PPT 提到的庞特里亚金最小值原理，就是推导这种最优控制形式的方法。

这里不要与上一部分混淆：

用途	加速度形式	位置形式
搜索扩展的单个运动基元	常量	二次多项式
计算启发式的理想连接	随时间线性变化	三次多项式

系数不是另行调节的参数，而是由边界条件和 T 算出来的。令：

则 PPT 中的矩阵公式等价于：

其中可以理解为：如果一直保持当前速度，最终位置还会差多少。

③ 先算给定时间的代价，再找最合适的时间

把刚才的线性加速度代入目标函数：

直接展开积分：

所以正确结果是：

此时只是找到了“规定在 T 秒到达时”的最优代价，还没有找到最佳到达时间。

为什么还要优化时间？以起终点速度都为零的运动为例：

时间太短，需要很大的加速度，控制代价高。

时间太长，带来的时间代价高。

因此还要求：

fast planner的做法，就是先将、代入，再求：

注意：求导时不能把 当作常数，因为它们也依赖 T。

整理后会得到关于 T 的四次方程。比较满足时间条件的候选解以及适用的边界值，选出代价最小的时间。官方实现也采用了四次方程求根和时间下限筛选。

最后，PPT 将这个最小代价用作启发值：

它估计的是理想条件下连接目标的代价，并不意味着这条连接已经避开障碍物、满足全部飞行限制；实际连接是否可用，还需要单独检查。

④ 如何配合？

假设刚扩展出候选节点，算法依次完成：

步骤	作用
`Expand`	从当前状态生成候选运动段，得到
`EdgeCost`	计算新增运动段的代价，更新
`Heuristic`	估计从到目标状态的剩余代价

然后按 PPT 的公式：

给候选节点排序。例如，两个节点目前记录的代价为：

候选节点	已有代价 g	预计剩余代价 h	总评分 f
A	3	8	11
B	5	4	9

算法会优先扩展 B：虽然到达它已经多付出了一些代价，但从它的位置和速度出发，预计完成整段飞行的总代价更低。

后端

B-Spline（B 样条）：用一组控制点表示轨迹，通过移动控制点调整轨迹形状。

Convex Hull Property（凸包性质）：利用控制点的几何范围，约束曲线的位置以及导数。

Problem Formulation（优化问题建模）：把平滑性、避障和运动限制写成代价或约束，求出更合适的控制点。

轨迹表示

用五次多项式表示轨迹

先只看一个方向的位置：

所谓“轨迹参数化”，就是：用有限个参数 ，确定整条连续轨迹。

这页要求物体：

从位置 a 出发，初始速度、加速度均为零；

在时间 T 到达位置 b，最终速度、加速度也为零。

于是有六个边界条件：

六个条件，对应六个未知系数。把多项式及其导数代入，就得到图中的线性方程组：

其中：

：给定的起终点位置、速度和加速度；

：待求的多项式系数；

：代入时间并求导后形成的系数矩阵。

例如，在 t=0 时：

因此马上得到。

全部解出来后，可以写成很整齐的形式：

它描述了一次从静止出发，再平稳停下的运动。

这种表示有什么局限？给定 T>0 后，六个边界条件已经把整条曲线唯一确定了，没有剩余参数再自由调整中间形状。如果中间有障碍物，就需要增加分段或采用具有更多调整自由度的表示。

从“调系数”转向“用点控制曲线”

给定两个点，定义：

这就是线性插值：

参数	曲线上的点
t=0
t=0.5	，即中点
t=1

当 t 从 0 变化到 1，B 就沿着线段从移动到。

这里开始引入一个直观的想法：曲线上的位置，可以由控制点的加权和表示。

此处的 t 首先是一个取值为 [0,1] 的曲线参数。若要表示持续 T 秒的飞行，还需要建立它与实际时间的对应关系。

重复线性插值，就得到弯曲的轨迹

现在有三个控制点。

先在两条相邻线段上，用同一个参数 t 找两个点：

再在线段上，用同一个比例插值：

把 X,Y代入并展开：

这就是二次贝塞尔曲线。图里的比例公式表达的就是：三次取点使用了相同的插值比例 t。这种逐层插值的构造叫作 de Casteljau 算法。

可以把三个控制点理解为：

：起点；

：终点；

：控制曲线向哪里弯、弯多少。

曲线一般不经过中间控制点。例如在 t=0.5 时：

它受到的牵引，但通常不与重合。因此，控制点不一定是轨迹必须经过的航点。

这张图还提前展示了“凸包性质”。在内，三个权重都非负，并且：

所以 B(t) 始终是三个控制点的凸组合，整条曲线不会跑出它们围成的三角形。

这对避障很有用：如果整个三角形都位于安全区域，那么里面的曲线也安全。但仅仅三个控制点没有碰到障碍物，还不足以保证曲线安全——三角形内部仍可能存在障碍物。

贝塞尔曲线

现在完成一个过渡：从“用一整条贝塞尔曲线表示轨迹”，转向“用多段低次多项式组成 B 样条轨迹”。后者更方便进行局部避障和优化。

① 从二次贝塞尔推广到三次、更多次

之前用了三个控制点构造二次贝塞尔曲线。现在增加到四个控制点，就得到三次贝塞尔曲线：

构造方法不变：在相邻点之间按比例插值，再对插值得到的点继续插值，直到只剩一个点 B(t)。

对单段贝塞尔曲线：

控制点数量	通常使用的曲线次数
2 个	一次
3 个	二次
4 个	三次
5 个	四次

统一写成：

其中：

称为 Bernstein 基函数，可以把它理解为：在参数 t 处，第 i 个控制点占多大权重。

② 为什么要转向 B 样条？

贝塞尔曲线的两个特点。

第一，直接增加控制点，通常也就提高了多项式次数。如果想用很多控制点描述复杂环境中的长轨迹，整段多项式会变得很高次。

第二，修改一个控制点，会影响整段曲线的内部形状。

由公式可以直接看出来：如果只移动，则

在 0<t<1 内，这个权重一般不为零，因此影响会遍及整段内部。

例如，只想把轨迹中间一小段推离障碍物，调整一个控制点，却可能连带改变远处的轨迹。

B 样条仍然采用“控制点加权求和”：

但换了一套具有局部支撑性的基函数：它只在有限的参数区间内非零。这是 B 样条定义中的关键性质。

因此：

可以增加控制点和曲线段数，同时保持每段都是三次；

移动一个控制点，只影响附近的若干段曲线。

这里比较的是单段贝塞尔曲线与 B 样条；贝塞尔曲线也可以通过分段拼接来表示长轨迹。

③ 确定一条 B 样条，需要哪三样东西？

第一样：次数 。

例如，表示每个非零节点区间上的曲线都是至多三次多项式。

注意数学术语：degree 是次数，order 通常是次数加一，所以三次 B 样条通常也称四阶 B 样条。

第二样：控制点 。

一共有个，描述曲线的空间形状：

它们通常不是轨迹必须经过的航点。

第三样：节点向量 。

这里的“节点”是参数轴或时间轴上的分界值，不是三维空间中的控制点。

它规定多项式在哪些位置分段，也参与决定基函数的形状。节点按非递减顺序排列，允许重复。

数量关系是：

注意都是从零开始的末尾下标，因此：

例如，6 个控制点构造三次 B 样条，需要：

PPT中标准有效参数范围是：并不是直接使用节点向量的整个范围。两侧额外的节点用于定义边缘处的基函数。

④ 均匀 B 样条怎样计算位置？

“均匀”指图中的节点间隔相同：

它表示各段使用相同的时间间隔，不表示空间控制点等距，也不表示无人机匀速飞行。

计算某一时刻的位置，可以分三步。

第一步，确定时间落在哪一段。

假设：

第二步，把该段时间归一化。 于是，无论当前是哪一段，都用表示段内进度。

例如，该段从 2 秒到 2.5 秒，在 t=2.2 秒时：

第三步，取附近的控制点并计算加权和。

对三次 B 样条，该段只涉及四个控制点：

将计算整理成

右边按行向量形式输出三个位置分量。其中固定矩阵为：

这个矩阵不用死记，它只是把四个基函数的多项式系数集中放在一起。

展开之后，含义更直观：

这就是：当前段的位置，由附近四个控制点按比例混合得到。

例如，在这一段的起点 s=0：

因此，这种均匀 B 样条的段起点一般并不等于某个控制点。

进入下一段时，参与计算的控制点窗口向前移动：

相邻段共享三个控制点，并由 B 样条基函数保证衔接。对于这里没有重复节点的三次均匀 B 样条，位置、速度和加速度在段与段之间连续。

这也解释了它怎样帮助局部避障：保持节点向量不变时，移动一个控制点，只会改变它参与的至多四个相邻区间，远处的轨迹保持原样。

B样条

基函数

这部分在解释 B 样条的基函数 是怎样算出来的。它决定了：在曲线的某个位置，第 i 个控制点应该占多大权重。

先记住整体表达式：

其中，是空间控制点，是一个标量权重。这里的 u 是曲线参数，不是前面动力学模型中的控制输入 。

下面按照“零次 → 一次 → 二次 → 三次”的顺序理解。

① 基函数的下标是什么意思？

以为例：

i：第几个基函数，对应控制点；

p：基函数的多项式次数；

u：当前计算位置的参数。

基函数由节点向量和次数决定，控制点则决定曲线在空间中的形状。

这里采用均匀节点：

相邻节点形成区间：

② 零次基函数：一个区间内开，其他地方关

定义为：

例如：

可以把它理解为一个区间开关：

：，但。

所以零次基函数画出来是一组矩形脉冲。采用左闭右开的区间，是为了避免公共边界同时属于两个区间。

③ 递推公式：用两个相邻的低次函数，构造一个高次函数

Cox–de Boor 递推公式为：

先不用背分母，重点看结构：

因为线性因子乘以 p-1 次多项式，最高就变成 p 次。

例如：

右侧树状结构表达的就是这种依赖关系。若节点重复导致某个分母为零，按约定将对应项取为零。

④ 一次基函数：两个矩形构造一个三角形

取，代入均匀节点：

即：

分区间看，就很简单。

在 [0,1) 内，只有，所以：

在 [1,2) 内，只有，所以：

合起来：

它先从 0 上升到 1，再下降到 0，因此是一个三角形。

相邻的形状相同，只是整体右移一个单位，作用范围变成 [1,3]。

⑤ 二次基函数：两个三角形构造一座圆滑的小山

继续递推：

它可能非零的范围扩大到三个区间：

分别代入一次基函数：

中间一段来自两项共同贡献：

例如在 u=1.5：

所以：

它已经不再有三角形顶端的尖角：函数值和一阶导数在连接处都连续。

⑥ 三次基函数：再递推一次，获得更高的连续性

三次基函数由两个相邻的二次基函数构造：

其中，在均匀节点下：

这次作用范围扩大到 [0,4]，内部由四段三次多项式组成。

例如，在第一个区间内：

因此：

其余区间同样代入即可，不需要单独记忆长长的分段表达式。

在中心 u=2 处：

于是：

所以，单个基函数的峰值不一定是 1。要求“和为 1”的，是有效参数范围内那一整组基函数的权重之和。

对于这里没有重复节点的情况：

次数	单个基函数跨越的节点区间数	节点处的连续性
零次	1	可以跳变
一次	2	函数值连续
二次	3	函数值、一阶导数连续
三次	4	函数值、一阶、二阶导数连续

将三次 B 样条用于时间轨迹时，就能获得连续的位置、速度和加速度。

⑦ 这些基函数怎样控制实际轨迹？

对三次 B 样条，在某个非零节点区间内，最多只有四个基函数非零。因此，即使有很多控制点，计算当前的位置也只需要附近四个：

这些权重非负，并且在有效参数范围内加起来等于 1。因此，当前曲线段位于这四个控制点的凸包内。

更直接地，如果移动一个控制点，曲线的变化就是：

权重大的位置，曲线移动得多；

权重小的位置，曲线移动得少；

权重为零的位置，曲线完全不受影响。

这就是 Fast-Planner 能通过移动少量控制点调整局部轨迹的数学原因。上一部分的固定矩阵，则是把这些三次基函数在一个均匀区间内展开、整理后的计算形式。

控制点

一段 B 样条轨迹由哪些控制点决定，以及怎样用这些控制点计算位置。

① 当前段使用哪些控制点？

假设当前时刻位于：

对于次 B 样条，这一段涉及个控制点：

因此，三次 B 样条的每一段，由相邻四个控制点决定：

把它们按行排列，得到控制点矩阵：

每个控制点包含 x,y,z 三个坐标，所以是矩阵。

② 将时间转换成“当前段内的进度”

均匀 B 样条每段的时间间隔都是，定义：

其中是当前段的起始时间，不一定是整条轨迹的起始时间。

例如，一段从 2 秒到 3 秒：

t=2：s=0，刚进入这一段；

t=2.5：s=0.5，经过这一段的一半时间；

：，接近段末。

这里的一半是时间的一半，不一定是路程的一半。

③ 用矩阵计算位置

将归一化参数组成：

位置计算写成：

三个部分各司其职：

部分	作用
	表示当前段内的时间进度
	将时间进度转换为四个控制点的权重
	提供四个控制点的空间坐标

例如，在段内中点 s=0.5：

所以：

可以看到，这一时刻的位置主要由中间两个控制点决定，两侧控制点的影响较小。

在 Fast-Planner 的轨迹优化中，固定次数和节点时间后，不变，优化器主要通过调整控制点坐标来改变轨迹。例如，此时将向上移动 1 米，该时刻的轨迹点就向上移动：

同一个控制点在其他时刻的权重不同，因此它对整段轨迹的影响也随时间变化。

性质1

移动一个控制点，只会改变附近一段轨迹，而且改变多少可以直接计算。

① 移动控制点后，曲线如何变化？

原来的曲线为：

假设只移动第k个控制点：

其他控制点、节点向量和次数都保持不变。新曲线中，只有第 k 项发生变化，因此：

得到：

也就是：

这里用表示移动量

例如，将控制点向上移动 1 米：

某处的权重	对应轨迹点的变化
0.6	向上移动 0.6 米
0.2	向上移动 0.2 米
0	完全不动

所以轨迹不是整段平移，而是按照权重大小发生局部形变。

② 为什么只影响附近，而不会影响整条轨迹？

因为 B 样条基函数具有局部支撑性：

在这个范围之外：

因此，移动，只可能影响从到之间的轨迹，最多涉及个节点区间。

对于三次 B 样条，p=3：

③ 红色三角形具体表示什么？

它展示的是三次基函数的递推依赖关系：

最底层四个零次基函数分别对应：

因此，只可能在范围内起作用。移动控制点 ，就只会改变这个参数范围内的曲线。

注意，红色三角形表示的是基函数的依赖关系，不是无人机在空间中的运动范围。

④ 这对 Fast-Planner 的优化有什么帮助？

假设轨迹中间一段离障碍物太近，优化器可以移动影响该段的控制点，让局部轨迹向安全方向调整，远处不受这些控制点影响的轨迹保持不变。

这种关系也方便计算梯度：

它表达的是：已知希望轨迹点向哪里移动，就能计算应该怎样调整相关控制点。不过局部调整后，仍需检查整个受影响区间的碰撞情况和运动约束。

性质2

这部分是在推导：为什么一段三次 B 样条只需要四个控制点，以及固定矩阵是怎样得到的。

它与前一个性质正好是两个观察角度：

前一个性质：固定一个控制点，看它影响哪些曲线段。

现在的性质：固定一个曲线段，看它由哪些控制点决定。

① 为什么一段曲线只涉及四个控制点？

三次基函数只可能在以下范围内非零：

现在固定参数所在的区间：

在这个区间内部，可能非零的三次基函数只有：因此，原来对所有控制点求和的公式，可以缩小为：

例如，对区间，使用的就是：

倒三角形表示：从底部选定的一个区间出发，沿递推关系向上查找，最终只关联到顶部四个控制点。

② 均匀节点带来的便利：所有基函数形状相同，只是位置不同

当节点间隔固定为时，各个三次基函数都是同一个形状的平移。

因此，可以先定义一个作用范围为的标准三次基函数，其他基函数通过参数转换得到：

这里区分两个参数：

s：从当前曲线段起点开始计算的进度；

：从某个基函数作用范围的起点开始计算的位置。

它们的起点不同，因此数值不同。

当前段的归一化参数为：

例如，对第一个相关基函数：

最后一步利用了均匀节点的关系：

四个基函数对应的参数转换为：

当前段内的基函数	标准基函数参数	使用标准函数的哪一段
	s+3	[3,4)
	s+2	[2,3)
	s+1	[1,2)
	s	[0,1)

同一时刻，我们分别取了四个平移基函数的不同部分。

③ 分别求出四个控制点的权重

为了简洁，记：

第一个权重：使用标准函数的最后一段。

标准函数在内为：

代入：

它随 s 增大逐渐减小，表示最左侧控制点的影响逐渐退出。

第二个权重：代入 。

将标准函数在 [2,3) 内的表达式代入并整理，得到：

第三个权重：代入 。

展开过程为：

整理得到：

第四个权重：使用标准函数的第一段。

在内：

代入：

它从零逐渐增大，表示最右侧控制点的影响逐渐增强。

④ 把四个权重合起来，就得到当前曲线段

它们在内都非负，并满足：

因此，该段曲线始终是四个控制点的凸组合，位于它们的凸包内。

⑤ 固定矩阵 从哪里来？

把每个权重按照的顺序排列系数：

幂次
常数项	1	4	1	0
s	−3	0	3	0
	3	−6	3	0
	−1	3	−3	1

这张系数表就是：

所以：

再乘上控制点坐标，就得到位置：

矩阵的每一列，就是一个控制点对应权重的多项式系数。它并不是额外引入的近似，而是基函数公式的等价整理。

⑥ 从当前段进入下一段时，发生什么？

当前段开始时，s=0，权重为：

接近段末时，，权重趋于：

进入下一段后，控制点窗口向前移动一位：

同时，局部参数重新从 s=0 开始。此时的位置为：

恰好等于上一段的末端位置。这样，每一段都能使用同一个矩阵计算，只需更换四个控制点并更新局部参数 s。

性质3

这里重点说明：B 样条可以通过增加控制点，表示更长、更复杂的轨迹，同时保持每段多项式的次数不变。

① 控制点数量与曲线次数可以分开选择

B 样条的表达式是：

其中，两个下标承担不同作用：

n+1：控制点数量；

p：分段多项式的次数。

增加控制点数量，并不要求增大 p。

例如，一直固定 p=3，即使控制点从 5 个增加到 10 个，轨迹仍然是多段三次多项式组成的三次 B 样条，不会因此变成九次多项式。

基函数从矩形、三角形到圆滑曲线的变化，对应的是次数提高；而最后一组图中控制点逐渐增加，主要体现的是曲线段数增加、轨迹得到扩展。这是两种不同的操作。

② 为什么增加控制点，就能增加曲线段？

三次 B 样条的每一段使用相邻四个控制点。

假设最初有五个控制点：

它们形成两个相邻的控制点窗口：

再增加一个控制点，并相应延长节点向量，就可以增加：

新增的是一个三次曲线段，已有曲线段不需要提高次数。

对于这里采用的不重复、等间隔节点，若有 K 个控制点，三次 B 样条的标准有效区间内共有：

控制点数量	曲线段数	每段次数
5	2	三次
6	3	三次
7	4	三次
10	7	三次

③ 红色曲线为什么能不断延长、转弯，甚至形成回环？

绿色点是控制点，灰色折线是控制多边形，红色线是实际曲线。

随着新的控制点被放到不同位置，新的四点窗口会引导后续曲线：

控制点沿某个方向延伸，轨迹就向该方向延长；

控制点改变方向，后续轨迹随之弯曲；

控制点绕回原来的区域，轨迹可能形成回环，甚至自交。

所以，低次多项式也能表示复杂轨迹，复杂性来自多段组合和控制点布局。

不过，平滑并不意味着一定适合飞行。回环或急弯仍可能造成碰撞、过大的速度或加速度，需要进一步检查与优化。

④ 新增的曲线段怎样与前一段平滑连接？

相邻段共享三个控制点，并采用配套的 B 样条基函数。因此，在不重复节点处，三次 B 样条具有：也就是：都连续。参数取为时间时，对应位置、速度、加速度连续；三阶导数 jerk 则仍可能在段间跳变。

对于保持原有节点不变、在末端继续追加均匀节点和控制点的情况，还可以保持原有有效区间上的曲线不变，只向后扩展新的曲线段。

在 Fast-Planner 的轨迹表示中，这种结构使长轨迹仍然可以使用固定的三次基函数和固定矩阵计算。无论共有多少控制点，计算某一段的位置时，依然只需要附近四个控制点。

性质4

B 样条求导后，仍然可以表示成 B 样条，只是次数降低，控制点变成了相邻控制点的差分。

这意味着，描述位置轨迹之后，就能方便地得到速度、加速度，进而检查无人机是否飞得过快、加速是否过猛。

① 先看结果：求导后，什么变了？

原来的曲线为：

它的一阶导数可以写成：

其中：

再求一次导数：

其中：

这里的分别是一阶、二阶导数曲线的控制点。

以三次位置 B 样条为例：

曲线	多项式次数	控制点数量
位置	三次	n+1
一阶导数	二次	n
二阶导数	一次	n-1

只有当 u 就是实际时间时，一阶和二阶导数才直接对应物理速度、加速度。若使用归一化时间，还要补上时间缩放因子。

② 为什么基函数求导后，次数会降低？

关键公式是：

意思是：

一个 p 次基函数的导数，可以由两个相邻的 p-1 次基函数相减得到。

先看最容易理解的一次基函数。对于单位间隔节点：

它是一个三角形，因此在各开区间内，导数为：

恰好对应：

三角形左侧上升，所以导数为正；右侧下降，所以导数为负。尖角处则不具有普通意义下的导数。

③ 中间较长的推导，是在证明一般次数下也成立

证明采用数学归纳法。

先验证 p=1 成立，再假设 p-1 次的求导公式成立。将递推公式简写为：

其中：

使用乘积求导法则：

接着：

将两个低次基函数的导数，替换成归纳假设中的表达式；

合并相同的 p-2 次基函数；

利用递推公式，将它们重新组合成 p-1 次基函数。

最后就得到前面的求导公式。

这里真正需要掌握的是：求导结果仍由低一次的 B 样条基函数构成。长串代数变形是在验证这一点。

④ 为什么导数控制点是“相邻控制点之差”？

对整条曲线求导：

代入基函数求导公式后，把相同基函数的项放在一起。

例如，基函数会收到两份贡献：

从对应的项中，得到正贡献；

从对应的项中，得到负贡献。

合起来：

也就是：

因此：

直观上，在相同的时间尺度下，相邻位置控制点相距越远，就对应越大的速度控制点。

严格地说，将导数单独表示成一条 B 样条时，其节点向量也要去掉原节点向量的首尾各一个节点；再次求导时再各去掉一个。

⑤ 均匀 B 样条中，公式会大幅简化

现在直接用实际时间 t，并假设节点严格等间隔，间隔为。

因为：

所以：

注意，分子中的p 与分母中的 p 抵消了。对于这里的三次均匀 B 样条，不需要再额外乘以 3。

同理：

因此：

于是，速度对应一阶差分，加速度对应二阶差分。

例如，只看一个坐标轴：

则：

这些是导数曲线的控制点，并不意味着无人机在某个节点时刻的实际速度就等于。实际速度仍需用二次基函数对加权求和。

⑥ 图中的黄色框和绿色框表示什么？

在某个三次位置曲线段内：

位置由 4 个位置控制点加权得到；

速度由 3 个速度控制点加权得到；

加速度由 2 个加速度控制点加权得到。

对应的基函数次数逐级降低：

黄色框圈出二次基函数这一层，绿色框圈出一次基函数这一层。它们说明：速度、加速度也能沿用 B 样条的局部计算方式。

⑦ 这为什么对 Fast-Planner 很有用？

首先，可以把运动限制转化为对导数控制点的约束。

例如，由于速度曲线是速度控制点的凸组合，如果所有相关速度控制点都满足：

就能保证对应曲线段上：

加速度同理。这是保证限制成立的充分条件；某个控制点超限，并不必然意味着实际曲线已经超限。

其次，时间调整的效果也很清楚。保持位置控制点不变，将全部节点时间间隔放大为原来的k倍：

则在相同轨迹进度处：

例如，把整条轨迹的执行时间延长到两倍，空间路径保持不变，速度降为一半，加速度降为四分之一。这就是通过调整时间来改善运动可行性的数学依据。

性质小结

三次 B 样条需要记住的四个性质

局部调整：移动一个控制点，最多影响附近四个节点区间。

局部计算：每段曲线只由相邻四个控制点决定。

平滑连接：在不重复的内部节点处，位置、速度、加速度连续。

求导仍是 B 样条：三次位置曲线 → 二次速度曲线 → 一次加速度曲线。

这些性质使 Fast-Planner 能方便地调整局部轨迹、计算运动状态，并约束速度和加速度。

凸包性质

凸包性质：用有限个控制点围成的区域，约束整段连续曲线。它既可以用于避障，也可以用于限制速度。

① 什么是凸包？为什么曲线在里面？

可以把凸包想象成：用一根橡皮筋包住几个点，收紧后围成的区域。

B 样条的位置是控制点的加权和：

有效区间内，权重满足：

所以曲线上的点是控制点的凸组合，不会跑出对应的凸包。

对三次 B 样条，每段曲线都在其四个相关控制点的凸包内。左图中的绿色虚线表示局部凸包，对应曲线段被包在里面。

② 右图是速度空间中的凸包

三次位置曲线求导后，是二次速度 B 样条。因此：

一段位置曲线由 4 个位置控制点决定；

对应速度曲线由 3 个速度控制点决定，落在它们围成的三角形内。

这里的是速度向量，右图表示速度分量之间的关系，不是无人机在空间中的另一条飞行路线。

例如，如果所有相关速度控制点都满足：

由于速度允许范围是凸集，就能保证整段实际速度也不超限。

③ 如何用凸包保证避障？

思路是：这是充分条件，可能比较保守：凸包碰到了障碍物，曲线本身不一定碰到。

另外，仅仅控制点都在障碍物外面，并不能保证安全，因为控制点之间的区域仍可能穿过障碍物。

④ 第二张图的距离公式是什么意思？

选择控制点作为参考：

：到最近障碍物的距离；

：凸包内任意一点；

：到最近障碍物的距离。

根据三角不等式：

直观地说：参考点离障碍物有 远，向任意方向移动 ，剩余距离至少还有 。

这里严格的数学关系应写成“”，因为有可能取等号。

再定义相邻控制点的距离：

凸包内任意点到的距离，都可以用这条控制折线的总长作为上界：

因此：

只要：

就能保证所有凸包内的点都有，从而保证凸包无碰撞。

⑤ 为什么每一段距离要小于 ？

因为这是保证上面总长度条件的一种简单办法：

三个不等式相加，就有：

例如，参考点离障碍物 3 米，三段控制点间距分别为 0.6、0.8、0.7 米，那么：

因此，这个凸包及其内部的曲线段，到障碍物至少还有 0.9 米距离。

问题构建

把中间控制点的位置作为未知量，寻找一条既平滑、又安全、还满足运动限制的轨迹。

① 优化哪些量？

这里先固定：

B 样条次数：；

时间间隔：；

起点和终点的边界状态。

对于这种三次均匀 B 样条，首端的位置、速度、加速度由前三个控制点决定，末端由最后三个决定。因此，固定两端各三个控制点，只调整中间的控制点：

例如，共有 10 个控制点，则调整：

每个点包含 XYZ 三个坐标，共有个优化变量。

② 用什么标准判断轨迹好不好？

总目标是：

代价项	希望实现的效果
：平滑代价	减少控制点排列中的不必要弯折和间距变化
：碰撞代价	将控制点推离障碍物
：速度代价	减少速度超限
：加速度代价	减少加速度超限

是权重，用来调节不同目标的重要程度。

③ 平滑代价：让相邻控制点的变化更均匀

这里采用：

整理就是：

这正是控制点的二阶差分。

令左右邻居的中点为：

则单项代价等于：

所以它会推动：

效果不仅是减少弯折，还会倾向于让控制点间距更均匀。在没有其他要求、边界条件也允许时，控制点会趋向沿直线均匀排列。

这个代价没有包含，因此整体拉长执行时间，不会改变它对控制点布局的评分。

之后对照源码时要留意：Fast-Planner 当前公开实现的平滑项采用三阶差分平方和，对应控制点层面的 jerk 度量，与这里的二阶差分形式有所不同。

④ 运动可行性代价：不超限不罚，超限才罚

以一个方向的速度分量为例：

它的意思很直接：

速度在允许范围内，代价为零；

正向或反向速度超限，都会受到惩罚；

超限越严重，代价越大。

加速度使用相同形式，将换成即可。

因为导数仍是 B 样条，可以直接对速度、加速度控制点计算：

再将各控制点的 XYZ 分量惩罚相加，得到。

这里使用的是逐轴限制，例如，不要直接等同于三维合速度

凸包性质保证：如果所有相关导数控制点都满足这些限制，那么它们加权得到的实际速度、加速度也满足限制。但把超限写成代价属于软约束，仅仅完成优化，并不自动保证超限完全消失。

⑤ 碰撞代价：距离不够时，把控制点推远

定义：

正确的惩罚方向应当是：

应该惩罚“距离太近”，而不是惩罚“距离足够远”。

例如，安全距离为 1 米：

距离 1.5米：不惩罚；

距离 0.5 米：代价为 0.25；

距离 0.2 米：代价为 0.64。

把需要优化的控制点代价相加

但控制点离障碍物足够远，不单独构成整段曲线安全的证明。还要结合控制点间距、凸包范围或轨迹碰撞检查。控制点布局较密，只是有利于降低控制点之间穿过障碍物的风险。

⑥ ESDF 和势场：告诉优化器“有多近、往哪走”

ESDF 是欧氏有符号距离场。可以把它理解为一张三维地图：不仅记录哪里有障碍物，还能查询某个位置到障碍物的距离。

优化器需要两样信息：

距离 d：判断安全距离够不够；

梯度：指出距离增大的局部方向。

对于不在体素网格节点上的位置，可以通过周围网格值进行三线性插值，估计距离及其变化。

当控制点太靠近障碍物时：

因为，沿负代价梯度更新控制点，就会朝距离增大的方向移动。

势场图可以这样理解：障碍物附近是代价较高的“山”，控制点受到避障项的作用，会向低代价区域移动。与此同时，平滑项又会把轨迹拉得更顺，运动可行性项则抑制过大的速度和加速度。优化器根据三者的合成梯度反复调整控制点，通常得到初始轨迹附近的一个局部较优解。

时间调节

轨迹优化后，如果仍然飞得太快、加速度太大，就给相关路段分配更多时间。

① 为什么优化后还可能超限？

避障优化会把轨迹推离障碍物，有时会让路线变长。如果飞行时间不变，无人机就可能需要更大的速度或加速度。

而且，之前的速度、加速度限制采用的是软惩罚，并不保证优化结束后完全没有超限。

因此，Fast-Planner 还需要调整节点时间：只延长部分节点间隔后，各段时间不再相同，轨迹就从均匀 B 样条变成非均匀 B 样条。

② 速度超限：时间按速度超限比例放大

速度控制点为：

保持位置控制点不变，将分母对应的时间跨度放大倍：

那么：

取这个速度控制点中，绝对值最大的分量：

若它超过逐轴速度上限，理想的放大倍数是：

例如，最大分量为，上限为：

将对应时间跨度延长到原来的1.5倍，这个速度控制点的最大分量就降到。

③ 加速度超限：时间按超限比例的平方根放大

加速度控制点为：

这里有两层时间关系：

本身就与时间成反比；

计算加速度时，还要再除以一个时间跨度。

因此，把相关节点间隔统一放大 倍，使两个速度控制点及加速度分母都按该比例缩放，就得到：

令最大超限加速度分量为：

则选择：

例如，加速度为，上限为：

相关时间间隔放大两倍，加速度就降为原来的四分之一。

这里不能只改加速度公式的分母，必须同时考虑两个速度控制点涉及的时间间隔。

④ 为什么要反复调整，而不是一次完成？

因为节点间隔是共享的：一个时间间隔会影响多个相邻的速度、加速度控制点。修改一处后，周围控制点的数值也会发生变化，需要重新检查。

迭代过程是：

找出超限的速度、加速度控制点。

根据速度超限情况，延长对应节点间隔。

根据加速度超限情况，延长相关节点间隔。

重新计算并检查，直到两类超限集合都为空。

算法中的表示筛选出的超限控制点，撇号在这里不是再次求导。

⑤ 为什么每轮只允许延长一点？

为了避免某一处一次增加太多时间，实际每轮采用：

其中是略大于 1 的单次放大上限。

例如，理想上需要放大 1.5 倍，但单次上限为 1.1，这一轮就只放大 1.1 倍，再重新检查。这能避免多个相邻超限点反复调整共享区间，造成时间过度膨胀。

还有一个区别需要记住：整条时间轴等比例拉长，可以保持空间曲线不变；只调整局部节点间隔，会改变基函数，空间曲线也可能随之改变。因此，局部时间调整后，仍需确认轨迹的碰撞安全性及需要保持的边界状态。

前端到后端

“B 样条拟合”：先沿前端轨迹采样，再反过来求一组控制点，让 B 样条尽量贴近这些采样点。

整个衔接过程是：

① 前端得到的，其实已经是带时间的轨迹

在 Fast-Planner 中，搜索节点之间由运动基元连接。例如，一段恒加速度运动为：

搜索结束后，沿父节点回溯，就能得到每段的初始状态、加速度和持续时间，从而重建完整的分段轨迹；如果使用了解析连接，还包含最后的连接段。

因此，可以计算这条轨迹在任意时刻的位置。

② 按固定时间间隔，取一组采样点

例如，每隔取一个位置：

得到：

同时提取起点、终点的速度和加速度，作为拟合的边界信息。

注意区分：

：已知的轨迹采样点；

：需要求解的 B 样条控制点。

它们一般不相等。

③ 利用 B 样条公式，反求控制点

你已经学过：给定控制点，可以计算曲线上的位置。

现在反过来：已知希望曲线经过或靠近的位置，求控制点应该放在哪里。

对于三次均匀 B 样条，在等间隔采样时刻有：

希望它接近前端采样点，因此建立：

例如，前三个采样点对应：

控制点之间的关系是线性的，所以可以通过解线性方程组求出来。

还要加入边界速度、加速度关系。例如起点：

终点也有对应的两条关系。

④ 实际上是一个最小二乘拟合问题

将位置和边界导数关系放在一起：

求解：

Fast-Planner 的 parameterizeToBspline() 正是做这件事：对于 K 个采样点，建立K+4 行方程，求 K+2 个控制点，并分别求解 X Y Z 三个坐标。它使用 QR 分解进行最小二乘求解，因此一般不保证所有采样点和边界导数都被精确匹配。

在中， 是已知的系数矩阵， 是希望拟合的已知数据， 是待求的控制点坐标。

⑤ 拟合与后端优化，是两个不同步骤

步骤	主要任务
初始拟合	找一组控制点，让 B 样条尽量接近前端搜索轨迹
后端优化	从这组控制点出发，进一步改善平滑性、障碍物距离和运动可行性

源码中的衔接也很直接：先调用 getSamples()，再调用 parameterizeToBspline()，然后用求出的控制点构造三次 B 样条。

所以，前端提供了“轨迹大致应该沿哪里走”的参考，拟合负责把它转换成后端方便调整的控制点表示。由于拟合可能改变原轨迹，前端轨迹安全并不自动保证拟合后的曲线安全，后续还需要避障优化和检查。

义父，请我喝杯蜜雪冰城吧。