apollo localization模块学习

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定位模块很简单,主要流程如下:
apollo localization模块学习

主要将gps以及imu中的msg填充到Localization中相应的位置。

ndt

ndt定位算法流程如下:
apollo localization模块学习

主要的流程在黄色步骤,主要进行ndt算法匹配。几个主要的函数实现如下:

LidarLocatorNdt::Update

具体流程如下:

  1. 组合导航之间的位置差,作为预测位置
  2. 根据当前预测位置加载周边地图
  3. 对当前帧地图进行滤波
  4. 获取点云地图,并将点云数据转到激光坐标系
  5. 进行NDT匹配
  6. 获取NDT匹配结果。

LidarLocatorNdt::ComposeMapCells

apollo localization模块学习

具体流程如下:

  1. 计算9个块的start end坐标,存入map_nodes_zones
  2. 遍历每一个块的区域,计算每一个区域的mean以及icov_,数据存到cell_map_中。

BaseMap::LoadMapArea

流程比较简单:

  1. 构建9块的map_ids
  2. 根据map_ids的值,导入所有的map_ids.LoadMapNodes(&map_ids)

NDT步骤

NDT算法的基本思想是先根据参考数据(reference scan)来构建多维变量的正态分布,如果变换参数能使得两幅激光数据匹配的很好,那么变换点在参考系中的概率密度将会很大。因此,可以考虑用优化的方法求出使得概率密度之和最大的变换参数,此时两幅激光点云数据将匹配的最好。
apollo localization模块学习
apollo localization模块学习

msf

msf定位算法流程如下:
apollo localization模块学习

主要的流程在黄色部分,主要进行多传感器融合。

apollo localization模块学习

其中Lidar Localization算法流程如下:
apollo localization模块学习

主要采用LK算法进行heading的计算,采用直方图滤波算法计算(x, y)。
apollo localization模块学习

其中, r m , r z r_m, r_z rm​,rz​表示,地图中和当前帧点云反射率。 σ m , σ z \sigma_m, \sigma_z σm​,σz​表示反射率标准差。
apollo localization模块学习

其中, a m , a z a_m, a_z am​,az​表示,地图中和当前帧点云的高度值。

似然计算如下:apollo localization模块学习

方差计算放如下:
apollo localization模块学习

权重 γ \gamma γ计算放如下:
apollo localization模块学习

区域 Z Z Z中最佳的offset计算如下:
apollo localization模块学习

协方差矩阵如下:
apollo localization模块学习

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

上一篇:Graph Convolutional Networks for Temporal Action Localization


下一篇:2-Entity RANSAC for robust visual localization in changing environment论文笔记