用于自主駕駛的反應式路徑規劃的制作方法
【專利摘要】本發明涉及用于自主駕駛的反應式路徑規劃。公開了一種適應性地重新生成用于自主駕駛操縱的規劃路徑的方法。基于行駛道路中感測到的物體生成物體圖。重新設置并且致動計時器。生成用于在感測到的物體周圍自主地操縱車輛的規劃路徑。沿著所述規劃路徑自主地操縱車輛。基于來自基于車輛的裝置的感測到的數據來更新物體圖。基于更新的物體圖執行安全性檢驗以便確定所述規劃路徑是否可行。響應于現有路徑不可行的決策重新生成規劃路徑，否則作出關于所述計時器是否已期滿的決策。如果所述計時器未期滿，則重新執行安全性檢驗，否則返回以重新規劃路徑。
【專利說明】
用于自主駕駛的反應式路徑規劃
技術領域
[0001] 實施例涉及自主路徑規劃。
【背景技術】
[0002] 自主和半自主公路駕駛和諸如避免碰撞的高級駕駛員輔助系統均需要路徑規劃。 路徑規劃必須對主車輛動態以及道路上其它靜態和動態的物體的變化作出反應。所規劃的 路徑必須產生在道路邊界內的安全無碰撞路徑，其對于根據諸如最大橫向加速度/加加速 度（jerk)的車輛動態約束進行主車輛控制而言也必須是可行的。已知的路徑規劃技術或者 沒有考慮到主車輛和其它運動的目標車輛的動態，或者對于實時應用在恰當時間中作出反 應而言過于計算密集。

【發明內容】

[0003] 實施例的優點在于一種用于自主駕駛操縱的快速路徑規劃技術，其對于主車輛和 其它運動車輛的動態以及主車輛周圍的靜止物體作出反應。該技術利用德洛內三角剖分 (Delaunay Triangulation)方法來識別用于生成規劃的路徑的線段。所識別的線段基于成 本-距離函數來選擇，其考慮了各種因素，包括:最短長度、從之前規劃的路徑的偏移、從車 道中心的偏移、每個所選擇的線段相對于相鄰線段的斜率，以及與其它車輛和目標的距離。 此外，例程通過以下方式來改善規劃的路徑:識別從規劃的路徑偏移的狹帶(corridor)并 且識別狹帶內的平滑路徑。該技術進一步通過如下方式來確定重新規劃的路徑的可行性： 識別車輛的橫向加速度和與其它動態車輛的距離。此外，該技術在預定時間段之后將重新 確定規劃路徑;然而，該技術將在預定時間段期間連續地檢驗現有路徑的安全性。因此，本 文描述的技術通過僅以定時的間隔或者當現有路徑不再可行時重新生成規劃路徑來減少 重新生成規劃路徑所需要的時間量;不過，不斷地在定時間隔之間執行檢驗，以核實規劃的 路徑是否保持可行。
[0004]實施例設想一種用于自主駕駛操縱的適應性地重新生成規劃路徑的方法，所述方 法包括如下步驟：（a)通過基于車輛的裝置獲取與在行駛道路中感測到的物體相關聯的物 體數據；（b)基于所述在行駛道路中感測到的物體由處理器構造物體圖（object map) ; (c) 重新設置并且致動計時器；（d)由所述處理器生成用于在所述感測到的物體周圍自主地操 縱車輛的規劃路徑，所述規劃路徑基于成本-距離函數生成；（e)沿著所述規劃路徑自主地 操縱所述車輛；（f)基于來自所述基于車輛的裝置的更新的感測到的數據來更新所述物體 圖；（g)基于所述更新的物體圖確定所述規劃路徑是否可行；（h)響應于所述規劃路徑不可 行的決策，返回至步驟(a);否則繼續至步驟確定所述計時器是否已經期滿；以及 (j)響應于所述計時器期滿返回至步驟(a);否則，返回至步驟(f)。
[0005]本發明還公開了以下技術方案： 方案1. 一種適應性地重新生成用于自主駕駛操縱的規劃路徑的方法，所述方法包括 以下步驟： (a) 由基于車輛的裝置獲得與行駛道路中感測到的物體相關聯的物體數據； (b) 基于所述行駛道路中感測到的物體由處理器構造物體圖； (c) 重新設置并且致動計時器； (d) 由所述處理器生成用于在所述感測到的物體周圍自主地操縱所述車輛的規劃路 徑，所述規劃路徑基于成本-距離函數生成； (e) 沿著所述規劃路徑自主地操縱所述車輛； (f) 基于來自所述基于車輛的裝置的更新的感測到的數據更新所述物體圖； (g) 基于更新的所述物體圖確定所述規劃路徑是否可行； (h) 響應于所述規劃路徑不可行的決策，返回至步驟(a);否則繼續至步驟(i); (i) 確定所述計時器是否期滿；以及 (j) 響應于所述計時器期滿返回至步驟(a);否則返回至步驟(f)。
[0006] 方案2.根據方案1所述的方法，其中，生成所述規劃路徑還包括以下步驟： 基于所述行駛道路中感測到的物體識別虛擬節點；以及 應用德洛內三角剖分以便在所選擇的虛擬節點當中生成三角形。
[0007] 方案3.根據方案2所述的方法，其中，所述虛擬節點包括車道虛擬節點、主車輛虛 擬節點、終點虛擬節點以及移位掃描節點，其中，所述車道虛擬節點表示所述道路的車道邊 界，其中，所述主車輛虛擬節點表示所述主車輛的位置和搜索空間的起點，其中，所述終點 虛擬節點表示所述搜索空間的終點，以及其中，所述移位掃描節點表示基于所檢測到的動 態物體相對于所述主車輛的速度的動態物體的位移。
[0008] 方案4.根據方案3所述的方法，還包括以下步驟:沿所述三角形的邊緣識別頂點， 所述頂點沿每個三角形邊緣均勻地間隔；以及 在每個三角形內的每對頂點之間形成線性線段。
[0009] 方案5.根據方案4所述的方法，其中，只有當相應的頂點對屬于相同的相應三角 形并且當所述頂點不屬于所述相應三角形的相同邊緣時，才形成連接所述相應三角形的所 述相應頂點對的相應線性線段。
[0010] 方案6.根據方案4所述的方法，其中，不在所述道路的車道邊界上形成頂點。
[0011] 方案7.根據方案5所述的方法，其中，通過從每個三角形中選擇相應線性線段來 從所識別的主虛擬節點至所識別的終點虛擬節點生成所述規劃路徑，其中，從每個三角形 選擇的每個線性線段從所述所識別的主虛擬節點至所述所識別的終點虛擬節點形成連續 規劃路徑。
[0012] 方案8.根據方案7所述的方法，其中，基于成本-距離函數識別每個所述所選擇的 線性線段，所述成本函數生成為關于所述規劃路徑的長度的距離函數分量、每個線段的相 對斜率、所述規劃路徑相對先前確定的路徑的偏移、從當前駕駛車道的中心的偏移以及距 周圍障礙的偏移距尚。
[0013] 方案9.根據方案8所述的方法，其中，對每個所述相應距離函數分量進行加權，以 便識別在所述成本-距離函數中的使用程度。
[0014] 方案10.根據方案9所述的方法，其中，所述成本-距離函數由以下公式表示：
其中，仍是對于第i條路徑從源頂點至目標頂點的距離，#|、_、通|、:1|和_是第 i條路徑的第J個線段的距離函數分量，《、《、@和9是恒定權重系數，以及巧^和/^分別是 從所述源頂點至目標頂點的多條路徑和每條路徑中的多個線性線段。
[0015] 方案11.根據方案?ο所述的方法，其中，所述距離函數分量發I:對應于所述第i條 路徑的第it線段的實際長度，并且其中，:B|由以下方程表示：
其中，分別指明所述對應線段的源頂點和目標頂點。
[0016] 方案12.根據方案11所述的方法，其中，距離函數分量/?對應于所述第i條路徑 的第J個線段相對于所述主車輛或者所述車道的前行方向的相對斜率，其中，項1?表示如 下：
其中，是所述線性線段在地球參照系中的角度，你遺表示所述主車輛前方的近距離 的正的常數，是調整參數，以及瑪是所述線性線段的位置處的所述車道前行 方向。
[0017] 方案13.根據方案10所述的方法，其中，距離函數分量1?對應于所述第i條路徑 的第it線段相對于所述主車輛或者所述車道的前行方向的相對斜率，其中，項表示如 下：
其中，鐵I是所述線性線段在地球參照系中的角度，你處表示所述主車輛前方的近距離 的正的常數，是調整參數，以及是所述線性線段的位置處的所述車道前行 方向。
[0018] 方案14.根據方案10所述的方法，其中，所述距離函數分量￡^對應于所述搜索 圖中的線段至在先前規劃時間找到的所述主車輛路徑的距離，以便防止從所述先前的規劃 路徑顯著偏離，其中，所述距離函數分量_表示如下：
其中，和if?是所述線段的源頂點和目標頂點距所述先前的主車輛路徑的加權偏 移，并且權重作用于更接近所述主車輛的頂點。
[0019] 方案15.根據方案10所述的方法，其中，所述距離函數分量對應于所述搜索圖 中的線段至當前車道的中心線的距離，其中，所述距離函數分量表示如下：
其中，:_|、:_|和^1||分別是每個線性線段距所述當前車道的中心、相鄰左車道以及相 鄰右車道的偏移值。
[0020] 方案16.根據方案15所述的方法，其中，距離分量函數確保最短的所述規劃路 徑至少與所述周圍障礙相距安全距離閾值，同時考慮運動目標的動態，并且其中，虛擬勢場 值表示如下：
其中，和是距所識別的所述障礙的縱向安全距離和橫向安全距離。
[0021] 方案17.根據方案8所述的方法，還包括以下步驟:通過識別所述規劃路徑周圍的 狹帶來改善所述規劃路徑，所述狹帶具有從所述規劃路徑偏移的左狹帶邊界和右狹帶邊 界。
[0022] 方案18.根據方案17所述的方法，其中，基于左狹帶點和右狹帶點形成所述左狹 帶邊界和所述狹帶邊界，其中，所述左狹帶點和所述右狹帶點通過使用以下條件來確定：
其中，:__^}和_|\:3|^指明在線段的局部參照系中的第計左狹帶點和右狹帶 點。
[0023] 方案19.根據方案17所述的方法，還包括以下步驟:在所述狹帶內重新生成規劃 路徑，所述規劃路徑隨著各種路徑點處的估計的曲率和曲率變化率而改變。
[0024]方案20.根據方案19所述的方法，還包括以下步驟:確定所述重新生成的規劃路 徑的可行性，所述重新生成的規劃路徑的可行性隨著所述車輛的橫向加速度和與距周圍物 體的距離而改變。
【附圖說明】
[0025] 圖1是路徑規劃系統的框圖。
[0026] 圖2是反應式路徑規劃技術的流程圖。
[0027]圖3是示例性規劃路徑。
[0028]圖4是涉及障礙的示例性駕駛情景。
[0029]圖5是虛擬掃描節點的示例性情景。
[0030] 圖6是用于掃描的虛擬節點的德洛內三角剖分的示例性演示。
[0031] 圖7是展示頂點的示例性圖。
[0032]圖8是基于生成的規劃路徑。
[0033]圖9是規劃路徑狹帶。
[0034] 圖10是在規劃路徑狹帶內重新生成的規劃路徑。
[0035] 圖11是在規劃時間之間進行主車輛規劃路徑安全性檢驗的圖示。
[0036]
【具體實施方式】
[0037] 圖1圖示了用于車輛的路徑規劃系統10的框圖。車輛(下文稱為主車輛)包括基于 車輛的圖像捕獲裝置12和至少一個基于感測的裝置14。
[0038]圖像捕獲裝置12捕獲車輛外部的圖像。分析由圖像捕獲裝置12捕獲的圖像，以便 檢測由車道標線表示的道路的行駛車道。
[0039] 基于感測的裝置14可以包括但不限于:基于雷達的裝置、基于激光雷達的裝置、基 于超聲波的裝置，以便感測車輛周圍的靜止物體和運動物體兩者。
[0040] 處理器16處理由圖像捕獲裝置12捕獲的圖像數據以及由感測裝置14感測到的感 測數據。處理器16分析相應數據并且識別行駛道路中的物體以便確定用于生成規劃路徑操 縱的規劃路徑。
[0041] 處理器16可以聯接至一個或多個控制器18,以便發起或者致動控制動作，從而生 成規劃路徑操縱。可以致動和控制一個或多個車輛子系統，以便執行規劃路徑操縱。可以被 控制以便執行規劃路徑操縱的相應車輛子系統包括但不限于:轉向控制子系統20、速度控 制子系統22,以及制動控制子系統24。還可以利用通信系統26以便利用車輛間通信將規劃 路徑傳輸至檢測到的目標車輛，從而使檢測到的目標車輛知曉該規劃路徑操縱。
[0042]可以控制轉向子系統20以便致動轉向操縱，從而在主車輛行駛車道中圍繞檢測到 的目標使車輛轉向。
[0043]在當執行車道變更操縱時車輛需要自主輕制動力的情況中制動策略已準備就緒 時，制動子系統24可以啟用電氣制動系統、電液壓制動系統或者液壓制動系統。
[0044]速度控制子系統22可以控制車輛的速度，以便在車道變更操縱期間使車輛或者加 速或者減速。
[0045]圖2圖示了反應式路徑規劃(RPP)技術的流程圖。RPP技術優選地用于公路道路上 的自主駕駛。RPP技術使用從基于圖像的捕獲裝置檢測到的車道數據和來自基于傳感器的 裝置的感測數據并且將數據融合成物體圖，以便生成針對前方短距離的主車輛的路徑路點 (waypoint)。處理器可以包括基于預測的處理器，其能夠被用于針對主車輛追蹤生成的路 徑。RPP技術確保首先，所生成的路徑與諸如其它運動的目標車輛和靜止物體(例如，施工桶 (construct ion barrel))的檢測到的周圍物體相距安全距離。其次，RPP技術確保路徑是當 考慮到主車輛的動態約束時能夠被追蹤的可行路徑。最后，RPP技術確保在車道變更操縱期 間主車輛保持在道路上。
[0046]本文描述的RPP技術被稱為反應式RPP技術，這是因為在短時間段(例如，0.5秒)之 后甚至在之前確定的路徑到達終點之前就基于新的傳感器數據重新生成主車輛路徑。因 此，RPP技術對于車道數據或者物體圖數據的任何變化是反應性的。
[0047] 在步驟30中，啟用例程，并且該例程進行至步驟31。在步驟31中，分析標準以便識 別是否生成新的規劃路徑或者繼續分析當前的規劃路徑。應當理解，RPP技術是重復性的， 因為其基于周圍環境不斷地分析和修改規劃路徑。因此，步驟31中的決定就是貫穿RPP例程 不斷分析的條件的結果。如下條件用于確定是否應當生成新的規劃路徑或者例程是否應當 繼續監測現有路徑。條件包括但不限于：（1)識別現有規劃時間（T plan)是否期滿；（2)確定之 前的主車輛路徑是否不安全；（3)確定之前的主車輛路徑是否不可行；（4)確定從主車輛路 徑的偏移是否大于預定偏移閾值。如果存在這些條件中的任何條件，則例程前進至步驟32， 以便生成下一個規劃路徑;否則，例程前進至步驟42，以繼續分析現有規劃路徑。
[0048]在步驟32中，獲取物體圖數據和車道數據。圖3圖示了RPP技術的示例性情景，其 中，已經在三個不同瞬間生成主車輛路徑。生成車輛規劃路徑的每個瞬間被稱為規劃時間。 用于RPP技術的主要輸入是物體圖和車道數據。物體圖來源于物體圖數據輸入，其包括檢測 到的目標及其對應的掃描點的列表，如由如下表達式中所識別的：
其中，/2是檢測到的目標的數量，是第i個目標的唯一索引號，是變換至地 球參照系的第i個目標的中點，轉和if分別表示第i個目標的前行方向角度和速度， 以&>^)表示地球參照系中第〗個目標的第^個掃描點，以及?是對應于第〗個目標的多個掃 描點。
[0049]車道數據呈兩個三次多項式的系數的形式，所述兩個三次多項式表示左車道標線 和右車道標線。表達式如下：
其中，凡和/〃分別表示主車輛參照系中的左車道標線和右車道標線。
[0050] 車道數據還包括參數，該參數確定視覺傳感器讀數的品質以及車道標線的類型 (諸如實線或者虛線）。
[0051] RPP技術進一步利用其它主車輛數據，諸如車輛在地球參照系中的位置αΑ，ΓΑ)、前 行方向角度3ft、速度KA、偏航率《%，以及縱向加速度％和橫向加速度_￡。
[0052]主車輛參照系是指附接至主車輛的重心的參照系，并且X軸朝向車輛的前方。在每 個規劃瞬間時間地球參照系與主車輛參照系相同。這表明，只要生成新的路徑，地球參照系 就被變換為當前主車輛參照系。這還表明，當前主車輛參照系在規劃時間之間當場固定。 [0053]應當忽略對路徑規劃沒有影響的被檢測為遠離當前主車輛位置的目標。因此，圍 繞主車輛以及在方程（1)中識別出的對規劃路徑具有影響的所有目標形成虛擬窗口。窗口 表示如下：
其中，1_和心^表示圍繞車輛的窗口，1.5 是該窗口的長度并且2 是該窗口的 寬度，在該窗口中計算相應的規劃路徑。每個目標的掃描點也通過使用道格拉斯-普克 (Douglas-Peucker)方法進行簡化，以減少計算成本。
[0054]如果攝像機讀數的品質具有不充分的分辨率，則用于左車道標線和右車道標線的 筆直車道的以下默認值表示如下：
其中，車道系數的單位使得方程(2)的結果以米為單位。
[0055] 在步驟33中，通過添加利用德洛內三角剖分技術形成的虛擬節點來確定用于主車 輛操縱的安全空間。德洛內三角剖分技術在掃描點、相鄰車道和主車輛之間的空間中使用 三角形的組合。德洛內三角剖分技術采用虛擬節點的一部分作為輸入并且形成三角形以表 示由這些節點限定的凸空間。對于本文中所描述的RPP技術，德洛內三角剖分中利用的輸入 節點被限定為車道虛擬節點、主車輛虛擬節點、終點（ending)虛擬節點、以及移位掃描節 點。
[0056] 車道虛擬節點在道路的車道邊界上生成，以確保主車輛路徑處于道路上。車道虛 擬節點被定位成沿著車道邊界彼此相距相等的距離，并且計算如下：
其中，1_11和&§：||：|分別表示沿著左車道邊界和右車道邊界的主車輛參照系中 的虛擬節點，心是被采取為虛擬節點之間的縱向距離的主車輛的長度，是當前車道的寬 度，并且如果左車道標線和右車道標線是實線從而意味著在該側上不存在相鄰車道，則布 爾（Boo lean)變量7i和為真。
[0057]應當理解，方程(5)中的節點需要變換至地球參照系，以便由德洛內三角剖分技術 使用。
[0058]主車輛虛擬節點包括與主車輛的當前位置相距相等距離的兩個虛擬節點。這限定 搜索空間的起點并且由以下參數表示：
[0059] 終點虛擬節點限定搜索空間的終點并且表示如下：
方程(7)中的節點處于主車輛參照系中并且需要變換至地球參照系中。如果左車道標 線和/或右車道標線是實線，則分別忽略虛擬節點__遽)和/或，或者如果在規劃 窗口內未檢測到目標，則忽略上述虛擬節點。
[0060] 移位掃描節點是基于每個檢測到的目標車輛的相對速度移位的節點。方程（1)中 的掃描點根據以下方程所確定的那樣沿著道路移位：
其中，_是變量，表示主車輛到達第i個目標需要的時間量。:If被計算如下：
其中，和I,是恒定參數，指明使掃描點移位的距離和速度的閾值。如果滿足以下三 個條件，則將在三角剖分計算中考慮那些移位的掃描點：

[0061] 圖4表示包括兩輛低速運動的目標車輛沿與主車輛相同的方向運動的示例性情 景。主車輛(HV)左側的目標車輛(TV2)正以大約30 km/h行駛，并且主車輛前方的目標車輛 (TV1)正以大約30 km/h行駛。若干施工桶相對于主車輛位于右車道中。
[0062] 圖5示出如何使對應于TV1的掃描點移位以隨著時間的過去基于TV1的速度預測該 車輛相對于主車輛的位置的示例。注意的是，由于TV2的位置過于接近HV，所以沒有使TV2移 位。這由方程(9)中的第一個條件表不。
[0063]圖6中示出圖4中的示例性情景的德洛內三角剖分演示，其包括車道虛擬節點100、 主車輛虛擬節點102、終點虛擬節點104，以及移位掃描點106。掃描點的移位考慮到目標車 輛動態，使得能夠為主車輛確定安全的路徑。這還具有使所確定的路徑更接近駕駛員選擇 的路徑的優點。
[0064]在步驟34中，生成搜索圖。搜索圖是限定在三角剖分空間中的圖，并且由多個頂點 和連接相關聯的頂點的線段組成。頂點生成并且位于滿足以下條件的特定三角形邊緣上： (1)每個邊緣不是邊界上的邊緣；（2)每個邊緣不連接來自相同目標的兩個掃描點；（3)每個 邊緣不連接兩個車道虛擬節點；（4)每個邊緣長度具有大于閾值(4m)的長度；（5)每個邊緣 處于由車道虛擬節點限定的凸多邊形內。
[0065]如果三角形邊緣滿足上述條件中的每一個，則如圖7所示的那樣沿該邊緣限定頂 點108。頂點108被定位為沿著每個邊緣彼此等距并且在相應的掃描節點對之間或者在掃描 節點與車道虛擬節點之間相等地間隔。以下方程組用于求解頂點：
其中，?場指明第^個邊緣上的第計頂點，和1^氣||^分別表示第上個 邊緣的源節點和目標節點，A是沿著邊緣的頂點之間的恒定距離，A是滿足上述條件的邊緣 的數量，以及_是第i個邊緣的長度。
[0066]如果第i個邊緣在地球參照系中是豎直的（即，則使用以下公式來尋 找頂點：
[0067]除了在方程（13)和（14)中獲得的頂點之外，在當前主車輛位置處添加源頂點。此 外，添加目標頂點，并且該目標頂點的X坐標值大于其它頂點的最大X值并且其Y坐標值是方 程(7 )中的終點虛擬節點的Y坐標的平均值。在迪杰斯特拉算法(Di jkstra ' s a 1 gori thm)中 使用源頂點和目標頂點以找出最短路徑，這將在后文詳細地討論。
[0068]參數A是沿著邊緣的頂點之間的恒定距離，其在生成搜索圖中具有重要作用。如 果A被選擇為小，則圖中的頂點的數量將增加，這導致相應的三角形區域內的空間的圖形 分辨率更加精細，并且使最終路徑的平滑度增加，此外，該最終路徑還更接近駕駛員選擇的 路徑;不過，具有更精細的分辨率將需要算法具有更多的計算能力。因此，當選擇?ΛΒ寸，平衡 是優選的，原因在于在該技術中這是工具調節參數。為了減少計算能力同時保持所選擇的 路徑的合理的平滑度水平，為A和每個三角形邊緣設置兩個候選值。基于所選擇的邊緣至 主車輛以及道路上移位的目標車輛的接近性選擇相應值。如果三角形邊緣接近主車輛或者 目標車輛的移位點，則為A選擇更小的值，否則選擇相對于該更小的值的更大的值。
[0069] -旦確定頂點108，則由圖段(graph segment) 110來連接它們。圖段連接兩個頂 點，這兩個頂點：（1)屬于相同三角形，并且(2)不屬于相同邊緣。示例情景的頂點108和圖段 110在圖7中標繪示出。
[0070] 在步驟35中，通過使用迪杰斯特拉算法來執行搜索，以在搜索圖中尋找連接源頂 點與目標頂點的最短路徑。該最短路徑不是依據傳統距離定義來尋找，而是依據定義如下 的成本-距離函數：
其中，ft是第i條路徑從源頂點至目標頂點的距離，轉、轉、增、轉和_是第i條路徑的 第J個線段的距離函數分量，和Φ是恒定權重系數，以及/^a和/^分別是從源頂點 至目標頂點的路徑的數量和每條路徑中的線段的數量。
[0071] 方程（15)中的項If對應于第i條路徑的第it線段的長度。該項考慮了來自傳統 測量的最短距離。用于確定項的方程表示如下：
其中，分別指明對應線段的源頂點和目標頂點。
[0072] 距離函數中包括方程(15)中的項蹲，使得路徑的實際長度在計算最短距離中發揮 作用。分母中的_:^表示線段的最大可能長度并且被用于將長度成本歸一化至[ο 1]范 圍。權重系數4是正的常數，其需要與其它權重系數一起調整以針對不同情景生成實用的 主車輛路徑。
[0073] 方程（15)中的項對應于第i條路徑的第升線段相對于主車輛或者車道的前行 方向的相對斜率。該參數考慮了轉向操縱的平滑過渡，諸如使轉向操縱中的突變/沖擊最小 化。用于確定項的方程表示如下：
其中，載^是線段在地球參照系中的角度，你邊表示主車輛前方的近距離的正的常數， _嘗_ 1】是調整參數，以及_^_是該線段的位置處的車道前行方向。車道前行方向朽《Μ 表示如下：
[0074] 方程（15)中的項isg;包括在距離函數中，以確保所產生的最短路徑在當前主車輛 前方的鄰近區域中充分地與該車輛前行方向對齊并且隨后與車道前行方向對齊。采用正的 角度__以將成本歸一化至[0 1]范圍，并且權重系數_是調整參數，相比于其它距離函 數分量，該調整參數將影響最短路徑與主車輛的前行方向的對齊。
[0075] 方程（15)中的項與搜索圖中的線段至在先前的規劃時間所找出的主車輛路徑 的距離有關。這防止從先前的規劃路徑的任何顯著偏離，否則這會導致車輛的顯著轉向變 化。用于確定項δ|的方程表示如下：
其中，nf和:sf是線段的源頂點和目標頂點從先前的主車輛路徑的加權偏移，并且具 體地，更多的權重作用于更接近主車輛的頂點，
其中，111??1?指明在先前可行的主車輛路徑中的第女個路點，并且~邊主車輛路 徑路點的數量。
[0076] 距離函數中包括項Dg，使得當前最短路徑（尤其是更接近當前主車輛位置的線 段)被迫充分地接近先前的規劃路徑。這有助于處理器更加有效地追蹤所生成的主車輛路 徑。如前文所描述的，被用于使該成本歸一化至[0 1]范圍，并且是調整參數。
[0077] 方程（15)中的項.1瑪與搜索圖中的線段至當前車道的中心線的距離有關。這涉及 從每個車道的中心的偏移。用于確定項i|的方程表示如下：
其中，和誠 f分別是線段從當前車道的中心、從相鄰左車道以及從相鄰右車道 的偏移值。這些偏移表示如下：
[0078] 距離函數中包括方程（15)中的項$|，使得最短路徑被迫充分地接近距離對應線 段最近的車道的中心。當周圍沒有目標車輛或者主車輛沒有受到威脅時，這將產生車道居 中行為。如前文所描述的，為被用于使該成本歸一化至[0 1]范圍，并且是調整參數。
[0079] 包括方程(15)中的項￡|，以確保最短路徑至少與周圍障礙(諸如其它運動的目標 車輛或者靜止物體)相距安全距離。當規劃無碰撞路徑時，該分量考慮運動目標的動態。參 考方程(15)，基于對應線段頂點的位置使掃描點移位如下：
其中，是第滌路徑的第計線段的掃描節點PfelH的移位點，并且指 明主車輛到達該線段的最大X節點需要的時間段，其表示如下：
接著使用以下方程將移位掃描點變換至線段的局部參照系：
接著從以下表達式獲得第i條路徑的第it線段的虛擬勢場值聲t:
其中，:??和是與障礙相距的縱向安全距離和橫向安全距離，以及_&被用 于使勢場歸一化至[0 1]范圍，并且轉^是調整參數。
[0080] 為了確保所產生的路徑與障礙相距安全距離，優選地選擇使％>在方程（15)中的 所有權重系數當中具有最高值。這在確定用于操縱的規劃路徑中提供了對安全性的最大程 度的強調。應當理解，不同于本文中所描述的權重系數，權重系數可以成比例變化，以適應 制造商列出的駕駛條件。
[0081] 如果線段的長度短于閾值或者其相對于當前車道的相對斜率大于閾值? ，則由當前車道的前行方向角度替換方程(26)中的線段前行方向ife。執行該替換以確保具 有短的長度和/或相對大斜率的任何線段不會引起線段的非真實勢場。
[0082] 針對圖4中圖示的示例情景在圖8中示出所產生的最短路徑112以及方程（15)中的 距離函數。如在本示例中所示，該技術將左車道識別為最短路徑，原因在于與主車輛相比左 車道上的目標車輛運動得顯著更慢，因此是最安全的選項。應當理解，考慮到其它車輛的可 能車輛動態或者由于連接線性線段之間所需要的高橫向加速度，規劃路徑盡管可能是最短 路徑，但卻可能不是最佳路徑。因此，可能需要對規劃路徑的進一步分析和規劃路徑的完 善。
[0083] 在步驟36中，作出源頂點是否利用所產生的路徑連接目標頂點的決策。如果作出 規劃路徑未連接相應頂點的決策，則例程前進至步驟42，其中例程將該路徑設置為不可行。 例程返回至步驟31以重新規劃路徑。如果在步驟36中作出所產生的路徑將源頂點連接至目 標頂點的決策，則例程前進至步驟37。
[0084] 在步驟37中，響應于確定最短路徑，在最短路徑周圍生成安全狹帶。識別安全狹帶 使得當主車輛路徑處于該狹帶內時其滿足以下條件。條件如下：（1)主車輛路徑十分接近最 短路徑；（2)主車輛路徑與所有周圍物體相距安全距離；以及(3)主車輛路徑保持在道路上。
[0085] 為尋找安全狹帶，利用沿著最短路徑的頂點12廣_'，~卩。在利用這 些參數時，針對每個頂點計算左狹帶點和右狹帶點。此外，在計算中必須考慮運動的目標的 動態。掃描點首先移位如下：
其中，pSsSp最短路徑的第計線段的掃描節點歡Ι，Λ)的移位點，以及指 明主車輛達到頂點所需要的時間段。時間段_*|表示如下：
[0086] 接著，通過使用以下公式將移位掃描點變換至最短路徑線段的局部參照系：
其中，％丨是將頂連接的線段的斜率。
[0087] 接著，利用以下條件確定以下轉移的掃描點的最小值和最大值：
[0088] 通過使用以下條件計算相應最短路徑頂點的左狹帶點和右狹帶點：
其中，1???和指明在線段的局部參照系中的第J個左狹帶點和右狹帶 點。
[0089] 接著，將方程（32)中的點變換回地球參照系，以獲得和 如果線段的長度短于閾值或者其相對于當前車道的相對 斜率大于閾值:1?，則由當前車道的前行方向角度替換方程(30)中的線段前行方向瑪分。
[0090] 圖9圖示了用于示例性情景的相應安全狹帶。用于左側狹帶點的第一組連接線114 表示安全狹帶的左邊界，并且用于右側狹帶點的第二組連接線116表示安全狹帶的右邊界。 [0091 ]在步驟38中，響應于識別安全狹帶，例程為主車輛確定狹帶內的路徑，該路徑足夠 平滑以被追蹤。由主車輛施加的動態約束是道路上的最大橫向加速度和橫向加加速度。這 意味著對于可行的主車輛路徑的最大曲率和曲率變化率的限制。因此，該技術在狹帶內改 善路徑點，使得最終路徑盡可能接近能夠由主車輛追蹤的路徑并且使規劃路徑中的過大曲 率最小化。
[0092]這種步驟在本文中被描述為子例程。在步驟38-1中，確定在地球參照系中等距離 豎直線處于安全狹帶范圍中，并且表示如下：
其中，?是表示豎直線之間的距離的恒定參數。
[0093]在步驟38-2中，使用左狹帶線和右狹帶線來確定方程(33)中的豎直線的交叉點。 所產生的點被稱為豎直狹帶點。左狹帶點由pf:(尤·,)表示，并且右狹帶點由 ?Ι?，表示。
[0094]在步驟38-3中，路徑點(乃)被限定如下：
其中，馬:€釋_|;使路徑點乃沿著豎直線在狹帶內運動。
[0095] 在步驟38-4中，通過設置4=0.5,對于i=l，2，…，/wk，使每個路徑點最初被定 位在豎直狹帶點之間豎直線的中間點處。
[0096]在步驟38-5中，針對路徑點A·，將成本函數限定如下：
其中，G和AG分別指明A·處的估計的曲率和曲率變化率，
方程(32)中的sin 〇函數通過使用交叉乘積公式被計算成：
[0097]在步驟38-6中，假設一'和|;巧一Il-il是恒定值，并且曲率和曲率變化率是 4的線性函數。因此，成本函數丹將是4的二次函數。通過求解邊馬/邊驗夂⑩來尋找成本函數 _ :儀j的最小點。
[0098] 在步驟38-7中，基于以下標準更新路徑點乃：
[0099] 在步驟38-8中，重復步驟38-5至38-7直到所有路徑乃，i-Ι，2，···，伽κ均更新一次。 [0100]在步驟38-9中，返回重復步驟38-5至38-8,重復最大數量的迭代次數直到所 有路徑點處的曲率和曲率變化率小于預定閾值率值。如果在迭代次數之后，最大曲率和 曲率變化率超過其相應預定閾值率，則例程停止，從而意味著不能夠找到可行的主車輛路 徑。
[0101] 圖10中示出將這種技術應用至圖9中的安全狹帶所產生的路徑，其中，線118表示 平滑的主車輛規劃路徑。應當理解，針對主車輛路徑，考慮的路點的最大數量是有限制的。 因此，主車輛路徑可能不覆蓋整個路徑規劃范圍，如圖10中所示。
[0102] 在步驟39中，響應于獲得步驟38中所確定的主車輛路徑，通過利用兩個條件作出 核實，以確保所產生的路徑是可行的。首先，主車輛路徑路點處的最大橫向加速度必須小于 預定閾值加速度值。可以通過利用以下方程來估計橫向加速度：
其中，G是在方程(36)中計算的估計的曲率值。
[0103] 第二個條件是主車輛路徑必須與所有周圍障礙相距安全距離。為了核實這個條 件，首先通過使用方程(28)-(30)使掃描點移位并且將其變換至主車輛路徑線段的局部參 照系，其中，由主車輛路點替換最短路徑頂點。然后針對所有主車輛路徑路點檢驗以下條 件，以確保主車輛路徑充分地遠離任何目標車輛或者其它障礙。條件表示如下：
[0104] 在步驟40中，作出關于路徑重新生成的規劃路徑是否可行的決定。如果基于在步 驟40中執行的可行性分析作出路徑不可行的決策，則例程前進至步驟31。如果作出路徑可 行的決策，則例程前進至步驟41。
[0105] 在步驟41中，將確定為可行的主車輛規劃路徑發送至控制器，其中控制器自主地 執行規劃路徑。響應于實施規劃路徑，返回至步驟31。
[0106] 在步驟31中，檢驗自規劃上一條路徑以來的時間是否已期滿。如前文所描述的，規 劃路徑在每個周期時間Tph的末尾生成。7^_表示例程等待直到規劃下一條路徑的第一預 定時間率(rate of time)。因此，系統將會等待等于rpia/3的一定持續時間（除非作出返回， 從而指示現有路徑不安全或者不可行），并且然后在該等于Tph的持續時間期滿之后生成 下一個規劃路徑。例如，可以是設置為0.5秒的持續時間。因此，處理器將每0.5秒就規 劃新的路徑。應當理解，0.5秒的持續時間是示例性的并且可以利用0.5秒之外的其它時間。 因此，當例程從步驟42循環至步驟32時，作出r pia/3(例如，0.5秒)是否已經消逝的決策。如果 作出7^_還未消逝并且現有規劃路徑不存在安全性或者可行性問題的決策，則例程前進至 步驟42。
[0107] 在步驟42中，從感測裝置和成像裝置獲得物體圖數據以執行安全性檢驗。安全性 檢驗以第二預定時間率7；(例如，10毫秒)執行。應當理解，10毫秒是示例性的并且可以使用 其它時間率。還應進一步理解，安全性檢查不會確定或者生成下一個規劃路徑;而是安全性 檢驗反復地在規劃路徑時間之間檢驗當前路徑的安全性，以便核實沒有新的威脅被引入上 一個規劃路徑。因此，基于在路徑規劃階段之間獲得的新的傳感器數據和成像數據反復地 執行多次安全性檢驗。
[0108] 在步驟43中，通過監測進入的傳感器數據分析當前路徑的安全性并且確定當前規 劃路徑是否安全。在安全性檢驗期間，在基于新的傳感器數據重新規劃路徑之前，被認為可 行的最近一次的主車輛規劃路徑將在Tph的時間段內保持不變。這樣做是為了減少實現車 輛上實際實時實施中的快速RPP規劃過程的計算成本。盡管被設置為足夠短以假設道 路情景在該時間段期間不顯著地變化，但還仍每7；執行額外的安全性檢驗，以確保現有規 劃路徑車輛路徑在規劃時間段之間是安全的。
[0109] 為了檢驗當前規劃路徑的安全性，所有主車輛路徑路點均被變換至每個運動的目 標的局部參照系，如由以下表達式所表示：
如圖11所示，限定每個運動目標前方的區域并且識別該區域內部的最小X路點（如果存 在）。這表示如下：
然后通過以下表達來確定為了達到該最小點的用于運動目標的時間間隔和用于 主車輛的時間間隔
[0110] 如果所有運動目標滿足以下條件，則認為現有主車輛路徑是安全的：
[0111] 在步驟44中，作出現有規劃路徑是否仍然安全的決策。如果作出現有規劃路徑仍 然安全的決策，則例程前進至步驟45。
[0112] 在步驟45中，確定主車輛的偏移并且在步驟41中向控制器提供該信息。例程返回 至步驟31，其中，如果未期滿，則例程執行另一安全性檢驗。如果作出7^_期滿的決 策，則例程前進至步驟32以規劃下一條路徑。
[0113] 再次參考步驟44,如果作出現有路徑不安全的決策，則返回至步驟31。在步驟31 中，關于現有路徑的安全性作出標記，并且例程立即前進至步驟32,以基于新獲得的物體圖 數據重新計算下一個規劃路徑，而不論7^_是否期滿。響應于新規劃路徑的決策，重新設置 Tph，并且在Tph期滿時設置用于下一個規劃路徑決策的時間段。
[0114] 盡管已經對本發明的某些實施例進行了詳細描述，但本發明所涉及領域的技術人 員將認識到如以下權利要求所限定的那樣的用于實踐本發明的各種替代性設計和實施例。
【主權項】
1. 一種適應性地重新生成用于自主駕駛操縱的規劃路徑的方法，所述方法包括W下步 驟： (a) 由基于車輛的裝置獲得與行駛道路中感測到的物體相關聯的物體數據； (b) 基于所述行駛道路中感測到的物體由處理器構造物體圖； (C)重新設置并且致動計時器； (d) 由所述處理器生成用于在所述感測到的物體周圍自主地操縱所述車輛的規劃路 徑，所述規劃路徑基于成本-距離函數生成； (e) 沿著所述規劃路徑自主地操縱所述車輛； (f) 基于來自所述基于車輛的裝置的更新的感測到的數據更新所述物體圖； (g) 基于更新的所述物體圖確定所述規劃路徑是否可行； (h) 響應于所述規劃路徑不可行的決策，返回至步驟(a);否則繼續至步驟(i); (i )確定所述計時器是否期滿；W及 (j)響應于所述計時器期滿返回至步驟(a);否則返回至步驟(f)。2. 根據權利要求1所述的方法，其中，生成所述規劃路徑還包括W下步驟： 基于所述行駛道路中感測到的物體識別虛擬節點；W及 應用德洛內Ξ角剖分W便在所選擇的虛擬節點當中生成Ξ角形。3. 根據權利要求2所述的方法，其中，所述虛擬節點包括車道虛擬節點、主車輛虛擬節 點、終點虛擬節點W及移位掃描節點，其中，所述車道虛擬節點表示所述道路的車道邊界， 其中，所述主車輛虛擬節點表示所述主車輛的位置和捜索空間的起點，其中，所述終點虛擬 節點表示所述捜索空間的終點，W及其中，所述移位掃描節點表示基于所檢測到的動態物 體相對于所述主車輛的速度的動態物體的位移。4. 根據權利要求3所述的方法，還包括W下步驟:沿所述Ξ角形的邊緣識別頂點，所述 頂點沿每個Ξ角形邊緣均勻地間隔；W及 在每個Ξ角形內的每對頂點之間形成線性線段。5. 根據權利要求4所述的方法，其中，只有當相應的頂點對屬于相同的相應Ξ角形并且 當所述頂點不屬于所述相應Ξ角形的相同邊緣時，才形成連接所述相應Ξ角形的所述相應 頂點對的相應線性線段。6. 根據權利要求4所述的方法，其中，不在所述道路的車道邊界上形成頂點。7. 根據權利要求5所述的方法，其中，通過從每個Ξ角形中選擇相應線性線段來從所識 別的主虛擬節點至所識別的終點虛擬節點生成所述規劃路徑，其中，從每個Ξ角形選擇的 每個線性線段從所述所識別的主虛擬節點至所述所識別的終點虛擬節點形成連續規劃路 徑。8. 根據權利要求7所述的方法，其中，基于成本-距離函數識別每個所述所選擇的線性 線段，所述成本函數生成為關于所述規劃路徑的長度的距離函數分量、每個線段的相對斜 率、所述規劃路徑相對先前確定的路徑的偏移、從當前駕駛車道的中屯、的偏移W及距周圍 障礙的偏移距離。9. 根據權利要求8所述的方法，其中，對每個所述相應距離函數分量進行加權，W便識 別在所述成本-距離函數中的使用程度。10. 根據權利要求9所述的方法，其中，所述成本-距離函數由W下公式表示：其中，化是第i條路徑從源頂點至目標頂點的距離，/.^^巧、邱},、&§1、|1￡^是第^'條路徑 的第J個線段的距離函數分量，心《、確日9是恒定權重系數，W及/WA和/W分別是從所述 源頂點至目標頂點的多條路徑和每條路徑中的多個線性線段。
【文檔編號】G05D1/02GK106094812SQ201610267697
【公開日】2016年11月9日
【申請日】2016年4月27日 公開號201610267697.8, CN 106094812 A, CN 106094812A, CN 201610267697, CN-A-106094812, CN106094812 A, CN106094812A, CN201610267697, CN201610267697.8
【發明人】S.曾, R.賈法里, N.K.莫什楚克, B.B.利特庫希
【申請人】通用汽車環球科技運作有限責任公司