CN113012191A - 一种基于点云多视角投影图的激光里程计算法 - Google Patents

Abstract

本发明提出一种基于点云多视角投影图的激光里程计算法，应用于移动机器人以及自动驾驶平台，包括如下步骤：步骤1、对三维激光点云数据进行多视角二维投影，得到环境的柱面全景投影视图、鸟瞰图，进一步对投影进行分割得到多视角点云投影图；步骤2、构建多视角位姿估计网络，在点云投影图上提取环境几何结构特征；步骤3、计算基于位姿绝对值的位姿损失函数以及基于稳定点区域的光度重投影误差损失函数；步骤4、根据位姿损失函数训练神经网络模型，所述神经网络模型能够根据输入的前后帧点云数据估计相对位姿变换。

Description

一种基于点云多视角投影图的激光里程计算法

技术领域

本发明涉及移动机器人技术以及自动驾驶领域，尤其涉及深度学习方法和SLAM(同步定位与建图)框架中的激光里程计相关领域。

背景技术

随着制造工艺的提升和人们对安全问题的重视程度日益增长，激光雷达在自动驾驶、无人物流等前沿应用中具有巨大的应用潜力。因而，基于激光点云的目标检测、语义分割、里程计、重定位等一系列三维视觉任务成为了相关领域的研究热点。然而，由于激光点云的无序性和稀疏性，在其被广泛应用于无人驾驶和机器人领域之前，还需要解决以下几个问题：

1)在交通场景中64线激光雷达的一次扫描数据包含10^{^5}个点云，任何逐点检索操作将导致算法不能够实时运行，进而影响到算法在移动平台的部署以及会引来一系列严重的安全性问题。

2)点云的无序性体现在将点云数据打乱顺序不影响最终对环境信息的表达。因而在对点云进行特征提取时只能提取其顺序不变的特征，目前常见的做法是使用maxpooling对点云的全局特征进行提取，但是导致了点云局部特征信息的损失。

发明内容

本发明的目的是提供一种可应用于移动机器人以及自动驾驶平台的基于点云多视角投影图的激光里程计算法，该算法能够发挥激光点云对环境感知的鲁棒性、精确性，同时利用成熟的二维卷积对点云投影图进行高效处理，应用投影图的多视角特征鲁棒地提取环境几何结构，提升三维激光里程计在复杂交通场景下的位姿估计精度和效率，从而得到较为精确的轨迹。

本发明的技术方案为：一种基于点云多视角投影图的激光里程计算法，应用于移动机器人以及自动驾驶平台，包括如下步骤：

步骤1、对三维激光点云数据进行多视角二维投影，得到环境的柱面全景投影视图、鸟瞰图，进一步对投影进行分割得到多视角点云投影图；

步骤2、构建多视角位姿估计网络，在点云投影图上提取环境几何结构特征；

步骤3、计算基于位姿绝对值的位姿损失函数以及基于稳定点区域的光度重投影误差损失函数；

步骤4、根据位姿损失函数训练神经网络模型，所述神经网络模型能够根据输入的前后帧点云数据估计相对位姿变换。

进一步的，在步骤1中，分别根据俯视视角、环视视角对点云投影，得到鸟瞰图以及柱面全景投影图，在柱面全景投影图的基础上，构造多视角点云投影图。

进一步的，在步骤2中构建多视角位姿估计网络，包括图像特征提取网络、旋转预测网络、平移预测网络，对步骤1中得到的点云投影图分别提取特征，然后用旋转预测网络和平移预测网络对融合特征进行处理，得到旋转量预测以及平移量预测，具体过程为：

使用柱面投影图特征提取网络、鸟瞰图特征提取网络以及基于FlowNets的多视角光流特征提取网络，对各个投影图分别进行处理，然后融合特征，构建两个相同的深层全连接网络，分别用于输出旋转估计量以及平移估计量。

进一步的，在步骤3中，所述构造基于位姿绝对值的位姿损失函数为基于欧拉角和平移量的L2损失函数，计算基于稳定区域的光度重投影误差损失函数为基于双线性插值的重投影误差损失函数，具体过程为：

首先计算基于欧拉角和平移量的L2损失函数：

其中

分别表示平移量真值和旋转量真值，p、q表示平移量和旋转量的预测值，k为权重；

然后将前后帧点云根据预测的相对位姿变换对齐：

P′_tgt＝TP_src

其中P_src表示前一帧点云，P′_tgt表示变换到后一帧点云的坐标系的前一帧点云，T为预测的相对位姿变换；

并分别构建稳定区域掩模：

M＝M₁*M₂

其中Φ()表示柱面投影函数，r_threshold为稳定区域判定阈值，

表示求解当变量(i,j)在集合E范围内变化时的最小值，

分别表示两帧点云的柱面投影图对应的稳定区域掩膜的第i行、第j列的值，M为最终的稳定区域掩膜，表示两帧点云的柱面投影图的稳定区域的交集。

然后根据像素之间的对应关系，在每一个像素的邻域内对柱面投影图进行双线性插值：

其中Δ_u1、Δ_u2为插值目标位置到其附近上下两个像素的纵向距离，Δv₁、Δv₂为插值目标位置到其附近上下两个像素的横向距离；

构建基于双线性插值的重投影误差损失函数：

其中H、W为柱面投影图的高和宽，M_ij为稳定区域掩膜在(i,j)处的值；

最后得到总的损失函数：

Loss＝w₁*L_pose+w₂*L_pho

其中w₁、w₂为损失项的对应权重。

有益效果：

本激光里程计方法能够处理复杂、动态交通场景下的户外大范围轨迹跟踪，其端到端的特性无需额外的数据预处理步骤，运行高效；基于稳定静态区域的光度重投影损失函数弥补了端到端模型的精度缺陷，双线性插值求解匹配深度值克服了量化误差带来的精度损失。此外，该方案整体成本较低，不需要对环境及机器人自身进行改造，可移植性强。

附图说明

图1为本发明实施例提供的复杂车辆环境下的基于点云多视角柱面投影图的三维激光里程计模型。

图2为基于三维点云柱面投影图的多视角分割示意图。

图3为多视角位姿估计网络的结构示意图。

图4为计算点云柱面投影图中像素的邻域梯度时，像素及其四邻域的示意图。

图5为计算基于稳定点区域的光度重投影误差时双线性插值示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整的描述，显然，所描述的实施例仅为本发明的一部分实施例，而不是全部的实施例，基于本发明中的实施例，本领域的普通技术人员在不付出创造性劳动的前提下所获得的所有其他实施例，都属于本发明的保护范围。

根据本发明的实施例，为了克服现有技术方案中存在的缺陷而提出针对复杂交通场景以及移动机器人平台，提出一种基于三维点云多视角投影的激光里程计算法，该算法充分考虑成本与精度的要求，设计出一种鲁棒性强、成本低，精度高的里程计算法，该方案中车辆装有一台64线激光雷达设备。具体包括如下步骤：

步骤1、对三维激光点云数据进行多视角二维投影，得到环境的柱面全景投影视图、鸟瞰图，进一步对投影进行分割得到多视角点云投影图；

根据本发明的实施例，首先对相邻两帧的三维点云数据分别进行俯视投影得到鸟瞰图，然后对两帧点云分别进行柱面投影，即将点云包含的三维坐标信息以及计算得到的点云到激光雷达的距离信息作为柱面投影图的通道数据，得到三维点云的2.5D表示，投影函数Φ可以描述为：

w＝arctan2(y,x)/Δθ

其中(x,y,z)是点云的三维坐标，(w,h)是柱面投影图的像素坐标，Δθ、Δφ分别为激光雷达的水平、竖直分辨率，r是柱面投影图的像素值，代表点云到激光雷达的距离。

最后根据得到的柱面投影图，分割得到多视角投影图。如对于高、宽分别为H和W的柱面投影图，可在横向将其等比例地切分为前、后、左、右四块，其中后视图由柱面投影图的两端拼接在一起，得到四张高、宽分别为H和W/4的多视角投影图,如图2所示。然后分别左右延展一定的像素(如20个)确保不会有信息丢失，得到尺寸为64*340的四张多视角柱面投影图。

步骤2、构建多视角位姿估计网络，在点云投影图上提取环境几何结构特征；

根据本发明的实施例，首先构建多视角位姿估计网络，在点云投影图上提取多视角下的环境几何结构特征，将所提取的多视角特征输入到端到端多视角位姿估计网络中得到六自由度位姿预测，多视角位姿估计网络包括柱面图特征提取网络、鸟瞰图特征提取网络、多视角特征提取网络以及两个伪孪生深层全连接位姿预测网络。首先点云的多种投影表示分别输入到对应的特征提取网络中，然后将提取的特征向量深度融合，并分别输入到两个全连接位姿预测网络中对平移量和旋转量分别进行预测，如图3所示。

下面对各个网络进行详细介绍：

根据本发明的实施例，柱面投影图特征提取网络、鸟瞰图特征提取网络两个网络的输入分别为点云的柱面投影图以及鸟瞰图，其网络结构均基于VGG-16。由于柱面投影图的长宽比较大，因此柱面特征提取网络的最后三层卷积层仅在水平方向进行降采样。

根据本发明的实施例，多视角特征提取网络的输入为基于柱面图的多视角分割图。模型结构基于flownets，一个使用编解码结构的高效、轻量的光流特征提取网络。在实际中考虑到多视角分割图尺寸较小，因此仅用其前三层降采样网络和最后两层上采样网络提取光流特征。多视角分割图以前后视图、左右视图为划分，分别输入到该模块中，得到前后、左右的光流特征图。

根据本发明的实施例，特征融合网络为了融合柱面图特征、鸟瞰图特征以及多视角光流特征图，需要先使用均值池化对特征图进行自适应降采样以得到尺寸一致的特征图，然后将等尺寸的特征图进行通道连接融合，最后转化成特征向量输入到全连接位姿预测网络中。

根据本发明的实施例，伪孪生深层全连接位姿预测网络如下：

六自由度位姿中的旋转量与平移量不同，在笛卡尔坐标系中旋转量误差相对于三维坐标具有非线性的特点，因此需要对旋转量和平移量分别构造一个全连接网络进行预测，即伪孪生深层全连接位姿预测网络。旋转量位姿预测网络由5层全连接层构成，每层全连接层后接LeakyReLU激活层，并在第3、4层的激活函数后接Dropout层抑制过拟合。平移量位姿预测层与旋转量位姿预测层相同。每层神经元数量为512、128、64、16、3。

其中选择欧拉角作为旋转量的表示，欧拉角包含三个参数i、j、k，分别表示绕X、Y、Z轴的旋转角度。平移量包含x、y、z三个参数，表示在三维空间中的坐标。

步骤3、计算基于位姿绝对值的位姿损失函数以及基于稳定点区域的光度重投影误差损失函数；损失函数由两部分组成，基于欧拉角和平移量的L2损失和基于双线性插值的重投影误差。

基于欧拉角和平移量的L2损失：

根据真值标签和预测的位姿量计算二者之间的L2距离损失：

其中

分别表示平移量真值和旋转量真值，p、q表示平移量和旋转量的预测值。k为权重。

基于双线性插值的重投影误差损失函数计算过程如下：

1、根据预测的相对位姿变换T将参考点云P_src转换到目标点云P_tgt的坐标系中：

P′_tgt＝TP_src

2、分别计算两个点云的稳定点掩模。

具体的，对于柱面图上坐标为(i，j)的像素，考虑其与四邻域内的四个像素构成的集合E，如图4。

计算像素(i,j)的邻域梯度：

根据领域梯度，得到稳定点掩模M：

对于点云P′_tgt、P_tgt分别计算得到稳定点掩模M1，M2，则最终的稳定点掩模：

M＝M1*M2

3、由于在激光点云的柱面投影图中，其像素值代表距离，因而天然满足光度一致假设，可以构造基于静态区域的光度损失函数：

需要注意的是，柱面投影函数Φ()会不可避免地引入量化误差。实际上，柱面投影函数分为投影和下取整两个步骤，即：

φ(P)表示投影后的点云未经下取整时在图像中的精确像素坐标，经过下取整后得到整数形式的像素坐标。

因此，为了避免上述下取整操作带来的精度损失，上述光度损失函数中使用了经过双线性插值后的点云柱面投影距离图

具体地：

对于柱面投影图Φ(P′_tgt)中坐标为(i,j)的像素点Φ(P′_tgt)_i，j，根据Φ(P′_tgt)_i，j未经下取整的精确像素坐标Φ(P′_tgt)_i，j，在柱面投影图Φ(P_tgt)的点集(Φ(P_tgt)_i，j，Φ(P_tgt)_i+1，j，Φ(P_tgt)_i，j+1，Φ(P_tgt)_i+1，j+1)中计算φ(P′_tgt)_i，j位置的插值距离值

(如图5)。

其中Δu₁、Δu₂为插值目标位置φ(P′_tgt)_i，j到Φ(P_tgt)_i，j、Φ(P_tgt)_i，j+1的纵向距离，Δv₁、Δv₂为插值目标位置φ(P′_tgt)_i，j到Φ(P_tgt)_i，j+1，Φ(P_tgt)_i+1，j+1的横向距离。

据此可以求出基于静态稳定区域的光度损失函数L_pho。

最后，计算总的损失函数：

Loss＝w₁*L_pose+w₂*L_pho

其中w₁、w₂为损失项的对应权重。

步骤4、根据位姿损失函数训练神经网络模型，保存权重，所述神经网络模型能够根据输入的前后帧点云数据估计相对位姿变换。

完成网络训练后，网络即可根据输入的前后帧点云数据鲁棒、高效地估计相对位姿变换。

尽管上面对本发明说明性的具体实施方式进行了描述，以便于本技术领域的技术人员理解本发明，且应该清楚，本发明不限于具体实施方式的范围，对本技术领域的普通技术人员来讲，只要各种变化在所附的权利要求限定和确定的本发明的精神和范围内，这些变化是显而易见的，一切利用本发明构思的发明创造均在保护之列。

Claims (4)

1.一种基于点云多视角投影图的激光里程计算法，应用于移动机器人以及自动驾驶平台，其特征在于，包括如下步骤：

步骤1、对三维激光点云数据进行多视角二维投影，得到环境的柱面全景投影视图、鸟瞰图，进一步对投影进行分割得到多视角点云投影图；

步骤2、构建多视角位姿估计网络，在点云投影图上提取环境几何结构特征；

步骤3、计算基于位姿绝对值的位姿损失函数以及基于稳定点区域的光度重投影误差损失函数；

步骤4、根据位姿损失函数训练神经网络模型，所述神经网络模型能够根据输入的前后帧点云数据估计相对位姿变换。

2.根据权利要求1所述基于点云多视角投影图的激光里程计算法，其特征在于，在步骤1中，分别根据俯视视角、环视视角对点云投影，得到鸟瞰图以及柱面全景投影图，在柱面全景投影图的基础上，构造多视角点云投影图。

3.根据权利要求1所述基于点云多视角投影图的激光里程计算法，其特征在于，在步骤2中构建多视角位姿估计网络，包括图像特征提取网络、旋转预测网络、平移预测网络，对步骤1中得到的点云投影图分别提取特征，然后用旋转预测网络和平移预测网络对融合特征进行处理，得到旋转量预测以及平移量预测，具体过程为：

使用柱面投影图特征提取网络、鸟瞰图特征提取网络以及基于FlowNets的多视角光流特征提取网络，对各个投影图分别进行处理，然后融合特征，构建两个相同的深层全连接网络，分别用于输出旋转估计量以及平移估计量。

4.根据权利要求1所述基于点云多视角投影图的激光里程计算法，其特征在于，在步骤3中，所述构造基于位姿绝对值的位姿损失函数为基于欧拉角和平移量的L2损失函数，计算基于稳定区域的光度重投影误差损失函数为基于双线性插值的重投影误差损失函数，具体过程为：

首先计算基于欧拉角和平移量的L2损失函数：

其中

分别表示平移量真值和旋转量真值，p、q表示平移量和旋转量的预测值，k为权重；

然后将前后帧点云根据预测的相对位姿变换对齐：

P′_tgt＝TP_src

其中P_src表示前一帧点云，P′_tgt表示变换到后一帧点云的坐标系的前一帧点云，T为预测的相对位姿变换；

并分别构建稳定区域掩模：

M＝M₁*M₂

其中Φ()表示柱面投影函数，r_threshold为稳定区域判定阈值，

表示求解当变量(i,j)在集合E范围内变化时的最小值，

分别表示两帧点云的柱面投影图对应的稳定区域掩膜的第i行、第j列的值，M为最终的稳定区域掩膜，表示两帧点云的柱面投影图的稳定区域的交集；

然后根据像素之间的对应关系，在每一个像素的邻域内对柱面投影图进行双线性插值：

其中Δu₁、Δu₂为插值目标位置到其附近上下两个像素的纵向距离，Δv₁、Δv₂为插值目标位置到其附近上下两个像素的横向距离；

构建基于双线性插值的重投影误差损失函数：

其中H、W为柱面投影图的高和宽，M_ij为稳定区域掩膜在(i,j)处的值；

最后得到总的损失函数：

Loss＝w₁*L_pose+w₂*L_pho

其中w₁、w₂为损失项的对应权重。

Images