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))$
- 计算$\Delta p$
$H \Delta p = -g$
其中H是Hessian矩阵，g是梯度矩阵；
- 更新姿态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