ego和fast的对比和总结

Fast-Planner

Fast-Planner采用基于启发式搜索和线性二次型最小时间控制的动态路径搜索方法，在离散空间中有效地搜索一条安全可行且时间最短的轨迹，然后，在设计的B样条优化中对初始路径进行优化，利用B样条的凸包特性来结合梯度信息和动态约束。最后，将轨迹表示为非均匀B样条，研究了导数控制点与时间分配之间的关系。基于这些关系，采用迭代时间平差方法，在避免保守约束的情况下，将不可行的速度和加速度从剖面中挤出。
前端kinodynamic路径搜索模块源自于首先针对无人驾驶汽车提出的混合状态A *搜索，它在体素栅格地图中搜索一条关于时间长度和控制代价最小的安全和动力学可行轨迹。
基元生成：通过三个独立的一维时间参数化多项式表示轨迹，无人机系统相当于一个线性时不变系统，以此建立状态方程，在给定的当前状态下结合持续时间内的离散控制输入进行状态扩展，即离散控制空间，得到下一时刻无人机可能的位置。
成本函数：考虑时间和控制成本方面最优。通过EdgeCost()计算离散化输入u和持续时间τ生成的运动基元的成本，路径由J个基元组成，从开始状态到当前状态的成本就是J个基元的成本的总和。为了加快搜索，需要设计一个启发式函数，Fast中应用庞特里亚金（Pontryagins）最小原理计算从当前状态到目标状态的成本函数最小化的轨迹
分析扩展：由于采用离散控制输入方法，当开集弹出一个节点时，使用上述的成本函数计算从xc到xg的轨迹，利用庞特里亚金最小原理计算出控制输入u后，计算相应的状态点，如果该节点通过了安全性和动态可行性检查，则提前终止搜索。该方法可以提高搜索效率。
搜索得到初始路径后，利用B样条对轨迹进行优化。
尽管在路径搜索和优化中限制了动力学可行性，但有时仍然会得到不可行的轨迹。原因是梯度信息倾向于延长整体轨迹，同时将其推离障碍物。因此，四旋翼飞行器必须更积极地飞行才能在同一时间内飞行更长的距离，如果原始运动已经接近物理极限，这就会导致过度激进的运动。因此采用了一种基于导数控制点与非均匀B样条的时间分配（结跨度）关系的时间调整方法。

Ego-Planner

ego-planner设计了一个无ESDF的基于梯度的局部规划框架EGO（EGO取自 ESDF-free Gradient-based lOcal），由基于梯度的样条优化器和后处理过程组成。
与传统方法查询预计算的ESDF不同，ego通过比较障碍物内的轨迹和一条引导的无碰撞路径来建模碰撞成本。然后将力投影到碰撞轨迹上，并生成估计的梯度，将轨迹从障碍物中包裹起来。在优化过程中，轨迹会在附近障碍物之间反弹几次，最终终止在安全区域内。用这种方法只在必要时计算梯度，避免在与局部轨迹无关的区域计算ESDF。
避撞力估计：首先给定一条不考虑碰撞的B样条曲线进行优化，对于迭代中检测到的每个碰撞段，生成无碰撞路径，碰撞段的每个控制点Qi将在障碍物表面被分配一个锚点p_ij，并具有相应的斥力方向矢量v_ij，以此形成{p，v}对，每一个{p, v}对仅属于一个特定控制点。
基于梯度的轨迹优化：优化问题考虑平滑惩罚，碰撞惩罚及可行性惩罚。
fast中的平滑惩罚仅考虑了轨迹的几何信息而没有考虑时间分配，ego结合了Usenko等人的“Real-time trajectory replanning for mavs using uniform b-splines and a 3d circular buffer,”中，平滑惩罚被公式化为轨迹(加速度、加加速度等)的平方导数的时间积分的方法，不需要时间积分，仅惩罚加速度和加加速度的平方。
fast中的碰撞惩罚通过直接计算控制点与障碍物的距离得到障碍物的斥力，从而将控制点推离障碍物，但它需要确保相邻控制点之间的距离足够小才可以保证轨迹安全，ego中构造了一个二次连续可微的惩罚函数jc，自变量为安全距离与控制点到障碍物距离的差值，随着距离的减小，减小函数的斜率

对所有Qi进行成本合并，得到总碰撞成本Jc
fast中的可行性惩罚计算速度和加速度与阈值的差值的平方来进行可行性约束，ego中考虑了速度，加速度，加加速度，并设计了代价函数进行约束
时间重分配：fast将轨迹参数化为一个非均匀B样条曲线，并在速度或加速度不可行时迭代的延长结跨度，但是这样可能会导致轨迹的高阶不连续性。ego基于上述优化后的安全轨迹，通过合理的时间重分配重新生成均匀的B样条轨迹，然后，提出了一种各向异性曲线拟合方法，使轨迹自由优化其控制点以满足高阶导数约束，同时保持与原安全轨迹几乎相同的形状。首先像Fast-Planner一样计算超出限制的比例，即计算不可行速度，加速度，加加速度超出限制的比例，并取其中的最大值，这表示应该将新轨迹的时间分配相对于原轨迹延长多少，以此得到新的时间跨度，生成新轨迹，然后通过优化完善平滑性和可行性，增加适应度惩罚函数，为从新轨迹上的点到相应的原轨迹上的点的各向异性位移的积分，由于拟合曲线Φs已经是无碰撞的，因此为两条曲线分配低惩罚权重的轴向位移以放宽平滑度调整限制，并以高惩罚​​权重的径向位移来避免碰撞。（具体公式看ego-planner的介绍）解该优化问题就可以得到最优解。