一种基于模式切换的建图方法及激光雷达设备与流程

1.本发明涉及激光雷达建图领域，尤其涉及一种基于模式切换的建图方法及激光雷达设备。


背景技术：

2.传统旋转激光雷达式测绘方法仅能固定工作地点，重建四周环境的点云地图，因而无法构建的大面积室内点云地图，这极大地制约了该方法的适用范围。直接将多帧旋转激光雷达式建图方法的重建结果进行点云配准会受到该类方法采集速度慢、里程计漂移、视野遮挡等问题的影响，导致拼接的精度低下。传统基于激光雷达的同时定位与建图算法能够快速、低里程计漂移地构建室内场景点云地图，但是由于激光雷达水平放置，对于环境中距离激光雷达较远的区域，如天花板、地面等，只能获取稀疏的点云，难以高分辨率地完成对环境的点云地图建模。因此为克服传统旋转激光雷达测绘方法的运动受限和传统基于激光雷达的同时定位与建图算法分辨率受限的缺点，本发明提出了依靠机械结构完成功能切换的激光雷达设备，以及基于旋转测绘模式和同时定位与建图模式切换的点云建图方法。


技术实现要素：

3.本发明的目的在于针对现有技术的不足，提供一种基于模式切换的建图方法及激光雷达设备。
4.本发明的目的是通过以下技术方案来实现的：
5.本发明提供一种基于模式切换的建图方法，该方法包括：
6.将激光雷达的纵向扫描构成旋转测绘模式；
7.将激光雷达的横向扫描构成同时定位与建图模式；
8.通过旋转测绘模式进行全景覆盖扫描，得到初始点云地图后切换到同时定位与建图模式；
9.在同时定位与建图模式下，通过运动覆盖多角度场景信息，得到实时点云数据，针对实时点云数据进行位姿估计是否退化的特征评价，如果评价结果表明退化，则切换回旋转测绘模式；
10.将两种模式下获取的点云数据进行拼接，完成环境建图。
11.进一步地，所述旋转测绘模式由电机联合机械结构带动激光雷达纵向旋转实现，在固定地点进行至少一轮旋转扫描。
12.进一步地，所述旋转测绘模式下，将扫描过程中采集到的点云数据利用电机编码器反馈的旋转角度值信息进行点云拼接，形成当前场景的稠密点云地图，具体为：
13.对一轮旋转测绘所得到的n帧点云，利用电机编码器获取对应的旋转角度值θi,i＝0,...,n；对编号为1～n的点云序列，利用其与编号为0的点云之间的旋转角度的差值所对应的旋转矩阵将点云投影至第0帧扫描时激光雷
达坐标系下，完成点云拼接；
14.所述旋转角度的差值及其所对应的旋转矩阵的具体形式为：
[0015][0016][0017]
进一步地，所述旋转测绘模式下，将扫描过程中采集到的点云数据利用点云配准算法进行点云拼接，形成当前场景的稠密点云地图，具体为：
[0018]
对一轮旋转测绘所得到的n帧点云，利用常见的icp(迭代最近点算法)或ndt(正态分布变换)等点云配准算法，完成帧间变换矩阵估计，并利用回环检测算法及位姿图优化算法缓解点云配准算法的漂移问题，利用优化后的帧间变换矩阵结果完成帧间点云融合，保证点云融合结果的准确性。
[0019]
进一步地，所述同时定位与建图模式下，由至少一个激光雷达在环境中不断运动扫描得到点云数据，扫描过程覆盖所述旋转测绘模式下缺失的多角度场景信息，同时估计自身位姿及构建环境的点云地图。
[0020]
进一步地，所述同时定位与建图模式下，估计自身位姿过程中，通过对各帧点云数据进行点云配准，实现激光雷达帧间位姿估计；点云配准算法包括且不限于icp(迭代最近点算法)或ndt(正态分布变换)等点云配准算法，且还可以先对各帧点云数据进行如提取角点、面点等预处理操作；
[0021]
构建环境的点云地图过程中，利用自身位姿估计结果，将各帧点云施加旋转平移变换，完成点云拼接。
[0022]
进一步地，所述针对实时点云数据进行位姿估计是否退化的特征评价，具体为：将实时点云数据与已有的点云地图进行配准，配准的优化过程使用高斯牛顿算法，其中待优化变量乘以雅可比矩阵得到残差矩阵；通过判断雅可比矩阵每一项大小，从而判断残差项对待优化位姿每个维度的影响程度；若雅可比矩阵中存在小于设定阈值的项(例如阈值设置为0.01)，则该项所对应的位姿维度发生退化；δe为残差项，是标量；δt为待优化位姿，是61向量；j为雅可比矩阵，是16向量；
[0023]
δe＝jδt
[0024]
j＝[j
1 j
2 j
3 j
4 j
5 j6]
[0025][0026]
其中，[δr
x
,δry,δrz]表示激光雷达x,y,z三个方向的旋转角，[δt
x
,δty,δtz]表示激光雷达x,y,z三个方向的平移量。
[0027]
进一步地，将旋转测绘模式下获取的点云数据进行优化，具体为：遍历所有点云，分割出平面点，构建非线性优化问题，其中优化函数目标为平面点云的厚度，优化变量为点云采集时的位姿；通过高斯牛顿优化，不断迭代点云位姿，使旋转叠加的点云精度提升。
[0028]
进一步地，将两种模式下获取的点云数据进行拼接，具体为：将多个旋转测绘模式得到的点云地图进行对齐；如果因为共同区域不足等原因导致对齐失败，则先部分融合相邻时间的同时定位与建图模式采集的点云数据，增大点云地图的共同区域，完成多个地图对齐，拼接形成新的点云地图；最后将同时定位与建图模式下采集的点云数据，重新在新地图上运行一遍位姿求解过程，根据优化后的位姿将点云投影至新地图上，完成全部点云数据拼接。
[0029]
本发明还提供一种实现上述基于模式切换的建图方法的激光雷达设备，包括硬件设备、旋转测绘模块、同时定位与建图模块、模式切换模块和点云地图拼接模块；
[0030]
所述硬件设备包括依次连接的第一旋转机构、第二旋转机构和激光雷达；
[0031]
所述旋转测绘模块，保持第二旋转机构竖直，由第一旋转机构联合机械结构带动激光雷达纵向旋转，实现激光雷达的多角度纵向扫描，此状态为旋转测绘模式；
[0032]
所述同时定位与建图模块，将第二旋转机构旋转至水平，第一旋转机构无输出，激光雷达在环境中不断运动扫描得到点云数据，扫描过程覆盖所述旋转测绘模式下缺失的多角度场景信息，同时估计自身位姿及构建环境的点云地图，此状态为同时定位与建图模式；
[0033]
所述模式切换模块，在判断所述旋转测绘模式完成全景覆盖扫描，得到初始点云地图后，切换到同时定位与建图模式；在同时定位与建图模式下，通过运动覆盖多角度场景信息，得到实时点云数据，针对实时点云数据进行位姿估计是否退化的特征评价，如果评价结果表明退化，则切换回旋转测绘模式；
[0034]
所述点云地图拼接模块用于将两种模式下获取的点云数据进行拼接，完成环境建图。
[0035]
本发明的有益效果是：本发明通过旋转测绘模式获取稠密精确的建图效果，通过同时定位与建图模式完成运动估计，进而通过运动覆盖多角度场景信息。对实时点云位姿估计是否退化进行评价，通过评价结果决策是否切换到旋转测绘模式。本发明具有两种模式的优点，能够提升环境测绘精度，增强环境测绘的场景覆盖能力。
附图说明
[0036]
为了更清楚地说明本发明的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本发明的某些实施例，因此不应被看作是对本发明保护范围的限定。在各个附图中，类似的构成部分采用类似的编号。
[0037]
图1为一个实施例中提供的基于模式切换的建图方法流程示意图；
[0038]
图2为一个实施例中提供的模式切换判断示意图；
[0039]
图3为一个实施例中提供旋的转测绘模式与同时定位与建图模式的硬件示意图，包括激光雷达、舵机、电机。
具体实施方式
[0040]
下面将结合本发明实施例中附图，对本发明实施例中的技术方案进行清楚、完整
地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。
[0041]
通常在此处附图中描述和示出的本发明实施例的组件可以以各种不同的配置来布置和设计。因此，以下对在附图中提供的本发明的实施例的详细描述并非旨在限制要求保护的本发明的范围，而是仅仅表示本发明的选定实施例。基于本发明的实施例，本领域技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0042]
在下文中，将更全面地描述本公开的各种实施例。本公开可具有各种实施例，并且可在其中做出调整和改变。然而，应理解：不存在将本公开的各种实施例限于在此公开的特定实施例的意图，而是应将本公开理解为涵盖落入本公开的各种实施例的精神和范围内的所有调整、等同物和/或可选方案。
[0043]
在下文中，可在本发明的各种实施例中使用的术语“包括”、“具有”及其同源词仅意在表示特定特征、数字、步骤、操作、元件、组件或前述项的组合，并且不应被理解为首先排除一个或更多个其它特征、数字、步骤、操作、元件、组件或前述项的组合的存在或增加一个或更多个特征、数字、步骤、操作、元件、组件或前述项的组合的可能性。
[0044]
此外，术语“第一”、“第二”、“第三”等仅用于区分描述，而不能理解为指示或暗示相对重要性。
[0045]
除非另有限定，否则在这里使用的所有术语(包括技术术语和科学术语)具有与本发明的各种实施例所属领域普通技术人员通常理解的含义相同的含义。所述术语(诸如在一般使用的词典中限定的术语)将被解释为具有与在相关技术领域中的语境含义相同的含义并且将不被解释为具有理想化的含义或过于正式的含义，除非在本发明的各种实施例中被清楚地限定。
[0046]
图1为一个实施例中提供的一种基于模式切换的建图方法，该方法包括：
[0047]
步骤s110，激光点云输入。
[0048]
步骤s120，模式切换。
[0049]
步骤s130，如果为初始状态或位姿估计发生退化，则采用旋转测绘模式。
[0050]
步骤s131，如果为旋转测绘完毕或位姿估计未发生退化，则采用同时定位与建图模式。
[0051]
步骤s140，将两种模式下获取的点云数据进行拼接，完成环境建图。
[0052]
具体地，激光雷达的纵向扫描构成旋转测绘模式，通过旋转测绘模式进行全景覆盖扫描，得到初始点云地图后切换到同时定位与建图模式；
[0053]
旋转测绘模式可以由电机联合机械结构带动激光雷达纵向旋转实现，在固定地点进行至少一轮旋转扫描。
[0054]
旋转测绘模式下，可将扫描过程中采集到的点云数据利用电机编码器反馈的旋转角度值信息进行点云拼接，形成当前场景的稠密点云地图，具体为：
[0055]
对一轮旋转测绘所得到的n帧点云，利用电机编码器获取对应的旋转角度值θi,i＝0,...,n；对编号为1～n的点云序列，利用其与编号为0的点云之间的旋转角度的差值所对应的旋转矩阵将点云投影至第0帧扫描时激光雷达坐标系下，完成点云拼接；旋转角度的差值及其所对应的旋转矩阵的具体形式为：
[0056][0057][0058]
旋转测绘模式下，可将扫描过程中采集到的点云数据利用点云配准算法进行点云拼接，形成当前场景的稠密点云地图，具体为：
[0059]
对一轮旋转测绘所得到的n帧点云，利用常见的icp(迭代最近点算法)或ndt(正态分布变换)等点云配准算法，完成帧间变换矩阵估计，并利用回环检测算法及位姿图优化算法缓解点云配准算法的漂移问题，利用优化后的帧间变换矩阵结果完成帧间点云融合，保证点云融合结果的准确性。
[0060]
具体地，激光雷达的横向扫描构成同时定位与建图模式；在同时定位与建图模式下，通过运动覆盖多角度场景信息，得到实时点云数据。
[0061]
同时定位与建图模式下，由至少一个激光雷达在环境中不断运动扫描得到点云数据，扫描过程覆盖所述旋转测绘模式下缺失的多角度场景信息，同时估计自身位姿及构建环境的点云地图。可采用以下方式：
[0062]
估计自身位姿过程中，通过对各帧点云数据进行点云配准，实现激光雷达帧间位姿估计；点云配准算法包括且不限于icp(迭代最近点算法)或ndt(正态分布变换)等点云配准算法，且还可以先对各帧点云数据进行如提取角点、面点等预处理操作；
[0063]
构建环境的点云地图过程中，利用自身位姿估计结果，将各帧点云施加旋转平移变换，完成点云拼接。
[0064]
图2为一个实施例中提供的一种针对实时点云数据进行位姿估计是否退化的特征评价方法的流程示意图，包括：
[0065]
步骤s210，构建残差与位姿的优化函数。
[0066]
步骤s220，求解雅可比矩阵。
[0067]
步骤s230，判断是否小于阈值。
[0068]
步骤s240，若小于阈值，则进行模式切换，切换回旋转测绘模式。
[0069]
步骤s241，若大于阈值，则继续同时定位与建图模式。
[0070]
其具体过程为：设备初始化运行时使用旋转测绘模式，完成后切换至同时定位与建图模式，通过点云位姿估计退化评价方法决定是否执行模式切换，并不断循环此运作流程，直到建图任务完成。
[0071]
具体地，将两种模式下获取的点云数据进行拼接，可采用如下方式：将多个旋转测绘模式得到的点云地图进行对齐；如果因为共同区域不足等原因导致对齐失败，则先部分融合相邻时间的同时定位与建图模式采集的点云数据，增大点云地图的共同区域，完成多个地图对齐，拼接形成新的点云地图；最后将同时定位与建图模式下采集的点云数据，重新在新地图上运行一遍位姿求解过程，根据优化后的位姿将点云投影至新地图上，完成全部点云数据拼接。
[0072]
在一个实施例中，提供一种实现上述基于模式切换的建图方法的激光雷达设备，该激光雷达设备包括硬件设备、旋转测绘模块、同时定位与建图模块、模式切换模块和点云
地图拼接模块；
[0073]
硬件设备包括依次连接的第一旋转机构、第二旋转机构和激光雷达；
[0074]
旋转测绘模块，保持第二旋转机构竖直，由第一旋转机构联合机械结构带动激光雷达纵向旋转，实现激光雷达的多角度纵向扫描，此状态为旋转测绘模式；
[0075]
同时定位与建图模块，将第二旋转机构旋转至水平，第一旋转机构无输出，激光雷达在环境中不断运动扫描得到点云数据，扫描过程覆盖所述旋转测绘模式下缺失的多角度场景信息，同时估计自身位姿及构建环境的点云地图，此状态为同时定位与建图模式；
[0076]
模式切换模块，在判断所述旋转测绘模式完成全景覆盖扫描，得到初始点云地图后，切换到同时定位与建图模式；在同时定位与建图模式下，通过运动覆盖多角度场景信息，得到实时点云数据，针对实时点云数据进行位姿估计是否退化的特征评价，如果评价结果表明退化，则切换回旋转测绘模式；
[0077]
点云地图拼接模块用于将两种模式下获取的点云数据进行拼接，完成环境建图。
[0078]
具体地，如图3所示，第一旋转机构可以采用电机实现，第二旋转机构可以采用舵机实现，但不限于此。
[0079]
以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本发明的保护范围之内。