【Autoware1.14建图与定位】NDT-Mapping与N

2021-03-31 本文已影响0人 ClarkWang1214

一、ndt mapping源码解析

0）基础知识：正态分布变换算法NDT

NDT，Normal Distributions Transform正态分布变换算法的简称，其是一种统计学模型。如果一组随机向量满足正态分布，那么它的概率密度函数为：
$p(x)=\frac{1}{(2\pi)^{\frac{D}{2}} \sqrt{\Sigma}} e^{-\frac{(x-\vec \mu)^T \Sigma^{-1} (x-\vec \mu)}{2 \sigma^2}}$

其中D表示维度， $\vec \mu$ 表示均值向量， $\Sigma$ 表示随机向量的协方差矩阵。由于扫描得到的激光点云数据点是三维空间点坐标，所以需要采用三维正态分布。NDT能够通过概率的形式描述点云的分部情况，这有利于减少配准所需要的时间。

正态分布变换NDT算法的流程

1）网格化目标点云
首先需要将目标点云网格化grid。点云配准一般是对两个点云数据进行两两配准，需要先固定一个点云数据，另外一个点云数据再通过旋转平移来和固定点云进行匹配拼接。这里的固定点云就是目标点云，平移旋转的点云就是源点云。网格化目标点云主要是利用立方体将激光点云所在空间进行均匀划分，使得激光点处于相应的立方体中，这一步作为NDT算法的第一步非常重要。
2）注册激光点云数据
目标点云划分网格后，将点云加载到网格内，首先计算均值向量
$\vec \mu = \frac{1}{m} \sum_{k=1}^{m} \vec y_k$
其中 $\vec y_{k=1,2,3,...,m}$ 表示网格中所有扫描点的坐标。
然后，计算协方差矩阵：
$\Sigma \frac{1}{m} \sum_{k=1}^m (\vec y_k-\vec \mu)(\vec y_k - \vec \mu)^T$
最后，求出每个网格内的正态分布概率密度函数：
$p(\vec x) = \frac{1}{2^{\frac{3}{2}}\sqrt{|\Sigma|}} e^{-\frac{(\vec x - \vec \mu)^T \Sigma^{-1}((\vec x - \vec \mu))}{2}}$
由于需要用到协方差矩阵的逆矩阵，所以每个格子中激光点不可少于三个，一般至少要保证有五个点。
3）求出源点云相对于目标点云的初始坐标变换参数
坐标变换通常涉及到平移与旋转，平移通过平移向量表示，旋转则可以通过旋转矩阵表示，旋转是关于自身zyx三个固定轴的旋转，转角分别用 $\alpha$ ， $\beta$ ， $\gamma$ 表示，分别与yaw，pitch，roll对应。

z	y	x
yaw	pitch	roll
$\alpha$	$\beta$	$\gamma$

旋转矩阵R可以表示为： $R_{3 \times 3} = [\delta, \epsilon,\eta]$ 。
其中
$\delta=\left[ \begin{matrix} cos \beta cos \alpha \\ sin \alpha cos \beta \\ -sin \beta \end{matrix} \right]$
$\epsilon=\left[ \begin{matrix} cos \alpha sin \beta sin \gamma - sin \alpha cos \gamma \\ cos \alpha cos \gamma + sin \alpha sin \beta sin \gamma\\ cos \beta sin \gamma \end{matrix} \right]$
$\eta=\left[ \begin{matrix} cos \alpha sin \beta cos \gamma + sin \alpha sin \gamma \\ sin \alpha sin \beta cos \gamma - cos \alpha sin \gamma\\ cos \gamma cos \beta \end{matrix} \right]$

平移向量t为：
$t = [t_x,t_y,t_z]^T$

假设某一个激光点在两个点云坐标系下的位置坐标分别为：
$X = \left[ \begin{matrix} x_i \\ y_i \\ z_i \end{matrix} \right]$
$X' = \left[ \begin{matrix} x'_i \\ y'_i \\ z'_i \end{matrix} \right]$

那么通过坐标变换公式：
$X' = R_{3 \times 3} X + t_{3 \times 1} = TX$
可以得到同一个激光点云在这两个点云坐标系下的位置坐标之间的关系，其中T表示坐标变换参数变换。

这一步是为了寻找一个合适的初始坐标变换使得源点云大致处于目标点云的坐标平面当中。这一步提供的变换参数的初值，为下一步变换参数的迭代提供距离最优点较近的初值。在自动驾驶里初始值的提供可以依靠GNSS、Odom或者IMU惯性导航，利用这些传感器获取车辆的当前位姿，通过坐标变换得到相对于目标点云的坐标变换参数，也就会位移t与旋转R。单纯依靠里程计或者IMU会产生累计误差，Autoware利用里程计计算速度确定位置，利用IMU确定姿态的变化。

4）源点云进行初始坐标变换，并计算在目标点云网格中的概率
源点云根据初始变换参数将坐标转换到目标点云中，此时源点云会分布在目标点云网格中，然后转换后的源点云的坐标 $X'$ ，由对应所在的网格的正态分布概率密度函数，求出激光点坐标为 $X'$ 的概率。
5）求出最大目标似然函数
将每个点的概率乘积起来作为目标似然函数。通过似然函数找到概率乘积最大时候的坐标转换关系。简单来说就是由最大的概率找到最优的坐标变换。
$\psi = \prod_{k=1}^n p(X')$
此时需要将最大似然函数做一个变换，最优化问题涉及求解一阶导数和二阶导数，乘积的形式不容易求导，因此将它转换为负对数的形式，就能转换成累加的形式，方便求导的操作了。
$- \log \psi = - \sum_{k=1}^n \log (p(X'))$
6）利用高斯牛顿法进行优化，找出最佳变换参数p完成点云配准
然后利用高斯牛顿法进行求解优化目标函数 $- \log \psi = - \sum_{k=1}^n \log (p(X'))$ 的核心是求解 $H\Delta\vec p = -\vec g$ ，其中H是目标函数的海森矩阵，g是目标函数的梯度向量。迭代更新公式为 $T = T + \Delta T$ ，其迭代参数为坐标变换矩阵T

1）Autoware中的定位模块

Autoware的定位模块分为基于GNSS定位以及基于激光雷达点云数据定位两个部分。

而基于激光点云数据的定位采用NDT Matching配准算法得到自身在激光点云地图中的位姿信息，其中激光点云地图由车载激光雷达前期扫描行车环境得到的激光点云数据压缩拼接而成（NDT Mapping）。

NDT Matching配准的时候，需要手动设置初始位置，因此Autoware需要借助GNSS定位对其进行初始化，同时基于激光点云数据的定位结果进行修正。

1）点云配准原理

在激光点云地图建图过程中，由于激光雷达扫描距离存在限制，一次扫描难以获取完整的目标环境，并且距离激光雷达越远，点云就会变得越稀疏，所以需要经过连续多次扫描，然后将每次扫描的点云数据进行配准拼接，最终才能形成连续完整的激光点云地图。从不同角度扫描同一场景所得到的的点云数据统一转换到同一坐标系的过程叫做点云配准。简单地说就是将离散的点云数据在统一的坐标系下拼接成一整个完整的点云数据。通常点云配准算法能够利用两个点集之间的最小距离或者利用统计学方法，得到两个点集之间的变换关系，使得点云达到变换配准的效果，问题关键在于如何得到激光点云之间的坐标变换矩阵T，也就是得到旋转矩阵R以及平移向量t。通常可以直接利用PCL开源点云库来对相关点云数据进行处理，PCL中包含了基于NDT正态分布变换等多种点云配准算法。

ndt_mapping主要利用的是scan_to_map的方式实现激光点云建图，该方法经常使用在SLAM同时定位与建图中，当激光雷达进行扫描建图的时候，由于受到扫描距离等因素的限制，使得激光雷达不能一次扫描得到完整的环境地图，因此需要进行连续多次的扫描。

最终扫描得到的整个环境地图就是全局地图map，而其中scan表示当前扫描得到的激光点云数据，可以通过固定目标点云地图submap，然后利用NDT配准算法，将每一帧扫描得到的激光点云数据scan变换到目标点云submap中，并使得两者拼接在一起，最终得到拼接完整的全局地图map。

具体的步骤如下：
1.激光雷达扫描得到的激光点云数据需要去除距离车体较近与较远的激光点集，然后利用体素滤波过滤剩下的激光点云数据，在保持点云统计特征的情况下，降低激光点云数据集的尺寸大小，最好将降采样后的过滤点云作为NDT配准算法的输入源点云。

2.第一帧激光点云作为全局地图加载成功，将其作为NDT配准的输入目标点云。

3.为了快速得到准确的配准结果，需要给NDT算法提供良好的初始值，该节点通过IMU、Odom以及两者联合来求得初始位姿估计。

4.将前三者得到的结果作为参数传入到NDT配准算法中进行激光点云配准。

5.最后将1中过滤得到的点云数据变换到全局地图map中，进行地图更新。

2）源码解析

根据图形化界面，找到Autoware源码中Computing界面对应的配置文件computing.yaml，位置在autoware\src\autoware\utilities\runtime_manager\scripts
打开computing.yaml文件，可以看到清晰的autoware图形化界面的布局以及控件对应执行的指令。
根据computing.yaml中的内容，当勾选了ndt_mapping进行建图的时候，会去cmd运行lidar_localizer中的ndt_mapping.launch文件。

ndt_mapping.launch文件如下：

<!-- -->
<launch>
  <!-- pcl_generic=0, pcl_anh=1, pcl_anh_gpu=2, pcl_openmp=3 --> 
  <arg name="method_type" default="0" /> <!-- 默认使用CPU -->
  <arg name="use_odom" default="false" /> <!-- 默认不使用里程计 -->
  <arg name="use_imu" default="false" /> <!-- 默认不使用IMU -->
  <arg name="imu_upside_down" default="false" /> <!-- 默认不进行IMU方向正负变换 -->
  <arg name="imu_topic" default="/imu_raw" />
  <arg name="incremental_voxel_update" default="false" />

  <!-- rosrun lidar_localizer ndt_mapping  -->
  <node pkg="lidar_localizer" type="queue_counter" name="queue_counter" output="screen"/>
  <node pkg="lidar_localizer" type="ndt_mapping" name="ndt_mapping" output="screen">
    <param name="method_type" value="$(arg method_type)" />
    <param name="use_imu" value="$(arg use_imu)" />
    <param name="use_odom" value="$(arg use_odom)" />
    <param name="imu_upside_down" value="$(arg imu_upside_down)" />
    <param name="imu_topic" value="$(arg imu_topic)" />
    <param name="incremental_voxel_update" value="$(arg incremental_voxel_update)" />
  </node>

</launch>

根据

二、ndt mapping园区实战

1）启动autoware

source ~/Projects/Autoware-All/install/setup.bash
roslaunch runtime_manager runtime_manager.launch

2）切换到Simulation界面

加载rosbag，从零开始不循环播放，进度到1%的时候，点击Pause暂停

3）切换到Setup界面

直接点击Vehicle Model加载黑色的车辆模型，然后点击TF，加载从车身底盘坐标系到激光雷达坐标系之间的坐标转换关系，tf_x，tf_y，tf_z是两者之间的相对位姿关系。

4）切换到Computing界面

点击ndt_mapping的app，设置ndt 的配准参数，暂时可以使用其默认值即可
设置完毕参数后，勾选ndt_mapping

5）回到Simulation界面

点击Pause，取消暂停，继续播放ROSBAG数据包，开始NDT 配准建图，直到结束为止，可以在终端看到建图的过程：

也可以在rviz中看到实时建图的过程（当ROSBAG数据包太大时候，打开rviz可视化会非常的卡，fps基本为0，卡的动不了，实际路测的时候，可以关闭）

等待进度条走到100%ROSBAG数据包播放完毕

6）保存全局地图成pcd文件

当ndt_mapping建图完毕后，切回到computing界面，再次点击ndt_mapping右侧的app按钮，在下方输入要保存建好的全局地图的pcd文件的路径，然后点击PCD OUTPUT完成点云地图的保存。

ndt matching点云定位

在经过ndt mapping建图过程后，得到了rosbag包的完整全局点云地图pcd文件，于是可以通过ndt matching进行点云定位

1）再次播放ROSBAG数据包

切换界面到Simulation，从头开始Play播放数据包，1%进度后按暂停

2）加载车辆模型以及车身地盘坐标系到激光雷达坐标系的变换关系tf

在Setup界面点击Baselink to Localizer下的TF按钮，可以使用其默认值，来设置车身底盘坐标系到激光雷达传感器坐标系之间的相对位姿关系。并且点击Vehicle Model加载车辆模型。

3）加载世界坐标系到全局地图坐标系之间的变换关系tf_local

首先在本地某处新建一个tf_local.launch文件，然后在其中写入如下内容用于表示车身世界坐标系到全局地图的位姿变换关系（一般世界坐标系world与地图坐标系是重合的）：

<launch>

    <node pkg="tf" type="static_transform_publisher" name="world_to_map" args="0 0 0 0 0 0 /world /map 10" />

</launch>

仍然在Map界面的下方，点击 TF右侧ref，加载刚才新建的tf_local.launch文件

4）加载全局地图

切换界面到Map界面，点击Point Cloud的Ref按钮，加载刚才构建好的点云地图pcd文件，点击Point Cloud按钮后，知道进度条变为100%，右侧出现OK表示加载完毕

5）设置体素滤波进行点云降采样（核心思想是将网格voxel内的点云数据用其质心来近似表达）

切换到Sensing界面，点击Points Downsampler中的voxel_grid_filter右侧的app设置体素滤波的参数（可使用默认的参数值，voxel_leaf_size表示立方体网格的边长，measurement表示点云的有效距离）

6）进行ndt_matching点云配准定位

切换到Computing界面，点击Lidar_localizer下的ndt_matching右侧的app对点云配准进行参数设置，勾选Initial Pose（默认是单选在GNSS），来设置激光点云定位前车辆的初始位置，假设存在GNSS数据，则选择GNSS，采用GNSS作为初始位置数据输入。确定后，勾选ndt_matching按钮。