ESDF-free：

被這種類型的障礙物死死卡住的情況：

在一定范圍內建立ESDF：

Ego-Planner框架：

找到{p,v} pair：

【注意】：首先根據在障礙物內航跡上的點Q，以及與它相鄰但不在障礙物內的兩個點，假設從左往右進行飛行，左邊點為起點，右邊點為終點，通過A*算法求出一條不經過障礙物的藍色范圍航跡，然后取Q點此時的速度方向，通過速度方向求到在Q點的法向平面與障礙物的交點為p，其單位方向向量為v，這兩個{p,v}就叫對pair，此時這個d就叫做障礙物安全距離。控制好安全距離即可安全避障。

通過設置將障礙物距離設置為優化函數進行求解：

基于梯度的航跡優化：

平滑性懲罰：

動力學可行性懲罰：

障礙物避障懲罰：

Ego Planner與Fast Planner之間的區別：

【注意】：無需ESDF地圖進行3D三線性插值，現在的避障方式時間短，且是連續的。

時間重分配與航跡優化：

時間重分配完，找到優化初值，然后再進行優化：

時間重分配讓變長：

航跡優化：初值

新軌跡在新時間上的軌跡與原軌跡在原時間上的軌跡相同：

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

優化問題：

原軌跡已經考慮了避障的要求，只需要在速度方向一致的位置進行優化：