CN110728288B - 一种基于三维激光点云的角点特征提取方法及其应用 - Google Patents

Abstract

本发明涉及无人驾驶机器人技术领域，具体地说是一种基于三维激光点云的角点特征提取方法及其在智能清洁机器人中的应用，该方法包括以下步骤：1)获取三维激光点云数据；2)沿着三维激光点云中单条激光线，对于每个激光点选择前后两个激光点计算该点处的角度Θ和空间连续性C；3)过滤掉角度Θ小于角度阈值或者空间连续性C小于连续性阈值的点，留下的点作为候选角点；4)最后，基于滑动窗口，在候选角点中提取所有的角度峰值点，即作为最终的角点；本发明提取方法基于三维激光点云中单条激光线上的角度变化和连续性，实现了一种稳定的激光点云角点提取，解决了以往激光角点特征提取方法在复杂环境中稳定性差的问题，提高了稳定性和精度。

Description

一种基于三维激光点云的角点特征提取方法及其应用

[技术领域]

本发明涉及无人驾驶机器人技术领域，特别涉及一种基于三维激光点云的角点特征提取方法及其在智能清洁机器人中的应用。

[背景技术]

随着机器人技术以及激光传感器的发展，基于激光传感器的建图与定位技术得到越来越广泛的应用。尤其是在安防、清洁、快递等领域，具有自主建图和定位的无人驾驶机器人给人们的生活带来了极大的方便。然而，在一些复杂场景中，基于激光点云的建图与定位技术会出现无法正常提取特征点的问题。

目前，基于激光三维点云匹配的ICP(Iterative Closest Point)算法，通过使用所有激光点或者平面度较高的点进行点到点，点到线或者点到面的匹配。其中，使用平面度较高的点进行点到面的匹配，而使用平面度较低的点进行点到线的匹配。然而，后者使用的点当中往往会包含大量特征不明显，或者一些动态障碍物导致的不稳定激光点，从而使得ICP算法失效。

[发明内容]

本发明的目的就是要解决上述的不足而提供一种基于三维激光点云的角点特征提取方法，提高了稳定性和精度，解决了以往激光角点特征提取方法在复杂环境中稳定性差的问题。

为实现上述目的设计一种基于三维激光点云的角点特征提取方法，包括以下步骤：

1)获取三维激光点云数据；

2)沿着三维激光点云中的单条激光线，对于每个激光点分别计算该点处的激光线角度和空间连续性；

3)通过阈值筛选得到候选角点；

4)最后，对所有候选角点，基于滑动窗口，在候选角点中提取所有的角度峰值点，即作为最终的角点。

进一步地，步骤2)中，每条激光线有多个激光点，沿着单条激光线，对于每个激光点选择前后两个激光点计算该点处的角度Θ和空间连续性C，角度Θ和空间连续性C的计算步骤如下：

①每条激光线有多个激光点，沿着单条激光线，对于每个当前激光点P选择前后两个激光点A、B；作为优选，选择前后两个激光点A、B时，使得激光点A、B分别与激光点P之间间隔5个以内的激光数目；

②角度Θ的计算，使用当前激光点P和前后两个激光点A、B组成的两个向量的夹角，如公式(1)所示，公式(1)中，PA和PB分别表示当前激光点P和前后两个激光点A、B组成的三维空间向量，acos为反余弦函数，

Θ＝acos(PA·PB/(|PA|×|PB|)) (1)

③空间连续性C的计算，使用当前激光点P和前后两个激光点A、B组成的两个向量的长度的比值，并选择小的长度比大的长度，使得C的取值在0到1之间，公式(2)中，设|PA|小于|PB|，

C＝|PA|/|PB| (2)。

进一步地，步骤3)中，过滤掉角度Θ小于角度阈值或者空间连续性C小于连续性阈值的点，留下的点作为候选角点，角度阈值取80度到90度之间，连续性阈值取0.5到0.8之间。

进一步地，步骤4)中，在固定大小的滑动窗口中提取角度峰值点Q，该固定大小的滑动窗口由起始位置E和结束位置F确定，其中，角度峰值点Q位于滑动窗口的正中间位置，且其角度Θ为滑动窗口中的最大值，否则不作为角度峰值点；计算完毕后，滑动窗口沿着固定方向H进行滑动，并继续尝试提取角度峰值点。

进一步地，本发明提供了一种智能清洁机器人，包括：通信接口；存储器，用于存储计算机程序；一个或多个处理器，用于执行所述存储器中存储的计算机程序，且所述计算机程序被执行时，实现上述方法。

进一步地，本发明提供了一种计算机存储介质，所述存储介质中存储计算机程序，所述计算机程序在执行时实现上述方法。

本发明同现有技术相比，通过沿着单条激光线，对于每个激光点分别计算该点处的激光线角度和空间连续性，并通过阈值筛选得到候选角点，最后对所有候选角点，使用基于滑动窗口的角点峰值提取方法提取角度的峰值点作为最终的角点特征，该提取方法基于三维激光点云中单条激光线上的角度变化和连续性，实现了一种稳定的激光点云角点提取算法，解决了以往激光角点特征提取方法在复杂环境中稳定性差的问题，提高了稳定性和精度，值得推广应用。

本申请的实施方式的附加方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本申请的实施方式的实践了解到。

[附图说明]

图1是本发明实施方式的流程示意图；

图2是本发明实施方式中角度和空间连续性的计算示意图；

图3是本发明实施方式中基于滑动窗口的角点峰值提取示意图；

图4是本发明的应用示例图。

图5是本申请某些实施方式的智能清洁机器人的模块示意图，图中，20为处理器，30为存储器，40为通信接口，100为智能清洁机器人。

[具体实施方式]

下面详细描述本申请的实施方式，所述实施方式的示例在附图中示出，其中，相同或类似的标号自始至终表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施方式是示例性的，仅用于解释本申请的实施方式，而不能理解为对本申请的实施方式的限制。

激光点云强度:激光点云照射到不同材质表面，会有不同的强度值。如：车道线上的激光点云的强度比道路上的激光点的强度值大许多。

ICP(Iterative Closest Point):一种点云到点云的优化算法，优化不同时刻的点云直接的坐标转换。

SLAM(Simultaneous Localization And Mapping)：同步定位和建图技术，主要用来解决机器人在陌生环境中构建地图以及定位的问题。

本发明基于三维激光点云中单条激光线上的角度变化和连续性，实现了一种稳定的角点提取，该方法为：沿着单条激光线，对于每个激光点分别计算该点处的激光线角度和空间连续性，并通过阈值筛选得到候选角点，最后对所有候选角点，使用基于滑动窗口的角点峰值提取方法提取角度的峰值点作为最终的角点特征。

下面结合附图对本发明实施方式作以下进一步说明：

如附图1所示，为本发明基于三维激光点云的角点提取方法的整体流程图，其步骤如下：

1.获取三维激光点云数据；

2.沿着三维激光点云中每条激光线，对于每个激光点选择前后两个激光点计算该点处的角度Θ和空间连续性C；

3.过滤掉角度Θ小于角度阈值(一般80度到90度)或者空间连续性C小于连续性阈值的点(一般取0.5到0.8之间)，留下的点作为候选角点；

4.最后，利用滑动窗口，在候选角点中提取所有的角度峰值点，即作为最终的角点。

如附图2所示，每条激光线有多个激光点(图中圆点)，沿着激光线顺序，对于每个当前激光点(图中P点)选择前后两个激光点(图中A、B点)计算角度和空间连续性。其中，前后两个激光点A、B的选择可间隔一定的激光数目(一般间隔5个以内，如附图1中为间隔了1个)。

角度Θ的计算，使用当前激光点P和前后两个激光点A、B组成的两个向量的夹角，如公式(1)所示：

Θ＝acos(PA·PB/(|PA|×|PB|)) (1)

公式(1)中，PA和PB分别表示当前激光点P和前后两个激光点A、B组成的三维空间向量，acos为反余弦函数。

空间连续性C的计算，使用当前激光点P和前后两个激光点A、B组成的两个向量的长度的比值。同时，为了使得C的取值在0到1之间，此处选择小长度比大长度的结果。如公式(2)中，假设|PA|小于|PB|：

C＝|PA|/|PB| (2)

如附图3所示，在固定大小的滑动窗口(由起始位置E和结束位置F确定)中提取角度峰值点Q。其中，角度峰值点必须位于滑动窗口的正中间位置，并且角度Θ须为滑动窗口中的最大值，否则不作为角度峰值。计算完毕后，滑动窗口沿着固定方向H进行滑动，并继续尝试提取角度峰值。

如附图4所示，为本发明的应用示例图，具体为在某地下车库的点云效果图。其中，图A为一个上坡，对应的角点提取效果如图B所示，可见墙角处的角点可以被提取到；图C所示为地下车库总体点云概况，图D对应的为墙柱上的角点的提取效果。

图5是根据本申请一个实施例的智能清洁机器人100的模块示意图。该智能清洁机器人100包括存储器30、处理器20及存储在存储器30上并可在处理器20上运行的计算机程序。

处理器20执行程序时实现上述实施例中提供的智能清洁机器人100以及角点特征提取方法。

存储器30用于存放可在处理器20上运行的计算机程序。存储器30可能包含高速RAM存储器，也可能还包括非易失性存储器(non-volatile memory)，例如至少一个磁盘存储器。

进一步地，智能清洁机器人100还可包括通信接口40，通信接口40用于存储器30和处理器20之间的通信。

如果存储器30、处理器20和通信接口40独立实现，则通信接口40、存储器30和处理器20可以通过总线相互连接并完成相互间的通信。总线可以是工业标准体系结构(Industry Standard Architecture，简称为ISA)总线、外部设备互连(PeripheralComponent，简称为PCI)总线或扩展工业标准体系结构(Extended Industry StandardArchitecture，简称为EISA)总线等。总线可以分为地址总线、数据总线、控制总线等。为便于表示，图5中仅用一条粗线表示，但并不表示仅有一根总线或一种类型的总线。

可选的，在具体实现上，如果存储器30、处理器20及通信接口40，集成在一块芯片上实现，则存储器30、处理器20及通信接口40可以通过内部接口完成相互间的通信。

处理器20可能是一个中央处理器(Central Processing Unit，简称为CPU)，或者是特定集成电路(Application Specific Integrated Circuit，简称为ASIC)，或者是被配置成实施本申请实施例的一个或多个集成电路。

本实施例还提供一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器20执行时实现如上的智能清洁机器人100的角点特征提取方法。

综上，本申请实施方式的智能清洁机器人100以及其基于三维激光点云中单条激光线上的角度变化和连续性，实现了一种稳定的激光点云角点提取，解决了以往激光角点特征提取方法在复杂环境中稳定性差的问题，提高了稳定性和精度。

流程图中或在此以其他方式描述的任何过程或方法描述可以被理解为，表示包括一个或更多个用于实现定制逻辑功能或过程的步骤的可执行指令的代码的模块、片段或部分，并且本申请的优选实施方式的范围包括另外的实现，其中可以不按所示出或讨论的顺序，包括根据所涉及的功能按基本同时的方式或按相反的顺序，来执行功能，这应被本申请的实施例所属技术领域的技术人员所理解。

在流程图中表示或在此以其他方式描述的逻辑和/或步骤，例如，可以被认为是用于实现逻辑功能的可执行指令的定序列表，可以具体实现在任何计算机可读介质中，以供指令执行系统、装置或设备(如基于计算机的系统、包括处理器的系统或其他可以从指令执行系统、装置或设备取指令并执行指令的系统)使用，或结合这些指令执行系统、装置或设备而使用。就本说明书而言，"计算机可读介质"可以是任何可以包含、存储、通信、传播或传输程序以供指令执行系统、装置或设备或结合这些指令执行系统、装置或设备而使用的装置。计算机可读介质的更具体的示例(非穷尽性列表)包括以下：具有一个或多个布线的电连接部(电子装置)，便携式计算机盘盒(磁装置)，随机存取存储器(RAM)，只读存储器(ROM)，可擦除可编辑只读存储器(EPROM或闪速存储器)，光纤装置，以及便携式光盘只读存储器(CDROM)。另外，计算机可读介质甚至可以是可在其上打印所述程序的纸或其他合适的介质，因为可以例如通过对纸或其他介质进行光学扫描，接着进行编辑、解译或必要时以其他合适方式进行处理来以电子方式获得所述程序，然后将其存储在计算机存储器中。

应当理解，本申请的各部分可以用硬件、软件、固件或它们的组合来实现。在上述实施方式中，多个步骤或方法可以用存储在存储器中且由合适的指令执行系统执行的软件或固件来实现。如，如果用硬件来实现和在另一实施方式中一样，可用本领域公知的下列技术中的任一项或他们的组合来实现：具有用于对数据信号实现逻辑功能的逻辑门电路的离散逻辑电路，具有合适的组合逻辑门电路的专用集成电路，可编程门阵列(PGA)，现场可编程门阵列(FPGA)等。

本技术领域的普通技术人员可以理解实现上述实施例方法携带的全部或部分步骤是可以通过程序来指令相关的硬件完成，所述的程序可以存储于一种计算机可读存储介质中，该程序在执行时，包括方法实施例的步骤之一或其组合。

此外，在本申请各个实施例中的各功能单元可以集成在一个处理模块中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个模块中。上述集成的模块既可以采用硬件的形式实现，也可以采用软件功能模块的形式实现。所述集成的模块如果以软件功能模块的形式实现并作为独立的产品销售或使用时，也可以存储在一个计算机可读取存储介质中。上述提到的存储介质可以是只读存储器，磁盘或光盘等。

在本说明书的描述中，参考术语“某些实施方式”、“一个实施方式”、“一些实施方式”、“示意性实施方式”、“示例”、“具体示例”、或“一些示例”的描述意指结合所述实施方式或示例描述的具体特征、结构、材料或者特点包含于本申请的至少一个实施方式或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施方式或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施方式或示例中以合适的方式结合。

此外，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括至少一个所述特征。在本申请的描述中，“多个”的含义是至少两个，例如两个，三个，除非另有明确具体的限定。

尽管上面已经示出和描述了本申请的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本申请的限制，本领域的普通技术人员在本申请的范围内可以对上述实施例进行变化、修改、替换和变型，本申请的范围由权利要求及其等同物限定。

Claims (8)

1.一种基于三维激光点云的角点特征提取方法，其特征在于，包括以下步骤：

1)获取三维激光点云数据；

2)沿着三维激光点云中的单条激光线，对于每个激光点分别计算该点处的激光线角度和空间连续性；

3)通过阈值筛选得到候选角点；

4)最后，对所有候选角点，基于滑动窗口，在候选角点中提取所有的角度峰值点，作为最终的角点；

角度Θ和空间连续性C的计算步骤如下：

①每条激光线有多个激光点，沿着单条激光线，对于每个当前激光点P选择前后两个激光点A、B；

②角度Θ的计算，使用当前激光点P和前后两个激光点A、B组成的两个向量的夹角，如公式(1)所示，公式(1)中，PA和PB分别表示当前激光点P和前后两个激光点A、B组成的三维空间向量，acos为反余弦函数，

Θ＝acos((PA·PB)/(|PA|×|PB|)) (1)

③空间连续性C的计算，使用当前激光点P和前后两个激光点A、B组成的两个向量的长度的比值，使得C的取值在0到1之间，公式(2)中，设|PA|小于|PB|，

C＝|PA|/|PB| (2)。

2.根据权利要求1所述的方法，其特征在于：步骤①中，选择前后两个激光点A、B时，使得激光点A、B分别与激光点P之间间隔5个以内的激光数目。

3.根据权利要求1所述的方法，其特征在于：步骤3)进一步为，过滤掉角度Θ小于角度阈值或者空间连续性C小于连续性阈值的点，留下的点作为候选角点。

4.根据权利要求3所述的方法，其特征在于：角度阈值取80度到90度之间，连续性阈值取0.5到0.8之间。

5.根据权利要求1所述的方法，其特征在于：步骤4)中，在固定大小的滑动窗口中提取角度峰值点Q，该固定大小的滑动窗口由起始位置E和结束位置F确定；计算完毕后，滑动窗口沿着固定方向H进行滑动，并继续尝试提取角度峰值点。

6.根据权利要求5所述的方法，其特征在于：其中，角度峰值点Q位于滑动窗口的正中间位置，且其角度Θ为滑动窗口中的最大值，否则不作为角度峰值点。

7.一种智能清洁机器人，其特征在于，包括：通信接口；存储器，用于存储计算机程序；一个或多个处理器，用于执行所述存储器中存储的计算机程序，且所述计算机程序被执行时，实现上述权利要求1-6任一所述的方法。

8.一种计算机存储介质，其特征在于，所述存储介质中存储计算机程序，所述计算机程序在执行时实现如权利要求1-6中任一项所述的方法。

Images