對論文 Focused Trajectory Planning for Autonomous On-Road Driving的解析
本文對Focused Trajectory Planning for Autonomous On-Road Driving此篇論文進行解析，這批論文來自CMU Dolan小組的成果，此小組參加過Darpa城市賽並取得不錯名次，所使用方法均基本在同一體系下。下面進行詳解。
本文所論述的方法僅適用於靜態障礙物避障，切不考慮碰撞檢測成功性，屬於簡單不可行的方法，但具有一定參考意義。名義上這是對Focused Trajectory Planning for Autonomous On-Road Driving論文中的方法的簡化。
本方法的結構是這樣的：
核心結構上由兩個planner組成，第一個是爲了減少計算量所設計的‘’粗糙‘’planner, 計算出一點直線段構成的0階連續曲線，然後擬合成三階多項式（曲率連續）。第二個planner 再上一個planner的基礎上對曲線進行進一步優化，是曲率更加平滑，同時保證橫向偏差儘量小。
第一個planner:
這張圖表示了planner的工作流程，首先我們均勻採樣參考線上的點，參考線一般選擇路的中心線。參考線的採樣長度用delta_s 表示， 在參考線法向我們均勻撒點，採樣長度用delta_L表示。兩者關係符合：
然後我們還要建立代價函數：
wd是weight, d 表示相鄰的兩個互相連接的點的長度，o表示lateral offset, 我們不希望這個曲線太長，這樣運動效率就不高，同時我們也不希望曲線偏離中心線。最後一個delta是碰撞檢測：
要解得目標函數是：
用動態規劃的方式很容易就可以解出來，然後對這些點構成的直線段在進行一次三階多項式擬合，獲得一條粗糙的原始曲線。
接下來就是進行第二次優化，下圖表示了第二次優化和第一次的粗糙曲線的關係：
我們已經有了黑線，也就是上一個planner擬合出來的三階多項式曲線，現在我們要在這個曲線的附件做一個優化，至於到底如何偏離就是我們需要優化的內容了，數學關係如下：
我們要在原有的基礎上，在法向在移動一個距離Oi.
中間還有一些變量的關係如下：
意思是，首先一定距離一定是在路的邊界以內的，還有幾個關係式幫助你計算路的曲率。
最後我們要優化對象是：
我們希望優化後的曲線累計曲率儘量小，同時我們還希望累計橫向偏差儘量小。
從上面的優化過程可以看出，能夠調節的兩個參數也就是每個planner裏面的weight了，下圖給出了不同weight下的優化結果：
對原有曲率的優化效果也很明顯：
但是問題也很明顯了，原來的三階多項式是那個黑線，如果黑線旁邊就有障礙物，優化出來的曲線極有可能發生碰撞。作者沒有對優化後的曲線對於原曲線的偏移做出限制，同時對新曲線也不做碰撞檢測，生成出來的優化曲線其實是不一定能通過碰撞檢測的，也就是毫無作用的，不知道作者在做什麼鬼。
速度方面，我們也要做一定的限制：
分別是：
道路限速，橫向加速度限制，以及車輛本身處於舒適性考慮的加減速限制
第一個很好做，道路車速限制是提前給出的。
第二個計算一下曲率(這就是爲什麼上面我們特地計算出道路曲率的原因)，安裝公式給出此時的最高可行速度。
第三個按照以下算法即可： 意思是相鄰的兩個採樣點的距離是知道的，最高的加減速限制也是已知的，這就決定了我們在兩個採樣點之間的最大的速度變化區間。按照這個理念計算出對應的車速即可。
總的來講這個方法是有巨大缺陷的，怎麼讓他發了IV2013真的不懂。我考慮了一下解決方法還是有的，第一，優化的方向是需要被確定的，通常來講，如果原有曲線往上偏離，那麼障礙物一定在下面，那我直接設定優化過程必須是向上優化而不是向下優化，這可以避免大多數的碰撞情況。第二，如果還是可以隨意優化我們在第二個planner裏面還是要做一次碰撞檢測的，但這次碰撞檢測可以使用滯後碰撞檢測，二次優化獲得的結果如果不通過碰撞檢測，就把次優解拿過來做碰撞檢測，幸運的話在幾次碰撞檢測中就能獲得可行解。
