一种基于点云语义激光SLAM回环检测方法、装置及机器人与流程

一种基于点云语义激光slam回环检测方法、装置及机器人
技术领域
1.本发明涉及机器人技术领域，具体来说，涉及一种基于点云语义激光slam回环检测方法、装置及机器人。


背景技术：

2.巡逻机器人需要在工作场景内进行自主巡逻，需要预先通过激光slam(simultaneous localization and mapping)技术对工作场景构建3d点云地图。由于由于激光雷达精度高，并且相比相机能实现全天候工作，基于lidar的slam系统比其他系统更准确和稳定。
3.巡逻机器人的前端激光里程计会带来不可避免的漂移误差，因此，需要通过回环检测技术来优化扫描匹配里程计的状态估计误差。其中回环检测主要是用来判断机器人之前是否到过当前位置a，如果能知道机器人是之前时刻t到过位置a，则当前时刻与时刻t可以认为机器人的位置是“相近”的，可以基于此来约束误差。
4.gps常常用来当作辅助精准定位的传感器，但是在某些条件下，比如城市中林立的高楼大厦就会造成信号遮挡，gps信号的度量误差可以达到10m级，难以满足回环检测检测的效果。
5.相机的回环检测技术虽然比较成熟，但是相机的性能受环境影响较大，如有较强的光照变化或者比较大的视角变化，相机就难以正确检测出回环。
6.激光雷达传感器就没有这些缺点，很适合完成slam和回环检测任务。但是目前存在的激光雷达回环检测算法无论是在检测回环的效果上难以满足实际使用需求。
7.本文提供的背景描述用于总体上呈现本公开的上下文的目的。除非本文另外指示，在该章节中描述的资料不是该申请的权利要求的现有技术并且不要通过包括在该章节内来承认其成为现有技术。


技术实现要素：

8.针对相关技术中的上述技术问题，本发明提出一种基于点云语义激光slam回环检测方法，其包括如下步骤：
9.s1，获取机器人行走过程中的任意两帧激光雷达点云扫描数据，记为第一帧scan_i，第二帧scan_j；
10.s2，将所述第一帧scan_i，第二帧scan_j分别输入到语义分割网络中，对scan_i、scan_j进行点云语义分割，并对其中语义块添加对应标签；
11.s3，将所述第一帧scan_i，第二帧scan_j分别按照点的水平扫描距离dis_xy大小分成m数量份，并分别使用一第一帧语义向量a描述所述第一帧scan_i的语义信息，第二帧语义向量b描述所述第二帧scan_j的语义信息；其中所述第一帧语义向量a、第二帧语义向量b均是一m*n维向量；
12.s4，第一帧语义向量a、第二帧语义向量b做1-范数差；获取所述1-范数差小于一预
设值的帧，作为回环候选帧；
13.s5，将所述回环候选帧作为参数输入配准算法中，用于构建回环约束。
14.具体的，所述步骤s2使用pointnet 网络对所述激光帧进行语义分割。
15.具体的，所述步骤s3中的m＝4，n＝12，使用一个12维的列向量[x1，x2，
…
x
12
]分别来表示小汽车类、货车类、杆状类、墙类、不规则石头类状、圆形类、桌子类、椅子类、亭子类、垃圾桶类、指示牌类、楼梯类，其中xi＝1表示有此语义，xi＝0表示无此语义。
[0016]
具体的，所述步骤s5包括，将激光扫描帧scan_i，scan_j，当作目标帧target，源帧source输入配准算法中，获取两帧的配准位姿，构建回环约束。
[0017]
具体的，所述配准算法是icp点云配准算法。
[0018]
第二方面，本发明的另一个实施例公开了一种基于点云语义激光slam回环检测装置，其包括如下单元：
[0019]
激光雷达数据获取单元，用于获取机器人行走过程中的任意两帧激光雷达点云扫描数据，记为第一帧scan_i，第二帧scan_j；
[0020]
语义获取单元，用于将所述第一帧scan_i，第二帧scan_j分别输入到语义分割网络中，对scan_i、scan_j进行点云语义分割，并对其中语义块添加对应标签；
[0021]
语义帧获取单元，用于将所述第一帧scan_i，第二帧scan_j分别按照点的水平扫描距离dis_xy大小分成m数量份，并分别使用一第一帧语义向量a描述所述第一帧scan_i的语义信息，第二帧语义向量b描述所述第二帧scan_j的语义信息；其中所述第一帧语义向量a、第二帧语义向量b均是一m*n维向量；
[0022]
回环候选帧获取单元，用于第一帧语义向量a、第二帧语义向量b做1-范数差；获取所述1-范数差小于一预设值的帧，作为回环候选帧；
[0023]
回环约束构建单元，用于将所述回环候选帧作为参数输入配准算法中，用于构建回环约束。
[0024]
具体的，所述语义获取单元使用pointnet 网络对所述激光帧进行语义分割。
[0025]
具体的，所述语义帧获取单元中的m＝4，n＝12，使用一个12维的列向量[x1，x2，
…
x
12
]分别来表示小汽车类、货车类、杆状类、墙类、不规则石头类状、圆形类、桌子类、椅子类、亭子类、垃圾桶类、指示牌类、楼梯类，其中xi＝1表示有此语义，xi＝0表示无此语义。
[0026]
具体的，所述回环约束构建单元包括，将激光扫描帧scan_i，scan_j，当作目标帧target，源帧source输入icp点云配准算法中，获取两帧的配准位姿，构建回环约束。
[0027]
第三方面，本发明的另一个实施例公开了一种机器人，所述机器人包括一中央处理器，一存储器，一激光雷达，所述存储器上存储有指令，所述处理器在执行所述指令时用以实现上述的基于点云语义激光slam回环检测方法。
[0028]
第四方面，本发明的另一个实施例公开了一种非易失性存储器，所述存储器上存储有指令，所述处理器在执行所述指令时用以实现上述的基于点云语义激光slam回环检测方法。
[0029]
本发明使用点云的语义分割算法将激光雷达获取的点云数据进行分割并赋予语义信息，并进而对激光雷达的探测距离进行4等分，使用一个列向量描述每一个等分后内的语义信息，并基于语义信息进行1-范数计算，从而可以大大减少对于两帧图像中的计算量，特别是通过对激光雷达的探测距离进行划分，可以在保证基本对激光点云中的语义进行描
述的情况下，进而减少使用全部语义描述带来的向量的大小不确定性，使得计算复杂度增加，从而满足实时性的要求。本实施例通过对激光点云进行4等份划分，可以在不同的探测距离上分别描述是否有对应的语义对象出现，即一个语义对象最多出现4次，并且也是和距离有光，一方面利用了位置信息，另一方面也减少了因语义对象过多时，例如杆状类过多时，带来的语义对象过多的问题，减少了计算复杂度。
附图说明
[0030]
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
[0031]
图1是本发明实施例提供的一种基于点云语义激光slam回环检测方法流程图；
[0032]
图2是本发明实施例提供的一种基于点云语义激光slam回环检测装置示意图；
[0033]
图3是本发明实施例提供的一种基于点云语义激光slam回环检测设备示意图。
具体实施方式
[0034]
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员所获得的所有其他实施例，都属于本发明保护的范围。
[0035]
实施例一
[0036]
参考图1，本实施例公开了一种基于点云语义激光slam回环检测方法，其包括如下步骤：
[0037]
s1，获取机器人行走过程中的任意两帧激光雷达点云扫描数据，记为第一帧scan_i，第二帧scan_j；
[0038]
本实施例的机器人配置有激光雷达，机器人在进行工作时，可以根据激光雷达进行自主导航。具体的，本实施例的机器人在进行巡逻时，可以自主的按照规划的路线进行行走，也可以手动控制机器人行走例如接收控制人员的控制指令，优选的本实施例的机器人一般是按照规划好的路线进行自主导航。
[0039]
s2，将所述第一帧scan_i，第二帧scan_j分别输入到语义分割网络中，对scan_i、scan_j进行点云语义分割，并对其中语义块添加对应标签；
[0040]
本实施例使用pointnet 网络对所述激光帧进行语义分割。
[0041]
本实施例的机器人主要运行在户外，本实施例根据户外的实际情况，将激光帧的物体分别以：小汽车类、货车类、杆状类、墙类、不规则石头类状、圆形类、桌子类、椅子类、亭子类、垃圾桶类、指示牌类、楼梯类等12类进行分割，并添加对应标签。
[0042]
s3，将所述第一帧scan_i，第二帧scan_j分别按照点的水平扫描距离dis_xy大小分成m数量份，并分别使用一第一帧语义向量a描述所述第一帧scan_i的语义信息，第二帧语义向量b描述所述第二帧scan_j的语义信息；其中所述第一帧语义向量a、第二帧语义向量b均是一m*n维向量；
[0043]
本实施例按照点的水平扫描距离dis_xy大小分成4份，并分别用4个12维列向量，来描述scan_i的语义信息，列向量每一个元素代表一个语义，元素数字1或0代表语义块是否有这个语义块；
[0044]
本实施例使用一个12维的列向量[x1，x2，
…
x
12
]分别来表示小汽车类、货车类、杆状类、墙类、不规则石头类状、圆形类、桌子类、椅子类、亭子类、垃圾桶类、指示牌类、楼梯类，其中x1＝1表示有小汽车，x1＝0表示没有小汽车，其他类似。
[0045]
点的水平扫描距离dis_xy：
[0046][0047]
其中p代表是一个3d点，激光扫描返回点云，点云包括多个3d点，每一个3d点用一个三维坐标来表示(x y z)。p.x代表这个点三维坐标x方向的大小；p.y代表这个点三维坐标y方向的大小。
[0048]
3d激光雷达的最大扫描距离一般大于100m，但对于当距离越来越远时，其精度开始下降，本实施例的机器人一般获取其前70m即可以满足导航的需要，因此本实施例取3d激光雷达的探测距离70米进行说明，以15m为距离进行划分，最后一个划分区间为25米。具体的，本实施的具体划分区间为：
[0049]
第一份dis_xy《＝15m
[0050]
第二份15m《dis_xy《＝30m
[0051]
第三份30m《dis_xy《＝45m
[0052]
第四份45m《dis_xy《70m
[0053]
s4，第一帧语义向量a、第二帧语义向量b做1-范数差；获取所述1-范数差小于一预设值的帧，作为回环候选帧；
[0054]
dist＝|a-b|
[0055]
dist越小，代表当前机器人越有可能在同一个位置，当dist《10，我们认为可能发生回环，也就是回环候选帧；
[0056]
s5，将所述回环候选帧作为参数输入配准算法中，用于构建回环约束；
[0057]
在步骤s4中，如果认为发生了回环，则把激光扫描帧scan_i，scan_j，当作target，source输入配准算法中；
[0058]
配准初值设为0，利用icp点云配准算法进行target和source配准，获取两帧的配准位姿，构建回环约束
[0059]
err(pose_i,pose_j)＝‖pose_i-pose_j-pose_ij‖2[0060]
本实施例使用点云的语义分割算法将激光雷达获取的点云数据进行分割并赋予语义信息，并进而对激光雷达的探测距离进行4等分，使用一个列向量描述每一个等分后内的语义信息，并基于语义信息进行1-范数计算，从而可以大大减少对于两帧图像中的计算量，特别是通过对激光雷达的探测距离进行划分，可以在保证基本对激光点云中的语义进行描述的情况下，进而减少使用全部语义描述带来的向量的大小不确定性，使得计算复杂度增加，从而满足实时性的要求。本实施例通过对激光点云进行4等份划分，可以在不同的探测距离上分别描述是否有对应的语义对象出现，即一个语义对象最多出现4次，并且也是和距离有光，一方面利用了位置信息，另一方面也减少了因语义对象过多时，例如杆状类过多时，带来的语义对象过多的问题，减少了计算复杂度。
[0061]
实施例二
[0062]
参考图2，本实施例公开了一种基于点云语义激光slam回环检测装置，其包括如下单元：
[0063]
激光雷达数据获取单元，用于获取机器人行走过程中的任意两帧激光雷达点云扫描数据，记为第一帧scan_i，第二帧scan_j；
[0064]
本实施例的机器人配置有激光雷达，机器人在进行工作时，可以根据激光雷达进行自主导航。具体的，本实施例的机器人在进行巡逻时，可以自主的按照规划的路线进行行走，也可以手动控制机器人行走例如接收控制人员的控制指令，优选的本实施例的机器人一般是按照规划好的路线进行自主导航。
[0065]
语义获取单元，用于将所述第一帧scan_i，第二帧scan_j分别输入到语义分割网络中，对scan_i、scan_j进行点云语义分割，并对其中语义块添加对应标签；
[0066]
本实施例使用pointnet 网络对所述激光帧进行语义分割。
[0067]
本实施例的机器人主要运行在户外，本实施例根据户外的实际情况，将激光帧的物体分别以：小汽车类、货车类、杆状类、墙类、不规则石头类状、圆形类、桌子类、椅子类、亭子类、垃圾桶类、指示牌类、楼梯类等12类进行分割，并添加对应标签。
[0068]
语义帧获取单元，用于将所述第一帧scan_i，第二帧scan_j分别按照点的水平扫描距离dis_xy大小分成m数量份，并分别使用一第一帧语义向量a描述所述第一帧scan_i的语义信息，第二帧语义向量b描述所述第二帧scan_j的语义信息；其中所述第一帧语义向量a、第二帧语义向量b均是一m*n维向量；
[0069]
本实施例按照点的水平扫描距离dis_xy大小分成4份，并分别用4个12维列向量，来描述scan_i的语义信息，列向量每一个元素代表一个语义，元素数字1或0代表语义块是否有这个语义块；
[0070]
本实施例使用一个12维的列向量[x1，x2，
…
x
12
]分别来表示小汽车类、货车类、杆状类、墙类、不规则石头类状、圆形类、桌子类、椅子类、亭子类、垃圾桶类、指示牌类、楼梯类，其中x1＝1表示有小汽车，x1＝0表示没有小汽车，其他类似。
[0071]
点的水平扫描距离dis_xy：
[0072][0073]
其中p代表是一个3d点，激光扫描返回点云，点云包括多个3d点，每一个3d点用一个三维坐标来表示(x y z)。p.x代表这个点三维坐标x方向的大小；p.y代表这个点三维坐标y方向的大小。
[0074]
3d激光雷达的最大扫描距离一般大于100m，但对于当距离越来越远时，其精度开始下降，本实施例的机器人一般获取其前70m即可以满足导航的需要，因此本实施例取3d激光雷达的探测距离70米进行说明，以15m为距离进行划分，最后一个划分区间为25米。具体的，本实施的具体划分区间为：
[0075]
第一份dis_xy《＝15m
[0076]
第二份15m《dis_xy《＝30m
[0077]
第三份30m《dis_xy《＝45m
[0078]
第四份45m《dis_xy《70m
[0079]
回环候选帧获取单元，用于对第一帧语义向量a、第二帧语义向量b做1-范数差；获
取所述1-范数差小于一预设值的帧，作为回环候选帧；
[0080]
dist＝|a-b|
[0081]
dist越小，代表当前机器人越有可能在同一个位置，当dist《10，我们认为可能发生回环，也就是回环候选帧；
[0082]
回环约束构建单元，用于将所述回环候选帧作为参数输入配准算法中，用于构建回环约束；
[0083]
在回环候选帧获取单元中，如果认为发生了回环，则把激光扫描帧scan_i，scan_j，当作target，source输入配准算法中；
[0084]
配准初值设为0，利用icp点云配准算法进行target和source配准，获取两帧的配准位姿，构建回环约束
[0085]
err(pose_i,pose_j)＝‖pose_i-pose_j-pose_ij‖2[0086]
本实施例使用点云的语义分割算法将激光雷达获取的点云数据进行分割并赋予语义信息，并进而对激光雷达的探测距离进行4等分，使用一个列向量描述每一个等分后内的语义信息，并基于语义信息进行1-范数计算，从而可以大大减少对于两帧图像中的计算量，特别是通过对激光雷达的探测距离进行划分，可以在保证基本对激光点云中的语义进行描述的情况下，进而减少使用全部语义描述带来的向量的大小不确定性，使得计算复杂度增加，从而满足实时性的要求。本实施例通过对激光点云进行4等份划分，可以在不同的探测距离上分别描述是否有对应的语义对象出现，即一个语义对象最多出现4次，并且也是和距离有光，一方面利用了位置信息，另一方面也减少了因语义对象过多时，例如杆状类过多时，带来的语义对象过多的问题，减少了计算复杂度。
[0087]
实施例三
[0088]
参考图3，图3是本实施例的一种基于点云语义激光slam回环检测设备的结构示意图。该实施例的基于点云语义激光slam回环检测设备20包括处理器21、存储器22以及存储在所述存储器22中并可在所述处理器21上运行的计算机程序。所述处理器21执行所述计算机程序时实现上述方法实施例中的步骤。或者，所述处理器21执行所述计算机程序时实现上述各装置实施例中各模块/单元的功能。
[0089]
示例性的，所述计算机程序可以被分割成一个或多个模块/单元，所述一个或者多个模块/单元被存储在所述存储器22中，并由所述处理器21执行，以完成本发明。所述一个或多个模块/单元可以是能够完成特定功能的一系列计算机程序指令段，该指令段用于描述所述计算机程序在所述基于点云语义激光slam回环检测设备20中的执行过程。例如，所述计算机程序可以被分割成实施例二中的各个模块,各模块具体功能请参考上述实施例所述的装置的工作过程，在此不再赘述。
[0090]
所述基于点云语义激光slam回环检测设备20可包括，但不仅限于，处理器21、存储器22。本领域技术人员可以理解，所述示意图仅仅是基于点云语义激光slam回环检测设备20的示例，并不构成对基于点云语义激光slam回环检测设备20的限定，可以包括比图示更多或更少的部件，或者组合某些部件，或者不同的部件，例如所述基于点云语义激光slam回环检测设备20还可以包括输入输出设备、网络接入设备、总线等。
[0091]
所述处理器21可以是中央处理单元(central processing unit，cpu)，还可以是其他通用处理器、数字信号处理器(digital signal processor，dsp)、专用集成电路
(application specific integrated circuit，asic)、现成可编程门阵列(field-programmable gate array，fpga)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等，所述处理器21是所述基于点云语义激光slam回环检测设备20的控制中心，利用各种接口和线路连接整个基于点云语义激光slam回环检测设备20的各个部分。
[0092]
所述存储器22可用于存储所述计算机程序和/或模块，所述处理器21通过运行或执行存储在所述存储器22内的计算机程序和/或模块，以及调用存储在存储器22内的数据，实现所述基于点云语义激光slam回环检测设备20的各种功能。所述存储器22可主要包括存储程序区和存储数据区，其中，存储程序区可存储操作系统、至少一个功能所需的应用程序(比如声音播放功能、图像播放功能等)等；存储数据区可存储根据手机的使用所创建的数据(比如音频数据、电话本等)等。此外，存储器22可以包括高速随机存取存储器，还可以包括非易失性存储器，例如硬盘、内存、插接式硬盘，智能存储卡(smart media card,smc)，安全数字(secure digital,sd)卡，闪存卡(flash card)、至少一个磁盘存储器件、闪存器件、或其他易失性固态存储器件。
[0093]
其中，所述基于点云语义激光slam回环检测设备20集成的模块/单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明实现上述实施例方法中的全部或部分流程，也可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一计算机可读存储介质中，该计算机程序在被处理器21执行时，可实现上述各个方法实施例的步骤。其中，所述计算机程序包括计算机程序代码，所述计算机程序代码可以为源代码形式、对象代码形式、可执行文件或某些中间形式等。所述计算机可读介质可以包括：能够携带所述计算机程序代码的任何实体或装置、记录介质、u盘、移动硬盘、磁碟、光盘、计算机存储器、只读存储器(rom，read-only memory)、随机存取存储器(ram，random access memory)、电载波信号、电信信号以及软件分发介质等。需要说明的是，所述计算机可读介质包含的内容可以根据司法管辖区内立法和专利实践的要求进行适当的增减，例如在某些司法管辖区，根据立法和专利实践，计算机可读介质不包括电载波信号和电信信号。
[0094]
需说明的是，以上所描述的装置实施例仅仅是示意性的，其中所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部模块来实现本实施例方案的目的。另外，本发明提供的装置实施例附图中，模块之间的连接关系表示它们之间具有通信连接，具体可以实现为一条或多条通信总线或信号线。本领域普通技术人员在不付出创造性劳动的情况下，即可以理解并实施。
[0095]
以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。