前言
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
)
(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
声明
本人所有文章仅作为自己的学习记录,若有侵权,联系立删。