摘要

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

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

介紹

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

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

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

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

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

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

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

相關工作

A.基于傳統視覺特征的方法

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

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

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

C.語義數據關聯

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

系統概述

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

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

方法

A.語義特征和檢測

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

B.語義數據與高精度地圖關聯

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

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

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

C.位姿圖優化

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

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

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

實驗評估

A.典型場景中仿真

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

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

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

B.真實城市道路場景

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

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

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

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

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

總結

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