ESDF-free：

被这种类型的障碍物死死卡住的情况：

在一定范围内建立ESDF：

Ego-Planner框架：

找到{p,v} pair：

【注意】：首先根据在障碍物内航迹上的点Q，以及与它相邻但不在障碍物内的两个点，假设从左往右进行飞行，左边点为起点，右边点为终点，通过A*算法求出一条不经过障碍物的蓝色范围航迹，然后取Q点此时的速度方向，通过速度方向求到在Q点的法向平面与障碍物的交点为p，其单位方向向量为v，这两个{p,v}就叫对pair，此时这个d就叫做障碍物安全距离。控制好安全距离即可安全避障。

通过设置将障碍物距离设置为优化函数进行求解：

基于梯度的航迹优化：

平滑性惩罚：

动力学可行性惩罚：

障碍物避障惩罚：

Ego Planner与Fast Planner之间的区别：

【注意】：无需ESDF地图进行3D三线性插值，现在的避障方式时间短，且是连续的。

时间重分配与航迹优化：

时间重分配完，找到优化初值，然后再进行优化：

时间重分配让变长：

航迹优化：初值

新轨迹在新时间上的轨迹与原轨迹在原时间上的轨迹相同：

【注意】：用最小二乘进行求解。

优化问题：

原轨迹已经考虑了避障的要求，只需要在速度方向一致的位置进行优化：