文中ppt來源：深藍學院，強烈推薦
重新學習一下軟約束軌跡優化的理論知識。
Distance-based Trajectory Optimization
Motivation：
硬約束的可能的不足：
安全走廊內的區域是等價的，所以軌跡有可能離障礙物很近，控制不好就撞了。
對於視覺無人機，受限於視覺傳感器的范圍和質量，如果有噪點，那飛行走廊質量就下降了。
軟約束的不足：
不能保證每一種約束都滿足，而是傾向於滿足。如果目標函數設計的不好，是有可能碰撞的。
問題描述：
目標函數包括光滑項、碰撞項、動力學項。
光滑項：根minimum snap中的是一樣的。直接用上一節課推導的閉式解。區別在於本解不在限制所有的waypoints固定，而是只有首位固定，也就是dF中很多項都放在了dP中。（這里理解即可，先不推導）
碰撞懲罰項：沿着軌跡積分，積的軌跡點所在的距離場產生的推力。
這里就需要ESDF地圖，歐式距離場，障礙物內是-1，隨着遠離障礙物，依次為1，2，3....
那其實我自己也可以很簡單的構建，一圈一圈往外擴就行啦。
注意：這里是沿着曲線的積分，而不是對時間的積分。如果是對時間的積分，時間變短（速度變快），也可以使得積分結果變小，而我們想要的是因為距離障礙物遠而得到的。
如何積分：
將ds近似成速度*時間，曲線分成很多段直線段。
動力學懲罰項：
對於超過速度和加速度限制的進行懲罰。與碰撞懲罰項類似。
目標函數求導/Jacobian：
整個問題，肯定不是凸的，因為首先ESDF距離場就不是凸的，那碰撞懲罰項就不是凸的。
這里也沒有辦法轉化成凸優化的問題，所以只能通過非線性優化求解：寫出目標函數->求導數。
第一項Js的求導，這個比較簡單，對dp求導即可（注意這里的dp跟minimum snap中推導的有些差異，中間的waypoints不在固定，所以都加到了dp中）
第二項Jc的求導，對c(pt) * ||v(t)||求導。這里μ代表了x,y,z方向。
先對c(pt)求導，c()這個函數就是自己定義了，根據離障礙物的位置遠近，定義一個。
到現在為止，起碼導數都是可以推導出來的，如果推導不出來，就只能數值上加擾動。
一階導數是Jacbian,代表梯度。優化的話，一階導數就可以了。
二階導數是海森，代表梯度的梯度。牛頓法的話是二階導數。
ESDF地圖：
可以用指數函數作為cost function c(p(t))。當然，簡單的話，我覺得可以用一個分段函數。
動力學項，也可以用指數的函數，超過限制時，爆炸式增長。
數值優化：
無約束非線性優化問題，求最小值，也就是求導數為零。
不是全局最優的，初值敏感。
利用梯度，往導數的負方向走。
二階方法就不是簡單的往梯度的下降的方向了，比如牛頓法，是用-一階導數/二階導數
LM方法，結合了一階和二階方法，lambda很大時，就是一階法，lambda很小時，就是二階法。
ceres--最小二乘、非線性優化
NLopt--收錄的求解器特別多，作planning經常用。
寫出目標函數，寫出梯度，就可以用求解器求解，上邊的兩個求解器，甚至都不用你寫梯度。
初值選擇：
基於梯度的優化，初始值如何選擇。
方法1.用前端得到的路徑，好處是是不碰撞的，但光滑性很差。
方法2.前端路徑，然后用閉式minimum snap求解以后，作為初值。好處是光滑，但是不安全。
一般來說，我們更願意初始解是安全的。基於這個初始解，再加上指數爆炸上升的碰撞懲罰函數，可以確保最后軌跡安全。
優化策略：
第一次優化規划，只保留Jc碰撞項。使得軌跡離障礙物遠一點。
第二次再把Js和JD加進來。
缺點：
碰撞項和動力學項，都需要積分，而且需要離散成很多小段，如果離散的太多細密，對算力復雜度是負擔。
解決方法：B-樣條，B-樣條和貝塞爾一樣，具有凸包性質，就不用積分了，直接限制控制點即可。
這里的光滑項也是TEB planner使用的。
