CN110084272A - 一种聚类地图创建方法及基于聚类地图和位置描述子匹配的重定位方法 - Google Patents

Abstract

本发明公开了一种聚类地图创建方法及基于聚类地图和位置描述子匹配的重定位方法，提出了一种具有完整系统结构的三维视觉激光SLAM系统，以及一种聚类地图的创建方法，并基于聚类地图和位置描述子设计了一种无人车的重定位方法，面向城市动态环境，创建了与真实场景尺度一致的聚类地图，同时设计了用于描述点云聚类位置特征的位置描述子，并且利用位置描述子的匹配与筛选实现了无人车在城市环境中的可靠重新定位。通过忽略单个物体形貌上的变化，重点关注不同物体之间的相互位置关系，本发明提出的重定位方法能够有效应对多种场景条件的不利变化，并实现较高的重定位成功率，具有较为广泛的应用前景与较高的研究价值。

Description

一种聚类地图创建方法及基于聚类地图和位置描述子匹配的 重定位方法

技术领域

本发明涉及无人设备定位领域，具体涉及一种聚类地图创建方法及基于聚类地图和位置描述子匹配的重定位方法。

背景技术

在无人机和无人车领域中，SLAM技术(即“同时定位与地图构建”)，它能够在未知环境中为无人车提供位置和地图信息，就像人的感知系统一样。因此在很多自动驾驶的研究领域，都会用到SLAM技术，为无人车的主动寻路与动态避障等提供技术支持。而在自动驾驶的相关应用场景中，经常需要解决的一个问题被称之为“绑架问题”，该问题需要自动驾驶汽车在由于某些原因导致连续定位失败时，仍然能够通过当前的环境信息和已有的环境地图，重新确定其自身在地图中所处的正确位置，也就是说，无人车需要具有重新定位的能力。是否能够可靠、有效地实现重定位，是无人车自主定位能力的一种体现，而且关系着环境地图的有效使用期限和自动驾驶车辆的行驶安全。目前在城市环境中完成无人车定位建图与重定位所面临的一些困难，例如，单传感器的定位能力不足，而多传感器组合的方式成本又相对较高。城市环境的场景变化度较大、动态物体较多，容易对地图的创建和无人车的自主重定位造成一定的困难等。

发明专利《一种无人车在地下车库中的精确定位方法》(公开号：CN108917761A)公开了一种无人车在地下车库中的精确定位方法，该方法包括：激光点云地面分割模块、圆柱特征提取与特征匹配，以及非地面有效点选择模块、激光有效点合并与降采样模块、栅格地图产生与三维地图匹配模块、初始位姿模块。该方法的思路是利用圆柱特征，剔除激光点云场景中的动态障碍物，选择属于地下车库本身结构的激光有效点，然后利用地图匹配技术实现车辆精确定位。这种方法适用于无人车在地下车库中的定位与导航，而在室外的城市道路环境中，由于自然植被的存在，使得该方法不能有效实现特征信息的提取与匹配，因此无法可靠完成无人车的定位与重定位。

发明专利《一种基于稀疏视觉特征地图的无人车位置跟踪方法》(公开号：CN106548486A)公开了一种基于稀疏视觉特征地图的无人车位置跟踪方法。在已经探索过的野外环境中，由用户指定环境地图中的某一位置相对于地图的坐标。基于已建立的稀疏视觉特征地图提供的全局定位信息，生成目标位置跟踪控制量，控制无人车自动向目标位置行驶。这种方法适用于较短时间间隔内的无人车定位与重新定位。因为在室外环境，由于植被的生长会导致场景形貌的变化，因此只依靠视觉特征点无法保证特征描述的一致性。

发明专利《基于三维激光雷达的无人车自主定位方法》(公开号：CN107015238A)公开了一种基于三维激光雷达的无人车定位算法，能够从单帧的激光点云数据中提取出路沿轮廓，并与高精度地图数据进行匹配，计算出当前车辆的侧向、纵向和航向角偏差作为观测值，输入到卡尔曼滤波器进行车辆位姿估计。能够保证车辆长时间，长距离的稳定定位。不过由于高精度地图的建图与维护成本较高，使得这种方法无法大面积应用到城市环境中的无人车定位与重新定位。

发明内容

为了解决上述技术问题，本发明目的之一是提供一种聚类地图创建方法，解决当前城市环境的场景变化度较大、动态物体较多，容易对地图的创建和无人车的自主重定位造成一定的困难的问题。

为实现上述发明目的，本发明采取的技术方案如下：

一种聚类地图的创建方法，包括如下步骤：

步骤1.通过三维视觉-激光SLAM系统框架创建所述聚类地图并提供定位信息，所述三维视觉-激光SLAM系统框架包括路径追踪线程、局部地图建图线程、回环检测线程；在所述路径追踪线程中，通过点云注册模块获取相应激光雷达的位姿，并融合单目视觉图像和三维激光点云数据，然后经过深度值提取模块从三维激光点云数据中提取相应视觉特征点的深度值用于SLAM系统的定位与建图；提取相应视觉特征点的深度值可以提高视觉特征点深度值提取的效率与精度，增强SLAM系统的实时性与稳定性；

步骤2.通过当前相应视觉特征点的深度值对局部地图初始化，在路径追踪线程中，利用局部地图追踪路径，判定当前关键帧，然后在局部地图建图线程中将关键帧插入局部地图中并剔除地图中的相应原始点，增加新的地图点，在剔除关键帧后进行回环检测线程，在所述回环检测线程中，实时检测路径回环；回环检测线程能够实时检测路径回环并在完成回环闭合后利用全局非线性优化的方法同步调整关键帧位姿和地图点，消除运动估计产生的累积误差，使得SLAM系统能够获得与真实运动状态尺度一致的定位结果；

步骤3.通过实时进行路径追踪线程、局部地图建图线程、回环检测线程来获取三维点云数据，建立实时三维点云地图和八叉树地图；

步骤4.在点云聚类模块中将所述三维点云地图中的三维点云数据根据物体的不同，分割成不同的点云聚类，然后在聚类注册模块中，将所有的点云聚类都加入到同一个地图坐标系下，将同一物体产生的点云聚类重新融合到一起，并将属于动态物体的点云聚类滤除后建立聚类地图；聚类地图能够将场景中的动态物体滤除，只保留属于静态物体的点云聚类，使其能够在具有较小数据量的同时，又能保留场景的静态稳定特征；

步骤5.通过聚类地图中的每个点云聚类与其周围其他点云聚类之间的相互位置关系，为所述点云聚类创建位置描述子，用以描述该物体的位置特征。

优选的，在所述步骤1中的所述路径追踪线程中，单目图像为相机采集到的图像，通过当前单目图像中提取到的二维视觉特征点和三维地图点之间的特征匹配关系，二维视觉特征点为ORB特征，建立特征点之间的对应关系，并利用PnP算法实时估计当前相机的位姿；当获得了一系列连续的相机位姿之后；在所述点云注册模块中，通过六自由度相机位姿插值的方法获得相应激光雷达的位姿，从而实现单目图像信息和三维点云信息的数据同步，通过插值法能够对三维激光点云数据进行稠密化处理，实现单目图像信息和三维点云信息的数据同步。

优选的，所述六自由度相机位姿插值的计算过程如下：

X_lidar＝s₁X_b+s₂X_f

当前接收到的三维激光点云数据时间戳为其前一时刻接收到的单目图像信息时间戳为并且与其相应的相机位姿为X_b，其后一时刻接收到的单目图像时间戳为并且与其相对应的相机位姿为X_f，则当前的激光雷达位姿X_lidar可以由上述的插值算法获得，通过插值，获得每一帧激光点云数据之后，便能将多帧点云数据进行融合，以此获得稠密度更高的点云数据。从而提高深度值提取模块中从三维点云直接获取相应视觉特征点深度值的精确度与成功率。与传统单目视觉里程计中通过三角测量与深度滤波等方式计算特征点深度值的方法相比，直接通过三维激光点云获取深度值的方法能够节省大量运算时间，并且其结果也更为准确可靠，从而能够提高SLAM系统的运行效率与定位精度。

优选的，在所述步骤2中增加新的地图点后，对新的地图点的三维点云数据进行BA优化，并剔除关键帧；在所述回环检测线程中，通过候选帧与当前帧匹配完成候选帧检测，计算坐标变换矩阵，调整所有关键帧位姿和地图点坐标，然后完成回环融合和要素图优化。一旦成功完成路径闭环，该三维视觉-激光SLAM系统将利用全局非线性优化算法同步调整所有关键帧位姿和地图点坐标，进一步增加SLAM系统的定位和建图精度。

优选的，所述步骤4中将同一物体产生的点云聚类重新融合到一起的判别方法如下：

S1.选取点云聚类A中的一个成员点p_j，以其为球心，rad为半径的球体范围内搜索另一组点云聚类B的成员点，rad的值应根据实际的点云稠密程度进行设定，通常可以设为0.25～0.35米；

S2.当在成员点p_j的球体范围内出现另一组点云聚类B的成员点，则p_j为点云聚类B的一个邻近点；

S3.当步骤S2中的点云聚类A中和成员点p_j的情况相同的点大于60％时，则判别点云聚类A和点云聚类B都是由真实场景中的同一个物体生成的；

S4.将点云聚类A和点云聚类B这两个点云聚类重新融合到一起；

S5.以此类推在所有的点云聚类内寻找同一物体产生的点云聚类并进行重新融合。

优选的，所述步骤S2中邻近点的数量超过点云聚类A中所有成员点总数量的70％时，则判别点云聚类A和点云聚类B都是由真实场景中的同一个物体生成的；

优选的，所述步骤5中所述点云聚类的位置描述子的创建包括如下步骤：

a1.选取一个点云聚类C_i创建位置描述子L_i，首先以点云聚类C_i为圆心，以SR为搜索半径，搜索点云聚类C_i附近的其他点云聚类，点云聚类C_i的位置描述子L_i由多个子位置描述符ⁱL_k构成，其中SR的值可以根据实际建图范围进行设定，通常设为35米，而每个子位置描述符ⁱL_k又将由多个位置关系组合而成；

a2.选取步骤a1中的点云聚类C_i附近的点云聚类ⁱC_k1，在点云聚类C_i和点云聚类ⁱC_k1之间建立一条参考轴再选取点云聚类C_i附近的点云聚类在点云聚类ⁱC_k1和点云聚类之间建立一条测量轴则点云聚类ⁱC_k1和点云聚类之间的位置关系可以用两个参数和进行描述，其中其中是测量轴的长度，是测量轴和参考轴之间的夹角，其符号遵守右手定则，即

a3.按照步骤a2的操作，在点云聚类C_i搜索半径内遍历点云聚类ⁱC_k1周围的其他点云聚类，可以获得其他的多组位置关系，设多组位置关系数量为n，将这n组位置关系组合在一起，便能获得点云聚类ⁱC_k1的子位置描述符；

a4.按照步骤a3的操作，采用相同的方法，遍历C_i周围的除点云聚类ⁱC_k1之外的其他点云聚类，获得其他点云聚类的子位置描述符，所述子位置描述符数量为k，多组子位置描述符构成点云聚类C_i的位置描述子L_i。

优选的，步骤a1中的点云聚类C_i的搜索半径SR通过同心圆进行分割，将其划分为多个等宽的环带区域，如A₀,A₁,…,A_x等，将不同编号的环带区域中的k组子位置描述符组合在一起，获得点云聚类C_i的位置描述子。(说明书详细说明)

本发明目的之二是提供一种基于聚类地图和位置描述子匹配的重定位方法，能够很好地解决城市环境中长时间间隔后的无人车重定位问题，具体包括如下步骤：

b1.首先建立当前环境的原始聚类地图，当无人车需要进行重定位时，根据当前环境信息建立临时聚类地图；

b2.通过临时聚类地图与原始聚类地图中点云聚类的位置描述子的匹配，建立临时聚类地图与原始聚类地图之间的点云聚类的匹配关系，得到临时聚类地图中的每个点云聚类与原始聚类地图中匹配到的多个点云聚类候选项；

b3.将候选项集合到候选项集合中，利用几何筛选条件对步骤b2中的候选项集合中的候选项进行筛选，根据筛选匹配结果，建立临时地图和原始地图之间点云聚类的一一对应关系；

b4.根据临时地图和原始地图之间点云聚类的一一对应关系，计算两幅地图之间的坐标变换矩阵；

b5.通过构建如下的优化函数，求解出两幅地图之间的坐标变换矩阵T，完成无人车的重新定位：

式中，O_i为原始聚类地图中点云聚类的坐标；O_j为临时聚类地图中点云聚类的坐标；m_suc表示临时聚类地图和原始聚类地图中能够产生成功匹配的点云聚类组数；

所述步骤b1-b4中的聚类地图为上述所述的聚类地图。

优选的，所述步骤b2中的临时聚类地图与原始聚类地图中点云聚类的位置描述子的匹配过程如下：

通过选取临时聚类地图与原始聚类地图中两个位置描述子L_i和L_j，计算这两个位置描述子之间的“位置距离”为D_ij，位置距离为D_ij则可以通过下式进行计算：

上述公式中，k_i为位置描述子L_i中所有子位置描述符的数量，k_suc为位置描述子L_i和L_j中能够产生成功匹配的子位置描述符的数量，^ijD_k1k2表示两个子位置描述符ⁱL_k1和^jL_k2之间的匹配距离，该距离被称为“子位置距离”，其中，ⁱL_k1为位置描述子L_i中的一个子位置描述符，而^jL_k2为位置描述子L_j中的一个子位置描述符，两个子位置描述符之间的子位置距离可以通过下式计算获得：

上述公式中，n_k1为子位置描述符ⁱL_k1中包含的所有位置关系的数量，n_suc为子位置描述符ⁱL_k1和^jL_k2中，能够产生成功匹配的位置关系的数量，表示两个位置关系和之间的“几何距离”，其中，为子位置描述符ⁱL_k1中的一个位置关系，为子位置描述符^jL_k2中的一个位置关系；假设位置关系 则这两个位置关系之间的几何距离可以通过下式计算获得：

通过计算两个位置描述子之间的位置距离来判断两个点云聚类位置特征的相似程度，位置距离越小，代表两个点云聚类的位置特征越相近。因此在无人车需要进行重定位时，对于临时聚类地图中的每个点云聚类，都能利用位置描述子匹配的方法在原始聚类地图中找到多个与该临时点云聚类具有相似位置特征的原始点云聚类。将这些原始聚类地图中匹配到的多个点云聚类称为最佳匹配对象的候选项。

优选的，步骤b3中选取三个几何筛选条件对步骤b2中的点云聚类的匹配关系进行筛选，经过三个几何筛选条件筛选后从每个候选项集合中挑选出一个最佳匹配对象，获得临时聚类地图和原始聚类地图之间的点云聚类的一一对应关系，三个所述几何筛选条件分别为长度几何筛选条件、三角形几何筛选条件和包含几何筛选条件。

相对于现有技术，本发明取得了有益的技术效果：

本发明提供的聚类地图创建方法及基于聚类地图和位置描述子匹配的重定位方法面向城市动态环境，创建了与真实场景尺度一致的聚类地图，同时设计了用于描述点云聚类位置特征的位置描述子，并且利用位置描述子的匹配与筛选实现了无人车在城市环境中的可靠重新定位。通过忽略单个物体形貌上的变化，重点关注不同物体之间的相互位置关系，本发明提出的重定位方法能够有效应对多种场景条件的不利变化，并实现较高的重定位成功率。因此，本发明具有较为广泛的应用前景与较高的研究价值，同时能够为今后城市动态环境下自动驾驶技术的相关研究提供新的研究思路与研究方法。

附图说明

图1为本发明聚类地图创建过程中的三维视觉-激光SLAM系统框架和聚类地图创建流程图；

图2为本发明聚类地图创建过程中不同点云聚类融合示意图；

图3为本发明的三维点云地图和三维聚类地图对比效果；

图4为本发明的二维聚类地图；

图5为本发明聚类地图中点云聚类的位置描述子的创建过程；

图6为本发明聚类地图创建过程中位置描述子数据结构示意图；

图7为本发明基于聚类地图和位置描述子匹配的重定位方法的定位流程图；

图8为本发明基于聚类地图和位置描述子匹配的重定位方法中长度几何筛选条件示意图；

图9为本发明基于聚类地图和位置描述子匹配的重定位方法中三角形几何筛选条件示意图；

图10为本发明基于聚类地图和位置描述子匹配的重定位方法中包含几何筛选条件示意图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合实施例对本发明进行进一步详细说明，但本发明要求保护的范围并不局限于下述具体实施例。

如图1所示，一种聚类地图的创建方法，包括如下步骤：

步骤1.通过三维视觉-激光SLAM系统框架创建所述聚类地图并提供定位信息，所述三维视觉-激光SLAM系统框架包括路径追踪线程、局部地图建图线程、回环检测线程；在所述路径追踪线程中，通过点云注册模块获取相应激光雷达的位姿，并融合单目视觉图像和三维激光点云数据，然后经过深度值提取模块从三维激光点云数据中提取相应视觉特征点的深度值用于SLAM系统的定位与建图；提取相应视觉特征点的深度值可以提高视觉特征点深度值提取的效率与精度，增强SLAM系统的实时性与稳定性；

步骤2.通过当前相应视觉特征点的深度值对局部地图初始化，在路径追踪线程中，利用局部地图追踪路径，判定当前关键帧，然后在局部地图建图线程中将关键帧插入局部地图中并剔除地图中的相应原始点，增加新的地图点，在剔除关键帧后进行回环检测线程，在所述回环检测线程中，实时检测路径回环；回环检测线程能够实时检测路径回环并在完成回环闭合后利用全局非线性优化的方法同步调整关键帧位姿和地图点，消除运动估计产生的累积误差，使得SLAM系统能够获得与真实运动状态尺度一致的定位结果；

在所述步骤2中增加新的地图点后，还可以对新的地图点的三维点云数据进行BA优化，并剔除关键帧；在所述回环检测线程中，通过候选帧与当前帧匹配完成候选帧检测，计算坐标变换矩阵，调整所有关键帧位姿和地图点坐标，然后完成回环融合和要素图优化。一旦成功完成路径闭环，该三维视觉-激光SLAM系统将利用全局非线性优化算法同步调整所有关键帧位姿和地图点坐标，进一步增加SLAM系统的定位和建图精度；

步骤3.通过实时进行路径追踪线程、局部地图建图线程、回环检测线程来获取三维点云数据，建立实时三维点云地图和八叉树地图；

步骤4.在点云聚类模块中将所述三维点云地图中的三维点云数据根据物体的不同，分割成不同的点云聚类，然后在聚类注册模块中，将所有的点云聚类都加入到同一个地图坐标系下，将同一物体产生的点云聚类重新融合到一起，并将属于动态物体的点云聚类滤除后建立聚类地图；聚类地图能够将场景中的动态物体滤除，只保留属于静态物体的点云聚类，使其能够在具有较小数据量的同时，又能保留场景的静态稳定特征；

步骤5.通过聚类地图中的每个点云聚类与其周围其他点云聚类之间的相互位置关系，为所述点云聚类创建位置描述子，用以描述该物体的位置特征。

在所述步骤1中的所述路径追踪线程中，单目图像为相机采集到的图像，通过当前单目图像中提取到的二维视觉特征点和三维地图点之间的特征匹配关系，二维视觉特征点为ORB特征，建立特征点之间的对应关系，并利用PnP算法实时估计当前相机的位姿，得出相机的初始化位姿估计；当获得了一系列连续的相机位姿之后；利用局部地图追踪路径，在所述点云注册模块中，通过六自由度相机位姿插值的方法获得相应激光雷达的位姿，从而实现单目图像信息和三维点云信息的数据同步，通过插值法能够对三维激光点云数据进行稠密化处理，实现单目图像信息和三维点云信息的数据同步。所述六自由度相机位姿插值的计算过程如下：

X_lidar＝s₁X_b+s₂X_f

当前接收到的三维激光点云数据时间戳为其前一时刻接收到的单目图像信息时间戳为并且与其相应的相机位姿为X_b，其后一时刻接收到的单目图像时间戳为并且与其相对应的相机位姿为X_f，则当前的激光雷达位姿X_lidar可以由上述的插值算法获得，通过插值，获得每一帧激光点云数据之后，便能将多帧点云数据进行融合，以此获得稠密度更高的点云数据。从而提高深度值提取模块中从三维点云直接获取相应视觉特征点深度值的精确度与成功率。与传统单目视觉里程计中通过三角测量与深度滤波等方式计算特征点深度值的方法相比，直接通过三维激光点云获取深度值的方法能够节省大量运算时间，并且其结果也更为准确可靠，从而能够提高SLAM系统的运行效率与定位精度。

在聚类地图创建过程中，利用三维点云数据，由同一物体产生的点云聚类可以重新融合，将环境中的特征物体分割出来，如树木，路灯，墙柱等，生成不同的点云聚类。同时，聚类地图能够将场景中的动态物体滤除，只保留属于静态物体的点云聚类，使其能够在具有较小数据量的同时，又能保留场景的静态稳定特征。并且，对于聚类地图中的每个点云聚类，也就是场景中的每个物体，本发明利用该物体与其周围其他物体之间的相互位置关系，为该物体创建位置描述子，用以描述该物体的位置特征，可以作为无人车在城市环境中进行重新定位时的参考信息。首先，在图1的实时点云聚类模块中，本发明利用点云聚类的算法，将原始三维激光点云，根据物体的不同，分割成不同的点云聚类。然后在聚类注册模块中，本发明会将所有的点云聚类都加入到同一个地图坐标系下，以增强点云聚类算法的分类一致性，并将属于动态物体的点云聚类滤除。

在所述步骤4中将同一物体产生的点云聚类重新融合到一起的判别方法如下：

S1.选取点云聚类A中的一个成员点p_j，以其为球心，rad为半径的球体范围内搜索另一组点云聚类B的成员点，rad的值应根据实际的点云稠密程度进行设定，通常可以设为0.25～0.35米，优选的为0.3米；

S2.当在成员点p_j的球体范围内出现另一组点云聚类B的成员点，则p_j为点云聚类B的一个邻近点；

S3.当步骤S2中的点云聚类A中和成员点p_j的情况相同的点较多时，则判别点云聚类A和点云聚类B都是由真实场景中的同一个物体生成的；优选的，所述步骤S2中邻近点的数量超过点云聚类A中所有成员点总数量的70％时，则判别点云聚类A和点云聚类B都是由真实场景中的同一个物体生成的；

S4.将点云聚类A和点云聚类B这两个点云聚类重新融合到一起；

S5.以此类推在所有的点云聚类内寻找同一物体产生的点云聚类并进行重新融合。

具体的如图2所示，假设以灰色点代表点云聚类A，黑色点代表点云聚类B。则对于点云聚类A的一个成员点p_j，以其为球心，rad为半径的球体范围内，可能会出现点云聚类B的成员点，如果是这样的话，可以称p_j为点云聚类B的一个“邻近点”。如果在点云聚类A中，像p_j这样的邻近点数量较多的话，即可认为点云聚类A和点云聚类B都是由真实场景中的同一个物体生成的，只不过在不同的原始点云数据中，被分割成了不同的点云形态，因此可以将这两个点云聚类重新融合到一起，从而提高点云聚类算法的分类一致性。

根据这种融合方法，在步骤4中剔除动态点云聚类，当聚类地图中出现一个新的点云聚类N时，聚类注册模块将首先判断点云聚类N出现的位置处，是否已经存在其他点云聚类，若不存在，则将点云聚类N作为新的点云聚类加入聚类地图中，并将其出现次数初始化为1。若已存在其他点云聚类，则将两个点云聚类融合到一起，并将点云聚类的出现次数在原有基础上加1，这个出现次数将作为动态物体过滤的依据。如图3所示，是使用相同的数据分别创建的同一个场景下的三维点云地图和三维聚类地图，左侧为点云地图，右侧为聚类地图。通过点云地图的建图效果可以发现，在创建点云地图的过程中，场景中出现的动态行人等运动物体会在点云地图中留下拖影(如图中被圈出的区域所示)。而在聚类地图中，由于本发明只保留出现在地图中同一个位置次数较多的点云聚类，因此能够很好地将场景中的动态物体滤除，因此在聚类地图的相同位置处，并未出现动态物体的点云聚类。

如图4所示。在一般情况下，从物体层面来看，环境中的物体大致是在一个大的平面上进行分布三维。因此，在为每个物体创建位置描述子时，本发明将对三维聚类地图进行简化，生成形式更为简洁的二维聚类地图。

如图5所示，所述步骤5中所述点云聚类的位置描述子的创建包括如下步骤：

a1.选取一个点云聚类C_i创建位置描述子L_i，首先以点云聚类C_i为圆心，以SR为搜索半径，搜索点云聚类C_i附近的其他点云聚类，点云聚类C_i的位置描述子L_i由多个子位置描述符ⁱL_k构成，其中SR的值可以根据实际建图范围进行设定，通常设为30～40米，优选的为35米，而每个子位置描述符ⁱL_k又将由多个位置关系组合而成；

a2.选取步骤a1中的点云聚类C_i附近的点云聚类ⁱC_k1，在点云聚类C_i和点云聚类ⁱC_k1之间建立一条参考轴再选取点云聚类C_i附近的点云聚类在点云聚类ⁱC_k1和点云聚类之间建立一条测量轴则点云聚类ⁱC_k1和点云聚类之间的位置关系可以用两个参数和进行描述，其中其中是测量轴的长度，是测量轴和参考轴之间的夹角，其符号遵守右手定则，即

a3.按照步骤a2的操作，在点云聚类C_i搜索半径内遍历点云聚类ⁱC_k1周围的其他点云聚类，可以获得其他的多组位置关系，设多组位置关系数量为n，将这n组位置关系组合在一起，便能获得点云聚类ⁱC_k1的子位置描述符；

a4.按照步骤a3的操作，采用相同的方法，遍历C_i周围的除点云聚类ⁱC_k1之外的其他点云聚类，获得其他点云聚类的子位置描述符，所述子位置描述符数量为k，多组子位置描述符构成点云聚类C_i的位置描述子L_i。

优选的，步骤a1中的点云聚类C_i的搜索半径SR通过同心圆进行分割，将其划分为多个等宽的环带区域，并为每个环带区域设置不同的编号，如A₀,A₁,…,A_x等，根据每个环带区域的编号将这k组子位置描述符组合在一起，即将不同编号的环带区域中的k组子位置描述符组合在一起，获得点云聚类C_i的位置描述子。

如图6所示，在实际的程序编写过程中，本发明将利用三维向量数据结构对这种结构形式的位置描述子进行存储与处理。A₁,A₂,…,A_x中每一环均包含若干个点云聚类，每一环中均包括与该环内点云聚类数量相同的若干个子位置描述符，每个点云聚类均包括n组位置关系，所有的子位置描述符组合后有K组，k组子位置描述符组合后形成点云聚类C_i的位置描述子。

如图7所示，本发明目的还提供一种基于聚类地图和位置描述子匹配的重定位方法，能够很好地解决城市环境中长时间间隔后的无人车重定位问题，具体包括如下步骤：

b1.首先建立当前环境的原始聚类地图，当无人车需要进行重定位时，根据当前环境信息建立临时聚类地图；

b2.通过临时聚类地图与原始聚类地图中点云聚类的位置描述子的匹配，建立临时聚类地图与原始聚类地图之间的点云聚类的匹配关系，得到临时聚类地图中的每个点云聚类与原始聚类地图中匹配到的多个点云聚类候选项；

b3.将候选项集合到候选项集合中，利用几何筛选条件对步骤b2中的候选项集合中的候选项进行筛选，根据筛选匹配结果，建立临时地图和原始地图之间点云聚类的一一对应关系；

b4.根据临时地图和原始地图之间点云聚类的一一对应关系，计算两幅地图之间的坐标变换矩阵；

b5.通过构建如下的优化函数，求解出两幅地图之间的坐标变换矩阵T，完成无人车的重新定位：

式中，O_i为原始聚类地图中点云聚类的坐标；O_j为临时聚类地图中点云聚类的坐标；m_suc表示临时聚类地图和原始聚类地图中能够产生成功匹配的点云聚类组数；

所述步骤b1-b4中的聚类地图为上述所述的聚类地图。

所述步骤b2中的临时聚类地图与原始聚类地图中点云聚类的位置描述子的匹配过程如下：

通过选取临时聚类地图与原始聚类地图中两个位置描述子L_i和L_j，计算这两个位置描述子之间的“位置距离”为D_ij，位置距离为D_ij则可以通过下式进行计算：

上述公式中，k_i为位置描述子L_i中所有子位置描述符的数量，k_suc为位置描述子L_i和L_j中能够产生成功匹配的子位置描述符的数量，^ijD_k1k2表示两个子位置描述符ⁱL_k1和^jL_k2之间的匹配距离，该距离被称为“子位置距离”，其中，ⁱL_k1为位置描述子L_i中的一个子位置描述符，而^jL_k2为位置描述子L_j中的一个子位置描述符，两个子位置描述符之间的子位置距离可以通过下式计算获得：

上述公式中，n_k1为子位置描述符ⁱL_k1中包含的所有位置关系的数量，n_suc为子位置描述符ⁱL_k1和^jL_k2中，能够产生成功匹配的位置关系的数量，表示两个位置关系和之间的“几何距离”，其中，为子位置描述符ⁱL_k1中的一个位置关系，为子位置描述符^jL_k2中的一个位置关系；假设位置关系 则这两个位置关系之间的几何距离可以通过下式计算获得：

通过计算两个位置描述子之间的位置距离来判断两个点云聚类位置特征的相似程度，位置距离越小，代表两个点云聚类的位置特征越相近。当“位置距离”Dij的值小于某个阈值才可以判别是可以匹配的原始点云聚类，即可以判定两个点云聚类匹配成功，阈值的数值为0.1～0.3m，优选的是0.2m。再者，有些临时点云聚类无法找到合适的匹配类，为了保证有充足的匹配数，一般要求临时聚类地图中包含足够数量的临时点云聚类，临时点云聚类的数量为20个以上。因此在无人车需要进行重定位时，对于临时聚类地图中的每个点云聚类，都能利用位置描述子匹配的方法在原始聚类地图中找到多个与该临时点云聚类具有相似位置特征的原始点云聚类。将这些原始聚类地图中匹配到的多个点云聚类称为最佳匹配对象的候选项。

本发明将在步骤b3中选取三个几何筛选条件对步骤b2中的点云聚类的匹配关系进行筛选，经过三个几何筛选条件筛选后从每个候选项集合中挑选出一个最佳匹配对象，可以从三个几何筛选条件筛选出的最佳匹配对象中再选取一个最优匹配对象，获得临时聚类地图和原始聚类地图之间的点云聚类的一一对应关系，三个所述几何筛选条件分别为长度几何筛选条件、三角形几何筛选条件和包含几何筛选条件。

长度几何筛选条件、三角形几何筛选条件和包含几何筛选条件的具体筛选过程如下：

(1).长度几何筛选条件

如图8所示，假设临时聚类地图中的点云聚类A能够在原始地图中匹配到A₁～A₄共四个候选点云聚类，并且临时地图中的点云聚类B能够在原始地图中匹配到B₁～B₃共三个候选点云聚类。同时，在临时聚类地图中，能够获得点云聚类A和点云聚类B之间的直线距离为L_AB。根据这一条件，本发明能够确定在原始聚类地图中，A₃为A的最佳匹配对象，B₂为B的最佳匹配对象，从而能够在原始地图中的多个候选项中，挑选出A和B的最佳匹配对象，建立如图所示的点云聚类之间的一一对应关系。因为在原始聚类地图中，A₃和B₂之间的直线距离也是L_AB，这便是点云聚类之间的长度几何筛选条件。

(2).三角形几何筛选条件

如图9所示，假设临时聚类地图中的点云聚类C能够在原始聚类地图中匹配到C₁～C₄共四个候选项。则根据临时聚类地图中，三个点云聚类A,B,C所形成的三角形可以判断，原始地图中的C₃为点云聚类C的最佳匹配对象。因为在原始聚类地图中，点云聚类A₃,B₂,C₃这三个点云聚类所形成的三角形与△ABC最为相似。这便是三角形几何筛选条件。

(3).包含几何筛选条件

如图10所示，假设临时聚类地图中的点云聚类D能够在原始聚类地图中匹配到D₁～D₃共三个候选点云聚类。同时，在临时聚类地图中，本发明能够搜索距离点云聚类D最远的一个点云聚类，设该最远距离为L_MAX。则在临时地图中，以点云聚类D为圆心，以L_MAX为半径的圆形范围内，能够包含临时聚类地图中所有的A,B,C,D四个点云聚类。则根据这个条件，本发明能够判断D₁为D的最佳匹配对象。因为在原始聚类地图中，以D₁为圆心，L_MAX为半径的圆形范围内，能够包含四种不同的点云聚类候选项。这便是包含几何筛选条件。

根据三个几何条件的筛选，本发明能够建立起临时聚类地图和原始聚类地图之间，点云聚类的一一对应关系。

根据上述说明书的揭示和教导，本发明所属领域的技术人员还可以对上述实施方式进行变更和修改。因此，本发明并不局限于上面揭示和描述的具体实施方式，对发明的一些修改和变更也应当落入本发明的权利要求的保护范围内。此外，尽管本说明书中使用了一些特定的术语，但这些术语只是为了方便说明，并不对发明构成任何限制。

Claims (10)

1.一种聚类地图的创建方法，其特征在于，包括如下步骤：

步骤1.通过三维视觉-激光SLAM系统框架创建聚类地图并提供定位信息，所述三维视觉-激光SLAM系统框架包括路径追踪线程、局部地图建图线程、回环检测线程；在所述路径追踪线程中，通过点云注册模块获取相应激光雷达的位姿，并融合单目视觉图像和三维激光点云数据，然后经过深度值提取模块从三维激光点云数据中提取相应视觉特征点的深度值用于SLAM系统的定位与建图；

步骤2.通过当前相应视觉特征点的深度值对局部地图初始化，在路径追踪线程中，利用局部地图追踪路径，判定当前关键帧，然后在局部地图建图线程中将关键帧插入局部地图中并剔除地图中的相应原始点，增加新的地图点，在剔除关键帧后进行回环检测线程，在所述回环检测线程中，实时检测路径回环；

步骤3.通过实时进行路径追踪线程、局部地图建图线程、回环检测线程来获取三维点云数据，建立实时三维点云地图；

步骤4.在点云聚类模块中将所述三维点云地图中的三维点云数据根据物体的不同，分割成不同的点云聚类，然后在聚类注册模块中，将所有的点云聚类都加入到同一个地图坐标系下，将同一物体产生的点云聚类重新融合到一起，并将属于动态物体的点云聚类滤除后建立聚类地图；

步骤5.通过聚类地图中的每个点云聚类与其周围其他点云聚类之间的相互位置关系，为所述点云聚类创建位置描述子，用以描述该物体的位置特征。

2.根据权利要求1所述的一种聚类地图的创建方法，其特征在于，在所述步骤1中的所述路径追踪线程中，通过当前单目图像中提取到的二维视觉特征点和三维地图点之间的特征匹配关系，建立特征点之间的对应关系，并利用PnP算法实时估计当前相机的位姿；当获得了一系列连续的相机位姿之后；在所述点云注册模块中，通过六自由度相机位姿插值的方法获得相应激光雷达的位姿，通过插值法能够对三维激光点云数据进行稠密化处理，实现单目图像信息和三维点云信息的数据同步。

3.根据权利要求2所述的一种聚类地图的创建方法，其特征在于，所述六自由度相机位姿插值的计算过程如下：

X_lidar＝s₁X_b+s₂X_f

当前接收到的三维激光点云数据时间戳为Tp_lidar，其前一时刻接收到的单目图像信息时间戳为Tp_b，并且与其相应的相机位姿为X_b，其后一时刻接收到的单目图像时间戳为Tp_f，并且与其相对应的相机位姿为X_f，则当前的激光雷达位姿X_lidar可以由上述的插值算法获得，通过插值，获得每一帧激光点云数据之后，便能将多帧点云数据进行融合，以此获得稠密度更高的点云数据。

4.根据权利要求1所述的一种聚类地图的创建方法，其特征在于，在所述步骤2中增加新的地图点后，对新的地图点的三维点云数据进行BA优化，并剔除关键帧；在所述回环检测线程中，通过候选帧与当前帧匹配完成候选帧检测，计算坐标变换矩阵，调整所有关键帧位姿和地图点坐标，然后完成回环融合和要素图优化。

5.根据权利要求1所述的一种聚类地图的创建方法，其特征在于，所述步骤4中将同一物体产生的点云聚类重新融合到一起的判别方法如下：

S1.选取点云聚类A中的一个成员点p_j，以其为球心，rad为半径的球体范围内搜索另一组点云聚类B的成员点；

S2.当在成员点p_j的球体范围内出现另一组点云聚类B的成员点，则p_j为点云聚类B的一个邻近点；

S3.当步骤S2中的点云聚类A中和成员点p_j的情况相同的点大于60时，则判别点云聚类A和点云聚类B都是由真实场景中的同一个物体生成的；

S4.将点云聚类A和点云聚类B这两个点云聚类重新融合到一起；

S5.以此类推在所有的点云聚类内寻找同一物体产生的点云聚类并进行重新融合。

6.根据权利要求1所述的一种聚类地图的创建方法，其特征在于，所述步骤5中所述点云聚类的位置描述子的创建包括如下步骤：

a1.选取一个点云聚类C_i创建位置描述子L_i，首先以点云聚类C_i为圆心，以SR为搜索半径，搜索点云聚类C_i附近的其他点云聚类，点云聚类C_i的位置描述子L_i由多个子位置描述符ⁱL_k构成，而每个子位置描述符ⁱL_k又将由多个位置关系组合而成；

a2.选取步骤a1中的点云聚类C_i附近的点云聚类ⁱC_k1，在点云聚类C_i和点云聚类ⁱC_k1之间建立一条参考轴再选取点云聚类C_i附近的点云聚类在点云聚类ⁱC_k1和点云聚类之间建立一条测量轴则点云聚类ⁱC_k1和点云聚类之间的位置关系可以用两个参数和进行描述，其中其中是测量轴的长度，是测量轴和参考轴之间的夹角，其符号遵守右手定则，即

a3.按照步骤a2的操作，在点云聚类C_i搜索半径内遍历点云聚类ⁱC_k1周围的其他点云聚类，可以获得其他的多组位置关系，设多组位置关系数量为n，将这n组位置关系组合在一起，便能获得点云聚类ⁱC_k1的子位置描述符；

a4.按照步骤a3的操作，采用相同的方法，遍历C_i周围的除点云聚类ⁱC_k1之外的其他点云聚类，获得其他点云聚类的子位置描述符，所述子位置描述符数量为k，多组子位置描述符构成点云聚类C_i的位置描述子L_i。

7.根据权利要求6所述的一种聚类地图的创建方法，其特征在于，将步骤a1中的点云聚类C_i的搜索半径SR通过同心圆进行分割，将其划分为多个等宽的环带区域，如A₀,A₁,…,A_x等，将不同编号的环带区域中的k组子位置描述符组合在一起，获得点云聚类C_i的位置描述子。

8.一种基于聚类地图和位置描述子匹配的重定位方法，其特征在于，包括如下步骤：

b1.首先建立当前环境的原始聚类地图，当无人车需要进行重定位时，根据当前环境信息建立临时聚类地图；

b2.通过临时聚类地图与原始聚类地图中点云聚类的位置描述子的匹配，建立临时聚类地图与原始聚类地图之间的点云聚类的匹配关系，得到临时聚类地图中的每个点云聚类与原始聚类地图中匹配到的多个点云聚类候选项；

b3.将候选项集合到候选项集合中，利用几何筛选条件对步骤b2中的候选项集合中的候选项进行筛选，根据筛选匹配结果，建立临时地图和原始地图之间点云聚类的一一对应关系；

b4.根据临时地图和原始地图之间点云聚类的一一对应关系，计算两幅地图之间的坐标变换矩阵；

b5.通过构建如下的优化函数，求解出两幅地图之间的坐标变换矩阵T，完成无人车的重新定位：

式中，O_i为原始聚类地图中点云聚类的坐标；O_j为临时聚类地图中点云聚类的坐标；m_suc表示临时聚类地图和原始聚类地图中能够产生成功匹配的点云聚类组数；

所述步骤b1-b4中的聚类地图为权利要求1-7任一项所述的聚类地图。

9.根据权利要求8所述的一种基于聚类地图和位置描述子匹配的重定位方法，其特征在于，所述步骤b2中的临时聚类地图与原始聚类地图中点云聚类的位置描述子的匹配过程如下：

通过选取临时聚类地图与原始聚类地图中两个位置描述子L_i和L_j，计算这两个位置描述子之间的“位置距离”为D_ij，位置距离D_ij则可以通过下式进行计算：

上述公式中，k_i为位置描述子L_i中所有子位置描述符的数量，k_suc为位置描述子L_i和L_j中能够产生成功匹配的子位置描述符的数量，^ijD_k1k2表示两个子位置描述符ⁱL_k1和ⁱL_k2之间的匹配距离，该距离被称为“子位置距离”，其中，ⁱL_k1为位置描述子L_i中的一个子位置描述符，而^jL_k2为位置描述子L_j中的一个子位置描述符，两个子位置描述符之间的子位置距离可以通过下式计算获得：

上述公式中，n_k1为子位置描述符ⁱL_k1中包含的所有位置关系的数量，n_suc为子位置描述符ⁱL_k1和^jL_k2中，能够产生成功匹配的位置关系的数量，表示两个位置关系和之间的“几何距离”，其中，为子位置描述符ⁱL_k1中的一个位置关系，为子位置描述符^jL_k2中的一个位置关系；假设位置关系 则这两个位置关系之间的几何距离可以通过下式计算获得：

通过计算两个位置描述子之间的位置距离来判断两个点云聚类位置特征的相似程度，位置距离越小，代表两个点云聚类的位置特征越相近。

10.根据权利要求8所述的一种基于聚类地图和位置描述子匹配的重定位方法，其特征在于，步骤b3中选取三个几何筛选条件对步骤b2中的点云聚类的匹配关系进行筛选，经过三个几何筛选条件筛选后从每个候选项集合中挑选出一个最佳匹配对象，获得临时聚类地图和原始聚类地图之间的点云聚类的一一对应关系，三个所述几何筛选条件分别为长度几何筛选条件、三角形几何筛选条件和包含几何筛选条件。