文章探讨了一种基于3D雷达的LOAM（LidarOdometryandMapping）方法，指出了在无后端优化和闭环检测时建图存在的误差，主要表现为与GNSS真值的偏差和地图重影。通过evo工具评估，显示出显著的定位误差。代码架构包括四个主要节点，涉及点云处理、里程计构建、地图构建和误差评估。特征点提取通过曲率判断，而位姿估计采用线性补偿来处理运动畸变。由于缺乏闭环检测，导致长期漂移，强调了闭环检测在SLAM中的重要性。
程序效果
红色为laser_odomerty，绿色为gnss的真值，从效果上看，二者偏差较大，且构建的地图重影现象严重，反映出在只用3D雷达，没有后端优化和闭环检测时建图存在较大误差。具体误差可用evo评价工具进行测评。
在trajectory目录下输入
evo_rpe kitti ground_truth.txt laser_odom.txt -r trans_part --delta 100 --plot --plot_mode xyz
输入evo_ape kitti ground_truth.txt laser_odom.txt -r full --plot --plot_mode xyz
代码架构
该节代码是基于A_LOAM源码进行构建的，在运行时用rqt查看node_graph如下，主要分为四个node文件：
scan_registration_node.cpp//对激光雷达输出的点云数据进行过滤，并将点云数据分类发布
alom_laser_odometry_node.cpp//订阅分类好的特征点云，通过帧到帧之间的位姿关系构建laser_odometry
alom_mapping_node.cpp//订阅帧到帧的里程计信息，将特征点云分类进行存储，构建submap，将当前帧与submap进行匹配，优化odometry信息
evaluation_node.cpp//订阅kitti的gnss数据，处理后发布，作比较
LOAM论文阅读
特征提取
针对于两个旋转轴组成的3D激光雷达，实现实时性构建里程计与建图的方法。一般来说，激光雷达每旋转一周得到的点云称为一帧（scan），数据是一帧一帧进行读取的。而在每一帧中，先把点云按线数的顺序进行先后排列，下文的特征点提取就是针对于单个线而言。
经典的匹配方法，如ICP、NDT是基于直接匹配的方法，就是对于原始的点云，不进行特殊处理，直接进行匹配，来得到scan to scan的相对位姿。而LOAM是先对点云进行特征点提取，再利用少量的特征点进行匹配。
LOAM中定义了两种特征点，即corner_sharp与surface_flat（针对于室内环境特征较为明显），在这些特征上的点称为特征点。在LOAM中，引入曲率c的概念来判定1个点是否为特征点，如下式，就是在计算平均点与中间点X的差距。其中Xi是X相邻的几个点，程序中选取的是前后各5个点，除以||X||是考虑到不同距离下得到激光点的疏密程度不一样，为了归一化。（这里可以改进吗？）
如图所示，红线表示曲率的大小，论文中将曲率大的称为corne_sharp(角点)，曲率小的称为surface_flat（平面点）。
如图所示，红色点为corner_sharp，蓝色点为surface_flat，二者为在点云数据中提取出的特征点。绿色为点云数据。
在程序中，对特征点进行细化，除了角点和面点，还有曲率不那么大的点（corner_less_sharp）和曲率不那么小的点（surface_less_flat）
这样的特征提取方法意味着适用于结构化比较好的室内环境，因为室内环境充满了墙壁、地面平直的转折角。对于室外的环境就没这么理想了，可能充满了树叶、石块、车辆等不规则的物体。
位姿估计
LOAM通过线性补偿的方法解决了点云畸变问题。运动畸变产生的原因是：激光雷达在扫描过程中是运动的，如果激光雷达的扫描频率很高，比自身的运动快得多，我们假设运动畸变很小可以忽略。但是大多数雷达的频率都不是很高，假设激光雷达扫描频率为10HZ，机器人的运动速度为1m/s，那么在一帧的扫描过程中，机器人已经移动了10cm，这时就不能忽略了。
LOAM根据每个点的相对时间进行线性补偿，前提是假设机器人在匀速运动，也就是说在一次扫描的过程中，线速度和角速度是不变的（针对于剧烈运动，应用IMU进行补偿）。假设第k帧点云表示为
，将投影到
时刻（第k帧结束时刻），去除点云畸变的影响(代码中是按照点云的接收时间进行线性变换)，投影后的点云表示为
。将和第k+1帧点云
一起用于雷达的运动估计。为了快速搜索，将存储在3D KD-tree中（代码中是将corner_point和flat_point分别存储的）。
将时刻相对于
时刻的激光雷达相对位姿记为
，对于每个特征点i计算它的补偿变换矩阵
，如下式所示：
用变换矩阵对特征点i进行变换即可完成补偿。
经过投影转换的将和第k+1帧点云一起用于雷达的运动估计。对当前帧点云（k+1帧）进行特征点提取，分别将conner_point和flat_point的集合记为
和
。在每次迭代中，通过当前的估计位姿把和投影到第k+1帧开始时刻的位置，分别记为
和
。
如图所示，对于和中每一个点i，都会在中搜索距离其最近的点j，然后按一定的方法取剩余点（两点构成一条线，不在同一直线上的三点构成一个平面）。（代码中直接搜索KD-tree中临近的五个点，构造线和平面）。然后通过以下两个公式分别求解i到构造线\面的距离作为残差，通过最小化残差来recover雷达的运动估计。构建最小二乘问题来求解帧与帧之间的位姿，采用优化目标：
（代码中使用ceres库来构建优化问题）。
点到直线的距离
点到面的距离
在代码中，两次投影转换中，都是采用的这种方式，即transform_to_start()和transform_to_end()两个函数。先对corner_sharp和surface_flat两类特征点进行transform_to_start()，进行匹配，再进行位姿优化，再将所有点云数据transform_to_end()，为下次位姿匹配做准备。
建图
LOAM缺少回环检测模块，建图的目的是为了纠正漂移误差（还是进行状态估计），每扫描一帧就对地图进行一次更新（位姿估计的频率呢？laser_odomerty）。
laser_odometry中可以理解为scan_to_scan的位姿匹配，mapping_odometry可理解为scan_to_map的位姿匹配。将去畸变后的当前帧（k+1帧）与之前累积的map做匹配，目的是优化
，然后将当前帧转换到世界坐标系下，更新地图。
在寻找特征点的corresponse_point方面与laser_odometry环节不同。如下图所示，把点云数据按一定的比例尺存放在一定大小的cube中（有很多的cube），构成map。在做与地图做匹配的过程中，不会将地图的所有数据都拿来做匹配，而是先根据laser_odomerty的估计位姿将当前帧变换到世界坐标系下，而其附近的cube构成了一个submap，匹配过程正是将submap与当前帧做匹配。
找线或面的方式与laser_odometry中有所不同。把submap存储到KD-tree中，然后搜索其临近的点（surrounding_points），计算其协方差矩阵及其特征值与特征向量。对于corner_point对应的edge_line特征，其协方差矩阵必定有一个特征值远远大于其他特征值，该特征值对应的特征向量的方向就是edge_line的方向；对于flat_point对应的flat特征，则会有两个较大的特征值，其对应的两个特征向量就组成了该平面。由此可求出点到线/面的距离来进行位姿的进一步优化（PCA主成分分析方法）。在代码中，实际上用最小二乘法来拟合平面。协方差矩阵的特征值与特征向量可以表示点云的分布，其原理如下图：
总结
A_LOAM没有闭环检测环节，使用的传感器仅仅只有一个3D激光雷达。整个代码是双线程执行的，一边是较快频率的laser_odometry，一边是较慢频率的mapping_odometry，mapping_odometry是对位姿进行进一步优化，每隔一段时间，消除一下漂移，但是效果并不是很好。因为在该框架中，已经转化为map存储的点云不会随着后面的优化环节作出改变，即优化只是单次的，所以全局的地图偏差会较大。根据kitti数据集的测试效果来看，当车辆回到之前走过的点时,laser_odometry_path和ground_truth会有较大的误差。更加印证了闭环检测对于SLAM的重要性。
注：主要是为了学习使用，所以引用了很多其他博客的图解与文字，在此列出参考。
参考：
LOAM: Lidar Odometry and Mapping in Real-time
扫一扫
1057
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
