激光SLAM里程计算法总结（ICP、NDT与运动畸变）

MIKingZCC

已于 2022-07-15 15:12:50 修改

阅读量2.8k

点赞数 1

文章标签：自动驾驶 slam 激光雷达人工智能传感器融合

于 2022-07-13 17:06:26 首次发布

本文链接：https://blog.csdn.net/weixin_44156680/article/details/125743529

版权

本文总结了激光里程计中的关键配准算法，包括ICP、PL-ICP和NDT。ICP是一种迭代最近点算法，但可能陷入局部最优；PL-ICP以点到线误差为目标，精度更高但对初始值敏感；NDT假设点云服从正态分布，寻找最佳姿态匹配。此外，还讨论了点云运动畸变及其校正方法，指出配准时需要一定重叠度并考虑传感器运动导致的坐标差异。

摘要由CSDN通过智能技术生成

在导航系统中，里程计（Odometry）数据用来估算机器人位置随时间的改变量；

一、ICP配准

ICP （Iterative Closest Point）迭代最近点算法；
在这里插入图片描述

PCL点云库中ICP方法的使用：

setMaximumIterations，最大迭代次数，icp是一个迭代的方法，最多迭代这些次；
setEuclideanFitnessEpsilon，设置收敛条件是均方误差和小于阈值，停止迭代；
setTransformtionEpsilon, 设置两次变化矩阵之间的差值（一般设置为1e-10即可）；
setMaxCorrespondenaceDistance,设置对应点对之间的最大距离（此值对配准结果影响较大）。

（1）ICP 方法需要配准的两个点云具有一定的重叠度；
（2）ICP 选取的所有点进行配准，速度较慢；
（3）易收到噪声干扰，陷入局部最优；

二、PL-ICP配准

在这里插入图片描述
PL-ICP是二阶收敛，ICP是一阶收敛
PL-ICP对初始值更敏感
以点到线的误差为目标函数，PL-ICP的求解精度更高

在这里插入图片描述

三、NDT配准

正态分布变换 (Normal Distribution Transform)
与ICP不同, NDT 假设点云服从正态分布，我们的目的是找一个姿态，使得当前扫描点位于扫描参考平面的可能性最大。
在这里插入图片描述

最后概率密度函数可以表示为：

目标函数为：
$s(\vec{p})=\sum_{k=1}^{n}\tilde{p}(T(\vec{p},\vec{x}_k))$

PCL点云库中NDT方法的使用：
（1）NDT 方法需要配准的两个点云具有一定的重叠度；
（2）NDT 允许两个点云存在少量差异；
在这里插入图片描述

四、点云的运动畸变

点云运动畸变产生的原因是一帧点云中的点不是在同一时刻采集的，在采集过程中，雷达随着载体在运动，但是雷达点测量的是物体和雷达之间的距离，所以不同激光点的坐标系就不一样了，会导致周围物体产生重影。
在这里插入图片描述
对于地面点而言，未去畸变时，表现成完整的圆（下图中左边未去畸变，右边为去畸变）：

点云去畸变基本方法：
在工程中为了减少运动前后方向的畸变，通常在安装时使y轴朝向载体的左右两侧； $(2)$ 中提到的相对位姿变换 $(R,T)^t_{t+1}$ 可以通过IMU、RTK以及轮速计获得。