上週我們釋出了無人駕駛技術的規劃（上），車輛基於高精地圖、感知和預測模組的資料來進行這一規劃。我們詳細介紹了A*演算法，並規劃了前往目的地的地圖路線。

本期我們將繼續學習路徑-速度解耦規劃和Lattice規劃來構建免碰撞軌跡。規劃是無人駕駛開發過程中最困難的部分之一。希望開發者能夠繼續努力，充分掌握規劃的相關知識，在開發的道路上越走越遠。

1、路徑-速度解耦規劃

路徑-速度解耦規劃將軌跡規劃分為兩步：路徑規劃、速度規劃。首先在路徑規劃步驟中生成候選曲線，這是車輛可行駛的路徑。使用成本函式對每條路徑進行評估，該函式包含平滑度、安全性、與車道中心的偏離以及開發者想要考慮的其他任何因素。然後按成本對路徑進行排名並選擇成本最低的路徑。

下一步是確定沿這條路線行進的速度。我們可能希望改變在該路徑上的速度，所以真正需要選擇的是與路徑點相關的一系列速度，而不是單個速度。我們將該序列稱作“速度曲線”。我們可以使用優化功能為路徑選擇受到各種限制的良好速度曲線。通過將路徑和速度曲線相結合可構建車輛行駛軌跡。

2、路徑生成與選擇

為了在路徑-速度解耦規劃中生成候選路徑，首先將路段分割成單元格。然後對這些單元格中的點進行隨機取樣。通過從每個單元格中取一個點並將點連線，我們建立了候選路徑。通過重複此過程可以構建多個候選路徑。使用成本函式對這些路徑進行評估並選擇成本最低的路徑，成本函式可能考慮以下因素：與車道中心的偏離、與障礙物的距離、速度和曲率的變化、對車輛的壓力、或希望列入的任何其他因素。

3、ST圖

選擇路徑後的下一步是選擇與該路徑關聯的速度曲線，一個被稱為“ST 圖”的工具可以幫助設計和選擇速度曲線。在ST圖中，“s”表示車輛的縱向位移、“t”表示時間。ST 圖上的曲線是對車輛運動的描述，因為它說明了車輛在不同時間的位置。由於速度是位置變化的速率，所以可以通過檢視曲線的斜率從 ST 圖上推斷速度。斜坡越陡則表示在更短的時間段內有更大的移動，對應更快的速度。

4、速度規劃

為構建最佳速度曲線需要將 ST 圖離散為多個單元格。單元格之間的速度有所變化，但在每個單元格內速度保持不變，該方法可簡化速度曲線的構建並維持曲線的近似度。在 ST 圖中可以將障礙物繪製為在特定時間段內阻擋道路的某些部分的矩形。例如，假設預測模組預測車輛將在 t0 到 t1 的時間段內駛入的車道。由於該車將在此期間佔據位置 s0 到 s1，因此在 ST 圖上繪製了一個矩形，它將在時間段 t0 到 t1 期間阻擋位置 s0 到 s1。為避免碰撞，速度曲線不得與此矩形相交。既然有了一張各種單元格被阻擋的 ST 圖，便可以使用優化引擎為該圖選擇最佳的速度曲線。優化演算法通過複雜的數學運算來搜尋受到各種限制的低成本解決方案。這些限制可能包括：法律限制，如速度限制；距離限制，如與障礙物的距離；汽車的物理限制，如加速度限制。

5、優化

路徑-速度解耦規劃在很大程度上取決於離散化。路徑選擇涉及將道路劃分為單元格，速度曲線構建涉及將 ST 圖劃分為單元格。儘管離散化使這些問題更容易解決，但該解決方案生成的軌跡並不平滑。

為了將離散解決方案轉換為平滑軌跡，可使用“二次規劃”技術（Quadratic Programming)。二次規劃將平滑的非線性曲線與這些分段式線性段擬合。儘管二次規劃背後的數學運算很複雜，但對於我們的目的而言，細節並不是必需的。我們只需簡單使用幾種不同的優化包中的一種，包括一種由 Apollo 推出的執行方案來生成平滑的軌跡，一旦路徑和速度曲線就緒，便可以用其構建三維軌跡。

6、路徑-速度規劃的軌跡生成

回顧一下端到端路徑-速度解耦規劃。假設我們正在路上行駛，感知系統觀察到一輛緩慢行駛的車輛離我們越來越近。首先，在這輛車的周圍生成多條候選路線，使用成本函式對這些候選路徑進行評估並選擇成本最低的路徑。然後使用 ST 圖來進行速度規劃，根據其他車輛隨時間變化的位置阻擋了 ST 圖的部分割槽域。優化引擎可幫助確定該圖的最佳速度曲線，該曲線受制於約束和成本函式。我們可以使用二次規劃讓路徑和速度曲線變平滑。最後，將路徑和速度曲線合併構建軌跡。這裡的軌跡在速度較快時為紅色，在速度較慢時為藍色。我們使用該軌跡來安全地繞開其他車輛並繼續我們的旅程。

7、Lattice規劃

現在來學習一種稱為 Lattice 規劃的軌跡生成方法，通過使用 Frenet 座標可以將環境投射到縱軸和橫軸上，目標是生成三維軌跡：縱向維度、橫向維度、時間維度。

可以將三維問題分解成兩個單獨的二維問題，這是通過分離軌跡的縱向和橫向分量來解決的。其中一個二維軌跡是具有時間戳的縱向軌跡稱之為 ST 軌跡，另一個二維軌跡是相對於縱向軌跡的橫向偏移稱之為 SL 軌跡。

Lattice 規劃具有兩個步驟即先分別建立 ST 和 SL 軌跡，然後將它們合併為生成縱向和橫向二維軌跡。先將初始車輛狀態投射到 ST 座標系和 SL 座標系中，通過對預選模式中的多個候選最終狀態進行取樣。來選擇最終車輛狀態。對於每個候選最終狀態構建了一組軌跡將車輛從其初始狀態轉換為最終狀態，使用成本函式對這些軌跡進行評估並選擇成本最低的軌跡。

8、ST軌跡的終止狀態

根據情況可以將狀態分成 3 組：巡航 、跟隨、停止。巡航意味著車輛將在完成規劃步驟後定速行駛，實際上在對圖上的點進行取樣，在圖中橫軸代表時間，縱軸代表速度。對於該圖上的點，這意味著汽車將進入巡航狀態，在時間 t 以 s 點的速度巡航，對於這種模式，所有最終狀態的加速度均為零。

下一個要考慮的模式為跟隨車輛，在這種情況下要對位置和時間狀態進行取樣，並嘗試在時間t出現在某輛車後面，在跟隨車輛時，需要與前方的車保持安全距離，這時速度和加速度將取決於要跟隨的車輛，這意味著在這種模式下，速度和加速度都會進行修正。

最後一種模式是停止，對於這種模式只需對汽車何時何地停止進行抽樣，這裡速度和加速度會被修正為 0 。

9、SL軌跡的終止狀態

根據這樣一個假設來進行SL 規劃，即無論車輛進入怎樣的終止狀態，車輛都應該穩定地與車道中心線對齊。這意味著只需要在一個小區域內，對橫向終止位置進行取樣。具體來說取樣的是道路上相鄰車道中心線周圍的位置。為了確保穩定性，汽車駛向的終止狀態應該與車道中心一致。當用橫向位置與縱向位置作圖時 ，想要的候選軌跡應該以車輛與車道對齊並直線行駛而結束。為了達到這種終止狀態，車的朝向和位置的一階和二階導數都應該為零。這意味著車輛既不是橫向移動的，那是一階導數；也不是橫向加速，那是二階導數。這意味著車輛正沿著車道直行。

10、Lattice規劃的軌跡生成

一旦同時擁有了 ST 和 SL 軌跡，就需要將它們重新轉換為笛卡爾座標系。然後可以將它們相結合構建由二維路徑點和一維時間戳組成的三維軌跡。ST 軌跡是隨時間變化的縱向位移，SL 軌跡是縱向軌跡上每個點的橫向偏移。由於兩個軌跡都有縱座標 S，所以可以通過將其 S 值進行匹配來合併軌跡。