摘要
基于梯度的轨迹优化(GTO)在四旋翼飞行器的轨迹重新规划中得到了广泛的应用。但是它存在局部极小值，这不仅对安全是致命的，而且不利于航行的顺利进行。本文提出了一种基于GTO的再规划方法，系统地解决了这一问题。针对不可行的局部极小点，提出了一种路径引导优化(PGO)方法，大大提高了重规划的成功率。提出了一种拓扑路径搜索算法，用于捕获三维环境中不同的有用路径集合，每条路径引导一个独立的轨迹优化。它激活了对解决方案空间的更全面的探索，并输出了更优的重新规划的轨迹。基准评估结果表明，该方法在重新规划成功率和最优性方面优于目前最先进的方法。给出了具有挑战性的主动自主飞行实验，证明了该方法的鲁棒性。
相关工作
基于梯度的路径优化
GTO是一种主要的路径生成算法，把路径生成看作一个最小化目标函数的非线性优化问题。问题：局部最小值。
拓扑路径规划
已经有人利用拓扑上不同路径的思想在2维平面或者3维空间进行路径规划，其中有一种寻找属于可见性不同形类的路径的算法。其缺点为算法的复杂度过高，本文基于此做出改进。
路径引导轨迹优化
PATH-GUIDED TRAJECTORY OPTIMIZATION ,PGO
A. 优化失效分析
GTO的失败与不利的初始化有关，即初始路径以某种方式通过障碍通常会被卡住。典型的GTO方法将欧几里得符号距离场(ESDF)的梯度纳入碰撞成本中，以推动轨道脱离障碍物。将该代价与平滑性代价和动态可行性代价相结合，形成目标函数，其梯度迭代地将轨迹变形为平稳、安全的轨迹。
但如下图所示，为GTO优化失败的典型例子，当轨迹穿过ESDF(用橙色虚线表示)的“谷”(a)或“脊”(b)时，轨迹的相邻部分被推向相反的方向。红色箭头表示ESDF的梯度，黄色箭头表示目标函数的梯度。这个目标函数就是我们B节中问题公式化需要产生的函数。
对于这种情况，仅仅靠ESDF的梯度并不够，需要额外的信息(目标函数)。
B. 问题公式化(PGO的实现方法的数学形式)
文中提出的PGO方法是对上面GTO的改进，它的路径用B样条来表示。对于PGO方法，分为两步，第一个阶段产生一个过渡的预热轨迹（warmup trajectory），第二阶段，对这个预热轨迹的平滑度以及与间隙值(与障碍物之间的)再进行优化。两个阶段如下图：
第一阶段：a图的绿色是初始B样条轨迹，橙色的是几何引导路径，几何引导路径把初始轨迹拉到没有碰撞的地方形成预热路径（蓝色)
第二阶段：b图中，对预热路径再进一步进行平滑度和间隙值(与障碍物之间的)的优化，得到红色最终轨迹。这个几何引导路径通过A星算法或者RRT等传统方法就可以得到，但本文用的是*采样的方法(拓扑路径搜索 , TOPOLOGICAL PATH SEARCHING)得到这条引导路径。
第一阶段的优化目标函数是：
这里$f_s$是轨迹平滑性惩罚函数，而$f_g$是引导路径和b样条轨迹之间的距离的惩罚代价，其定义如下，主要是利用模拟弹性力来构造惩罚函数，其中$Q_i$为b样条轨迹的控制点，每个控制点$Q_i$都被分配了一个引导路径上关联的点$G_i$，其由沿着引导路径进行均匀采样得到：
第二阶段的优化目标函数如下，$f_s$是轨迹平滑性惩罚(成本)函数，$f_c$是在ESDF上评估的碰撞成本函数，当轨迹接近障碍物时，ESDF的碰撞成本迅速增长。再利用惩罚函数$f_v$与$f_a$对速度和加速度的不可行程度进行量化，第二阶段的主要内容参考fast-planner文章：Robust and Efficient Quadrotor Trajectory Generation for Fast Autonomous Flight | IEEE Journals & Magazine | IEEE Xplore。
拓扑路径搜索
TOPOLOGICAL PATH SEARCHING
本文提出了一个采样的方法来寻找几条不同的引导路径来引导上面PGO，以实时解决路径再规划问题。
A. 拓扑等价关系
通过改进 Path Deformation Roadmaps: Compact Graphs with Useful Cycles for Motion Planning 中的可见性不同形(visibility deformation，VD)，定义了均匀的可见性不同形(uniform visibility deformation,UVD)，它也捕获了大量有用的轨迹信息，并对等价性检验更有效。
可见性不同形：两条终点起点相同的轨迹，两者之间无障碍物(连线无碰撞，即可见)，但形状不相同
对于等价性检查来说，VD的计算代价很高,要测试VD关系，应该计算一个可见性图，并在其中进行路径搜索，这比检验UVD的复杂性更高。
因此，提出一种均匀的可见性不同形(uniform visibility deformation,UVD)的定义：
存在两条可以被s∈[0,1]参数化的轨迹$τ_1(s)、 τ_2(s)$并且满足$τ1(0) =τ2(0)， τ1(1) =τ2(1)$，如果对所有情况，连接$τ_1(s)、τ_2(s)$的线段是无碰撞的。那么这两条轨迹属于均匀的可见性不同形(UVD)类。
B. 拓扑路径图
拓扑路径生成图如图7，e图中，红色和橙色路径都属于同一个UVD类，而粉色路径是其UVD类的唯一成员。
在设计拓扑路径图中，我们引入了两种不同的图节点，即guard(守卫点)和connector(连接点)，类似于Visibility-PRM。guard负责探索自由空间的不同部分，任何两个guard点g1和g2是彼此是不可见的(连接g1和g2的线段存在碰撞)。每次在地图上采样一个点，如果这个点另外的任何一个guard都看不到，那么这个点就记作一个新的guard点。然后继续采样，如果一个采样点刚好可以被两个guard点看到，那么就把这个点记作connector，然后把这个connector和这两个guard点连起来。连接好以后做两个事：如果这是一条全新的拓扑路径，那么就保留，否则判断两条拓扑路径的长度，把长的那条去掉，保留短的那条。
伪代码说明：在设计拓扑路径图中，我们引入了两种不同的图节点，即guard(守卫点)和connector(连接点)，类似于Visibility-PRM。guard负责探索自由空间的不同部分，任何两个guard点g1和g2是彼此是不可见的(连接g1和g2的线段存在碰撞)。在主循环之前，在起始点和结束点创建两个guard点。每当采样点对所有其他guard都不可见时，就会在此点创建一个新的guard(第6-7行)。为了形成路线图的路径，使用连接器连接不同的guard(第7-19行)。当采样点恰好对两个guard可见时，就会创建一个新connector，要么连接guard以形成拓扑上不同的连接(第19-20行)，要么替换现有connector以创建更短的路径(第16-17行)。设置时间限制(t_max)或采样次数限制(N_max)来终止循环。其中起始点和终点之间的结点搜索算法参考Integrated online trajectory planning and optimization in distinctive topologies - ScienceDirect。
设计拓扑路径的算法伪代码：
设计拓扑路径的算法伪代码中函数的解释：
AddGuard(G,b)增加guard节点到G中
sample()通过采样节点，
VisibleGuard(G，Ps)得到G中能观测到Ps的所有节点
.size(a)计算节点个数（能观测到Ps）
Path(g1,c,g2)连接connector点与guard点生成路径
SharedNeighbors(G,g1,g2)从G体提取能够与g1、g2连接的连接点集
Equivalent(p1，p2)判断是否的拓扑连接是是否相同
len(p)计算路径长度
Replace(G，p，n)替换connector点（p换下n）
distinctbool型数据，true表示拓扑连接有差别，false表示拓扑连接无差别
t_max时间限制
N_max采样个数限制
G所有guard点以及connect点的集合
C. 路径缩短和修剪
如图7(e)所示，由算法1得到的一些路径可能会绕行。导致PGO的第一阶段会使轨迹过度变形，使轨迹变得不光滑。因此，需要通过算法2为深度优先搜索得到的每条路径$P_r$找到一个拓扑上等效的捷径路径$P_s$(如图8所示)。
该算法首先将路径$P_r$一致地离散化为一组$P_d$点，而$P_r$的第一个点即为$P_s$的第一个点。在每次迭代中，如果$P_s$中的一个点第一次（按离散化的顺序）看不见$P_d$中的一个点(两点连线被障碍物占据的体素挡住了)(Line 3,4)，则第一个阻塞了P_s中的前一个点的视野的障碍物对应的占据的体素的中心点就可以被找到(Line 5)。然后通过以垂直于l_d与且与ESDF梯度共面的方向推离障碍物中心点得到新点$P_o$(Line 6)。之后，这个新点被添加$P_s$中(第7行)。这个过程一直持续到最后一个点。(离开的点)
算法2伪代码函数
Disecreze(P)将轨迹统一离散为点集
.front()取点集中的前一个点
Line(a,b)得到连接a和b的线段
LineVisib(l)判断线段是否被障碍物遮挡
BlockPoint(l)得到遮挡线段障碍物的体素的中心点
PushAwayObs(p,l)以l方向推离点得到新点P_o
.push_back(p)将P点加入点集
实时拓扑路径规划
路径引导轨迹优化的算法输出了一组有效的路径，可以引导轨迹优化。我们将它们与PGO进行适当的集成，以便实时重新规划。在飞行过程中，特定视界内的一段全球轨道会被定期检查以确保安全。一旦检测到碰撞，拓扑路线图的构建就会在一个立方体内触发，这是由段的开始和结束位置以及(rx, ry, rz)指定立方体的大小决定的。从路线图中提取的路径被缩短和修剪，之后每条路径调用一个独立的PGO。
值得注意的是，可选择的UVD类的数量随着障碍的数量呈指数增长。因此，在复杂的环境中，为所有路径进行优化可能是很难的。由于这个原因，我们只选择第一次中的k_max中的最短路径，长度超过最短路径rmax的数倍的路径也被排除在外。这种策略限制了复杂性，不会导致潜在最优性的缺失，因为非常长的路径不太可能产生最优轨迹。在实际应用中，我们发现kmax = 5是充分的。
以上内容，本文参考 Robust Real-time UAV Replanning Using Guided Gradient-based Optimization and Topological Paths_无心留踪迹的博客-CSDN博客进行修正与补充。
源码精读
未完待续~~~
