系列文章目录

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

文章目录

系列文章目录
前言
一、正态分布变换（NDT）定位解决的主要问题
二、正态分布变换（NDT）定位的核心思想
三、NDT算法流程
- 1.多元正态分布概念
- 2.网格化并计算正态分布参数
- 3.变换参数和最大似然
四、使用C++实现NDT配准
总结
参考资料

前言

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

本文先对依赖高精度地图和激光雷达的定位技术–正态分布变换（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

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

基于激光雷达构建高精度点云地图及其感知定位
目前,前期大多数自动驾驶公司采用高精度惯导(imu+gnss+rtk)来实现高精度定位,但存在应用场景缺陷,无法应用隧道.高楼.林阴路等非开阔场景.所以感知定位方式应运而生,感知定位方式大多分为两种: ...
采用Cartographer、LIO-SAM构建三维点云地图，采用Octomap构建八叉树地图（三维栅格地图）
采用Cartographer.LIO-SAM构建三维点云地图,采用Octomap构建八叉树地图(三维栅格地图) 采用Cartographer构建三维点云地图采用的数据集是安装Cartographer ...
基于已知点云地图的NDT的激光SLAM定位
概述对于L4级自动驾驶系统而言,定位模块通常会融合GNSS.IMU.轮速计(车辆底盘).摄像头以及激光雷达odometry多种测量,使用滤波算法(EKF.UKF等)以获得平滑.厘米级别的绝对定位,其 ...
matlab读取八叉树,基于八叉树表示的三维栅格地图路径规划系统及方法技术方案...
[技术实现步骤摘要] 基于八叉树表示的三维栅格地图路径规划系统及方法本专利技术涉及地图路径规划技术,具体涉及基于八叉树表示的三维栅格地图路径规划系统及方法. 技术介绍随着各项性能的提高,服务机器人 ...
A-LOAM构建3d点云地图并实时转存二维栅格地图
文章参考 A-LOAM/LOAM/Lego-LOAM/SC_Lego_LOAM实时构建3d点云地图与2d栅格地图(octomap) 1 安装 octomap 可以从 apt 安装或者以功能包的形式安装 ...
ROS2+cartorgrapher+激光雷达建图并保存
目录写在前面修改记录安装cartographer 查看cartographer包名字并安装 cartographer初了解激光雷达底层驱动编译编译驱动时遇到的bug 编译完后文件树配置文 ...
激光雷达建图（基于ROS）及定位数据获取步骤
去年7月,思岚科技推出了业内首款激光建图雷达SLAMTEC Mapper,赢得了市场的高度肯定,它与传统的激光雷达有所不同,内置了SLAM实时地图构建和定位功能,在手持情况下,可直接对外输出高品质地图 ...
几种室内定位方案技术对比,高精度室内定位方案-新导智能
从古至今,人类的方方面面就离不开"定位"技术,从古代远洋航海罗盘,再到现如今每个电子终端都有的GPS,定位技术在我们身边可谓是无处不在.但人类已经演变成生活在钢筋混凝土森林的动物, ...
煤矿，隧道，管廊高精度人员定位_基于UWB技术的TWR一维定位方案介绍
针对工业/汽车.物流仓储.传统制造业.电力行业.医疗保健.高危化工业.隧道/管廊.建筑工地.公检司法的重点安保区域等高精度人员定位需求,SKYLAB推出了基于UWB技术的定位系统.通过在定位区域内部署 ...
基于激光雷达的里程计及3D点云地图中的定位方法
本文转载自公众号@点云PCL,基于激光雷达的里程计及3D点云地图中的定位方法 :https://mp.weixin.qq.com/s/laA1YAPBCpqlzdGi0yb2cQ 论文:LOL: Li ...

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