LidarSLAM（一）：NDT

NDT的提出与原理：

ICP配准方法前提是环境大部分是不变的，但是完全不变的环境其实也是很少的，比如一辆车飞驰而过，一个人走过等。更多应该考虑的是允许小部分差异的配准，这时候点对点匹配比如ICP就会出现一些问题，而NDT则可以很好地解决细微差别，因为NDT是通过近似正态分布概率来完成匹配的（概率吗就具有了兼容性）。主要步骤为：

一个三维空间的正态分布如图所示：

简单理解就是将每一个划分出的网格近似为一个专属的概率分布然后与地图对应网格内的分布求解相似程度（这就是匹配的过程）。

找到了一张很好的示意图：

对比两种概率的分布，常用方法是利用贝叶斯分布，即：求解后验概率的问题可以转变为最大化先验*似然估计的问题，先验是相同的所以也就等价于最大化似然估计，为了计算方便可等价于最小化负对数似然估计，然后对此式进行牛顿法迭代优化。只要涉及到就牛顿法优化一定会涉及到初值的给定，所以理论上NDT程序中会有一个超参数调整优化初值，该超参数也会影响定位精度。

另外一个问题是，点云配准问题是一个非凸优化问题，NDT和ICP算法的本质是局部搜索，通过对目标函数的迭代求解求得一个初值附近的局部最优解，而正确的空间转换可以理解成对目标函数的全局最优解。但目标函数是非凸的，在整个参数空间上有许多局部最优解，如果初值不在全局最优解附近，就可能收敛于其他次优解，导致找不到正确的转换结果。换句话说点云对于局部特征的描述较好，但对于全局特征的描述能力表现较弱,所以对点云定位的第一步——定位初始化要求较高。通过当前点云帧和全局地图匹配进而得到当前位置不太可能实现，也就是说初始化一定是要选择正确对应的各自或者相邻的格子才会运行成功，所以在实际应用中会有一个“令初始化成功的范围”。

TODOs：

NDT-GPU库：https://github.com/zju-sclab/NDT-library

有一个超参数可调节hdl准确率与匹配速度，我们可以进一步提升准确率并由GPU加速。或许就不必额外使用EKF，也就不必给定协方差矩阵（协方差矩阵是个很不容易求解的东西）。

以下是一个机器人中应用的方案举例

基于NDT的三维点云匹配定位方案

基于三维激光雷达进行定位的主流方法主要有两种：一是直接使用SLAM（即时定位与地图构建）技术；二是将SLAM中的“L”与“M”进行解耦，使用单独的二维或三维定位方法替换其中的“L”部分。一般来说在实际工程应用中后者的定位精度较高故采用后者解耦方式完成定位任务。单独的二维定位方法中比较有代表性的是自适应蒙特卡洛定位方法(adaptive Monte Carlo Localization，AMCL)，将三维地图投影到二维预估会丧失很多本就稀疏的信息，故而计划直接采用单独的三维定位方法。这其中比较有代表性或者说实际工程中常用方法为NDT（normal distribution transformation）方法。NDT算法对环境进行概率建模更符合实际情况，其对匹配初值要求低，能容忍一定程度的环境变化且匹配速度较快。

NDT的实现：hdl_localization

NDT算法的定位精度相对较高，但从零实现过程中需要考虑初始值选择、正态分布超参数选择等问题。hdl_localization是用于使用三维激光雷达（例如velodyne的 HDL32e或者VLP16）进行实时3D本地化的ROS软件包，由日本丰桥工业大学有源智能系统实验室进行维护。hdl_localization以NDT匹配为主体，配合Unscented Kalman滤波器进行姿势估计，稳定性较高。本方案计划基于hdl_localization程序包进行优化改进并用于爬壁机器人定位任务。

hdl_localization定位方案

工程结构：主要分为两个并行的流程。流程1：维护地图服务节点；维护定位节点。

流程2：维护状态估计节点；维护UKF的实现节点

初始位姿的给定：仿真环境中验证算法时初始位姿可通过rviz中的 “2D Pose Estimate” 来给定。实体机器人运行时可直接初始化时设定参数，但需注意比如爬壁机器人工作表面垂直于地面，基于机器人坐标系进行工作时则不需修改。而基于地面坐标系（如任务需求装配地面喷涂工具）进行定位工作则需要调整对应的初始化坐标参照。

全局匹配地图的给定：由基于三维激光雷达建图的算法（如LOAM算法或者Cartoprapher算法）生成Pcd格式的全局地图。地图服务模块初始化时读取全局地图并进行降采样操作然后发布到全局定位话题，定位节点收到后将该地图设置为NDT匹配的目标。

IMU数据的处理：IMU数据为可选项，如存在IMU数据可将其通过状态估计节点直接存入内存。本方案中计划使用IMU，通过卡尔曼滤波将IMU数据与三维激光雷达数据融合进行定位可有效提升定位精度，此外仍需注意IMU的坐标参考系。使用前应单独对IMU的安装调试进行评估，确保IMU与建图算法参考坐标系一致。

使用NDT对全局地图进行离散：定位节点接收全局点云后使用NDT将空间点云离散为数个立方体，这样就可以将采样的点云划分到不同的网格中，可以很方便的描述点云的局部特性，例如点云局部的形状（直线、平面或者球体）、方向（平面法向、直线方向等）。

实时激光数据的处理：对于每一帧点云，都会进入状态估计节点。节点内进行点云转PCL并降采样，同时检测有无IMU数据，如有IMU数据则遍历积累的所有IMU数据，读取IMU的加速度与角速度并校正。

NDT匹配：将处理后的实时激光点云投票到各个划分网格，之后计算格子的正态分布PDF参数，与离散后的全局点云各个网格中的PDF参数通过遍历进行匹配以确定机器人所属位置。

方案输入输出强调

输入：

3D Lidar的实时点云信息

由上一级系统即建图算法生成的pcd格式的地图，作为NDT匹配的参照

初始化参数，包括初始位姿，下采样的比例参数以及匹配速度等级

可选IMU数据

输出：

进行NDT匹配后单独的位姿话题，该话题作为其他多传感器融合算法（如robot_localization）的输入,也可单独利用用以评价NDT匹配效果

以move_base导航算法为例，需要定位算法提供TF信息，hdl_localization程序包也预留了相应接口。

备注

NDT-OMP是一种加速版的NDT算法，计算速度比pcl原始版本快10倍，主要是对ndt中计算梯度以及hessian矩阵进行了多线程优化，hdl_localization中应用这个版本的NDT。

友情链接：

http://xchu.net/2019/09/14/NDT%E5%8E%9F%E7%90%86%E6%8E%A8%E5%AF%BC/#more