自動(dòng)駕駛汽車避障、路徑規(guī)劃和控制技術(shù)詳解

編者按：自動(dòng)駕駛汽車的行駛安全性是一個(gè)具有挑戰(zhàn)的問題，目前研究熱點(diǎn)在于車輛自主避障。論文針對這一問題，提出了一個(gè)全局的避障控制框架，包括感知、規(guī)劃和控制三個(gè)模塊。在感知模塊采用基于信度網(wǎng)格占用的方法識別障礙物，軌跡規(guī)劃模塊利用參數(shù)化的Signoid函數(shù)曲線設(shè)計(jì)避障軌跡，最后設(shè)計(jì)前饋加魯棒反饋的橫向控制器完成車輛的軌跡跟蹤，在常用的方法基礎(chǔ)上，作者還采用滾動(dòng)地平線以及調(diào)節(jié)特定點(diǎn)橫向和橫向誤差的方法減少計(jì)算量，最后在實(shí)驗(yàn)平臺ARTEMIPS上驗(yàn)證了所提出框架以及各方法的有效性。

摘要：在自動(dòng)駕駛車輛中，避障需要三個(gè)主要層次，即感知、路徑規(guī)劃和制導(dǎo)控制。在這篇論文中，通過考慮這三個(gè)層次之間的聯(lián)系，提出了一個(gè)全局架構(gòu)。在環(huán)境感知層面，提出了一種基于信度網(wǎng)格占用的動(dòng)態(tài)障礙物檢測方法。因此，在軌跡生成中需要考慮目標(biāo)的姿態(tài)。后者是基于平滑軌跡的s形函數(shù)。最后，控制制導(dǎo)利用該避障軌跡生成適當(dāng)?shù)霓D(zhuǎn)向角。整個(gè)策略在實(shí)驗(yàn)車上得到了驗(yàn)證。實(shí)驗(yàn)結(jié)果表明了該方法的有效性。

01介紹

智能交通系統(tǒng)解決了復(fù)雜環(huán)境中具有挑戰(zhàn)性的自主性和安全性問題，因此它吸引了研究人員特別的關(guān)注。自動(dòng)駕駛汽車概念的主要模塊是感知、規(guī)劃和控制。

實(shí)際上，感知是由環(huán)境建模和本地化組成的。它們分別依賴于外界和本體的傳感器。接下來，規(guī)劃旨在基于感知結(jié)果傳遞的信息來生成最佳軌跡，以便到達(dá)給定的目的地。最后，控制模塊專用于通過命令車輛的執(zhí)行器來跟蹤生成的軌跡。

本文將針對避免障礙的具體情況介紹該過程的每個(gè)模塊。這些任務(wù)在全局體系結(jié)構(gòu)中的集成是本文的主要貢獻(xiàn)。感知模塊確保根據(jù)準(zhǔn)確的網(wǎng)格表示來描述環(huán)境。占用柵格地圖（OGM）的使用對于避免障礙特別方便，因?yàn)樗梢宰R別可行駛空間并在場景中定位靜態(tài)和動(dòng)態(tài)對象。然后在路徑規(guī)劃級別使用要避免的物體的姿態(tài)，該路徑規(guī)劃根據(jù)[1]中所示的 S型參數(shù)化函數(shù)和滾動(dòng)水平線生成軌跡和速度曲線。所獲得的曲率輪廓被認(rèn)為是引導(dǎo)控制模塊的參考路徑。根據(jù)使用撞擊中心（CoP）而非經(jīng)典重心的橫向引導(dǎo)控制器，此水平為車輛提供了適當(dāng)?shù)霓D(zhuǎn)向角。所提出的控制器基于前饋和魯棒的狀態(tài)反饋動(dòng)作，以分別減少干擾對橫向誤差的影響并保證橫向穩(wěn)定性[2]。

該文件的組織結(jié)構(gòu)如下：第二部分介紹了全局方法，其中包含為避免避障而將要實(shí)施的不同模塊。第三部分介紹了基于信度網(wǎng)格占用的動(dòng)態(tài)物體檢測方法。第四部分說明了基于參數(shù)化S型函數(shù)和滾動(dòng)地平線的避障算法。第五節(jié)詳細(xì)介紹了基于前饋耦合到魯棒狀態(tài)反饋的控制器設(shè)計(jì)。第六部分說明了該實(shí)驗(yàn)方法的實(shí)驗(yàn)平臺和結(jié)果。最后，第七節(jié)總結(jié)了論文。

02避障策略

本節(jié)介紹基于三個(gè)模塊的全局避障策略的標(biāo)題，如圖1所示。本節(jié)將簡要介紹每個(gè)層次。

圖1?避障策略

A.?感知模塊

正確而有效地感知環(huán)境對于自動(dòng)駕駛汽車是強(qiáng)制性的。這項(xiàng)研究主要關(guān)注環(huán)境感知，以提取靜態(tài)/動(dòng)態(tài)物體的位置以及基于外部感知傳感器的可駕駛路徑。定位部分未被視為車輛的位置被認(rèn)為是已知和可靠的。提取道路和周圍物體信息的最常用方法之一是“占用網(wǎng)格”（OG）。它可用于多種應(yīng)用，例如避免碰撞，傳感器融合，目標(biāo)跟蹤以及同時(shí)定位和映射（SLAM）[3]。OG的基本思想是將環(huán)境圖表示為二進(jìn)制隨機(jī)變量的均勻間隔字段，每個(gè)變量均表示環(huán)境中該位置處是否存在障礙物[4]?？梢愿鶕?jù)許多形式來生成它，以在已知車輛姿態(tài)的前提下處理嘈雜和不確定的傳感器測量數(shù)據(jù)。在本文中，OG是由Dempster和Shafer [5] [6]提出的信念理論定義的，因?yàn)樗鼘Σ淮_定性，不精確性和未知部分進(jìn)行建模，還允許管理數(shù)據(jù)融合中的沖突。第三部分給出了更多細(xì)節(jié)。

B.?參考軌跡生成模塊

該模塊專用于定義軌跡以及車輛要遵循的相應(yīng)速度曲線。規(guī)劃者從感知模塊接收可駕駛區(qū)域和障礙物位置。根據(jù)這些信息，可以生成幾何軌跡以及速度曲線。本文著重于路徑規(guī)劃策略。本部分旨在基于感知的可駕駛區(qū)域，提供從起點(diǎn)到終點(diǎn)的名義軌跡。

當(dāng)檢測到障礙物時(shí)，計(jì)算第二軌跡（避障軌跡）以確保自動(dòng)駕駛車輛乘客的安全性和舒適性，并在避開后加入標(biāo)稱軌跡。這種避免軌跡可以通過局部規(guī)劃獲得，因?yàn)樗簧婕皹?biāo)稱軌跡的一小部分。為了降低軌跡生成算法的計(jì)算成本，采用了滾動(dòng)地平線方法，如[1]所述，其工作在本文中進(jìn)行了擴(kuò)展，請參見第IV節(jié)。然后，可以將這些軌跡（標(biāo)稱和避障）視為控制模塊（主要是橫向控制器）的參考。

C.?控制模塊

控制模塊由兩個(gè)主要部分組成：縱向和橫向控制器，確保自動(dòng)駕駛控制。這里主要關(guān)注側(cè)向控制器以處理避障。實(shí)際上，適當(dāng)?shù)霓D(zhuǎn)向角由側(cè)向控制器提供，以遵循參考生成模塊給定的期望路徑?？梢酝ㄟ^減少兩個(gè)跟蹤誤差，即橫向誤差和航向誤差，來實(shí)現(xiàn)對所需路徑的跟蹤。在文獻(xiàn)[7]中存在的幾何和動(dòng)態(tài)橫向引導(dǎo)策略中，此處采用了基于碰撞中心（CoP）的動(dòng)態(tài)方法[8]。該選擇取決于此控制方法的性能。CoP是位于車輛重心（CoG）前面的幾何點(diǎn)，可以預(yù)測橫向位置誤差。然后可以期望更好的軌跡跟蹤。另一方面，由于CoP的運(yùn)動(dòng)與后輪胎側(cè)向力[9]分離，如第V節(jié)所示，橫向動(dòng)力學(xué)方程變得不太復(fù)雜。

03基于動(dòng)態(tài)網(wǎng)格的動(dòng)態(tài)障礙物檢測

OG是一種將空間多維細(xì)分化為單元的表示形式，每個(gè)單元都存儲其占用狀態(tài)的知識[4]。如今，由于可以使用更強(qiáng)大的資源來處理OG的計(jì)算復(fù)雜性，因此OG大量使用。網(wǎng)格的構(gòu)造已應(yīng)用于多個(gè)維度（2D，2.5D和3D）[10]，其中每個(gè)單元狀態(tài)均根據(jù)選定的形式描述。最常見的是貝葉斯框架，該框架首先被Elfes [4]所采用，隨后又進(jìn)行了許多擴(kuò)展，成為了著名的貝葉斯占用過濾器（BOF）[11]。其他作品則提出了基于Dempster-Shafer理論的形式主義，該理論也被稱為證據(jù)論，隨后進(jìn)行描述。

A.?使用信念理論

歸納為概率論，信念理論提供了數(shù)據(jù)和源缺陷的充分表示，因此適合于ITS中的感知。它提供了廣泛的融合算子，可以根據(jù)應(yīng)用程序處理這些屬性。一些使用Belief框架構(gòu)建OG的研究可以在[12]，[13]中找到。這項(xiàng)工作源自[13]的研究，該研究提出了一種基于由此產(chǎn)生的沖突的移動(dòng)物體檢測和可行駛空間確定方法。為此，將識別幀定義為包括認(rèn)為其為空閑（F）或已占用（O）的單元的狀態(tài)。識別框是Ω={F，O}。參考功效集框架包含以下假設(shè)的所有可能組合：2Ω= {?,F,O,{F,O}}。為了表達(dá)對每個(gè)狀態(tài)的信念，質(zhì)量函數(shù)m(.)分別表示沖突m(?)，自由狀態(tài)m(F)，占領(lǐng)狀態(tài)m(O)和未知狀態(tài)m({F,O}) 。

B.?傳感器模型

基本上，傳感器模型是如何計(jì)算與度量對應(yīng)的狀態(tài)的質(zhì)量函數(shù)。在我們的應(yīng)用中，要使用的傳感器是3D多回聲LIDAR（請參見第VI節(jié)）。輸入數(shù)據(jù)將包括根據(jù)一點(diǎn)pi范圍ri和一個(gè)角度θi。根據(jù)此數(shù)據(jù)集，構(gòu)造極坐標(biāo)中的掃描網(wǎng)格（SG）。SG的每一行對應(yīng)定義在RxΘ的一個(gè)角扇區(qū)Θ=[θ-，θ+]。單元格的范圍是R=[r-,r+] 這意味著每個(gè)像元由一對質(zhì)量定義為m{Θ,R} 。每個(gè)命題對應(yīng)的質(zhì)量A∈Ω在這里找到[13]：

其中μF和μO分別對應(yīng)于傳感器的誤報(bào)警和漏檢的概率。為簡單起見，將說明這些質(zhì)量函數(shù)，m(O)，m(F)和 m(Θ)。

圖2?地圖網(wǎng)格(MG)構(gòu)造

圖2示出了使用在時(shí)間t處提供的傳感器點(diǎn)云來建立和更新MG的過程。此更新是根據(jù)多網(wǎng)格證據(jù)融合完成的。這是該過程中最有趣的部分，因?yàn)樗试S時(shí)間更新地圖網(wǎng)格并評估單元狀態(tài)。在信念理論的各種運(yùn)算符中，使用了Dempster-Shafer組合規(guī)則：

其中 mMG,t和mMG,t-1分別表示在時(shí)間t的地圖網(wǎng)格和掃描網(wǎng)格的質(zhì)量函數(shù)。運(yùn)算符定義為：

其中，

產(chǎn)生的結(jié)果 mMG,t(A)定義每個(gè)單元的狀態(tài)，該狀態(tài)取決于先前的狀態(tài)和新的度量。發(fā)現(xiàn)根據(jù)每種狀態(tài)產(chǎn)生的質(zhì)量如下[13]：

基本上，此屬性顯示了在t-1和t之間的不協(xié)調(diào)。不協(xié)調(diào)的出現(xiàn)在單元從自由狀態(tài)（Free）改變到被占用狀態(tài)（Occupied）或者相反。因此，沖突的檢測可以導(dǎo)致對動(dòng)態(tài)單元格的評估。沖突允許標(biāo)記占用的單元格，它們根據(jù)兩種沖突類型改變其狀態(tài)：

其中，

融合過程通過總沖突將狀態(tài)質(zhì)量歸一化，但是考慮使用此信息來標(biāo)記定義動(dòng)態(tài)對象的移動(dòng)單元。然后，將每個(gè)檢測的姿勢用作下一部分中軌跡生成的輸入。

04軌跡生成

本節(jié)專門用于路徑規(guī)劃，即創(chuàng)建幾何軌跡（跟隨坐標(biāo)點(diǎn)）Ai(xi,yi)。由于本文旨在驗(yàn)證所提出的回避體系結(jié)構(gòu)的可行性，因此未考慮速度曲線以及相關(guān)的縱向控制。如第二部分所述，路徑規(guī)劃模塊具有兩個(gè)目標(biāo)：根據(jù)起點(diǎn)和到達(dá)點(diǎn)生成全局標(biāo)稱軌跡，并生成局部軌跡以避免檢測到障礙物。在此，焦點(diǎn)集中在回避軌跡的產(chǎn)生上。該回避軌跡必須遵守安全標(biāo)準(zhǔn)，尤其是與障礙物之間的縱向和橫向距離。這些距離可以相等，如[1]中最近建議的那樣，在障礙物周圍創(chuàng)建了一個(gè)圓形安全區(qū)。本文通過考慮橫向和縱向安全性標(biāo)準(zhǔn)不同的整體情況，提出該方法的一般化。

為了獲得軌跡的幾何形式，基于回旋曲線，貝塞爾曲線或樣條曲線等函數(shù)，存在幾種數(shù)學(xué)方法[14]，[15]。[16]中對所有這些幾何方法進(jìn)行了詳盡的回顧。這些方法具有有趣的特征（平滑度，在候選集合中選擇最佳軌跡等），但是它們的計(jì)算成本可能很高。其中，sigmoid函數(shù)代表了平滑度和計(jì)算成本之間的公平取舍。考慮的方法建議將此數(shù)學(xué)方法與本地Horizon計(jì)劃程序結(jié)合使用，以減少計(jì)算成本。這種計(jì)劃方法的優(yōu)點(diǎn)在[1]中進(jìn)行了很大的討論。該本地計(jì)劃者考慮從占用柵格中檢測到的障礙物的信息，以定義適當(dāng)?shù)钠椒€(wěn)回避操作，并返回到標(biāo)稱軌跡。

A.?幾何規(guī)避

圖3給出了不同的軌跡：標(biāo)稱軌跡，安全區(qū)和最終的平滑回避軌跡。安全區(qū)定義R是檢測到障礙物之后的第一步。該區(qū)域不可導(dǎo)航，以免由于與障礙物接近而發(fā)生碰撞。Lx 和 Ly橢圓的半長軸和半短軸分別是定義區(qū)域的安全標(biāo)準(zhǔn)。定義后，就可以設(shè)計(jì)回避軌跡。為了確保乘客的舒適性，選擇了基于S形的功能。在圖3中，A 指的是起點(diǎn)（即自我車輛的重心）， B是sigmoid的屈曲點(diǎn)，而WP是要達(dá)到的起點(diǎn)。平滑度C （）可以進(jìn)行調(diào)整，因此避免軌跡可以定義為

為了獲得在障礙物移動(dòng)時(shí)具有魯棒性的算法，對每個(gè)水平矢量樣本重復(fù)整個(gè)過程（確定安全區(qū)域并計(jì)算基于S形函數(shù)的路點(diǎn)）。

圖3?軌跡規(guī)劃

B.?水平規(guī)劃

為了減少算法的計(jì)算成本，使用了局部規(guī)劃器。它沒有遵循整個(gè)避障軌跡，而是分為幾個(gè)部分。在此離散水平的每個(gè)樣本處都計(jì)算局部軌跡，從而降低了計(jì)算成本并使算法對動(dòng)態(tài)障礙物具有魯棒性?？梢詤?shù)化兩個(gè)參數(shù)：樣本大小和水平長度。最后一個(gè)取決于配備的感知傳感器（硬件約束）和車速（滾動(dòng)水平線）。樣本步驟代表軌跡在局部段中的細(xì)分。整個(gè)原理總結(jié)在圖4中。

圖4?地平線規(guī)劃

當(dāng)車輛到達(dá)下一個(gè)采樣步驟時(shí)，將再次計(jì)算局部水平線。可以看到，兩次迭代之間有一個(gè)共同的部分，允許算法處理動(dòng)態(tài)障礙。與離散時(shí)域一樣，樣本大小的選擇需要在準(zhǔn)確性和計(jì)算成本之間進(jìn)行權(quán)衡。該算法進(jìn)行迭代，直到地平線矢量到達(dá)完整軌跡的末尾，即，當(dāng)感知傳感器覆蓋軌跡的所有細(xì)分時(shí)。該幾何軌跡是制導(dǎo)控制級的輸入。

05控制器設(shè)計(jì)

本節(jié)介紹了在圖1所示的控制模塊中使用的橫向控制器設(shè)計(jì)。橫向制導(dǎo)旨在減少兩種誤差，即橫向誤差，車輛的CoG與參考軌跡之間的距離以及航向誤差，車輛縱軸與參考軌跡之間的距離，如圖5所示：

圖5?橫向和航向誤差

這里，代替經(jīng)典的CoG橫向誤差 ey，建議在CoP定義為[9]時(shí)使用橫向誤差：

CoP和CoG之間的距離xcop僅取決于車輛配置：

其中m和 Iz 車輛質(zhì)量和偏航慣性，以及 lf是CoG與前軸之間的距離。從（12）可以看出CoP橫向誤差ecop高于圖5側(cè)向誤差ey。這樣，可以預(yù)期橫向位置誤差，并且可以預(yù)期更好的軌跡跟蹤。與基于CoG（重心）的經(jīng)典控制器相比，此處的撞擊中心（CoP）被用作車輛上的幾何點(diǎn)。CoP的主要優(yōu)點(diǎn)是降低了橫向動(dòng)力學(xué)方程式的復(fù)雜度，因?yàn)楹筝喬M向力不會影響CoP的運(yùn)動(dòng)[9]。

基于平面自行車模型[17]，并使用跟蹤誤差（11）和（12），用于設(shè)計(jì)CoP橫向?qū)Ш娇刂破鞯母櫿`差模型為：

其中狀態(tài)向量，δf是前輪轉(zhuǎn)角，作為擾動(dòng)項(xiàng)包含期望橫擺角速度和橫擺角加速度。

Lr是CoG與后軸之間的距離，Cf和 Cr是前和后輪胎側(cè)偏剛度。注意后Cr 不在Ac第二行? 因此，使用CoP可以減少不確定參數(shù)的數(shù)量。

橫向控制器計(jì)算適當(dāng)?shù)摩膄為了保證狀態(tài)向量誤差的收斂接近零。此外，由于跟蹤誤差模型的動(dòng)力學(xué)特性受到wref的影響，控制器還必須確保其影響的衰減水平。為了實(shí)現(xiàn)這些目標(biāo)，提出了一種由前饋耦合到魯棒狀態(tài)反饋的橫向控制器[2]：

LFF和 KFB分別是前饋和魯棒的反饋增益。前饋動(dòng)作旨在部分消除向量的影響wref。CoP的好處在于，獲得的前饋不需要了解Cr。狀態(tài)反饋動(dòng)作可確保誤差向量趨于零的指數(shù)收斂并減弱矢量wref的影響?？梢允褂镁€性矩陣不等式（LMI）來表示這種魯棒的控制問題，如[2]所示。

06實(shí)驗(yàn)結(jié)果

A.?實(shí)驗(yàn)設(shè)置

實(shí)驗(yàn)平臺ARTEMIPS是配備有多個(gè)傳感器的自動(dòng)測試車：采用DGPS技術(shù)的高精度IMU（慣性測量單元）RT-3002、2臺IBEO LUX 2D 4層激光掃描儀，2臺VLP-16 Velodyne3D激光掃描儀和高范圍攝像機(jī)MANTA-G125（參見圖6））。RT-3002用作位置，速度，加速度和方向測量的參考傳感器。LUX掃描儀用于在汽車的前部和后部提供遠(yuǎn)程檢測（以4層點(diǎn)云的形式）。VLP-16用于完成對汽車兩側(cè)環(huán)境的檢測（它們提供16層點(diǎn)云，并具有360°環(huán)繞視野）。ARTEMIPS還配備了3個(gè)執(zhí)行器和2個(gè)集成的伺服電動(dòng)機(jī)MAC-141，用于控制方向盤和制動(dòng)踏板，以及用于導(dǎo)航汽車發(fā)動(dòng)機(jī)的多功能NI-daq系統(tǒng)。所有傳感器和執(zhí)行器都鏈接到運(yùn)行Intempora的RTMaps軟件解決方案的嵌入式計(jì)算機(jī)。它是專用于多傳感器和多執(zhí)行器系統(tǒng)的平臺。

圖6?實(shí)驗(yàn)平臺ARTEMIPS及其參考框架

B.?實(shí)驗(yàn)結(jié)果

出于可讀性目的，僅通過考慮障礙物避開情況的一種實(shí)驗(yàn)方案來評估所提出體系結(jié)構(gòu)的性能。此測試以恒定速度執(zhí)行vx=10km/h。

根據(jù)來自四個(gè)激光掃描儀的記錄數(shù)據(jù)集，根據(jù)第三部分中所述的方法，將點(diǎn)云用于OG的構(gòu)造。圖7顯示了周圍的場景和要避免的對象。OG的時(shí)間融合突顯了描述動(dòng)態(tài)單元的沖突。應(yīng)用了層次聚類算法（來自MATLAB中的統(tǒng)計(jì)和機(jī)器學(xué)習(xí)工具箱）來構(gòu)造動(dòng)態(tài)對象。它們?nèi)鐖D8所示。通過3D邊界框。顯示的坐標(biāo)對應(yīng)于基于GPS數(shù)據(jù)的車輛姿態(tài)。要避免的對象是紅色的對象?？梢宰⒁獾?，由于方法對定位誤差的敏感性，可以發(fā)現(xiàn)一些錯(cuò)誤的檢測結(jié)果。

圖7?用于避障測試的順序

圖8?點(diǎn)云，原點(diǎn)坐標(biāo)和障礙物檢測

實(shí)驗(yàn)結(jié)果如圖9所示。從左上圖可以看出，標(biāo)稱軌跡線與物體的位置相交，而生成的紅色路徑避免了障礙物。還可以觀察到，橫向控制器確保了良好的軌跡跟蹤，并可以避免在13 s和20 s之間檢測到障礙物。在此時(shí)間間隔內(nèi)，控制器會生成從正值變?yōu)樨?fù)值的轉(zhuǎn)向角，以避免障礙物并確保較小的跟蹤誤差和。

圖9?轉(zhuǎn)向控制器結(jié)果

07結(jié)論

本文提出了一種基于感知，路徑規(guī)劃和控制指導(dǎo)三個(gè)層次的動(dòng)態(tài)避障方案。動(dòng)態(tài)障礙物檢測是根據(jù)證據(jù)占用網(wǎng)格進(jìn)行的。路徑規(guī)劃基于sigmo?d函數(shù)生成平滑的軌跡，以避免檢測到障礙物。最終，車輛通過碰撞中心的基于橫向控制的策略跟隨車輛生成的參考軌跡。在我們的測試車輛上的實(shí)驗(yàn)結(jié)果表明，該方法可有效避開障礙物。未來的工作將包括考慮定位策略以及在更復(fù)雜的情況下對該方法的評估。

審核編輯：湯梓紅