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.