CN111679261B - 一种基于反光板的激光雷达定位方法及系统 - Google Patents

Abstract

本发明涉及一种基于反光板的激光雷达定位方法及系统。该方法包括：建立基于反光板定位的地图数据集；获取激光雷达数据集；对激光雷达数据集进行运动学防畸变处理；对防畸变处理后的数据集进行分组处理；对分组处理后的数据集所包含的每一组子数据集分别进行拟合得到反光板圆心位置集；根据反光板圆心位置集和地图数据集，得到第一匹配数据集；从第一匹配数据集中选取第一反光板位置点和第二反光板位置点；对第一反光板位置点进行校正；根据校正后第一反光板位置点和第二反光板位置点，得到第二匹配数据集；根据第二匹配数据集，确定校正反光板位置后的定位位姿。本发明能够得到稳定的定位位姿。

Description

一种基于反光板的激光雷达定位方法及系统

技术领域

本发明涉及反光板定位领域，特别是涉及一种基于反光板的激光雷达定位方法及系统。

背景技术

近年来随着科学技术的进步和科技水平的提高，各行各业对移动机器人的需求与日俱增。在工厂的日常生产中需要高定位精度，高抗干扰能力的移动机器人，基于反光板的定位精度相对于基于环境特征的定位方式更具有高精度，高抗干扰的性能，现实生产中，移动机器人通过激光雷达借助周边布置的反光板可以很精确的定位，但由于激光雷达角分辨率的限制，由于远距离的反光板上扫描点数量少、工厂工作环境限制而使反光板布置边缘化等等不利因素，导致没有足够的位置“可靠”的反光板来进行定位。这通常会导致定位精度下降，零位漂移等等现象的发生，不利于需要高精度的工位对接。

发明内容

本发明的目的是提供一种基于反光板的激光雷达定位方法及系统，能够得到稳定的定位位姿。

为实现上述目的，本发明提供了如下方案：

一种基于反光板的激光雷达定位方法，包括：

通过三边定位的方法，建立基于反光板定位的地图数据集；

通过数据接口，获取激光雷达数据集；

对所述激光雷达数据集进行运动学防畸变处理，得到防畸变数据集；

对所述防畸变数据集进行分组处理，得到分组数据集；

对所述分组数据集所包含的每一组子数据集分别进行拟合，得到反光板圆心位置集；

根据所述反光板圆心位置集和所述地图数据集，得到第一匹配数据集；

从所述第一匹配数据集中选取拟合圆心过后的反光板位置点距离大于设定距离阈值的反光板位置点，记为第一反光板位置点；

从所述第一匹配数据集中选取拟合圆心过后的反光板位置点距离小于或等于设定距离阈值的反光板位置点，记为第二反光板位置点；

对所述第一反光板位置点进行校正，得到校正后第一反光板位置点；

根据所述校正后第一反光板位置点和所述第二反光板位置点，得到第二匹配数据集；

根据所述第二匹配数据集，确定校正反光板位置后的定位位姿。

可选地，所述对所述激光雷达数据集进行运动学防畸变处理，得到防畸变数据集，具体包括：

利用运动学模型对所述激光雷达数据集进行运动学防畸变处理，得到防畸变数据集。

可选地，所述根据所述反光板圆心位置集和所述地图数据集，得到第一匹配数据集，具体包括：

将所述反光板圆心位置集和所述地图数据集通过三角形匹配方法建立对应关系，得到第一匹配数据集。

可选地，所述根据所述第二匹配数据集，确定校正反光板位置后的定位位姿，具体包括：

根据所述第二匹配数据集采用三边定理的定位算法，确定校正反光板位置后的定位位姿。

一种基于反光板的激光雷达定位系统，包括：

地图数据集建立模块，用于通过三边定位的方法，建立基于反光板定位的地图数据集；

激光雷达数据集获取模块，用于通过数据接口，获取激光雷达数据集；

防畸变处理模块，用于对所述激光雷达数据集进行运动学防畸变处理，得到防畸变数据集；

分组处理模块，用于对所述防畸变数据集进行分组处理，得到分组数据集；

反光板圆心位置集确定模块，用于对所述分组数据集所包含的每一组子数据集分别进行拟合，得到反光板圆心位置集；

第一匹配数据集确定模块，用于根据所述反光板圆心位置集和所述地图数据集，得到第一匹配数据集；

第一反光板位置点确定模块，用于从所述第一匹配数据集中选取拟合圆心过后的反光板位置点距离大于设定距离阈值的反光板位置点，记为第一反光板位置点；

第二反光板位置点确定模块，用于从所述第一匹配数据集中选取拟合圆心过后的反光板位置点距离小于或等于设定距离阈值的反光板位置点，记为第二反光板位置点；

第一反光板位置点校正模块，用于对所述第一反光板位置点进行校正，得到校正后第一反光板位置点；

第二匹配数据集确定模块，用于根据所述校正后第一反光板位置点和所述第二反光板位置点，得到第二匹配数据集；

定位位姿确定模块，用于根据所述第二匹配数据集，确定校正反光板位置后的定位位姿。

可选地，所述防畸变处理模块，具体包括：

防畸变处理单元，用于利用运动学模型对所述激光雷达数据集进行运动学防畸变处理，得到防畸变数据集。

可选地，所述第一匹配数据集确定模块，具体包括：

第一匹配数据集确定单元，用于将所述反光板圆心位置集和所述地图数据集通过三角形匹配方法建立对应关系，得到第一匹配数据集。

可选地，所述定位位姿确定模块，具体包括：

定位位姿确定单元，用于根据所述第二匹配数据集采用三边定理的定位算法，确定校正反光板位置后的定位位姿。

根据本发明提供的具体实施例，本发明公开了以下技术效果：

本发明提供一种基于反光板的激光雷达定位方法，通过三边定位的方法，建立基于反光板定位的地图数据集；通过数据接口，获取激光雷达数据集；对所述激光雷达数据集进行运动学防畸变处理，得到防畸变数据集；对所述防畸变数据集进行分组处理，得到分组数据集；对所述分组数据集所包含的每一组子数据集分别进行拟合，得到反光板圆心位置集；根据所述反光板圆心位置集和所述地图数据集，得到第一匹配数据集；从所述第一匹配数据集中选取拟合圆心过后的反光板位置点距离大于设定距离阈值的反光板位置点，记为第一反光板位置点；从所述第一匹配数据集中选取拟合圆心过后的反光板位置点距离小于或等于设定距离阈值的反光板位置点，记为第二反光板位置点；对所述第一反光板位置点进行校正，得到校正后第一反光板位置点；根据所述校正后第一反光板位置点和所述第二反光板位置点，得到第二匹配数据集；根据所述第二匹配数据集，确定校正反光板位置后的定位位姿。通过上述方法本发明能够在建立了精确的基于反光板位置地图的基础上，对遮挡以及远距离反光板位置的处理更加优越，从而能够得到相对稳定的定位位姿。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。

图1为本发明基于反光板的激光雷达定位方法流程图；

图2为本发明基于反光板的激光雷达定位系统结构图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本发明的目的是提供一种基于反光板的激光雷达定位方法及系统，能够得到稳定的定位位姿。

为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。

图1为本发明基于反光板的激光雷达定位方法流程图。如图1所示，一种基于反光板的激光雷达定位方法包括：

步骤101：通过三边定位的方法，建立基于反光板定位的地图数据集。

通过三边定位的方法建立基于反光板定位的高精度地图，在建立地图时仅选取近距离的反光板添加至地图数据集，并且需保证现场布置的反光板具有分布的连续性，得到预存地图数据集MAP。

步骤102：通过数据接口，获取激光雷达数据集。

该步骤通过数据接口获取激光雷达数据集A{Pscani}。

步骤103：对所述激光雷达数据集进行运动学防畸变处理，得到防畸变数据集，具体包括：

利用运动学模型对所述激光雷达数据集进行运动学防畸变处理，得到防畸变数据集。

该步骤对数据进行运动学防畸变处理，采用运动学模型对激光雷达数据集A进行推算，得到新的防畸变数据集B{Preali}：

Preal_i＝f(Pscan_i,v_t,w_t)

其中，v_t为实际速度，单位：m/s；w_t为角速度，单位：rad/s；Pscan_i为激光雷达实际扫描数据信息，Preal_i为激光雷达扫描数据信息经防畸变处理后的真实位置。

步骤104：对所述防畸变数据集进行分组处理，得到分组数据集。

防畸变数据集B{Preal_i}根据反馈光强值，按照预设光强阈值对数据集B进行分组，并进行降噪处理得到新的分组数据集 C{Point_group_k}，(Point_group_k＝(Preal_i...Preal_m)，i至m均属于同一反光板的特征点)。

步骤105：对所述分组数据集所包含的每一组子数据集分别进行拟合，得到反光板圆心位置集。

分组数据集C所包含的每一组子数据集分别进行拟合，得到反光板圆心位置集D(reflectors_pos_i)。

步骤106：根据所述反光板圆心位置集和所述地图数据集，得到第一匹配数据集，具体包括：

将所述反光板圆心位置集和所述地图数据集通过三角形匹配方法建立对应关系，得到第一匹配数据集。

将数据集D(reflectors_pos_i)和步骤101中已建立地图中数据集 MAP通过三角形匹配方法建立对应关系，得到匹配数据集 SCAN_MAP(该数据集包含了扫描反光板的距离值，角度值和在步骤 101中MAP数据集中相对应的值)。

步骤107：从所述第一匹配数据集中选取拟合圆心过后的反光板位置点距离大于设定距离阈值的反光板位置点，记为第一反光板位置点。

步骤108：从所述第一匹配数据集中选取拟合圆心过后的反光板位置点距离小于或等于设定距离阈值的反光板位置点，记为第二反光板位置点。

该步骤107和步骤108中的匹配数据集Scan_map将拟合圆心过后的反光板位置点距离大于某一距离阈值的反光板挑出，将大于该阈值的拟合圆心过后的反光板位置点挑出并记为Point_untrust(i)，将剩余小于该阈值的拟合圆心过后的反光板位置点记为Point_trust(i)，该距离阈值与角分辨率相关：

其中，n_i为第i个反光板最多能扫到的扫描点，r为实际使用的反光板半径，d为第i个反光板距离激光雷达的距离，θ_scan为激光雷达角分辨率。

上式表明，在使用的激光雷达角分辨率确定的情况下，每个反光板包含的扫描点和该反光板与激光雷达距离相关，需要根据现场环境等等因素结合每个反光板上能获得的扫描点数选取合适的阈值 d_rectify。

步骤109：对所述第一反光板位置点进行校正，得到校正后第一反光板位置点。

针对步骤108中的Point_untrust(i)，都作出以下处理来校正不可信赖的位置点：

其中，

为第i个可信赖反光板在校正第j个反光板时权值函数。

式中η为归一化因子,d为第i个反光板距离激光雷达的距离。f_rectify(Point_trust(i))是基于余弦定理的校正函数,表示为:

由于拟合圆心过后的反光板位置点在远距离时角度变化较小，故不需对角度进行校正，θ即为激光雷达原始测量数据。

步骤110：根据所述校正后第一反光板位置点和所述第二反光板位置点，得到第二匹配数据集。

步骤111：根据所述第二匹配数据集，确定校正反光板位置后的定位位姿，具体包括：

根据所述第二匹配数据集采用三边定理的定位算法，确定校正反光板位置后的定位位姿。

将校正过后的Point_untrust(i)和Point_trust(i)重新组合成第二匹配数据集Scan_map带入三边定理的定位算法式中，求得校正反光板位置后的定位位姿Pose(x,y,theta)。

现实生产中，移动机器人通过激光雷达借助周边布置的反光板可以很精确的定位，但由于激光雷达角分辨率的限制，由于远距离的反光板上扫描点数量少、工厂工作环境限制而使反光板布置边缘化等等不利因素，导致没有足够的位置“可靠”的反光板来进行定位。这通常会导致定位精度下降，零位漂移等等现象的发生，不利于需要高精度的工位对接。由于近距离的反光板可以借由更多的扫描点拟合从而达到更高的拟合精度，步骤107-110结合先前建立的高精度地图，使用扫描数据加推算结果相结合的方法，得到一个更可靠的远距离反光板的位置，再将结果带入定位算法可以得到更稳定的定位效果。

对应于上述方法，本发明还提供一种基于反光板的激光雷达定位系统，如图2所示，该系统包括：

地图数据集建立模块201，用于通过三边定位的方法，建立基于反光板定位的地图数据集。

激光雷达数据集获取模块202，用于通过数据接口，获取激光雷达数据集。

防畸变处理模块203：用于对所述激光雷达数据集进行运动学防畸变处理，得到防畸变数据集。

分组处理模块204，用于对所述防畸变数据集进行分组处理，得到分组数据集。

反光板圆心位置集确定模块205，用于对所述分组数据集所包含的每一组子数据集分别进行拟合，得到反光板圆心位置集。

第一匹配数据集确定模块206，用于根据所述反光板圆心位置集和所述地图数据集，得到第一匹配数据集。

第一反光板位置点确定模块207，用于从所述第一匹配数据集中选取拟合圆心过后的反光板位置点距离大于设定距离阈值的反光板位置点，记为第一反光板位置点。

第二反光板位置点确定模块208，用于从所述第一匹配数据集中选取拟合圆心过后的反光板位置点距离小于或等于设定距离阈值的反光板位置点，记为第二反光板位置点。

第一反光板位置点校正模块209，用于对所述第一反光板位置点进行校正，得到校正后第一反光板位置点。

第二匹配数据集确定模块210，用于根据所述校正后第一反光板位置点和所述第二反光板位置点，得到第二匹配数据集。

定位位姿确定模块211，用于根据所述第二匹配数据集，确定校正反光板位置后的定位位姿。

所述防畸变处理模块203，具体包括：

防畸变处理单元，用于利用运动学模型对所述激光雷达数据集进行运动学防畸变处理，得到防畸变数据集。

所述第一匹配数据集确定模块206，具体包括：

第一匹配数据集确定单元，用于将所述反光板圆心位置集和所述地图数据集通过三角形匹配方法建立对应关系，得到第一匹配数据集。

所述定位位姿确定模块211，具体包括：

定位位姿确定单元，用于根据所述第二匹配数据集采用三边定理的定位算法，确定校正反光板位置后的定位位姿。

本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的系统而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。

本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上所述，本说明书内容不应理解为对本发明的限制。

Claims (6)

1.一种基于反光板的激光雷达定位方法，其特征在于，包括：

通过三边定位的方法，建立基于反光板定位的地图数据集；

通过数据接口，获取激光雷达数据集；

对所述激光雷达数据集进行运动学防畸变处理，得到防畸变数据集；

所述对所述激光雷达数据集进行运动学防畸变处理，得到防畸变数据集，具体包括：

利用运动学模型对所述激光雷达数据集进行运动学防畸变处理，得到防畸变数据集；

Preal_i＝f(Pscan_i,v_t,w_t)

其中，v_t为实际速度，单位：m/s；w_t为角速度，单位：rad/s；Pscan_i为激光雷达数据集，Preal_i为防畸变数据集；

对所述防畸变数据集进行分组处理，得到分组数据集；

对所述分组数据集所包含的每一组子数据集分别进行拟合，得到反光板圆心位置集；

根据所述反光板圆心位置集和所述地图数据集，得到第一匹配数据集；

从所述第一匹配数据集中选取拟合圆心过后的反光板位置点距离大于设定距离阈值的反光板位置点，记为第一反光板位置点Point_untrust(i)；

从所述第一匹配数据集中选取拟合圆心过后的反光板位置点距离小于或等于设定距离阈值的反光板位置点，记为第二反光板位置点Point_trust(i)；

对所述第一反光板位置点进行校正，得到校正后第一反光板位置点；

对Point_untrust(i)，都作出以下处理来校正不可信赖的位置点：

其中，

为第i个可信赖反光板在校正第j个反光板时权值函数；

式中η为归一化因子，d为第i个反光板距离激光雷达的距离；f_rectify(Point_trust(i))是基于余弦定理的校正函数，表示为:

由于拟合圆心过后的反光板位置点在远距离时角度变化较小，故不需对角度进行校正，θ即为激光雷达原始测量数据；

根据所述校正后第一反光板位置点和所述第二反光板位置点，得到第二匹配数据集；

根据所述第二匹配数据集，确定校正反光板位置后的定位位姿。

2.根据权利要求1所述的基于反光板的激光雷达定位方法，其特征在于，所述根据所述反光板圆心位置集和所述地图数据集，得到第一匹配数据集，具体包括：

将所述反光板圆心位置集和所述地图数据集通过三角形匹配方法建立对应关系，得到第一匹配数据集。

3.根据权利要求1所述的基于反光板的激光雷达定位方法，其特征在于，所述根据所述第二匹配数据集，确定校正反光板位置后的定位位姿，具体包括：

根据所述第二匹配数据集采用三边定理的定位算法，确定校正反光板位置后的定位位姿。

4.一种基于反光板的激光雷达定位系统，其特征在于，包括：

地图数据集建立模块，用于通过三边定位的方法，建立基于反光板定位的地图数据集；

激光雷达数据集获取模块，用于通过数据接口，获取激光雷达数据集；

防畸变处理模块，用于对所述激光雷达数据集进行运动学防畸变处理，得到防畸变数据集；

所述防畸变处理模块，具体包括：

防畸变处理单元，用于利用运动学模型对所述激光雷达数据集进行运动学防畸变处理，得到防畸变数据集；

Preal_i＝f(Pscan_i,v_t,w_t)

其中，v_t为实际速度，单位：m/s；w_t为角速度，单位：rad/s；Pscan_i为激光雷达数据集，Preal_i为防畸变数据集；

分组处理模块，用于对所述防畸变数据集进行分组处理，得到分组数据集；

反光板圆心位置集确定模块，用于对所述分组数据集所包含的每一组子数据集分别进行拟合，得到反光板圆心位置集；

第一匹配数据集确定模块，用于根据所述反光板圆心位置集和所述地图数据集，得到第一匹配数据集；

第一反光板位置点确定模块，用于从所述第一匹配数据集中选取拟合圆心过后的反光板位置点距离大于设定距离阈值的反光板位置点，记为第一反光板位置点Point_untrust(i)；

第二反光板位置点确定模块，用于从所述第一匹配数据集中选取拟合圆心过后的反光板位置点距离小于或等于设定距离阈值的反光板位置点，记为第二反光板位置点Point_trust(i)；

第一反光板位置点校正模块，用于对所述第一反光板位置点进行校正，得到校正后第一反光板位置点；

对Point_untrust(i)，都作出以下处理来校正不可信赖的位置点：

其中，

为第i个可信赖反光板在校正第j个反光板时权值函数；

式中η为归一化因子，d为第i个反光板距离激光雷达的距离；f_rectify(Point_trust(i))是基于余弦定理的校正函数,表示为:

由于拟合圆心过后的反光板位置点在远距离时角度变化较小，故不需对角度进行校正，θ即为激光雷达原始测量数据；

第二匹配数据集确定模块，用于根据所述校正后第一反光板位置点和所述第二反光板位置点，得到第二匹配数据集；

定位位姿确定模块，用于根据所述第二匹配数据集，确定校正反光板位置后的定位位姿。

5.根据权利要求4所述的基于反光板的激光雷达定位系统，其特征在于，所述第一匹配数据集确定模块，具体包括：

第一匹配数据集确定单元，用于将所述反光板圆心位置集和所述地图数据集通过三角形匹配方法建立对应关系，得到第一匹配数据集。

6.根据权利要求4所述的基于反光板的激光雷达定位系统，其特征在于，所述定位位姿确定模块，具体包括：

定位位姿确定单元，用于根据所述第二匹配数据集采用三边定理的定位算法，确定校正反光板位置后的定位位姿。

Images