CN116148808B - 一种基于点云描述符的自动驾驶激光重定位方法和系统 - Google Patents

Abstract

本发明公开了一种基于点云描述符的自动驾驶激光重定位方法和系统，属于自动驾驶技术领域。本发明提供的自动驾驶激光重定位装置包括：激光建图模块，用于构建全局特征点云地图；描述符构建与匹配模块，用于构建点云描述符数据库；位姿优化模块，用于在自动驾驶车辆开始定位时或者定位丢失的情形下获得候选关键帧集合以及当前帧位姿集合，然后计算出每一候选关键帧所预测的当前时刻车辆位姿，作为匹配初值，再利用正态分布变换方法将当前帧与激光建图模块提供的全局特征点云地图进行匹配，对匹配初值进行优化，再者利用迭代最近点算法进一步优化车辆位姿，最后筛选出当前时刻最优的车辆位姿。本发明能够提高激光重定位的准确性，并降低重定位耗时。

Description

一种基于点云描述符的自动驾驶激光重定位方法和系统

技术领域

本发明涉及自动驾驶技术领域，特别是关于一种基于点云描述符的自动驾驶激光重定位方法和系统。

背景技术

近年来，自动驾驶技术研究已成为热点和趋势。自动驾驶技术包含环境感知、定位导航、路径规划和决策控制。自动驾驶中的定位模块涉及重定位，重定位是指基于先验地图完成定位的过程，可有效解决自动驾驶定位丢失的情形。目前自动驾驶重定位常用解决方案分为两类：基于GNSS(Global Navigation Satellite System，全球导航卫星系统)和基于图像描述符。这两种方案均存在不足之处，GNSS的信号和精度会受环境的影响，而光照变化明显或环境纹理特征缺失会影响图像特征的提取与匹配。激光点云描述符不受光照的影响，同时激光同时定位与建图不受信号的限制，利用点云描述符可扩展重定位适用场景。目前，基于点云描述符的重定位探索相对较少。

尽管现有一专利文献公布了一种基于激光雷达的重定位装置和方法，它是提前采集环境信息，构建完整点云地图，并生成包含关键帧信息的点云库；当机器人定位丢失时，获取机器人当前帧，基于Scan Context(中文意思是：点云的几何信息)描述符，将机器人当前帧与点云库进行匹配搜索，获得搜索匹配对应的关键帧；计算当前的激光点云和关键帧的快速点特征直方图，使用截断最小乘和半定松弛算法获得激光点云和关键帧的相对旋转变量，并利用迭代最近点(英文全称为“Iterative Closest Point”，简称为“ICP”)算法优化机器人全局位姿信息以实现重定位。但是，其中的Scan Context描述符只利用了点云的最大高度信息，同时不具有旋转不变性。利用Scan Context描述符实现点云库的搜索与匹配，需要进行暴力搜索。基于FPFH的匹配导致算法复杂度较高，计算时间过长。

另外，现有的基于点云的大规模建图场景的快速重定位方法公开了使用同时定位与建图(英文全称为“Simultaneous Localization and Mapping”，简称为“SLAM”)技术进行建图，建图过程中，生成3D点云的关键帧和对应环-域特征向量库、多分辨率子地图库以及进行数据预处理，所述数据预处理为：计算多分辨率子地图树形结构中的每个节点的分数，生成节点分数数据库；开始重定位，使用当前时刻传感器数据，进行环-域特征向量匹配，得到候选子地图库；从候选子地图中得到最佳子地图和载具的位姿初值；在最佳子地图中进行精匹配，得到最终重定位结果。但是，其中的多分辨率子地图库会明显增加重定位的运算量，同时该系统还缺乏重定位正确性的检验策略，有可能出现错误的重定位信息。

发明内容

本发明的目的在于提供一种基于点云描述符的自动驾驶激光重定位方法和系统，以提高激光重定位的准确性，降低重定位耗时。

为实现上述目的，本发明提供一种基于点云描述符的自动驾驶激光重定位方法，其包括：

步骤1，根据多线激光雷达采集的原始点云，构建全局特征点云地图；

步骤2，根据多线激光雷达采集的原始点云以及步骤1提供的全局特征点云地图，利用基于帧与地图的匹配方法，采集关键帧对应的位姿和点云数据，再根据点云中的点云描述符数据结构，构建点云描述符数据库，在自动驾驶车辆开始定位时或者定位丢失的情形下，根据当前帧对应的激光雷达数据以及点云描述符数据库，构建车辆当前帧对应的点云描述符，再根据当前帧对应的点云描述符，在点云描述符数据库中进行匹配搜索，寻找满足相似度阈值的候选关键帧，从而获得候选关键帧集合以及当前帧位姿集合；

步骤3，根据候选关键帧集合和当前帧位姿集合，计算出每一候选关键帧所预测的当前时刻车辆位姿，作为匹配初值，再利用正态分布变换方法将当前帧与步骤1提供的全局特征点云地图进行匹配，以优化匹配初值，然后利用迭代最近点算法进一步优化车辆位姿，最后筛选出当前时刻最优的车辆位姿；

步骤2中，“点云描述符数据结构”包括：Scan Context、带高度的俯视图、每个环的最大高度平均值及其转化后的数组和每个轴向扇形区域的最大高度平均值；步骤2具体包括：

步骤21，将当前帧转化为鸟瞰图，再根据高度信息进行编码，获得点云描述符；“根据高度信息进行编码”的方法具体包括：首先，在鸟瞰图上以激光雷达的位置为中心，选取一个预设面积和形状的有效感应区，然后，以有效感应区为单位，将当前帧离散为若干预设尺寸的方框，再者，使用八位二进制代码对同一个方框内的所有点进行编码，最后，将每个方框的8位二进制数转换为十进制数，作为点云描述符的像素强度，进而获得点云描述符；

步骤22，分别从当前帧所构建的点云描述符与每一历史关键帧所对应的点云描述符提取二值特征；

步骤23，根据提取的二值特征，计算当前帧所构建的点云描述符与每一历史关键帧之间的汉明距离，并将小于汉明距离阈值的历史关键帧作为候选帧；其中，历史关键帧对应角点特征集合和平面点特征集合根据不同时刻间相对位姿统一变换到初始坐标系下，实现角点局部点云地图和平面点局部点云地图的构建；

步骤24，将候选帧对应的点云描述符进行傅里叶变换，获得当前帧所构建的点云描述符与每一历史关键帧所对应的点云描述符之间的平移量和旋转量；

步骤2中，“自动驾驶车辆定位丢失的情形”根据前后两帧定位变化量是否异常判定；

步骤3包括：

步骤31，利用正态分布变换方法将当前帧与步骤1提供的全局特征点云地图进行匹配，对匹配初值进行优化，得到优化后的车辆位姿；

步骤32，在步骤2获取的候选关键帧集合中，利用迭代最近点算法将步骤31提供的优化后的车辆位姿进行帧与局部点云地图的匹配，获取车辆当前时刻最优位姿，再将车辆当前时刻最优位姿投影当前帧，计算当前帧与全局特征点云地图的重合度；步骤32中的迭代优化方法的具体实现方式为：在局部点云地图中利用KD树算法快速寻找出角点特征对应的直线以及平面点特征对应的平面，从而利用点到直线以及点到平面的距离构建激光里程计残差，并通过ceres优化求解器进行迭代优化；

步骤33，进行点云重合度排序和筛选；其中，重合度排序用于剔除小于重合度阈值的候选关键帧；为了从候选关键帧集合中筛选出对应历史帧，利用点云匹配重合度进行筛选，即在迭代最近点迭代求解过程中，计算点到直线和点到平面的距离是否满足对应阈值，叠加迭代求解过程中满足阈值的对应点数量，完成集合中所有候选关键帧对应点数总量，根据数量进行排序，选取数量最高的那组作为最终的优化位姿；

步骤34，根据一致性检验，获取车辆当前时刻最优位姿。

进一步地，步骤1具体包括：

步骤11，根据采集到的当前帧中邻域点信息，计算点云的曲率，再根据曲率值排序，筛选出特征点云，特征点云包括角点和平面点；

步骤12，先根据上一时刻优化的车辆位姿，更新局部点云地图，再将该局部点云地图与当前帧匹配，构建角点到其所对应的直线、以及平面点到其所对应的平面的激光里程计残差，并优化；

步骤13，通过回环检测，选出正确的回环候选帧，构建角点到其所对应的直线、以及平面点到其所对应的平面的激光回环检测残差；

步骤14，根据激光回环检测残差，计算车辆的累计漂移，还根据关键帧构建位姿图，利用优化库优化求解器更新回环帧中的历史位姿信息，最后根据优化后的历史位姿信息和特征点云构建环境地图，输出全局特征点云地图。

本发明还提供一种基于点云描述符的自动驾驶激光重定位系统，其包括：

激光建图模块，用于根据多线激光雷达采集的原始点云，构建全局特征点云地图；

描述符构建与匹配模块，用于根据多线激光雷达采集的原始点云以及激光建图模块提供的全局特征点云地图，利用基于帧与地图的匹配方法，采集关键帧对应的位姿和点云数据，再根据点云中的点云描述符数据结构，构建点云描述符数据库；以及用于在自动驾驶车辆开始定位时或者定位丢失的情形下，根据当前帧对应的激光雷达数据以及点云描述符数据库，构建车辆当前帧对应的点云描述符，再根据当前帧对应的点云描述符，在点云描述符数据库中进行匹配搜索，寻找满足相似度阈值的候选关键帧，从而获得候选关键帧集合以及当前帧位姿集合；

位姿优化模块，用于根据候选关键帧集合以及当前帧位姿集合，计算出每一候选关键帧所预测的当前时刻车辆位姿，作为匹配初值，再利用正态分布变换方法将当前帧与激光建图模块提供的全局特征点云地图进行匹配，对匹配初值进行优化，再者利用迭代最近点算法进一步优化车辆位姿，最后筛选出当前时刻最优的车辆位姿；

“点云描述符数据结构”包括：Scan Context、带高度的俯视图、每个环的最大高度平均值及其转化后的数组和每个轴向扇形区域的最大高度平均值；描述符构建与匹配模块(2)包括点云描述符构建单元(21)、二值特征提取单元(22)、二值特征匹配单元(23)和点云描述符傅立叶变换单元(24)，其中：

点云描述符构建单元(21)用于将当前帧转化为鸟瞰图，再根据高度信息进行编码，获得点云描述符，“根据高度信息进行编码”的方法具体包括：首先，在鸟瞰图上以激光雷达的位置为中心，选取一个预设面积和形状的有效感应区，然后，以有效感应区为单位，将当前帧离散为若干预设尺寸的方框，再者，使用八位二进制代码对同一个方框内的所有点进行编码，最后，将每个方框的8位二进制数转换为十进制数，作为点云描述符的像素强度，进而获得点云描述符；二值特征提取单元(22)用于分别从当前帧所构建的点云描述符与历史帧集合中一帧点云所对应的点云描述符提取二值特征；

二值特征匹配单元(23)用于根据提取的二值特征，计算当前帧所构建的点云描述符与每一历史关键帧之间的汉明距离，并将小于汉明距离阈值的历史关键帧作为候选帧；其中，历史关键帧对应角点特征集合和平面点特征集合根据不同时刻间相对位姿统一变换到初始坐标系下，实现角点局部点云地图和平面点局部点云地图的构建；

点云描述符傅立叶变换单元(24)用于将候选帧对应的点云描述符进行傅里叶变换，获得当前帧所构建的点云描述符与每一历史关键帧所对应的点云描述符之间的平移量和旋转量；

位姿优化模块(3)具体包括正态分布变换粗匹配单元(31)、迭代最近点精匹配单元(32)、位姿排序筛选单元(33)和一致性检验单元(34)，其中：

正态分布变换粗匹配单元(31)用于利用正态分布变换方法将当前帧与激光建图模块(1)提供的全局特征点云地图进行匹配，对描述符构建与匹配模块(2)提供的匹配初值进行优化，得到优化后的车辆位姿；

迭代最近点精匹配单元(32)用于在描述符构建与匹配模块(2)获取的候选关键帧集合中，利用迭代最近点算法将正态分布变换粗匹配单元(31)提供的优化后的车辆位姿进行帧与局部点云地图的匹配，获取车辆当前时刻最优位姿，再将车辆当前时刻最优位姿投影当前帧，计算当前帧与全局特征点云地图的重合度；其中的迭代优化方法的具体实现方式为：在局部点云地图中利用KD树算法快速寻找出角点特征对应的直线以及平面点特征对应的平面，从而利用点到直线以及点到平面的距离构建激光里程计残差，并通过ceres优化求解器进行迭代优化；

位姿排序筛选单元(33)用于进行点云重合度排序和筛选；其中，重合度排序用于剔除小于重合度阈值的候选关键帧；为了从候选关键帧集合中筛选出对应历史帧，利用点云匹配重合度进行筛选，即在迭代最近点迭代求解过程中，计算点到直线和点到平面的距离是否满足对应阈值，叠加迭代求解过程中满足阈值的对应点数量，完成集合中所有候选关键帧对应点数总量，根据数量进行排序，选取数量最高的那组作为最终的优化位姿；

一致性检验单元(34)用于根据一致性检验，获取车辆当前时刻最优位姿。

进一步地，激光建图模块具体包括：

点云特征提取单元，用于根据采集到的当前帧中邻域点信息，计算点云的曲率，再根据曲率值排序，筛选出特征点云，特征点云包括角点和平面点；

帧与地图匹配单元，用于先根据上一时刻优化的车辆位姿，更新局部点云地图，再将该局部点云地图与当前帧匹配，构建角点到其所对应的直线、以及平面点到其所对应的平面的激光里程计残差，并优化；

回环检测单元，用于通过回环检测，选出正确的回环候选帧，构建角点到其所对应的直线、以及平面点到其所对应的平面的激光回环检测残差；

位姿图优化单元，用于根据激光回环检测残差，计算车辆的累计漂移，还根据关键帧构建位姿图，利用优化库优化求解器更新回环帧中的历史位姿信息，最后根据优化后的历史位姿信息和特征点云构建环境地图，输出全局特征点云地图。

进一步地，回环检测单元通过回环检测，选出正确的回环候选帧的方法包括利用欧式距离和点云描述符相结合的方式进行回环检测，具体包括：

首先，利用k近邻算法寻找角点到其所对应的直线、以及平面点到其所对应的平面，再计算角点到其所对应的直线、以及平面点到其所对应的平面的距离；

然后，将满足最小距离阈值的特征点云作为匹配点，再计算匹配与特征点云的数目比值；

最后，判断数目比值是否满足设定阈值，如果是，则认定回环候选帧为正确回环。

由于本发明根据点云描述符信息，提供对比点云库，快速确定对应历史关键帧，并结合正态分布变换(英文全称为“Normal Distance Transform”，简称为“NDT”)方法和迭代最近点匹配方法准确估算车辆当前时刻位姿，其具有以下优点：

1.本发明利用了点云描述符所具有旋转不变性，避免了暴力搜索，节约了计算资源。同时，现有的点云全局描述符相比，该描述符的性能较高，从而实现了重定位地快速、准确完成。

2.在激光建图模块中利用欧式距离和点云描述符相结合的方式进行回环检测，提高了回环检测的准确性。

3.融合正态分布变换和迭代最近点匹配算法实现位姿优化，双重匹配方式扩展了重定位的有效范围，同时通过点云匹配重合度的筛选和一致性检验，保证了重定位的准确性。

附图说明

图1为本发明实施例的基于点云描述符的自动驾驶激光重定位的架构图。

具体实施方式

下面结合附图和实施例对本发明进行详细的描述。

如图1所示，本发明实施例提供的基于点云描述符的自动驾驶激光重定位系统包括激光建图模块1、描述符构建与匹配模块2以及位姿优化模块3，其中：

激光建图模块1用于根据多线激光雷达采集的原始点云，构建待定位环境的特征点云地图，即全局特征点云地图。

例如，可以先在无人或少量动态障碍物的情况下，控制车辆匀速遍历定位环境所在的空间，通过多线激光雷达等雷达设备采集点云，再基于激光同时定位与建图构建得到全局特征点云地图。

优选地，在车辆遍历环境空间的过程中，可以控制车辆按照回环轨迹行驶，这样可以降低累计漂移。此外，激光雷达的扫描范围可以是几十米到一百米不等，只要保证能建图过程中能够扫完整个工作区域即可。

作为激光建图模块1的一种实现方式，其具体包括点云特征提取单元11、帧与地图匹配单元12、回环检测单元13和位姿图优化单元14。其中：

点云特征提取单元11用于根据采集到的当前帧中邻域点信息，计算点云的曲率，再根据曲率值排序，筛选出特征点云，特征点云包括角点和平面点。

在一个实施例中，“根据采集到的当前帧中邻域点信息，计算点云的曲率”的方法可以根据角度信息确定点云所属线束关系，在每条扫描线上利用邻域点集合S进行点云曲率计算，如下式所示：

其中，c表示点云的局部曲率，L表示激光雷达坐标系，k表示第k帧点云，i表示第i个点，第i个点在下文简化为点i，j表示点i的第j个邻域点，第j个领域点在下文简化为邻域点j，领域点可以理解为点i左右若干(例如5)个点中的一个，表示测量到的在激光雷达坐标系L下第k帧点云中点i的坐标值，/>表示测量到的在激光雷达坐标系L下第k帧点云中的邻域点j的坐标值。

在一个实施例中，“筛选特征点云”的原则具体包括：

原则一，需去除的点云的特征包括：

1)可能被遮挡的点。

2)已经选过点周围的点。

3)位于激光线接近平行的平面点。

原则二，角点满足曲率大于角点阈值的点云。其中，角点阈值需要保证提取到的角点特征数量不是特别多，一般为点云总数量的10％即可，保证算法运行的实时，本实施例中，角点阈值设置为0.1。

原则三，平面点满足曲率小于平面点阈值的点云。其中，平面点阈值例如可以是0.05。

在一个实施例中，为了实现特征点云的均匀采样，每条扫描线束均匀划分为若干个区域，再在每个区域筛选若干个角点和平面点。

帧与地图匹配单元12用于先根据上一时刻优化的车辆位姿，更新局部点云地图，再将该局部点云地图与当前帧匹配，构建角点到其所对应的直线、以及平面点到其所对应的平面的激光里程计残差，并优化。

其中，局部点云地图用于确定准确的特征对应关系，根据平移和旋转的变化量，筛选关键帧，同时为保证特征规模及匹配搜索效率，对于局部点云地图点云进行降采样操作。同样地，优化方法比如可以是高斯牛顿法原理等方法。

回环检测单元13用于通过回环检测，选出正确的回环候选帧，构建角点到其所对应的直线、以及平面点到其所对应的平面的激光回环检测残差，从而根据历史位姿信息和当前位姿信息计算累计漂移，进而减小激光里程计的累计漂移。

其中，回环检测单元13通过回环检测选出正确的回环候选帧，这样能够提高回环检测的准确性。优选地，该回环检测方法包括利用欧式距离和点云描述符相结合的方式进行回环检测，具体包括：

首先，利用k近邻算法寻找角点到其所对应的直线、以及平面点到其所对应的平面，再计算角点到其所对应的直线、以及平面点到其所对应的平面的距离；

然后，将满足最小距离阈值的特征点云作为匹配点，再计算匹配与特征点云的数目比值；

最后，判断数目比值是否满足设定阈值，如果是，则认定回环候选帧为正确回环。

其中，“角点到其所对应的直线、以及平面点到其所对应的平面”可以采用但不限于KD树算法，在局部点云地图中快速找到。最小距离阈值例如可以是0.1m，设定阈值可以是好0.75。

位姿图优化单元14用于根据激光回环检测残差，计算车辆的累计漂移，还根据关键帧构建位姿图，利用Ceres、g2o以及gtsam等优化库优化求解器更新回环帧中的历史位姿信息，最后根据优化后的历史位姿信息和特征点云构建环境地图，输出全局特征点云地图。其中，历史位姿信息指根据当前帧所找到的对应回环帧所对应时刻车辆位姿。

融合正态分布变换和迭代最近点匹配算法实现位姿优化，双重匹配方式扩展了重定位的有效范围，实现更远距离的重定位，一定程度上可以减小点云描述符数据库的大小，提高运算效率，同时通过点云匹配重合度的筛选和一致性检验保证了重定位的准确性。

除了采用上述步骤提供的SLAM算法实现激光建图模块1的功能，还可以通过变化其中的部分细节，实现激光建图模块1的功能。

描述符构建与匹配模块2用于根据多线激光雷达采集的原始点云以及激光建图模块1提供的全局特征点云地图，利用基于帧与地图的匹配方法，采集关键帧对应的位姿和点云数据，再根据点云中的点云描述符数据结构，构建点云描述符数据库。

本实施例通过点云描述符数据库，可以尽可能地降低重定位耗时，便于后续步骤的重定位。

其中，“关键帧”可以根据距离的差异以及角度的差异确定得到，比如需要每超过1m或者角度超过30度取一帧作为关键帧，那么，将原始点云的第一帧作为初始关键帧，然后随着车辆的运动，当前帧与上一个关键帧之间的距离或角度差异超过设定阈值，则当前帧作为新的一关键帧。

“点云描述符数据结构”包括：Scan Context、带高度的俯视图、每个环的最大高度平均值及其转化后的数组和每个轴向扇形区域(sector)的最大高度平均值。

描述符构建与匹配模块2还用于根据当前帧对应的激光雷达数据以及点云描述符数据库，构建车辆当前帧对应的点云描述符，再根据当前帧对应的点云描述符，在点云描述符数据库中进行匹配搜索，寻找满足相似度阈值(可以选取但不限于0.65)的候选关键帧，从而获得候选关键帧集合以及当前帧位姿集合。

作为描述符构建与匹配模块2的一种实现方式，可以具体包括点云描述符构建单元21、二值特征提取单元22、二值特征匹配单元23和点云描述符傅立叶变换单元24。其中：

点云描述符构建单元21用于将当前帧转化为鸟瞰图，再根据高度信息进行编码，获得点云描述符。

在一个实施例中，“根据高度信息进行编码”的方法具体包括：

首先，在鸟瞰图上以激光雷达的位置为中心，选取一个预设面积和形状的有效感应区，例如：面积为k×k的正方形，k为经验值,选择正方形是为了便于计算。有效感应区的形状也可以选择其他形状，比如圆形，只要方便计算即可。

然后，以有效感应区为单位，将当前帧离散为若干预设尺寸的方框，例如：将点云离散为80(径向)*360(角向)的方框。也可以选择其的数值进行划分，预设尺寸为经验值，只要能够平衡精度和耗时。

再者，使用八位二进制代码对同一个方框内的所有点进行编码。例如：对于每一个方框内的点云集，首先按照点云在鸟瞰图上对应的高度大小排序，线性离散为8个方框，有点的方框值为1，否则为0，从而获得8位二进制编码。

最后，将每个方框的8位二进制数转换为十进制数，作为点云描述符的像素强度，进而获得点云描述符。

在构建点云描述符时，为了完整表示点云信息，对每个面元内的点采用一些特征提取方法，还可以采用如高度、距离、反射强度等属性，均可以完成发明目的。

二值特征提取单元22用于分别从当前帧所构建的点云描述符与历史帧集合中一帧点云所对应的点云描述符提取二值特征。

其中，可以使用LoG-Gabor滤波器从点云描述符中深入提取特征，因为LoG-Gabor滤波器能够将点云描述符中的数据分解为以不同分辨率出现的分量，与传统的傅里叶变换相比，LoG-Gabor滤波器的优势在于允许频率数据局部化，允许在相同位置和分辨率进行特征匹配。在一个实施例中，为了确保实时性，可以使用一维LoG Gabor滤波器。当然，也可以采用现有的其他LoG Gabor滤波器实现。

二值特征匹配单元23用于根据提取的二值特征，计算当前帧所构建的点云描述符与每一历史关键帧之间的汉明距离，并将小于汉明距离阈值的历史关键帧作为候选帧。

其中，历史关键帧对应角点特征集合和平面点特征集合根据不同时刻间相对位姿统一变换到初始坐标系下，实现角点局部点云地图和平面点局部点云地图的构建。汉明距离阈值比如可以是10。

点云描述符傅立叶变换单元24用于将候选帧对应的点云描述符进行傅里叶变换，获得当前帧所构建的点云描述符与每一历史关键帧所对应的点云描述符之间的平移量和旋转量。

平移变化量会显著影响点云描述符。为了解决这个问题，采用傅里叶变换估计两幅点云描述符之间的平移。基于傅里叶变换的方案能够估计大的旋转和平移。点云的旋转对应于经过傅里叶变换后的点云描述符的水平平移,点云的平移不仅对应傅里叶变换后的点云描述符的垂直平移，还会引起点云描述符像素强度的轻微变化,但是点云描述符以方框为最小单位保留了点云的绝对内部结构，提高了辨别能力并且对图像像素强度的变化具有鲁棒性。

需要说明的是，还可以采用其他现有方法实现描述符构建与匹配模块2的功能，在在此不再赘述。

位姿优化模块3用于在自动驾驶车辆开始定位时或者定位丢失的情形下，根据候选关键帧集合以及当前帧位姿集合，计算出每一候选关键帧所预测的当前时刻车辆位姿，作为匹配初值，再利用正态分布变换方法将当前帧与激光建图模块1提供的全局特征点云地图进行匹配，对匹配初值进行优化，再者利用迭代最近点算法进一步优化车辆位姿，最后筛选出当前时刻最优的车辆位姿。

其中，“自动驾驶车辆定位丢失的情形”可以根据前后两帧定位变化量是否异常判定。

作为位姿优化模块3的一种方式，其具体包括正态分布变换粗匹配单元31、迭代最近点精匹配单元32、位姿排序筛选单元33和一致性检验单元34。其中：

正态分布变换粗匹配单元31用于利用正态分布变换方法将当前帧与激光建图模块1提供的全局特征点云地图进行匹配，对描述符构建与匹配模块2提供的匹配初值进行优化，得到优化后的车辆位姿。

迭代最近点精匹配单元32用于在描述符构建与匹配模块2获取的候选关键帧集合中，利用迭代最近点算法将正态分布变换粗匹配单元31提供的优化后的车辆位姿进行帧与局部点云地图的匹配，获取车辆当前时刻最优位姿，再将车辆当前时刻最优位姿投影当前帧，计算当前帧与全局特征点云地图的重合度。

为进一步提高车辆当前位姿的精度，“迭代最近点精匹配单元32”中的迭代优化方法的具体实现方式为：在局部点云地图中利用KD树算法快速寻找出角点特征对应的直线以及平面点特征对应的平面，从而利用点到直线以及点到平面的距离构建激光里程计残差，并通过ceres优化求解器进行迭代优化。

位姿排序筛选单元33用于进行点云重合度排序和筛选。

其中，重合度是数值，可以按照从高到低排序，目的在于剔除小于重合度阈值的候选关键帧。重合度阈值比如可以是0.75。

为了从候选关键帧集合中筛选出对应历史帧，利用点云匹配重合度进行筛选，即在迭代最近点迭代求解过程中，计算点到直线和点到平面的距离是否满足对应阈值，叠加迭代求解过程中满足阈值的对应点数量，完成集合中所有候选关键帧对应点数总量。即，根据数量进行排序，选取数量最高的那组作为最终的优化位姿。其中，点到直线和点到平面的距离阈值均可以为0.1。

一致性检验单元34用于根据一致性检验，获取车辆当前时刻最优位姿。

上述实施例是实现位姿优化模块3的功能的方式之一，还可以采用其他方式实现。

上述实施例提供的重定位的准确性对于自动驾驶非常重要。为了保证重定位的准确性，需要连续三次重定位成功方可认为重定位完成，重定位成功的条件是：三次对应历史帧之间的距离小于特定阈值(例如1m)，同时三次估计的车辆位姿与对应历史帧的距离满足特定阈值。

本发明实施例提供的基于点云描述符的自动驾驶激光重定位方法包括：

步骤1，根据多线激光雷达采集的原始点云，构建待定位环境的特征点云地图，即全局特征点云地图。

步骤2，根据多线激光雷达采集的原始点云以及步骤1提供的全局特征点云地图，利用基于帧与地图的匹配方法，采集关键帧对应的位姿和点云数据，再根据点云中的点云描述符数据结构，构建点云描述符数据库。

由于点云描述符数据库中存储了每个关键帧所对应的车辆位姿信息，该位姿信息包括车辆的平移量和旋转量。步骤2则是在自动驾驶车辆开始定位时或者定位丢失的情形下，根据当前帧对应的点云以及点云描述符数据库，构建车辆当前帧对应的点云描述符，如点云描述符数据库构建方法。再根据当前帧对应的点云描述符，在点云描述符数据库中进行匹配搜索，寻找满足相似度阈值的候选关键帧，从而获得当前帧与每一候选关键帧的转换位姿，进而通过候选关键帧获得候选关键帧集合，通过当前帧与每一候选关键帧的转换位姿和当前帧本身所对应的车辆位姿，获得当前帧位姿集合。

步骤3，根据候选关键帧集合和当前帧位姿集合，计算出每一候选关键帧所预测的当前时刻车辆位姿，作为匹配初值，再利用正态分布变换方法将当前帧与步骤1提供的全局特征点云地图进行匹配，以优化匹配初值，然后利用迭代最近点算法进一步优化车辆位姿，最后根据点云匹配重合度和一致性检验，筛选出当前时刻最优的车辆位姿，以实现重定位。

本实施例利用点云描述符实现激光重定位，点云描述符具有旋转不变性，避免了暴力搜索，节约了计算资源。同时，现有的点云全局描述符相比，该描述符的性能较高，从而实现了重定位的快速、准确完成。

在一个实施例中，步骤1具体包括：

步骤11，根据采集到的当前帧中邻域点信息，计算点云的曲率，再根据曲率值排序，筛选出特征点云，特征点云包括角点和平面点；

步骤12，先根据上一时刻优化的车辆位姿，更新局部点云地图，再将该局部点云地图与当前帧匹配，构建角点到其所对应的直线、以及平面点到其所对应的平面的激光里程计残差，并优化；

步骤13，通过回环检测，选出正确的回环候选帧，构建角点到其所对应的直线、以及平面点到其所对应的平面的激光回环检测残差；

步骤14，根据激光回环检测残差，计算车辆的累计漂移，还根据关键帧构建位姿图，利用优化库优化求解器更新回环帧中的历史位姿信息，最后根据优化后的历史位姿信息和特征点云构建环境地图，输出全局特征点云地图。

在一个实施例中，步骤2具体包括：

步骤21，将当前帧转化为鸟瞰图，再根据高度信息进行编码，获得点云描述符；

步骤22，分别从当前帧所构建的点云描述符与每一历史关键帧所对应的点云描述符提取二值特征；

步骤23，根据提取的二值特征，计算当前帧所构建的点云描述符与每一历史关键帧之间的汉明距离，并将小于汉明距离阈值的历史关键帧作为候选帧；

步骤24，将候选帧对应的点云描述符进行傅里叶变换，获得当前帧所构建的点云描述符与每一历史关键帧所对应的点云描述符之间的平移量和旋转量。

在一个实施例中，步骤3包括：

步骤31，利用正态分布变换方法将当前帧与步骤1提供的全局特征点云地图进行匹配，对匹配初值进行优化，得到优化后的车辆位姿；

步骤32，在步骤2获取的候选关键帧集合中，利用迭代最近点算法将步骤31提供的优化后的车辆位姿进行帧与局部点云地图的匹配，获取车辆当前时刻最优位姿，再将车辆当前时刻最优位姿投影当前帧，计算当前帧与全局特征点云地图的重合度；

步骤33，进行点云重合度排序和筛选；

步骤34，根据一致性检验，获取车辆当前时刻最优位姿。

最后需要指出的是：以上实施例仅用以说明本发明的技术方案，而非对其限制。本领域的普通技术人员应当理解：可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。

Claims (5)

1.一种基于点云描述符的自动驾驶激光重定位方法，其特征在于，包括：

步骤1，根据多线激光雷达采集的原始点云，构建全局特征点云地图；

步骤2，根据多线激光雷达采集的原始点云以及步骤1提供的全局特征点云地图，利用基于帧与地图的匹配方法，采集关键帧对应的位姿和点云数据，再根据点云中的点云描述符数据结构，构建点云描述符数据库；在自动驾驶车辆开始定位时或者定位丢失的情形下，根据当前帧对应的激光雷达数据以及点云描述符数据库，构建车辆当前帧对应的点云描述符，再根据当前帧对应的点云描述符，在点云描述符数据库中进行匹配搜索，寻找满足相似度阈值的候选关键帧，从而获得候选关键帧集合以及当前帧位姿集合；

步骤3，根据候选关键帧集合和当前帧位姿集合，计算出每一候选关键帧所预测的当前时刻车辆位姿，作为匹配初值，再利用正态分布变换方法将当前帧与步骤1提供的全局特征点云地图进行匹配，以优化匹配初值，然后利用迭代最近点算法进一步优化车辆位姿，最后筛选出当前时刻最优的车辆位姿；

步骤2中，“点云描述符数据结构”包括：Scan Context、带高度的俯视图、每个环的最大高度平均值及其转化后的数组和每个轴向扇形区域的最大高度平均值；步骤2具体包括：

步骤21，将当前帧转化为鸟瞰图，再根据高度信息进行编码，获得点云描述符；“根据高度信息进行编码”的方法具体包括：首先，在鸟瞰图上以激光雷达的位置为中心，选取一个预设面积和形状的有效感应区，然后，以有效感应区为单位，将当前帧离散为若干预设尺寸的方框，再者，使用八位二进制代码对同一个方框内的所有点进行编码，最后，将每个方框的8位二进制数转换为十进制数，作为点云描述符的像素强度，进而获得点云描述符；

步骤22，分别从当前帧所构建的点云描述符与每一历史关键帧所对应的点云描述符提取二值特征；

步骤23，根据提取的二值特征，计算当前帧所构建的点云描述符与每一历史关键帧之间的汉明距离，并将小于汉明距离阈值的历史关键帧作为候选帧；其中，历史关键帧对应角点特征集合和平面点特征集合根据不同时刻间相对位姿统一变换到初始坐标系下，实现角点局部点云地图和平面点局部点云地图的构建；

步骤24，将候选帧对应的点云描述符进行傅里叶变换，获得当前帧所构建的点云描述符与每一历史关键帧所对应的点云描述符之间的平移量和旋转量；

步骤2中，“自动驾驶车辆定位丢失的情形”根据前后两帧定位变化量是否异常判定；

步骤3包括：

步骤31，利用正态分布变换方法将当前帧与步骤1提供的全局特征点云地图进行匹配，对匹配初值进行优化，得到优化后的车辆位姿；

步骤32，在步骤2获取的候选关键帧集合中，利用迭代最近点算法将步骤31提供的优化后的车辆位姿进行帧与局部点云地图的匹配，获取车辆当前时刻最优位姿，再将车辆当前时刻最优位姿投影当前帧，计算当前帧与全局特征点云地图的重合度；步骤32中的迭代优化方法的具体实现方式为：在局部点云地图中利用KD树算法快速寻找出角点特征对应的直线以及平面点特征对应的平面，从而利用点到直线以及点到平面的距离构建激光里程计残差，并通过ceres优化求解器进行迭代优化；

步骤33，进行点云重合度排序和筛选；其中，重合度排序用于剔除小于重合度阈值的候选关键帧；为了从候选关键帧集合中筛选出对应历史帧，利用点云匹配重合度进行筛选，即在迭代最近点迭代求解过程中，计算点到直线和点到平面的距离是否满足对应阈值，叠加迭代求解过程中满足阈值的对应点数量，完成集合中所有候选关键帧对应点数总量，根据数量进行排序，选取数量最高的那组作为最终的优化位姿；

步骤34，根据一致性检验，获取车辆当前时刻最优位姿。

2.如权利要求1所述的基于点云描述符的自动驾驶激光重定位方法，其特征在于，步骤1具体包括：

步骤11，根据采集到的当前帧中邻域点信息，计算点云的曲率，再根据曲率值排序，筛选出特征点云，特征点云包括角点和平面点；

步骤12，先根据上一时刻优化的车辆位姿，更新局部点云地图，再将该局部点云地图与当前帧匹配，构建角点到其所对应的直线、以及平面点到其所对应的平面的激光里程计残差，并优化；

步骤13，通过回环检测，选出正确的回环候选帧，构建角点到其所对应的直线、以及平面点到其所对应的平面的激光回环检测残差；

步骤14，根据激光回环检测残差，计算车辆的累计漂移，还根据关键帧构建位姿图，利用优化库优化求解器更新回环帧中的历史位姿信息，最后根据优化后的历史位姿信息和特征点云构建环境地图，输出全局特征点云地图。

3.一种基于点云描述符的自动驾驶激光重定位系统，其特征在于，包括：

激光建图模块(1)，用于根据多线激光雷达采集的原始点云，构建全局特征点云地图；

描述符构建与匹配模块(2)，用于根据多线激光雷达采集的原始点云以及激光建图模块(1)提供的全局特征点云地图，利用基于帧与地图的匹配方法，采集关键帧对应的位姿和点云数据，再根据点云中的点云描述符数据结构，构建点云描述符数据库；以及用于在自动驾驶车辆开始定位时或者定位丢失的情形下，根据当前帧对应的激光雷达数据以及点云描述符数据库，构建车辆当前帧对应的点云描述符，再根据当前帧对应的点云描述符，在点云描述符数据库中进行匹配搜索，寻找满足相似度阈值的候选关键帧，从而获得候选关键帧集合以及当前帧位姿集合；

位姿优化模块(3)，用于根据候选关键帧集合以及当前帧位姿集合，计算出每一候选关键帧所预测的当前时刻车辆位姿，作为匹配初值，再利用正态分布变换方法将当前帧与激光建图模块(1)提供的全局特征点云地图进行匹配，对匹配初值进行优化，再者利用迭代最近点算法进一步优化车辆位姿，最后筛选出当前时刻最优的车辆位姿；

“点云描述符数据结构”包括：Scan Context、带高度的俯视图、每个环的最大高度平均值及其转化后的数组和每个轴向扇形区域的最大高度平均值；描述符构建与匹配模块(2)包括点云描述符构建单元(21)、二值特征提取单元(22)、二值特征匹配单元(23)和点云描述符傅立叶变换单元(24)，其中：

点云描述符构建单元(21)用于将当前帧转化为鸟瞰图，再根据高度信息进行编码，获得点云描述符，“根据高度信息进行编码”的方法具体包括：首先，在鸟瞰图上以激光雷达的位置为中心，选取一个预设面积和形状的有效感应区，然后，以有效感应区为单位，将当前帧离散为若干预设尺寸的方框，再者，使用八位二进制代码对同一个方框内的所有点进行编码，最后，将每个方框的8位二进制数转换为十进制数，作为点云描述符的像素强度，进而获得点云描述符；二值特征提取单元(22)用于分别从当前帧所构建的点云描述符与历史帧集合中一帧点云所对应的点云描述符提取二值特征；

二值特征匹配单元(23)用于根据提取的二值特征，计算当前帧所构建的点云描述符与每一历史关键帧之间的汉明距离，并将小于汉明距离阈值的历史关键帧作为候选帧；其中，历史关键帧对应角点特征集合和平面点特征集合根据不同时刻间相对位姿统一变换到初始坐标系下，实现角点局部点云地图和平面点局部点云地图的构建；

点云描述符傅立叶变换单元(24)用于将候选帧对应的点云描述符进行傅里叶变换，获得当前帧所构建的点云描述符与每一历史关键帧所对应的点云描述符之间的平移量和旋转量；

位姿优化模块(3)具体包括正态分布变换粗匹配单元(31)、迭代最近点精匹配单元(32)、位姿排序筛选单元(33)和一致性检验单元(34)，其中：

正态分布变换粗匹配单元(31)用于利用正态分布变换方法将当前帧与激光建图模块(1)提供的全局特征点云地图进行匹配，对描述符构建与匹配模块(2)提供的匹配初值进行优化，得到优化后的车辆位姿；

迭代最近点精匹配单元(32)用于在描述符构建与匹配模块(2)获取的候选关键帧集合中，利用迭代最近点算法将正态分布变换粗匹配单元(31)提供的优化后的车辆位姿进行帧与局部点云地图的匹配，获取车辆当前时刻最优位姿，再将车辆当前时刻最优位姿投影当前帧，计算当前帧与全局特征点云地图的重合度；其中的迭代优化方法的具体实现方式为：在局部点云地图中利用KD树算法快速寻找出角点特征对应的直线以及平面点特征对应的平面，从而利用点到直线以及点到平面的距离构建激光里程计残差，并通过ceres优化求解器进行迭代优化；

位姿排序筛选单元(33)用于进行点云重合度排序和筛选；其中，重合度排序用于剔除小于重合度阈值的候选关键帧；为了从候选关键帧集合中筛选出对应历史帧，利用点云匹配重合度进行筛选，即在迭代最近点迭代求解过程中，计算点到直线和点到平面的距离是否满足对应阈值，叠加迭代求解过程中满足阈值的对应点数量，完成集合中所有候选关键帧对应点数总量，根据数量进行排序，选取数量最高的那组作为最终的优化位姿；

一致性检验单元(34)用于根据一致性检验，获取车辆当前时刻最优位姿。

4.如权利要求3所述的基于点云描述符的自动驾驶激光重定位系统，其特征在于，激光建图模块(1)具体包括：

点云特征提取单元(11)，用于根据采集到的当前帧中邻域点信息，计算点云的曲率，再根据曲率值排序，筛选出特征点云，特征点云包括角点和平面点；

帧与地图匹配单元(12)，用于先根据上一时刻优化的车辆位姿，更新局部点云地图，再将该局部点云地图与当前帧匹配，构建角点到其所对应的直线、以及平面点到其所对应的平面的激光里程计残差，并优化；

回环检测单元(13)，用于通过回环检测，选出正确的回环候选帧，构建角点到其所对应的直线、以及平面点到其所对应的平面的激光回环检测残差；

位姿图优化单元(14)，用于根据激光回环检测残差，计算车辆的累计漂移，还根据关键帧构建位姿图，利用优化库优化求解器更新回环帧中的历史位姿信息，最后根据优化后的历史位姿信息和特征点云构建环境地图，输出全局特征点云地图。

5.如权利要求4所述的基于点云描述符的自动驾驶激光重定位系统，其特征在于，回环检测单元(13)通过回环检测，选出正确的回环候选帧的方法包括利用欧式距离和点云描述符相结合的方式进行回环检测，具体包括：

首先，利用k近邻算法寻找角点到其所对应的直线、以及平面点到其所对应的平面，再计算角点到其所对应的直线、以及平面点到其所对应的平面的距离；

然后，将满足最小距离阈值的特征点云作为匹配点，再计算匹配与特征点云的数目比值；

最后，判断数目比值是否满足设定阈值，如果是，则认定回环候选帧为正确回环。