apollo localization模块学习
定位
输入
在提供的RTK方法中,有两个输入:
- GPS-全球定位系统。
- IMU-惯性测量单元。
在所提供的多传感器融合定位方法中,有三个输入:
- GPS-全球定位系统。
- IMU-惯性测量单元。
- 激光雷达-光探测与测距传感器
输出
- 一个Protobuf message类型的
LocalizationEstimate
实例,它可以在localization/proto/localization.proto
中找到。
目录
下面是localization的目录结构,看之前最好看下该模块的readme文件
├── common // 声明配置(flags),从conf目录中读取相应的值
├── conf // 配置文件存放目录
├── dag // cyber DAG流
├── launch // cyber的配置文件,依赖DAG图(这2个和cyber有关的后面再分析)
├── msf // 融合定位(gnss,点云,IMU融合定位)
├── ndt // ndt定位
├── proto // 消息格式
├── rtk // rtk定位
└── testdata // imu和gps的测试数据
可以看到,主要是rtk,ndt,msf这3个目录分别代表了不同的定位方法,而proto是消息的格式定义,common和conf主要是存放一些配置和消息TOPIC。下面我们逐个分析各个模块。
rtk
rtk定位模块很简单,主要流程如下:
主要将gps以及imu中的msg填充到Localization中相应的位置。
ndt
ndt定位算法流程如下:
主要的流程在黄色步骤,主要进行ndt算法匹配。几个主要的函数实现如下:
LidarLocatorNdt::Update
具体流程如下:
- 组合导航之间的位置差,作为预测位置
- 根据当前预测位置加载周边地图
- 对当前帧地图进行滤波
- 获取点云地图,并将点云数据转到激光坐标系
- 进行NDT匹配
- 获取NDT匹配结果。
LidarLocatorNdt::ComposeMapCells
具体流程如下:
- 计算9个块的start end坐标,存入map_nodes_zones
- 遍历每一个块的区域,计算每一个区域的mean以及icov_,数据存到cell_map_中。
BaseMap::LoadMapArea
流程比较简单:
- 构建9块的map_ids
- 根据map_ids的值,导入所有的map_ids.LoadMapNodes(&map_ids)
NDT步骤
NDT算法的基本思想是先根据参考数据(reference scan)来构建多维变量的正态分布,如果变换参数能使得两幅激光数据匹配的很好,那么变换点在参考系中的概率密度将会很大。因此,可以考虑用优化的方法求出使得概率密度之和最大的变换参数,此时两幅激光点云数据将匹配的最好。
msf
msf定位算法流程如下:
主要的流程在黄色部分,主要进行多传感器融合。
其中Lidar Localization算法流程如下:
主要采用LK算法进行heading的计算,采用直方图滤波算法计算(x, y)。
其中,
r
m
,
r
z
r_m, r_z
rm,rz表示,地图中和当前帧点云反射率。
σ
m
,
σ
z
\sigma_m, \sigma_z
σm,σz表示反射率标准差。
其中, a m , a z a_m, a_z am,az表示,地图中和当前帧点云的高度值。
似然计算如下:
方差计算放如下:
权重
γ
\gamma
γ计算放如下:
区域
Z
Z
Z中最佳的offset计算如下:
协方差矩阵如下:
config.xml
<?xml version="1.0" encoding="utf-8"?>
<map>
<map_config>
<version>map_ndt_v01</version>
<node_size>
<x>128</x> # 1024个像素,每个像素0.125米
<y>128</y>
</node_size>
<range> # 默认值(单位米)
<min_x>0</min_x>
<min_y>0</min_y>
<max_x>1000448</max_x>
<max_y>10000384</max_y>
</range>
<compression>true</compression>
<resolutions>
<resolution>1</resolution> # 第0个分辨率
</resolutions>
<resolutions_z>
<resolution>1</resolution>
</resolutions_z>
</map_config>
<map_runtime>
<map_ground_height_offset>0</map_ground_height_offset>
</map_runtime>
<map_record>
<datasets>
<dataset>/apollo/modules/localization/msf/local_map/test_data/ndt_map/map_data</dataset>
</datasets>
</map_record>
</map>
地图路径命名
└── 000 // 分辨率id,resolution是一个数组,000表示第0个resolution
└── north // zone_id > 0 则为orth, 否则为south
└── 10 // zone_id
├── 00032352 // north 方向的计数
│ ├── 00004595 // east 方向的计数
│ ├── 00004596
│ └── 00004597
├── 00032353
│ ├── 00004595
│ ├── 00004596
│ └── 00004597
└── 00032354
├── 00004595
└── 00004596