本文根據作者的ACC講稿FOAD：Fast Optimization Based Autonomous Driving Motion Planner整理而成。FOAD的主要優點有：1）穩定性，保證了存在底層控制器誤差的情況下的閉環穩定性；2）實時性，可以至少支援10Hz的實時規劃及再規劃頻率；3）廣適性，可以處理多種複雜場景。
在自動駕駛軟體的開發中，運動規劃是最核心的模組之一。它將綜合來自感知，定位和地圖等資訊，規劃出無人車未來一段時間（約10秒）的一系列動作指令（方向盤轉角，油門，剎車等）。為了更加量化地描述這一過程，我們用兩個部分來定義一個運動規劃的問題——目標函式（objective function）和約束（constraint）。運動規劃的最終目的就是找出一條最優的運動軌跡，使其能夠最小化（或者最大化）目標函式，並且不違背任何約束。在自動駕駛中，這兩部分主要包含了以下內容：
目標函式：
駕駛行為：例如舒適性，能耗和駕駛效率等；
軟性交通規則：例如車道保持以及限速等；
約束：
碰撞規避：無人車不能在任何時刻與任何障礙物發生碰撞。障礙物可以是靜止的也可以是運動的；
硬性交通規則：例如紅綠燈指示，停車線等；
車輛動力學：所規劃的軌跡需要滿足車身動力學方程，以及方向盤轉角限制，加速度限制等。
機器人學家們提出了多種方案來解決運動規劃問題，它們按照主要思想可以大致分為搜尋法（search-based），取樣法（sampling-based） 以及數值優化法（optimization-based）等。目前並沒有說哪一種方法就一定是最好的，它們各有所長，主要還是看應用場景。由於這是一門內容相當豐富的學科，我們在此無法詳細介紹，感興趣的同學可以參考參考相關書籍《Principles of Robot Motion》，《Planning Algorithms》，本文將主要介紹我們設計的一種基於數值優化的規劃演算法。之所以選擇使用數值優化法，是因為看中了它相比於其他方法的一些優勢：
（1） 準確性：基於優化演算法的規劃在連續空間中進行，不需要離散化；
（2） 完備性：無論可行區域多麼狹小都能找到解，不像搜尋法等，它們要求解析度足夠小，而過於精細的解析度會使得計算量爆炸式增長；
（3） 高效性：在優化中整條軌跡可以同時進行調整，而不是像搜尋或取樣法那樣需要按照時間順序展開。
但是在無人車的應用場景中，優化演算法同時也面臨著很多挑戰，它們主要來自兩個方面：
（1） 非凸可行域（non-convex feasible set）：我們首先簡單介紹一下凸集(convex set)的概念。如圖一（a）中所示，在一個凸集中，任意取兩點x和y，其連線段上的任意一點任然屬於這個集合。若一個集合無法滿足上述條件則它就是非凸的。如圖一（b）所示，假設O集合代表了一個障礙物，其包含的區域我們不能進入，這就使得我們的可行域變成了非凸的。而對無人車來說，有可能到處都是障礙物，這使得整個可行域呈現高度非凸性。由於優化中的大多數工具都依賴凸的性質，因此這種高度非凸性給數值優化帶來了很大的困難。
圖一：（a）橢圓表示一個凸集，其中任意兩點x，y的連線段上的點都在此集中。（b）橢圓表示障礙物，障礙物之外的區域為可行域，由於此可行域集合不是凸的，因此它是一個非凸可行域。
（2） 高維度(high dimensionality)：雖然我們用來描述一輛車的當前狀態的維度並不高，比如其位置x,y一共二維。然而當我們把整條軌跡上的狀態疊起來時，就會形成一個高維度的向量[x1,y1,...,xN,yN]。這裡下標表示時間戳，在我們的應用中N大概是40~50步，這樣整個向量的維度就達到接近一百維。而我們的優化正是以這個高維向量作為決策變數的。
上述挑戰使得用優化方法來解自動駕駛運動規劃變得十分困難，而自動駕駛又對實時性要求極高。事實上現在在自動駕駛上的數值優化規劃演算法的應用都做了較大的妥協，要麼是減少了所考慮的軌跡長度，使得無人車變得短視，要麼是進行了大量的線性化，使得最終結果不夠精確。
我們提出了一種名為凸可行集的優化演算法[1]，在一系列模擬場景測試中，我們發現它不僅能非常穩定地在錯綜複雜的障礙物中找出一條最優軌跡，而且其效率遠高於現有的演算法（如圖二）。
圖二：凸可行集演算法（CFS）與序列二次規劃（SQP）和內點法（ITP）在Matlab（XXX-M）和C++（XXX-C）上的計算效率比較。
將CFS演算法應用於一個車輛模型後，得力於其快速性和處理非凸優化問題的有效性，無人車能夠在極複雜的環境中快速找到一條到達目標地點的最優運動軌跡（如圖三）。
圖三：藍色方塊為無人車，黃色方塊為障礙車，紅色虛線框為無人車需要到達的目標地點。
然而以上只是開環地規劃了一條從起點到終點的軌跡，在無人車的應用中，為了應對不斷變化的環境，我們必須根據新接收到的環境感知資料不斷地重新規劃。這時在每次重規劃時直接使用CFS演算法將使得系統變得不穩定，原因是底層軌跡跟蹤控制無法完美跟蹤規劃出來的軌跡，導致下一時刻的系統約束可能受到違背。為了應對此問題，我們提出了軟約束凸可行集（SCCFS）演算法。我們在約束邊界附近加了一片緩衝區，在這個緩衝區內製造了一定的排斥勢場，使得約束條件更加魯棒。最終我們成功地將演算法應用在了無人車的運動規劃中[2]，下面展示一些應用結果。首先是一個雙車道多車超車場景模擬：
圖四：雙車道超車場景。其中藍色方框代表無人車，紅色方框代表其他車輛，紅色點畫線代表所規劃的軌跡。
接下來是一個停車場的場景模擬，無人車需要繞開車輛和行人然後停入指定車位：
圖五：停車庫場景。其中黃色方塊代表行人，藍色虛線框為指定停車位，藍色實線為參考路徑。無人車需要繞開車輛行人停入車位中。
除了模擬以外，我們還在真車中實現了該演算法，並應用於上述類似場景。超車場景：
停車場場景：
具體實現方法可參見論文：
[1] Changliu Liu, Chung-Yen Lin, and Masayoshi Tomizuka, “The convex feasible set algorithm for real time optimization in motion planning”, to appear in SIAM Journal on Control and Optimization. arXiv:1709.00627.
[2] Jianyu Chen, Changliu Liu, and Masayoshi Tomizuka, “FOAD: Fast optimization-based autonomous driving motion planner”, American Control Conference, IEEE, 2018, pp. 4725-4732.