主要贡献

基于Loam算法适配小FoV固态激光雷达
扫描匹配的鲁棒外点剔除
场景动态物体过滤
激光雷达运动畸变补偿(采用分段法，比插值法有效)

一.固态激光雷达的特点

以大疆投资的子公司生产的Livox MID40 LiDar（https://www.livoxtech.com/mid-40-and-mid-100）为例来说明。

1. 小FoV

固态雷达MID-40: 38.4度，特征更少，匹配性能下降，更容易受运动物体干扰
传感机械雷达VLP-16: 360度

2. 不规律的扫描模式

固态激光雷达: 固态激光雷达是基于单激光束的连续扫描，图案是累积的，不规律的，MID-40是玫瑰状的扫描图案，相邻的两次扫描几乎完全不一样。
传统机械雷达: 激光发射接收器在一行垂直排列，旋转的时候进行扫描，结果作为一组平行的环状扫描。角点可以通过对环形线上的点深度值插值得到，可以方便的进行特征提取。

3. 非重复扫描

为了达到尽可能大的扫描覆盖率，激光点的连续扫描位置是不会与之前的轨迹重复的。

4. 运动模糊

由于是基于单激光头的连续扫描，因此在同一帧里扫描的3D点是在不同的时刻采样的。而与此同时，帧内的雷达运动会使得点云发生畸变，从而引起运动模糊。

二. 特征点提取和筛选

在这里插入图片描述
系统整体描述见上，前端部分主要由特征点提取和筛选组成。特征点基于雷达的属性(比如激光斑大小，信噪比)将有效点定义为"good points"。

１. 3D点的筛选

在这里插入图片描述
计算激光雷达坐标系( $X - Y - Z$ 前左上，FLU)下对应3D点 $p = [x, y, z]$ 的特征，具体为:

点的深度D
$sprt(x^2+y^2+z^2) \tag{1}$
激光偏转角 $\phi$ , 为X轴与激光束之间的夹角
$\phi(P) = tan^{-1}(sqrt((y^2+z^2)/x^2)) \tag{2}$
点的强度 $I$ ，R是雷达返回的的反射率/强度，强度越低表示点越远或者物体的反射率越低
$I(P)=R/D(P)^2 \tag{3}$
入射角 $\theta$ (incident angle), 为激光束和测量点附近平面的夹角
$\theta(P_{b})=cos^{-1}\frac{(P_a-Pc)\cdot P_b}{|P_a-Pc|\cdot|Pb|} \tag{4}$
为了提高定位和建图精度，将以下点剔除:

剔除在FoV边缘位置的点(MID-40中偏转角 $\phi$ >17度)。　这些位置的扫描轨迹曲率较大，影响特征提取可靠性；
剔除强度过大或者过小的点( $I<=7\times10^{-3}, or I >=1\times10^{-1}$ )。信号强度太大容易导致接受端电路饱和或者受到干扰，导致探测精度下降。信号太弱则一般信噪比过低，也会探测精度也会下降。
剔除入射角在 $\pi$ 和0附近的点( $\theta<=5°,or, \theta >=175°$ )。在这些位置的点容易使得激光斑发生延长，探测距离是覆盖区域的平均值，因此导致探测精度下降。
剔除隐藏在物体后面的点。这容易引起错误的边缘特征检测。基于下面公式判断点 $P_e$ 是否是隐藏点, $P_d$ 为激光扫描顺序上距离最近的另一个测量点：
$P_e-P_d|>=0.1|P_e|, and |P_e|>|Pd|$

2. 特征提取

基于以上筛选出的"good points"进行特征提取，主要和Loam中的方法类似，通过计算候选点的局部平滑度来提取平面点特征和边缘点特征。由于使用的固态激光雷达FoV较小，因此引入雷达的反射率作为第4维度的的测量信息。如果一个3D点的反射率与相邻的点不同，将其也作为边缘点考虑(形状发生变化时会存在边缘点，同样材料属性不同也会因为反射率不同出现边缘点)。这在面对同时存在门和窗户的墙面等类似场景时可以起到积极的有益作用。

三. 迭代姿态优化

由于固态激光雷达的非重复扫描特性，提取的特征不能像传统Loam算法中那样进行正常跟踪。比如，即使激光雷达是静止不动的，则当前帧也与上一帧的扫描图案和特征点不一致。本文使用一种迭代姿态优化(Iterative pose optimization)方法来计算雷达的pose, 经过适当的优化，可以实现20Hz实时的里程计和建图。

1.边缘点残差(Residual of edge-to-edge)

$\xi_k$ 表示当前帧中所有边缘点特征的集合， $\xi_m$ 表示地图中所有边缘点特征的集合。对 $\xi_k$ 中的每个点，在 $\xi_m$ 中查找距离最近的5个点。为了加快查找速度，将 $\xi_m$ 建立为一个kd树，而且kd树是当上一帧数据接收到并匹配成功后通过另外一个并行线程进行建立的。这能够保证当新的一帧数据到来时kd树能够立刻可用。
当前帧中的特征点是在当前帧坐标系(k-th frame, local Lidia frame)，地图点特征是在global坐标系下。为了在地图点中找到当前帧中特征点 $P_l$ 的最近点，则需要将当前帧中的点投影到global坐标系下:
$P_w = R_kP_l+T_k \tag{5}$
其中， $R_k, T_k)$ 分别表示当前帧中上一个点被采样时LIDAR的pose, 由pose优化步骤决定。此处使用当前帧中的上一个特征点时刻的雷达pose作为整帧的pose，将此帧的所有特征点都投影到global地图中。需要注意的是，当前帧中的最后一个特征点也是下一帧的第一个特征点。这样位姿的先验信息存在时间序和空间序上的连续性。
$P_i$ 表示在地图特征点集合中查找到的最近的第i个特征点。为了保证几个 $P_i$ 在同一条线上，计算这几个点位置的均值 $\mu$ 和协方差矩阵 $\Sigma$ 。文中查找的最近点数量为5。如果协方差矩阵的最大特征值比第二大特征值的3倍还要大，则认为这些查找到的最近点可以组成一条线，而且 $P_w$ 应该也在这条线上。 接着，边缘特征点的残差可以计算(面积法):
$r_{e2e} = \frac{|(P_w-P_5)\times(P_w-P_1)|}{|P_5-P_1|} \tag{6}$

2. 平面点的残差(Residual of plane-to-plane)

与上述边缘点特征类似，同样基于当前帧中的平面点特征在地图中的平面点中查找对应最近的5个点。同样为了确保这些点在同一个平面上，需要计算这些点的协方差矩阵 $\Sigma$ 。如果协方差矩阵的最小特征值的３倍比第二小特征值还要小，则计算当前帧中的此平面点到地图中这5个点组成的平面的距离。如下所示，此距离也是当前帧平面点特征的残差(体积法):
$r_{p2p} = \frac{(P_w-P_1)^T\cdot((P_w-P_1)\times(P_3-P_1))}{|(P_3-P_5)\times(P_3-P_1)|} \tag{7}$

3. 帧内运动补偿

因为扫描的点是在雷达运动过程中不同的时间对应不同的pose下采样得到的，因此会存在运动模糊情况。一般有两种方法处理运动模糊:
在这里插入图片描述
1) 分段处理：作者将完整的一帧分为3个子帧序列，将这3个子帧分别与当时累积的相同的地图进行匹配。每一个子帧的时间间隔是原始帧的1/3。尽管方法比较简单，但是作者经过实验验证可用性极强，尤其是可以使用多核处理器并行处理多个子帧的时候。
2) 线性插值: 原始Loam版本中就使用的线性插值方法。主要是根据上一帧的pose，当前帧的pose, 基于时间间隔进行T和R的插值。

4. 外点剔除，动态物体过滤

首先进行小次数迭代(文中为2)。每次pose优化时，都重新查找地图中的最近点，并重新计算边缘点误差(6)和平面点误差(7)，然后将其增加到目标函数中（此时目标函数包含两次查找邻近点增加的残差）；
小次数迭代优化结束后，计算公式(6)和公式(7)中的每组误差，并移除最大的20%的残差（外点）；
最后进行完整的pose迭代优化，直到到达设置的迭代次数最大值或者目标函数收敛到一定的阈值，则pose优化结束。
具体过程见下:

四. 结果

在这里插入图片描述

1. 建图评估

由图8中的两种运动补偿方法可见，不用任何运动补偿时，在楼梯和栏杆处会比较模糊，而且空间尺度较大时会发生轨迹畸变。分段法和线性插值法均可有效处理运动模糊的情况。但是线性插值法无法比较好的解决长时间的漂移问题，这是因为数据采集时是手持设备进行采集，运动比较剧烈，简单的线性插值模型不能很好地进行运动预测。