Teach-Repeat-Replan: A Complete and Robust System for Aggressive Flight in Complex Environments 论文笔记

飞行走廊生成方法

围绕 teaching trajectory 教学轨迹构建飞行走廊，进行时空轨迹优化，提出了一种生成一系列大的凸多面体的方法构成安全飞行走廊，提出基于突集的方法：

• 不假设凸簇的生长方向，根据密集占用图生成完全无碰撞的多面体。
• 介绍几个精心设计的注意事项，这些注意事项将显著加快集群的速度。
• 充分利用了该算法的并行结构，并在gpu中加速了一个数量级。
• 从构建凸多面体集群到建立轨迹优化约束的完整轨迹。

凸多面体膨胀

构建飞行走廊的核心算法是在给定坐标处寻找最大的自由凸多面体。
使用栅格地图来表示环境，飞行走廊中的每个多面体都是一个体素集的凸包，我们将体素集命名为凸集群。在保持凹凸性的同时迭代膨胀这样一个集群的轨迹：

以上为迭代膨胀算法，表现为 原始的凸多面体一层一层地迭代膨胀，直到没有可能加入多面体凸集的体素网格。在迭代中，每一个 C+ 中的体素 p+ 需要通过CHECK_CONVEXITY(p+, C, M)判断是否能够保存在可用的凸集中，将光线从 p+ 投射到 C 中每个存在的体素上：

射线的产生方式如下图：

在(a)和(b)中，将所有符合条件的邻居体素添加到凸集中。在©和(d)中，由于被占用的体素遮挡了一条射线(绿色箭头)到其中一个体素上，因此测试体素(黄色)被排除在凸集之外。

CPU加速

对所有体素和射线进行迭代使得该多面体膨胀算法不可能实时运行，特别是当栅格地图有一个很好的分辨率（体素更多）。对实现进行了仔细的工程考虑，并提出了一些显著提高整体效率的关键技术：

• 多面体初始化: 将每个凸集群初始化为一个轴向立方体，这可以非常快地完成膨胀。
• 当一条射线从一个候选体素遍历到凸集中的一个体素时，当它到达一个标记为内的体素时（即在凸集群内部的体素），我们会提前终止射线投射。
• 给定一个候选体素，只有外部体素被用来检查它的凸性。

GPU加速

我们找到活动集合C *的所有邻居，并检查它们的射线都是同时进行的。

在并行集合中，同时检测的体素可能会将冲突的体素添加到集合中。因此，我们引入一个额外的变量r来记录体素的顺序信息：

算法中考虑到候选膨胀体素网格之间的影响，在遍历检验每一个体素网格时除了对凸集合中的体素进行射线连接检验，还需要对已经检验过的候选膨胀体素网格进行射线连接检验，候选膨胀体素网格之间的影响如下图：

如果只考虑与凸集合内的体素射线连接检查，体素A和体素B都会被加入凸集合，这样破坏了集合的凸优性，添加了对候选体素的检查后，如果A已经加入到凸集合，那么B就不再加入凸集合中，保证了集合的凸优性。

飞行走廊生成与环路消除

用户提供的轨迹可能是任意颠簸的并包含局部循环，我们引入了一种特别设计的机制来消除不必要的循环，排除可重复多面体。

在检查是否多面体重复时，如果它超出了最后一个多面体(G[−1])，我们进一步检查无人机是否发现了新的自由空间。如果无人机包含在倒数第二个多面体(G[−2])内，我们可以确定教学轨迹有一个环路。
如下图中：

在©中，无人机离开最后一个多面体并返回到倒数第二个多面体，因此最后一个多面体从走廊中删除。

---

时空全局轨迹优化

通过 空间和时间选择优化 来解决全局轨迹生成问题。

空间轨迹优化

使用Bernstein基将轨迹表示为分段Bézier曲线，因为它可以很容易地限制在飞行走廊通过加强控制点的约束。包含：

• 边界约束
• 连续性约束
• 安全约束

时间轨迹优化

在空间优化中，给定固定的时间分配，生成走廊约束的空间轨迹。为了在时间上优化轨迹，我们设计了一个重计时函数 { t (τ) : τ→t }来将原始时间变量t映射到一个变量τ：

时间优化的效果。t 和 τ 为优化前后空间轨迹的时间配置。
一些约束包含：

• 连续性约束：每一个轨迹片段的末端与下一个轨迹片段的初始转状态一致；
• 动态可行性约束：速度v、加速度a的最大值约束。

---

在线局部重规划

在基于假设：

• 地图环境完美创建并且保持不变；
• 提供了全局一致的姿态估计。

条件下，一旦全局路径规划完成，无人机将不考虑其他因素执行轨迹。

然而，第一个假设并不总是成立的，特别是当新的障碍突然出现或环境发生变化时。对于第二个假设，我们的全局姿态估计依赖于环路闭合检测，这也不能保证极高的反应率。在姿态漂移明显但没有及时修正环路闭合的情况下，无人机可能会与障碍物发生碰撞。

局部重规划框架

采用快速滑动窗口重规划方法，**欧几里得符号距离场(ESDF)**及时更新。用于障碍规避。

迭代的改进 在每次迭代中，我们检查所有优化控制点的碰撞和可行性。如果检测到碰撞，我们通过增加 λ2 来增加碰撞项 J_c，再次解决优化问题。

由于我们封装局部轨迹以绕过障碍物，因此优化后轨迹总是被拉长。因此，使用原始的时间参数化将不可避免地导致更高的攻击性，这意味着四旋翼飞行器往往飞得更快。然后，它的速度和加速度将很容易超过预定义的限制。因此，我们调整局部轨迹的时间参数化，以挤出动力学上的不可行性。

通过上面的启发式方法略微扩大b样条的不可行的结点跨度，迭代延长时间，直到得到可行解或超过最大迭代极限。v_m 和 a_m 分别是不可行的速度和加速度。
如果调整时间后不存在可行解，则增大动态可行性惩罚的权重，再次优化轨迹。