Apollo星火计划学习笔记——第五讲（Apollo自动驾驶定位技术）

前言

Apollo星火计划课程链接如下
星火计划2.0基础课：https://apollo.baidu.com/community/online-course/2
星火计划2.0专项课：https://apollo.baidu.com/community/online-course/12

1. 定位的作用

    自动驾驶汽车面临的第一个问题:从哪里来，到哪里去?

    运输是指某一物体 (人或者是货物)通过运输工具(或交通工具与运输路径)，由甲地移动至乙地，完成某个经济目的的行为。

1.1 定位和地图: 一对孪生兄弟

    定位，获取当前的地理位置信息，通常要通过地图来记录和表示。
    先有定位还是先有地图(鸡和蛋) ?
    通过定位来绘制地图，而通过地图来表示车辆当前的位置。

1.2 参考系

    参考系:地理位置有多种表示方式，地理学上大致可以分为2种。
  ➢相对位置——相对位置则是透过标示目标物与其他事物之间的关系,以表达事物位置的标示法。当沿着大部分主要公路行驶时，都有里程标志，指示到下一个城镇的距离。这些信息表示您相对于即将到来的地点的当前位置。
  ➢绝对位置——绝对位置表达标记事物在空间中单一 、不会因观察者所在位置改变而更改的标示法。基于特定的地理坐标(例如经度和纬度)来引用地球表面上的某个位置。

1.3 定位的作用

    定位模块除了需要告诉自动驾驶系统自动驾驶汽车当前的位置之外，还提供更多的信息，实际上定位模块还提供车辆当前的运动状态。
    除了位置之外，定位模块还提供:车的航向，速度，加速度，角速度等状态信息。

2. 定位用到的算法

2.1 GPS

    GPS全球定位系统(Global Positioning System, GPS)，是一种以人造地球 卫星为基础的高精度无线电导航的定位系统，它在全球任何地方以及近地空间都能够提供准确的地理位置、车辆行驶速度及精确的时间信息。GPS自问世以来，就以其高精度、全天候、全球覆盖、方便灵活吸引了众多用户。

    全球四大导航系统:
➢美国全球定位系统
➢欧盟“伽利略”系统.
➢俄罗斯“格洛纳斯”系统
➢中国北斗卫星导航系统

2.1.1 GPS 原理

    全球定位系统由以下三个部分组成:空间部分(GPS卫星)、地面监控部分和用户部分。
    GPS定位的基本原理是:至少测量出当前位置与4颗卫星的距离和对应时间，通过解方程即可求得当前位置在地球坐标系下的位置$(x,y,z)$。
    为什么是4颗呢?因为除了需要确定$(x,y,z)$三个位置参数，还需要一个卫星提供相对零点时间从而消除时间误差。

2.2 RTK(GPS的扩展)

    RTK ( Real - time kinematic，实时动态)载波相位差分技术，是实时处理两个测量站载波相位观测量的差分方法，将基准站采集的载波相位发给用户接收机，进行求差解算坐标。
    卫星定位是存在误差的，误差既来自系统的内部，也来自外部。例如卫星信号穿透电离层和对流层时产生的误差，还有卫星高速移动产生的多普勒效应引起的误差，以及多径效应误差、通道误差、卫星钟误差、星历误差、内部噪声误差等。这些误差，有些可以完全消除，有些无法消除或只能部分消除。它们影响了系统的准确性和可靠性。

• 基准站先观测和接收卫星数据。
• 基准站通过旁边的无线电台(数据链)，将观测数据实时发送给流动站(距离一般不超过20公里)。
• 流动站收到基准站数据的同时，也观测和接收了卫星数据。
• 流动站在基准站数据和自身数据的基础上， 根据相对定位原理，进行实时差分运算，从 而解算出流动站的三维坐标及其精度，其定位精度可达1cm~2cm。

    利用先验信息进行定位。

2.3 惯性测量单元

    惯性测量单元(英语: Inertial measurement unit,简称IMU )是测量物体三轴姿态角(或角速率)以及加速度的装置。
    一般的，一个IMU内会装有三轴的陀螺仪和三个方向的加速度计，来测量物体在三维空间中的角速度和加速度，并以此解算出物体的姿态。为了提高可靠性，还可以为每个轴配备更多的传感器。一般而言IMU)要安装在被测物体的重心上。

参数指标:
陀螺仪:角度偏差为$0.1°/s-0.001°/h$。
加速度计:加速度偏差为$100mg-10\mu g$。
    由距离计算公式$s=\frac{1}{2}a{t}^2$可以得出，$100mg和10\mu g$偏差的加速度计在60s之后的测量误差分别是3528m和0.35m,差距非常大。

2.4 GNSS

GPS + IMU组合导航

• GPS定位非常方便，但在高楼、隧道和停车场等场所会出现多径效应和信号衰减，导致测量精度下降。GPS的更新频率也比较低，一般为1s刷新一次。
• 为了解决上述问题，IMU通常用来辅助GPS进行定位，IMU不需要接收信号，不受信号质量影响，而且更新频率快，工作频率通常可达100Hz以上，可以弥补短期GPS刷新频率不足的问题。但是IMU有累计误差，在一段时间后需要校准， 所以目前无人车主要通过GPS和IMU融合定位。

表2-1 GPS和IMU优劣势对比

	GPS	IMU
更新频率	低( 1HZ )	高( 100HZ)
累积误差	无	有
外部依赖	有	无

2.5 先验地图定位

GPS+IMU的局限性:
    GPS+IMU的组合极大的提高了定位的精度和鲁棒性，但还是有一些特殊场景，例如城市高楼，树荫以及比较长的隧道等场景下，定位精度会受到影响。
    因此引入了新的方法:先验地图
    通过离线的方式，事先建好当前区域的3维地图，也就是我们常说的高精度地图定位图层。

先验地图:一场寻宝游戏 。

    以下是一段寻宝的过程“从岛的北面上岸，向前走，是一大片绿地,在上面可以看到一棵橡树和一棵松树,还有一个绞架，从绞架开始沿直线朝橡树走20步，宝藏就在那里。”
    先验地图的核心就是拿当前观测到的特征和事先建好的地图做匹配。

先验地图的2大核心问题。
1.特征提取和特征匹配( Feature matching )
2.重定位( Relocation )
特征提取和特征匹配
●ICP ( iterative closest point )
●NDT ( Normal Distributions Transform )
●基于优化的方法
重定位( Relocation )
●在环境中找到自己的初始坐标，例如
在停车场中启动的时候。

2.6 NDT算法

NDT ( Normal Distributions Transform )
    NDT非常快速和准确，适合应用于大规模数据，但它对初始化也很敏感，需要足够准确的初始猜测，因此它通常用于从粗到细的对齐策略。可以节省计算量。

NDT三种特征类型
●球型特征
●线型特征
●面型特征

2.7 实时定位和建图

    先验地图的好处非常明显，通过事先建立好地图，然后在线做匹配从而得到目标当前的位置。但环境会发生改变，如果周围的环境变化了，而地图没有更新，则会得到错误的定位信息。
    有没有能够实时定位和建图的方法呢?
    SL AM是Simultaneous localization and mapping缩写，意为“同步定位与建图”。根据类型划分分为：

• 视觉SL AM
• 激光SL AM

视觉SL .AM算法
●ORB-SL AM
●LSD-SLAM
●VINS-Mono
激光SL AM算法
●Cartographer
●LOAM系列

    目前的精度已经足够，鲁棒性有待加强，也可以用于大规模分布式实时建图。

3. Apollo定位框架介绍

3.1 目录结构

3.2 RTK模块

3.2.1 RTK模块入口,输入,输出

3.2.2 RTK模块底层逻辑

    RTK模块接受GPS和IMU的消息，发布定位消息和定位状态，给其它模块使用。

底层逻辑:
●可以看到RTK模块的核心在于GPS+IMU设备给出结果的精度，这些信息的解算由GPS和IMU设备提供，当然也可以直接拿到GPS和IMU的原始数据进行解算。

3.3 NDT模块

3.3.1 NDT模块入口,输入,输出

3.3.2 NDT模块底层逻辑

    NDT模块接受GPS、IMU和激光雷达点云消息，通过和事先建好的NDT地图进行匹配，得到车辆当前的定位消息和定位状态，发布给其它模块使用。

底层逻辑:

• NDT模块依赖事先建好的NDT定位地图，地图的构建可以通过SLAM框架得到，之后保存到本地，根据车当前的位置加载地图，进行NDT匹配得到当前的位置信息。
• 当前NDT模块对GPS和NDT做了简单的融合，在NDT失效的情况下，输出GPS的定位结果。
• 定位地图的制作通过ndt_ map_creator来完成。

3.3.3 NDT地图构建

    准备工作:首先准备录制好的数据包，从包中解压出点云和点云当前的pose,对pose进行校正之后，保存在指定的文件夹。

    NDT建图过程:

• 从文件中读取点云和pose
• 根据点云的位置选择对应的node
• 对点云强度信息采样并且放入对应的Node

3.4 MSF模块

3.4.1 MSF模块入口,输入,输出

3.4.2 MSF模块底层逻辑

    MSF模块接受GPS、IMU和激光雷达点云消息，通过NDT输出的结果和组合导航输出的结果进行融合，得到车辆当前的定位消息和定位状态，发布给其它模块使用。

底层逻辑:
●MSF模块对GPS和NDT的结果进行融合，从而提高系统的鲁棒性。因此如何融合是MSF的关键。

3.4.3 MSF模块融合框架

    MSF模块接受GPS、IMU和激光雷达点云消息，通过NDT输出的结果和组合导航输出的结果，通过卡尔曼滤波进行融合，输出最后的定位结果。

4. 实践

云实验
实验分为2个大的步骤：
    制作NDT地图。根据解压事先录制好的record包，得到激光雷达点云信息和自动驾驶车辆姿态信息，然后校正激光点云姿态信息，最后调用NDT制图工具生成NDT地图。
    启动NDT模块，查看结果。制作好NDT地图之后，接下来，我们就可以运行NDT定位模块，它会加载NDT地图，它的输出是融合之后的定位信息。

4.1 制作NDT地图

    解压数据包，得到原始的激光雷达点云信息和车辆姿态信息，需要注意点云的cloud_topic信息，要根据自己系统中发布的点云topic进行匹配，如果不清楚，可以通过cyber_record info查看数据包中的topic信息，从而找到激光雷达的cloud_topic。

    解压好数据包之后，通过imu的信息对车辆的位置进行插值，得到校正之后的车辆pose。

    有了激光雷达点云信息和准确的pose之后，根据这些信息生成NDT地图，注意这里采用的是NDT的强度信息，因此如果点云中缺少强度信息，则实现不了定位。

ndt_map_creator --pcd_folders=/apollo/data/pcd --pose_files=/apollo/data/pcd/poses.txt --resolution_type=single --resolution=1 --zone_id=50 --map_folder=/apollo/modules/localization/map/ndt_map/local_map

4.2 启动NDT模块

首先我们需要根据车辆的情况修改以下配置
修改地图路径，我们的地图在“/apollo/modules/localization/map”中，修改“global_flagfile.txt”中的“–map_dir”为当前路径。

vi /apollo/modules/common/data/global_flagfile.txt
--map_dir=/apollo/modules/localization/map

修改zone_id和雷达外参，根据地图的UTM zone，修改zone id，例如北京的zone_id=50，修改激光雷达的外参文件，以及lidar的topic，要与数据包，或者实际发布的topic一致。

vi /apollo/modules/localization/conf/localization.conf
local_utm_zone_id = 50
lidar_extrinsics_file = apollo/modules/localization/msf/params/velodyne_params/velodyne64_novatel_extrinsics_example.yaml
lidar_topic = /apollo/sensor/hesai40/compensator/PointCloud2

上述配置完成之后，启动NDT模块

cyber_launch start /apollo/modules/localization/launch/ndt_localization.launch

接着播放数据包，模拟传感器发送消息

cyber_recorder play -f cyberecord_MKZ109_default_10_20220218103727_20220218103737.record -c /apollo/sensor/gnss/odometry /apollo/sensor/hesai40/compensator/PointCloud2 /apollo/sensor/gnss/ins_stat -l

最后通过cyber_monitor查看定位信息

cyber_monitor

声明

本人所有文章仅作为自己的学习记录，若有侵权，联系立删。