一种激光雷达地图构建方法和机器人自主导航方法与流程

1.本发明涉及激光雷达领域。更具体地，涉及一种激光雷达地图构建方法、机器人自主导航方法、存储介质和计算机设备。


背景技术：

2.同步定位和建图(slam)技术一直是机器人领域的一个研究热点。slam有两个基本的任务，分别是定位与建图，定位是指确定机器人的位置和方向，而建图是指对环境进行测绘。机器人位于未知环境中，通过传感器收集环境测量数据。slam算法会逐步建立起一致环境地图，同时确定机器人在地图中相对位姿。slam具有十分重要的学术价值和应用价值，它在自动驾驶、服务型机器人、无人机、ar/vr等场景有着广泛的应用。该技术被认为是机器人实现自主功能的必备技术。
3.在技术发展方向来说，激光雷达具有很好的稳定性和精度，被广泛应用于移动机器人自主导航。机器人在运动过程中通过编码器与惯性测量单元计算获得里程数据，根据机器人的运动模型计算机器人的位姿，通过激光雷达数据，建立基本地图模型，在移动过程中，更新地图，同时结合观测模型对机器人的位姿进行精确校正，得到机器人精准定位，然后把机器人位置添加到栅格地图中，反复如此，最后完成整个场景的地图构建。
4.由于使用场景中结构化障碍物占大多数，激光雷达扫描使用环境中的平直的物体，如墙面、地面、门等表面返回的点云数据，应是一条平直的点云线段。然而，实际情况中，由于误差的存在，激光返回的测量扫描数据中存在大量点云波动的现象，导致最终建立的地图中也会存在许多波动，可以清晰的看到地图中障碍物的真实轮廓附近出现大量的误差点，导致障碍物轮廓不够平直清晰，影响移动机器人的自主性能发挥。


技术实现要素：

5.有鉴于此，本发明实施例提供一种激光雷达地图构建方法、机器人自主导航方法、存储介质和计算机设备，以实现对于激光雷达地图构建算法进一步的优化，添加建图算法对直线特征的描述，减少测量的激光数据噪点对最终地图呈现效果的影响。
6.本发明实施例第一方面提供一种激光雷达地图构建方法，包括：
7.s1、采集初始雷达点云数据；
8.s2、根据所示点云数据构建初始地图；
9.s3、利于改进的霍夫变换算法对所述点云数据进行直线特征提取，得到多个点云候选区域；
10.s4、利于最小二乘法对候选区域的点云进行直线拟合，得到候选点云数据的线段表示；
11.s5、输出更清晰的地图。
12.在一个具体实施方式中，所述s3包括：
13.s31、对所述初始地图进行预处理；
14.s32、从预处理后地图的边缘点中随机抽取两个中心像素点(x1，y1)和(x2，y2)，通过交叉扫描算法在两个中心像素点间进行扫描，返回遍历次数n，并统计得到经过的实际核心像素点数量m，其中，m，n为大于等于0的整数；
15.s33、判断m/n是否大于预定值，若是，就在(x1，y1)和(x2，y2)构成的直线参数(r，θ)上累加m次，并删除刚经过的点；重复s32过程k次，并统计满足m/n大于预定值的空间以及对应的实际核心像素点数量，其中，k为大于0的整数；
16.s34、通过对k条直线参数空间进行聚类，统计聚类结果中每一类别包含的实际核心像素点总数，m
cj
＝∑
i
m
i
，其中，m
cj
为j类别中的实际核心像素点总数，m
i
为j类别中i组对应的实际核心像素点数量；
17.s35、如果类别的实际核心像素点总数m
cj
大于设定阈值，则返回该类别的所有点云。
18.在一个具体实施方式中，所述交叉扫描算法包括：
19.s321、如|x1
‑
x2|≥|y1
‑
y2|，设置初始扫描方向为正x方向，横坐标小的像素点作为起始点(x3，y3)，另外的像素点中心点(x4，y4)作为终点；如|x1
‑
x2|≤|y1
‑
y2|，设置初始扫描方向为正y方向，纵坐标小的像素点作为起始点(x3，y3)，另一个的像素点(x4，y4)作为终点；
20.s322、判断(x3，y3)和(x4，y4)是否重合，若是，则扫描过程中止，返回m和n；
21.s323、若扫描方向为正x轴，则x3＝x3+1；若扫描方向为正y轴，则y3＝y3+1；判断点(x3,y3)处是否是实际核心像素点，如果是则m＝m+1，n＝n+1；如果否，则m＝m，n＝n+1；
22.s324、判断点(x3，y3)是否在点(x1，y1)和点(x2，y2)构成直线的区间内，如果点(x3，y3)位于两条直线的区间内转至步骤s322，否则改变方向扫描一次再转至步骤s322。
23.在一个具体实施方式中，所述预处理包括图像二值化和边缘检测。
24.在一个具体实施方式中，所述s4包括：
25.将s35返回的点云映射到图像空间，依次计算第k条直线附近的点到该直线的距离，其中，k为大于0的整数；
26.将距离大于误差阈值的点排除，得到第k条直线附近的一个点集p
k
；
27.将点集p
k
作为拟合数据集，利用最小二乘法拟合出直线，以点集p
k
中横坐标最大和最小的两点作为直线的端点，以此得到拟合第k条直线段的区间。
28.本发明实施例第二方面提供一种移动机器人自主导航方法，
29.获取所述机器人里程数据和位姿；
30.通过本发明实施例第一方面提供方法构建地图；
31.计算出机器人的实际位置，与栅格地图进行匹配，实时获取精准位置和位姿；
32.根据当前位置、目标位置和四周环境进行路径规划。
33.本发明实施例第三方面提供一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时实现如本发明实施例第一方面提供的方法或者本发明实施例第二方面提供的方法。
34.本发明实施例第四方面提供一种计算设备，包括处理器，其特征在于，所述处理器执行程序时实现如本发明实施例第一方面提供的方法或者本发明实施例第二方面提供的方法。
35.本发明的有益效果如下：本发明实施例的对地图构建算法进一步的优化，添加建图算法对直线特征的描述，降低点云波动幅度过大对地图精度造成的不利影响，使得构建出的地图中障碍物的轮廓更加清晰。
附图说明
36.下面结合附图对本发明的具体实施方式作进一步详细的说明。
37.图1示出一种激光雷达地图构建方法结构图。
38.图2示出本发明实施例的原始扫描点建图和直线拟合后的建图。
39.图3示出本发明实施例的一种计算机设备结构示意图。
具体实施方式
40.为了更清楚地说明本发明，下面结合优选实施例和附图对本发明做进一步的说明。附图中相似的部件以相同的附图标记进行表示。本领域技术人员应当理解，下面所具体描述的内容是说明性的而非限制性的，不应以此限制本发明的保护范围。
41.本发明公开的一种基于直线特征的改进激光雷达地图构建算法要解决的技术问题是，通过直线特征提取拟合减小点云波动对地图构建精度带来的不利影响；具有如下优点：结构化场景下精度更高；点云波动、噪点更少，适合机器人执行规划任务；计算简单，可扩展应用到许多建图框架中。
42.本发明的原理是首先对采集的雷达点云数据进行直线特征提取；然后基于此，将直线段附近符合要求的点云移动绑定到直线上，进而减小点云波动幅度。以此，降低点云波动幅度过大对地图精度造成的不利影响，使得构建出的地图中障碍物的轮廓更加清晰。
43.利用改进霍夫变换直线检测算法结合最小二乘拟合算法实现结构化区域直线特征精确拟合，进而有效消除直线附近噪声影响，降低点云波动幅度过大对地图精度造成的不利影响，构建更清晰地图。
44.本发明第一个实施例提供一种激光雷达地图构建方法，包括：
45.s1、采集初始雷达点云数据；
46.s2、根据所示点云数据构建初始地图；
47.s3、利于改进的霍夫变换算法对所述点云数据进行直线特征提取，得到多个点云候选区域；
48.s31、对所述初始地图进行预处理，包括图像二值化和边缘检测；
49.s32、从预处理后地图的边缘点中随机抽取两个中心像素点(x1，y1)和(x2，y2)，通过交叉扫描算法在两个中心像素点间进行扫描，返回遍历次数n，并统计得到经过的实际核心像素点数量m，其中，m，n为大于等于0的整数；
50.霍夫变换(houghtransform)是一种从图像中检测或者分割几何对象的典型方法。对于直角坐标空间中的直线来说，用极坐标可以表示为：
51.r＝xcosθ+ysinθ(r≥0,0<θ≤2π)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(1)
52.式中：r为原点到直线的距离，θ为r与x轴的夹角。通过两点(x1,y1)和(x2,y2)可以联立方程求解(r,θ)。
53.所述交叉扫描算法包括：
54.s321、如|x1
‑
x2|≥|y1
‑
y2|，设置初始扫描方向为正x方向，横坐标小的像素点作为起始点(x3，y3)，另外的像素点中心点(x4，y4)作为终点；如|x1
‑
x2|≤|y1
‑
y2|，设置初始扫描方向为正y方向，纵坐标小的像素点作为起始点(x3，y3)，另一个的像素点(x4，y4)作为终点；
55.s322、判断(x3，y3)和(x4，y4)是否重合，若是，则扫描过程中止，返回m和n；
56.s323、若扫描方向为正x轴，则x3＝x3+1；若扫描方向为正y轴，则y3＝y3+1；判断点(x3,y3)处是否是实际核心像素点，如果是则m＝m+1，n＝n+1；如果否，则m＝m，n＝n+1；
57.s324、判断点(x3，y3)是否在点(x1，y1)和点(x2，y2)构成直线的区间内，如果点(x3，y3)位于两条直线的区间内转至步骤s322，否则改变方向扫描一次再转至步骤s322。
58.s33、判断m/n是否大于预定值，若是，就在(x1，y1)和(x2，y2)构成的直线参数(r,θ)上累加m次，并删除刚经过的点；重复s32过程k次，并统计满足m/n大于预定值的空间以及对应的实际核心像素点数量，其中，k为大于0的整数；本示例中，预定值为0.5
59.s34、通过对k条直线参数空间进行聚类，统计聚类结果中每一类别包含的实际核心像素点总数，m
cj
＝∑
i
m
i
，其中，m
cj
为j类别中的实际核心像素点总数，m
i
为j类别中i组对应的实际核心像素点数量；也就是(r
j
,θ
j
)的实际核心像素点总数为m
cj
。
60.s35、如果类别的实际核心像素点总数m
cj
大于设定阈值，则返回该类别的所有点云。本示例中，设定阈值40。
61.s4、利于最小二乘法对候选区域的点云进行直线拟合，得到候选点云数据的线段表示；
62.最小二乘法的核心思想是通过最小化模型和实际数据的误差平方和，寻找与真实数据的最佳匹配模型。最小二乘拟合直线，主要思想是在相关数据中找到一条直线，使得所有已知的点到该直线的误差平方和最小。
63.具体步骤为：
64.将s35返回的点云映射到图像空间，依次计算第k条直线附近的点到该直线的距离，其中，k为大于0的整数；
65.将式(1)的直线改写成斜截式表示为式(2)：
66.y＝kx+b
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(2)
67.式中：
[0068][0069]
直线外一点到直线的距离为：
[0070][0071]
将距离大于误差阈值d
e
的点排除，得到第k条直线附近的一个点集p
k
；
[0072]
将点集p
k
作为拟合数据集，利用最小二乘法拟合出直线，以点集p
k
中横坐标最大和最小的两点作为直线的端点，以此得到拟合第k条直线段的区间。
[0073]
s5、输出更清晰的地图。
[0074]
本发明实施例的对地图构建算法进一步的优化，添加建图算法对直线特征的描述，降低点云波动幅度过大对地图精度造成的不利影响，使得构建出的地图中障碍物的轮
廓更加清晰。
[0075]
为了验证本实施例的方法，本发明使用gazebo仿真平台搭建实验环境。将激光雷达对平坦的墙壁进行扫描来获取激光雷达数据，并将处理后的数据进行可视化。如图2所示，明显观察到原始扫描点存在较大的点云波动现象，点云的波动将会对建图的效果造成不利的影响。通过直线拟合后，能明显的降低点云波动的幅度，并提升建图的效果。
[0076]
采用改进前的点云构建的地图中障碍物的边缘较厚，且在正确的障碍物轮廓边缘，存在着许多不规则的杂乱点。这些杂乱点影响了物流机器人的地图精度。采用改进后的点云数据建图，地图中障碍物轮廓边缘相对改进前更平滑且清晰，有效的改善了点云的波动对物流机器人地图构建的不利影响。从整体建图效果来看，采用直线特征提取方式来减小点云数据波动幅度的方法，能有效的提升地图的构建的精度。
[0077]
本发明第二个实施例提供一种移动机器人自主导航方法，
[0078]
获取所述机器人里程数据和位姿；
[0079]
通过本发明实施例第一方面提供方法构建地图；
[0080]
计算出机器人的实际位置，与栅格地图进行匹配，实时获取精准位置和位姿；
[0081]
根据当前位置、目标位置和四周环境进行路径规划。
[0082]
本发明第三个实施例提供一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时实现本发明第一个或第二个实施例所述的方法。
[0083]
在实际应用中，所述计算机可读存储介质可以采用一个或多个计算机可读的介质的任意组合。计算机可读介质可以是计算机可读信号介质或者计算机可读存储介质。计算机可读存储介质例如可以是但不限于电、磁、光、电磁、红外线、或半导体的系统、装置或器件，或者任意以上的组合。计算机可读存储介质的更具体的例子(非穷举的列表)包括：具有一个或多个导线的电连接、便携式计算机磁盘、硬盘、随机存取存储器(ram)、只读存储器(rom)、可擦式可编程只读存储器(eprom或闪存)、光纤、便携式紧凑磁盘只读存储器(cd
‑
rom)、光存储器件、磁存储器件、或者上述的任意合适的组合。在本实施例中，计算机可读存储介质可以是任何包含或存储程序的有形介质，该程序可以被指令执行系统、装置或者器件使用或者与其结合使用。
[0084]
计算机可读的信号介质可以包括在基带中或者作为载波一部分传播的数据信号，其中承载了计算机可读的程序代码。这种传播的数据信号可以采用多种形式，包括但不限于电磁信号、光信号或上述的任意合适的组合。计算机可读的信号介质还可以是计算机可读存储介质以外的任何计算机可读介质，该计算机可读介质可以发送、传播或者传输用于由指令执行系统、装置或者器件使用或者与其结合使用的程序。
[0085]
计算机可读介质上包含的程序代码可以用任何适当的介质传输，包括但不限于无线、电线、光缆、rf等等，或者上述的任意合适的组合。
[0086]
可以以一种或多种程序设计语言或其组合来编写用于执行本发明操作的计算机程序代码，所述程序设计语言包括面向对象的程序设计语言—诸如java、smalltalk、c++，还包括常规的过程式程序设计语言—诸如“c”语言或类似的程序设计语言。程序代码可以完全地在用户计算机上执行、部分地在用户计算机上执行、作为一个独立的软件包执行、部分在用户计算机上部分在远程计算机上执行、或者完全在远程计算机或服务器上执行。在涉及远程计算机的情形中，远程计算机可以通过任意种类的网络——包括局域网(lan)或
广域网(wan)—连接到用户计算机，或者，可以连接到外部计算机(例如利用因特网服务提供商来通过因特网连接)。
[0087]
如图3所示，本发明的第四个实施例提供个实施例提供的一种计算机设备的结构示意图。图3显示的计算机设备12仅仅是一个示例，不应对本发明实施例的功能和使用范围带来任何限制。
[0088]
如图3所示，计算机设备12以通用计算设备的形式表现。计算机设备12的组件可以包括但不限于：一个或者多个处理器或者处理单元16，系统存储器28，连接不同系统组件(包括系统存储器28和处理单元16)的总线18。
[0089]
总线18表示几类总线结构中的一种或多种，包括存储器总线或者存储器控制器，外围总线，图形加速端口，处理器或者使用多种总线结构中的任意总线结构的局域总线。举例来说，这些体系结构包括但不限于工业标准体系结构(isa)总线，微通道体系结构(mac)总线，增强型isa总线、视频电子标准协会(vesa)局域总线以及外围组件互连(pci)总线。
[0090]
计算机设备12典型地包括多种计算机系统可读介质。这些介质可以是任何能够被计算机设备12访问的可用介质，包括易失性和非易失性介质，可移动的和不可移动的介质。
[0091]
系统存储器28可以包括易失性存储器形式的计算机系统可读介质，例如随机存取存储器(ram)30和/或高速缓存存储器32。计算机设备12可以进一步包括其它可移动/不可移动的、易失性/非易失性计算机系统存储介质。仅作为举例，存储系统34可以用于读写不可移动的、非易失性磁介质(图3未显示，通常称为“硬盘驱动器”)。尽管图3中未示出，可以提供用于对可移动非易失性磁盘(例如“软盘”)读写的磁盘驱动器，以及对可移动非易失性光盘(例如cd
‑
rom，dvd
‑
rom或者其它光介质)读写的光盘驱动器。在这些情况下，每个驱动器可以通过一个或者多个数据介质接口与总线18相连。存储器28可以包括至少一个程序产品，该程序产品具有一组(例如至少一个)程序模块，这些程序模块被配置以执行本发明各实施例的功能。
[0092]
具有一组(至少一个)程序模块42的程序/实用工具40，可以存储在例如存储器28中，这样的程序模块42包括但不限于操作系统、一个或者多个应用程序、其它程序模块以及程序数据，这些示例中的每一个或某种组合中可能包括网络环境的实现。程序模块42通常执行本发明所描述的实施例中的功能和/或方法。
[0093]
计算机设备12也可以与一个或多个外部设备14(例如键盘、指向设备、显示器24等)通信，还可与一个或者多个使得用户能与该计算机设备12交互的设备通信，和/或与使得该计算机设备12能与一个或多个其它计算设备进行通信的任何设备(例如网卡，调制解调器等等)通信。这种通信可以通过输入/输出(i/o)接口22进行。并且，计算机设备12还可以通过网络适配器20与一个或者多个网络(例如局域网(lan)，广域网(wan)和/或公共网络，例如因特网)通信。如图3所示，网络适配器20通过总线18与计算机设备12的其它模块通信。应当明白，尽管图3中未示出，可以结合计算机设备12使用其它硬件和/或软件模块，包括但不限于：微代码、设备驱动器、冗余处理单元、外部磁盘驱动阵列、raid系统、磁带驱动器以及数据备份存储系统等。
[0094]
处理器单元16通过运行存储在系统存储器28中的程序，从而执行各种功能应用以及数据处理，例如实现本发明第一个或第二个实施例所提供方法。
[0095]
显然，本发明的上述实施例仅仅是为清楚地说明本发明所作的举例，而并非是对
本发明的实施方式的限定，对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动，这里无法对所有的实施方式予以穷举，凡是属于本发明的技术方案所引伸出的显而易见的变化或变动仍处于本发明的保护范围之列。