CN115797425B - 一种基于点云鸟瞰图和由粗到精策略的激光全局定位方法 - Google Patents

Abstract

本发明涉及导航系统机器人全局定位领域，公开了一种基于点云鸟瞰图和由粗到精策略的激光全局定位方法，该方法充分考虑了当前全局定位方法的一些不足与定位成本问题，通过将激光点云表示成鸟瞰图，可以充分地利用稀疏点云的大部分信息，之后进行傅里叶变换和滤波处理，使其成为具有旋转和平移不变性的描述符，以进行特征检索；使用由粗到精两步定位的方式，在检索到粗略子场景后使用当前点云和局部点云地图配准的方式来实现精细的位姿估计，展现出了高精度的全局定位结果。

Description

一种基于点云鸟瞰图和由粗到精策略的激光全局定位方法

技术领域

本发明涉及导航系统机器人全局定位领域，具体涉及一种基于点云鸟瞰图和由粗到精策略的激光全局定位方法。

背景技术

随着科技的发展，通过机器人进行导航的应用越来越广泛，高精度定位是无人地面车辆实现自主导航的关键任务。目前，全球导航卫星系统（GNSS）是在室外环境中实现定位的主要方法。然而，GNSS的覆盖范围是有限的，并且在一些室外受限环境中定位精度会严重降低。因此，基于地图匹配的定位框架是一种替代方法。全局位置识别是获取车辆相对于环境的初始位姿，这是在先验地图中定位的前提。视觉传感器因其可以收集丰富的信息而被广泛应用于地点识别领域。然而，基于视觉的方法通常会受到光照和视野的影响，容易出现定位失败的结果。对于具备激光雷达的移动机器人而言，全局定位的方式往往是将当前的激光扫描数据与全局地图进行匹配，找到匹配程度最高的位置作为重定位的结果。这种做法对大范围的全局地图的构建精度有较高的要求，并且需要对全局位置都进行一次搜索，定位所需要的时间较长。考虑到激光雷达的成本问题，低线束如VLP-16激光雷达的成本较低，但是同时由于其线束较少所产生的激光点云往往是稀疏的，所包含的三维信息也较为稀疏。因此，需要一种更加有效和快速的、针对无序且稀疏激光点云的空间编码方法，使其可以充分利用点云的信息。其次，还需要一种与局部点云地图相适应的工程化全局定位框架。

中国专利《基于八叉树搜索反光柱的机器人全局定位方法及系统》（公开号：CN114593738A）通过对反光柱全局地图中的反光柱进行一次遍历,提取满足需求的反光柱三角形,并存储到八叉树数据结构中；遍历当前观测到的反光柱局部点云地图形成的反光柱三角形,利用八叉树快速搜索全局地图中与之相同的反光柱三角形；根据八叉树搜索的结果,利用三边定位算法计算当前机器人位姿,并根据当前机器人位姿校验当前观测到的其余反光柱是否与全局地图匹配。这种方案需要环境中存在特定几何结构的物体且对光度变换较为敏感，因此不适合将其推广到室外大规模场景中应用。

中国专利《一种基于GNSS、视觉以及IMU进行精确定位的数据融合算法》（公开号：CN114646993A）通过在移动设备上挂载GNSS卫星接收装置、单目相机以及IMU模块,在移动过程中采集GNSS信号、图像数据以及IMU数据,对视觉、IMU数据进行初始化对齐。之后通过图像数据进行前端视觉跟踪以及SFM视觉位姿求解来得到全局定位结果。这需要处理多种传感器的信号，并进行时序对齐与畸变去除，增加了定位结果的不确定性。例如GNSS信号在楼宇密集的场景下表现不佳，同时多种传感器的使用也提高了定位系统的复杂性。

目前的机器人全局定位的方法中缺乏一个适用于大规模室外场景下高精度的全局定位方案，主要存在的问题总结如下：

1）室外环境结构复杂，且重复性不高，缺乏几何结构规则的物体结构。

2）多传感器联合定位的方法比较盛行，因此对输入信号的处理上需要耗费大量算力，缺乏对单一传感器数据的深入研究。

本发明为了改善当前全局定位方法中缺乏对稀疏点云的编码方式这一现状，并考虑了当前全局定位方法的一些不足，如算法只适用于特定场景，传感器使用较多计算资源消耗高的等，提出了一种基于点云鸟瞰图和由粗到精策略的激光全局定位方法。

发明内容

本发明为了实现同步定位与建图（SLAM）系统中精准的全局位姿估计，提出了一种基于点云鸟瞰图和由粗到精策略的激光全局定位方法，该方法充分考虑了当前全局定位方法的一些不足与定位成本问题，通过将激光点云表示成鸟瞰图，可以充分地利用稀疏点云的大部分信息，之后进行傅里叶变换和滤波处理，使其成为具有旋转和平移不变性的描述符，以进行特征检索；使用由粗到精两步定位的方式，在检索到粗略子场景后使用当前点云和局部点云地图配准的方式来实现精细的位姿估计，展现出了高精度的全局定位结果。

本发明中的基于点云鸟瞰图和由粗到精策略的激光全局定位方法，具体包括以下步骤：

步骤一、激光点云帧的鸟瞰图表示：

将先验地图中的每个激光点云帧，以极坐标的形式表示为一个径向距离离散为[1m,80m]，角度离散为[0°,360°]的圆形鸟瞰图，鸟瞰图中的每个面元的值为面元中点云信息的二进制编码；

步骤二、具有平移不变形的傅里叶变换：

对每个由激光点云帧生成的鸟瞰图进行傅里叶变换，来使鸟瞰图具有平移方向上的不变性，由此避免鸟瞰图在一定范围内平移引起描述符的变化；

步骤三、LoG-Gabor滤波器的二值化特征提取：

使用LoG-Gabor滤波器从鸟瞰图的图像中深入提取特征，并通过简单地阈值运算将LoG-Gabor滤波器的滤波结果转成为二值化特征，将二值化特征叠加到每个鸟瞰图的二值化特征图中；将所有鸟瞰图的二值化特征图组合为描述符数据库；

步骤四、粗略子场景识别：

将当前的激光点云帧表示成二值化特征图，在由先验地图生成的描述符数据库中通过计算汉明距离，检索出相似度最高的激光点云帧作为最佳粗略子场景；

步骤五、局部点云地图生成：

取最佳粗略子场景对应的激光点云帧及周围点云，按照全局位姿统一投影到原点坐标系中，拼接成为一个局部点云地图；

步骤六、全局位姿估计：

将当前激光点云帧与全局坐标下的局部点云地图利用Fast-Gicp进行点云配准，以此得到当前激光点云帧的全局位姿。

具体地，步骤一中，将先验地图中每个激光点云帧以极坐标的形式表示为圆形的鸟瞰图时，以激光点云帧的位置作为中心，保留一个面积为K（m）*K（m）的正方形作为有效感应区，K为设定值；以所述正方形为单位，将激光点云帧中的点云离散为径向距离为[1m,80m]、角度为[0°,360°]的由一系列面元组成的鸟瞰图，按径向距离进行离散的单位为1米，按角度进行离散的单位为1°；对鸟瞰图中的每个面元

，采用一些特征提取方法，如高度、距离、反射强度、环等，使用八位二进制编码对同一个面元内的所有点进行编码；其中k为面元/>

按径向距离排列的索引号，j为面元/>

按角度排列的索引号。以高度的编码方式为例，对于每个面元内的点云集，首先按照高度的大小顺序，线性离散成8个二进制数（bin）,有点云存在时bin值为1，否则为0，从而可获得每个面元的8位二进制编码。最后遍历每个面元，把各面元内的8位二进制数转换成十进制数，以此将鸟瞰图扩展成具有旋转不变性的图像条，且可以充分利用稀疏点云的大部分信息。

由于平移变换会引起由鸟瞰图生成的图像条描述子显著退化，为了使描述子可以具有平移不变性，基于傅里叶变换的方案能够估计大的平移与旋转。具体地，步骤二中，对每个由激光点云帧生成的鸟瞰图进行傅里叶变换时，假设两个鸟瞰图

和/>

仅相差一个位移/>

，使得/>

，那么/>

和/>

的傅立叶变换为

，相应地，归一化互功率谱由下式给出：

；

其中，

、/>

分别为位移/>

在x轴方向和y轴方向的分量，/>

为鸟瞰图/>

的频域表示，/>

为鸟瞰图/>

的频域表示，/>

、/>

分别为自变量x、y的频域形式，i表示虚部单位；通过逆傅里叶变换计算出两个鸟瞰图之间的平移变换，使当前激光点云的鸟瞰图表示按照计算结果进行移动后再进行相似度比对，以解决激光点云之间的相对平移。另外尽管激光点云的平移对应傅里叶变换后的图像条垂直平移，虽然还会引起图像像素强度的轻微变化，但是鸟瞰图以面元为最小单位保留了点云的绝对内部结构，提高了辨别能力并且对图像强度的变化具有鲁棒性，所以可以忽略点云在一定范围内平移引起的图像中强度的变化。

具体地，步骤三中使用Gabor滤波器从鸟瞰图的图像中深入提取特征，LoG-Gabor滤波器可用于将鸟瞰图区域中的数据分解为以不同分辨率出现的分量，与传统的傅里叶变换相比，它的优势在于允许频率数据局部化，允许在相同位置和分辨率进行特征匹配。为了确保实时性，使用1D LoG-Gabor滤波器，1D Log-Gabor滤波器的频率响应

，/>

表示滤波器的中心频率，/>

为影响滤波器带宽的参数，f表示当前频率，利用1D Log-Gabor滤波器对鸟瞰图的每一行进行卷积，滤波器的波长增加相同的因子，从而得到每个滤波器的实部和虚部，最后通过简单的阈值运算，将四个滤波器的卷积响应转化为二值化特征，将二值化特征叠加到每个鸟瞰图的大型二值化特征图中。

具体地，步骤四中，通过计算汉明距离检索出相似度最高的激光点云帧作为最佳粗略子场景时，将由先验地图生成的二值化特征图按列组合构成高维向量，之后再构建成KD树形式表示以便于进行匹配检索；通过计算汉明距离得到当前激光点云帧的二值化特征图

与描述符数据库/>

中的二值化特征图/>

的相似程度，取相似程度最高且在距离阈值之内的二值化特征图对应的激光点云帧作为最佳粗略子场景

，其中，/>

是给定的距离阈值，/>

表示汉明距离，

为/>

函数。

具体地，步骤五中拼接局部点云地图时，取最佳粗略子场景对应的激光点云帧以及最佳粗略子场景对应激光点云帧的前、后各五帧激光点云帧，记为

，按照先验地图的全局位姿投影到原点坐标系中，记为/>

，则/>

，其中/>

为当前激光点云帧坐标系到原点坐标系的变换矩阵，以此拼接成为一个局部点云地图。

最后利用Fast-Gicp点云配准算法，将当前查询的激光点云帧与局部点云地图进行配准，即可得到当前激光点云帧的全局位姿。

与现有技术相比，本发明的有益技术效果是：

本发明利用点云鸟瞰图表示制作的具有平移与旋转不变性的特征描述符，可以充分利用稀疏点云的大部分信息，避免了检索过程中的暴力搜索，可以更快速准确地获取“粗略”子场景。在全局位姿估计的过程中采用扫描与局部点云地图的匹配，提高了点云配准的成功率。整体方案的定位精度高，定位所需时间短，并且对不同的场景都有很好的鲁棒性。

同时本方案的定位平均误差（rmse）在室外大规模场景下可以达到0.8m左右，对比目前的一些定位方案，在定位精度上也有着不错的优势。

附图说明

图1为本发明基于点云鸟瞰图和由粗到精策略的激光全局定位方法的流程图；

图2为本发明对激光点云帧进行鸟瞰图表示的示意图；

图3为本发明激光点云配准过程示意图。

具体实施方式

下面结合附图对本发明的一种优选实施方式作详细的说明。

本发明提供一种基于点云鸟瞰图和由粗到精策略的激光全局定位方法，总体流程图如图1所示，包括一下步骤：

步骤一、激光点云帧的鸟瞰图表示：

以激光点云帧的位置作为中心，保留一个面积为K（m）*K（m）的正方形作为有效感应区，K为设定值；以所述正方形为单位，将激光点云帧中的点云离散为径向距离为[1m,80m]、角度为[0°,360°]的由一系列面元组成的鸟瞰图，按径向距离进行离散的单位为1米，按角度进行离散的单位为1°。为了完整地表示点云，对鸟瞰图中的每个面元

，采用一些特征提取方法，如高度、距离、反射强度、环等，使用八位二进制编码对同一个面元内的所有点进行编码；其中k为面元/>

按径向距离排列的索引号，j为面元/>

按角度排列的索引号。以高度的编码方式为例，对于每个面元内的点云集，首先按照高度的大小顺序，线性离散成8个二进制数（bin）,有点存在时bin值为1，否则为0，从而可获得每个面元的8位二进制编码。最后遍历每个面元，把各面元内的8位二进制数转换成十进制数，以此将鸟瞰图扩展成具有旋转不变性的图像条，且可以充分利用稀疏点云的大部分信息，如图2所示。

步骤二、具有平移不变形的傅里叶变换：

由于平移变换会引起由鸟瞰图生成的图像条描述子显著退化，为了使描述子可以具有平移不变性，基于傅里叶变换的方案能够估计大的平移与旋转。具体地，步骤二中，对每个由激光点云帧生成的鸟瞰图进行傅里叶变换时，假设两个鸟瞰图

和/>

仅相差一个位移/>

，使得/>

，那么/>

和/>

的傅立叶变换为

，相应地，归一化互功率谱由下式给出：

；

其中，

、/>

分别为位移/>

在x轴方向和y轴方向的分量，/>

为鸟瞰图/>

的频域表示，/>

为鸟瞰图/>

的频域表示，/>

、/>

分别为自变量x、y的频域形式，i表示虚部单位；通过逆傅里叶变换计算出两个鸟瞰图之间的平移变换，使当前激光点云帧的鸟瞰图表示按照计算结果进行移动后再进行相似度比对，以解决激光点云之间的相对平移。另外尽管激光点云的平移对应傅里叶变换后的图像条垂直平移，虽然还会引起图像像素强度的轻微变化，但是鸟瞰图以面元为最小单位保留了点云的绝对内部结构，提高了辨别能力并且对图像强度的变化具有鲁棒性，所以可以忽略点云在一定范围内平移引起的图像中强度的变化。

步骤三、LoG-Gabor滤波器的二值化特征提取：

步骤三中使用Gabor滤波器从鸟瞰图的图像中深入提取特征，LoG-Gabor滤波器可用于将鸟瞰图区域中的数据分解为以不同分辨率出现的分量，与传统的傅里叶变换相比，它的优势在于允许频率数据局部化，允许在相同位置和分辨率进行特征匹配。为了确保实时性，仅使用1DLoG-Gabor滤波器，一维Log-Gabor滤波器的频率响应

，/>

表示滤波器的中心频率，/>

为影响滤波器带宽的参数，f表示当前频率，利用1D Log-Gabor滤波器对鸟瞰图的每一行进行卷积，滤波器的波长增加相同的因子，从而得到每个滤波器的实部和虚部，最后通过简单的阈值运算，将四个滤波器的卷积响应转化为二值化特征，将二值化特征叠加到每个鸟瞰图的大型二值化特征图中。

步骤四、粗略子场景识别：

将由先验地图生成的二值化特征图按列组合构成高维向量，之后再构建成KD树形式表示以便于进行匹配检索；通过计算汉明距离得到当前激光点云帧的二值化特征图

与描述符数据库/>

中的二值化特征图/>

的相似程度，取相似程度最高且在距离阈值之内的二值化特征图对应的激光点云帧作为最佳粗略子场景

，其中，/>

是给定的距离阈值，/>

表示汉明距离，

为/>

函数。

步骤五、局部点云地图生成：

在数据库中检索到的最佳相似点云后，取最佳粗略子场景对应的激光点云帧以及最佳粗略子场景对应激光点云帧的前、后各五帧激光点云帧，记为

，按照先验地图的全局位姿投影到原点坐标系中，记为/>

，则/>

，其中/>

为当前激光点云帧坐标系到原点坐标系的变换矩阵，以此拼接成为一个局部点云地图。

实际中通常由于位姿变换真值的测量频率高于激光扫描频率，因此可以利用差分的方法求解当前帧点云的位姿变换矩阵：

，其中/>

、/>

分别为当前时刻t的前后相邻时刻的/>

、/>

的位姿真值。之后再按照上面的方法将激光点云帧坐标系的点投影到原点坐标系拼接成局部点云地图。

步骤六、全局位姿估计：

将当前扫描的激光点云帧和局部点云地图进行无效点剔除和外点滤波操作后，利用Fast-Gicp点云配准算法，以匹配到的最佳子场景激光点云帧的位姿真值作为点云配准的迭代初值，将当前激光点云帧作为源激光点云与局部点云地图作为目标点云进行配准对齐，如图3所示，收敛后即可得到当前激光点云帧的全局位姿。

对于本领域技术人员而言，显然本发明不限于上述示范性实施例的细节，而且在不背离本发明的精神或基本特征的情况下，能够以其他的具体形式实现本发明。因此无论从哪一点来看，均应将实施例看作是示范性的，而且是非限制性的，本发明的范围由所附权利要求而不是上述说明限定，因此旨在将落在权利要求的等同要件的含义和范围内的所有变化囊括在本发明内，不应将权利要求中的任何附图标记视为限制所涉及的权利要求。

此外，应当理解，虽然本说明书按照实施方式加以描述，但并非每个实施方式仅包含一个独立技术方案，说明书的这种叙述方式仅仅是为了清楚起见，本领域技术人员应当将说明书作为一个整体，各实施例中的技术方案也可以经适当组合，形成本领域技术人员可以理解的其他实施方式。

Claims (4)

1.一种基于点云鸟瞰图和由粗到精策略的激光全局定位方法，包括以下步骤：

步骤一、激光点云帧的鸟瞰图表示：

将先验地图中的每个激光点云帧，以极坐标的形式表示为圆形的鸟瞰图，鸟瞰图中的每个面元的值为面元中点云信息的二进制编码；

步骤二、具有平移不变形的傅里叶变换：

对每个由激光点云帧生成的鸟瞰图进行傅里叶变换，以使鸟瞰图具有平移方向上的不变性；

步骤三、LoG-Gabor滤波器的二值化特征提取：

使用LoG-Gabor滤波器从鸟瞰图中提取特征，并通过阈值运算将LoG-Gabor滤波器的滤波结果转成为二值化特征，将二值化特征叠加到每个鸟瞰图的二值化特征图中；将所有鸟瞰图的二值化特征图组合为描述符数据库；

步骤四、粗略子场景识别：

将当前的激光点云帧表示成二值化特征图，在由先验地图生成的描述符数据库中通过计算汉明距离，检索出相似度最高的激光点云帧作为最佳粗略子场景；

步骤五、局部点云地图生成：

取最佳粗略子场景对应的激光点云帧及最佳粗略子场景对应激光点云帧的周围点云，按照全局位姿统一投影到原点坐标系中，拼接成为一个局部点云地图；

步骤六、全局位姿估计：

将当前激光点云帧与局部点云地图利用Fast-Gicp进行点云配准，以此得到当前激光点云帧的全局位姿；

步骤一中，将先验地图中每个激光点云帧以极坐标的形式表示为圆形的鸟瞰图时，以激光点云帧的位置作为中心，保留一个面积为

的正方形作为有效感应区，K为设定值，单位为米；以所述正方形为单位，将激光点云帧中的点云离散为径向距离为[1米,80米]、角度为[0°,360°]的由一系列面元组成的鸟瞰图，按径向距离进行离散的单位为1米，按角度进行离散的单位为1°；对鸟瞰图中的每个面元/>

，采用高度特征、距离特征、反射强度特征或者环特征的特征提取方法，使用八位二进制编码对同一个面元内的所有点进行编码；其中k为面元/>

按径向距离排列的索引号，j为面元/>

按角度排列的索引号；

步骤三中使用LoG-Gabor滤波器从鸟瞰图中提取特征，并通过阈值运算将滤波结果转成为二值化特征时，使用1D LoG-Gabor滤波器；1D Log-Gabor滤波器的频率响应

，/>

表示滤波器的中心频率，/>

为影响滤波器带宽的参数，f表示当前频率，利用1D Log-Gabor滤波器对鸟瞰图的每一行进行卷积，滤波器的波长增加相同的因子，从而得到每个滤波器的实部和虚部，最后通过阈值运算，将四个滤波器的卷积响应转化为二值化特征，将二值化特征叠加到每个鸟瞰图的二值化特征图中。

2.根据权利要求1所述的基于点云鸟瞰图和由粗到精策略的激光全局定位方法，其特征在于，步骤二中，对每个由激光点云帧生成的鸟瞰图进行傅里叶变换时，假设两个鸟瞰图

和/>

仅相差一个位移/>

，使得/>

，那么/>

和/>

的傅立叶变换为/>

，相应地，归一化互功率谱

；

其中，

、/>

分别为位移/>

在x轴方向和y轴方向的分量，/>

为鸟瞰图/>

的频域表示，/>

为鸟瞰图/>

的频域表示，/>

、/>

分别为自变量x、y的频域形式，i表示虚部单位；通过逆傅里叶变换计算出两个鸟瞰图之间的平移变换。

3.根据权利要求1所述的基于点云鸟瞰图和由粗到精策略的激光全局定位方法，其特征在于，步骤四中，通过计算汉明距离检索出相似度最高的激光点云帧作为最佳粗略子场景时，将由先验地图生成的二值化特征图按列组合构成高维向量，再构建为KD树形式；通过计算汉明距离得到当前激光点云帧的二值化特征图

与描述符数据库/>

中的任意一个二值化特征图/>

的相似程度，取相似程度最高且在距离阈值之内的二值化特征图对应的激光点云帧作为最佳粗略子场景/>

，其中，/>

是给定的距离阈值，

表示汉明距离，/>

为/>

函数。

4.根据权利要求1所述的基于点云鸟瞰图和由粗到精策略的激光全局定位方法，其特征在于，步骤五中拼接局部点云地图时，取最佳粗略子场景对应的激光点云帧以及最佳粗略子场景对应激光点云帧的前、后各五帧激光点云帧，记为

，按照先验地图的全局位姿投影到原点坐标系中，记为/>

，则/>

，其中/>

为当前激光点云帧坐标系到原点坐标系的变换矩阵，以此拼接成为一个局部点云地图。

Images