CN110068829A - 简化的基于图形快速匹配的激光定位方法 - Google Patents

Abstract

本发明公开一种简化的基于图形快速匹配的激光定位方法，包括：在结构化场景空间内任意安装至少三个圆筒形反光筒；初始化世界坐标系下的参考地图，识别特征为三角形；输入激光雷达数据时，对数据进行容错处理，存储对应强度下的反光筒有效距离并转换到运动坐标系下；采用拟合技术求解每个反光筒位置；将运动坐标系与世界坐标系下的三角形进行图形化匹配，匹配依据是三角形的角度差小于设定的阈值则匹配成功；将运动坐标系下检测到反光筒的位置与之匹配成功的世界坐标系下反光筒的位置分别构造成一个检测矩阵和参考矩阵，然后进行矩阵分解，求解其位姿。本发明的优点是匹配效率高，准确率高，计算量少，实时性强，定位精度高。

Description

简化的基于图形快速匹配的激光定位方法

技术领域

本发明涉及激光定位的技术领域，特别涉及一种简化的基于图形快速匹配的激光定位方法。

背景技术

定位精度一直是自动驾驶领域一个重要研究方向，其中影响定位精度的一个重要因素是特征误匹配，它已成为制约视觉或激光SLAM定位精度的主要技术瓶颈之一。

特征匹配是影响SLAM定位精度的一个关键因素。传统的特征匹配方法包括视觉SLAM领域的基于描述子的特征点匹配、激光SLAM领域广泛采用的ICP等方法，这些方法在一定程度上都会存在误匹配的问题，这也直接引起定位误差，在一定的时间范围内,这种定位误差是可以接受的，但是随着时间的推进，这类定位误差会不断积累,最终导致定位效果越来越差，消除长时间积累的定位误差主要是通过采用全局优化的方法。从某种程度上讲，采用全局优化的方法比较耗时，要满足实时性，需要对原算法进行优化、改进。

当前在工程应用当中，已存在采用以图形为特征的匹配方法，以三角图形特征为例，该方法采用以三角形周长为特征的匹配方法，此方法相对于之前的多边形匹配方法，具有耗费计算资源少、实时性好、误差可控等优点，但是该方法存在诸多限制条件，比如要存储三角形边长、角度、计算周长、提取的三角形边长与激光最大探测距离还要满足一定的约束等。

以上现有的技术方案主要存在的问题是：计算量大、实时性相对较低、定位误差不可控、对高动态环境的鲁棒性低、实现方法限制条件多。这是本申请需要着重改善的地方。

发明内容

本发明所要解决的技术问题是要提供一种简化的基于图形快速匹配的激光定位方法，能有效地减少限制条件、极大提高匹配准确率及在复杂环境下具有很高的鲁棒性。

为了解决以上的技术问题，本发明提供了一种简化的基于图形快速匹配的激光定位方法，包括如下的步骤：

⑴在结构化场景空间内的任意位置至少安装三个圆筒形反光筒，满足各个反光筒之间距离不同的条件；

⑵初始化世界坐标系下的参考地图，识别特征为三角形；

⑶输入激光雷达数据时，对数据进行容错处理，理论上激光扫描到反光筒上会得到一组连续的高强度反光点，但是由于激光硬件特性的不同，该组反光点往往不连续，本申请识别反光筒的一个重要依据就是连续的高反光强度点，故过滤反光筒的低反光强度点，通过容错处理提高了检测反光筒的鲁棒性，存储对应强度下的反光筒距离信息，将反光筒的有效距离转换到当前的运动坐标系下；

⑷采用拟合技术求解每个反光筒的圆心，用圆心表示反光筒的位置，将检测到反光筒的位置数据输入三角形组合模块，并存储组合后的三角形信息；

⑸上述第⑷步骤中得到当前三角形与世界坐标系下的三角形进行图形化匹配，匹配依据是当前三角形的每个角度与世界坐标系下的三角形的每个角度进行相减，角度差小于设定的阈值是匹配成功；具体包括如下的步骤：

①输入当前三角形n，判断n＜N，否，转第⑺步；

其中：N为运动坐标系下检测到的三角形的个数；

②设世界坐标系下共有M个参考三角形；

③输入世界坐标系下的三角形m；

④判断m＜M，否，转第①步；

⑤当前三角形n的角度与世界坐标系下的三角形m的角度做差值；

⑥判断角度差值是否小于角度差阀值，否转第③步；

⑦匹配成功，n++，转第①步；

⑹将运动坐标系下检测到反光筒的位置与之匹配成功的世界坐标系下反光筒的位置分别构造成一个检测矩阵和参考矩阵，然后进行矩阵分解，求解其位姿；

⑺返回到步骤⑶，进行下一轮的姿态求解；

⑻结束。

所述第⑸步骤中的图形化匹配是以3×N的匹配速度确认反光筒的位置，Ｎ表示有效循环次数。

所述第⑶步骤中的激光雷达数据，是激光扫到反光筒时的反射强度用以识别反光筒，检测到反光筒后并保存反光筒的有效距离数据。

所述第⑶步骤中的对数据进行容错处理，包括如下的步骤：

①输入一帧激光雷达数据，假设每帧数据3600点，即i=1，2，…，3600；

②判断第i个数据的反射强度是否大于设定阀值；否，转第⑤步骤；

③是，判断i是否小于允许的低反射强度点的数目；否，清空当前容器内容，转第⑤步骤；

④是，存储对应强度下的距离信息；

⑤i++；

⑥判断i是否大于3600，否，转第①步骤；

⑦结束。

所述第⑷步骤中的拟合技术，是使用非线性求解器拟合反光筒的圆心。

所述第⑵步骤中的初始化世界坐标系下的参考地图，识别特征为三角形，包括如下的步骤：

①利用全站仪测量各个反光筒在世界坐标系下的坐标位置Ｐ(x,y)并记录；

②在世界坐标系下，将反光筒的位置坐标作为三角形的顶点，对所有顶点进行排列组合，每次任选三个不同的顶点，最后存储三角形的顶点以及角度信息，制作基于三角形识别特征的参考地图。

所述第⑹步骤中的构造成一个检测矩阵和参考矩阵后，分别减去各自矩阵的质心，然后将参考矩阵的转置乘以检测矩阵，得到矩阵M，对M进行奇异值分解，最终求解得到位姿。

本发明的优势在于：

1）对激光雷达数据具有容错能力，对同一块反光筒出现不连续的高强度反光点会自动进行过滤，极大地提高了检测反光筒的鲁棒性；

2）利用圆心表示反光筒的位置，圆心的求解采用了谷歌的非线性求解器，求解圆心稳定，对表示反光筒位置具有很高的鲁棒性；

3）只需根据角度差就进行特征匹配，相比以三角形周长进行特征匹配，本申请是基于简化的图形化匹配方法；

4）匹配效率高，准确率高，计算量少，实时性强，定位精度高，定位鲁棒性高。

附图说明

构成本申请的一部分的说明书附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。在附图中：

图1为本发明的工作流程图。

具体实施方式

以下结合附图对本发明的实施例进行详细说明。

图1示出了本发明实施例的工作流程图。如图1所示，本发明提供了一种简化的基于图形快速匹配的激光定位方法，本发明实施以空间布局五个圆筒型反光筒，包括如下的步骤：

⑴在结构化场景空间内的任意位置安装五个圆筒形反光筒，满足各个反光筒之间距离不同的条件；

⑵利用全站仪测量五个反光筒在世界坐标系下的坐标位置Ｐ(x,y)并记录；

⑶在世界坐标系下，任选三个反光筒，将反光筒的圆心位置坐标作为三角形的顶点，对所有顶点进行排列组合C^３ _５，组合出的三角形共有十种情况，最后存储这十种三角形的顶点以及角度信息，制作基于三角形识别特征的参考地图；

⑷输入激光雷达数据时，对数据进行容错处理，过滤反光筒的低反光强度点，提高了检测反光筒的鲁棒性。因为反光筒的识别是根据激光扫到反光筒时的反射强度判断的。理想情况下，一帧激光雷达数据中遇到反光筒时应当连续地出现几个高强度反光点，以本实施例为例，当有连续的五个高强度反光点时，就认为已经检测到反光筒并开始保存有效地探测距离数据；但是由于雷达自身的原因，即使扫描同一块反光筒也会出现不连续的高强度反光点，因此，对激光雷达数据进行容错处理，具体做法是：当检测到一个高强度反光点时，就存储对应强度下的反光筒距离信息，然后开始判断下一个扫描点的反射强度，如果该点反射强度很低，就接着判断下一个扫描点的反射强度，忽略反光强度较低的点；最后，将反光筒的有效距离转换到当前的运动坐标系下；每个反光筒会有至少五个有效探测距离数据，将它们全部转换到当前的运动坐标系下；

⑸使用谷歌的非线性求解器拟合反光筒的圆心，用圆心表示反光筒的位置，采用拟合技术求解圆心来表示反光筒的位置，弥补激光雷达的探测误差，从而提高表示反光筒位置的鲁棒性；

将检测到反光筒的位置数据输入三角形组合模块，并存储组合后的三角形信息，包括三角形的顶点和角度；

⑹上述第⑸步骤中得到当前三角形与世界坐标系下的三角形进行图形化匹配，匹配依据是当前三角形的每个角度与世界坐标系下的三角形的每个角度进行相减，角度差小于设定的阈值则为匹配成功；

在动态坐标系下，设置的检测反光筒数量不少于三块，也就是每帧激光雷达数据至少检测出1个三角形，少于3块反光筒组合不出一个三角形，因此被认为检测无效，当检测出一个三角形时，就与世界坐标系下预先存储的所有三角形进行匹配，匹配的依据是：当前三角形n的每个角度与世界坐标系下的三角形m角度相减，只有小于一定的阈值0.4度被认为是同一个角度，则匹配成功；

所述图形化匹配是以3×N的匹配速度确认反光筒的位置，Ｎ表示有效循环次数。本实施例中，假设同时检测到五块反光筒，每当成功匹配一组三角形时，就可以认为空间中三块反光筒的位置已经确定，剩下的任务是确定另外二块反光筒的位置，就在剩下的三角形中匹配，当另外二块反光筒的位置也确认时，自动停止匹配，采用这种匹配方法以３×N的匹配速度确认反光筒的位置，从而加速了匹配过程，在提高匹配效率的前提下保证了匹配的准确率；

⑺将运动坐标系下检测到反光筒的位置与之匹配成功的世界坐标系下反光筒的位置分别构造成一个检测矩阵和参考矩阵，然后分别减去各自矩阵的质心；

⑻然后将参考矩阵的转置乘以检测矩阵，得到矩阵M，对M进行奇异值分解，求解其位姿，并将其发布到规划模块；

⑼返回到步骤⑷，进行下一轮的姿态求解；

⑽结束。

所述第⑷步骤中的对数据进行容错处理，包括如下的步骤：

①输入一帧激光雷达数据，假设每帧数据3600点，即i=1，2，…，3600；

②判断第i个数据的反射强度是否大于设定阀值；否，转第⑤步骤；

③是，判断i是否小于允许的低反射强度点的数目；否，清空当前容器内容，转第⑤步骤；

④是，存储对应强度下的距离信息；

⑤i++；

⑥判断i是否大于3600，否，转第①步骤；

⑦结束。

以上所述仅为本发明的优先实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围内之内。

Claims (7)

1.一种简化的基于图形快速匹配的激光定位方法，包括如下的步骤：

⑴在结构化场景空间内的任意位置至少安装三个圆筒形反光筒，满足各个反光筒之间距离不同的条件；

⑵初始化世界坐标系下的参考地图，识别特征为三角形；

⑶输入激光雷达数据时，对数据进行容错处理，过滤反光筒的低反光强度点，存储对应强度下的反光筒距离信息，转换到运动坐标系下；

⑷采用拟合技术求解每个反光筒的圆心，将检测到反光筒的位置数据输入三角形组合模块，并存储组合后的三角形信息；

⑸上述第⑷步骤中得到当前三角形与世界坐标系下的三角形进行图形化匹配，匹配依据是当前三角形的每个角度与世界坐标系下的三角形的每个角度进行相减，角度差小于设定的阈值是匹配成功；

⑹将运动坐标系下检测到反光筒的位置与之匹配成功的世界坐标系下反光筒的位置分别构造成一个检测矩阵和参考矩阵，然后进行矩阵分解，求解其位姿；

⑺返回到步骤⑶，进行下一轮的姿态求解；

⑻结束。

2.根据权利要求1所述的简化的基于图形快速匹配的激光定位方法，其特征在于： 所述第⑸步骤中的图形化匹配是以3×N的匹配速度确认反光筒的位置，Ｎ表示有效循环次数。

3.根据权利要求1所述的简化的基于图形快速匹配的激光定位方法，其特征在于：所述第⑶步骤中的激光雷达数据，是激光扫到反光筒时的反射强度用以识别反光筒，检测到反光筒后并保存反光筒的有效距离数据。

4.根据权利要求1所述的简化的基于图形快速匹配的激光定位方法，其特征在于：所述第⑶步骤中的对数据进行容错处理，包括如下的步骤：

①输入一帧激光雷达数据，假设每帧数据3600点，即i=1，2，…，3600；

②判断第i个数据的反射强度是否大于设定阀值；否，转第⑤步骤；

③是，判断i是否小于允许的低反射强度点的数目；否，清空当前容器内容，转第⑤步骤；

④是，存储对应强度下的距离信息；

⑤i++；

⑥判断i是否大于3600，否，转第①步骤；

结束。

5.根据权利要求1所述的简化的基于图形快速匹配的激光定位方法，其特征在于：所述第⑷步骤中的拟合技术，是使用非线性求解器拟合反光筒的圆心。

6.根据权利要求1所述的简化的基于图形快速匹配的激光定位方法，其特征在于：所述第⑵步骤中的初始化世界坐标系下的参考地图，识别特征为三角形，包括如下的步骤：

①利用全站仪测量各个反光筒在世界坐标系下的坐标位置Ｐ(x,y)并记录；

②在世界坐标系下，将反光筒的位置坐标作为三角形的顶点，对所有顶点进行排列组合，每次任选三个不同的顶点，最后存储三角形的顶点以及角度信息，制作基于三角形识别特征的参考地图。

7.根据权利要求1所述的简化的基于图形快速匹配的激光定位方法，其特征在于：所述第⑹步骤中的构造成一个检测矩阵和参考矩阵后，分别减去各自矩阵的质心，然后将参考矩阵的转置乘以检测矩阵，得到矩阵M，对M进行奇异值分解，最终求解得到位姿。