一、摘要内容：

固态激光雷达

优点：价格低、稳定性高、可行性强。

缺点：视场小，一帧点云中的特征少

Livox_mid-40特点：

1.不规则扫描模式

2.非重复扫描（为了最大化扫描范围）

3.由于单个激光头的连续扫描，当激光雷达在不断移动的过程中在不同的时间内被采样，帧内的运动会扭曲点云，导致运动模糊。

本文贡献：

1.为小视场激光雷达开发了一个完整的LOAM算法

2.提高了精确性和鲁棒性

3.提出了一种运动补偿方法

二、主要内容：

点的选择和特征提取：

A:点的选择

计算每个3D点P=的以下特征[x，y，z]，其中x−Y−Z轴对应于激光雷达的前左向上（FLU），见下图

图6：其中变量分别表示：(a):θ是入射角，φ是偏转角。(b):

其中R为目标反射率，由激光雷达测得。而在其他的一些激光雷达中，返回的是强度而不是目标反射率。但是在这个例子中，强度时可以直接获取的，强度低表示该点远离传感器或者物体物体的反射率低。

入射角θ是激光放置与测量点附近局部平面的夹角：

图5：为工作流程的概述。每个新帧直接与全局映射直接匹配，以提供测程输出。匹配结果依次用于将帧注册到全局地图中，导致相同的测程输出和地图更新速率（即20 Hz）。在我们的实现中，只有特征点（即边缘点和平面点）被保存在内存中，所有的原始点都被保存在硬盘中，以便进行可能的离线处理（例如，离线全局优化）。

为了提高定位和反射的精度，下面情况的点将被去除：

1.靠近视场边缘的点。（例如，用于LivoxMID40的φ(P)≥17◦）。在这一区域内，扫描轨迹具有较大的曲率，导致第三节。b节的特征提取可靠性较差。

2.强度过大或过小的点（例如I (P)≤7×10−3，或I (P)≥1×10−1forMID40）。这是因为强度直接表示所接收到的激光信号的强度。过大的强度（信号）通常会导致接收电路的饱和或失真，并降低测距精 度。另一方面，强度（信号）过小通常会导致信号噪声比较低，这也会降低测距精度。

3.入射角接近π或0的点（例如θ(P)≤5◦，或MID40的θ(P)≥175◦），如图6 (a).中的点Pf这是因为由激光束的非零散度角引起的激光光斑将被大大拉长。因此，测量的范围是被大点所覆盖的面积的平均值， 而不是一个特定的点。

4.隐藏在对象后面的点（例如，图6(a)中的Pe），否则会导致假边缘特征。Pe是隐藏点，其中，Pd为扫描顺序中最近的测量点：

B：特征提取

在选择点后，我们进行特征提取，从“好点”中提取特征。我们通过计算候选点的局部光滑度来提取平面和边缘特征，如在[12]中所述。此外，为了减轻有限视场和点选择导致的特征数量少而导致的匹配退化，我们采用激光雷达反射率作为四维测量。如果一个三维点的反射率与邻域点有很大的不同，我们将其作为边缘特征点（边缘的反射率由于材料的变化，而几何形状的边缘由于形状的变化）。这些点在一些退化的情况下是有益的，比如面对一堵门窗紧闭的墙。

迭代位姿优化

由于章节中提到的非重复扫描。我，被提取的特征不能像在[12,13,19]中那样被不断跟踪。一个简单的例子是，即使激光雷达是静态的，扫描的轨迹（和特征点）也与前一帧不同。在我们的工作中，我们使用一个迭代姿态优化过程来计算激光雷达姿态。通过稍后详细介绍的适当实现，我们实现了实时测程法和频率为20Hz的反射。

A：边到边的残差

定义Ek和Em分别为当前帧和已经构建地图中边缘特征的集合，对于Ek中的每个点，我们找离 中最近的5个点，还是和LOAM一样。利用kd-树储存。一旦接收到最后一个注册的帧/子帧，kd树将由另一个并行线程构建（见下图）。这使得kd树在接收到新帧时立即可用。

图7：为多核CPU的并行范例。每个子帧在一个专用线程上与全局映射独立匹配。然后，将匹配的子帧注册到全局地图中，并成为地图的一部分。另一个专用线程接收新的注册子帧，并构建一个KD3的更新地图，以便在下一帧中使用。

假设Pl为当前帧Ek(即第k帧）中的一个点。注意到Pl在局部激光点云帧中，而Ek在全局点云中（已经生成的地图），为了找到nearest points ofPl inEm，我们需要利用一下转换方式将其转换到全局映射。

其中（Rk，Tk）为当前帧中最后一点被采样时的激光雷达姿态（旋转和平移），需要通过姿态优化来确定。在这里，我们在一帧的最后一点上使用激光雷达姿态来表示整个帧的姿态，并且该帧中的所有点都使用这个姿态投影到全局地图上。还要注意，当前帧中的最后一点本质上是下一帧中的第一个点。

设Pi 表示Em的Pw的第i个最近点。为了确保Pi 确实在一条直线上，我们计算了由Pw的m个最近点形成的平均µ和协方差矩阵Σ。我们在工作中将m设为5。如果Σ的最大特征值是第二大特征值的三倍，我们保证Pw的最近点形成了一条Pw应该所在的线。然后将残差计算为（图6 (b)）。

B：面到面的残差

与边缘特征点相似，对于当前帧的平面特征集Pk中的一个点，我们在地图的平面特征集Pm中找到了5个最近的点（见图6(c)）。我们还通过计算它们的协方差矩阵Σ，保证了这5个最近的点确实在同一平面内。如果Σ的最小特征值是第二最小特征值的三倍，我们计算当前帧中的平面点到同一平面上的5个点所形成的平面的距离，如下，并将此残差加到姿态优化中。

在寻找残差中，利用了主成分分析方法（PCA)。目前对于6、7公式不是很懂，主要其中的变量不大懂，像公式6，p1和p5分别表示离pw最近的点和最远的点？

C：帧内运动补偿

如前所述，随着激光雷达运动的不断进行，三维点在不同的姿态（即运动模糊）进行采样。为了消除运动模糊的影响，提出了以下两种方法：

1)分段处理：分段处理是消除运动模糊影响的一种简单而有效的方法。我们将一个传入的帧分成三个连续的子帧。然后将这三个子帧与迄今为止独立积累的相同映射进行匹配，在扫描每个子帧期间，它的所有点都使用该子帧终点的激光雷达姿态投影到全局地图上。通过这样做，每个子帧的时间间隔为原始帧的1/3。虽然这个方法看起来非常简单，但如结果所示，它工作得非常好。此外，这种分段处理的好处是通过并行化每个子帧的匹配来利用现代cpu中的多核结构（见上图7）。

2)线性插值：另一种常用的运动补偿方法是线性插值，如[12,19]。表示（Rk，Tk）当前帧最后一点的激光雷达姿态和（Rk−1，Tk−1）前一帧，（Rkk−1，Tkk−1）前一帧和当前帧之间的相对旋转和平移，然后：

假设tk−1是前一帧中最后一个点的采样时间。对于当前帧中在t时刻采样的任何点，我们都有t∈[tk−1，tk]。计算=（t−tk−1）/（tk−tk−1），则=时刻的线性插值姿态为：

其中θ为大小，ω为Rkk−1旋转轴的单位向量。是ω的斜对称矩阵。从罗德里格的公式中，我们有：

这意味着当前帧的每个点只需要计算sin（sθ）和cos（sθ），而其余部分保持不变。这节省了一些计算。利用，当前时间的激光雷达姿态为：

然后，我们可以通过插值姿态将时间t处的点投影到全局映射中，如下所示：

图8：不同运动补偿方法的比较。第一列显示没有任何运动补偿的结果，第二列是线性插值，第三 列是分段处理。第四列中上面的图片是我们的手持式数据收集设备，下面的图片是映射区域的RGB图像。

D：异常值去除，动态目标过滤

为了避免移动对象在现实环境中降低扫描匹配的准确性，我们执行一个动态对象过滤如下：每次迭代姿态优化，我们重新找到每个特征点的最近的邻居和添加边缘残差(6)和平面残差(7)目标函数，我们首先执行姿态优化与少量的利用优化结果，我们计算了(6)和(7)中的两个残差，并去除前20%的最大残差。去除离群值后，最终进行整体姿态优化、迭代。在算法1中总结了完全迭代位姿优化算法，如下：

该伪代码描述了一个迭代式的 LiDAR 位姿优化算法，主要用于在当前帧中优化 LiDAR 的位姿估计，从而提高 LiDAR 定位的精度。

算法的输入包括当前帧的边缘特征集合 Ek 和平面特征集合 Pk，以及地图中的边缘特征集合 Em 和平面特征集合 Pm，以及上一帧的 LiDAR 位姿 Rk-1 和 Tk-1。算法的输出是当前帧的 LiDAR 位姿 Rk 和 Tk。

首先将当前帧的位姿初始化为上一帧的位姿，然后通过一个迭代过程逐步优化该位姿。迭代过程中，对于当前帧中的每一个边缘特征 Pl，算法会通过特征 Pl 计算出其在全局坐标系下的位置 Pw，并在地图中查找距离 Pw 最近的前 5 个点 {P1∼5}。如果这些点在地图中确实是一条直线，则添加边对边的残差项

，以进行优化。对于当前帧中的每一个平面特征 Pl，算法同样会计算出其在全局坐标系下的位置 Pw，并在地图中查找距离 Pw 最近的前 5 个点 {P1∼5}。如果这些点在地图中确实是一个平面，则添加平面对平面的残差项，以进行优化。

在添加完所有残差项之后，算法将执行两次非线性优化，并重新计算残差项，然后删除最大残差的 20%。如果在迭代的最大次数内，非线性优化收敛，则算法结束，返回当前帧的 LiDAR 位姿。

三、结果：

A：建图分析

两种运动补偿方法的比较如图8所示，我们可以看到，在没有任何运动补偿的情况下（图8 (a)），映射在局部区域（如楼梯、栏杆）中非常模糊，在更大范围内扭曲（如建筑弯曲）。而在运动补偿下，线性插值和分段处理都有效地消除了运动模糊，楼梯台阶和栏杆可以相互区分。然而，线性插值具有相当大的长期漂移，如图8 (b).上图中的曲线建筑所示这是因为数据是由手持设备收集的，而运动可能相当不稳定，不能通过简单的线性插值准确地捕获。

B：测程分析

我们通过与GPS的测量结果进行比较，来评估我们的算法的定位，如图9所示。我们计算了我们的测程仪的两个位置的距离，然后将其与GPS的测量值进行比较。在两个数据集上的定位结果分别为0.41%和0.65%，说明其具有较高的定位精度。此外，我们通过将我们的结果与图10中所示的运动捕捉系统（mocap）进行比较，来评估旋转的准确性)。结果表明，我们的测程法和模帽的轨迹非常接近，欧拉角在三个方向上的平均误差均低至1.1°。

C：算法执行表现分析

效率更高

四、结论：

本文提出了一种小视场激光雷达的测程和映射算法。该算法继承了标准LOAM算法的基本结构和技术（如特征提取、匹配、线性插值运动补偿），但也有几个关键的新贡献，如点选择、迭代姿态优化和实现并行化。所开发的算法有其测程和映射都实时运行（即20 Hz）。序列扫描匹配在映射和定位方面具有较高的精度，但本质上存在漂移。利用循环闭合和滑动窗口优化等技术来减少这种漂移将在未来得到进一步的研究。