摘要

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

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

介紹

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

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

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

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

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

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

4）在仿真和真實城市道路上進行大量實驗來驗證DA的有效性和定位的精度。

相關(guān)工作

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

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

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

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

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

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

系統(tǒng)概述

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

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

方法

A.語義特征和檢測

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

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

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

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

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

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

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

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

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

實驗評估

A.典型場景中仿真

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

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

不同的特征分布也會影響定位性能。為了探索所提出算法的性能限制，我們在具有不同特征分布的各種場景中對其進行量化。我們將它們分為兩種情況，包括車輛和特征之間不同的縱向距離，以及車輛坐標系下特征之間不同的橫向距離。本文選擇一個典型場景，其中車輛兩側(cè)的兩個電線桿具有不同的距離。地圖和感知特征假設(shè)分別具有0.05m和2個像素的標準差。實驗結(jié)果如圖8所示，它表明縱向距離越長，縱向定位越差，但是橫向和偏航定位越好。然而，當橫向距離增加時，縱向精度變得越好，但是橫向和偏航定位變得越差。

B.真實城市道路場景

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

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

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

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

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

總結(jié)

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

審核編輯 ：李倩