CN116299536B - 基于位姿约束的激光雷达slam退化检测方法及系统 - Google Patents

Abstract

本发明提供一种基于位姿约束的激光雷达SLAM退化检测方法及系统，包括：通过激光雷达传感器采集连续帧点云；对所述连续帧点云进行分析，获取机器人位姿抗扰动鲁棒性的点云约束关系；根据所述点云约束关系，进行机器人位姿约束扰动识别，获得SLAM退化方向。本发明能解决激光雷达SLAM在退化场景下失效难以检测的问题，为激光SLAM的稳定运行提供预警，能实现较高的检测准确性，有利于实际应用。

Description

基于位姿约束的激光雷达SLAM退化检测方法及系统

技术领域

本发明涉及计算机视觉领域，具体地，涉及一种基于位姿约束的激光雷达SLAM退化检测方法及系统。

背景技术

激光雷达是目前一类应用广泛的传感器：发射模块基于光学原理，利用脉冲、调频、调幅等方式发射特定波长的激光；接收模块利用二极管元件对发射后反射回来的激光进行检测，通过光线飞行时间可以得到精确测距。激光雷达广泛应用于目标检测、场景建模、机器人导航等应用中，有较为广泛的商业与学术前景。

同步建图与定位算法(SLAM)是利用传感器提供的信息，建立所在环境场景的地图，并同时确定传感器在地图中的对应位置。目前SLAM技术广泛应用于机器人自主定位、导航任务中。而其中基于激光雷达的SLAM算法，因其高精度的测距信息与鲁棒的定位结果，广泛应用于无人物流、无人小巴等高难度定位导航任务中。

激光雷达SLAM算法的应用难题之一在于，面对特定场景的算法失效问题。这些场景包括：隧道、长廊、室外空旷停车场、高速公路等等，其特征在于场景中的结构性纹理较少，发生场景退化，定位时激光雷达提供的数据约束不足从而使算法失效。退化问题可以通过多传感器数据融合、状态空间建模补偿等方法处理，而如何对激光雷达SLAM的退化场景进行识别检测，成为算法优化的关键步骤。

发明内容

针对现有技术中的缺陷，本发明的目的是提供一种基于位姿约束的激光雷达SLAM退化检测方法及系统。

根据本发明的一个方面，提供一种基于位姿约束的激光雷达SLAM退化检测方法，包括：

通过激光雷达传感器采集连续帧点云；

对所述连续帧点云进行分析，获取机器人位姿抗扰动鲁棒性的点云约束关系；

根据所述点云约束关系，进行机器人位姿约束扰动识别，获得退化方向。

优选地，所述对所述连续帧点云进行分析，获取机器人位姿抗扰动鲁棒性的点云约束关系，包括：

将所述连续帧点云的局部表面近似为平面，每个测量点满足下列关系式：

其中，是雷达坐标系下激光光束的单位向量，用于指示光束方向，对应其光束长度，i∈{1,2,…,m}是激光点束的索引，是近似平面的法向量，p_i,0是近似平面上的一个点，分别描述了机器人的位置和姿态；

将记为d_i，则获得机器人位姿抗扰动鲁棒性的点云约束关系为：

优选地，所述根据所述点云约束关系，进行机器人位姿约束扰动识别，包括：

在点云配准过程中，机器人的位置和姿态(R,t)的变动较小，可以将(R,t)的变动视为扰动；

通过计算激光测量距离ρ_i对(R,t)扰动的敏感程度，判断激光雷达SLAM是否发生场景退化。

优选地，若机器人位姿受到扰动，而激光测量距离变化较大，那么当前描述机器人位姿约束较强；反之，若机器人位姿受到扰动，激光测量距离变化不大，那么当前描述机器人约束极弱，系统处于退化环境。

优选地，所述获得退化方向，包括：

基于机器人位姿中小角度变换的假设，使用R≈I+[θ]_×对求导问题线性化：

基于隐函数定理，建立等式：

推导可得，

根据上述隐函数定理获得的公式，构建F矩阵与T矩阵，分别表示激光约束对平移参数的敏感度与对旋转参数的敏感度：

对所述F矩阵和T矩阵进行特征值分解，获得一组特征向量，

其最小特征值对应的特征向量即为当前的退化方向：所述F矩阵的最小特征值用于判断平移量的退化方向；所述T矩阵的最小特征值用于判断旋转的退化方向。

优选地，矩阵在任一特征向量的投影，是特征向量自身固定的伸缩，伸缩比例是特征值，特征值反映矩阵在该特征向量方向的投影的比例大小，特征值越小，说明矩阵在该特征向量方向上的约束越小，约束小，说明在该方向上容易发生退化。

优选地，若λ_min小于所设定的阈值，代表当前场景发生退化；同时，最小特征值λ_min对应的特征向量即为当前的退化方向。

根据本发明的第二个方面，提供一种基于位姿约束的激光雷达SLAM退化检测系统，包括：

数据模块，该模块通过激光雷达传感器采集连续帧点云；

约束关系模块，该模块对所述连续帧点云进行分析，获取机器人位姿抗扰动鲁棒性的点云约束关系；

扰动识别模块，该模块根据所述点云约束关系，进行机器人位姿约束扰动识别。

根据本发明的第三个方面，提供一种终端，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时可用于执行上的基于位姿约束的激光雷达SLAM退化检测方法，或，运行上述的基于位姿约束的激光雷达SLAM退化检测系统。

根据本发明的第四个方面，提供一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时可用于执行上述的基于位姿约束的激光雷达SLAM退化检测方法，或，运行上述的基于位姿约束的激光雷达SLAM退化检测系统。

与现有技术相比，本发明具有如下的有益效果：

本发明能解决激光雷达SLAM在退化场景下失效难以检测的问题，为激光SLAM的稳定运行提供预警，能实现较高的检测准确性，有利于实际应用。

附图说明

通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：

图1是本发明一实施例中的基于位姿约束的激光雷达SLAM退化检测方法的流程图；

图2是本发明一优选实施例中的基于位姿约束的激光雷达SLAM退化检测的流程图；

图3是本发明一实施例中的机器人位姿所受约束的示意图。

具体实施方式

下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进。这些都属于本发明的保护范围。

首先，对本发明中涉及的名词(SLAM和退化场景)作出解释。SLAM：同步建图与定位算法，利用传感器提供的信息，在运动过程中建立所在环境场景的地图，并同时确定传感器在地图中的对应位置。SLAM技术广泛应用于机器人的环境建模与自主定位导航任务中。激光雷达SLAM即为根据激光雷达传感器获取的点云信息画出激光扫描场景的地图，并且确定该激光雷达传感器在该地图中的位置。退化场景：激光雷达使用场景中的结构性纹理较少，使得传感器在场景中提供的数据面对系统的优化问题时无法提供足够的约束，优化对象落入局部最优解而导致系统失效。激光雷达的退化场景包括隧道、长廊、室外空旷停车场、高速公路等。

本发明提供一个实施例，一种基于位姿约束的激光雷达SLAM退化检测方法，参见图1，包括：

S100，通过激光雷达传感器采集连续帧点云；

S200，对S200的连续帧点云进行分析，获取机器人位姿抗扰动鲁棒性的点云约束关系；

S300，根据S200的点云约束关系，进行机器人位姿约束扰动识别，获取退化方向。

本实施例解决了激光雷达SLAM在退化场景下失效难以检测的问题，为激光雷达SLAM的稳定运行提供预警，能实现较高的检测准确性，有利于实际应用。

在本发明的一个优选实施例中，实施S100，通过激光雷达传感器采集连续帧点云，具体的，机器人搭载激光雷达，激光雷达利用脉冲、调频、调幅等方式发射特定波长的激光，照射到物体后发生反射，接收发射后反射回来的激光，从而获得原始点云数据。每一帧包括一团点云，包括m个激光点发射的点云。其中，每一条激光在物体上发生的反射对应一个测量点。

在本发明的一个优选实施例中，实施S200，即对于激光雷达定位，获得激光雷达每个测量点对机器人的位姿约束，参见图2，具体过程为：

将点云的局部表面近似为平面，则对于每个测量点满足：

其中是雷达坐标系下激光光束的单位向量，用于指示光束方向，对应其光束长度，i∈{1,2,…,m}是激光点束的索引，是近似平面的法向量，p_i,0是近似平面上的一个点，分别描述了机器人的位置和姿态，可参见图3。

将记为d_i，则有：

将该式作为机器人位姿抗扰动鲁棒性的点云约束关系。该约束关系描述了机器人位姿和激光点之间的约束关系，为进一步检测激光点的测量是否发生了退化提供了准确的模型基础。

在本发明的一个优选实施中，实施S300，在点云配准(是指随着机器人运动，激光雷达连续获得了两帧点云数据，点云配准需要找到这两帧点云之间，点与点的匹配关系)中，两次配准对应的变换矩阵差异不大，因而(R,t)较小。(R,t)可视为扰动，描述系统的可定位能力，转换为激光测量距离ρ_i对(R,t)的敏感程度。如果机器人的位姿受到轻微扰动，而激光测量距离变化较大，那么当前描述机器人位姿约束较强。反之，若机器人位姿受到扰动，但激光测量距离变化不大，那么此时机器人约束极弱，系统处于退化环境。

在另一个较佳实施例中，参见图2，S300的优选过程如下：

S301：求激光测量距离ρ_i对(R,t)的偏导，基于小角度变换的假设，使用R≈I+[θ]_×对求导问题线性化：

S302：基于隐函数定理，建立等式如下：

结合S301推导可得：

步骤S303：根据步骤S302所得公式，构建F矩阵与T矩阵，分别表示激光约束对平移参数的敏感度与对旋转参数的敏感度：

步骤S304：根据步骤S303所得公式进行特征值分解，获得相应的特征向量。

步骤S305：其最小特征值对应的特征向量即为当前的退化方向：所述F矩阵的最小特征值用于判断平移量的退化方向；所述T矩阵的最小特征值用于判断旋转的退化方向。

特征向量，其中的任一个向量，在该坐标系(矩阵)上的投影，都只是自身固定的伸缩，而伸缩比例就是特征值的大小，因此特征值可以反映该向量方向在矩阵的比例大小，特征值越小，说明矩阵在该特征向量方向上的约束越小。约束小，说明在该方向上容易发生退化，因此最小特征值λ_min可以描述当前位置的可定位性。

进一步的，若λ_min小于所设定的阈值，代表当前场景发生退化。同时，最小特征值λ_min对应的特征向量也就是当前的退化方向。

基于相同的技术构思，本发明的其他实施例中，提供一种基于位姿约束的激光雷达SLAM退化检测系统，包括数据模块、约束关系模块和扰动识别模块。数据模块通过激光雷达传感器采集连续帧点云；约束关系模块对所述连续帧点云进行分析，获取机器人位姿抗扰动鲁棒性的点云约束关系；扰动识别模块根据所述点云约束关系，进行机器人位姿约束扰动识别。

基于相同的技术构思，本发明的其他实施例中，提供一种终端，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时可用于执行上的方法，或，运行上述的系统。

可选地，存储器，用于存储程序；存储器，可以包括易失性存储器(英文：volatilememory)，例如随机存取存储器(英文：random-access memory，缩写：RAM)，如静态随机存取存储器(英文：static random-access memory，缩写：SRAM)，双倍数据率同步动态随机存取存储器(英文：Double Data Rate Synchronous Dynamic Random Access Memory，缩写：DDR SDRAM)等；存储器也可以包括非易失性存储器(英文：non-volatile memory)，例如快闪存储器(英文：flash memory)。存储器用于存储计算机程序(如实现上述方法的应用程序、功能模块等)、计算机指令等，上述的计算机程序、计算机指令等可以分区存储在一个或多个存储器中。并且上述的计算机程序、计算机指令、数据等可以被处理器调用。

上述的计算机程序、计算机指令等可以分区存储在一个或多个存储器中。并且上述的计算机程序、计算机指令、数据等可以被处理器调用。

处理器，用于执行存储器存储的计算机程序，以实现上述实施例涉及的方法中的各个步骤。具体可以参见前面方法实施例中的相关描述。

处理器和存储器可以是独立结构，也可以是集成在一起的集成结构。当处理器和存储器是独立结构时，存储器、处理器可以通过总线耦合连接。

基于相同的技术构思，本发明的其他实施例中，提供一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时可用于执行上述的方法，或，运行上述的系统。

其中，计算机可读介质包括计算机存储介质和通信介质，其中通信介质包括便于从一个地方向另一个地方传送计算机程序的任何介质。存储介质可以是通用或专用计算机能够存取的任何可用介质。一种示例性的存储介质耦合至处理器，从而使处理器能够从该存储介质读取信息，且可向该存储介质写入信息。当然，存储介质也可以是处理器的组成部分。处理器和存储介质可以位于ASIC中。另外，该ASIC可以位于用户设备中。当然，处理器和存储介质也可以作为分立组件存在于通信设备中。

本领域内的技术人员应明白，本申请的实施例可提供为方法、系统、或计算机程序产品。因此，本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。

本申请是参照根据本申请的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。

这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。

这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。

显然，本领域的技术人员可以对本申请进行各种改动和变型而不脱离本申请的精神和范围。这样，倘若本申请的这些修改和变型属于本申请权利要求及其等同技术的范围之内，则本申请也意图包含这些改动和变型在内。

以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变形或修改，这并不影响本发明的实质内容。上述各优选特征在互不冲突的情况下，可以任意组合使用。

Claims (7)

1.一种基于位姿约束的激光雷达SLAM退化检测方法，其特征在于，包括：

通过激光雷达传感器采集连续帧点云；

对所述连续帧点云进行分析，获取机器人位姿抗扰动鲁棒性的点云约束关系；

根据所述点云约束关系，进行机器人位姿约束扰动识别，获得SLAM退化方向；

所述对所述连续帧点云进行分析，获取机器人位姿抗扰动鲁棒性的点云约束关系，包括：

将所述连续帧点云的局部表面近似为平面，每个测量点满足下列关系式：

其中，是激光雷达坐标系下激光光束的单位向量，用于指示激光光束方向，对应激光测量距离，i∈{1,2,…,m}是激光光束的索引，是近似平面的法向量，p_i,0是近似平面上的一个点，描述了机器人的位姿，R是旋转矩阵，t是平移距离；

将-记为d_i，则获得机器人位姿抗扰动鲁棒性的点云约束关系为：

所述根据所述点云约束关系，进行机器人位姿约束扰动识别，包括：

在点云配准过程中，将机器人的位姿(R,t)的变动视为扰动；

通过计算激光测量距离ρ_i对(R,t)扰动的敏感程度，判断激光雷达SLAM是否发生场景退化；

若机器人位姿受到扰动，而激光测量距离变化较大，那么当前描述机器人位姿约束较强；反之，若机器人位姿受到扰动，激光测量距离变化不大，那么当前描述机器人约束极弱，系统处于退化环境。

2.根据权利要求1所述的一种基于位姿约束的激光雷达SLAM退化检测方法，其特征在于，所述获得SLAM退化方向，包括：

基于机器人位姿中小角度θ变换的假设，使用R≈I+[θ]_×对求导问题线性化：

基于隐函数定理，建立等式：

推导得公式，

根据隐函数定理获得的所述等式和所述公式，构建F矩阵与T矩阵，分别表示激光约束对平移参数的敏感度与对旋转参数的敏感度：

对所述F矩阵和T矩阵进行特征值分解，获得一组特征向量，

其最小特征值对应的特征向量即为当前的退化方向：所述F矩阵的最小特征值用于判断平移量的退化方向；所述T矩阵的最小特征值用于判断旋转的退化方向。

3.根据权利要求2所述的一种基于位姿约束的激光雷达SLAM退化检测方法，其特征在于，矩阵在任一特征向量的投影，是特征向量自身固定的伸缩，伸缩比例是特征值，特征值反映矩阵在该特征向量方向的投影的比例大小，特征值越小，说明矩阵在该特征向量方向上的约束越小，约束小，说明在该方向上容易发生退化。

4.根据权利要求3所述的一种基于位姿约束的激光雷达SLAM退化检测方法，其特征在于，若最小特征值λ_min小于所设定的阈值，代表当前场景发生退化；同时，最小特征值λ_min对应的特征向量即为当前的退化方向。

5.一种基于位姿约束的激光雷达SLAM退化检测系统，使用如权利要求1-4任一项所述的激光雷达SLAM退化检测方法，其特征在于，包括：

数据模块，该模块通过激光雷达传感器采集连续帧点云；

约束关系模块，该模块对所述连续帧点云进行分析，获取机器人位姿抗扰动鲁棒性的点云约束关系；

扰动识别模块，该模块根据所述点云约束关系，进行机器人位姿约束扰动识别。

6.一种终端，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述程序时可用于执行权利要求1-3中任一项所述的方法。

7.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，该程序被处理器执行时可用于执行权利要求1-3中任一项所述的方法。