摘要

高度精確且魯棒的定位能力對(duì)于城市場(chǎng)景中自動(dòng)駕駛車輛（AVs）至關(guān)重要。傳統(tǒng)的基于視覺的方法由于光照、天氣、視角和外觀變化而導(dǎo)致定位丟失。在本文中，我們提出一種新型的視覺語義定位算法，它基于高精度地圖和具有緊湊表達(dá)的語義特征。語義特征廣泛出現(xiàn)在城市道路，并且對(duì)于光照、天氣、視角和外觀變化具有魯棒性。重復(fù)的結(jié)構(gòu)、漏檢和誤檢使得數(shù)據(jù)關(guān)聯(lián)（DA）具有高度奇異性。為此，本文提出了一種考慮局部結(jié)構(gòu)一致性、全局模式一致性和時(shí)序一致性的魯棒DA方法。此外，本文引入一種滑動(dòng)窗口因子圖優(yōu)化框架，以融合關(guān)聯(lián)結(jié)果和里程計(jì)測(cè)量信息，而不需要地圖特征的高精度絕對(duì)高度信息。

我們?cè)?a target="_blank">仿真和真實(shí)城市道路上評(píng)估所提出的定位框架。實(shí)驗(yàn)結(jié)果表明，所提出方法能夠?qū)崿F(xiàn)高度精確定位，其平均縱向誤差為0.43m，平均橫向誤差為0.12m，平均偏航角誤差為0.11°。

介紹

近年來，AVs已經(jīng)受到工業(yè)界和學(xué)術(shù)界的廣泛關(guān)注。高精度定位對(duì)于AVs而言是一項(xiàng)關(guān)鍵技術(shù)，因?yàn)闆Q策、規(guī)劃和控制等各種模塊都嚴(yán)重依賴于定位結(jié)果。為了實(shí)現(xiàn)精確定位，AVs裝載了各種傳感器，例如GNSS、相機(jī)、激光雷達(dá)、IMU、車輪編碼器等。由于激光雷達(dá)的價(jià)格昂貴，因此低成本相機(jī)和IMU更適用于商用級(jí)AVs定位。

城市場(chǎng)景中具有各種復(fù)雜的道路條件，例如城市峽谷、隧道、高架等，這使得AVs定位更具有挑戰(zhàn)性。為了在該場(chǎng)景中實(shí)現(xiàn)魯棒定位，涌現(xiàn)了各種方法，例如基于GNSS的方法、基于視覺的方法、基于視覺-慣性的方法、基于激光雷達(dá)的方法。基于GNSS的方法能夠在開闊場(chǎng)景中實(shí)現(xiàn)厘米級(jí)精度，但是它在遮擋和多路徑條件下不夠可靠。所以，提出了融合GNSS和IMU或者里程計(jì)的方法，以解決GNSS的問題，但是由于里程計(jì)漂移，它們?cè)陂L(zhǎng)期缺少全局定位信息的場(chǎng)景中仍然會(huì)失效。為了解決漂移的問題，廣泛應(yīng)用基于先驗(yàn)地圖的方法。最常用的地圖為點(diǎn)云地圖，它能夠通過ICP或者NDT方法實(shí)現(xiàn)厘米級(jí)定位，但是點(diǎn)云地圖的存儲(chǔ)對(duì)于商用級(jí)AVs而言是一個(gè)巨大的挑戰(zhàn)。傳統(tǒng)的視覺特征地圖也已經(jīng)被嘗試用于定位，但是由于光照、天氣、視角和外觀變化，它們受到跟蹤丟失的影響。

為了解決這個(gè)問題，我們提取視覺語義特征，并且基于高精度地圖進(jìn)行定位。與傳統(tǒng)視覺特征相比，語義特征廣泛出現(xiàn)在城市道路上，并且對(duì)于天氣、光照、視角和外觀變化是長(zhǎng)期穩(wěn)定且魯棒的。由于語義特征的奇異性、誤檢和漏檢，DA是最大的挑戰(zhàn)之一。因此，我們提出一種具有一致DA的精確且魯棒的視覺語義定位系統(tǒng)。本文的主要貢獻(xiàn)如下：

1）一種基于視覺語義特征和輕量級(jí)高精度地圖的精確且魯棒的定位算法，無需地圖特征的高精度絕對(duì)高度信息；

2）一種基于局部結(jié)構(gòu)一致性、全局模式一致性和時(shí)序一致性的魯棒DA方法來解決DA的奇異性；

3）一種緊耦合視覺語義測(cè)量和里程計(jì)測(cè)量的因子圖優(yōu)化框架用于魯棒定位；

4）在仿真和真實(shí)城市道路上進(jìn)行大量實(shí)驗(yàn)來驗(yàn)證DA的有效性和定位的精度。

相關(guān)工作

A.基于傳統(tǒng)視覺特征的方法

基于傳統(tǒng)視覺特征的方法提取幾何特征，例如點(diǎn)、線和平面，并且通過描述子執(zhí)行特征匹配。Mul-Artal和Sons分別使用ORB和BIRD描述子來構(gòu)建特征地圖，接著通過特征匹配來獲取位姿。ETH ASL LAB在這一領(lǐng)域已經(jīng)完成大量的工作，包括多地圖匯總、基于外觀的在線路標(biāo)選擇。然而，這些方法仍然無法擺脫天氣、光照、視角和外觀變化的影響。

B.基于道路上語義特征的方法

基于道路上語義特征的方法被廣泛應(yīng)用于AVs。語義特征由道路標(biāo)記、交通信號(hào)燈、交通標(biāo)志、電線桿等組成。Schreiber和Poggenhans檢測(cè)道路標(biāo)記和路緣，并且通過將特征與地圖匹配來定位AVs。Lu應(yīng)用chamfer匹配來構(gòu)建道路標(biāo)記的約束，并且求解一個(gè)非線性優(yōu)化問題來估計(jì)6自由度位姿。此外，Jeong對(duì)道路標(biāo)記分類來避免奇異性，并且通過子地圖匹配、回環(huán)和位姿圖優(yōu)化來實(shí)現(xiàn)精確定位。Wilbers和Spangenberg通過具有深度的電線桿來實(shí)現(xiàn)位姿估計(jì)。同時(shí)，Sefati融合來自相機(jī)和激光雷達(dá)的道路標(biāo)記和交通標(biāo)志以通過PF進(jìn)行定位。此外，Wu通過從相機(jī)圖像中提取的車道線和從占據(jù)柵格中提取的blob特征來執(zhí)行定位。Ma通過概率直方圖濾波來融合INS、GPS、車道線和交通標(biāo)志的定位結(jié)果。除了道路標(biāo)志和電線桿之外，Kummerle還通過激光提取建筑物垂直表面的幾何信息來實(shí)現(xiàn)精確定位。 在本文中，我們僅使用一個(gè)單目相機(jī)進(jìn)行定位。我們無需絕對(duì)高度，而是需要相對(duì)于當(dāng)前位置的路面高度。這簡(jiǎn)化了地圖構(gòu)建的難度，并且大大降低了成本。

C.語義數(shù)據(jù)關(guān)聯(lián)

由于語義特征的奇異性、誤檢和漏檢，實(shí)現(xiàn)正確且魯棒的數(shù)據(jù)關(guān)聯(lián)是極其具有挑戰(zhàn)性的。Spangenberg通過歐式距離和電線桿寬度將感知的電線桿與地圖關(guān)聯(lián)。同時(shí)，Hu和Xiao應(yīng)用RANSAC來消除誤匹配。進(jìn)一步，Kummerle和Wilbers通過隨著時(shí)間累積檢測(cè)結(jié)果來構(gòu)建子圖，從而解決DA的奇異性。在目標(biāo)跟蹤的領(lǐng)域內(nèi)，運(yùn)行Hungarian算法和多假設(shè)跟蹤。為了解決DA的奇異性，Bowman將概率DA引入到語義SLAM系統(tǒng)中。與這些方法不同，本文提出一種基于局部結(jié)構(gòu)一致性、全局模式一致性和時(shí)序一致性的魯棒DA方法來消除由奇異性造成的誤匹配。

系統(tǒng)概述

全局定位問題可以被定義為：給定一系列傳感器測(cè)量和高精度地圖，估計(jì)位姿序列，其表示狀態(tài)軌跡。位姿和特征位置被定義為和。定位問題被表示為如下最大后驗(yàn)（MAP）推斷問題： MAP問題可以基于DA被劃分為兩步，包括DA過程和位姿估計(jì)過程。公式（1）表示DA過程，有必要在執(zhí)行位姿估計(jì)之前基于先驗(yàn)位姿來建立測(cè)量和地圖之間的數(shù)據(jù)關(guān)聯(lián)。因此，MAP推斷問題能夠被重新定義為： 因此，定位框架被分為四個(gè)部分，即傳感器和地圖、檢測(cè)器、關(guān)聯(lián)以及優(yōu)化，如圖2所示。

傳感器由一個(gè)單目相機(jī)、一個(gè)IMU、兩個(gè)車輪編碼器和一個(gè)GNSS接收器組成。相機(jī)用于檢測(cè)語義特征。IMU和車輪編碼器構(gòu)成里程計(jì)來提供局部相對(duì)運(yùn)動(dòng)估計(jì)。GNSS接收器可以提供當(dāng)前位姿的粗略估計(jì)，其用于系統(tǒng)初始化。檢測(cè)器層從圖像檢測(cè)道路標(biāo)記、電線桿、交通信號(hào)燈和交通標(biāo)志。關(guān)聯(lián)層關(guān)聯(lián)從圖像中提取的語義特征和高精度地圖中的特征。關(guān)聯(lián)過程被分為五個(gè)步驟。第一步，在先驗(yàn)位姿附近生成候選位姿，并且基于每個(gè)采樣的位姿將地圖特征投影到圖像上。第二步，實(shí)現(xiàn)基于局部結(jié)構(gòu)一致性的粗略關(guān)聯(lián)以尋找近似最優(yōu)的采樣位姿。第三步，執(zhí)行一種考慮匹配數(shù)量、匹配相似度和局部結(jié)構(gòu)相似度的最優(yōu)關(guān)聯(lián)方法，以實(shí)現(xiàn)最優(yōu)的全局一致匹配。第四步，執(zhí)行連續(xù)幀之間的特征跟蹤。第五步，執(zhí)行時(shí)序平滑以獲取時(shí)序一致的DA。在最后的優(yōu)化層中，引入位姿圖優(yōu)化以估計(jì)基于DA的位姿和里程計(jì)測(cè)量。

方法

A.語義特征和檢測(cè)

語義特征的選擇對(duì)于定位性能而言是至關(guān)重要的。在本文中，我們基于工作Accurate and efficient self-localization on roads using basic geometric primitives中提出的準(zhǔn)則來選擇特征，因此選擇道路標(biāo)記、電線桿、交通信號(hào)燈和交通標(biāo)志來定位。它們易于檢測(cè)、在場(chǎng)景中頻繁出現(xiàn)、存儲(chǔ)高效、表示緊湊，對(duì)天氣、光照、視角和外觀具有不變性。 我們采用一種主流的卷積神經(jīng)網(wǎng)絡(luò)（CNN）方法YOLOV3來檢測(cè)特征。一個(gè)檢測(cè)到的標(biāo)志包含一個(gè)檢測(cè)到的類別，一個(gè)表示檢測(cè)結(jié)果置信度的分?jǐn)?shù)和一個(gè)邊界框。標(biāo)志的四個(gè)輪廓點(diǎn)存儲(chǔ)在高精度地圖中，并且每個(gè)點(diǎn)的高度為相對(duì)于當(dāng)前位置所在路面的高度。一個(gè)檢測(cè)到的電線桿由一個(gè)檢測(cè)到的類別、一個(gè)表示檢測(cè)結(jié)果置信度的分?jǐn)?shù)和表示兩個(gè)頂點(diǎn)的組成。電線桿通過兩個(gè)頂點(diǎn)存儲(chǔ)在高精度地圖中。道路標(biāo)記在圖像平面和高精度地圖中被表示為采樣點(diǎn)。

B.語義數(shù)據(jù)與高精度地圖關(guān)聯(lián)

由于語義特征的奇異性、誤檢和漏檢，DA成為語義定位系統(tǒng)中最具挑戰(zhàn)性的問題之一。在本文中，我們提出一種基于局部結(jié)構(gòu)、全局模式和時(shí)序一致性的魯棒DA方法以解決DA的奇異性問題，并且確保空間和時(shí)序一致性。為了說明提出的DA方法，在Algorithm 1中提供了偽代碼。

DA過程的細(xì)節(jié)信息如下： 步驟1：在由里程計(jì)獲取的先驗(yàn)位姿附近通過采樣生成候選位姿。對(duì)于每個(gè)采樣位姿，將地圖特征投影到圖像平面上： 其中，為第個(gè)地圖特征的位置。和為相機(jī)的內(nèi)參和外參。為相機(jī)坐標(biāo)系中第個(gè)地圖特征的z軸位置。 步驟2：執(zhí)行基于局部結(jié)構(gòu)一致性的粗略關(guān)聯(lián)來尋找近似最優(yōu)的采樣位姿來消除由巨大先驗(yàn)位姿誤差造成的誤匹配。局部結(jié)構(gòu)一致性使感知特征和對(duì)應(yīng)重投影特征的橫向位置分布保持一致。首先，根據(jù)橫向位置對(duì)感知特征和重投影特征進(jìn)行升序排序。其次，我們計(jì)算每個(gè)感知特征和每個(gè)重投影特征之間的相似度： 其中，和可以通過感知結(jié)果的離線學(xué)習(xí)來獲取。似然由位置和大小相似度組成： 其中，為一個(gè)學(xué)習(xí)的超參數(shù)，用于權(quán)衡位置相似度和大小相似度。、、和分別表示地圖特征和感知特征的位置和大小。和可以從感知結(jié)果中離線學(xué)習(xí)。電線桿的似然由位置、姿態(tài)和重疊相似度組成。 如果感知特征的最大相似度分?jǐn)?shù)大于一個(gè)閾值并且保留了局部結(jié)構(gòu)，則將它們作為匹配對(duì)。對(duì)于每個(gè)采樣的位姿，基于匹配數(shù)量和匹配誤差計(jì)算代價(jià)以近似評(píng)估它： 其中，為超參數(shù)。定義為特征和之間的橫向距離，如圖3所示。

具有最大值的候選位姿被認(rèn)為是近似最優(yōu)的匹配采樣位姿，并且將在步驟3中使用。 步驟3：基于近似最優(yōu)的匹配采樣位姿，執(zhí)行一種考慮匹配數(shù)量、匹配相似度和局部結(jié)構(gòu)相似度的最優(yōu)關(guān)聯(lián)方法，以實(shí)現(xiàn)最優(yōu)的全局一致匹配。通過求解以下優(yōu)化問題，將其表述為多次圖匹配問題： 其中，和位置特征和重投影特征的數(shù)量，為兩個(gè)特征之間邊的數(shù)量。、和為超參數(shù)。表示感知特征是否與重投影特征匹配。表示感知特征和重投影特征之間的相似度，它由公式（4）計(jì)算獲得。表示邊和之間的相似度： 其中，和表示特征和、以及特征和之間的橫向距離，如圖3所示。可以離線學(xué)習(xí)。優(yōu)化問題將通過通用的random re-weighted walk framework求解。 步驟4：特征跟蹤：該過程建立連續(xù)幀中特征之間的關(guān)聯(lián)。由于感知的特征是靜態(tài)且保持局部結(jié)構(gòu)的，我們將該過程表述為一個(gè)多次圖匹配問題，類似于公式（7）。 步驟5：時(shí)序平滑：該過程構(gòu)建連續(xù)幀中感知到的特征與地圖特征之間的最優(yōu)一致性匹配。通過滑動(dòng)窗口中先前匹配結(jié)果可以驗(yàn)證當(dāng)前幀的匹配正確性。進(jìn)一步，如果當(dāng)前幀中出現(xiàn)誤匹配，可以基于先前匹配和跟蹤找到并且校正。時(shí)序平滑通過在滑動(dòng)窗口中對(duì)每一幀上的匹配和匹配置信度進(jìn)行加權(quán)以獲取地圖特征對(duì)應(yīng)的感知特征： 其中，表示是否地圖特征與感知特征相匹配。通過評(píng)估特征和局部結(jié)構(gòu)相似度來給出匹配置信度： 如果最佳感知特征的累積置信度比次佳感知特征的累積置信度高很多，則最佳感知特征將被認(rèn)為是地圖特征的匹配對(duì)。否則，地圖特征被認(rèn)為具有不確定的匹配，并且可以給出每個(gè)感知特征的匹配概率。該過程區(qū)分確定和不確定的匹配，這可以解決由奇異性造成的誤匹配問題。

C.位姿圖優(yōu)化

公式（2）的位姿估計(jì)過程可以被定義為先驗(yàn)概率和似然的乘積： 根據(jù)高斯分布假設(shè)，通過里程計(jì)的相對(duì)運(yùn)動(dòng)估計(jì)來獲取先驗(yàn)分布。我們基于里程計(jì)測(cè)量和特征的匹配對(duì)表述一個(gè)滑動(dòng)窗口非線性最小二乘估計(jì)器來估計(jì)最新的個(gè)位姿。與常用的濾波方法相比，優(yōu)化方法可以處理異步且延時(shí)的測(cè)量數(shù)據(jù)，并且以相同的計(jì)算資源實(shí)現(xiàn)更高的精度。優(yōu)化目標(biāo)表示為： 其中，每個(gè)誤差項(xiàng)與對(duì)應(yīng)的信息矩陣一起被認(rèn)為是一個(gè)因子，每個(gè)狀態(tài)變量可以認(rèn)為是一個(gè)節(jié)點(diǎn)，因此定位問題可以通過因子圖表示，如圖4所示。

誤差項(xiàng)由里程計(jì)誤差、語義測(cè)量誤差和地圖誤差組成。里程計(jì)誤差定義為： 語義測(cè)量誤差因子表示為： 其中，表示向量的首個(gè)元素。測(cè)量誤差僅采用橫向誤差來消除高度誤差的影響和地圖特征對(duì)精確絕對(duì)高度的要求，如圖5所示。

地圖誤差因子表示為： 其中，為第個(gè)地圖特征的位置。在本文中，我們采用參考文獻(xiàn)Localization with sliding window factor graphs on third-party maps for automated driving中提出的地圖特征方差構(gòu)造方法。在地圖因子的各向同性假設(shè)下，根據(jù)假設(shè)的地圖質(zhì)量，地圖因子的方差可以定義為： 其中，為逆卡方累積分布函數(shù)，表示置信度，表示半徑。 非線性優(yōu)化問題可以通過一個(gè)迭代算法直接求解。采用滑動(dòng)窗口而不是批量方法來提高計(jì)算效率，同時(shí)確保定位精度。舊的狀態(tài)被截?cái)嗖⑶抑苯雍雎浴＿吘壔椒ㄒ部梢蕴幚砝系臓顟B(tài)，但是它累積了線性誤差，使得系統(tǒng)矩陣變得稠密，并且導(dǎo)致死鎖。邊緣化方法基于過去的數(shù)據(jù)約束位姿，但是使用地圖特征作為先驗(yàn)足以約束車輛位姿。

實(shí)驗(yàn)評(píng)估

A.典型場(chǎng)景中仿真

我們首先評(píng)估感知和地圖誤差如何影響所提出算法的性能。選擇一個(gè)典型的交叉路口場(chǎng)景，其中道路兩側(cè)有四根電線桿，車輛前方有兩個(gè)交通信號(hào)燈和兩個(gè)交通標(biāo)志。在不同感知和地圖誤差下的位置和偏航角誤差如圖6所示。

所有結(jié)果均為1000次實(shí)驗(yàn)的平均值，以消除隨機(jī)性。結(jié)果表明，所提出方法獲得高度精確的定位結(jié)果，其在0.2m地圖誤差和5個(gè)像素感知誤差下，位置誤差為0.23m，偏航角誤差為0.064°。 所提出算法不需要地圖特征的高度精確絕對(duì)高度。我們將本文方法與6自由度位姿估計(jì)方法進(jìn)行比較，以展示對(duì)地圖特征不同高度誤差的影響。我們還在典型的交叉路口進(jìn)行實(shí)驗(yàn)。圖7說明了結(jié)果，它表明本文方法在位置和偏航角方面均獲得明顯更小的誤差。因此，我們的橫向誤差測(cè)量模型對(duì)所提出系統(tǒng)的性能有益。

不同的特征分布也會(huì)影響定位性能。為了探索所提出算法的性能限制，我們?cè)诰哂胁煌卣鞣植嫉母鞣N場(chǎng)景中對(duì)其進(jìn)行量化。我們將它們分為兩種情況，包括車輛和特征之間不同的縱向距離，以及車輛坐標(biāo)系下特征之間不同的橫向距離。本文選擇一個(gè)典型場(chǎng)景，其中車輛兩側(cè)的兩個(gè)電線桿具有不同的距離。地圖和感知特征假設(shè)分別具有0.05m和2個(gè)像素的標(biāo)準(zhǔn)差。實(shí)驗(yàn)結(jié)果如圖8所示，它表明縱向距離越長(zhǎng)，縱向定位越差，但是橫向和偏航定位越好。然而，當(dāng)橫向距離增加時(shí)，縱向精度變得越好，但是橫向和偏航定位變得越差。

B.真實(shí)城市道路場(chǎng)景

我們?cè)谝粭l長(zhǎng)30km且具有各種場(chǎng)景（例如城市峽谷、隧道和高架）的城市道路上使用我們的車輛來評(píng)估所提出算法的性能。為了評(píng)估定位精度，融合激光雷達(dá)、RTK和里程計(jì)的定位結(jié)果作為真值。所提出系統(tǒng)的定位性能與基于最近鄰（NN）的6自由度位姿估計(jì)方法在各種場(chǎng)景中的比較如表格I所示，它描述了所提出系統(tǒng)在各種場(chǎng)景中實(shí)現(xiàn)了更高的精度，其平均縱向誤差為0.43m，平均橫向誤差為0.12m，平均偏航角誤差為0.11°。

圖9給出具有定位誤差的軌跡，并且在高架道路上使用GNSS比較了定位精度。結(jié)果表明，本文方法具有明顯更低的誤差，這證明了本文方法在復(fù)雜道路場(chǎng)景中的有效性，在該場(chǎng)景中GNSS會(huì)失效。

圖10給出橫向、縱向和偏航誤差分布。

所提出DA方法的正確性也通過在各種具有誤檢和漏檢場(chǎng)景中的實(shí)驗(yàn)進(jìn)行評(píng)估。圖11給出兩個(gè)場(chǎng)景的DA結(jié)果。左圖展示本文方法在一般場(chǎng)景中的正確匹配結(jié)果。中間和右邊圖像說明關(guān)聯(lián)方法有效地處理由誤檢造成的奇異性，這主要獲益于局部結(jié)構(gòu)和時(shí)序一致性約束。

我們還比較高精度地圖與特征地圖和點(diǎn)云地圖的規(guī)模。高精度地圖僅需要大約每公里10KB的存儲(chǔ)空間，而特征地圖和壓縮的點(diǎn)云地圖分別需要每公里53MB和每公里3MB的存儲(chǔ)空間。

總結(jié)

在本文中，我們提出一種新型的語義定位算法，它利用圖像中提取的語義特征和高精度地圖中的特征。為了處理由重復(fù)結(jié)構(gòu)、漏檢和誤檢造成的DA奇異性，使用一種考慮局部結(jié)構(gòu)、全局模式和時(shí)序一致性的魯棒DA方法。一種緊耦合數(shù)據(jù)關(guān)聯(lián)和里程計(jì)測(cè)量數(shù)據(jù)的滑動(dòng)窗口因子圖優(yōu)化框架被設(shè)計(jì)用于精確且魯棒的定位。對(duì)所提出系統(tǒng)進(jìn)行驗(yàn)證，并且結(jié)果表明，所提出方法實(shí)現(xiàn)了亞米級(jí)的定位精度。 未來，我們希望使用更全面的理論來擴(kuò)展我們的框架，以實(shí)時(shí)地評(píng)估定位性能。我們還計(jì)劃?rùn)z測(cè)更多的語義特征來提高所提出系統(tǒng)的魯棒性。