【依赖高精度点云地图和三维激光雷达的定位方案】正态分布变换（NDT）定位及建图

系列文章目录

提示：这里可以添加系列文章的所有文章的目录，目录需要自己手动添加
TODO:写完再整理

---

前言

认知有限，望大家多多包涵，有什么问题也希望能够与大家多交流，共同成长！

本文先对依赖高精度地图和激光雷达的定位技术–正态分布变换（NDT）定位做个简单的介绍，具体内容后续再更，其他模块可以参考去我其他文章

---

提示：以下是本篇文章正文内容

真正实现无人车的SLAM是非常困难的，作为交通工具，远距离的行驶会使得实时构建地图的偏差变大。所以，在已经拥有高精度地图的前提下去做无人车的定位，是更加现实，简单的。NDT就是一类利用已有的高精度地图和激光雷达实时测量数据实现高精度定位的技术。

一、正态分布变换（NDT）定位解决的主要问题

比较lidar扫描得到的点云和我们的地图点云，其中一个问题在于：lidar扫描得到的点云可能和地图的点云存在细微的区别，这里的偏差可能来自于测量误差，也有可能这个“场景”发生了一下变化（比如说行人，车辆）。NDT配准就是用于解决这些细微的偏差问题。

.
.

二、正态分布变换（NDT）定位的核心思想

NDT并没有比较地图点云和传感器点云中两个点云点与点之间的差距，而是先将参考点云（即高精度地图）转换为多维变量的正态分布，如果变换参数能使得两幅激光数据匹配的很好，那么变换点在参考系中的概率密度将会很大。因此，可以考虑用优化的方法求出使得概率密度之和最大的变换参数，此时两幅激光点云数据将匹配的最好
.
.

三、NDT算法流程

1.多元正态分布概念

我们知道，如果随机变量X满足正态分布（即 X∼N(μ,σ) ）,则其概率密度函数为：

其中的 μ 为正态分布的均值， σ2 为方差，这是对于维度 D=1 的情况而言的。对于多元正态分布而言，其概率密度函数可以表示为

其中 x 就表示均值向量，而 Σ 表示协方差矩阵,我们知道，协方差矩阵对角元素表示的是对应的元素的方差，非对角元素则表示对应的两个元素（行与列）的相关性。

下图表示一个二元的正态分布

.
.

2.网格化并计算正态分布参数

NDT算法的第一步就是将参考点云网格化（对于三维地图来说，即使用一个一个小立方体对整个空间的扫描点进行划分），对于每一个网格（cell）,基于网格内的点计算其概率密度函数（probability density function, PDF）

其中

表示一个网格内所有的扫描点

那么一个网格的概率密度函数则为

使用正态分布来表示原本离散的点云有诸多好处，这种分块的（通过一个个cell）光滑的表示是连续可导的，每一个概率密度函数可以被认为是一个局部表面（local surface）的近似,它不但描述了这个表面在空间中的位置，同时还包含了这个表面的方向和光滑性等信息。下图是一个3D点云及其网格化效果:

上图中立方体的边长为 1 米，其中越明亮的位置表示概率越高。此外，局部表面的方向和光滑性则可以通过协方差矩阵的特征值和特征向量反映出来。我们以三维的概率密度函数为例，如果三个特征值很接近，那么这个正态分布描述的表面是一个球面，如果一个特征值远大于另外两个特征值，则这个正态分布描述的是一条线，如果一个特征值远小于另外两个特征值，则这个正态分布描述的是一个平面

.
.

3.变换参数和最大似然

当使用NDT配准时，目标是找到当前扫描的姿态，使当前扫描的点位于参考扫描（3D地图）表面上的可能性最大化。那么我们需要优化的参数就是对当前扫描的点云的变换（旋转，平移等），我们使用一个变换参数 p→ 来描述。当前扫描为一个点云 X={x1,…,xn},给定扫描点集合 X 和变换参数 p→ ，令空间转换函数 T(p ,xk) 表示使用使用姿态变换 p→ 来移动点x→k ,结合之前的一组状态密度函数（每个网格都有一个PDF），那么最好的变换参数 p→ 应该是最大化似然函数的姿态变换：

那么最大化似然也就相当于最小化负对数似然 −logΘ;

到这里，就到了我们最熟悉的最优化的部分了。现在的任务就是使用优化算法来调整变换参数 p⃗ p→ 来最小化这个负对数似然。NDT算法使用牛顿法进行参数优化。我们不难看出，这里的概率密度函数 f(x⃗ )f(x→) 其实并不要求一定是正态分布，任何能够反映扫描表面的结构信息且对异常扫描点具有鲁棒性的概率密度函数都是可以的。
.
.

四、使用C++实现NDT配准

完整代码：
https://gitee.com/AdamShan/NDT_PCL_demo

.
.

总结

退化场景中无法使用；
不能在地图中去除动态障碍物产生的噪声；
重定位需要给定初始值，【防盗标记–盒子君hzj】最好使用IMU，NDT对角度比较敏感；
无回环检测，大环境建图会发生漂移。

.
.

---

参考资料

https://blog.csdn.net/AdamShan/article/details/79230612

（1）开源自动驾驶系统Autoware提供的Mapping模块

（2）论文与数学推导的理论
https://blog.csdn.net/AdamShan/article/details/79230612
http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.10.7059&rep=rep1&type=pdf

（3）Autoware的建图教程
https://blog.csdn.net/Travis_X/article/details/105455195