【论文阅读】RAPTOR: Robust and Perception-Aware Trajectory Replanning for Quadrotor Fast Flight

【论文阅读】《RAPTOR: Robust and Perception-Aware Trajectory Replanning for Quadrotor Fast Flight》
发表自香港科学技术大学的沈劭劼大佬的实验室。
RAPTOR：四旋翼飞行器的高速飞行轨迹重新规划，以下为四旋翼飞行器的样子。

介绍

对于无人机来说，在未知且高度杂乱的环境中实现自主高速飞行一直是一个很大的挑战。
现有的重新规划轨迹(trajectory replanning)方法存在以下缺点：
a）在高速飞行中发现障碍物到撞到障碍物的时间非常的短，trajetory replanning方法必须要在短时间内重新规划路径，但现有trajectory replanning方法不能保证在有限时间内找到另一条可行路径。
b）现有方法找到的路径都是单一的，而且是拓扑等价（如果一个图形可以连续变形成另外一个图形，我们称这两个图形是拓扑等价的。连续变形是指一个形状被弯曲、扭曲、拉伸或压缩）的，这导致它很容易陷入局部最小，而且飞行路径不够平滑和安全。
c）现有方法没有考虑到环境感知。比如，下图中，为了减小能源消耗，无人机的路径是沿着墙行进的，这就导致它看不到视角之外的区域，也看不到藏在转弯处的四边形障碍物，更容易发生碰撞。

本文提出了一个Robust And Perception-aware TrajectOry Replanning (RAPTOR)框架解决了这些问题。
创新点：

• 1）为了即时规划路径，我们提出了一种在路径限制下的梯度优化算法(path-guided gradient-based
  optimization method)，它利用了几何方法来排除不可行路径(utilize geometric guiding paths
  to eliminate infeasible local minima)。

• 2）提出了一个在线拓扑路径规划方法（topological
  path-guided optimization，PGO）来提取一组代表路径来辅助优化，来防止陷入局部最优并提升生成路径的质量。

• 3）上述两方法是基于路径不会出现如图1的障碍物的情况，是在乐观的假设前提下设计的。对此，本文提出了一个预测风险的路径修正方法，来更好的规划路径。它可以更早地发现未知区域的障碍物，并及时修正路径。

• 4）我们在双层动作规划框架中加入了四旋翼无人机的偏航角(yaw
  angle,实际航向与计划航向之间的夹角)的变量，它会在离散状态空间中寻找一组偏航角来最大化信息增益和路径平滑度。这样可以帮助无人机来更好地探索未知区域。

相关工作

四旋翼无人机路径规划

现有方法的四旋翼路径规划方法可以分为强约束方法(hard-constrained)和梯度优化方法(gradient-based optimization)。

强约束方法最早起源于一个最小化snap轨迹(minimum-snap trajectory)，它通过二次规划（quadratic programming，QP）分段生成多项式轨迹。后来Richter求解了最小化snap trajectory的闭式解。之后一个双层框架被提出，首先在第一层，它会探索出一个安全区域来作为初始路径，然后再用QP在安全区域里面求解路径。

梯度优化方法将轨迹生成视作一个非线性最优化问题，并结合了欧几里得有向距离场(Euclidean signed distance field，ESDF，)来保证路径安全。

拓扑路径规划

现有的拓扑路径规划主要寻找不同的homotopy classes或者visibility deformable groups。

未知环境中的导航

许多方法会把未知的区域当做无碰撞区域来对待，这样可以提高实现目标的概率，但是可能会造成碰撞。也有方法将位置区域视为不安全的，并且只允许在已知区域或者传感器视野(field of view,FOV)里运动。

系统整体框架

如图，算法分为两个部分，在第一部分，robust optimistic replanning通过PGO生成多个局部最优轨迹，优化是在拓扑路径搜索中找到的拓扑代表路径的指导下进行的。
在第二部分。采用了基于感知的规划，它对路径进行修正来减少碰撞风险，然后再采用偏航角修正来更好地探索未知区域。

Path-guided轨迹优化

本节提出了一个PGO算法，它采用了几何导向路径(geometric guiding path)来完成优化。

优化错误分析

基于梯度的轨迹优化算法（GTO）的效果通常依赖于一个好的初始化，如果初始路径通过了障碍物，那么优化可能得不到好的结果。
如图是2个典型的优化错误示例。图a是山谷的形状，灰色表示山谷深度，绿色是规划的轨迹，红色箭头代表着欧几里得有向距离场ESDF的方向。图b是一个山脊ridge。
典型的GTO方法在collision cost中计算了ESDF的梯度来使轨迹远离障碍物。在这些ESDF的山谷和山脊中，有些地方的梯度会变化很大，因此，如果轨迹经过障碍物和这些区域，ESDF的梯度会在某些点上突然变化很大，导致优化的时候会趋向于将轨迹上的点移向相反的方向，导致优化失败。

这主要是因为这些ESDF中的地形代表的是与附近障碍物有着相同距离的空间，单靠ESDF很难继续优化，因此本文提出了一种额外的信息来改变轨迹避开障碍物（也就是将轨迹引到free space），弥补这种不足。

Problem Formulation

样条spline是通过一组特定点集来生成平滑曲线的柔性带。B-splines，B样条曲线就是通过控制点局部控制形状的曲线。详见：https://mp.weixin.qq.com/s/w0_NCYBiTuLQve3xgXU70w、
均匀B样条曲线为节点距相同的B样条曲线：
我们定义pb次均匀B样条的控制点为{q0,q1,q2,…,qN}，以及节点距Δt。

PGO由两部分组成，第一部分生成一个过渡预热轨迹。
在第一部分中，PGO采用了一个几何导向路径（很容易通过A 或者RRT 算法得到）引导初始路径来到free space。在本文中，几何导向路径是通过一个基于采样的拓扑路径搜索得到的。
第一部分的目标函数是：

fs是一个平滑项，用来定义一个弹性带(elastic band)损失函数，elastic band将给定的路径视为受内外力影响的弹性橡皮筋，使其变形，而内外力相互平衡，使路径收缩，同时与障碍物保持一定的距离，其中内外力就是对机器人运动的所有约束。由于轨迹边界状态约束，只有中间的控制点子集用于估计平滑度。
fg是导向路径和B样条轨迹的距离的惩罚项。基于B样条曲线路径是由控制点决定的性质，我们将每个控制点qi都对应于导向路径上的点gi，计算他们之间的欧氏距离的平方来估计轨迹间的距离，gi是在导向路径上均匀采样得到的。
最小化fp1损失可以看做一个无约束二次规划问题，也就是它的最优解是一个解析解(闭式解)的形式。生成的过渡预热路径会很接近导向路径，因为导向路径是一个无碰撞路径，所以一般来说预热路径也会是无碰撞的，即使它不是无碰撞的，那么它的主要区域也会是无碰撞的。

在第二部分中，我们采用B样条曲线优化方法来修正预热路径，使其光滑、安全并且动态可行，它的目标函数是：

F()函数如下，fc是一个碰撞惩罚损失，如果轨迹与障碍物的距离无限趋近于dmin，那么它会变得很大，d(q)是q在欧几里得有向距离场中的距离。

fv和fa分别是对速度和加速度的惩罚，其中，使他们的各个轴的值不要超过最大值。


虽然PGO分为两个部分的路径优化，但它的运行速度是十分地快的，首先，它的第一部分是一个二次规划问题，计算closed-form解时间可以忽略不计，其次，有着预热路径，那么第二部分的优化可以减少大量的优化时间。

拓扑路径搜索

给定一条导向路径，PGO算法可以得到一个局部最优路径。但是在优化过程中只是对轨迹进行变形，不能让它跨过障碍物。因此它很容易陷入局部最优。其中一个解决方法就是直接搜索全局最优的运动动力学路径，但它需要的时间太长了。
因此本文提出了一种快速的3D拓扑路径搜索方法来加快优化。它首先找到一组截然不同的路径来在不同拓扑中并行优化轨迹，相比于单轨迹方法，它可以探索更多的路径空间，取得更优解。

拓扑等价关系(topology equivalence relation)

在拓扑学中，两个定义在拓扑空间之间的连续函数，如果其中一个能“连续地形变”为另一个，则这两个函数称为同伦的。这样的形变称为两个函数之间的同伦。虽然同伦应用广泛，但它还不足够在3D环境中寻找轨迹。Jaillet提出了一个3D空间中的更有效的关系，称为可见性变形（visibility deformation，VD），但是因为要做变形检查，所以它的计算成本很高。

因此本文提出了一个均匀可见性变形方法(uniform visibility deformation, UVD)，它可以捕捉到大量有用的轨迹，同时也可以更快地确认变形可行性。
如图6，(a)中展示了三条路径，他们是同伦的但是这三轨迹却代表着不同路径。(b)是UVD的示例，这三条线是同伦的，但蓝色和紫色是UVD的，但黄色和它们不是UVD的。这说明了相比于同伦，UVD可以更好地指示每条路径的不同。

设有两条轨迹τ1(s)，τ2(s)，s∈[0,1]，并且τ1(0)=τ2(0)，τ1(1)=τ2(1)，也就是起点和终点相同。并且对于任意s，都满足τ1(s)到τ2(s)之间的连线是无碰撞的，那么这样这两条曲线就是可见的。
如图7，UVD和VD方法都是直接将轨迹上的两点连线，但VD上的各个点对应的s是不一样的，而UVD对应的s是一样的。虽然UVD不会像VD一样更泛用，但是它极大降低了计算成本。

在UVD中，它令si=i/K，然后判断τ1(si)τ2(si)之间有无碰撞。

拓扑路线图

为了找到在拓扑结构上不同的路径，需要建立一个骨架图来捕获free space的结构。算法1可以用来建造UVD路线图G，它其中包含的路线只有极少部分是可以UVD变形到其他路线的。
图的节点有guard和connector两种，guard会探索free space的不同部分，并且任意两个guard之间是相互不可见的，也就是它们的连线之间有障碍物。
在最开始，两个guard会被初始化为起点s和终点g。
在循环中，如果采样点相对于其他guard是不可见的，那么就将这个采样点变成一个新的guard。
为了形成路径，connectors用来连接不同的guard。如果采样点可以看到2个guard，那么就会创建一个connector来连接guard，形成一个拓扑唯一的连接，或者如果当前采样点比现在的connector路径更短的话，那么就用这个采样点代替现有的connector。

在路线图构建完成后，我们会采用一个深度优先搜索算法来搜索s到g的路径。

缩短路径和剪枝

算法2采用了一种拓扑等价的缩短-剪枝路径方法，它为上述每条搜索到的路径Pr找到一条快捷(shortcut)路径Ps。
针对路径Pr，算法首先将Pr离散为一组点Pd，Ps中的第一个点初始化为Pd的起点。
对于Pd中的每个点pd，如果它相对于Ps中的最后一个点不可见，记二者的连线为ld，那么第一个阻挡了Ps最后一个点的视野的voxel就被找到了，记该点为pb，把pb沿着ld的垂直方向远离障碍物，并使其在ld和pb点的ESDF梯度上共面，得到一个新的点po，将这个新的点加到Ps中去。


注意，拓扑结构唯一的路径数量会随着障碍物的增加呈指数增加。所以在密集场景中，是很难使用全部路径来指导并行优化的，这样的计算成本太高了。因此，本文只选择了前K个最短路径。路径长度是最短路径的rm倍以上的路径也被剪枝。

基于风险感知的轨迹修正

轨迹修正以并行优化轨迹pi(t)中的最佳轨迹作为输入，并在它的邻近空间中修正，输出修正后的轨迹pr(t)。它的核心是在轨迹优化中结合了视野和安全性约束。
如下，算法3首先检查每条路径pi(t)的可视状态，检索优化所需的必要信息。鉴于安全性在一开始得到保障，所以约束会在迭代过程中逐步提升可视和反应距离。接下来的部分我们会详细地讲解各个函数的意思。

检查可视状态

可视状态可以用tf、pf、tc、pc、vc这几个变量编码。
1）Frontier Intersecting Point交点：在未知环境中，只有部分环境会被观测到，在时间tf，轨迹pi(t)从已知空间进入未知客户，记位置pf=pi(tf)，那么pf需要被优先观察。因为：首先，它和未来飞行高度相关；而且，它可能会对飞行造成危险，最坏的情况就是在tf下一刻就会撞上障碍物；最后，相比于其他未知区域，轨迹会最先抵达pf位置。因此，在FrontierIntersection函数中，我们计算得到pf和tf。
2）visibility metric：在飞行中，我们定义了一个visibility level ψ来度量每个点的可见性，并希望它不要低于ψmin。如下式，ψ是p到pf路径上的离障碍物最小的欧几里得距离，通过我们的欧几里得距离场，这个可以通过常数时间计算得到。

3）ctirical view direction：当对于所有的t小于tf，都有ψ(pi(t),pf)＞ψmin，那么这条路径就是安全的，不需要修改路径。否则，我们定义，当tc小于tf时，ψ(pi(tc),pf)=ψmin，也就是在pi(tc)=pc位置，我们刚好可以看到pf。那么定义pf到pc的单位向量为vc。这个vc是critical view direction。

迭代修正

在最坏的情形中，pf点之后会出现一个障碍物。为了保证安全性，那么需要修正路线。如果pc点的加速度不超过加速度的最大值（算法3的5-7行，Rq是四旋翼体积和其他干扰的补偿），这样就能在碰撞前停下，就不需要修正路线，否则的话，就需要修正路线，使得它可以更早地看到pf点，定义这个更早的时间点为ts。
给定critical view direction vc，约束可以表示为：
它的意思就是，在ts时刻，四旋翼必须在**pf点沿vc方向的射线（pf+λvc，λ＞0）**上，因为在这条射线上，pf是可视的。除此之外，ps到pf点的距离必须大于安全距离ds，ds是最短制动距离。


但是vs并不能在得到轨迹前提前计算，因此我们使用了一种迭代优化策略。在最开始，我们使用时间[t0,tf]的平均速度作为vs，然后根据算法3中的10-12行计算新的路径，之后我们计算当前vs是否满足安全距离，如果满足，则结束优化，否则将vs乘上一个略大于1的系数α（这里不太明白，照理说vs变小才能满足制动距离，但是这里却把vs变大了？？？？），继续优化。
在RefineTrajectory()函数中，相比于之前的优化算法，新增了view和安全距离的约束。直接计算ts需要大量时间，因此我们简化了计算，如算法3的11行。

偏航角规划

四旋翼一般只有几个传感器提供视野，为了提高飞行安全性，我们规划了轨迹的偏航角来积极观察环境。我们将偏航角规划问题划分为了图搜索问题和轨迹优化问题两个部分。

图搜索

我们建立了一个图搜索问题来寻找一组偏航角 Ξ：{ξ0, ξ1,…,ξM}，它可以修正轨迹来平衡轨迹平滑性和未知空间的信息增益IG。
在时间上均匀采样M+1个点，i∈[0,1,…,M]。在p0点，偏航角是由当前四旋翼状态决定的。令j∈[0,1,…,J]，图节点nij关联了不同的角度ξij和信息增益gij，gij是状态(pi,ξij)的信息增益。对于节点对ni,j，ni+1,j2，它们有着邻近的位置，因此将它们用边相连。由此建立一个图，之后使用dijkstra算法就可以找到最大IG和平滑度的路径。


信息增益IG是相机看到的未被建图的voxel的数量。直接通过射线扫描来计算数量时间成本太高，因此我们保存了所有voxel的可见性来减少重复搜索。并且我们采用了并行计算，还使用了subsampling来近似实际增益。
在其他文献中，将所有voxel的权重设为相等的，这是为了让四旋翼尽可能地探索所有空间。但是在我们的系统中，并不把全覆盖作为目标，因为在某些情景下，是很难做到探索全部未知空间的。离轨迹越近的voxel点才对飞行更有意义。因此，我们对各个voxel点增加了权重。Dl是侧面距离，Ds是纵向距离。


有了这些偏航角，我们就能优化路径了。和前面一样，我们将路径离散为N个控制点{φc,0, φc,1,…,φc,Nc } a，节点距δtφ.因为B样条曲线的凸包性质，所以我们不用担心优化后的路径会超过速度和加速度的限制。
接着我们最小化以下目标函数，第一项代表平滑性，第二项是一个soft waypoint约束，使得控制点的偏航角与计算得到的偏航角接近，最后两项是路径动态可行性的约束。

结果

实现细节

我们在real world和simulation都进行了测试。在real worls测试中，四旋翼使用了一个Intel RealSense深度相机D435，所有实验是在Intel COre i7-8550 U CPU上进行的。在simulation时，我们使用了一个包含四旋翼动力学模型，随机地图生成，深度图片渲染的工具。四旋翼动力学模型基于numeric ODE solver odeint。深度相机使用GPU渲染，它将周围障碍物的点云映射到图片平面上。为了模拟真实情况，模拟还加入了随机噪声。
轨迹优化是使用一个非线性优化求解器NLopt求解的。
1）global panning：在最开始，我们采用最简单的方式得到一个粗糙的初始轨迹，比如直接连接起点、目标点的直线。
2）volumetric mapping体积映射：我们采用了一个volumetric mapping框架来将从立体相机得到的深度图转化为一个occupancy grid map。然后根据一个distance transform算法计算得到ESDF。为了减少离散grid map的误差，我们还采用了三线性插值。
3）状态估计和控制：在real world test中，我们用一个鲁棒视觉惯性状态估计器来定位无人机，在simulation中，由四旋翼动力学模型来生成odometry，我们使用一个几何控制器来追踪位置和偏航角。

引导路径初始化

我们将我们的路径初始化方式和其他两种方法做了比较。
第一个方法采用了resolution-complete 运动动力学搜索，首先通过求解一个最优控制问题生成motion primitives，将状态空间网格化。然后再通过图搜索算法找到最优轨迹。
第二个方法和第一个方法类似，多采用了一个剪枝策略。
我们的初始化方法采用了是简单的将起点和目标点连线，然后通过拓扑路径优化。
对比如下：

基于感知的规划

更多结果分析详见原论文…

（以上全部内容全属个人理解，刚开始接触这个领域，许多地方不是很懂，文中有写的不对的地方还请批评指正）