点击上方“3D视觉工坊”，选择“星标”

干货第一时间送达

来源丨计算机视觉SLAM

作者丨Realcat

标题：Normal Distributions Transform Monte-Carlo Localization (NDT-MCL)

作者：Jari Saarinen, Henrik Andreasson, Todor Stoyanov and Achim J. Lilienthal

来源：IROS 2013

编译：黄玉玺

审核：lionheart

摘要

NDT算法与MCL算法（Monte-Carlo Localization

，也是我们常说的粒子滤波定位算法）是激光定位中两种经典算法，本文将两种算法结合起来，算法上使用NDT地图代替常用的概率栅格图，配合粒子滤波实现了NDT- MCL定位算法。并且通过实验验证， 与Grid-MCL相比NDT-MCL在定位的精度、效率以及平滑性上都表现更好。

下面首先简单回顾下，NDT与Grid-MCL定位算法的基本原理（相关论文与源码链接会放在文末）。

NDT算法

NDT算法（2d版本）最早在2003年由图宾根大学的Peter Biber提出^[2]；2009年由瑞典厄勒布鲁大学的Martin Magnusson扩展至3d并且将推导过程完整的发表在了他的博士论文中^[3]。PCL中NDT源码的实现参考的就是这篇论文。

图1 NDT（3d）地图示意图

NDT算法主要有以下几步：

1、地图网格生成。将地图点云按照固定的分辨率划分到不同的三维网格中；

2、计算每个网格点云的均值与协方差。这样就生成了NDT地图；

3、投影与近邻关系搜索。根据初始位姿将待匹配点云投影到NDT地图中，并搜索得到与每个匹配点最近的地图网格；

4、计算代价值。根据每个待匹配点及其搜索到的地图网格，计算代价值，并且使用非线性优化方法计算出位姿增量以供迭代计算优化；NDT论文^[3]核心公式如下，为节省篇幅不做赘述，感兴趣可以看论文。

5、不断迭代，直到满足条件退出迭代，得到最优位姿；

Grid-MCL算法

Grid-MCL算法是基于2d栅格图的粒子滤波定位算法的一个统称,其中经典的实现算法是ros中开源的amcl算法[5]，amcl算法是单线激光常用的定位算法之一。Grid-MCL算法通常有以下几步：

1、建立栅格地图。这里的栅格地图是一种概率地图，栅格图中每个网格通常有三个状态：占用、空旷、未知。其中，每个状态会根据机器人在建图时激光观测的次数等信息计算出不同的概率，用于后面计算每个粒子定位的概率。在工程实践中，建图与定位通常会分开使用不同的算法，ros中常用的经典的建图算法有：gmapping[5],cartographer[6]等。

2、粒子群初始化。即根据先验位姿信息、概率分布模型生成对应的粒子；

3、计算最优粒子与权重。每个粒子与地图匹配，得出最优粒子，即最优位姿。每个粒子都可以计算出概率（即得分）与权重。相当于计算代价函数的代价值，得分最高的即为最优粒子，计算公式[1]如下：

4、重采样。根据上时刻匹配结果、机器人的运动状态，生成新的粒子群。

5、重定位。粒子群与地图匹配，得出最优粒子。

NDT-MCL算法

该算法结合了以上两种算法的特点：

1、使用NDT特征作为定位地图，代替栅格地图；

2、使用粒子滤波定位，代替NDT迭代优化匹配；

代价函数如下^[1]：

可以看到该公式与ndt的代价函数如出一辙。每个粒子的权重计算如下：

从公式可以看出，此刻权重与上一时刻权重，代价成正相关。

NDT-MCL实验结果

为了测试Grip-MCL与NDT-MCL效果，作者分别设计了两种实验场景：离线测试（offline accuracy test）、闭环测试（closed-loop tests）。离线测试是指控制机器人移动时，输入控制器的定位信息来自外部定位系统（也是本实验的真值），而Grid-MCL/NDT-MCL的定位结果只是记录下来，并不影响机器人的控制器。而闭环测试是指将MCL/NDT-MCL的定位信息输入到机器人控制器中，来观测其对机器移动状态的影响（并不是slam中常说的闭环匹配）。论文中实验较多，为了简洁，选几个代表性的结果：

图4 离线测试示意图

1、精度方面。如图5所示，NDT-MCL定位误差小于5cm明显优于Grid-MCL（误差最小也是5.4cm），并且NDT-MCL算法受地图分辨率的影响小于Grid-MCL。

图5 轨迹精度受地图分辨率的影响示意图

2、鲁棒性。作者在离线实验中加入了动态物体（8个移动的人）的干扰（如图4）。

图6不同环境对算法影响示意图

如图6所示，虽然NDT-MCL在不同环境下测试精度仍然优于Grid-MCL，但是可以明显看出NDT-MCL在动态环境下，定位产生了1-2cm的误差。

3、效率。如图7所示，NDT-MCL平均运行耗时低与Grid-MCL，并且NDT-MCL随着地图分辨率的降低，耗时也随之降低，然而Grid-MCL则变化不明显。

图7 运行时间对比

4、平滑性。图8为闭环测试中，轨迹平滑性的测试，从中可以看出，使用NDT-MCL定位控制机器人时，机器人航向调整的次数比Grid-MCL定位时低3.2%，反应出NDT-MCL机器人运动的平滑性更好，侧面反应出轨迹平滑性更好。

图8 轨迹平滑性测试示意图（其中Reflector指外部定位系统，即真值。E：与真值比，机器人调整角度次数占比，越低代表调整测试越少，轨迹跟踪越好，机器人运动越平滑）

总结

本文创新地将NDT与MCL结合起来提出了一种以NDT地图+粒子滤波的定位方法，并且经过验证NDT-MCL在精度、效率、平滑性上均优于Grid-MCL,缺点是在动态环境下NDT-MCL受影响略大，另外，本文也提到并没有做定位初始化。

小编觉得在2d环境下做三自由度定位时，该方法值得一试，但是在3d环境下做6自由度定位时，要实现精确的定位定姿，需要的粒子数会很多，计算量会是问题，需要做针对性优化才能使用。

论文与源码链接

1、Normal Distributions Transform Monte-Carlo Localization(NDT-MCL)：http://vigir.missouri.edu/~gdesouza/Research/Conference_CDs/IEEE_IROS_2013/media/files/0907.pdf

2、The Normal Distributions Transform: A New Approach to Laser Scan Matching：http://www.researchgate.net/profile/Peter_Biber/publication/4045903_The_normal_distributions_transform_a_new_approach_to_laser_scan_matching/links/00b7d51726745ca71c000000.pdf

3、The Three-Dimensional Normal-Distributions Transform：https://www.diva-portal.org/smash/get/diva2:276162/FULLTEXT02.pdf

4、amcl源码地址：https://github.com/ros-planning/navigation/tree/noetic-devel/amcl

5、gmapping源码地址：https://github.com/ros-perception/slam_gmapping

6、cartographer源码地址: https://github.com/cartographer-project/cartographer_ros

Abstract

Abstract— Industrial applications often impose hard requirements on the precision of autonomous vehicle systems. As aconsequence industrial Automatically Guided Vehicle (AGV)systems still use high-cost infrastructure based positioningsolutions. In this paper we propose a map based localization method that fulfills the requirements on precision andrepeatability, typical for industrial application scenarios. Theproposed method - Normal Distributions Transform MonteCarlo Localization (NDT-MCL) is based on a well establishedprobabilistic framework. In a novel contribution, we formulatethe MCL localization approach using the Normal DistributionsTransform (NDT) as an underlying representation for bothmap and sensor data. By relaxing the hard discretization assumption imposed by grid-map models and utilizing the piecewise continuous NDT representation the proposed algorithmachieves substantially improved accuracy and repeatability.The proposed NDT-MCL algorithm is evaluated using offlinedata sets from both a laboratory and a real-world industrialenvironments. Additionally, we report a comparison of theproposed algorithm to grid-based MCL and to a commerciallocalization system when used in a closed-loop with the controlsystem of an AGV platform. In all tests the proposed algorithmis demonstrated to provide performance superior to that ofstandard grid-based MCL and comparable to the performanceof the commercial infrastructure based positioning system.

本文仅做学术分享，如有侵权，请联系删文。

3D视觉精品课程推荐：

1.面向自动驾驶领域的多传感器数据融合技术

2.面向自动驾驶领域的3D点云目标检测全栈学习路线！(单模态+多模态/数据+代码)
3.彻底搞透视觉三维重建：原理剖析、代码讲解、及优化改进
4.国内首个面向工业级实战的点云处理课程
5.激光-视觉-IMU-GPS融合SLAM算法梳理和代码讲解
6.彻底搞懂视觉-惯性SLAM：基于VINS-Fusion正式开课啦
7.彻底搞懂基于LOAM框架的3D激光SLAM: 源码剖析到算法优化
8.彻底剖析室内、室外激光SLAM关键算法原理、代码和实战(cartographer+LOAM +LIO-SAM)

9.从零搭建一套结构光3D重建系统[理论+源码+实践]

10.单目深度估计方法：算法梳理与代码实现

11.自动驾驶中的深度学习模型部署实战

12.相机模型与标定(单目+双目+鱼眼）

重磅！3DCVer-学术论文写作投稿 交流群已成立

扫码添加小助手微信，可申请加入3D视觉工坊-学术论文写作与投稿 微信交流群，旨在交流顶会、顶刊、SCI、EI等写作与投稿事宜。

同时也可申请加入我们的细分方向交流群，目前主要有3D视觉、CV&深度学习、SLAM、三维重建、点云后处理、自动驾驶、多传感器融合、CV入门、三维测量、VR/AR、3D人脸识别、医疗影像、缺陷检测、行人重识别、目标跟踪、视觉产品落地、视觉竞赛、车牌识别、硬件选型、学术交流、求职交流、ORB-SLAM系列源码交流、深度估计等微信群。

一定要备注：研究方向+学校/公司+昵称，例如：”3D视觉 + 上海交大 + 静静“。请按照格式备注，可快速被通过且邀请进群。原创投稿也请联系。

▲长按加微信群或投稿

▲长按关注公众号

3D视觉从入门到精通知识星球：针对3D视觉领域的视频课程（三维重建系列、三维点云系列、结构光系列、手眼标定、相机标定、激光/视觉SLAM、自动驾驶等）、知识点汇总、入门进阶学习路线、最新paper分享、疑问解答五个方面进行深耕，更有各类大厂的算法工程人员进行技术指导。与此同时，星球将联合知名企业发布3D视觉相关算法开发岗位以及项目对接信息，打造成集技术与就业为一体的铁杆粉丝聚集区，近4000星球成员为创造更好的AI世界共同进步，知识星球入口：

学习3D视觉核心技术，扫描查看介绍，3天内无条件退款

圈里有高质量教程资料、可答疑解惑、助你高效解决问题

觉得有用，麻烦给个赞和在看~