Bubble Planner: Planning High-speed Smooth Quadrotor Trajectories using Receding Corridors 论文解析

在浙江大学 高飞 2022 新的一篇论文 《Bubble Planner: Planning High-speed Smooth Quadrotor Trajectories using Receding Corridors 》中，在无人机的高速飞行上有了新的突破，与当前的几个路径规划在速度上相比具有很大的提升，速度能够达到13.7m/s。在高飞的实验中，路径生成的耗时仅为4.69ms。论文中的实验视频展现在：https://www.youtube.com/watch?v=7tQCV6KBzSY

论文的主要贡献为提出了 实现高速轨迹优化的前端设计，为后端的轨迹优化带来很大的便利，特别是为 生成实现高速运动的轨迹 提供了足够的安全空间。本文提出的优化主要表现在两点：

1. 对生成飞行走廊的改进：将组成飞行走廊的球形最大化，并且考虑球形之前的重叠空间的最大。
2. 在重规划中的改进：无人机在飞行过程中重规划的前面的一部分与上一次规划的部分重叠，也就是轨迹规划的重利用。

基于走廊的路径规划： 近几年比较流行，表现为在八叉地图上的正方体，或者用多面体代表无障碍的空间。球形也非常常用，球体在轨迹优化中只施加一个约束半径R，可以通过kd-tree结构快速获得。之前使用的那些只考虑到相邻球体的连通性，通常只有很小的重叠。

后端的路径优化直接采用 MINCO 。

飞行走廊的改进

组成飞行走廊的球体 由 圆心，最临近障碍物的距离 和 无人机机身半径 决定。绿色的点代表走廊的中心，红色点代表最邻近障碍物点云位置，球体的半径为中心点到障碍物的距离减去无人机机身半径。

使用障碍点云构建KD-Tree，产生新的节点后通过KD-Tree寻找最邻近的障碍点，求得球体的半径，生成一个满足安全约束的球体。在KD-Tree的构建方法下寻找最邻近点，与全局遍历的方法相比，具有极大的优势，快速找出最邻近点，节省大量的规划时间。 KD-Tree相关：点击查看KD-Tree

A* 产生导向路径，从导向路径中选择离当前球体外最邻近的 导向点，在导向点附近进行三维高斯分布采样，每一个采样点生成一个新的球体，给每一个球体进行打分，将得分最高的球体添加到飞行走廊中。重复这个过程直到产生的新的球体中包含终点。飞行过程中的路径点和每一段路径分配的时间在后端的轨迹优化中实现。
飞行走廊的优化：最大化球体半径，考虑球体之间的重叠空间。较大的相交空间意味着优化过程的更大自由度。

---

关于生成飞行走廊的算法：
B 代表一个飞行走廊队列，p0 和 pg 分别代表其实状态和目标状态，T 为导向路径。
首先将起点初始化到我们的飞行走廊中，然后再生成飞行走廊：获取一个抽样点，以抽样点为球心产生一个球体，将球体加入到飞行走廊队列。循环这个过程生成飞行走廊，直到生成的球心包含我们设置好的目标点，就代表已经找到飞行路径。最后一行其实执行的是路径优化，采用的是MINCO算法。

抽样的过程中，首先使用A*找到最短路径，然后仅在该路径周围进行批处理三维高斯分布采样，不需要使用类似RRT的方法并从整个空间中采样，使得计算时间显著减少。在这个范围内计算每一个采样点的得分，选择得分最高的球体加入飞行走廊序列。这样每次加入飞行走廊的球体都是最优的，给后端的轨迹优化提供了空间，利于高速飞行的轨迹调整。
球体的得分：Score = ρrVcand + ρvVinter
ρr 和 ρv 代表权重系数，Vcand代表球体的体积，Vinter表示新的球体和上一个球体的重叠部分的体积。

飞行走廊的球体数量更少，体积更大。

优化效果：

(a) 中的飞行走廊就是这篇论文提出的算法改进产生的效果，与 (b) 之前的成果相比，产生的球体更少，包含的空间更大。

生成飞行走廊之后，我们的路径点就默认为每两个重叠球体的重叠空间的中心点，每一段的分配时间采用最大速度来计算：（qi-qi-1)/vmax 。

生成路径的重利用

无人机在飞行时，需要不断地对路径路径进行 重规划 来避免感知到的障碍物。论文中的方案在固定距离D（即规划范围）内规划轨迹，如果当前点与上一次规划的点距离大于一个设定的值，重新规划过程就被触发 ，当一个新的重计划被触发时，选择q中最近的未来路径点drp作为初始状态。当发现执行中的当前轨迹与任何障碍物发生碰撞时，也会触发重新规划。在重规划的过程中，飞行走廊重复利用之前路径上的一些球体到当前的重规划任务中。这样能够减少无人机飞行过程中的重规划次数，能够快速产生向前的飞行走廊。

图 (a) 中绿色和粉色虚线圆圈分别是上一个和当前复制图中的规划地平线。© 中绿色的两个球体为上一次规划产生的飞行走廊的一部分，蓝色的球体是新生成的飞行走廊的一部分。
该方案确保每个复制平面中的走廊始终包含足够的空间，以供四旋翼机从其当前状态进行机动（因为当前四旋翼机状态位于前一条走廊中包含的前一条轨迹上），从而显著扩大了后端轨迹优化中的可行区域。

重利用的弊端
由于在先前的激光雷达测量中可能被遮挡的新感测到的障碍物，上一个规划周期中重复使用的走廊不能保证无障碍。这就使得无人机在复杂的环境下高速飞行可能会产生一条失败的飞行走廊，降低飞行成功率。可以通过将（重新）规划的前几条走廊放置在已知的空闲空间（而不是空闲和未知的空间），可以克服这一限制，以便在下一个规划周期中安全地重复使用这些空闲走廊。将前几个球体限制在自由空间中，还可以规划安全的备用轨迹，以确保安全飞行。

贝塞尔曲线 & B样条曲线

要求一条光滑的曲线尽量逼近所有点，多项式拟合，比较适用与轨迹点较少的情况，当轨迹点较多时，采用多项式拟合难度较大并且会出现轨迹的密集震荡，存在较大的起伏。

贝塞尔曲线：
一个插值多项式，具有凸性。de Casteljau 算法绘制贝塞尔曲线。在这里不讲述贝塞尔曲线的公式和计算，描述一下图形理解：

图中有四个固定的点 0、1、2、3，我们用三次贝塞尔曲线来描述生成的曲线。
取 0 -> 1 的中点为一次贝塞尔曲线上的点， 1 -> 2 ， 2 -> 3 的中点也是一次贝塞尔曲线上的点。这时获得了三个一次贝塞尔曲线上的点，将它们相邻之间用直线连接起来，按照相同的方法从一次贝塞尔曲线上取二次贝塞尔曲线上的点，即为图中蓝色的点（有两个），连接起来取中点即为三次贝塞尔曲线上的点。
将这些点在其所在的线段中从左到右按规律移动，黄色点经过的轨迹就是我们的贝塞尔曲线。
可以在视频中更好地理解：图形动画理解贝塞尔曲线
Bezier曲线存在的不足：
（1）一旦确定了特征多边形的顶点数（n+1）个，也就决定了曲线的阶次（n）次
（2）Bezier曲线或曲面的拼接比较复杂012
（3）Bezier曲线或曲面不能作局部修改

B样条曲线：
同样我们采用图形的方法理解B样条曲线：

首先我们取三个点，相邻的两个点用直线连接起来，在对应的三角形中不相邻两点所在直线对应的中线上三分之一处取一个点，图中绿色点所在的位置。

接着继续取下一个点，与前面两个点构成三角形，用相同的方法取中线三分之一处，与前一个点用曲线连接。后面继续取点，以此类推，形成一条B样条曲线。图形动画理解B样条曲线

MINCON 后端轨迹优化