大家好，我已經把CSDN上的部落格遷移到了知乎上，歡迎大家在知乎關注我的專欄慢慢悠悠小馬車（https://zhuanlan.zhihu.com/duangduangduang）。希望大家可以多多交流，互相學習。
在百度Apollo Lattice軌迹規劃算法中，針對跟車（follow）和超車（overtake）2種不同的場景，使用了ST圖在縱向上來采樣障礙物車輛周邊的目标點，作為拟合軌迹的終點。跟車時，在ST圖中障礙物下方采樣，即自車速度小于并趨近于障礙物車速度；超車時，在ST圖中障礙物上方采樣，即自車速度大于障礙物車速度。至于橫向采樣，因為參考線會切換到目标車道中心線，故橫向一般圍繞0（參考線即s軸）左右對稱采樣，如 [-0.5，0，0.5]。
因為Lattice Planner并沒有對變道場景單獨處理，常會發生變道軌迹規劃失敗的情況。提升變道成功率，便成了非常重要的任務。
在做變道時的軌迹規劃時，我查到有些論文采用了建立變道評估模型以選擇最佳變道插入間隙（gap）的方法，以指導軌迹規劃的目标點所在區間。對比這個思路和Lattice的思路，這種模型在規劃前提前篩選出了最優區間，減少了采樣點數目，降低了規劃的計算量，同時，采樣點的減少，卻也降低了最終規劃出的軌迹的最優性。是以，我覺得Lattice是對這個思路的泛化，也啟發了我使用ST圖規劃變道軌迹的方法設計。
Lattice中，ST圖用來描述自車所在車道中的障礙物縱向運動情況，也就是縱向速度。在變道場景中，我們需要用ST圖反映變道目标車道的障礙物速度。假設自車前方沒有障礙物（在自車車道上）（圖1），目标車道的ST圖如圖2，則可以在
區間内、且無障礙物占據的空白處采樣，作為自車可到達的目标狀态。然後由cost functions評價哪一條軌迹是最優軌迹。因為高速場景中自車不能倒車，故s<0的情況可以忽略。采樣前，也可以對欲采樣區間的長度進行檢驗，過短的區間可直接忽略。
圖1
圖2
若自車E前方有障礙物D，如圖3所示，且與該障礙物橫向相鄰區間内無障礙物，則E依然可以在縱向采樣時到達A-B、B-C之間的間隙（A-D、B-D之間的縱向距離要大于某個距離門檻值，否則自車E無法安全的穿過間隙）。即障礙物D對于上述采樣過程沒有影響。
圖3
若自車E前方有障礙物D，如圖4所示，且與該障礙物橫向相鄰區間内有障礙物B（或B-D雖然不緊密相鄰、但相距較近，小于某距離門檻值），則B-D“卡”住了E向前縱向采樣的區間上限，該上限為障礙物B的車尾縱向坐标。自車隻能在目标車道的B車尾後方範圍内縱向采樣。
因為ST圖隻反映一條車道的縱向運動資訊，對于B-D的考察，則把相鄰2條車道的橫向限制納入進來，使得橫縱向資訊結合起來。
圖4
按照上述思路進行軌迹生成，有2種可能會導緻規劃失敗，一是采樣區間都不符合長度限制，沒有有效的采樣點；二是生成的軌迹無法通過有效性檢測（如加速度和jerk的限制）和碰撞檢測。當變道軌迹規劃失敗時，自車應該繼續沿目前車道行進，繼續尋找變道的機會。為實作這一點，在按上述政策在目标車道采樣後，增加對自車車道的橫縱向采樣，并差別對待2車道采樣的優先級。
算法實作後的仿真效果：
自動駕駛運動規劃仿真 lattice
自動駕駛運動規劃仿真 lattice
自動駕駛運動規劃仿真 lattice