第3章-基于激光雷达的机器人环境感知技术

基于激光雷达的机器人环境感知技术机器人环境感知技术-第3章湖南大学∙人工智能与机器人学院第一讲激光雷达传感器原理与预处理定义：激光雷达是一种主动式光学传感技术，它通过发射激光脉冲、接收反射信号，计算目标的距离、速度、形状等参数，生成二维/三维点云。核心优势：1.高精度（厘米级测距）、高角分辨率。2.全天候工作（不受光照影响，夜间性能稳定）3.快速三维数据采集，支持实时环境建模。局限性：激光无法穿透浓雾、浓烟，恶劣天气下性能下降。

激光雷达基本概念机械式激光雷达发展历史最为悠久，这类激光雷达通过电机带动光机结构整体360°旋转，多束激光束沿垂直方向排列，发射模块以一定频率发射激光线，通过连续旋转发射头实现动态扫描，形成点云，在性能上达到最优。机械式激光雷达的工作原理主要基于旋转扫描机制和激光测距技术。它的旋转机制使激光雷达能够覆盖周围环境的每一个角度。

3.1.1机械式激光雷达机械式激光雷达（1）多面旋转镜型多面旋转镜型激光雷达使用转镜折射光线实现激光在视场区域内的覆盖，通常与线光源配合使用，形成视场面的覆盖，也可以与振镜组合使用，配合点光源形成视场面的覆盖。多面旋转镜型激光雷达通常采用多面体旋转镜，这些反射镜专门设计用于高效反射特定波长的激光（如905nm、940nm、1550nm），通常配置为三面或四面镜。转镜的设计简洁，仅需匀速旋转，无需进行变速或其他复杂控制，镜面围绕一个中心轴线排列，形成类似于棱柱或多面体的结构。激光发射器发射激光脉冲，激光束直接照射到旋转镜上。

3.1.2半固态激光雷达多面旋转镜型激光雷达（2）MEMS微振子型

MEMS微振子型激光雷达，主要是通过MEMS振镜，进行水平方向和垂直方向的振动，实现激光束的扫描。MEMS微振镜集成在硅芯片上，镜面以一定的谐波频率悬挂在一对扭杆之间，旋转的微振镜反射来自激光的光线，实现扫描。但大尺寸镜面MEMS微振子悬梁材料存在疲劳问题，无法通过冲击、振动、高低温等汽车认证，使用寿命不稳定。

3.1.2半固态激光雷达MEMS微振子型激光雷达RS-LiDAR-M1（3）楔形棱镜型双楔形棱镜激光雷达由两块同轴放置的楔形棱镜组成，随着两个棱镜以不同速度旋转，将在前方扫出类似菊花的图样。如图所示为大疆Livox生产的Livox觅道Mid-40，该设备采用独特且不重复的扫描方法，随着扫描时间逐步增长，可以实现近100%的视角覆盖率；然而其移动迅速而且设备易于出现疲劳或损坏现象，其使用寿命相当有限，可靠性也相对较低。而且，非重复扫描的方法并不有利于自动驾驶算法在融合和匹配方面的应用。

3.1.2半固态激光雷达双楔形棱镜激光雷达Livox觅道Mid-40（1）Flash固态激光雷达Flash固态激光雷达工作原理和TOF相机有一些类似，其发射的激光束会直接向各个方向漫射，只要一次快闪就能照亮整个视场，并由位于焦平面的探测器采集不同方向反射的信号，不同目标的光有不同的飞行时间，从而获取深度信息。Flash固态激光雷达可以分为脉冲式和连续式。雷达激光直接发射到较大的检测区域，随后由接收器阵列计算每个像素对应的距离信息。对外部环境响应无延迟，无运动部件，稳定性更高，发射端方案更成熟。与传统机械式激光雷达相比，Flash固态激光雷达无需机械运动，能够实现更快速的数据采集，适用于高速移动的自动驾驶场景。

3.1.3全固态激光雷达Flash固态激光雷达（2）OPA固态激光雷达

OPA固态激光雷达是一种高级的激光雷达技术，使用光学相控阵技术来精确控制激光束的方向，而不依赖于任何机械旋转部件。OPA固态激光雷达利用一个由许多小型发射单元组成的相控阵列。每个发射单元可以独立控制激光的相位（即光波的相对位置）。通过调整阵列中各单元的相位，可以引导整个激光束的传播方向，实现精确的扫描而无需物理移动。在系统中，激光通过这些微小的发射单元被发射出去。每个单元发射的激光波相位可以精确调控，通过相位的差异，可以使得这些波相互干涉，形成一个统一的、方向可控的激光束。通过电子方式改变每个发射单元的激光相位，OPA系统可以迅速改变激光束的方向。这种方向的改变是通过调整激光波前的相位来实现的，而不需要任何物理移动，从而达到高速扫描的目的。但是，由于相位控制可能产生所谓的“侧瓣”效应，即在主激光束之外形成额外的光束，这可能会导致数据的干扰和噪声。

3.1.3全固态激光雷达OPA固态激光雷达第二讲激光点云配准

在点云配准方法中，ICP（IterativeClosestPoint，最近点迭代算法）是一种广泛使用的点云配准算法，用于估计两个三维点云之间的刚体变换（旋转和平移），以便将它们对齐。ICP算法的基本思想是通过迭代的方式，不断地寻找两个点云之间的对应点，并基于这些对应点来优化变换参数，直到达到某种收敛条件。是一种常用的点云配准技术，其基本思想是通过迭代优化的方式，寻找目标点云与参考点云之间的最佳变换矩阵，从而实现两者的对齐。下图介绍了ICP算法的基本原理及步骤。3.2.1最近点迭代算法

3.2.1最近点迭代算法（1）刚性变换矩阵通过一组映射关系可以完成将不同坐标系下的点云数据统一到同一坐标系中，坐标系的变换包括旋转和平移，这是点云配准的最终目的。其中，映射关系可以通过公式表示：其中𝑹表示旋转矩阵，𝑻表示平移向量，V表示透视变换向量，S表示目标整体的比例因子。：

3.2.1最近点迭代算法（1）刚性变换矩阵因为形变是不存在于设备扫描的过程中的，在不同帧的点云数据处理中，仅仅存在旋转和平移现象，因此，可以把V设定为零向量，利用比例因子来进行映射变换，以此方式表示其特性：其中旋转矩阵可表示为：

3.2.1最近点迭代算法（2）刚性变换矩阵相关参数的估算通过使用特定公式，可以对不同坐标系中的点云信息进行坐标式的转换，具体如下：其中，由此可推出：其中，以上提供的公式涉及了6个不确定的变量，要解决这6个变量需要最少六组方程。

3.2.1最近点迭代算法（2）刚性变换矩阵相关参数的估算定义源点集和目标点集：

3.2.2激光点云配准正态分布变换算法是一种在点云配准和机器人视觉等领域中广泛使用的算法。其核心思想是通过将点云数据划分为多个小区域，并在每个小区域内计算点云数据的概率分布（通常为正态分布），从而构建出一个基于概率密度的表示形式。后通过优化计算，找到最佳的变换参数，旨在使源云至目标云的概率密度分布最大化，确保两者之间达到最佳的匹配。在进行点云配准之前进行网格化操作，然后把它们划分成特定大小的网格结构。在每个网格内，计算点云数据的均值和协方差矩阵，从而得到该网格内点云数据的正态分布参数。某网络中的激光点概率可以表示为：如右图所示，（a）图是激光点数据，（b）图是概率图，明亮程度代表着这个地方是障碍物的可能性，越亮的地方越可能是障碍物。（1）计算NDT过程

3.2.2激光点云配准通过变换T将需要配准的点云转换到参考点云的网格中：（2）帧间匹配其中T是两帧激光点云的变换矩阵，𝑥′,𝑦′,𝑧′是上一帧激光点云数据，𝑥,𝑦,𝑧式当前激光点云数据，计算T

的过程如下：①首先，构建首帧（前一帧）激光点云的NDT（概率值）；②对前一帧和当前帧之间的位姿进行初始化（针对特定的移动机器人，里程计可以用来初始化）；③经过转换矩阵将当前帧（第二帧）的激光点转换到前一帧激光的坐标下（根据初始化的位姿，完成当前帧激光的坐标转换）；④经过坐标转换后，计算当前帧激光概率分布情况；⑤适当改变位姿，重复③、④步骤，直到收敛；

3.2.2激光点云配准

（3）牛顿算法优化𝜀的梯度方向为：

3.2.2激光点云配准（3）牛顿算法优化

根据变换方程，向量𝒒，对变换参数𝑝的二阶导数的向量为：第三讲基于激光雷达的环境建图算法

二维栅格地图构建算法中最具代表性的谷歌开源的Cartographer，在实现二维SLAM中，它可以生成精度为5cm的二维栅格实时地图，采用回环检测来优化子图的位姿，消除建图过程中的累积误差，同时实现了计算量和实时性之间的平衡。其系统框架结构如图。SLAM框架结构图3.3.1二维栅格地图构建

Cartographer的主要思路是利用闭环检测来减少建图过程中的累积误差。该研究算法能凭借激光测距仪等数据，精确生成高精度栅格地图。算法大致可以划分为两个关键环节：局部SLAM模块通过对每一帧激光雷达扫描数据的精细处理，构建并不断更新一系列独立的子地图结构；这些子地图实质上是由网格形式表示的局部环境模型。在处理新的激光雷达扫描数据时，运用Ceres扫描匹配算法精细地确定其最适宜的插入点，将其无缝融入整体的子图结构中。全局SLAM模块子图结构在处理过程中容易累积误差，为了修正这一问题，通过全局SLAM来解决，主要是通过闭环检测的手段削减累计误差。当一个子图的建立，也就是不会有新的激光雷达扫描数据进入此子图中时，算法会把这一子图整合进闭环监测程序里，通过实施闭环检验技术，系统性地检测并校正累积的误差，确保定位精度。3.3.1二维栅格地图构建

3.3.1二维栅格地图构建湖南大学机器人学院建图结果本实验使用ROS中的Cartographer功能包进行建图实验，结果如图所示。实验使用的教育机器人搭载了单线激光雷达、IMU以及光电编码器，可以看出实验结果存在一定地图漂移，原因是建图效果受到激光雷达与里程计精度的影响。

三维激光雷达获得的点云数据完成特征提取后进行运动估计并构建遍历环境地图的主要流程，包括回环检测与后端优化部分。其核心流程可总结如下。1.特征提取与运动估计：提取激光点云特征（如边缘、平面），估计帧间运动。2.回环检测：粗查找：用Scan-context/IRIS快速筛选潜在回环帧；精配准：用ICP/NDT优化回环帧位姿，计算约束。3.后端优化：图优化（节点=位姿，边=帧间约束/回环约束），非线性最小二乘最小化全局误差。4.地图拼接：将优化后的位姿应用于原始点云，生成全局三维点云地图。3.3.2三维点云地图构建点云输入运动估计回环检测后端优化地图输出

本节简要介绍在对三维激光雷达获得的点云数据完成特征提取后进行运动估计并构建遍历环境地图的主要流程，包括回环检测与后端优化部分。1.回环检测激光SLAM会通过二阶段的方法实现激光回环，分为粗查找与精配准：粗查找有许多算法提出，如CSM-cartographer，Scan-context，IRIS等，因为回环检测速度是需要足够的快来跟上后端收到数据速度的，否则会导致处理不过来，所以该阶段会在时间限制要求上达到不错的召回与准确率，为后一阶段提供一个好的初值与匹配对，另外该阶段也是有许多创意满满的实现思路的，差异性较大。精匹配思路相对就比较固定了，仅考虑到常规点云配准范畴就满足使用了，如ICP系列，NDT等，该部分在初值不佳，或匹配次数多的时候，都会造成大量耗时，会影响到回环检测效率，是需要合适的粗查找筛选绝大一部分来降低精匹配的消耗的。由于精查找是回环检测最后一关，假如各位姿图加入不当的回环，会产生十分不良影响，因此该处一般会建议加入归一化评分机制来确定最终效果是否合适，如ICP或NDT都有相应的误差得分考虑。3.3.2三维点云地图构建

2.后端优化为了解决局部误差的持续累积，人们普遍采用的做法是使用滤波器后端和图优化后端。基于滤波的方法主要是粒子滤波，如蒙特卡洛方法，以及卡尔曼滤波系列，如扩展卡尔曼滤波（EKF），迭代卡尔曼滤波（IEKF），无迹卡尔曼滤波（UKF）等；另一方面，图优化策略凭借图论的原理，巧妙地将机器人在环境中的位置信息抽象为图结构，通过构建节点(Node)来代表机器人姿态，通过边(Edge)来体现节点间的空间约束关系，这种方法直观且高效，如图所示。在构建地图的过程中，由于机器人操作可能导致误差累积，因此采用非线性最小二乘优化技术至关重要。这种优化策略旨在系统地处理所有帧之间的约束关系，通过迭代线性化的方法精确地校正和减小这些误差。在本章节的核心内容中，本文将深入探讨两种主要的优化策略，包括利用回环检测结果和惯导先验位姿两种方法，分别实现消除激光里程计累计误差的功能，回环在此处提供的是两帧之间的相对位姿。3.3.2三维点云地图构建

（1）基于回环的位姿修正位姿图优化所有观测与状态，误差公式的求和为各模块残差之和。在实际操作中，每个残差会依据特定的重要性被赋予一个权重，即所谓的信息矩阵，这种权重处理方式是对残差进行了一种加权调整。在分析信息矩阵之后，总的误差或残差项可以数学化地表示为：3.3.2三维点云地图构建2.后端优化此时优化问题可以表示为：随后构建残差，第𝑖帧和第𝑗帧之间的相对位姿，在李群𝑆𝐸(3)上可以表示为：也可以在李代数上表示为：

若位姿没有误差，则上面两个式子是精确相等的，但当位姿有误差存在时，便可以使用等式的左右两端计算残差项：3.3.2三维点云地图构建2.后端优化位姿图优化的核心理念在于，通过对状态变量，即位姿进行微调，目标是减小残差函数的值。为此，本文需要计算残差项对位姿的雅可比矩阵，以便有效地应用高斯-牛顿优化算法进行迭代优化。

根据上述公式，求雅可比的方式是对位姿添加扰动，此时残差表示为：

其中：按照高斯牛顿法的流程，需要对残差进行一阶泰勒展开，此后便可以使用高斯牛顿法进行优化。

（2）基于先验观测的位姿修正先验观测是一种独特的单向关联，不同于帧间观测，它并不涉及前后状态的链接，它直接给出的就是该状态量的观测值。对应的残差就是观测值与状态量之间的差异，即3.3.2三维点云地图构建2.后端优化对残差添加扰动，可得利用伴随性质和BCH公式进行化简，可得

其中：

在完成了回环检测以及后端优化后，下一步的操作是进行地图的拼接，即建立点云地图。在点云地图生成流程设计中，其核心策略着重于高效且精确地整合来自里程计的相对位置数据、闭环检测的相对定位信息，以及惯性导航系统的预设位姿。如图所示为点云地图构建的详细流程。3.3.2三维点云地图构建3.三维点云地图建立点云地图建立流程图第四讲基于激光雷达的环境语义分析

本节将介绍激光雷达物体检测领域中两个里程碑式的工作——VoxelNet和PointNet++，后续的点云处理基本上就基于这两个方向展开，下面对这两种方法进行详细介绍。3.4.1基于激光雷达的物体检测VoxelNetPoubtNet++

1.VoxelNetVoxelNet方法是一个端到端（EndtoEnd）的利用激光点云进行三维目标检测的神经网络架构，它的主要特征是将点云转换成体素（voxel）形式后在进行特征提取，VoxelNet方法的整体架构如图所示。3.4.1基于激光雷达的物体检测VoxelNet整体架构图整体网络从下到上被分为三层：1）特征提取层（FeatureLearningNetwork）：主要的任务是将点云划分为体素，然后提取特征；2）中间卷积层（ConvolutionalMiddleLayers）：对提取到的体素特征进行卷积，得到高维特征；3）RPN层（RegionProposalNetwork）：用于生成三维物体的边界框。

2.PointNet++

PointNet++是斯坦福大学的CharlesR.Q等人在他们原来提出的PointNet方法上的一个改进、迭代更新的版本。原有的PonitNet方法使用具有置换不变性的对称函数解决了由于点云无序性带来的操作问题，是将深度学习运用到点云领域的一个开创性的方法，而PointNet++核心的想法在局部区域重复性的迭代使用PointNet。PointNet++的网络结构如图所示。3.4.1基于激光雷达的物体检测PointNet++网络架构图PointNet++的网络结构是在PointNet的基础上引入了分层点集特征学习的方法，它构建基于点的分层分组，并沿着层次结构逐步抽象出逐渐增大的局部区域网络结构。

分层点集特征学习由多个集合抽象层组成，每一个集合抽象层包含有一个采样层、一个分组层和一个PointNet层。

在VoxelNet和PointNet++这两个方法被提出后，基于激光雷达的物体检测进入到了一个快速发展期，许多的算法被提出用于改进这两个方法，如SECOND、PIXOR以及Point-RCNN等，目前最新的研究方向是将基于视图的方法以及基于原始点云的方法进行融合，基本的融合思路是：利用较低分辨率的Voxel来提取上下文特征或生成候选的物体框，然后再与原始的点云集进行结合，目前较为成熟的融合方法是SIENet与VoxelR-CNN。3.4.1基于激光雷达的物体检测

本节主要针对语义分割任务展开详细介绍。在与点云物体检测算法类似，按照输入数据的不同组织形式，语义分割的方法也可以分为传统和深度学习的方法、基于点的方法，基于图像的方法，基于体素的方法以及基于图的方法。3.4.2基于激光雷达的语义分割

3.4.2基于激光雷达的语义分割1.传统和深度学习的方法

深度学习流行之前，语义分割根据特征表示与处理流程主要被传统学习方法和深度学习方法。传统的三维语义分常通过手工提取特征，并从分类器中输出点的标签，机器学习领域中经典的分类器，比如SVM，AdaBoost，RandomForest等都可以采用。其次，传统的方法的首先对点进行聚类分割，然后进行特征提取和语义分类；另外一种是直接设计每一个点的特征向量，不经过事先分割，但是原始的特征提取和分类并没有考虑大范围的上下文信息，而这部分信息对语义分割来说是不可或缺的。因此，在局部分类的基础上，还需要一个上下文模型来提高分割结果的正确性和平滑性。这里最常用的是条件随机场（ConditionalRandomFields，CRF)。一般来说，CRF可以作为一个正则项与局部分类器的优化目标相结合，从而将两个步骤整合为一个优化问题来求解。

最近，利用深度神经网络开发了许多关于三维激光雷达语义分割的研究，根据输入数据的格式，它们大致可分为后面四组，下面对这四类方法进行详细说明。

3.4.2基于激光雷达的语义分割2.基于点的方法基于点的方法直接将原始点云（三维坐标和颜色与法线等特征）作为输入，输出逐点类别标签。PointNet：通过“对称函数（最大池化）”解决点云无序性问题，提取全局特征后与点特征拼接，实现逐点分类。PointNet++：改进PointNet的“局部特征缺失”问题，通过分层特征学习（“采样→分组→特征提取”循环）构建多尺度特征。其他改进算法PointCNN：通过“χ算子”对K近邻点按空间距离排序，构建有序特征序列，实现点云卷积；KPConv（核心点卷积）：在局部区域定义“核心点”（类比卷积核），计算点与核心点的相对位置特征，提升几何表征能力；PointAtrousNet：引入空洞卷积（PAC），在不增加参数的情况下扩大感受野，提取多尺度几何特征。对于点云数据来说，点的位置是不固定的，卷积操作也无法在空间位置上找到对应关系。2019年KPConv提出在一个领域范围内定义相对位置固定的一些核心点卷积（KernelPointConvolution）来计算特征（类比传统的卷积核）。

3.4.2基于激光雷达的语义分割3.基于图像的方法核心思路：将三维激光雷达点云投影为二维图像（如球面映射的Range图像、多视角合成图像），复用成熟的2D图像语义分割算法（如FCN），再将2D分割结果反投影回原始3D点云，实现逐点语义标注。详细流程如下。全卷积分割网络结构图点云到Range图像转换2D全卷积分割2D到3D反投影3D后处理优化

3.4.2基于激光雷达的语义分割4.基于体素的方法基于体素的方法——量化为3D体素网格核心思路：将三维点云量化为均匀的体素网格（VoxelGrid），通过3D卷积神经网络提取体素级特征，为每个体素分配类别标签，网格内所有点共享该标签，平衡精度与效率。代表算法：VoxelNet扩展（语义分割方向）原理：沿用“体素划分→随机采样→特征编码→3D卷积”流程，将原用于目标检测的RPN层替换为语义分割头（如U型3DCNN），输出体素语义标签；FCPN（全卷积点云网络）差异：与VoxelNet“固定体素大小、点数量不固定”不同，FCPN在每个体素内采集固定数量的点，构建稠密3D张量，通过3D卷积提取多尺度特征，提升分割精度。优劣势分析：优势：3D卷积能捕捉空间邻域关系、适配大规模点云；不足：体素分辨率难平衡（高分辨率→算力爆炸，低分辨率→细节丢失）、空体素（无点）造成计算资源浪费。

3.4.2基于激光雷达的语义分割5.基于图的方法核心