ESDF-free：
被这种类型的障碍物死死卡住的情况：
在一定范围内建立ESDF：
Ego-Planner框架：
找到{p,v} pair：
【注意】：首先根据在障碍物内航迹上的点Q，以及与它相邻但不在障碍物内的两个点，假设从左往右进行飞行，左边点为起点，右边点为终点，通过A*算法求出一条不经过障碍物的蓝色范围航迹，然后取Q点此时的速度方向，通过速度方向求到在Q点的法向平面与障碍物的交点为p，其单位方向向量为v，这两个{p,v}就叫对pair，此时这个d就叫做障碍物安全距离。控制好安全距离即可安全避障。
通过设置将障碍物距离设置为优化函数进行求解：
基于梯度的航迹优化：
平滑性惩罚：
动力学可行性惩罚：
障碍物避障惩罚：
Ego Planner与Fast Planner之间的区别：
【注意】：无需ESDF地图进行3D三线性插值，现在的避障方式时间短，且是连续的。
时间重分配与航迹优化：
时间重分配完，找到优化初值，然后再进行优化：
时间重分配让
变长：
航迹优化：初值
新轨迹在新时间上的轨迹与原轨迹在原时间上的轨迹相同：
【注意】：用最小二乘进行求解。
优化问题：
原轨迹已经考虑了避障的要求，只需要在速度方向一致的位置进行优化：
