一种基于激光反光板和里程计的融合定位方法及系统与流程

1.本发明涉及一种基于激光反光板和里程计的融合定位方法及系统，属于机器人技术领域。


背景技术：

2.目前机器人相对定位方法有，slam定位、uwb定位、二维码+视觉定位等，这些定位各自有不同的特性。
3.二维码+视觉的定位方法是相对定位领域应用较多的方法，其精度和稳定性都比较适中，也有很多优秀的算法足够精进，能达到很高的定位精度，其不足之处在于这种方法计算量较大，且不易实现较高频率的定位。
4.激光反光板定位是一种比较廉价、易实现、精度高、稳定性强的方案，目前激光反光板定位方法大多数采样一种v字型反光板进行相对定位，此种方法为了容易找到反光板上的某一特定点，故意将形状设计为v字型，利用中间的角点进行定位，效果比较明显，但是对标志物的加工难度较大，成本较大，且有误识别的风险。
5.目前基于激光反光板的定位方法在机器人发生位移后的定位中具有计算量大，精度低、稳定性差的缺陷。


技术实现要素：

6.本发明的目的在于克服现有技术中的不足，提供一种基于激光反光板和里程计的融合定位方法及系统，能够提高定位精度和稳定性。为达到上述目的，本发明是采用下述技术方案实现的：
7.第一方面，本发明提供了一种基于激光反光板和里程计的融合定位方法，包括：
8.获取地图数据和激光点云数据；
9.将点云数据和地图数据进行匹配，得到全局定位的位置p0；
10.里程计从全局定位的位置p0开始进行位置跟踪，累计跟踪位移得到里程计定位预测的位置p1；
11.基于里程计定位预测的位置p1，进行局部激光定位，得到激光匹配测量位置p2以及位置p1和p2间的匹配率score；
12.基于匹配率score、位置p0、p1和p2，采用预设的融合定位方法，得到最终定位位置。
13.结合第一方面，进一步地，所述获取地图数据，包括：
14.将地图图片加载成一张灰度图m1，对m1按像素进行筛选，采用浮点型数组f[]表示m1，数组中的元素与m1中的像素一一对应，如果像素的灰度值小于100，则数组中对应的元素值置为0.99，否则数组中对应元素的值置为0；
[0015]
按照m1的长宽，将浮点型数组f[]还原为栅格地图map，栅格元素值为浮点类型，与f[]中的值对应；
[0016]
使用膨胀算法处理栅格地图map，对于地图中的每个栅格p，如果p的值不是0.99，则寻找离p最近的值为0.99的栅格计算距离d，更新p的值c＝0.99(1-ek·d)；
[0017]
机器人开机后使用滑动窗口最大值方法从横向和纵向分别处理map，滑动窗口的长度依次为2、4、8、16、32、64，分别得到图层map2、图层map3、图层map4、图层map5、图层map6、图层map7。
[0018]
结合第一方面，进一步地，所述获取点云数据，包括：
[0019]
从机器人激光雷达获取一帧激光点云scan，对此点云进行栅格滤波，保证每个栅格中只保留一个点获得过滤后的点云scan_；
[0020]
对点云进行旋转处理，分别旋转0度、1度、...、359度，得到360组点云scan_0,scan_1,...,scan_359。
[0021]
结合第一方面，进一步地，所述得到全局定位的位置p0，包括：
[0022]
在最模糊的图层map7中，使用原始分辨率0.05m乘以最大滑动窗口长度64得到3.2米为步长，分别使用scan_0,scan_1,...,scan_359进行暴力匹配，以将点云插入到指定的位置后，每一个点落入的栅格的值的加和为打分规则为点云及其位置打分；
[0023]
选取分值最高的点云，在最高分的位置开一个窗口，窗口的大小为3.2*2＝6.4m，以3.2/2＝1.6m为步长，使用下一级地图map6进行暴力匹配，依此规律一直匹配到map2，得到的位置即为全局定位的位置p0。
[0024]
结合第一方面，进一步地，所述得到里程计定位预测的位置p1，包括：
[0025]
对齐里程计和全局定位，使里程计从全局定位的位置p0开始进行位置跟踪；
[0026]
在位置跟踪的每个时间步长中，计算里程计累计的位移，将位移和上一次定位结果相加，得到里程计定位预测的位置p1。
[0027]
结合第一方面，进一步地，所述得到激光匹配测量位置p2以及位置p1和p2间的匹配率score，包括：
[0028]
在里程计定位预测的位置p1周围开一个窗口，窗口的大小和里程计累计位移量的大小成正比，
[0029]
在此窗口中按照0.05m为位置步长，0.017弧度为角度步长，进行激光数据暴力匹配，得到分值最高的匹配结果p2，并输出位置p1和p2间的匹配率score。
[0030]
结合第一方面，进一步地，所述预设的融合定位方法，包括：
[0031]
预设里程计测量值的权重为k1[k10,k11,k12]，激光匹配测量值权重为k2[k20,k21,k22]；
[0032]
判断1：判断匹配率score大于预设阈值ε1，若大于则激光匹配测量位置p2可靠，反之则不可靠；
[0033]
判断2：将激光匹配测量位置p2和上一次全局定位的位置p0的位移量做偏差，得到向量a0(dx1，dy1)，将向量a0分解到以p0的朝向为x轴的右手笛卡尔坐标系中得到变换后的向量a1(dx2，dy2)，判断dy2是否小于预设阈值ε2；
[0034]
判断3：若dy2小于预设阈值ε2，判断位置p1和p2的距离d是否大于预设阈值ε3；
[0035]
执行判断1，匹配率score小于等于预设阈值ε1，激光匹配测量位置p2不可靠，不执行判断2和判断3，里程计累计跟踪位移，使用里程计定位预测的位置p1作为定位结果；
[0036]
基于判断1匹配率score大于预设阈值ε1结果，执行判断2，dy2大于等于预设阈值ε
2，使用权重k1和权重k2对p1和p2求加权平均值，得到融合定位位置，重新对齐融合定位位置和里程计，不执行判断3，里程计累计跟踪位移，输出定位结果；
[0037]
基于判断3，位置p1和p2的距离d大于预设阈值ε3，升高权重k1中k10和k11的权重，得到权重k1’，升高的比例与d正相关，使用权重k1’和权重k2对p1和p2求加权平均值，得到融合定位位置，重新对齐融合定位位置和里程计，里程计累计跟踪位移，输出定位结果。
[0038]
结合第一方面，进一步地，还包括位姿修正：
[0039]
若匹配率score小于预设阈值ε4，则在位置p1上开一个2m*2m的窗口，将点云数据和地图数据进行匹配，得到位姿位置p3以及输出位置p1和p3间的匹配率score2；
[0040]
若匹配率score2大于预设阈值ε5，则新的位姿可靠，使用p3作为融合定位结果输出，并对齐里程计。
[0041]
第二方面，本发明提供了一种基于激光反光板和里程计的融合定位系统，包括：
[0042]
获取模块：用于获取地图数据和激光点云数据；
[0043]
初定位模块：用于将点云数据和地图数据进行匹配，得到全局定位的位置p0；
[0044]
里程计定位模块：用于里程计从全局定位的位置p0开始进行位置跟踪，累计跟踪位移得到里程计定位预测的位置p1；
[0045]
激光定位模块：用于基于里程计定位预测的位置p1，进行局部激光定位，得到激光匹配测量位置p2以及位置p1和p2间的匹配率score；
[0046]
融合定位模块：用于基于匹配率score、位置p0、p1和p2，采用预设的融合定位方法，得到最终定位位置。
[0047]
第三方面，本发明提供了一种存储一个或多个程序的计算机可读存储介质，其特征在于，所述一个或多个程序包括指令，所述指令当由计算设备执行时，使得所述计算设备执行根据第一方面所述的方法中的任一方法。
[0048]
与现有技术相比，本发明实施例所提供的一种基于激光反光板和里程计的融合定位方法及系统所达到的有益效果包括：
[0049]
本发明获取地图数据和激光点云数据；将点云数据和地图数据进行匹配，得到全局定位的位置p0；里程计从全局定位的位置p0开始进行位置跟踪，累计跟踪位移得到里程计定位预测的位置p1；基于里程计定位预测的位置p1，进行局部激光定位，得到激光匹配测量位置p2以及位置p1和p2间的匹配率score；基于匹配率score、位置p0、p1和p2，采用预设的融合定位方法，得到最终定位位置。本发明提供的融合定位方法法在机器人发生位移后的定位中计算精度高、稳定性强，受环境影响更小，更稳定。
附图说明
[0050]
图1为本发明实施例1提供的一种基于激光反光板和里程计的融合定位方法的流程图。
具体实施方式
[0051]
下面结合附图对本发明作进一步描述。以下实施例仅用于更加清楚地说明本发明的技术方案，而不能以此来限制本发明的保护范围。
[0052]
实施例一：
[0053]
如图1所示，一种基于激光反光板和里程计的融合定位方法，包括：
[0054]
获取地图数据和激光点云数据；
[0055]
将点云数据和地图数据进行匹配，得到全局定位的位置p0；
[0056]
里程计从全局定位的位置p0开始进行位置跟踪，累计跟踪位移得到里程计定位预测的位置p1；
[0057]
基于里程计定位预测的位置p1，进行局部激光定位，得到激光匹配测量位置p2以及位置p1和p2间的匹配率score；
[0058]
基于匹配率score、位置p0、p1和p2，采用预设的融合定位方法，得到最终定位位置。
[0059]
具体步骤包括：
[0060]
步骤1：获取地图数据。
[0061]
步骤1.1：地图加载。
[0062]
将地图图片加载成一张灰度图m1，对m1按像素进行筛选，采用浮点型数组f[]表示m1，数组中的元素与m1中的像素一一对应，如果像素的灰度值小于100，则数组中对应的元素值置为0.99，否则数组中对应元素的值置为0。
[0063]
按照m1的长宽，将浮点型数组f[]还原为栅格地图map，栅格元素值为浮点类型，与f[]中的值对应。
[0064]
使用膨胀算法处理栅格地图map，对于地图中的每个栅格p，如果p的值不是0.99，则寻找离p最近的值为0.99的栅格计算距离d，更新p的值c＝0.99(1-ek·d)。
[0065]
步骤1.2：开机重定位。
[0066]
机器人开机后使用滑动窗口最大值方法从横向和纵向分别处理map。
[0067]
滑动窗口最大值方法：选取一个窗口大小，沿一个线性数据结构滑动，将窗口内所有值都置为窗口范围内的最大值。
[0068]
比如原始数据为0 0 0 0 0 1 0 0 0 0，滑动窗口长度为2，则滑动过后得到0 0 0 0 1 1 1 0 0 0。
[0069]
如果是本实施例的二维数据，比如滑动窗口最大值处理前的数据为：
[0070]
[0071][0072]
经过长度为2的滑动窗口最大值横向和纵向处理后得到的数据为：
[0073][0074]
首先选取滑动窗口长度为2，对数据进行滑动窗口最大值处理，得到图层map2。然后选取滑动窗口长度4，对map2数据进行滑动窗口最大值处理，得到图层map3。选取滑动窗口长度为8、16、32、64对数据进行滑动窗口处理，分别得到图层map4、图层map5、图层map6、图层map7。
[0075]
步骤2：获取激光点云数据。
[0076]
从机器人激光雷达获取一帧激光点云scan，对此点云进行栅格滤波，保证每个栅格中只保留一个点获得过滤后的点云scan_。在本实施例中，使用5cm的栅格对激光点云进行过滤。
[0077]
对点云进行旋转处理，分别旋转0度、1度、...、359度，得到360组点云scan_0,scan_1,...,scan_359。
[0078]
步骤3：将点云数据和地图数据进行匹配，得到全局定位的位置p0。
[0079]
在最模糊的图层map7中，使用原始分辨率0.05m乘以最大滑动窗口长度64得到3.2米为步长，分别使用scan_0,scan_1,...,scan_359进行暴力匹配，以将点云插入到指定的位置后，每一个点落入的栅格的值的加和为打分规则为点云及其位置打分。
[0080]
选取分值最高的点云，在最高分的位置开一个窗口，窗口的大小为3.2*2＝6.4m，以3.2/2＝1.6m为步长，使用下一级地图map6进行暴力匹配，依此规律一直匹配到map2，得
到的位置即为全局定位的位置p0。
[0081]
步骤4：里程计从全局定位的位置p0开始进行位置跟踪，累计跟踪位移得到里程计定位预测的位置p1。
[0082]
对齐里程计和全局定位，使里程计从全局定位的位置p0开始进行位置跟踪。
[0083]
在位置跟踪的每个时间步长中，计算里程计累计的位移，将位移和上一次定位结果相加，得到里程计定位预测的位置p1。
[0084]
步骤5：基于里程计定位预测的位置p1，进行局部激光定位，得到激光匹配测量位置p2以及位置p1和p2间的匹配率score。
[0085]
在里程计定位预测的位置p1周围开一个窗口，窗口的大小和里程计累计位移量的大小成正比，在此窗口中按照0.05m为位置步长，0.017弧度为角度步长，进行激光数据暴力匹配，得到分值最高的匹配结果p2，并输出位置p1和p2间的匹配率score。
[0086]
步骤6：基于匹配率score、位置p0、p1和p2，采用预设的融合定位方法，得到最终定位位置。
[0087]
预设里程计测量值的权重为k1[k10,k11,k12]，激光匹配测量值权重为k2[k20,k21,k22]。
[0088]
判断1：判断匹配率score大于预设阈值ε1，若大于则激光匹配测量位置p2可靠，反之则不可靠；
[0089]
判断2：将激光匹配测量位置p2和上一次全局定位的位置p0的位移量做偏差，得到向量a0(dx1，dy1)，将向量a0分解到以p0的朝向为x轴的右手笛卡尔坐标系中得到变换后的向量a1(dx2，dy2)，判断dy2是否小于预设阈值ε2；
[0090]
判断3：若dy2小于预设阈值ε2，判断位置p1和p2的距离d是否大于预设阈值ε3。
[0091]
执行判断1，匹配率score小于等于预设阈值ε1，激光匹配测量位置p2不可靠，不执行判断2和判断3，里程计累计跟踪位移，使用里程计定位预测的位置p1作为定位结果。
[0092]
基于判断1匹配率score大于预设阈值ε1结果，执行判断2，dy2大于等于预设阈值ε2，使用权重k1和权重k2对p1和p2求加权平均值，得到融合定位位置，重新对齐融合定位位置和里程计，不执行判断3，里程计累计跟踪位移，输出定位结果。
[0093]
基于判断3，位置p1和p2的距离d大于预设阈值ε3，升高权重k1中k10和k11的权重，得到权重k1’，升高的比例与d正相关，使用权重k1’和权重k2对p1和p2求加权平均值，得到融合定位位置，重新对齐融合定位位置和里程计，里程计累计跟踪位移，输出定位结果。
[0094]
需要说明的是，还包括位姿修正：
[0095]
若匹配率score小于预设阈值ε4，则在位置p1上开一个2m*2m的窗口，将点云数据和地图数据进行匹配，得到位姿位置p3以及输出位置p1和p3间的匹配率score2；
[0096]
若匹配率score2大于预设阈值ε5，则新的位姿可靠，使用p3作为融合定位结果输出，并对齐里程计。
[0097]
本发明提供的融合定位方法法在机器人发生位移后的定位中计算精度高、稳定性强，受环境影响更小，更稳定。
[0098]
实施例二：
[0099]
本发明实施例提供了一种基于激光反光板和里程计的定位系统，包括：
[0100]
获取模块：用于获取地图数据和激光点云数据；
[0101]
初定位模块：用于将点云数据和地图数据进行匹配，得到全局定位的位置p0；
[0102]
里程计定位模块：用于里程计从全局定位的位置p0开始进行位置跟踪，累计跟踪位移得到里程计定位预测的位置p1；
[0103]
激光定位模块：用于基于里程计定位预测的位置p1，进行局部激光定位，得到激光匹配测量位置p2以及位置p1和p2间的匹配率score；
[0104]
融合定位模块：用于基于匹配率score、位置p0、p1和p2，采用预设的融合定位方法，得到最终定位位置。
[0105]
实施例三：
[0106]
本发明实施例提供了一种存储一个或多个程序的计算机可读存储介质，其特征在于，所述一个或多个程序包括指令，所述指令当由计算设备执行时，使得所述计算设备执行根据实施例一所述的方法。
[0107]
本领域内的技术人员应明白，本技术的实施例可提供为方法、系统、或计算机程序产品。因此，本技术可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本技术可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、cd-rom、光学存储器等)上实施的计算机程序产品的形式。
[0108]
本技术是参照根据本技术实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
[0109]
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
[0110]
这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
[0111]
以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明技术原理的前提下，还可以做出若干改进和变形，这些改进和变形也应视为本发明的保护范围。