CN115372989A

CN115372989A - 基于激光雷达的越野自动小车长距离实时定位系统及方法

Info

Publication number: CN115372989A
Application number: CN202210998319.2A
Authority: CN
Inventors: 李宏伟; 陆鸣; 陈姮; 芮挺; 王新晴; 何晓晖; 王聪; 杜子琰
Original assignee: Army Engineering University of PLA
Current assignee: Army Engineering University of PLA
Priority date: 2022-08-19
Filing date: 2022-08-19
Publication date: 2022-11-22

Abstract

本发明公开了车辆实时精确定位技术领域的基于激光雷达的越野自动小车长距离实时定位系统及方法，包括：对激光雷达旋转扫描获取的点云数据进行激光雷达标定和运动畸变去除；对点云数据进行栅格化处理后，滤除地面和动态物体，进行点云数据的粗匹配定位；通过体素化网格方法实现采样，根据标准ICP算法完成点云数据的精确匹配定位；对累计误差进行纠正，实现动态局部地图构建和定位校正。本发明通过激光雷达获取点云并进行运动畸变去除，通过粗匹配定位和精确匹配定位，完成越野自动小车动态局部地图构建和定位校正。

Description

基于激光雷达的越野自动小车长距离实时定位系统及方法

技术领域

本发明涉及基于激光雷达的越野自动小车长距离实时定位系统及方法，属于车辆实时精确定位技术领域。

背景技术

得益于时效性的独特属性，实时定位技术在室外越野车辆的自动驾驶研究等方面存在得天独厚的优势和广阔的应用前景。目前室外越野小车主要依靠高精度地图和GPS/INS设备完成定位，该方法定位成本低，操作简便，但定位精度一般。为应对一些高精度要求场景，会在上述方法基础上采用视觉里程计和LIDAR里程计对室外越野小车进行辅助定位，定位精度将会得到大幅提升。这种具有较高精度的定位方法在当前领域中受到了广泛的关注和研究。

然而，当GPS受到遮挡或没有高精度地图时，定位的精度将会急剧降低。等而传统的辅助定位方法也存在着明显的缺陷，例如视觉里程计受环境光照强度和场景结构影响较大，而LIDAR里程计的鲁棒性则是限制其精度的重要因素。定位设备及传感器自身的限制和局限性严重制约着定位技术的发展和革新，亟需发展全状态全工况下不受限制和约束的定位方法，为实时精确定位提供有效的实现手段。

发明内容

本发明的目的在于克服现有技术中的不足，提供基于激光雷达的越野自动小车长距离实时定位系统及方法，通过激光雷达获取点云并进行运动畸变去除，通过粗匹配定位和精确匹配定位，完成越野自动小车动态局部地图构建和定位校正。

为达到上述目的，本发明是采用下述技术方案实现的：

第一方面，本发明提供了基于激光雷达的越野自动小车长距离实时定位方法，包括：

对激光雷达旋转扫描获取的点云数据进行激光雷达标定和运动畸变去除；

对点云数据进行栅格化处理后，滤除地面和动态物体，进行点云数据的粗匹配定位；

通过体素化网格方法实现采样，根据标准ICP算法完成点云数据的精确匹配定位；

对累计误差进行纠正，实现动态局部地图构建和定位校正。

进一步的，所述激光雷达安装于越野自动小车的顶部，激光雷达坐标系的原点位于雷达中心，所述越野自动小车坐标系的原点位于小车的几何中心，所述激光雷达通过旋转扫描的方式获取点云数据，扫描的数据利用内参数将原始数据转化为激光雷达坐标系中的三维点。

进一步的，所述激光雷达标定包括：通过对激光雷达坐标系进行旋转和平移变换，完成外参数标定，拟合出地面所在的平面方程，完成俯仰角、横滚角的标定，以上述参数为基础，利用标定杆完成对偏航角的标定，并通过人工测量的方式完成平移向量的标定。

进一步的，所述运动畸变去除包括：通过激光雷达的激光线扫描角度计算扫描时间周期，求得越野小车相的运动量，采用线性插值的方法去消除3D点云图像的运动畸变。

进一步的，对点云数据进行栅格化处理后，滤除地面和动态物体，进行点云数据的粗匹配定位，包括：

将目标点云数据标准化处理后放入网格中进行栅格化处理；

采用智能搜索方案对所有冗余点对进行剔除；

使用栅格化的方法实现对地面的实时分割，对点云数据中的地面进行滤除，

将栅格地图映射至二维图像的方法，在二维图像上进行聚类操作，滤除点云数据中的动态物体；

利用处理过的点云数据，选取共面四点实现点云粗匹配。

进一步的，通过体素化网格方法实现采样，根据标准ICP算法完成点云数据的精确匹配定位，包括：

根据体素化网格方法对数据进行滤波处理；

根据输入点云下采样后得到的点云数据构成由体素重心组成的点云数据，调整网格分辨率以调整采样频率，保持点云数据的形状特征并提高点云数据配准速度；

利用在源点云数据和目标点云数据找到满足欧式距离最小的最近邻点组成匹配点对，计算出使误差函数最小的最优匹配参数；

通过不断迭代最终达到设定的匹配精度。

进一步的，对累计误差进行纠正，实现动态局部地图构建和定位校正，包括：

构建动态的体素网格地图，每个体素包含它的索引、质心、协方差矩阵、所含的点和数量和状态；

当越野自动小车移动时，动态网格地图删除位于在局部地图半径外的体素，对构建的地图进行动态更新并维持了固定的大小，保证构建的地图大小及配准的实时性；

将目标点云数据的空间划分为指定大小的体素网格，并将点云数据映射到网格中，将源点云数据中的每个点按初始变换矩阵变换到体素网格中去，根据源点云数据中的点落到的网格计算相应的概率密度函数；

将得到的点云数据描述成了分段光滑的表面，通过概率密度函数描述局部表面的近似模型，包括表面空间位置、方向和光滑度；

计算每个体素中的正态分布，通过NDT算法找到让概率密度之和最大的变换参数来求解最优匹配，实现当前关键帧与局部地图的配准。

第二方面，本发明提供了基于激光雷达的越野自动小车长距离实时定位系统，包括：

数据预处理模块：用于对激光雷达旋转扫描获取的点云数据进行激光雷达标定和运动畸变去除；

粗匹配定位模块：用于对点云数据进行栅格化处理后，滤除地面和动态物体，进行点云数据的粗匹配定位；

精确匹配定位模块：用于通过体素化网格方法实现采样，根据标准ICP算法完成点云数据的精确匹配定位；

定位校正模块：用于对累计误差进行纠正，实现动态局部地图构建和定位校正。

第三方面，本发明提供了基于激光雷达的越野自动小车长距离实时定位装置，包括处理器及存储介质；

所述存储介质用于存储指令；

所述处理器用于根据所述指令进行操作以执行根据上述任一项所述方法的步骤。

第四方面，本发明提供了一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时实现上述任一项所述方法的步骤。

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

本发明的越野小车长距离实时定位方法构建了一种基于激光雷达的实时激光雷达里程计方法，通过改进平面结构化配准算法提高匹配的效率和成功率，无需GPS或其他惯导设备辅助即可达到实时性和准确性的良好平衡，具有较好的鲁棒性和环境适应性，在较大位移和较低点云重叠情况下仍然能够达到较好的定位效果，适用于广泛的野外大范围、长距离定位应用场景，以及多场景、跨场景野外定位应用，能够在特征点少、动态噪声多的野外环境中能达到良好的效果，具有广阔的应用前景。

附图说明

图1是本发明实施例一提供的基于激光雷达的越野自动小车长距离实时定位方法流程框图；

图2是本发明实施例一提供的基于激光雷达的越野自动小车长距离实时定位方法第一步的流程框图；

图3是本发明实施例一提供的激光线扫描几何示意图；

图4是本发明实施例一提供的基于激光雷达的越野自动小车长距离实时定位方法第二步的流程框图；

图5是本发明实施例一提供的高程差分割地面示意图；

图6是本发明实施例一提供的基于激光雷达的越野自动小车长距离实时定位方法第三步的流程框图；

图7是本发明实施例一提供的基于激光雷达的越野自动小车长距离实时定位方法第四步的流程框图。

具体实施方式

下面结合附图对本发明作进一步描述。以下实施例仅用于更加清楚地说明本发明的技术方案，而不能以此来限制本发明的保护范围。

实施例一：

如图1所示，本实施例的基于激光雷达的越野自动小车长距离实时定位方法，包括以下步骤：

1、激光点云获取和运动畸变去除

激光雷达安装于越野自动小车的顶部，激光雷达坐标系的原点位于雷达中心，越野自动小车坐标系的原点位于越野自动小车的几何中心，基于激光雷达坐标系获取的点云数据；进行俯仰角和横滚角的标定以及偏航角和平移向量的标定；激光雷达通过旋转扫描的方式获取激光点云，采用线性插值的方法去除运动畸变的影响。算法的流程图如图2所示，具体步骤包括：

第一步：激光雷达布置。激光雷达安装于越野自动小车的顶部，激光雷达坐标系的原点位于雷达中心，越野自动小车坐标系的原点位于小车的几何中心。

第二步：点云数据获取。如图3所示，激光雷达通过旋转扫描的方式获取点云数据，扫描的数据利用内参数将原始数据转化为激光雷达坐标系中的三维点。

第三步：激光雷达标定。通过对激光雷达坐标系进行旋转和平移变换，完成外参数标定。拟合出地面所在的平面方程，完成俯仰角、横滚角的标定；以上面的参数为基础，利用标定杆完成对偏航角的标定，并通过人工测量的方式完成平移向量的标定。

第四步：运动畸变去除。通过激光线的扫描角度计算扫描时间周期，近似求得越野小车相的运动量；采用线性插值的方法去消除3D点云图像的运动畸变。

2、平面结构化粗匹配

利用栅格化点云的方法对根据距离搜索匹配点对的过程进行加速，并采用智能搜素方案对所有冗余点对进行剔除；将激光点映射到对应的栅格上去，使用栅格化的方法实现对地面的实时分割，对点云中的平坦地面进行滤除再进行匹配；对点云中的小型动态物体进行滤除，利用滤除小物体的方法来取代滤除动态物体，将栅格地图映射至二维图像的方法，在二维图像上进行聚类操作，实现了对动态小物体的实时分割；选取共面四点实现点云粗匹配。算法的流程图见图4，具体步骤包括：

第一步：栅格化点云。利用栅格化点云的方法，将目标点云标准化处理后放入网格中进行栅格化处理，加速根据距离搜索匹配点对的过程。

第二步：冗余点剔除。从点云中提取对应于线段之间的相同角度的四个共面点，在相同距离和相同不变量下对所有冗余点对进行剔除。

第三步：地面滤除。如图5所示为利用高程差分割地面示意图，地面一般是一帧点云数据中最大的平面，使用栅格化的方法滤除地面。首先建立一张栅格地图，将每个激光点映射到对应的栅格上去；然后对每个栅格中的点集集合，根据高度差值找到地面栅格；遍历所有栅格，去除其中的地面栅格，得到滤除地面信息的点云模型；

第四步：动态物体滤除。对于得到的地面滤除后点云模型，将对应的栅格地图映射到二维图像上。利用腐蚀的办法，使图像中断裂的墙体经过图像腐蚀操作后变得连续。根据区域标记的方法完成物体的聚类。通过计算每个类的矩形包围框，包围框用红色矩形表示，计算每个类包围框的面积，若其面积小于阈值，则在图像中将其滤除。最后通过栅格地图恢复出滤除地面和动态物体后的点云数据；

第五步：点云粗配准。利用处理过的点云数据，选取共面四点进行点云粗配准。粗匹配过程为后边的ICP匹配提供了一个良好的迭代初值。

3、ICP精确匹配定位

将点云中的点映射到一个三维体素网格中，同时用每个体素中所有点的重心来表示其余点，根据体素化网格方法对数据进行滤波处理；根据输入点云下采样后得到的点云构成由体素重心组成的点云，调整网格分辨率以调整采样频率，保持点云的形状特征并提高点云配准速度；利用在源点云P和目标点云Q找到满足欧式距离最小的最近邻点组成匹配点对，计算出使误差函数最小的最优匹配参数；通过不断迭代最终达到一定的匹配精度。算法的流程图如图6所示，具体步骤包括：

第一步：数据滤波。在ICP匹配之前对数据进行了滤波处理，即用体素化网格的方法实现下采样，提高点云配准速度。将点云中的点映射到一个三维体素网格中并调整网格的分辨率，用每个体素中所有点的重心来表示其余点，根据所有体素重心组成的点云得到对输入点云下采样后得到的点云。

第二步：最优匹配参数计算。通过数据滤波后的点云大小大大减小，在源点云P中选点集p_i∈P，并在目标点云Q中找到满足(p_i,q_i)欧式距离最小的点，计算使误差函数E(R,t)最小的最优匹配参数R和t，其中R为旋转矩阵，t为平移向量。误差函数为：

第三步：ICP精匹配。利用上一步求出的旋转矩阵R和平移向量t对p_i进行旋转和平移变换，得到新的点集p_i′＝Rp_i+t。计算新点集到q_i的平均欧氏距离d，并进行迭代直至d小于设定的阈值或达到了最大迭代次数。计算式如下：

4、动态局部地图构建和定位校正

构建动态的体素网格地图，每个体素包含它的索引、质心(均值)、协方差矩阵、它所含的点和数量、状态。当越野自动小车移动时，动态网格地图删除位于在局部地图半径外的体素，对构建的地图进行动态更新并维持了固定的大小，保证构建的地图大小及配准的实时性；计算每个体素中的正态分布，通过NDT算法找到让概率密度之和最大的变换参数来求解最优匹配，实现当前关键帧与局部地图的配准。算法的流程图如图7所示，具体步骤包括：

第一步：体素网格地图构建。将占据体素存在一个HashMap中，每个体素包含它的索引、质心(均值)、协方差矩阵、它所含的点和数量、状态，当体素中包含大于一定数量的点的时候其状态被认为是active的，在整个算法中只使用active的体素；

第二步：动态地图构建。当越野自动小车移动时，删除在局部地图半径外的体素，对构建的动态网格地图动态的进行更新，并维持了固定的大小。利用NDT匹配时，只重新计算有新点插入的体素的均值和协方差矩阵；

第三步：目标点云划分。将目标点云的空间划分为指定大小的体素网格，并将点云映射到网格中。将源点云中的每个点按初始变换矩阵变换到体素网格中去，根据源点云中的点落到的网格计算相应的概率密度函数；

第四步：表面化描述。将得到的点云描述成了分段光滑的表面，通过概率密度函数描述局部表面的近似模型，包括表面空间位置、方向和光滑度；

第五步：NDT配准。找到使两帧点云达到最优匹配的最优变换矩阵，使用牛顿迭代法知道最优匹配参数，实现当前关键帧与局部地图的配准。

实施例二：

基于激光雷达的越野自动小车长距离实时定位系统，可实现实施例一所述的基于激光雷达的越野自动小车长距离实时定位方法，包括：

实施例三：

本发明实施例还提供了基于激光雷达的越野自动小车长距离实时定位装置，可实现实施例一所述的基于激光雷达的越野自动小车长距离实时定位方法，包括处理器及存储介质；

所述存储介质用于存储指令；

所述处理器用于根据所述指令进行操作以执行下述方法的步骤：

对累计误差进行纠正，实现动态局部地图构建和定位校正。

实施例四：

本发明实施例还提供了一种计算机可读存储介质，可实现实施例一所述的基于激光雷达的越野自动小车长距离实时定位方法，其上存储有计算机程序，该程序被处理器执行时实现下述方法的步骤：

对累计误差进行纠正，实现动态局部地图构建和定位校正。

本领域内的技术人员应明白，本申请的实施例可提供为方法、系统、或计算机程序产品。因此，本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。

本申请是参照根据本申请实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。

这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。

这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明技术原理的前提下，还可以做出若干改进和变形，这些改进和变形也应视为本发明的保护范围。

Claims

1.基于激光雷达的越野自动小车长距离实时定位方法，其特征是，包括：

对累计误差进行纠正，实现动态局部地图构建和定位校正。

2.根据权利要求1所述的基于激光雷达的越野自动小车长距离实时定位方法，其特征是，所述激光雷达安装于越野自动小车的顶部，激光雷达坐标系的原点位于雷达中心，所述越野自动小车坐标系的原点位于小车的几何中心，所述激光雷达通过旋转扫描的方式获取点云数据，扫描的数据利用内参数将原始数据转化为激光雷达坐标系中的三维点。

3.根据权利要求1所述的基于激光雷达的越野自动小车长距离实时定位方法，其特征是，所述激光雷达标定包括：通过对激光雷达坐标系进行旋转和平移变换，完成外参数标定，拟合出地面所在的平面方程，完成俯仰角、横滚角的标定，以上述参数为基础，利用标定杆完成对偏航角的标定，并通过人工测量的方式完成平移向量的标定。

4.根据权利要求1所述的基于激光雷达的越野自动小车长距离实时定位方法，其特征是，所述运动畸变去除包括：通过激光雷达的激光线扫描角度计算扫描时间周期，求得越野小车相的运动量，采用线性插值的方法去消除3D点云图像的运动畸变。

5.根据权利要求1所述的基于激光雷达的越野自动小车长距离实时定位方法，其特征是，对点云数据进行栅格化处理后，滤除地面和动态物体，进行点云数据的粗匹配定位，包括：

将目标点云数据标准化处理后放入网格中进行栅格化处理；

采用智能搜索方案对所有冗余点对进行剔除；

利用处理过的点云数据，选取共面四点实现点云粗匹配。

6.根据权利要求1所述的基于激光雷达的越野自动小车长距离实时定位方法，其特征是，通过体素化网格方法实现采样，根据标准ICP算法完成点云数据的精确匹配定位，包括：

根据体素化网格方法对数据进行滤波处理；

通过不断迭代最终达到设定的匹配精度。

7.根据权利要求1所述的基于激光雷达的越野自动小车长距离实时定位方法，其特征是，对累计误差进行纠正，实现动态局部地图构建和定位校正，包括：

8.基于激光雷达的越野自动小车长距离实时定位系统，其特征是，包括：

9.基于激光雷达的越野自动小车长距离实时定位装置，其特征是，包括处理器及存储介质；

所述存储介质用于存储指令；

所述处理器用于根据所述指令进行操作以执行根据权利要求1～7任一项所述方法的步骤。

10.计算机可读存储介质，其上存储有计算机程序，其特征是，该程序被处理器执行时实现权利要求1～7任一项所述方法的步骤。