Apollo規劃子產品：基于二次規劃（QP）的車輛路徑規劃（論文研讀）

文章目錄

• 一、背景概述
• 二、算法思想
• 三、算法流程
• • 1、Frenet架構下的路徑規劃
  • 2、參考線的生成和平滑
  • 3、路徑邊界決策與計算
  • 4、路徑優化
• 四、應用場景

一、背景概述

存在挑戰：

對于自動駕駛車輛，路徑規劃不僅需要計算安全，而且要計算舒适路徑，即幾何特性發生合适變化的路徑，也必須滿足車輛的非完整限制。在城市駕駛環境中，車輛經常需要在狹窄的通道中穿梭。這些因素使得自動駕駛車輛的路徑規劃成為一個挑戰性問題。

解決問題

道路被兩側的許多車輛/障礙物堵塞，為生成符合車輛運動學且平滑的路徑，避免在複雜環境中發生碰撞，提出了一種新的二次規劃方法，該方法生成完全避免碰撞能力的最優路徑。

背景知識

差分限制的車輛的配置空間包括三維，二維（x，y）用于指定車輛某個參考點的坐标，通常為後軸中心或質心，一維θ用于指定地圖框中車輛的前進方向，即航向角。自行車模型假設車輛因車輪轉向而行駛一周。除了（x，y，θ）之外，還将一個次元κ（即轉向産生的圓的曲率）合并到配置空間中，以隐式表示轉向角。四維配置空間提供了更精确的位姿描述，并幫助控制器子產品更好地設計回報控制。

路徑規劃的目标是計算映射參數p的函數q∈ [0，1]到特定配置（x，y，θ，κ），并滿足一系列要求：1）無碰撞：路徑不能導緻車輛與障礙物碰撞。軌迹規劃總體政策将碰撞避免分為靜态和動态障礙物避免，路徑規劃旨在實作與靜态障礙物的碰撞避免。2）運動學上可行：路徑必須在車輛的運動學限制範圍内，以便車輛能夠實際跟随。

軟要求：舒适性，減少方向盤的擺動、減少不必要的急轉彎、

二、算法思想

（1）規劃架構從笛卡爾坐标系轉換為Frenet架構。将路徑規劃問題解耦為兩個階段：第一階段，隻關注從地圖資料中獲得可平穩駕駛的參考線，這是精确的地圖幀轉換的前提條件；第二階段，重點關注路徑生成的Frenet架構中的優化。

（2）提出 piecewise-jerk路徑公式。表示根據參考線的空間參數使用密集離散點序列的Frenet幀中的路徑，每個點包含到參考線的橫向距離及其一階和二階導數。在連續點之間，使用恒定的三階項拼接它們并保持路徑的二階連續性。該公式由于全局離散化和全局二階可微允許獲得更大的靈活性和平滑的路徑。

（3）第三種政策是在 piecewise-jerk路徑公式中使用優化來計算路徑。優化過程在Frenet架構中從安全駕駛走廊中搜尋安全且運動學上可行的解，通過考慮障礙物占用、道路邊界等進行計算。

ps： 工作空間： 機器人有形狀和大小，難以進行運動規劃；配置空間：将機器人看成一個點。将機器人半徑 r 對障礙物各向進行膨脹。

三、算法流程

1、Frenet架構下的路徑規劃

關鍵思想是将運動規劃問題從地圖架構轉換為Frenet架構，Frenet架構是根據假設的平滑定義的參考線。對于結構化環境中的車輛運動規劃，平滑的參考線可以是車輛目标要遵循的道路的中心線。

給定一條平滑的引導線，車輛在地圖架構中的運動被解耦為兩個獨立的一維運動，即在Frenet架構中沿引導線的縱向運動s和與引導線正交的橫向運動l。

此外，與将橫向運動l表示為時間t的函數不同，另一種方法是将橫向運動表示為空間參數s的函數。一階和二階導數l‘=dl/ds，l’’=d2l/ds2表示l 的一階和第二階變化率。該表示基本上定義了Frenet幀中的幾何形狀，即路徑。是以，路徑規劃問題現在是尋找滿足上述限制的函數l（s）。(為社麼不是橫向運動s(l)函數，了解是橫向運動是縱向運動引起的)

2、參考線的生成和平滑

在Frenet架構中規劃的前提條件是一條參考線。參考線表示為地圖上的位置坐标序列，即p0 =（x0，y0），p1=( x1, y1), . . . , pn−1 = (xn−1, yn−1)。沒有額外的幾何資訊，例如方向、曲率等。

（為什麼要進行參考線平滑）參考線是地圖和Frenet架構之間的“橋梁”，其平滑度極大地影響了計算路徑的品質。根據Map和Frenet轉換，為了準确地将路徑映射到曲率水準，參考線必須一階連續，即參考線必須在曲率導數水準上連續。

該文提出了一種基于優化的平滑算法，并在優化公式中考慮以下方面：

優化目标： 低且平滑的路徑曲率不僅有助于減少控制器在路徑跟蹤中的不穩定性或過沖，而且還提高了駕駛體驗的舒适性。在優化過程中，會對參考線曲率及其變化率進行懲罰。對于百度Aopollo平台針對的城市駕駛場景，始終鼓勵車輛停留在車道上。是以，建議參考線靠近車道中心。

限制： 為了考慮可能的地圖誤差，允許輸入點在一定程度上偏離其原始坐标，以實作可能更高的平滑度，同時保持線的原始形狀。此外，為了確定安全，平滑的引導線必須保持在車道邊界内。

過去的工作實作了一種用于參考線平滑的非線性優化算法。算法使用五次螺旋曲順序列對引導線進行模組化，并計算一個使空間長度和幾何特性波動最小的序列。該方法直接優化了直線的幾何特性。

然而，螺旋曲線的使用在曲線形狀及其位置屬性之間形成了非線性關系。是以導緻了一個困難且計算密集的非線性優化問題。

本文提出了一種基于快速二次規劃（QP）的算法，獲得了可接受的實證結果。在構造QP問題時，變量是n個輸入點的位置變量。給定連續三個點pi−1，pi，pi+1，最小化從圓周率點到圓周率中點的歐幾裡德距離pi−1和pi+1。幾何上，pi離中點越近，這三個點越直，是以曲率及其曲率變化率越小。允許這些點在一定程度上偏離其原始位置，以獲得可能更直的直線。點的偏差也包括在目标函數中

參考線平滑的輸出是具有優化位置的點序列。使用連續點位置之間的有限差分計算每個離散點上的幾何資訊。對于離散點之間的任何點，其幾何資訊使用線性插值近似。

3、路徑邊界決策與計算

路徑優化是在Frenet架構中執行的。目标和限制是基于生成的參考線制定的。讨論如何為優化過程生成可行的搜尋區域。路徑的完整搜尋區域可能包含一組幾何同倫組，例如，從左側或右側通過一個靜态障礙物形成兩個組。選擇最佳同倫群是一個非常重要的決策過程。文章提出了一種基于啟發式搜尋的決策政策，該政策考慮了道路資料（特定路段的可用車道）、ego車輛和靜态障礙物的位置和幾何資訊，并建立了數學不等式，供後續優化過程使用。通過兩個步驟實作：車道使用率決策和路徑邊界生成。

**A、車道利用決策：**計算出ego車輛要使用的道路車道。簡單的方法是使用所有可用車道作為可駕駛區域。會帶來如下問題：

（1）最終生成的路徑可能不必要地跨越幾個車道，會造成一定的風險。

（2）如果道路狹窄（例如在住宅區），且有一些其他障礙物阻塞了道路的一部分，我們需要臨時借用相鄰或反向車道通過。

利用基于規則的決策樹，根據交通規則、ego車輛狀态（速度、前進角度等）和障礙物資訊（障礙物類型、位置等）确定要使用的車道。該子產品輸出一組可用車道，供ego車輛根據參考線的空間參數使用。

B、路徑邊界生成：

這一步是通過考慮車輛位置和周圍障礙物，精細處理從上一步到特定邊界的可用車道。将空間次元s離散為預定義的分辨率∆s、 沿着參考線。從第一個點s0開始，我們向前搜尋。

如果si處沒有障礙物，則可用車道的邊界直接用作下限和上限橫向邊界，然後我們繼續下一個點。

如果si處存在靜态或低速障礙物，基于估計的自我車輛在si−1處的橫向位置l和障礙物的可用間距，對可能的繞行方向進行排序。然後從最寬的可行值開始，搜尋後面的sj。如果沿着這個選擇的搜尋在中間失敗，回溯并嘗試其他方向。

重複此過程，直到達到搜尋範圍（該文實作中為160m）。作為上述深度優先排序搜尋的結果，路徑邊界可能是所有采樣si的最合理邊界。此處不考慮動态或高速障礙物，因為它們将由速度規劃子產品處理。

該步驟的輸出是路徑邊界函數，該函數以空間參數s為輸入，并輸出ego車輛的安全邊界（lmin，lmax）。

4、路徑優化

這部分給定計算路徑邊界函數lB的Frenet架構中的詳細優化公式。

A、最優性模組化

在模組化路徑的最優性時，我們考慮以下因素：（1）無碰撞：路徑不得與環境中的障礙物相交。（2）最小橫向偏移：如果沒有碰撞風險，車輛應盡可能靠近車道中心。（3）最大障礙物距離：最大化車輛與障礙物之間的距離，以便車輛安全通過。這個因素可以表示為車輛與其相應路徑邊界中心之間的距離。

B、公式

公式表示為權重和，如下：

該方法根據空間參數s将路徑函數 l（s）離散化到一定的分辨率∆s，并使用這些離散化的點來控制路徑的形狀，并近似評估限制滿意度。

思想是将一維函數離散到二階導數水準，并使用恒定的三階導數項來“連接配接”連續離散點。位置變量的三階導數通常被稱為加加速度。該公式被稱piecewise-jerk。

優化變量是ｓ，ｌ，ｌ‘，ｌ‘’，這些空間參數控制路徑的形狀。在連續點之間，piecewise-jerk假設一個恒定的三階項來連接配接它們。該項的值通過有限差分二階項獲得。

三階項僅在兩個連續點内恒定；在不同的連續點中，其他值是可能的。為了保持路徑的連續性，在點i和i+1之間引入了額外的等式限制：

路徑優化的完整分段加加速度是最小化的ｌ，ｌ‘，ｌ‘’。

C、運動可行性的可變邊界估計

除了滿足幾何連續性和安全邊界限制外，計算出的路徑必須遵循車輛的運動學限制，才能在實體上行駛。由于運動學限制是在地圖架構中定義的，是以由于複雜的地圖-法語架構轉換，很難在Frenet架構中直接實施該限制。該方法對Frenet架構中變量的數值邊界進行了估計，以隐式地實施限制。

運動學可行性的最重要因素是路徑的曲率。路徑中一個點的曲率定義為以下方程：

為了簡化，做出假設：（１）車輛幾乎平行于行駛引導線，即假設車輛的航向角與相應點的引導線方向相同，是以∆θ=0。（2） 橫向加速度ｌ’‘在數值上很小，假設為0。曲率近似公式如下：

給定車輛的運動學模型（見圖2）和車輛的最大轉向角αmax，可以計算車輛的最大曲率：

将l的線性限制添加到運動學可行性的優化程式中，如下所示：

四、應用場景

随機規劃器（RRT等）：通用的方法，難以利用道路結構化資訊，用于特殊場景如停車等。

離散搜尋方法（State lattice search等），通過串聯一系列預先計算的路徑段或政策來計算路徑。通過檢查一個段的結束配置是否足夠接近目标段的開始配置來完成連接配接。路徑的形狀嚴重依賴于網格的離散化，這限制了路徑的靈活性，用于簡單場景。

基于優化的方法，靈活；直接在全局/地圖架構中運作二次規劃，以直接計算軌迹（同時進行路徑和速度規劃）。軌迹在地圖架構中被精細地離散化，這些離散化的位置屬性被用作優化變量。優化過程疊代以找到最小化目标函數的軌迹，該目标函數結合了安全措施和舒适因素。優點是提供了優化模組化的直接實施。此外，由于路徑/軌迹被密集離散化以用作優化變量，這些方法實作了對路徑/軌迹的最大控制，以處理複雜場景。

該文方法是将規劃問題轉換到不同的空間以降低規劃複雜性。在Frenet架構中執行軌迹規劃，該架構由平滑的駕駛參考線定義。車輛的運動被解耦為兩個一維運動，即相對于引導線的縱向和橫向運動。對于每個一維規劃問題，以多項式的形式生成一組候選運動。

具體地，提出了一種新的基于優化的自主車輛路徑規劃方法，可用于一般道路駕駛場景（如道路被兩側的許多車輛/障礙物堵塞）。該方法将路徑規劃分解為兩個主要階段：第一階段，驅動參考線平滑過程生成平滑線，這是在Frenet架構中規劃的前提條件；在第二階段，路徑優化器使用分段jerk公式找到最優且運動可行的路徑。利用了道路結構并實作了更清晰的場景了解。

在普通PC上計算效率高，平均總計算時間為40ms（用于引導線平滑的20ms，用于路徑查找的15ms）。該方法在百度Apollo開放平台上釋出，并已部署在硬體上，用于多種場景中的道路測試。

整理不易，歡迎點贊訂閱，一起學習交流！

參考與推薦學習連結：

論文： Optimal V ehicle Path Planning Using Quadratic Optimization for Baidu Apollo Open Platform

作者： Yajia Zhang* | Hongyi Sun | Jinyun Zhou | Jiacheng Pan | Jiangtao Hu | Jinghao Miao

學習連結： 二次規劃（QP）算法：https://zhuanlan.zhihu.com/p/325645742

代碼：https://github.com/ApolloAuto/apollo/tree/master/modules/planning

圖檔來源于網絡和參考連結，侵權聯系删