本文提出了一種基于A*算法的用于道路場(chǎng)景的軌跡規(guī)劃方法，該方法中，使用了圖形搜索近似，并且通過(guò)一種新的推理技術(shù)來(lái)考慮其他車(chē)輛的不確定預(yù)測(cè)，這種新的推理技術(shù)允許高效計(jì)算一組模擬場(chǎng)景類(lèi)型的危險(xiǎn)結(jié)果的概率。研究者相信本文提出的幾何推斷方法可以很好地適應(yīng)模型傳感器范圍和遮擋限制。

什么是路徑規(guī)劃和軌跡規(guī)劃？

車(chē)輛自主駕駛系統(tǒng)從本質(zhì)上講是一個(gè)智能控制機(jī)器，其研究?jī)?nèi)容大致可分為信息感知、行為決策及操縱控制三個(gè)子系統(tǒng)。

路徑規(guī)劃是智能車(chē)輛導(dǎo)航和控制的基礎(chǔ)，是從軌跡決策的角度考慮的，可分為局部路徑規(guī)劃和全局路徑規(guī)劃。全局路徑規(guī)劃的任務(wù)是根據(jù)全局地圖數(shù)據(jù)庫(kù)信息規(guī)劃出自起始點(diǎn)至目標(biāo)點(diǎn)的一條無(wú)碰撞、可通過(guò)的路徑。由于全局路徑規(guī)劃所生成的路徑只能是從起始點(diǎn)到目標(biāo)點(diǎn)的粗略路徑，并沒(méi)有考慮路徑的方向、寬度、曲率、道路交叉以及路障等細(xì)節(jié)信息，加之智能車(chē)輛在行駛過(guò)程中受局部環(huán)境和自身狀態(tài)的不確定性的影響，會(huì)遇到各種不可測(cè)的情況。

因此，在智能車(chē)輛的行駛過(guò)程中，必須以局部環(huán)境信息和自身狀態(tài)信息為基礎(chǔ)，規(guī)劃出一段無(wú)碰撞的理想局部路徑，這就是局部路徑規(guī)劃。通常路徑規(guī)劃的方法有:空間搜索法、層次法、動(dòng)作行為法、勢(shì)場(chǎng)域法、柵格法、模糊邏輯法和神經(jīng)網(wǎng)絡(luò)法等。

汽車(chē)自動(dòng)駕駛?cè)蝿?wù)可以分為三層，如圖所示，每層執(zhí)行不同任務(wù)，包括上層路徑規(guī)劃，中層行駛行為規(guī)劃和下層軌跡規(guī)劃。

上層路徑規(guī)劃在已知電子地圖、路網(wǎng)以及宏觀交通信息等先驗(yàn)信息下，根據(jù)某優(yōu)化目標(biāo)得到兩點(diǎn)之間的最優(yōu)路徑，完成路徑規(guī)劃的傳感信息主要來(lái)自于GPS定位信息以及電子地圖。

中層行駛行為規(guī)劃是指根據(jù)主車(chē)感興趣區(qū)域內(nèi)道路、交通車(chē)等環(huán)境信息，決策出當(dāng)前時(shí)刻滿足交通法規(guī)、結(jié)構(gòu)化道路約束的最優(yōu)行駛行為，動(dòng)態(tài)規(guī)劃的行駛行為序列組成宏觀路徑。行為規(guī)劃的傳感信息主要來(lái)自車(chē)載傳感器如雷達(dá)、照相機(jī)等，用以識(shí)別道路障礙、車(chē)道線、道路標(biāo)識(shí)信息和交通信號(hào)燈信息等。

下層軌跡規(guī)劃是指在當(dāng)前時(shí)刻，以完成當(dāng)前行車(chē)行為為目標(biāo)，考慮周?chē)煌ōh(huán)境并滿足不同約束條件，根據(jù)最優(yōu)目標(biāo)動(dòng)態(tài)規(guī)劃決策出的最優(yōu)軌跡。同時(shí)，車(chē)輛的動(dòng)力學(xué)約束也會(huì)在下層得到體現(xiàn)，下層軌跡規(guī)劃除了必要的外部環(huán)境信息外，還需要對(duì)主車(chē)狀態(tài)信息進(jìn)行測(cè)量或估計(jì)。

車(chē)輛路徑規(guī)劃問(wèn)題中的幾個(gè)關(guān)鍵點(diǎn)：路網(wǎng)模型、路徑規(guī)劃算法和交通信息的智能預(yù)測(cè)，涉及的方面較多。本文主要探討軌跡規(guī)劃。

用于道路場(chǎng)景的軌跡規(guī)劃方法

A概述

本文提出了一種基于A*算法的用于道路場(chǎng)景的軌跡規(guī)劃方法，該方法中，使用了圖形搜索近似，并且通過(guò)一種新的推理技術(shù)來(lái)考慮其他車(chē)輛的不確定預(yù)測(cè)，這種新的推理技術(shù)允許高效計(jì)算一組模擬場(chǎng)景類(lèi)型的危險(xiǎn)結(jié)果的概率。同時(shí)研究者還將這種軌跡規(guī)劃方法與基于蒙特卡羅方法的POMDP求解器進(jìn)行了比較，發(fā)現(xiàn)雖然POMDP求解器可以規(guī)劃比該研究的方法更保守更復(fù)雜的行為，但是它遭受粒子剝奪，這可能會(huì)導(dǎo)致危險(xiǎn)的行為。這是因?yàn)槊商乜_方法有時(shí)不會(huì)有足夠多的樣本用于罕見(jiàn)但危險(xiǎn)的未來(lái)情況。相反，我們的方法使用了對(duì)未來(lái)信念狀態(tài)的參數(shù)化描述，并且沒(méi)有遭受這個(gè)問(wèn)題的困擾，并且運(yùn)行時(shí)間明顯更長(zhǎng)。

圖1:道路行駛功能系統(tǒng)架構(gòu)

圖1顯示了用于自主道路操作的模塊的高級(jí)視圖。全局規(guī)劃模塊處理任務(wù)，為軌跡規(guī)劃模塊提供路線，這也需要定位、附近車(chē)道模型和描述附近物體狀態(tài)和行為的模型。軌跡規(guī)劃模塊應(yīng)該為自主車(chē)輛(稱(chēng)為ego車(chē)輛)找到合適的軌跡，以便遵循公式：

其中xe是自我車(chē)輛的狀態(tài)，Th是預(yù)測(cè)范圍的長(zhǎng)度。這是通過(guò)優(yōu)化成本函數(shù)來(lái)實(shí)現(xiàn)的，其中高風(fēng)險(xiǎn)的軌跡具有高成本。通過(guò)分析由 i 指數(shù)的附近其他車(chē)輛的不確定預(yù)測(cè)來(lái)估計(jì)風(fēng)險(xiǎn)。在不確定性下的這種計(jì)劃將在該方法中以固定間隔重復(fù)，而當(dāng)前最佳計(jì)劃由下級(jí)控制模塊執(zhí)行。

這里，我們通過(guò)借用[13]中的符號(hào)和定義簡(jiǎn)要概述不確定條件下的軌跡規(guī)劃問(wèn)題。我們假設(shè)環(huán)境狀態(tài)x =（xe，xi），i = 0，..，K在計(jì)劃范圍的離散時(shí)間步長(zhǎng)中演變：

其中xik是車(chē)輛 i 在時(shí)刻tnow+kΔtp的狀態(tài)，uk是對(duì)于自我車(chē)輛的控制動(dòng)作，ωk是擾動(dòng)，并且N = Th/Δtp其中Δtp是時(shí)間離散化。我們每次都會(huì)對(duì)環(huán)境進(jìn)行嘈雜的測(cè)量：

其中νk模擬測(cè)量噪聲。一般來(lái)說(shuō)，我們不能直接觀察xk，而是總結(jié)信息狀態(tài)Ik中的所有可用信息。目標(biāo)是為計(jì)劃范圍找到最優(yōu)策略Π* = {π*0（I0），...，π*N-1（IN-1）}，給出我們的控制動(dòng)作，在給定信息狀態(tài)的情況下最小化未來(lái)預(yù)期成本，對(duì)于附加成本函數(shù)：

為了找到最優(yōu)政策，我們計(jì)劃在信念空間中，并對(duì)未來(lái)的測(cè)量做出不同的假設(shè)，以獲得易處理的問(wèn)題。在信念狀態(tài)bk被定義為bk= p（xk| Ik）并且Ik是歷史I狀態(tài)的情況下：Ik=（I0，u0：k-1，y0：k-1）。這個(gè)問(wèn)題可以重新表述為隨機(jī)動(dòng)態(tài)規(guī)劃（SDP）問(wèn)題。

我們可以使用部分可觀察的決策過(guò)程（POMDP）方法來(lái)逼近SDP問(wèn)題的解決方案，或者我們可以以后退的方式解決一系列控制動(dòng)作，忽略未來(lái)的測(cè)量。我們?cè)诒疚闹惺褂玫暮笠环N方法稱(chēng)為開(kāi)環(huán)后退水平控制（OLRHC），這里信念狀態(tài)是“對(duì)象的開(kāi)環(huán)預(yù)測(cè)分布”[13]，反饋是通過(guò)重新規(guī)劃實(shí)現(xiàn)的。

如[5] [14]中所述，我們的軌跡規(guī)劃算法描述了自主車(chē)輛的狀態(tài)，在道路對(duì)齊的坐標(biāo)系中，橫坐標(biāo)對(duì)應(yīng)于沿參考路徑的弧長(zhǎng)s，縱坐標(biāo)表示有符號(hào)的橫向距離。路徑d。我們假設(shè)我們可以精確地測(cè)量與參考路徑（se0，de0）相關(guān)的當(dāng)前狀態(tài)，并且可以獲得其他車(chē)輛的開(kāi)環(huán)預(yù)測(cè)列表。設(shè)xik是慣性參考系中車(chē)輛 i 的姿態(tài)和速度，xik=（x，y，θ，v），xik是與參考路徑 r 相關(guān)的狀態(tài)，xik,r=（sik,r, dik,r, sik,r）和g（xik,r）= xik是在它們之間進(jìn)行映射的函數(shù)。 對(duì)每個(gè)其他車(chē)輛的預(yù)測(cè)表示為高斯混合序列，其中每個(gè)高斯模型表示車(chē)輛狀態(tài)相對(duì)于特定路線的分布：

其中αj是高斯prj（xik,r(j)）=N（μir(j),k，Σir(j),k）的混合分量權(quán)重，描述了假設(shè) j 的路徑 r（j）的車(chē)輛 i 的狀態(tài)分布。該表示允許我們表達(dá)單個(gè)車(chē)輛的多個(gè)運(yùn)動(dòng)假設(shè)，例如，我們模擬車(chē)輛是否將使用不同的αj值轉(zhuǎn)向兩個(gè)假設(shè)的不確定性，每個(gè)假設(shè)屬于不同的路線。分布N（μir(j),k，Σir(j),k）可以用不同的方法估計(jì)，例如[15]，或者諸如執(zhí)行卡爾曼濾波器的預(yù)測(cè)步驟的簡(jiǎn)單方法。我們還假設(shè)我們知道每條車(chē)輛沿其路線的縱向范圍。

軌跡規(guī)劃算法可以訪問(wèn)道路模型，在該道路模型中，軌道規(guī)劃算法可以快速確定兩條路線是否相鄰，例如，檢查車(chē)輛是否在我們的左側(cè)或右側(cè)，如果一輛車(chē)與另一輛車(chē)合并或者路線是否交叉。本文描述風(fēng)險(xiǎn)評(píng)估方法主要基于兩條車(chē)輛在路線上占據(jù)相同縱向位置的距離和車(chē)輛的相對(duì)速度。

B圖形搜索近似

決定什么時(shí)候改變車(chē)道，或者當(dāng)駕駛到兩個(gè)車(chē)道合并的道路的一部分時(shí)是一個(gè)非凸的問(wèn)題，特別是我們經(jīng)常可以選擇在另一輛車(chē)前面或后面，每個(gè)車(chē)輛都有一個(gè)局部最佳值選擇。 通過(guò)允許為我們的解決方案用一組采樣狀態(tài)值，我們將軌跡優(yōu)化轉(zhuǎn)換為圖搜索問(wèn)題：從有限集中找出狀態(tài)值的序列，其具有最低成本，滿足我們的約束并達(dá)到狀態(tài)預(yù)測(cè)范圍的結(jié)束。我們通過(guò)對(duì)和時(shí)間進(jìn)行采樣來(lái)構(gòu)造一個(gè)格子超過(guò)允許狀態(tài)，并定義模擬我們的控制動(dòng)作的運(yùn)動(dòng)原語(yǔ)u從時(shí)間k的狀態(tài)（s，d，v）轉(zhuǎn)換到時(shí)間k的另一個(gè)狀態(tài) k+1，k后的Δtp秒，從而形成可能的狀態(tài)序列的圖形表示。我們使用了一個(gè)簡(jiǎn)化的模型來(lái)說(shuō)明車(chē)輛在道路對(duì)齊坐標(biāo)系中的移動(dòng)方式：

其中運(yùn)動(dòng)原語(yǔ)由不同的預(yù)定義加速度和橫向速度組成。速度v被限制為在上面被vmax限制為非負(fù)，并且d被縮放到1和1之間，其中1是參考路徑右邊的一個(gè)車(chē)道寬度和左邊一個(gè)車(chē)道寬度。允許速度的集合是從v = 0開(kāi)始可達(dá)到的任何速度，其是非負(fù)的并且在規(guī)劃步驟的整個(gè)時(shí)間期間使用A中的任何加速度小于vmax，假設(shè)步驟之間的加速度瞬時(shí)變化。允許縱向位置的集合是時(shí)間步長(zhǎng)k = 0，...，N 從 s = 0 開(kāi)始以初始速度進(jìn)入并且使用A中的加速度行進(jìn)的距離的集合。在我們的簡(jiǎn)化模型中，我們?cè)试S加速度的瞬時(shí)變化這個(gè)誤差假設(shè)最多是dmin的縱向位置誤差，它是從執(zhí)行模塊轉(zhuǎn)換為非完整運(yùn)動(dòng)的距離中減去的。

對(duì)于我們?cè)贏 *算法中的啟發(fā)式，我們使用進(jìn)入結(jié)束狀態(tài)（k = N）的最優(yōu)成本，假設(shè)如果dk不在車(chē)道的中心并且在時(shí)間步長(zhǎng)k，k + 1之間將發(fā)生橫向移動(dòng)，并且沒(méi)有其他車(chē)輛。 這些成本是針對(duì)每個(gè)s，v，k組合離線計(jì)算的并存儲(chǔ)在查找表中。 這低估了達(dá)到目標(biāo)狀態(tài)的成本，因?yàn)閏l和cd總是大于或等于零，啟發(fā)式是可以接受的。

總結(jié)

自動(dòng)駕駛車(chē)輛的軌跡規(guī)劃應(yīng)試圖在傳感器數(shù)據(jù)噪聲和不確定預(yù)測(cè)的情況下最小化預(yù)期風(fēng)險(xiǎn)。本文提出了一種基于圖搜索近似的道路場(chǎng)景軌跡規(guī)劃方法。其他車(chē)輛的不確定預(yù)測(cè)是由一種新的推理技術(shù)來(lái)解釋的，這種推理技術(shù)可以有效地計(jì)算建模情況類(lèi)型集的危險(xiǎn)結(jié)果的概率。對(duì)于高速公路合并場(chǎng)景，這里使用簡(jiǎn)單的恒速預(yù)測(cè)，研究者并不考慮傳感器范圍和遮擋。未來(lái)的工作將考慮更復(fù)雜的預(yù)測(cè)模型，研究者相信本文提出的幾何推斷方法可以很好地適應(yīng)模型傳感器范圍和遮擋限制。例如，為了對(duì)超出傳感器范圍的可能車(chē)輛所構(gòu)成的風(fēng)險(xiǎn)進(jìn)行建模，研究者可以在給定未知車(chē)輛速度和位置的統(tǒng)一概率的情況下，整合約束違反概率。