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
来源: https://www.cnblogs.com/self-driving-car/p/12353092.html