網絡化自動駕駛軌跡規(guī)劃算法設計說明書V

1、網絡化自動駕駛軌跡規(guī)劃算法設計說明書作者胡學敏評審人日期2015/12/26環(huán)宇智行科技有限公司華為技術有限公司修訂記錄作者修訂時間版本號修改內容胡學敏2015/12/29V1.11. 更新三種算法的原理圖和相關實現2. 更新2.3的接口部分。胡學敏2016/1/3V1.31. 更新2.1的軌跡規(guī)劃流程。 2. 調整部分格式目錄1概述32框架和接口32.1軌跡規(guī)劃流程32.2算法選擇模塊42.3接口42.4算法介紹43算法1：虛擬車道算法43.1算法原理43.2算法實現54算法2：曲線軌跡簇算法54.1算法原理54.2算法實現61） 基本路線的構建62） 候選路徑的生成73）路徑的選擇85算法

2、3：A*算法85.1算法原理85.2算法實現91 概述圖1.1 整體路徑規(guī)劃過程示意圖軌跡規(guī)劃是實現智能車自主駕駛的關鍵模塊。要實現對自動駕駛車輛的控制前提就是根據車輛的當前道路環(huán)境，行駛狀態(tài)以及整體交通環(huán)境規(guī)劃出一條合理的軌跡，以便于下層控制器進行車輛控制。整個軌跡規(guī)劃從CSU的整體路網規(guī)劃（考慮整體車流和交通狀況）到RSU的局部交通流規(guī)劃（平衡局部車流），再到OBU的具體行駛軌跡規(guī)劃。以下著重討論OBU中的規(guī)劃算法。2 框架和接口2.1 軌跡規(guī)劃流程 圖2.1.1 OBU軌跡規(guī)劃 軌跡規(guī)劃流程：1. OBU，用戶在UI選擇目的地，OBURSU，RSU CSU，CSU做“全程車道級路徑規(guī)劃”

3、，輸出結果為“車道編號集合、task參數”，保存在CSU的該OBU_session上，然后CSU RSU：下發(fā)路徑規(guī)劃結果，RSU保存在該OBU_session里。RSU OBU：下發(fā)路徑規(guī)劃結果，保存在內存。2. OBU將感知融合的數據和網絡路徑規(guī)劃的結果投影到當前道路環(huán)境中，調整當前OBU規(guī)劃的軌跡，并顯示在UI上。OBU軌跡規(guī)劃數據源：地圖數據（包括道路邊界點，道路拓撲結構，車道線信息），感知融合數據（車線，障礙物信息等）。OBU軌跡規(guī)劃頻率與規(guī)劃距離：OBU規(guī)劃模塊每1秒規(guī)劃一次，每次規(guī)劃100米。2.2 算法選擇模塊 OBU規(guī)劃模塊創(chuàng)建算法選擇類，OBU直接調用該類的接口獲得軌跡，算

4、法選擇類支持注冊多種算法，并選擇一種算法進行軌跡規(guī)劃。2.3 接口OBU調用算法選擇模塊的接口，包括：l 當前車道與相鄰車道構成的4條線（當前車道與左右兩條車道，一共3條車道，需要4條車道線來標記）。l 障礙物、障礙物形狀、障礙物速度。l 路徑規(guī)劃參數。2.4 算法介紹OBU中軌跡規(guī)劃主要包括以下三種算法1) 虛擬車道算法：總是沿著道路的中線行駛，發(fā)現障礙物采用跟馳(/剎車)或換道，換道行為相當于虛擬了一個走到另一個車道的虛擬車道，然后繼續(xù)沿中線行駛。2) 曲線軌跡簇算法：以車的方向和道路的曲率構建曲線坐標系，然后在此坐標系內擬合曲線軌跡簇，選擇最佳曲線，然后轉化為笛卡爾坐標系。3) A*算法

5、：把待規(guī)劃區(qū)域的地圖劃分成笛卡爾坐標系的柵格（如邊長20cm），把障礙物定義為1的柵格，可行駛的為0的柵格，按目標做最短路徑搜索，再把搜索結果做平滑處理。算法選擇：虛擬車道法簡單可靠，且曲線平滑，舒適性好，可作為默認算法。但是此算法簡化為跟馳和換道兩個動作，應對不同車姿（曲線坐標軌跡簇算法的優(yōu)勢）和發(fā)現最佳路徑（A*算法的優(yōu)勢）的能力很弱，路況復雜時容易找不到出路。此時應降級為“曲線坐標軌跡簇算法”算法，此算法發(fā)現道路的能力較強，曲線也很平滑。如果此算法也得不到結果，則降級為“A*算法”，實現“有空就鉆”的路徑搜索（但行駛曲線不夠平滑），仍然沒有出路時，停車（通常為完全堵車了）。3 算法1：虛

6、擬車道算法3.1 算法原理總是沿著道路的中線行駛，發(fā)現障礙物采用跟馳(/剎車)或換道，換道行為相當于虛擬了一個走到另一個車道的虛擬車道，然后繼續(xù)沿中線行駛。圖 虛擬車道算法3.2 算法實現1） OBU通過車道線檢測實時確定當前車道線的位置，同時OBU加載地圖中的車道模型（車道寬度、車道數目、特征點）。2） OBU將當前檢測車道與車道模型進行匹配，確定當前車道和鄰近幾條車道。3） 車輛總是沿著車道中心線行駛，當車輛需要換道時，根據道路環(huán)境，虛擬出一條與相鄰車道的交匯車道，完成變道動作。4 算法2：曲線軌跡簇算法4.1 算法原理以車的方向和道路的曲率構建曲線坐標系，然后在此坐標系內擬合曲線軌跡簇，

7、選擇最佳曲線，然后轉化為笛卡爾坐標系。圖 軌跡簇算法4.2 算法實現算法實現的流程如圖4所示：圖4.2.1 曲線軌跡簇算法流程圖1） 基本路線的構建基本路線是由包含整體路徑規(guī)劃的道路中心點所組成的航跡點的樣條擬合構建的。參數化曲線經常被用來建立道路的集合模型并且定義運動路徑。采用參數化三次樣條來定義基于航跡點構建的基本路線的參數化曲線。2） 候選路徑的生成行駛路線應該能夠引導車輛隨著全局路線行走并且避開障礙物。為了達到這個目的，建立了一個基本路線的曲線坐標系統(tǒng)。在基本路線的曲線坐標系統(tǒng)中設計避免障礙物的光滑策略。圖5顯示了在笛卡爾坐標系統(tǒng)中的路徑和基本路徑的曲線坐標系統(tǒng)之間的幾何關系。該圖中代

8、表在整體路徑中移動距離的基本路徑的弧線長度s成為了曲線坐標系統(tǒng)的水平坐標，最近的偏移q成為了垂直坐標。圖 笛卡爾坐標與曲線坐標的幾何關系為了生成路徑，路徑的曲率是由基于基本路徑的曲率路徑的側向偏移所決定的。在計算路徑曲率時需要側向偏移的導數和二階導數，因此側向偏移函數要有一定的光滑性側向偏移函數描述如下 其中弧長si和當前位置的側向偏移qi由定位步驟給出。為當前位置車頭和基本路徑的夾角。采用邊界條件，我們可以得到三次多項式的系數。路徑規(guī)劃算法生成了一些候選路徑。每個路徑有一個明顯側向偏移qf. 覆蓋道路的所有候選路徑只是在側向偏移上有些許不同。路徑上的側向偏移變化能使自主駕駛車輛避開障礙物。3

9、）路徑的選擇我們要從生成的眾多路徑中，需要采用搜索算法來找到使帶權重的代價函數的線性組合極小的一條最優(yōu)路徑。代價函數要考慮安全性，光滑性以及路徑的穩(wěn)定性這最為主要的三個因素。這里i是路徑編號，Cs是路徑的安全代價，Ck是路徑的光滑性代價，Cc是路徑的平穩(wěn)性代價。每個代價都有相應的權重因子。而權重因子的取值與車輛自身，環(huán)境因素等實際情況有關。計算公式如下： 根據安全性代價、光滑性代價、連貫性代價，經過編程調試，采用經多次檢驗的算法，我們可以選擇出一條使得總的代價函數取得最小值的路徑。5 算法3：A*算法5.1 算法原理把待規(guī)劃區(qū)域的地圖劃分成笛卡爾坐標系的柵格（如邊長20cm），把障礙物定義為1

10、的柵格，可行駛的為0的柵格，按目標做最短路徑搜索，再把搜索結果做平滑處理。圖 地面格網和A*算法5.2 算法實現A*算法最為核心的部分，就在于它的一個估值函數的設計上：f(n)=g(n)+h(n) 其中f(n)是每個可能試探點的估值，它有兩部分組成：    一部分，為g(n)，它表示從起始搜索點到當前點的代價（通常用某結點在搜索樹中的深度來表示）。    另一部分，即h(n)，它表示啟發(fā)式搜索中最為重要的一部分，即當前結點到目標結點的估值， 一種具有f(n)=g(n)+h(n)策略的啟發(fā)式算法能成為A*算法的充分條件是：&#

11、160;     1、搜索樹上存在著從起始點到終了點的最優(yōu)路徑。      2、問題域是有限的。      3、所有結點的子結點的搜索代價值>0。      4、h(n)=<h*(n) （h*(n)為實際問題的代價值）。    當此四個條件都滿足時，一個具有f(n)=g(n)+h(n)策略的啟發(fā)式算法能成為A*算法，并一定能找到最優(yōu)解。對于一個搜索問題，顯然，條件1

12、,2,3都是很容易滿足的，而條件4： h(n)<=h*(n)是需要精心設計的，由于h*(n)顯然是無法知道的，所以，一個滿足條件4的啟發(fā)策略h(n)就來的難能可貴了。不過，對于圖的最優(yōu)路徑搜索和八數碼問題，有些相關策略h(n)不僅很好理解，而且已經在理論上證明是滿足條件4的，從而為這個算法的推廣起到了決定性的作用。且h(n)距離h*(n)的呈度不能過大，否則h(n)就沒有過強的區(qū)分能力，算法效率并不會很高。對一個好的h(n)的評價是：h(n)在h*(n)的下界之下，并且盡量接近h*(n)。  求V0->V5的路徑，V0->V5的過程中，可以經由V1，V2，V3，V4各點達到目的點V5。下面的問題，即是求此起始頂點V0->途徑任意頂點V1、V2、V3、V4->目標頂點V5的最短路徑。圖5.2.1 A*算法求解示例通過上圖，我們可以看出：A*算法最為核心的過程，就在每次選擇下一個當前搜索點時，是從所有已探知的但未搜索過點中(可能是不同層，亦可不在同一條支路上)，選取