解析論文：A real_time motion planner with trajectory optimization for autonomous vehicles
論文的核心結構是這樣的：
1，規劃上是時空分離的，橫向規劃是建立曲率kappa關於位置s的函數，縱向上建立速度v關於位置s的函數。
2，函數建立後使用lattice planner 的方式進行撒點，生成對應的cost function,選擇cost 最低的曲線作爲最優曲線。
3，完成曲線選取之後還多做一步優化，在時間和空間上進行滾動優化，進一步完善輸出結果。
總體的結構如圖：
接下來我們分析每一個模塊。
空間上的曲線生成：
關於空間上的曲線生成，我們有一些數學推導，我寫在了手稿中：
可以看出，按照lattice planner的方法，我們希望寫出一個曲率kappa關於位置s的多項式，至於多項式應該是幾階的，取決於我們定多少約束。文章中列舉了我上圖中寫的五個約束：起點處的曲率，終點處的曲率，終點處的x位置，終點處的y位置，終點處的heading 對於起點處heading的變換量。五個約束意味着多項式應該是4階的，所以作者使用了四階多項式進行空間上的曲線生成。
作者是這麼寫的，到底是會議論文，質量就是不行。說的是quartic四階多項式，寫的卻是三階，還亂扯說s也是個位置參量？我反正是佛了。再次強調，這裏是四階多項式。
時間上的曲線生成
作者強調，時間上的曲線生成也應該是關於位置s的多項式，而不是關於時間t的多項式，因爲我們剛纔在空間上做的曲線生成已經選擇了 函數是關於位置S的函數，因此爲了保證一致性，這裏也是關於s的函數。作者表示這裏選擇一個三階函數就差不多了。
三階多項式的參量有四個，我們定義四個約束即可：
作者選擇了起始點和終點的速度和加速度，其中起點的速度和加速度是從車上直接獲得的，終點的速度我們自己採樣，終點的加速度我們希望是0，這樣直接可以獲得這幾個參量的close form的表達：
cost function:
分成了靜態cost 和動態 cost, 論文裏面大家用的都差不多，沒什麼好解釋的：
最後的cost function 就是所有項的加權累計：
進一步速度和軌跡優化，
所謂的進一步優化就是花裏胡哨，做法就是稍微放鬆對終點的約束。對於軌跡優化來說就是我們雖然選擇了某條最優曲線，這個規劃終點的狀態都已定好，現在我們放鬆一點這個約束，終點可以在這個點的附近稍微動一動，做到微調。速度優化也是一樣，終點的採樣速度也可以在小範圍內微調。這種優化沒有意義，因爲你不一定保證微調後的曲線能夠通過碰撞檢測，所以你額外還要做一次碰撞檢測，另外微調的目的是讓曲線更合理，而對曲線合理性的評估已經通過cost function 給出來了，這裏就沒有必要多此一舉了。
