1.規劃簡介

路徑規劃是指通過一定的規則，找到一條通過世界的路徑來達到我們想去的地方。規劃的第一步是路線導航，側重於研究如何從地圖上的A點前往B點。在路線導航時，將地圖資料作為輸入，並輸出可行駛路徑。手機導航系統是路線導航的一個示例。在Apollo中，可以通過路線規劃模組處理該任務。一旦我們構建出高水平的路線，我們就會放大至軌跡規劃。該軌跡由一系列點定義，每個點都有一個關聯速度和一個指示何時應抵達那個點的時間戳。通過軌跡規劃，我們可以做出微妙的決策，以避開障礙物，併為乘客創造平穩的乘車體驗。在Apollo中，我們通過規劃模組處理該任務。路線規劃的目標是，找到從地圖上的A前往B的最佳路徑。軌跡規劃的目標是找到避免碰撞和保持舒適度的可執行軌跡。

路徑規劃使用三個輸入，第一個輸入為地圖，Apollo提供的地圖資料包括公路網和實時交通訊息。第二個輸入為我們當前在地圖上的位置。第三個輸入為我們的目的地，目的地取決於車輛中的乘客。

2.將地圖轉為圖形

當人們試圖在地圖上找到從A到B的路線時，通常會沿著道路追蹤路徑，以檢視是否存在通往目的地的任何路徑，這被稱為搜尋。Apollo也通過搜尋來查詢路線，但它使用了更智慧的搜尋演算法。在進行智慧搜尋演算法以前，我們需要將地圖資料重新格式化為“圖形”的資料結構。

該圖形由“節點”(node)和“邊緣”(edge)組成。節點代表路段，邊緣代表這些路段之間的連線。例如：在交叉路口，汽車可從節點1移動到節點2、節點3或節點4，反之亦然。

 我們可以對一個節點移動到另一個節點所需的成本進行建模。例如在現實生活中，拐過一個交叉路口比直行更費勁，所以從節點1到節點4的成本高於從節點1到節點3的成本。 將地圖轉換為圖形的好處在於，在電腦科學領域中，人們已發現許多用於在圖形中查詢路徑的快速演算法（下一節中會詳細介紹）。一旦我們在圖形中找到了一條好路徑，就可以輕鬆地將圖形中的路徑重新轉換為地圖上的路徑。

3. 路徑查詢演算法：A*

 A* 是經典的路徑查詢處理演算法。在本節中，我們將演示A* 如何通過網格進行工作。我們把網格中的每個單元格當作一個節點。並且規定能夠從任何一個節點移動到與其相鄰的任意節點。這個特殊網格還包含一些阻擋潛在路徑的牆壁（下圖中的灰色部分）。

 對人類來說，通過檢視圖形便可以輕易找出最佳路徑。但對於計算機而言，這並不明顯。計算機必須確認是否存在通往目的地的路徑，並竭盡所能地嘗試所有可能的路徑來從其中選出最優路徑。但是，這需要耗費大量的時間，尤其是在地圖非常大的時候。從任何給定的節點來看，最多有8個用於下一步的選項，因為存在8個相鄰的單元格。對於8箇中的每一個選項，下一步又有另外8個選項。如果我們展開所有節點，搜尋量將暴增至即使最快的計算機也無法處理的程度。

下面我們將展示A*演算法的詳細過程（如下圖）：從初始節點開始，我們需要確定8個相鄰節點中，哪個是最有希望的候選節點。對於每個候選節點，我們考慮兩件事情。首先，我們計算從開始節點到候選節點的成本。然後，我們估計從候選節點前往目的地的成本。計算前往候選節點的成本很容易，因為它已經與我們相鄰。計算從候選節點到目的地的成本很困難。但是，通過簡單計算從候選節點到目的地的距離，我們通常可以做出很好的估計。

我們用變數g和h表示每個成本。g值為開始節點前往候選節點的成本，h值為候選節點前往目的地的估計成本或啟發式成本。根據我們的具體情況，我們可以自定義成本估算方式。例如，交通堵塞會增加前往目的地的成本，所以交通繁忙的路徑具有更高的成本。對於每個候選節點，我們通過新增g值和h值來計算總和，即f值。最佳候選節點是f值最小的節點。每當我們抵達新節點時，我們通過重複此過程來選擇下一個候選節點，而且總是選擇我們尚未訪問過且具有最小f值的節點。這就是A* 演算法，它建立了一條穩定前往目的地的路徑。

現在我們來考慮一下現實世界中的地圖。

假設我們到達了一個交叉路口，我們可以沿著公路直走、左轉或右轉。首先，我們將把這張地圖轉換為具有三個候選節點（left,straight,right）的圖形。接下來，我們將對選項進行評估。在實踐中，拐過交叉路口很費勁，所以我們為left節點分配了更高的g值（g值表示從起始點到候選節點的成本）。在檢視公路選項之後，我們意識到必須走很長的路，才能離開公路並返回我們的目標，所以我們為straight選項分配了更高的h值（h值表示從候選節點到目的地的估計成本）。最後，我們通過將g值和h值相加來計算每個節點的f值。我們看到最低f值實際對應右邊的候選節點。所以，這是我們接下來要前往的節點。

4.軌跡生成

高等級地圖路線只是規劃過程中的一部分，我們仍需要構建沿這條路線前進的低等級軌跡。這意味著要處理一些不屬於地圖的物體：如其他車輛、自行車或行人。例如，我們可能需要與試圖在我們前面掉頭的汽車互動，或者我們可能希望超過一輛在公路上行駛的慢車。這些場景需要更低級別、更高精確度的規劃。我們將這一級別的規劃稱為軌跡生成。

軌跡生成的目標是生成一系列路徑點所定義的軌跡。我們為每個路徑點分配了一個時間戳和速度。由於移動的障礙物可能會暫時阻擋部分路段，軌跡中的每個路徑點都有時間戳。我們可以將時間戳與預測模組的輸出相結合，以確保我們計劃通過時，軌跡上的每個路徑點均未被佔用。這些時間戳和空間上的兩個維度（2D position）共同建立了一個三維軌跡（3D  Trajectory）。我們還為每個路徑點指定了一個速度，用於確保車輛按時到達每個路徑點。

現實世界中的規劃面臨多種約束。首先軌跡應能免於碰撞，這意味著必須沒有障礙物。其次，要讓乘客感到舒適，所以路徑點之間的過渡以及速度的任何變化都必須平滑。再者，路徑點對車輛應實際可行，例如高速行駛的汽車不能立即做180度轉彎。我們不能構建包含這種不可行機動的軌跡。最後，軌跡應合法。我們需要了解每個路徑點的交通法律，並確保軌跡遵守這些法律法規。

在道路的任何兩點，可能會有多個不會發生碰撞、行駛舒適、可行且合法的軌跡。我們如何選擇最佳軌跡呢？答案是使用“成本函式”。成本函式為每個軌跡分配了一個“成本”，我們選擇成本最低的軌跡。軌跡“成本”由各種犯規處罰組成，例如：偏離道路中心，有可能產生碰撞，速度限制，軌跡的曲率和加速度讓乘客感到不舒服等。

軌跡成本將所有這些缺陷聚合為單個數字，這使我們能對不同的軌跡按數字大小進行排名。車輛甚至可能在不同的環境中使用不同的成本函式。例如，高速路的成本函式可能與停車場的不同。

5.Frenet 座標系

我們通常使用笛卡爾座標系描述物體的位置，但笛卡爾座標系對車輛來說並不是最佳選擇。即使給出了車輛位置(x,y)，如果我們不知道道路在哪，就很難知道車輛行駛了多遠也很難知道車輛是否偏離了道路中心。

笛卡爾座標系的替代解決方案為Frenet座標系。Frenet座標系描述了汽車相對於道路的位置。在Frenet框架中，s代表沿道路的距離，也被稱為縱座標。d表示與縱向線的位移，也被稱為橫座標。在道路的每個點上，橫軸和縱軸都是垂直的。縱座標表示道路中的行駛距離，橫座標表示汽車偏離中心線的距離。