apollo localization模块学习

定位

定位

输入

在提供的RTK方法中，有两个输入：

GPS-全球定位系统。
IMU-惯性测量单元。

在所提供的多传感器融合定位方法中，有三个输入：

GPS-全球定位系统。
IMU-惯性测量单元。
激光雷达-光探测与测距传感器

输出

一个Protobuf message类型的LocalizationEstimate实例，它可以在localization/proto/localization.proto中找到。

├── 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$ 表示，地图中和当前帧点云反射率。 $\sigma_m, \sigma_z$ 表示反射率标准差。
在这里插入图片描述

其中， $a_m, a_z$ 表示，地图中和当前帧点云的高度值。

似然计算如下：在这里插入图片描述

方差计算放如下：
在这里插入图片描述

权重 $\gamma$ 计算放如下：
在这里插入图片描述

区域 $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