用于自主駕駛的反應(yīng)式路徑規(guī)劃的制作方法
【專利摘要】本發(fā)明涉及用于自主駕駛的反應(yīng)式路徑規(guī)劃。公開(kāi)了一種適應(yīng)性地重新生成用于自主駕駛操縱的規(guī)劃路徑的方法?；谛旭偟缆分懈袦y(cè)到的物體生成物體圖。重新設(shè)置并且致動(dòng)計(jì)時(shí)器。生成用于在感測(cè)到的物體周圍自主地操縱車輛的規(guī)劃路徑。沿著所述規(guī)劃路徑自主地操縱車輛?；趤?lái)自基于車輛的裝置的感測(cè)到的數(shù)據(jù)來(lái)更新物體圖?；诟碌奈矬w圖執(zhí)行安全性檢驗(yàn)以便確定所述規(guī)劃路徑是否可行。響應(yīng)于現(xiàn)有路徑不可行的決策重新生成規(guī)劃路徑，否則作出關(guān)于所述計(jì)時(shí)器是否已期滿的決策。如果所述計(jì)時(shí)器未期滿，則重新執(zhí)行安全性檢驗(yàn)，否則返回以重新規(guī)劃路徑。
【專利說(shuō)明】
用于自主駕駛的反應(yīng)式路徑規(guī)劃
技術(shù)領(lǐng)域
[0001] 實(shí)施例涉及自主路徑規(guī)劃。
【背景技術(shù)】
[0002] 自主和半自主公路駕駛和諸如避免碰撞的高級(jí)駕駛員輔助系統(tǒng)均需要路徑規(guī)劃。 路徑規(guī)劃必須對(duì)主車輛動(dòng)態(tài)以及道路上其它靜態(tài)和動(dòng)態(tài)的物體的變化作出反應(yīng)。所規(guī)劃的 路徑必須產(chǎn)生在道路邊界內(nèi)的安全無(wú)碰撞路徑，其對(duì)于根據(jù)諸如最大橫向加速度/加加速 度（jerk)的車輛動(dòng)態(tài)約束進(jìn)行主車輛控制而言也必須是可行的。已知的路徑規(guī)劃技術(shù)或者 沒(méi)有考慮到主車輛和其它運(yùn)動(dòng)的目標(biāo)車輛的動(dòng)態(tài)，或者對(duì)于實(shí)時(shí)應(yīng)用在恰當(dāng)時(shí)間中作出反 應(yīng)而言過(guò)于計(jì)算密集。

【發(fā)明內(nèi)容】

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

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