本文将介绍一种无人机的在线路径规划算法 Bubble Planner，Bubble Planner 是港大张富老师MaRS实验室提出了一种的新型规划算法，是一种用于解决复杂任务规划问题的算法，其通过将任务分解为可重复使用的子任务，并以“泡泡”形式呈现任务之间的依赖性，从而使规划过程更加直观和易于理解。这个算法基于人类的直觉和习惯，通过将任务分解为可重复使用的子任务，并以“泡泡”形式呈现任务之间的依赖性，从而使规划过程更加直观和易于理解。
在本文章中讲 Bubble Planner 的前端轨迹优化的算法 和模型图, 后端轨迹优化公式 的参数和作用列出来讲解 , 作为学习参考使用，详细推论请看原论文，由于编者水平有限 ，对于讲解中可能出现的错误，还望多多包涵!!
Bubble Planner 的球形走廊指由一系列球形空间构成的三维空间区域，用于限定无人机的飞行区域。具体来说，Bubble
Planner
通过不断生成球形空间，并将其合并到已有的球形空间中，来形成一个连续的、覆盖了起点到终点的空间区域，该区域即为球形走廊。球形走廊的直径可以根据需要进行调整，以满足不同的任务需求。在球形走廊中，无人机的运动轨迹被限定在球形空间内部，而不会超出球形空间的范围。由于球形空间具有良好的几何性质，Bubble
Planner 可以使用一系列基于球形空间采样和优化的算法来实现路径规划和控制，从而提高无人机的自主飞行能力和安全性。
要生成新球体，我们首先使用障碍点云对于给定的球体执行最近邻搜索（NN搜索）在KD树上找到最近的障碍点n，算法如下所示：
Bubble Planner 的 飞行走廊是通过球形空间采样的方式来生成的。具体而言，算法首先将起点p0作为球心，以给定的初始半径为半径生成一个初始球形空间Bcur。然后，算法根据全局引导路径T上Bcur的前方点ph，使用BatchSample方法在球形空间Bcur中进行样本采样，得到一组候选的路径片段，这些片段可以覆盖Bcur的前方区域。接下来，算法将这些路径片段与Bcur进行合并，形成一个新的、更大的球形空间Bnew，以覆盖更多的地面区域。这个过程一直进行到终点pg被包含在当前球形空间中为止，最终，所有生成的球形空间构成了航行走廊B，其中每个球形空间都覆盖了一段地面区域，且相邻的球形空间之间有一定的重叠部分，以保证航线的连续性和平滑性。生成的航行走廊B可以作为后续路径规划和控制的依据。
这个算法是一种用于生成飞行走廊中下一个可飞行球的方法，其中每个球代表一组飞行姿态。该算法的主要思想是在飞行走廊中通过随机采样器生成一系列可行的飞行球，然后通过对这些球的评分，选取得分最高的球作为最佳球。评分的过程包括根据球的几何特征计算得分，以及根据球和指导点之间的距离，以及球之间的距离计算得分。这样，算法可以在保证球之间的安全距离的同时，寻找最佳的飞行方案。
1.首先，定义符号表示：Bf表示最后一个可飞行球；ph表示指https://www.bilibili.com/video/BV1q44y1n7Tx/?spm_id_from=333.788.recommend_more_video.3&vd_source=7687c7ece2574806a613b44866bbd9d0导点；Bbest表示本轮中最佳球；S表示随机采样器；K表示最大采样数；rd表示安全距离；Q表示按球得分排序的优先级队列。
2.初始化采样器S，设置k=0。
3.在样本数量小于最大采样数K的情况下，进行以下循环操作。
4.从采样器S中获取一个随机样本pcand。
5.根据pcand生成一个球Bcand。
6.计算球Bcand的得分。
7.将球Bcand插入到优先级队列Q中。
8.增加k的值。
9.循环结束后，如果优先级队列Q为空，则返回采样失败；否则，选取Q中得分最高的球Bbest作为本轮中最佳球。
10.返回Bbest。
绿色圆圈Bf是上一轮批次中生成的球体 ,样品黄色点为指导点ph。紫色点为根据由橙色阴影区域。蓝色圆圈是这一轮中最好的球体。
这是一项算法产生的无人机道路比较。
（a） 论文试验中提出的方法
（b）Gao等人产生的走廊。
在同一测试中
（c） 两者重叠体积的比较100次测试中的相邻球体
（d） 每个球体的体积超过100
测验。阴影区域表示超过100的最大值和最小值
新规划的地平线走廊是一种在当前路径被阻塞时重新计算路径的方法。
当遇到阻碍物时，Bubble Planner会将当前飞行走廊分割成多个子段，并将每个子段表示为一系列的空间区域（称为“bubble”）。然后，Bubble Planner使用随机采样的方法在每个bubble中生成一组路径候选，评估它们的安全性和可行性，并选择最优的路径作为重新规划后的路径。
具体来说的重新规划流程如下：
后退的走廊战略
a） 绿色和粉色 虚线圆圈分别是上一个和当前复制图中的规划地平线。
b） 粉色点是重叠区域的中心，由默认初始化。
c） 绿色球形走廊之前使用热初始化生成。蓝色的走廊是新的使用默认初始化生成。
Bubble Planner 的后端轨迹优化包括两个部分：动态规划路径优化和轨迹加速度规划。
首先，Bubble Planner 使用动态规划对前端路径进行进一步优化，以最小化路径长度和时间成本，并同时考虑路径的安全性和航迹约束条件，如飞行器的最大速度、最大加速度和最小转弯半径等。动态规划方法可以对复杂的约束条件进行求解，并找到最优的路径。
其次，Bubble Planner 对动态规划生成的路径进行加速度规划，以实现平滑的运动轨迹。这个过程中，Bubble Planner 将采用类似样条函数的方法，通过插值实现平滑轨迹。在轨迹规划的过程中，Bubble Planner 还考虑了无人机的动力学模型，如动力、惯性、重力和空气阻力等，以确保规划的轨迹是安全且可行的。
这个式子表示了在时间区间 [ti-1, ti] 中的一段平滑的曲线，其中 pi 是这段曲线在时间 t = ti-1 时的起点，Bi 是一个圆柱体区域，表示无人机在这段时间内可以安全飞行的空间范围。这个曲线的形状是由时间区间内的点 pi 确定的，可以根据实际需要进行调整，但必须保证曲线在时间区间内始终位于圆柱体内，以确保无人机的安全飞行。
这是 Bubble Planner 后端轨迹优化的优化问题表述。目标是最小化一个代价函数 J，包括路径的时间长度、偏差与曲率，以及路径离原路径的距离。这个问题的约束条件包括起点和终点、路径通过指定的关键点、路径长度、速度和加速度的限制、以及路径在预先定义的安全区域内。这个优化问题的解决将会得到一条光滑的路径，该路径满足所有约束条件，同时具有最小化的代价。该路径将被视为无人机在执行任务时的最优路径，从而提高无人机的效率和安全性。
p(t)：路径
pi：第i段路径的起点
ti：第i段路径的起始时间
d0：路径的起始点
dg：路径的结束点
T：全局引导路径
kp(1)：速度惩罚系数
kp(2)：加速度惩罚系数
v_max：速度上限
a_max：加速度上限
s：总路径长度
M：路径段数
Bi：第i段路径的边界
ρT：全局引导路径惩罚系数
ρvel：速度平滑惩罚系数
ρacc：加速度平滑惩罚系数
L：Lagrangian乘子
pi(t) 表示在时间t时，机器人所在的位置，即轨迹。ci(q,T) 是一个路径约束函数，它描述了机器人运动必须遵守的限制条件，其中 q 是机器人当前的位置， T 是全局导引路径。β(t) 是一组表示机器人在时间 t 时的欧拉角姿态的函数。t ∈ [0, Ti] 表示时间范围，其中 Ti 是轨迹所在的时间区间。
这个函数是一种的平滑惩罚函数，它在x≤0时惩罚值为0，当x逐渐增大时，惩罚值也会逐渐增大，但增长过程不是线性的，而是带有一定的平滑性，这使得优化问题可以更好地被解决。
这个公式是用来表示Bubble Planner中的轨迹优化问题的目标函数。该问题的目标是最小化总轨迹长度和一些惩罚项，以确保生成的轨迹满足一些限制条件，如速度和加速度的最大值等。其中，kp和τi是优化变量，表示每个子轨迹的时间长度和每个子轨迹的位置。Lμ(x)是一个惩罚函数，用于对不符合限制条件的轨迹进行惩罚，例如速度和加速度超过了最大值。ρT，ρvel，ρacc和ρc是惩罚系数，用于调节不同约束条件的相对重要性。
τ和q是Bubble Planner的输入参数，表示一系列的时间段和时间段内的目标状态。
Ti表示第i个时间段的时长，Z表示时间段的数量。
pi(t)表示第i个时间段内的位置，也就是飞行器在第i个时间段内的轨迹。
kp(1)和kp(2)分别表示在时间段i内的速度和加速度，这些速度和加速度必须在一定的范围内，即不能超过vmax和amax。
Lµ(x)是一个损失函数，表示飞行器与障碍物之间的最小距离，其中x表示飞行器与障碍物之间的距离，µ是一个常数，用于平衡惩罚和飞行器的最小安全距离。
ρT，ρvel，ρacc和ρc是权重系数，用于平衡时间代价和空间代价的权重。
https://arxiv.org/abs/2202.12177
https://www.bilibili.com/video/BV1q44y1n7Tx/?spm_id_from=333.788.recommend_more_video.3&vd_source=7687c7ece2574806a613b44866bbd9d0
