新知一下
海量新知
5 9 7 5 1 3 9

NDT-MCL定位算法论文解读

3D视觉工坊 | 个人微信公众号:3D视觉工坊。 2021/11/25 10:21

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源码的实现参考的就是这篇论文。

新知达人, NDT-MCL定位算法论文解读

图1 NDT(3d)地图示意图

NDT算法主要有以下几步:

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

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

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

4、计算代价值。 根据每个待匹配点及其搜索到的地图网格,计算代价值,并且使用非线性优化方法计算出位姿增量以供迭代计算优化;NDT论文

[3]

核心公式如下,为节省篇幅不做赘述,感兴趣可以看论文。

新知达人, NDT-MCL定位算法论文解读

新知达人, NDT-MCL定位算法论文解读

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

Grid-MCL算法

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

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

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

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

新知达人, NDT-MCL定位算法论文解读

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

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

NDT-MCL算法

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

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

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

代价函数如下

[1]

新知达人, NDT-MCL定位算法论文解读

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

新知达人, NDT-MCL定位算法论文解读

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

NDT-MCL实验结果


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

新知达人, NDT-MCL定位算法论文解读

图4 离线测试示意图

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

新知达人, NDT-MCL定位算法论文解读

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

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

新知达人, NDT-MCL定位算法论文解读

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

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

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

新知达人, NDT-MCL定位算法论文解读

图7 运行时间对比

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

新知达人, NDT-MCL定位算法论文解读

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

总结

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

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


更多“算法”相关内容

更多“算法”相关内容

新知精选

更多新知精选