无人驾驶：高精地图与定位

自动车建图与定位技术简介

自动车定位作用

车辆定位

• 室内环境：地图定位，UWB 定位，人工标志
• 室外环境：卫星定位 GNSS：GNSS 是将多个全球定位系统 (GPS、北斗、伽利略、格洛纳斯…) 融合起来得到最终的定位结果，并可以通过差分定位 (RTK) 进一步减小定位误差；组合导航定位：卫星定位 ＋ RTK ＋ 惯导 ＋ 车辆信息，比单独卫星定位拥有更高的精度和可靠性

---

定位的作用

• 为下游算法模块提供车辆位姿信息 (位置＋姿态（角度）＝ 位姿)，例如路径规划模块依据车辆当前位姿和车道信息等计算后续最优化的行驶路径，控制模块根据车辆当前位姿相对于规划的车道路径的偏移给出控制指令，感知模块根据相对于车辆的物体位置和车辆的位姿解算出物体在全局坐标系里的位置，可以按全局坐标系位置实现对物体跟踪
  

SLAM 技术简介

• SLAM (Simultaneously Localization and Mapping，同步建图与定位)：已知观测位置＋观测数据可以建图，已知地图＋观测数据可以定位，SLAM 的目标就是利用视觉、激光雷达、里程计等传感器进行多传感器融合 (e.g. 滤波算法：EKF (扩展卡尔曼滤波) / IF / UKF；Particle Filters (PF) / Rao-Blackwellised Particle Filters (RBPF))，从而在移动中同时完成建图和定位。与在线实时定位不同，SLAM 是一种无线定位方式
  

里程计可以记录当前时刻相对上一时刻的位置，在得到 $t$ 时刻位置时，需要讲前面所有时刻的相对位置累加起来，这就会导致很大的累计误差

回环检测：采集很多圈的数据，融合多圈信息来修正误差

• 常见开源 SLAM：ROS: slam_gmapping；视觉 SLAM：ORB_SLAM
• More:
  • M. Montemerlo, S. Thrun, D. Koller and B. Wegbreit, “FastSLAM 2.0: An Improved Particle Filtering Algorithm for Simultaneous Localization and Mapping that Provably Converges", in International Joint Conferences on Artificial Intelligence, 2003
  • Austin I. Eliazar and Ronald Parr, “DP-SLAM 2.0”, in IEEE International Conference on Robotics and Automation, 2004.
  • G. Grisetti, C. Stachniss, and W. Burgard, “Improving grid-based SLAM with Rao-Blackwellised particle filters by adaptive proposals and selective resampling”, in International Conference on Robotics and Automation, Barcelona, Spain, 2005.
  • German Ros ∗ , Angel D. Sappa † , Daniel Ponsa ∗ and Antonio M. Lopez, Visual SLAM for Driverless Cars: a brief survey, Proceedings of the IEEE Intelligent Vehicles Symposium Workshops, 2012.
  • Guillaume Bresson, Zayed Alsayed, Li Yu, Sébastien Glaser, Simultaneous Localization And Mapping: A Survey of Current Trends in Autonomous Driving, IEEE TRANSACTIONS ON INTELLIGENT VEHICLES, 2017

自动车定位面临的技术挑战

• 传感器性能水平与故有缺陷
• 环境复杂：目标遮挡、 尺度/姿态多样性、光照多样性、场景多样性；环境干扰：光照、雨雪
• 基于地图的定位 (SLAM)：环境变化，移动物体 (移动物体可能会导致匹配算法错误)、建筑施工、季节变化
• 卫星定位：城市环境卫星信号易受遮挡、多路径效应干扰
• 计算资源限制：有限计算资源，高实时性要求 (100Hz) (组合导航 GNSS + IMU：GNSS 输出频率 1Hz，IMU 输出频率 100Hz)

行业自动车定位方案

车道线定位方案

• 车道保持 (L2 半自动驾驶)：观测前方车道线位置，计算车辆相对于车道线的位置，控制车辆保持在车道中央行驶
• 特点：不依赖地图，只需要智能摄像头，但不完全可靠，需要人工干预

---

卫星定位 / 组合导航方案

• 卫星定位＋IMU＋车辆信息：有成熟组合导航产品实现定位
• 特点：卫星信号良好的路段非常可靠，适合开阔环境，如高速公路，性价比优异，但受遮挡的环境下无法使用，城市环境下有时不可靠

---

基于地图的定位方案 (SLAM)

• 利用 SLAM 建图，然后进行多传感器融合定位：组合导航定位 (卫星定位＋惯导＋车辆信息)＋激光雷达定位 (激光点云实时匹配定位地图)＋视觉定位 (全局定位，适配不同的特征点和特征描述)

高精地图技术与应用

高精地图

• 高精地图：供自动驾驶使用的地图，包括底图图层：基本信息；定位图层：在定位时常常需要将定位地点与实际场景做点云匹配，这就需要把点云信息提前提取出来放在高精地图里作为环境地图；道路图层：车道信息、红绿灯等；动态图层：环境变化、动态物体等
• 高精地图特点
  • 高精度：绝对位置精度能达到厘米级
  • 高内容丰富度：实际物理元素（空中元素 (电线杆…)、道路元素 (车道线、路面箭头、停止线、人行横道、路沿、道路出入口/曲率/坡度…）、虚拟元素（路由参考线 (即下图道路中的线)、虚拟停止线…）、定制化信息（定位图层、感知图层、PNC 定制化图层等）
  • 高鲜度：现势性变化天级更新

高精地图作用

• 高精地图对定位：高精地图包含定位地图图层，提供定位所需要的环境信息。精确度高：误差在 20 ~ 5 cm；每个像素表示 ~（2cm x 2cm）的实际尺寸；定位地图图层格式与定位算法相关
• 高精地图对感知及预测：提前获知环境信息。高精地图包括环境地图和道路区域信息，可以通过物体所在位置辅助判定其类别；特定交通标志物位置，可以识别交通灯、识别道路标识、识别车道线… ；根据道路信息辅助预测物体运动意图
• 高精地图对规划控制：高精地图包括道路车道信息、规划道路路线和交通规则信息
  

高精地图组成

• 底图图层：制作高精地图道路图层的基础。通常由激光雷达扫描数据建立的高分辨率二维网格地图，每个网格记录激光雷达反射率信息。底图特点：较高分辨率 (5 cm) 、对建图精度较敏感。常用底图格式：激光反射平面图 (Orthographic Reflectivity Map)
  
• 定位图层：根据定位算法，生成定位算法需要的定位地图，它与使用的定位算法直接相关，例如在定位时常常需要将定位地点与实际场景做点云匹配，这就需要把点云信息提前提取出来放在高精地图里作为环境地图。定位地图格式通常有网格地图、GMM、特征地图、NDT 地图等
• 道路图层：为自动车系统提供道路元素的几何和语义信息。道路图层包含精确的道路信息。比如车道、车道线 、交通规则下的车道关联属性 (车道类型、连接关系、行车限制等，比如是否可以并道)；道路标识牌、交通信号灯的三维位置和基本属性；其他信息：比如道路限速，路边的栏杆花坛位置等
  
• 动态图层：为自动车提供其它环境变化情况，包括路况、环境变化、地图更新等信息，例如道路施工、交通事故、道路事件，新路开通关闭、禁行、限行、改道

---

高精地图格式

• OpenDrive 格式：OpenDrive 是一个地图标准，Apollo 的高精度地图采用了 OpenDrive 格式
• XML 格式：定义车道属性、连接方式、道路元素、逻辑关系、路口信号等等

制作方法：人工＋自动

• 高精地图制作的困难：大数据量；高精度采图、建图、标注、质检；法律政策管控
• 高精地图公司：无人驾驶公司：百度、Pony…；传统地图公司：百度、高德…；高精地图公司：宽凳科技…；其它公司

---

高精地图的制作

• (1) 传感器、数据采集 + (2) 地图构建 + (3) 地图标注 + (4) 地图质检发布
  

传感器、数据采集

• 挑战：道路变化：高精地图需要及时反映道路环境变化；效率与精度

---

采集车

• 专业地图数据采集车：性能好，经过严格标定；通常使用专业传感器、高性能 IMU；采图质量好，价格非常昂贵 (百万级)
  
• 众包采集车：通常只使用较廉价传感器；成本低。为了进一步提高精度，可以采集多趟数据，然后使用算法进行融合；也可以只将众包采集得到的数据用于更新地图

---

传感器、数据采集

• 采集车是数据采集的核心载体，用于搭载高精地图数据采集传感器：全球导航卫星系统 (GNSS)、惯性导航系统 (Inertial Navigation System, INS / IMU) (通常包含了 3 轴加速度计和 3 轴陀螺仪)、激光雷达、摄像头 (直接提取并输出道路元素，如 mobileye…)、轮测距器 (Wheel Odometer) (可以按已知的车轮直径推算出⻋辆行驶距离和大致方向)、其它设备：工控机等
• 采集的数据包括行车轨迹、图像、激光点云等数据。这些数据中包含车道线、交通灯、交通标志、路沿、隔离栏、路灯等信息。采集的数据保存至数据资料库中，供地图构建使用

---

传感器数据融合

• 采集数据中，卫星定位获得位置信息往往存在误差，需要进行修正，常用方案：车载组合导航，即卫星定位＋惯性导航系统 (IMU)＋轮速计 (Wheel Odometer)，使用差分定位 RTK 或者 PPK 后处理定位获得更精准的卫星定位结果
• 基于 LiDAR 的位姿修正：用 SLAM 对采集到的数据进行离线优化，优化车辆位姿信息和点云地图
• 基于视觉的位姿修正：Bundle Adjustment (SfM)

地图构建

• 地图构建的主要任务是：解析影像数据、激光点云数据及对应的车辆位置数据；对车辆位姿数据进行优化，然后根据位姿数据将雷达激光点云建立点云反射率图用于提取高精地图元素；融合视觉图像生成彩色底图，用于识别道路标线的颜色等信息；根据定位算法需要生成定位地图
• 地图构建常用方法
  • 卫星信号良好区域：可以考虑使用 RTK 或组合导航输出位姿；可以考虑使用 PPK 后处理位姿
  • 卫星信号不良区域：必须要进行位姿优化；兼顾准确性于计算量；考虑整体的一致性

---

局部点云匹配：ICP / GICP

• Aleksandr V. Segal, Dirk Haehnel, Sebastian Thrun, Generalized-ICP, Robotics: Science and Systems V, University of Washington, Seattle, USA, 2009.
• 位姿优化举例：注意到 $T_1$ 到 $T_3$ 的位移不等于 $T_{1,2}{T}_{2,3}$，也不等于 $T_{1,3}$。可以考虑用位移偏差设计一个误差函数 $E$：
  $$E(T_1,T_3)＝d(T^{1,3}－T_{1,2}{T}_{2,3}{)}^2+d(T^{1,3}－T_{1,3}{)}^2$$然后优化位置 $T_1\sim T_5$，使所有误差函数的平方和最小
  

地图标注

• 人工标注：庞大的人工作业量；重复性、繁琐、易出错；成本高 、效率低、耗时长
• 自动提取：道路元素提取、道路标识识别；数据维度高 (多传感器)：激光雷达、摄像头；传统特征提取；深度学习：标注训练数据，机器学习模型识别道路元素

地图质检发布

• 地图质检发布：对高精地图的质量进行评定和验收。数据完整性、逻辑一致性、位置准确性、元素准确性和时间准确性
• 内业质检：标注人员使用制图工具等手段检查高精地图标注的各类元素的完整准确与一致
• 外业质检：外业质检可以分为算法质检和实地质检
  • 算法质检主要指通过搭建相关的质检设备和质检平台，通过设计的算法提取出高精地图相关元素的位置和类别进行信息对比，输出误差报告
  • 实地质检可以用来检查高精地图元素的完备性和正确性，利用专业测量设备按照设计路线测量实际道路元素信息作为真值来评估高精地图的精度

自动检测算法介绍

• 激光雷达／反射率底图：使用图像处理算法或深度学习算法对反射率底图进行操作，提取车道线、路杆等道路元素；传统方法：使用提高对比度，滤波等手段对图像进行预处理，图像处理检测识别车道线边缘特征，分类判定车道线种类；深度学习方法：类似视觉语义特征提取步骤
  
• 视觉图像：分析图像提取车道线、红绿灯、道路标示牌等道路元素 (人工标注训练集基础上训练模型，运用模型提取道路元素＋人工质检，质检后的结果充实训练数据集)
  
• 激光视觉融合提高提取道路元素精度与可靠性
• 提取到的道路元素进行矢量化，分析生成道路信息，如车道位置，方向，宽度，相邻关系
• 分析道路拓扑关系，如路口道路连接关系，交通规则限制，红绿灯关系

方案／成果演示

高精地图成品

高精地图底图

三维点云地图

高精地图自动元素提取

自动车多传感器融合定位

自动车定位传感器方案

基于卫星的定位方法 (有成熟可靠的组合导航设备，适合开阔环境，如高速公路，性价比优异，但城市环境中卫星信号易受遮挡、受多路径效应干扰)

• 卫星定位 (GNSS)：GPS＋北斗＋伽利略＋格洛纳斯
• 差分定位 (卫星定位＋RTK): 高定位精度
• 组合导航定位: 卫星定位＋RTK＋惯导＋车辆信息。使用 GNSS、RTK 实现卫星信号良好区域的定位，使用 IMU、车体约束、轮速计实现卫星信号不良区域的定位，比单独卫星定位拥有更高的精度和可靠性
  • 常用算法：卡尔曼滤波
  • 松耦合：卫星定位模块提供估测的位置信息，算法融合卫星定位的位置、IMU、车辆底盘信息融合计算位姿输出
  • 紧耦合：对每个卫星的信号单独作为融合算法的输入，无卫星信号或卫星信号良好时效果和松耦合大致相同，弱卫星信号时表现好于松耦合

---

基于地图的定位方法 (地图依赖：环境变化、干扰、计算资源限制)

• 激光雷达定位: 实时激光点云匹配定位地图，计算出车辆位姿信息。激光雷达测距精度高，数据信息量小于视觉，但成本高
• 摄像头视觉定位: 摄像头视觉图像中提取特征点，实现定位。视觉成本低，信息丰富，但受光照等条件干扰

激光雷达定位算法简介

激光雷达定位的挑战

• 地图的创建与更新: 大范围厘米级精度定位地图，如何保障地图的精度并及时更新
• 复杂多变的城市场景：环境变化：道路或建筑施工、季节变化；环境干扰：天气、车流等；退化场景：很多场景的点云信息极为相似
• 激光定位算法：激光雷达点云匹配、激光点云特征（语义）匹配、激光里程计、SLAM
  

---

激光雷达定位

• 激光点云匹配定位：需要高精度定位地图，多线激光雷达点云处理后直接匹配定位地图，然后修正车辆位置与姿态
• 激光点云特征（语义）匹配：激光点云提取特征 (语义特征)，匹配实现记录的特征地图来修正车辆位置与姿态。通过提取语义信息降低环境变化对匹配算法的影响
• 其它：激光里程计、回环检测

---

常用激光定位算法

• GMM (Gaussian Mixture Maps)：采用方格地图，每方格 ~ 10cm。建图过程中对落在方格中所有的激光点进行统计，每个方格用高斯混合模型记录其中点云的高度分布 (假设高斯分布可以很好的表示环境)。在进行实时定位时，自动车会根据上一时刻的车辆位姿，对周围局部范围进行实时激光点云匹配 GMM 地图获得位姿 (假设车辆在高程、俯仰角、滚转角上误差小，即假设车辆在地面行驶。车辆的高程，俯仰角、滚转角可对照高精地图获得修正。搜索并找到匹配的 $X$，$Y$ (车辆位置)，$H$ (航向角)，使用 Hill Climbing 最后优化)，这要求车辆在短时间内估算的定位误差很小，仅需要在较小范围内进行定位 (局部定位)。特点：分辨率较高，能记录环境细节。但搜索运算量随方格数量增加而增加，为了减小计算量可以采用多分辨率 GMM，先使用较低分辨率 GMM 地图搜索可能的车辆位姿，然后从初选找到的可能的车辆位姿，不断缩小搜索范围，更精准的搜索直到找到最佳位置
  • Ryan W. Wolcott and Ryan M. Eustice, Fast LIDAR Localization using Multiresolution Gaussian Mixture Maps, IEEE International Conference on Robotics and Automation (ICRA), 2015.
    
• ICP／GICP (Iterative Closest Point)：点到点云、点到线、点到面匹配
• NDT (Normol Distribution Transform)：地图方格记录点云斜方差
• LOAM：提取线面特征定位

视觉等其它方案

视觉定位

• 按照特征分类：基于车道线的识别、基于特征点的定位
  
• 按照信息范围分类：视觉惯性里程计 (Visual-Inertial Odometry) (视觉 + IMU)、全局定位算法框架视觉 SLAM
• 视觉定位优缺点：通常成本较低、特定定位场景下适用；易受场景、光照、天气环境变化影响

---

多传感器融合定位

• 组合导航定位＋激光雷达定位＋视觉定位
  • 组合导航定位：卫星定位＋惯导＋车辆信息
  • 激光雷达定位：激光点云实时匹配定位地图
  • 视觉定位：全局定位，适配不同的特征点和特征描述
    
• 常用融合算法：卡尔曼滤波；优化算法：根据各个传感器定位结果，设计误差函数，求解使误差函数最小的位置轨迹作输出
  • 常见融合算法挑战：某个传感器定位结果异常，不同传感器定位结果冲突，传感器时序问题，延迟同步问题：不同传感器输出频率不同，组合导航设备、IMU：100Hz， 卫星定位: 1 ~ 5Hz，激光雷达：10 ~ 30Hz，摄像头：10 ~ 60Hz，但定位输出需要稳定、准确的高频率输出。通常使用 IMU 为定位输出骨架，使用其它定位手段修正定位输出轨迹，输出要求：准确、稳定、平滑，精度：10cm，频率：100Hz
    
• 实时定位效果：定位精度 (10cm)、输出频率 (100Hz)