自動駕駛高精地圖-概述與分析

1.HD Maps: Structure, Functionalities, Accuracy And Standards

在自動駕駛系統的功能系統架構中，高精地圖與定位功能緊密相關，與感知模塊交互，並最終支持規劃和控制模塊。

不一樣級別的自動駕駛對地圖的精細程度要求不一樣。高精地圖不只存儲Road和Lane等詳細信息，並且還存儲Landmark信息和周圍環境信息用於輔助車輛定位；車輛定位信息和高精地圖信息又做爲Perception模塊的輸入，實現爲物理環境建模的目的；Planning&Control模塊依據Perception模塊輸出的環境模型規劃和控制車輛的駕駛行爲；同時Perception的建模信息又能夠做爲高精地圖的輸入，實現對高精地圖的及時更新。

1.1 HD Map Structure

如上圖所示，雖然叫法不一樣，大部分地圖廠商的高精地圖都基本分爲三層:Road Model, Lane Model 和Localization Model。

1.2 HD Map Functionalities

如上圖所示，Road Model用於導航規劃；Lane Model用於感知和考慮當前道路和交通情況的路線規劃；Localization Model用於在地圖中定位車輛，只有當車輛在地圖中準肯定位時，Lane Model才能幫助車輛感知。

1.3 HD Map Accuracy

絕對精度(Absolute Accuracy)：Map Geometry偏移Ground Truth Geometry的最大空間距離。

相對精度(Relative Accuracy)： 首先Align Map Geometry和Ground Truth Geometry，而後計算兩者之間的最大空間距離。

1.4 HD Map Related Standards

ISO/TC 204 發佈的Geographic Data File (GDF) 提供了一個用於地圖信息存儲和交換的基礎版本。GDF 5.1將支持自動駕駛，並於2018年發佈了Local Dynamic Map (LDM)標準，存儲在LDM的信息包含天氣、道路、交通情況、靜態信息等。

許多政府和工業團體都在積極推進自動駕駛地圖的標準化，好比，Open AutoDrive Forum (OADF) (openauto-drive.org) 做爲cross-domain平臺推進自動駕駛標準化。Traveller Information Services Association (TISA) (tisa.org) 正在討論將Transport Protocol Experts Group (TPEGTM) 的交通訊息精度提高到車道級別。Advanced Driver Assistance Systems Interface Specification (ADASIS) Forum (adasis.org) 發佈ADASIS protocol V3以支持車內高精地圖分發。Sensoris (sensor-is.org) 正在致力於基於車輛的傳感器數據交換格式的標準化，以及車輛到雲和雲到雲接口的研發。Navigation Data Standard (NDS, 2016) 和 OpenDRIVE(OpenDRIVE, 2015)是兩大高精地圖工業級標準，它們與GDF的對好比下:

還有一些地區和國家也在研究自動駕駛的地圖標準。日本在研究Dynamic Map(en.sip- adus.go.jp)；2018年5月，中國智能與互聯汽車產業創新聯盟（CAICV）自動駕駛地圖工做組（caicv.org.cn）正式成立，該工做組的願景是實現中國自動駕駛和高清地圖的標準化。

2.HD Map Models

HD Map Model包含Road Model, Lane Model和Localization Model。

2.1 Road Model

Road Model使用有序的形狀點序列組成的線段表示，每一個Road Section都包含Start Nodes和End Nodes。使用形狀點序列定義Curved Road Geometry，優勢是表達簡單，增長中間點的密度便可以得到更好的精度，但缺點是須要存儲大量的信息，Road Model的道路屬性和豐富的語義信息能夠爲自動駕駛帶來更多的先驗知識。

此外，相對於傳統的2D導航地圖，高精地圖的Road Model增長了高度信息，對於精度的要求也更高。不一樣地圖提出的用於ADAS的元素級精度要求以下表:

2.2 Lane Model

最著名的車道地圖是DARPA城市挑戰賽使用的Road Network Description File (RDNF)，可是RDNF是2D地圖，而且也很是粗糙。

Bertha Drive項目使用的基於Lanelet的三維車道模型中，車道不只包含高度精確的左右邊界的可行駛區域，並且車道行駛所須要的交通規則信息。

一般狀況下，Lane Model包含以下信息：

2.2.1 Highly accurate geometry model

Lane Geometry Mode在很大程度上決定了Lane Model的準確性、存儲效率和可用性，該模型不只涵蓋車道中心線、車道邊界和RoadMarkings，還應包含潛在的三維道路結構，如斜坡和立交橋等。此外，它還應該具有輔助車輛高效計算的能力。

2.2.2 Lane attributes

Lane Attribute主要包括車道中心線(沿車道中間的理論線)和具備不一樣形狀、顏色和材質的車道邊界。

2.2.3 Traffic Regulations, Road Furniture And Parking

交通規則和相關信息/參數能夠嵌入到其餘屬性中，例如車道的道路類型能夠隱式地指示道路的默認限速。同時Lane Model也應具有分段車道屬性的表達能力。與車道關聯的停車位和道路設施也須要高度精確的位置信息。

2.2.4. Lane connectivity

Lane connectivity描述了車道或車道的鏈接關係。交叉路口的拓撲和語義方面由Traffic Matrices處理，Traffic Matrices定義了符合交通規則的全部操做。

車道連通性幾何信息經過鏈接出入口控制點的Virtual Lane表示，Virtual Lane可使用與普通車道相同的幾何車道模型(NDS, 2016)或者採用徹底不一樣的車道模型表示方法(由於兩個固定的控制點會影響車道曲線的連續性)。

下表列舉了各個地圖供應商和標準化組織提供的Lane Model內容和精度要求。

2.3 Localisation Model

Localisation Model用於輔助車輛定位。根據所使用傳感器方案的不一樣，定位技術分爲Feature Based的方法和Dense Information Based的方法。

Feature-based Localisation Model

Feature-Based Localisation Model一般被存儲爲Graph，每個Graph Node包含Image和3D Landmark，鏈接Graph Node的Edge是車輛Pose。Landmark一般使用特徵描述子(Feature Descriptor)表達，以方便在Live Image和Map進行Feature匹配。

Feature Map易用高效，可是製做Offline Map和Online Localization都須要進行Feature Extraction，過程繁瑣複雜。

Dense Information-Based Localisation Model

Dense Information-Based Localisation Model能夠進一步分爲Location Based(Grid Map)或View Based(如Point Cloud)。毫米波雷達和RGB-D相機也能夠用於收集Dense Information進行製圖，但LiDAR是目前測繪和自動駕駛公司(HERE、TomTom、Google等)最普遍使用的採集設備。

2D Grid Map探索X-Y平面(地面)和X-Z平面(垂直於地面)，如上左圖所示，假設道路表面平坦，每個Grid Cell被激光雷達的平均反射值填充，或者由激光雷達反射值的高斯分佈填充，兩種定位方法的相對精度均在10釐米量級。另外一種方法是使用Occupancy Grid Map，如上右圖所示，每個Grid Cell被由距道路中心距離和網格被佔用機率聯合定義的累積機率填充，經過該手段將道路的3D信息壓縮爲2D柵格地圖，從而達到40cm之內的絕對定位精度。2D Grid Map的缺點在於對於環境變化不具魯棒性。

2.5D地圖是包含高度信息的2D Map。Wolcott和Eustice(2014)在2D X-Y Reflectivity Grid Map中添加Z信息，以描繪道路的高度變化；Morales等人(2010)將Estimated Height添加到2D Map中心線地圖中，以在室外林地環境中實現定位；此外，Wolcott和Eustice(2015)提出了對Z高度和反射率值進行建模，以捕獲結構和外觀變化，並使用多分辨率搜索進行優化。

3D點雲地圖一般由latitude、longitude、altitude、ntensity信息組成的點序列組成，而且能夠用Camera Data賦予點雲數據紋理。3D點雲地圖隨着面積的增長，其內存佔用也呈現指數級增加，這給實際使用帶來不少問題。

定位模型或高精地圖面臨的挑戰之一是如何反映環境中的結構、季節或光照條件變化。Churchill和New-man(2013) 提出了考慮「在不一樣的時間、不一樣的天氣和光照條件」場景差別的方案，Maddern等人(2015)提出的3D PointCloud Map也採用了這種Experience-Based Navigation的方法。Irie(2010)嘗試將Grid Map和Feature整合到一個Map中，以提升對光照變化的魯棒性。在將來，實時地圖將是地圖的終極目標。

3. HD Map Mapping

HDMap Mapping的主要方法是收集車輛的傳感器數據(GNSS Receiver、IMU、Camera、Lidar、Wheel Odometry等)，而後依賴於Mobile Mapping System (MMS) 和 Simultaneous Localisation and Mapping (SLAM)等方法完成地圖製做。

NMS依賴於GNSS/IMU進行Pose Estimation，地圖採集車的全部傳感器數據都包含精確的時間戳，在用於製圖以前，須要對這些數據進行離線處理和校準，好比，經過合併同一場景的包含地理信息的圖像來建立Dense Localisation Model，從包含地理信息的圖像中提取Feature用於車道建模也是熱門的研究領域。另一個研究課題是如何用解析方程來擬合道路和車道曲線，Betaille等人(2010)提出clothoids曲線來擬合實際道路曲線； Gwon等人(2016)提出使用樣條曲線擬合實際道路曲線，樣條曲線的每個曲線段爲三次多項式。

SLAM最初是針對沒有GNSS的環境開發的，其目的是利用Ego Motion Measurements 和Loop Closures創建一個全球一致的Environment Representation ，該Representation提供的Environment Topology和Metric Representation能夠輔助進行自動駕駛定位。

MMS和SLAM都有本身的挑戰。對於NMS而言，挑戰在於GNSS在城市區域的可用性和準確性相對較低；對於SLAM而言，挑戰在於隨着採集區域的增長，計算成本和計算複雜度爆炸性增加。這些緣由催生了對SLAM和GNSS/IMU相互集成的研究，經過增長其餘信息來源，如可公開獲取的航空影像和數字地圖，而不是僅僅使用Loop Closure做爲減少SLAM不肯定性的惟一來源。

HDMap Mapping面臨的挑戰之一是實現全自動化，目前的方案是引入機器學習的方法從圖像數據中提取道路網絡語義信息；另外一個挑戰是實現大規模製圖，解決方案是數據衆包，利用來自同一條道路的車輛的傳感器數據用於改進更新現有地圖或製做新地圖。

若是政府在規劃、施工遵照標準施工方案，那麼從設計和施工文件中獲取HDMap信息也是一種潛在可行的方法。

4.Vehicle Localization With HD Map And Numerical Analysis

4.1 Map Relative Localization

Map Relative Localization是解決主車相對於HDMap的位置估計問題。自動駕駛要求很是精確的六自由度(DOF)定位，這對於定義主車視野(Field Of View，FOV)以有效利用車道模型(Lane Model)進行感知相當重要。Levinson和Thrun(2010)研究認爲釐米級定位精度對於任何開放道路的自動駕駛都足夠使用。自動駕駛車輛還須要較高的位置更新頻率：對於某些系統，更新頻率高達200HZ（Levinson等人，2007年）；10HZ，限速63千米/小時（Levinson和Thrun，2010年），20HZ，時速60千米（Cui等人，2014年）。

高精地圖(HDMap)提供已知的環境信息，而後使用環境感知傳感器感知的信息用於定位。經過將從實時圖像或掃描中檢測到的Feature註冊到定位模型中，相對於地圖的六自由度(DOF)車輛姿態估計能夠達到10-20釐米精度，有了定位信息，就能夠將車道模型(Lane Mode)引入到感知任務中，從而將複雜的靜態環境感知問題轉化爲定位問題。

上圖顯示了地圖相對定位的通常流程。Appearance Based的方法避開了Feature Detection的步驟。「Data Association」將環境傳感器測量信息與地圖關聯起來。當使用貝葉斯狀態估計器時，如Kalman Filter(KF)以及其變種(Extended、Unscented等)、Particle Filter (PF)等，Pose Estimation過程對應於State Estimation。

使用KF時，Data Association對於Pose Estimation是必不可少的；當使用PF時它能夠簡化，可是能夠分別考慮每一個粒子。Map Relative Localisation使用的Data Association方法包括基於描述子的Feature Matching、Direct Points Comparison、Point Set Registration(Iterative Closest Point，ICP)；Appearance-based Matching使用Normalised Mutual Information、Maximum Likelihood Estimate、Normalised Information Distance、ICP和 Normal Distribution Transformation (NDT)進行Direct Registration。

4.2 Numerical analysis

本節分析使用NDT做爲Data Association和主車Pose Estimation的HDMap-Based Localisation，並比較了使用GNSS和ICP的Matching效果。

Input Scan: S=xi​(i=1...Ns​)，Map:M=yi​(i=1...Nm​)，Scan和Map匹配的過程稱爲Scan Matching或者Registration，匹配的結果是Vehicle相對於Map的六自由度剛體變換參數:p=(ψ,θ,ϕ,tx​,ty​,tz​)。

ICP和NDT是兩個普遍使用的Registration方法。ICP將Registration任務看作Source和Target的幾何要素(點、線、面)的對應問題。NDT能夠避免創建這種對應關係。

本節研究使用Autoware的真實數據集，從數據集中提取了大約3000個Scan做爲Input Scan。Scan採用0.5m Voxel Grid進行下采樣，而後分別使用NDT(Cell Size=0.5m)和ICP與Autoware的3D Point Cloud Map進行Match。

記初始Pose爲\(p_0\)​、Input Scan爲S，地圖爲M，NDT過程分爲以下兩步：

Step 1. Build NDT map

如上圖所示，第一步是將Point Cloud分散到預先定義的固定大小的立方體\({\beta_i​}，=1,...,m\)。假設任意一個立方體\(\beta\)包含的點集\({z_k​},k=1,...,n\)。Mean Vector \(\mu\)和Covariance Matrix \(\sum\)定義以下:

對於每個\(\beta_i\)進行3D正態分佈建模:\(N(\mu, \sum)\)，它的機率密度函數以下:

\(prob(x)=\frac{1}{c}exp(-\frac{(x-\mu)^T \sum^{-1} (x-\mu)}{2})\)

Step 2. Align the input scan to the map.

初始Pose:\(p=p_0\)​，採用牛頓法迭代優化姿態p。

1.採用姿態p對Input Scan進行變換。以下，其中T是Transformation Function。

\(x_i^{\prime} = T(p, x_i)\)

將\(x_i^{\prime}\)映射到Map Cell，經過幾率密度函數計算Score:

\(s(p) = -\sum_{i=1}^{N_s} prob(T(p, x_i))\)

1. 計算\(\Delta p\)

\(H \Delta p = -g\)

其中H是Hessian矩陣，g是梯度矩陣；

1. 更新姿態p

\(p=p+\Delta p\)

持續上述過程，直至\(\Delta p\)小於預約閾值。

如上圖，NDT、ICP、GNSS RTK的RMS Difference都在10cm之內。

如上圖所示，NDT、ICP、GNSS RTK的水平座標差別大多數都在2σ範圍內(<20cm)，可是也有一些超出了4σ範圍，所以要應用在自動駕駛領域，須要對定位輸出作嚴格的質量控制。

---

我的博客網站地址: http://www.banbeichadexiaojiubei.com