星火计划学习笔记——第五讲（Apollo自动驾驶定位技术）
文章目录
- 1. 定位的作用
-
- 1.1 定位和地图: 一对孪生兄弟
- 1.2 参考系
- 1.3 定位的作用
- 2. 定位用到的算法
-
- 2.1 GPS
-
- 2.1.1 GPS 原理
- 2.2 RTK(GPS的扩展)
- 2.3 惯性测量单元
- 2.4 GNSS
- 2.5 先验地图定位
- 2.6 NDT算法
- 2.7 实时定位和建图
- 3. Apollo定位框架介绍
-
- 3.1 目录结构
- 3.2 RTK模块
-
- 3.2.1 RTK模块入口,输入,输出
- 3.2.2 RTK模块底层逻辑
- 3.3 NDT模块
-
- 3.3.1 NDT模块入口,输入,输出
- 3.3.2 NDT模块底层逻辑
- 3.3.3 NDT地图构建
- 3.4 MSF模块
-
- 3.4.1 MSF模块入口,输入,输出
- 3.4.2 MSF模块底层逻辑
- 3.4.3 MSF模块融合框架
- 4. 实践
-
- 4.1 制作NDT地图
- 4.2 启动NDT模块
- 声明
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 ) (x, y,z) (x,y,z)。
为什么是4颗呢?因为除了需要确定 ( x , y , z ) (x,y,z) (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 0.1° /s - 0.001° /h 0.1°/s−0.001°/h。
加速度计:加速度偏差为 100 m g − 10 μ g 100mg - 10μg 100mg−10μg。
由距离计算公式 s = 1 2 a t 2 s = \frac{{\rm{1}}}{{\rm{2}}}a{t^2} s=21at2可以得出， 100 m g 和 10 μ g 100mg和10μg 100mg和10μg偏差的加速度计在60s之后的测量误差分别是3528m和0.35m,差距非常大。
2.4 GNSS
GPS + IMU组合导航
- GPS定位非常方便，但在高楼、隧道和停车场等场所会出现多径效应和信号衰减，导致测量精度下降。GPS的更新频率也比较低，一般为1s刷新一次。
- 为了解决上述问题，IMU通常用来辅助GPS进行定位，IMU不需要接收信号，不受信号质量影响，而且更新频率快，工作频率通常可达100Hz以上，可以弥补短期GPS刷新频率不足的问题。但是IMU有累计误差，在一段时间后需要校准， 所以目前无人车主要通过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
声明
本人所有文章仅作为自己的学习记录，若有侵权，联系立删。