大疆出品，必属精品。

固态激光雷达里程计的工作现阶段还是比较少的，大疆自己出了固态激光雷达后在LOAM的基础上改进了一个适用于固态激光雷达的里程计，该系统可以用在小视场角和非重复性扫描的雷达上。作者已经开源了代码https://github.com/hku-mars/loam_livox

需要知道的介绍

需要知道的是（文章最后有相关的技术手册）:

固态激光雷达的视角很小，Livox MID40的视角仅有38.4度。但是想要获得大的视角可以使用多个固态的激光雷达（比如MID-100其实就是3个MID-40）。
扫描图案不均匀，原来的激光雷达规则扫描，极大的简化了特征提取和匹配。但是固态激光雷达扫描不均匀Livox MID40扫描的图案类似花瓣，相邻的两次扫描距离很远。
非重复扫描，这样可以最大化覆盖率（尽可能多的得到场景中的信息）。
运动畸变，同一帧的点云扫描有先后顺序的，由于运动可能导致同一帧的点云出现运动畸变。

为了解决上述的问题，作者创建了Loam_Livox工具包。解决了在小的视角下进行特征选择和提取的任务，有严格的外点剔除策略，移动的对象滤波和运动畸变补偿。在没有诸如IMU、相机、GPS等传感器的基础上，利用点云和局部地图的匹配鲁棒的实时计算雷达的位姿。

特征选择和提取

系统概述如下图所示，其前端处理包括点的选取和特征的提取。考虑到激光雷达传感器的low-level物理特性(如激光光斑大小、信噪比)的测量机制，我们进行点级选择来提取“优点”。

#### A.点的选择

计算点距离雷达的距离 D ( P ) = x 2 + y 2 + z 2 D(P)=\sqrt{x^2+y^2+z^2} D(P)=x2+y2+z2
计算光束和X轴的夹角 ϕ ( P ) = tan ⁡ − 1 ( ( y 2 + z 2 ) / x 2 ) \phi(P)=\tan^{-1}(\sqrt {(y^2+z^2)/x^2}) ϕ(P)=tan−1((y2+z2)/x2 )
计算反射率 I ( P ) = R / D ( P ) 2 I(P)=R/D(P)^2 I(P)=R/D(P)2，其中R是点的反射强度

当反射率I小的时候证明物体离雷达很远，反射强度也很小。
θ = ( ( P a − p c ) × P b ∣ P a − p c ∣ × ∣ P b ∣ ) \theta = \left( \frac {\left(P_a - p_c \right) \times P_b}{|P_a - p_c\rvert \times |P_b|} \right) θ=(∣Pa−pc∣×∣Pb∣(Pa−pc)×Pb)
为了提升定位和建图的精度,我们需要删除以下的这些点:

接近FoV边缘的点。（例如φ（P）≥17◦对于Livox MID40)在这样的区域，扫描轨迹曲率大，导致特征提取不太可靠。
强度太大或者太小的点
点的入射角大于0度或180度.
删除物体后边点

#### B.特征检测

选择点后，我们执行特征提取以从“好点”中提取特征。我们通过计算候选点的局部平滑度来提取平面和边缘特征，如[1]所示。此外，为了减轻由于有限的FoV和点选择导致的少量特征而导致的匹配退化，我们将LiDAR反射率用作第4维测量。如果3D点的反射率与其相邻点有很大不同，我们将其视为边缘特征点（由于材料变化而导致反射率的边缘，与由于形状变化而导致几何形状中的边缘相反）。在某些退化情况下，例如面对带有关闭的门窗的墙壁，这些点是有益的。

迭代的位姿优化

第一节提到的非重复扫描导致提取的特征无法像[12、13、19]中那样不断跟踪。一个简单的例子是，即使LiDAR是静态的，扫描的轨迹（和特征点）也不同于前一帧。在我们的工作中，我们使用迭代的方法进行姿态优化来计算LiDAR位姿。通过稍后详细介绍的适当实现，我们可以实现20Hz的实时里程计和建图。

A.边和边的残差

ε k \varepsilon_k εk和 ε m \varepsilon_m εm是当前帧和地图点的边缘特征集合，对于当前帧 ε k \varepsilon_k εk中的每一个点，我们在 ε m \varepsilon_m εm中找五个和它最近的点。为了提高搜索的速度，我们把 ε m \varepsilon_m εm构建成KD-Tree。需要注意的是KD-tree是在另外一个线程中构建的（当最后一帧点云对齐后），详细的见下图所示。

P l P_l Pl是当前帧边特征中的点，为了在地图 ε m \varepsilon_m εm找到和点 P l P_l Pl最近的点，我们需要把这个点利用当前位姿投影到 ε m \varepsilon_m εm中。利用最后一个点的接受的到后雷达的位姿作为当前帧的位姿，然后把所有的点都投影到全局地图中。 P i P_i Pi是地图 ε m \varepsilon_m εm中距离 P w P_w Pw第i近的点，为了保证 P i P_i Pi是线特征，为了找距离 P w P_w Pw最近的五个点，计算五个点的平均值和协方差，如果协方差矩阵中最大的特征值大于第二大的特征值的三倍，我们确定这五个点组成了一个直线。残差可以计算为:

其实就是点到线的距离，分子的叉乘计算的是三个点组成的三角形的面积，分母是底的长度。所以计算的就是三角形的高即点到线的距离。

B.面和面的残差

与边缘特征点相似，对于平面中的点当前帧的特征集Pk，我们在地图的平面特征集Pm中找到5个最近的点，同时通过计算它们的协方差矩阵Σ来确保这5的点确实在同一平面内，如果最小Σ的特征值比第二小的特征值小三倍，我们计算当前帧的地图中平面点到由这5个点组成的平面的距离，然后将此残差添加到姿势优化中。

分子后边的叉乘部分还是地图三个点组成的三角形的面积，再点乘一得到四面体的体积，再除以底面积得到四面体的高即点到平面的距离。

图像帧运动补偿

由于激光采集的点云并不是在同一时刻采集的，所以就会存在运动畸变（坐标系不同引起的），所以需要根据接收激光点的时间计算位姿把点云投影到同一坐标系。为了补偿每次扫描的时间和位姿不同，我们可以利用：

分段线性：把一个新来的帧分成三个连续的子帧，然后把这三个帧独立的和现阶段构建的地图做匹配。在每个子图做scan-match的过程中利用子图中最后一个点的位姿把所有的点投影到全局地图中，这样每帧采样的时间仅为原来的三分之一。尽管这种方法很简单，但是效果很好（LEGO—LOAM中把一帧分为6份应该也是分段线性的思想），分段线性对于多核的CPU并行运算也有好处。
线性插值：这部分在张籍博士的LAOM中介绍的比较详细，大家在看的时候可以参考。主要思想是在当前帧最后一个激光点得到的雷达位姿[R_k,T_k]和次新帧的[R_k-1,T_k-1]中间的时刻t，利用恒速模型计算中间时刻点的位姿。具体来说：

这里需要计算得到两帧的位姿变化量 R k − 1 k R^k_{k-1} Rk−1k及 T k − 1 k T_{k-1}^k Tk−1k：

很容易知道每个点采集的时间（自己计算或者激光雷达采集的时候该信息是可保存的），已经知道了上一帧最后一个点的采集时间t_k-1和当前帧最后一个点采集的时间是t_k，在这中间的当前帧的点采集的时间是t，令s=(t-t_k-1)/(t_k-t_k-1)，所以t时刻的位姿是:

其中θ是幅值，ω是 R k − 1 k R^k_{k-1} Rk−1k的旋转向量，加一个上尖表示反对称矩阵，利用罗德里格斯公式可以完成旋转向量和旋转矩阵的转换：

其中每个点的sin(sθ)和cos(sθ)都需要计算，最后当前点的位姿为：

最后就可以把t时刻的点投影到全局地图中：

最后论文给出的建图的效果如下所示：

算法一：雷达位姿优化

输入：当前帧获得的边的集合 ε k \varepsilon_k εk和平面的集合 P k P_k Pk；从地图获得的边的集合 ε m \varepsilon_m εm和平面的集合 P m P_m Pm；上一帧的雷达位姿 [ R k − 1 , T k − 1 ] [R_{k-1},T_{k-1}] [Rk−1,Tk−1]
输出:[ R k , T k R_k,T_k Rk,Tk]

（1）计算线和线的误差
（2）计算面和面的误差
（3）利用两个特征迭代的优化位姿
（4）删除残差中最大的20%
（5）在迭代中如果位姿收敛就break

D.外点和运动物体剔除

在实际跑的时候避免不了会有动态的车、人及其他物体。本文的动态物体剔除算法是在每一次迭代优化位姿的过程中重新找到每个特征的最近点把边边残差和面面残差加入到目标函数，第一次只优化两次，然后把残差中最大的20%丢弃来达到剔除外点的目的。

精度

室内和室外的定位精度分别是0.41%和0.65%（直接收尾点和GPS比，这个精度没什么可以参考的），在实际测试的过程中发现固态激光雷达在手持的状态下只要晃动的剧烈一点就会丢失，而且由于该激光雷达的特性（非重复性扫描），这个系统不能在快速运动的系统中使用。但是拿视觉或者激光里程计给他提供位姿，在低速的场景中单纯拿他建图效果还是很棒的。最后跑官方数据集的效果如下所示：

实验室的师弟也在实际场景中做了测试，建图效果如下所示：

最后给上调研的时候查到的一些资料：
固态激光雷达的优势：
①其结构简单，尺寸小，无需旋转部件，在结构和尺寸上可以大大压缩，提高使用寿命并使其成本降低。
②扫描精度高，光学相控阵的扫描精度取决于控制电信号的精度，可以达到千分之一度量级以上。
③可控性好，在允许的角度范围内可以做到任意指向，可以在重点区域进行高密度的扫描。
④扫描速度快，光学相控阵的扫描速度取决于所用材料的电子学特性，一般都可以达到MHz量级。
固态激光雷达劣势： ①扫描角有限，固态意味着激光雷达不能进行360度旋转，只能探测前方。因此要实现全方位扫描，需在不同方向布置多个（至少前后两个）固态激光雷达
②旁瓣问题，光栅衍射除了中央明纹外还会形成其他明纹，这一问题会让激光在最大功率方向以外形成旁瓣，分散激光的能量。
③加工难度高，光学相控阵要求阵列单元尺寸必须不大于半个波长，一般目前激光雷达的工作波长均在1微米左右，故阵列单元的尺寸必须不大于500nm。而且阵列密度越高，能量也越集中，这都提高了对加工精度的要求，需要一定的技术突破。
④接收面大、信噪比差：传统机械雷达只需要很小的接收窗口，但固态激光雷达却需要一整个接收面，因此会引入较多的环境光噪声，增加了扫描解析的难度。

MID-40简单使用说明\left[\begin{matrix} 0 \ 0\ 0\end{matrix}\right]

Livox Mid-40的检测范围可达260米。
Livox Mid-40的圆形视场范围为38.4°。Livox Mid-100是3个40的叠加,所以范围是98.4.

使用安装

1.将Livox Mid-40连接到Livox转换器。
2.使用以太网电缆将PC和Livox Converter连接到路由器。
3.将Livox转换器连接到外部电源。
额定功率是10w,电压范围10-16v.需要注意的是温度越低需要的功率就越大,所以需要合适的电源.

点云扫描方式

非重复扫,高覆盖率

固态激光雷达的里程计（loam_livox）相关推荐

一文详解固态激光雷达的里程计（loam_livox）
点击上方"计算机视觉工坊",选择"星标" 干货第一时间送达 Loam livox(2019 IROS) 介绍: 大疆出品,必属精品. 固态激光雷达里程计的工作现 ...
LiLi-OM: 走向高性能固态激光雷达惯性里程计和建图系统
点击上方"计算机视觉工坊",选择"星标" 干货第一时间送达编辑丨当SLAM遇见小王同学声明: 本文只是个人学习记录,侵权可删.论文版权与著作权等全归原作者所 ...
LiLi-OM: 面向高性能固态激光雷达惯性里程计和建图系统
论文:https://arxiv.org/abs/2010.13150 源码:https://github.com/KIT-ISAS/lili-om Towards High-Performance ...
基于激光雷达的里程计及3D点云地图中的定位方法
本文转载自公众号@点云PCL,基于激光雷达的里程计及3D点云地图中的定位方法 :https://mp.weixin.qq.com/s/laA1YAPBCpqlzdGi0yb2cQ 论文:LOL: Li ...
基于点线特征的激光雷达+单目视觉里程计
标题:Lidar-Monocular Visual Odometry using Point and Line Features 作者:Shi-Sheng Huang1, Ze-Yu Ma1, Tai ...
SLAM论文笔记-使用点和线特征的激光雷达-单目视觉里程计
使用点和线特征的激光雷达-单目视觉里程计首发在泡泡机器人slam公众号标题:Lidar-Monocular Visual Odometry using Point and Line Feature ...
LIO-SAM：通过平滑和映射的紧耦合激光雷达惯性里程计(翻译)
摘要我们提出了一种通过平滑和映射实现紧耦合的激光雷达惯性里程测量的框架LIO-SAM,该框架实现了高精度.实时的移动机器人轨迹估计和地图构建.LIO-SAM将激光雷达惯性里程表公式化为因子图,允许从 ...
Point-LIO：鲁棒高带宽激光雷达惯性里程计
作者 | 流川峰编辑 | 深蓝学院点击下方卡片,关注"自动驾驶之心"公众号 ADAS巨卷干货,即可获取点击进入→自动驾驶之心[SLAM]技术交流群本文介绍了一种新型的Li ...
面向固态激光雷达和惯导的里程计和建图
点云PCL免费知识星球,点云论文速读. 文章:Towards High-Performance Solid-State-LiDAR-Inertial Odometry and Mapping 作者:K ...

固态激光雷达的里程计（loam_livox）