路線規(guī)劃模塊是在給定離線地圖中定義的最終目的地的情況下，在離線地圖中計(jì)算從當(dāng)前狀態(tài)到最終目標(biāo)的路線。路徑是一系列路徑點(diǎn)，其中每個(gè)路徑點(diǎn)是離線地圖中的坐標(biāo)對(duì)。

軌跡規(guī)劃模塊是在給定路徑情況下，考慮汽車狀態(tài)和環(huán)境的內(nèi)部表示以及交通規(guī)則，計(jì)算出單條路徑的多個(gè)組合。路徑是一系列姿態(tài)，其中每個(gè)姿勢(shì)是離線地圖中的坐標(biāo)對(duì)，以及在此定義的位置處所需的汽車方向的坐標(biāo)對(duì)。

行為決策模塊負(fù)責(zé)選擇當(dāng)前的駕駛行為，例如車道保持，交叉路口處理，交通燈信號(hào)處理等。該模塊用來選擇在當(dāng)前狀態(tài)（決策范圍）前幾秒鐘（大概是5秒時(shí)間）在路徑中以及此姿勢(shì)狀態(tài)下的所需速度。姿勢(shì)和相關(guān)的速度組成一對(duì)稱為Goal，行為決策模塊需要在考慮當(dāng)前的駕駛行為，并避免在決策時(shí)間范圍內(nèi)與環(huán)境中的靜態(tài)和移動(dòng)障礙物發(fā)生碰撞下選擇一個(gè)Goal。

運(yùn)動(dòng)規(guī)劃模塊負(fù)責(zé)計(jì)算從當(dāng)前汽車狀態(tài)到當(dāng)前Goal的軌跡，該目標(biāo)遵循行為決策模塊定義的路徑，并滿足汽車的運(yùn)動(dòng)和動(dòng)態(tài)約束，和乘客的舒適感。一個(gè)軌跡需要將汽車從當(dāng)前狀態(tài)平穩(wěn)地、安全地帶到目標(biāo)位置。

避障模塊接收由運(yùn)動(dòng)規(guī)劃模塊計(jì)算的軌跡并在必要時(shí)更改它（通常降低速度）以避免碰撞。

最后，控制模塊接收運(yùn)動(dòng)規(guī)劃模塊得到軌跡以及經(jīng)由避障模塊修改后的軌跡（通常是是降低速度），并計(jì)算并發(fā)送有作用效果的命令到方向盤，油門和制動(dòng)器的執(zhí)行器，以使汽車最好地在真實(shí)物理環(huán)境下修改軌跡。

下面是一些有關(guān)以上模塊形成的方法。

感知系統(tǒng)

在該系統(tǒng)中，包含了汽車定位、靜態(tài)障礙物繪制、道路映射、移動(dòng)障礙物跟蹤以及交通信號(hào)檢測(cè)和識(shí)別幾大模塊。

1.1汽車定位

定位模塊負(fù)責(zé)估計(jì)相對(duì)于地圖或道路（例如，由路緣或道路標(biāo)記表示）的自動(dòng)駕駛汽車姿態(tài)（位置和方向）。大多數(shù)通用定位子系統(tǒng)都基于GPS的。然而，總的來說，它們不適用于城市自動(dòng)駕駛汽車，因?yàn)镚PS信號(hào)在閉塞區(qū)域無法保證，例如在樹下、城市峽谷（被大型建筑物包圍的道路）或隧道中。文獻(xiàn)中提出了各種不依賴GPS的定位方法。它們主要可分為三類：基于激光雷達(dá)的、基于激光雷達(dá)加相機(jī)的和基于相機(jī)的。僅依賴于LIDAR傳感器的基于激光雷達(dá)的定位方法可提供測(cè)量精度和得到數(shù)據(jù)易于處理。然而，盡管LIDAR行業(yè)努力降低生產(chǎn)成本，但是其與相機(jī)相比仍然具有較高的價(jià)格。在典型的LIDAR加上攝像機(jī)的定位方法中，LIDAR數(shù)據(jù)僅用于構(gòu)建地圖，并且通過攝像機(jī)數(shù)據(jù)估計(jì)車輛相對(duì)于地圖的位置。這樣的做法降低了成本?；谙鄼C(jī)的定位方法便宜且方便，但是這種方法通常不太精確和可靠。

1.1.1基于激光雷達(dá)的汽車定位

（1）一種在LIDAR激光射線的環(huán)境反射率（激光反射網(wǎng)格圖，如圖2）使用概率分布的離線網(wǎng)格圖用來定位的方法。該方法使用了HDL-64E-Velodyne激光雷達(dá)并通過無監(jiān)督校準(zhǔn)方法來校準(zhǔn)HDL-64E-Velodyne激光束。這使得激光雷達(dá)對(duì)具有相同亮度的物體的響應(yīng)類似。使用二維直方圖濾波器估計(jì)自動(dòng)駕駛汽車的位置。通常，濾波器由兩部分組成：運(yùn)動(dòng)更新（或預(yù)測(cè)）：用來減少基于運(yùn)動(dòng)的估計(jì)的置信度；以及測(cè)量更新（或校正）：用來增加基于傳感器數(shù)據(jù)的估計(jì)的置信度。在運(yùn)動(dòng)更新過程中，汽車運(yùn)動(dòng)以高斯分布噪聲隨機(jī)“游走”，從通過航位推算坐標(biāo)系（使用Applanix LV-420導(dǎo)航系統(tǒng)的慣性更新計(jì)算）漂移到離線地圖的全局坐標(biāo)系。在測(cè)量步驟中，對(duì)于不同的位移，它們使用在線計(jì)算的反射圖與離線計(jì)算的反射圖之間的相似性。其中，每個(gè)位移對(duì)應(yīng)于直方圖濾波器中的直方圖的一個(gè)單元。為了將直方圖概括為單個(gè)姿勢(shì)估計(jì)，它們使用由直方圖建模的概率分布的質(zhì)心。然而，該方法沒有描述他們?nèi)绾喂烙?jì)方向。該方法顯示均方根（RMS）橫向誤差為9厘米，均方根（RMS）縱向誤差為12厘米。

（2）一種蒙特卡洛（MCL）定位方法，該方法將衛(wèi)星地圖與反射得到的地圖進(jìn)行比較。衛(wèi)星地圖從互聯(lián)網(wǎng)上離線下載，如OpenStreetMap，并且通過LIDAR接受反射強(qiáng)度數(shù)據(jù)實(shí)時(shí)構(gòu)建反射的地圖。MCL算法用于通過使用歸一化互信息（NMI）度量將反射得到地圖與航衛(wèi)星地圖匹配從而來估計(jì)汽車姿態(tài)以計(jì)算粒子可能性。該方法在機(jī)器人汽車“IARA”收集的6.5 km數(shù)據(jù)集上進(jìn)行評(píng)估，并獲得0.89 m的位置估計(jì)精度。該方法的一個(gè)優(yōu)點(diǎn)是它不需要專門為該方法構(gòu)建地圖。

（3）一種基于道路特征檢測(cè)的定位方法。他們的路緣檢測(cè)算法使用環(huán)壓縮分析和最小修剪方塊來分析由多層LIDAR（HDL-32E-Velodyne）掃描形成的連續(xù)同心測(cè)量（或環(huán)）之間的距離。道路標(biāo)記檢測(cè)算法使用Otsu閾值來分析LIDAR反射強(qiáng)度數(shù)據(jù)。路緣和道路標(biāo)記功能存儲(chǔ)在網(wǎng)格地圖中。蒙特卡洛定位（MCL）算法用于通過將從多層激光雷達(dá)測(cè)量中提取的道路特征與網(wǎng)格圖匹配來估計(jì)汽車姿態(tài)。該方法在自主車輛“CARINA”上進(jìn)行了評(píng)估，并且顯示出橫向和縱向定位估計(jì)誤差小于0.30 m。

（4）一種多層自適應(yīng)蒙特卡羅定位（ML-AMCL）方法，流程如圖3所示，該方法與3D點(diǎn)配準(zhǔn)算法結(jié)合使用。為了估計(jì)汽車姿勢(shì)，該方法從3D LIDAR測(cè)量中提取水平層，并且使用單獨(dú)的自適應(yīng)蒙特卡羅方法來對(duì)準(zhǔn)具有使用3D點(diǎn)配準(zhǔn)算法構(gòu)建的3D點(diǎn)云圖的2D投影的層。對(duì)于每個(gè)姿勢(shì)估計(jì)，執(zhí)行針對(duì)一系列測(cè)距測(cè)量的一致性檢查，并將一致的姿勢(shì)估計(jì)與最終姿勢(shì)估計(jì)融合。該方法在現(xiàn)實(shí)世界數(shù)據(jù)上進(jìn)行評(píng)估，并獲得相對(duì)于GPS參考的0.25米的位置估計(jì)誤差。

圖3  多層自適應(yīng)蒙特卡洛定位方法示意圖

（5）一種基于蒙特卡羅算法的定位方法，該方法通過2D占據(jù)柵格地圖和2D離線占據(jù)柵格地圖之間的地圖匹配來校正粒子的姿態(tài)，如圖4所示。對(duì)兩個(gè)地圖匹配距離函數(shù)（兩個(gè)網(wǎng)格圖之間的傳統(tǒng)似然場(chǎng)的距離的改進(jìn)版本，以及兩個(gè)高維向量之間的自適應(yīng)標(biāo)準(zhǔn)余弦距離）進(jìn)行了評(píng)估。其中在IARA自動(dòng)駕駛汽車的實(shí)驗(yàn)評(píng)估表明，定位方法能夠使用余弦距離函數(shù)在約100Hz下工作，并且橫向和縱向誤差分別為0.13 m和0.26 m。

圖4  占據(jù)柵格地圖匹配定位方法

（6）一種概率定位方法。該方法將世界建模為高斯混合的多分辨率圖（如圖5所示）。其中，高斯混合圖由多層LIDAR掃描儀（HDL-32E-Velodyne）測(cè)量的場(chǎng)景下的高度和反射強(qiáng)度（remission）分布辨識(shí)。擴(kuò)展卡爾曼濾波器（EKF）定位算法用于通過高斯混合多分辨率圖的3D點(diǎn)云來估計(jì)汽車的姿態(tài)。該方法在惡劣天氣條件下對(duì)兩輛無人駕駛汽車進(jìn)行了評(píng)估，結(jié)果顯示了該方法定位估算誤差約0.15米。

圖5  概率定位方法

1.1.2基于激光雷達(dá)加攝像頭的汽車定位

一些方法通過使用LIDAR數(shù)據(jù)來構(gòu)建地圖以及攝像頭獲取的數(shù)據(jù)，估計(jì)自動(dòng)駕駛車相對(duì)于該地圖的位置。

（1）一種將立體圖像與3D點(diǎn)云圖匹配的定位方法。該地圖由地圖公司生成（http://www.whatmms.com），地圖由幾何數(shù)據(jù)（緯度，經(jīng)度和高度）以及里程表，RTK-GPS和2D LIDAR掃描儀獲得的反射的數(shù)據(jù)組成。該算法可用于解決使用立體相機(jī)的基于三維（3D）點(diǎn)云地圖（PCL）的定位問題。該3D點(diǎn)云圖由密集的3D幾何信息和基于3D光檢測(cè)和測(cè)距（LIDAR）掃描儀的映射系統(tǒng)生成的表面反射率值的強(qiáng)度度量組成。盡管已經(jīng)提出了一些基于LIDAR的定位算法，但該方法提出了一種使用更便宜的商品立體相機(jī)，且精度能達(dá)到厘米級(jí)的定位算法。具體而言，在每個(gè)候選位置，將3D數(shù)據(jù)點(diǎn)從真實(shí)世界坐標(biāo)系轉(zhuǎn)換到相機(jī)坐標(biāo)系，并從3D PCL地圖合成虛擬深度和強(qiáng)度圖像。通過將這些虛擬圖像與立體深度和強(qiáng)度圖像相匹配來估計(jì)每幀中真實(shí)世界和車輛坐標(biāo)之間的轉(zhuǎn)換，從而定位自我車輛。流程圖如下圖6所示：

圖6  3D點(diǎn)云地圖定位

（2）一種將陸地的全景圖像與一年中不同季節(jié)捕獲的衛(wèi)星圖像相匹配自動(dòng)駕駛汽車定位方法。在該方法中，LIDAR數(shù)據(jù)被分類為地面和非地面類別。接著，使用LIDAR數(shù)據(jù)將由自動(dòng)駕駛車中的全景相機(jī)捕獲的地面圖像分割成地面和非地面區(qū)域，然后變換為鳥瞰圖。其中，通過使用KMeans聚類方法將衛(wèi)星圖像分割成地面/非地面區(qū)域。然后使用蒙特卡羅算法將鳥瞰圖像與衛(wèi)星圖像匹配，從而估計(jì)姿勢(shì)。該方法在NavLab11自動(dòng)駕駛汽車上進(jìn)行了評(píng)估，并實(shí)現(xiàn)了3米到4.8米之間的位置估算誤差。

1.1.3基于攝像頭的汽車定位

在定位方法，有一些方法主要依靠攝像頭數(shù)據(jù)來定位自動(dòng)駕駛汽車。

（1）一種基于視覺里程計(jì)和路線圖的定位方法。他們使用OpenStreetMap，從該圖中提取所有交叉點(diǎn)和所有可行駛道路（用分段線性段表示），并將它們連接到感興趣的區(qū)域。然后，他們構(gòu)建了這個(gè)路線圖的基于圖形的表示以及汽車如何遍歷該圖的概率模型。使用這種概率模型和視覺里程測(cè)量，他們估計(jì)相對(duì)于路線圖的汽車位移。

遞歸貝葉斯過濾算法用于通過利用其結(jié)構(gòu)和汽車運(yùn)動(dòng)速度（通過視覺里程計(jì)測(cè)量）的模型來執(zhí)行圖中的推斷。該算法通過增加當(dāng)前位于汽車最新運(yùn)動(dòng)（直線距離以及近期曲線）圖的位置點(diǎn)的姿勢(shì)的可能性以及降低與之不相關(guān)點(diǎn)的可能性來精確定位汽車在地圖中的位置。（2）在一些方法中，特征圖通過使用相機(jī)數(shù)據(jù)來構(gòu)建。該方法中描述了自動(dòng)駕駛汽車Bertha自主駕駛的定位方法。論文開發(fā)了一種基于互補(bǔ)視覺的定位技術(shù)：基于點(diǎn)特征的定位（PFL）和基于車道特征的定位（LFL）。如下圖7所示（其中a為點(diǎn)特征，b為車道特征），在PFL（點(diǎn)特征的定位）中，將當(dāng)前相機(jī)的圖像同先前進(jìn)行測(cè)繪過程中提取的DIRD描述符中獲取的一系列相機(jī)圖像的圖像進(jìn)行比較。全局位置估計(jì)從制圖過程中獲取圖像中的全局位置恢復(fù)。在LFL（車道特征的定位）中，半自動(dòng)計(jì)算的地圖提供道路標(biāo)記特征（水平道路信號(hào)化）的全局幾何表征。通過檢測(cè)并關(guān)聯(lián)從相機(jī)圖像的鳥瞰視圖提取的道路標(biāo)記特征與存儲(chǔ)在地圖中的水平道路信號(hào)，將當(dāng)前相機(jī)圖像與地圖匹配。然后通過卡爾曼濾波器組合由PFL和LFL獲得的位置估計(jì)。

圖7  視覺互補(bǔ)定位方法（3）另一些方法通過使用相機(jī)數(shù)據(jù)來構(gòu)建特征圖，但還是采用替代類型的特征。如一種在城市場(chǎng)景中使用柱狀地標(biāo)作為主要特征（如下圖8所示）的方法，因?yàn)橹鶢畹貥?biāo)是獨(dú)特的、長(zhǎng)期穩(wěn)定的，并且可以通過立體攝像系統(tǒng)可靠地檢測(cè)到。此外，生成的地圖表示是內(nèi)存高效的，允許輕松存儲(chǔ)和在線更新。定位由作為主傳感器的立體攝像系統(tǒng)實(shí)時(shí)執(zhí)行，使用車輛里程計(jì)和現(xiàn)成的GPS作為輔助信息源。通過粒子濾波方法進(jìn)行定位，結(jié)合卡爾曼濾波實(shí)現(xiàn)魯棒性和傳感器融合。

圖8  利用柱狀地標(biāo)實(shí)現(xiàn)穩(wěn)定定位