作者: 19级 lz
日期:2021-11-3
论文:《Teach-Repeat-Replan:A Complete and Robust System for Aggressive Flight in Complex Environments》
一、背景:
本文针对自主四旋翼飞行器的飞行问题,提出了一个完整的鲁棒运动规划系统。该方法建立在经典的教学重复框架上,广泛应用于基础设施检测、空中运输和搜救。
在这些应用中,由人类完全决定无人机飞行轨迹的拓扑结构。然而,糟糕的引导轨迹和不断变化的环境阻碍了一个简单的引导-重复(teach-repeat)系统的较好的应用。
在本文中,我们提出了一种方法,能够自动转换一个人的任意不稳定的驾驶轨迹到一个拓扑等价的轨迹,而不是命令无人机精确地跟随人的导航轨迹。生成的轨迹保证平滑、安全、动力学可行。此外,为了避免飞行过程中未映射的或动态的障碍,我们在系统中引入了滑动窗口局部感知和重新规划方法,以生成安全的机载局部轨迹。我们将我们的系统命名为teach-repeat-replan。它可以捕捉用户对飞行任务的意图,将任意不稳定的教学路径转换为平滑的相同轨迹,并生成安全的局部重新规划,以避免未建图或移动的障碍。
二、有关工作:
引导-重复(teach-repeat)系统:
近年来,许多机器人引导-重复工作已经发表,特别是移动机器人。它们大多侧重于提高运算符重复/跟随路径的准确性或健壮性,这与本文的动机有根本的不同。
轨迹优化:
轨迹优化是从较差的教学轨迹生成安全、可执行的重复轨迹的关键。本文首先寻找一种安全的几何路径来指导轨迹的生成。通过在路径中迭代地添加中间路径点,多次求解minimum-snap问题,最终生成安全轨迹。
飞行通道优化:
我们全球规划的第一步是围绕引导轨迹建立一个飞行走廊,用于时空轨迹优化。在我们之前的工作中,我们通过寻找一系列轴向的立方体来构建飞行走廊,这可能会牺牲很多空间,特别是在高度非凸的环境中。
其中凸多面体比简单的立方体获得了更多的自由空间。使用简单的轴向立方体极大地限制了轨迹优化的求解空间,可能会导致较差的解。更重要的是,在自由空间非常有限的情况下,例如飞行通过一个非常狭窄的圆,一个基于立方体的走廊甚至可能不能覆盖所有的教学轨迹,导致走廊中不存在任何解决方案。因此,为了更充分地利用自由空间并适应即使是极其杂乱的映射,我们提出了一种基于凸集聚类生成一般、自由、大凸多面体的方法。
三、研究过程及结果:
凸簇膨胀
凸簇定义:如果连接S中任意两点的线段完全位于S,则向量空间中的该合集称为凸集。
体素被标记为被占用或自由
算法1:
define:
C:代表目标凸簇
C+:是迭代中尝试添加到C的体素集
C*:包含新添加的体素,保持了凸簇性。
(1)簇膨胀首先加入种子体素体素
(2)加入p体素的邻居到C +中。
(3)在迭代中,使用CHECK_CONVEXITY(p+,C,M)函数检查每个C+中的体素p+是否能保持凸性。
(4)基于此标准,合格的体素被认为是活性体素,并被添加到C和C∗中
(5活性体素p∗的邻近体素将被GETNEIGHBORS(C∗)遍历和收集,以进行下一次迭代。
(6)当c +为空时结束膨胀
理解: 为什么用C*? 只添加扩展了的体素的邻居,防止重复计算不能扩展的体素.
凸簇膨胀的过程
算法2(凸性检查):
将射线从p+投射到C中的每个体素。
根据凸集的定义,当且仅当所有射线都是无碰撞的,带有p+体素的M集是凸集。
多面体空间表示方法:
三维多面体的h -表示是一组仿射函数:
例:
优化
优化1:凸聚类初始化
将每个凸聚类初始化为一个轴对齐的立方体,这可以非常快地完成,因为只需要索引查询(O(1))操作。将立方体扩充到最大体积后,切换到凸聚类,进一步对立方体周围的凸自由空间进行分组。提出的多面体初始化可能导致最终多面体不同于从零开始聚类的多面体。这是因为轴对齐的立方体只在x、y、z方向膨胀,而凸簇在所有可能的方向(3D网格地图中的26个连接)。
然而,这个初始化过程是合理的。我们的目的不是让每个多面体都达到最佳状态,而是比一个简单的立方体获得更多的自由空间。在实际应用中,初始化提供了快速发现邻近空间的方法,易于分组,且不妨碍后续的凸簇膨胀来细化多面体并找到可观的空闲空间。这种初始化过程显著地提高了计算效率,但只牺牲了最终多面体的体积。
优化2:提前判断
将聚类中的所有体素标记为凸聚类内部的内体素和凸聚类边界上的外体素。当射线从一个候选体素穿越到凸聚类中的一个体素时,当射线到达标记为内体素时,我们提前终止射线投射。
定理1: 内体素的早期终止对于检验凸性是充分的。
证明:根据凸集的定义,在凸聚类中,连接一个内体素与任何其他体素的射线完全位于凸聚类中。因此,内体素的延长线必须位于凸聚类内,因此必须通过凸性检验。
优化3:位素选择
为了进一步减少需要投射光线的体素数量,给定一个候选体素,只使用外部体素来检查其凹凸性。
定理2:利用凸聚类的外体素来检查凸性是充分的。
证明:很明显,凸聚类是一个边界有外体素的封闭集。候选体素在这个集合之外。因此,从任何内体素到候选体素的光线必须通过一个外体素。根据定理1,当射线通过一个外体素后,检查射线的凸性就终止了,这意味着对于一个候选体素,检查投射到外体素的射线就足够了。
算法3:
对算法做了并行化工作.
原因:体素扩展顺序会影响扩展结果
算法4:
具体思想:
define status:
true:可以直接扩展
false:不能扩展
pending:不能直接扩展,需要考虑序列号比自己小的候选体素
- 给每一个候选体素设置序列号(序列号越大的体素越优先扩展)
- 假设每一个候选体素都可以直接扩展(status=true)(L3)
- 和C中体素之间有障碍物的p+直接排除(status=false)(L4-L11)
- 每一个pi+检查自己与比自己序列号小的候选体素(用pj+表示)之间有没有障碍物,如果有则把pi+状态标记为pending(表示有可能不能扩展),并且在list记录下pj+的序列号(表示将来若扩展有冲突,pi+需要让着pj+)(L12-L20)
- 如果体素可以直接扩展(status=true),直接加入C*中(L26-L28)
- 如果体素不可以直接扩展(status=pending),则查看list中的体素有没有被扩展,如果都没有扩展,则该体素可以扩展.(L29-L35)
走廊生成和环路消除
由于用户提供的轨迹可能具有局部环路,我们引入了一种特殊设计的机制来消除不必要的环路.
在教学的开始,飞行走廊被初始化,找到最大的多面体周围的无人机的位置。然后当人类驾驶无人机移动的时候,我们继续检查无人机的位置。如果它超出了最后一个多面体,我们进一步检查无人机是否发现了新的自由空间。
- 如果无人机包含在倒数第二个多面体,我们可以确定教学轨迹有一个循环,如图7©所示。被认为是可重复的,因此这个多面体从走廊里弹出。
- 否则,如图7(d)所示,无人驾驶飞机被引导去发现新的空间。然后一个新的多面体膨胀起来,加到走廊的尾部。
教学结束后,走廊生成就终止了。最后一个飞行走廊与教学轨迹具有相同的拓扑结构,因为走廊中没有包含任何障碍物。且没有不必要的循环。
空间轨迹优化
(分段)贝塞尔曲线优化
最小化jerk
边界约束
保证贝塞尔曲线首尾相连
连续性约束
保证贝塞尔曲线光滑
安全约束
保证贝塞尔曲线无碰撞
时间轨迹优化
对时间做一个映射
最小化时间公式
加入正则项
替换
速度与加速度越是
Socp 重构(离散化)
引入新变量
损失函数转化为
引入松弛变量
局部重规划
蓝色曲线:全球轨道
和绿色曲线:实际飞行路线
紫色曲线和点是滑动窗口中的全局轨迹及其对应的控制点。
红色的曲线和点代表重新规划的局部轨迹及其控制点。
黄色的圆锥体显示了无人机的感知视界。
基于 b 样条优化
光滑性保证
使轨迹平滑
可靠性保证
d(Qi): 控制点到最近障碍物的距离
将轨迹退离障碍物
时间调整
原因:轨迹拉长
如果没有可行解,就扩大时间,直到有可行解
基准比较
GPU优化效果
CPU_raw:基础凸集群膨胀
CPU+:立方体初始化
CPU++:立方体初始化、顶点选择和提前终止
GPU:GPU优化
立方体初始化效果
1)用多面体代替轴对齐的立方体可以显著增加飞行走廊的体积。
2)使用初始化只是稍微牺牲了飞行走廊的体积。以中等或粗细分辨率(0.15∼0.25米)可忽略牺牲。
轨迹优化效果
基于 waypoint-based 方法和基于梯度的方法都需要一个分段线性路径作为初始化。
Waypointbased方法只能在初始路径上添加中间路径点。因此,它主要由其初始化决定,并倾向于输出低质量的解决方案。
基于梯度的方法没有这种限制,可以利用梯度信息自动调整路径。然而,由于碰撞代价定义在一个非凸的 esdf 上,其优化公式是非凸的。因此,基于梯度的方法总是围绕其初始猜测找到一个局部最优解。
与这两种方法相比,我们的方法是无初始化的。我们提出的方法的时间和空间优化都享有其公式的凸性。保证在飞行走廊中找到全局能量最优解和时间最优解。当然,更平滑的轨迹也会产生更快的时间轮廓。
conclusion
在本文中,我们提出了一个框架,示教-重复-再规划的四旋翼攻击飞行在复杂环境中。该工作的主要思想是寻找用户教学轨迹的拓扑等效自由空间,利用时空轨迹优化获得节能重复轨迹,并结合在线感知和重新规划,确保安全抵御环境变化和移动障碍物。