CN113640822B - 一种基于非地图元素过滤的高精度地图构建方法 - Google Patents

一种基于非地图元素过滤的高精度地图构建方法 Download PDF

Info

Publication number
CN113640822B
CN113640822B CN202110770937.7A CN202110770937A CN113640822B CN 113640822 B CN113640822 B CN 113640822B CN 202110770937 A CN202110770937 A CN 202110770937A CN 113640822 B CN113640822 B CN 113640822B
Authority
CN
China
Prior art keywords
point cloud
map
grid
probability
grids
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202110770937.7A
Other languages
English (en)
Other versions
CN113640822A (zh
Inventor
李巍华
李伟
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
South China University of Technology SCUT
Original Assignee
South China University of Technology SCUT
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by South China University of Technology SCUT filed Critical South China University of Technology SCUT
Priority to CN202110770937.7A priority Critical patent/CN113640822B/zh
Publication of CN113640822A publication Critical patent/CN113640822A/zh
Application granted granted Critical
Publication of CN113640822B publication Critical patent/CN113640822B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/88—Lidar systems specially adapted for specific applications
    • G01S17/89—Lidar systems specially adapted for specific applications for mapping or imaging
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
    • G01C21/30—Map- or contour-matching
    • G01C21/32—Structuring or formatting of map data
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/38—Electronic maps specially adapted for navigation; Updating thereof
    • G01C21/3804—Creation or updating of map data
    • G01C21/3807—Creation or updating of map data characterised by the type of data
    • G01C21/3811—Point data, e.g. Point of Interest [POI]
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/38—Electronic maps specially adapted for navigation; Updating thereof
    • G01C21/3804—Creation or updating of map data
    • G01C21/3833—Creation or updating of map data characterised by the source of data
    • G01C21/3841—Data obtained from two or more sources, e.g. probe vehicles
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/38—Electronic maps specially adapted for navigation; Updating thereof
    • G01C21/3804—Creation or updating of map data
    • G01C21/3833—Creation or updating of map data characterised by the source of data
    • G01C21/3852—Data derived from aerial or satellite images
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders

Landscapes

  • Engineering & Computer Science (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Electromagnetism (AREA)
  • Processing Or Creating Images (AREA)
  • Traffic Control Systems (AREA)

Abstract

本发明公开一种基于非地图元素过滤的高精度地图构建方法，包括：S1、采集点云数据；S2、对点云数据进行预处理；S3、根据是否为第一帧点云，进行点云地图初始化；S4、利用正态分布变换匹配得到位姿变换矩阵后，将该位姿变换矩阵应用到新的观测点云上，配准前后两帧点云以进行局部拼接地图；S5、得到点云地图的栅格化表示并把第一次更新的栅格初始化其占据栅格概率为一默认值；否则基于观测到的栅格占据与否，更新自由状态计数器，并用计数器值作为权重，重新计算占据概率，设置稳定自由栅格，并将稳定自由栅格内的点云标记为非地图元素，删除稳定自由栅格内的点云，得到滤除了非地图元素的地图。本发明能够实时构建滤除了非地图元素的高精度地图。

Description

一种基于非地图元素过滤的高精度地图构建方法
技术领域
本发明涉及高等级自动驾驶定位领域，尤其涉及一种基于非地图元素过滤的高精度地图构建方法。
背景技术
随着我国智能汽车行业的蓬勃发展，高等级自动驾驶需求渐长。根据美国汽车工程学会的要求，L4级别以上的自动驾驶车辆必须全程无人类驾驶员参与，因此稳定的定位系统是安全性的基础。目前最常用的激光雷达定位模块依赖离线的高精度地图作为参考点云，将实时扫描得到的点云与参考点云配准从而解算车辆的当前位姿。所以地图的构建精度将直接影响点云的配准和定位效果。自动驾驶车辆必须在道路交通参与者繁多的路段采集数据和构建地图，难免在地图中引入非地图元素。
与室内场景不同，室外如城市开放道路和封闭园区，存在大量的移动物体如车辆、行人，传统高精地图构建流程的缺点包括：
1、激光雷达点云投射在动态物体上返回的三维点不确定性大，容易在帧间匹配过程中引入误差，导致地图的“鬼影”问题；
2、徐国艳等在《基于三维激光点云的目标识别与跟踪研究[J]》(汽车工程,2020,42(01):38–46.)中采用目标检测算法进行识别，但目标检测算法对未知类别的移动物体识别效果差，且依赖于检测模型的训练和深度学习网络，计算复杂度高，不适合用于算力有限的车载平台上，而且还可能引入检测误差。
发明内容
本发明的目的是以提高高精地图的构建质量为目标，综合考虑鲁棒性与实时性，提出一种基于非地图元素过滤的高精度地图构建方法。本发明首先对点云数据进行预处理，然后基于八叉树数据结构将点云地图转换成以叶子节点和占据概率描述的八叉树地图，引入自由状态计数器降低不稳定栅格的占据概率，设置阈值来滤除长时间未达到稳定占据状态的栅格，实现非地图元素过滤效果。
本发明的目的至少通过如下技术方案之一实现。
一种基于非地图元素过滤的高精度地图构建方法，包括以下步骤：
S1、利用激光雷达扫描周围环境，采集点云数据；
S2、对点云数据进行预处理；
S3、根据是否为第一帧点云，进行点云地图初始化；
S4、利用正态分布变换匹配得到位姿变换矩阵后，将该位姿变换矩阵应用到新的观测点云上，配准前后两帧点云以进行局部拼接地图；
S5、利用八叉树地图得到点云地图的栅格化表示并把第一次更新的栅格初始化其占据栅格概率为一默认值；否则基于观测到的栅格占据与否，更新自由状态计数器，并用计数器值作为权重，重新计算占据概率，将占据概率小于等于阈值的栅格设置为稳定自由栅格，并将稳定自由栅格内的点云标记为非地图元素，删除稳定自由栅格内的点云，得到滤除了非地图元素的地图。
进一步地，步骤S1具体包括以下步骤：
S1.1、将一机械旋转式多线数激光雷达固定在车辆上，且所固定的位置不会在激光雷达的扫描范围内形成遮挡；
S1.2、准备车载计算单元，在车载计算单元的计算平台中安装Ubuntu操作系统和ROS通讯包，配置激光雷达驱动节点；
S1.3、启动激光雷达驱动节点，设置采集频率和数据格式，采集数据过程中，在预设的时间内切为一个数据包；
S1.4、使用ROS操作系统中的rosbag工具按帧回放录制好的点云数据包。
进一步地，步骤S3中，所述点云地图初始化包括以下步骤：
S3.1、在计算平台的ROS通讯包中，配置GPS/RTK单元驱动节点；
S3.2、启动GPS/RTK单元驱动节点，实时读取、解析全局定位数据，获得自车所在位置经纬度；
S3.3、记录第一帧激光雷达数据到来时，车辆的全局坐标；
S3.4、设置地图原点为该全局坐标和朝向角，后续位姿为相对该原点的偏移量和相对夹角；
S3.5、记录第一帧数据的时间戳，后续观测的时间戳为相对第一帧的偏移时间，并将平均时间差值作为雷达是否正常工作的判断条件，当平均时间大于阈值时停止录制，保存数据。
进一步地，步骤S3中，所述点云地图初始化的参数包括坐标原点、体素滤波分辨率、最大邻近点数和初始占据概率。
进一步地，步骤S2中，所述对点云数据进行预处理包括利用统计滤波器去除点云中的传感器噪声和由环境遮挡引起的边缘离群点，具体包括以下步骤：
S2.1、设置最大邻近点数A，针对点云中的某个点，计算它们与周围距离最小的最多A个点的平均距离μ和标准差σ；
S2.2、根据μ和σ构建高斯分布；
S2.3、若某个点在该高斯分布下的标准差大于一倍的标准差σ，则视为离群噪点予以删除；
S2.4、遍历点云中的每个点，执行S2.1-S2.3，直到完成所有点的处理，保存处理后的点云。
进一步地，步骤S2中，利用体素滤波器对点云进行定分辨率下采样，在保留充分的纹理特征的前提下降低计算量。所述对点云数据还包括采用体素滤波器对经统计滤波器处理后的点云进行下采样，包括：
S2.5、对于经统计滤波器处理后得到的点云，按照预设的第一分辨率L1将点云划分成若干个三维栅格，划分后，落在每个栅格中的点数大于Nmin；
S2.6、计算栅格中所有点的重心，使用重心代替栅格中的其他点；
S2.7、遍历点云的所有栅格，执行S2.6，得到下采样后的点云。
进一步地，步骤S4中，所述利用正态分布变换匹配得到位姿变换矩阵后，将该位姿变换矩阵应用到新的观测点云上，配准前后两帧点云包括以下步骤：
S4.1、将上一帧点云记为参考点云，当前帧记为待配准点云x；
S4.2、按照预设的第二分辨率L2将参考点云栅格化；
S4.3、对于划分后的栅格，基于栅格内的点云分布计算其均值和协方差；
式中，u是栅格内所有点的平均坐标向量，xi是栅格内某个点的坐标向量，i是点的序号，T是向量的转置、∑是栅格内所有点的坐标协方差，n是栅格内点云的数量；
S4.4、基于均值和协方差，建立多维概率密度函数，所述多维概率密度函数描述了栅格内的点云块的空间位置、方向和平整度；
S4.5、在待配准的新一帧的观测点云q上应用位姿变换矩阵，得到配准点云q′，利用配准点云q′和参考点云构建最大似然函数；
S4.6、求解最大似然函数，输出最终位姿变换矩阵，并将位姿变换矩阵应用在待配准的观测点云q上得到最终配准点云，拼接到点云地图上。
进一步地，S4.5中所述利用配准点云q′和参考点云构建最大似然函数包括以下步骤：
S4.5.1、初始化位姿变换矩阵为一个单位矩阵I；
S4.5.2、将待配准点云q′使用预设的第二分辨率L2栅格化，索引得到同一坐标位置的参考点云栅格，计算相应的概率密度分布函数：
式中，u为栅格内所有点的平均坐标向量；
S4.5.3、遍历所有栅格，针对大于Nmin个点的栅格，返回步骤S4.5.2进行处理；
S4.5.4、令最大似然函数为所有栅格的概率分布函数之和，并作为步骤S4.6的优化目标函数：
式中，score(p)为最大似然函数值，p(xi′)为经过变换后的栅格内点云的概率密度函数，xi′为经位姿变换后的点云三维坐标向量。
进一步地，S5中将配准点云转化为八叉树地图包括以下步骤：
S5.1、对于经过位姿变换后得到的点云，以第一分辨率L1将点云划分成若干个三维栅格，并初始化叶子节点的分辨率为L3；
S5.2、从点云地图开始，以八叉树数据结构的方式划分栅格，每个节点从三个方向划分为多个子节点，直到节点的长度小于等于叶子节点分辨率L3；
S5.3、将叶子节点的占据概率初始化为一默认值，即栅格的占据状态未知，同时初始化一自由状态计数器，当栅格被观测到自由时，自由状态计数器加一；
S5.4、随点云观测，基于贝叶斯公式更新叶子节点的占据概率，并取所有叶子节点的平均值作为父节点的占据概率；
S5.5、设置最大占据概率阈值Smin和最小占据概率阈值Smax，从八叉树地图中去除占据概率小于等于最小占据概率阈值Smax的稳定自由栅格。
进一步地，所述的利用观测更新栅格的占据概率，包括以下步骤：
S5.6、若栅格当前为稳定状态栅格，跳过该步骤，否则进行步骤S5.7；
S5.7、根据观测结果，更新自由状态计数器；
S5.8、根据观测和自由状态计数器，基于贝叶斯公式更新栅格的占据概率：
式中，St为当前时刻的占据概率值，St-1为上一时刻占据概率值，C为自由状态计数器值，Sz为当前观测所得占据标记值；
S5.9、判断当前节点是否达到稳定状态：记占据概率到达最大阈值的栅格为稳定占据栅格，记占据概率到达最小阈值的为稳定自由栅格；
S5.10、返回步骤S5.5，实时对叶子节点进行修正，得到滤除了非地图元素的高精度地图。
进一步地，激光雷达为80线激光雷达；ROS通讯包的版本号为Melodic。
进一步地，步骤S2中，利用统计滤波器去除离群点具体包括以下步骤：
S2.1、设置最大邻近点数A，针对点云中的某个点，计算它们与周围距离最小的最多A个点的平均距离μ和标准差σ；
S2.2、根据μ和σ构建一个如下式的高斯分布；
S2.3、若某个点在该高斯分布下的标准差大于一倍的标准差σ，则视为离群噪点予以删除；
S2.4、遍历点云中的每个点，执行S2.1-S2.3，直到完成所有点的处理，保存处理后的点云作为下步输入。
步骤S2中，利用体素滤波器下采样具体包括以下步骤：
S2.5、对于步骤S2.4中的点云，将事先设定好的参数L1作为分辨率，划分成若干个三维栅格，要求在划分后，落在每个栅格中的点数大于Nmin；
S2.6、计算栅格中所有点的重心，即平均三维坐标，使用重心代替栅格中的其他点；
S2.7、遍历点云的所有栅格，执行S2.6，得到下采样后的点云，保存该点云作为下步输入。
进一步地，步骤S3中，所述点云地图初始化的参数包括坐标原点、体素滤波分辨率、最大邻近点数和初始占据概率。
与现有技术相比，本发明能够实现的有益效果至少如下：
1、提供一种基于贝叶斯公式的改进占据概率更新公式，先利用激光里程计拼接点云得到局部地图，然后随观测更新八叉树地图，实时构建滤除了非地图元素的高精度地图。
2、本发明利用环境中动态障碍物在连续帧点云观测中的投影不确定性，降低地图中非地图元素占比，在减少建图误差的同时不影响算法效率，达到同时兼顾准确性和实时性的目的。此方法构建的高精点云地图可作为高精度地图的基础图层来确保后续语义标注精度。
附图说明
图1是本实施例流程示意图。
图2是本实施例所述占据概率更新流程图。
具体实施方式
下面通过具体实施例对本发明的目的作进一步详细的描述。
本发明中，为降低成本及方便实验测试，在本发明其中一个实施例中应用线控底盘模拟车辆的应用场景。其中，用锥桶模拟静态感兴趣目标。采集数据时，摇控底盘在锥桶摆成的赛道中行驶，模拟车辆在城市中跟随结构化道路中心线行驶的场景。该实验设置是为了方便阐述本发明细节，本发明的实际实施方式并不因此限定于以下实施例。
本发明提供的一种基于非地图元素过滤的高精度地图构建方法，如图1所示，包括以下步骤：
S1、利用车载激光雷达扫描周围环境，采集点云数据。
本步骤具体包括以下子步骤：
S1.1、将一激光雷达固定在车辆上方滑轨中间处，且所选择的安装位置不会在雷达360度扫描范围内形成遮挡；
在本发明其中一个实施例中，激光雷达采用机械旋转式多线数激光雷达。
S1.2、准备车载计算单元，在计算平台中安装Ubuntu操作系统和ROS通讯包，配置激光雷达驱动节点；
S1.3、启动激光雷达驱动节点，设置采集频率为10Hz、数据格式为坐标与反射强度，采集数据过程中，在预设的时间内切为一个数据包；
在本发明其中一个实施例中，每5分钟切为一个数据包，不足5分钟的按5分钟处理，以防止数据包过大。
S1.4、使用ROS操作系统中的rosbag工具按帧回放录制好的点云数据包，每一帧点云按时序依次作为下一步骤的输入。
在本发明其中一个实施例中，激光雷达为80线激光雷达；ROS通讯包的版本号为Melodic。
S2、对点云数据进行预处理。
本步骤中，所述对点云数据进行预处理，包括：利用统计滤波器去除点云中的传感器噪声和由环境遮挡引起的边缘离群点；利用体素滤波器对点云进行定分辨率下采样，在保留充分的纹理特征的前提下降低计算量。
具体的，在本发明其中一个实施例中，对点云数据进行预处理具体包括以下步骤：
S2.1、设置最大邻近点数A为100，针对点云中的某个点，计算它们与周围距离最小的最多100个点的平均距离μ和标准差σ；
S2.2、根据μ和σ构建一个如下式的高斯分布；
S2.3、若某个点在该高斯分布下的标准差大于一倍的标准差σ，则视为离群噪点予以删除；
S2.4、遍历点云中的每个点，执行S2.1-S2.3，直到完成所有点的处理，保存处理后的点云。
S2.5、对于步骤S2.4中得到的点云，将事先设定好的第一分辨率L1为作为分辨率，划分成若干个三维栅格，要求在划分后，落在每个栅格中的点数大于Nmin；
本实施例中的第一分辨率L1为1m。可以理解的是，在其他实施例中，可以根据需要将第一分辨率L1的取值确定为其他数值。
本实施例中，Nmin＝5。在其他实施例中，Nmin可取其他数值，其确定原则是保证计算出的概率密度分布函数能够反应点云的实际分布情况，可进行多次重复实验以确定最优值。
S2.6、计算栅格中所有点的重心，即平均三维坐标，使用重心代替栅格中的其他点；
S2.7、遍历点云的所有栅格，执行S2.6，得到下采样后的点云并保存。
S3、根据是否为第一帧点云，进行点云地图初始化。
所述车载激光雷达安装参数包括车载激光雷达与后轴中心的高度差、车载激光雷达与车辆轴线方向的距离和雷达相位角。
在本发明其中一个实施例中，所述点云地图初始化具体包括以下步骤：
S3.1、在计算平台的ROS通讯包中，配置GPS/RTK单元驱动节点；
S3.2、启动GPS/RTK单元驱动节点，实时读取、解析全局定位数据，获得自车所在位置经纬度；
S3.3、记录第一帧激光雷达数据到来时，车辆的全局坐标；
S3.4、设置地图原点为该全局坐标和朝向角，后续位姿为相对该原点的偏移量和相对夹角；
S3.5、记录第一帧数据的时间戳，后续观测的时间戳为相对第一帧的偏移时间，并将平均时间差值作为雷达是否正常工作的判断条件，当平均时间大于预设阈值时提前停止录制，保存数据。
S4、利用基于正态分布变换的激光里程计匹配得到位姿变换矩阵后，将该位姿变换矩阵应用到新的观测点云上，配准前后两帧点云。
利用正态分布变换匹配，配准前后两帧点云，得到位姿变换矩阵，具体包括以下步骤：在本发明其中一个实施例中，本步骤具体包括：
S4.1、将上一帧点云记为参考点云，当前帧记为待配准点云；
S4.2、按照预设的第二分辨率L2将参考点云栅格化，此处分辨率可与下采样时不同，由实际点云密度确定；
本实施例中的第二分辨率L2为1m。可以理解的是，在其他实施例中，可以根据需要将第一分辨率L1的取值确定为其他数值。
S4.3、对于划分后的栅格，基于栅格内的点云分布计算其均值和协方差；
式中，u是栅格内所有点的平均坐标向量，xi是栅格内某个点的坐标向量，i是点的序号，T是向量的转置，∑是栅格内所有点的坐标协方差，n是栅格内点云的数量；
S4.4、基于均值和协方差，建立多维概率密度函数，该函数描述了栅格内的点云块的空间位置、方向和平整度；
S4.5、在待配准的新一帧的观测点云q上应用位姿变换矩阵，得到配准点云q′，利用配准点云q′和参考点云构建最大似然函数，具体包括以下步骤：
S4.5.1、初始化位姿变换矩阵为一个单位矩阵I；
S4.5.2、将待配准的观测点云q′使用预设的第二分辨率L2栅格化，索引得到同一坐标位置的参考点云栅格，计算相应的概率密度分布函数：
式中，x为栅格内的点云三维坐标向量。
S4.5.3、遍历所有栅格，对于大于Nmin个点的栅格(在本发明其中一个实施例中，Nmin＝5)，返回步骤S4.5.2进行处理；
S4.5.4、令最大似然函数为所有栅格的概率分布函数之和，并作为步骤S4.6的优化目标函数：
式中，xi′为经位姿变换后的点云三维坐标向量。
S4.6、使用牛顿法求解最大似然函数，输出最终位姿变换矩阵，并将该矩阵应用在待配准点云上得到最终配准点云，拼接到点云地图上。
S5、利用八叉树地图得到点云地图的栅格化表示并把第一次更新的栅格初始化其占据栅格概率为一默认值；否则基于栅格占据与否，更新自由状态计数器，并用计数器值作为权重，重新计算占据概率，将占据概率低于阈值的栅格设置为稳定自由栅格，并将稳定自由栅格内的点云标记为非地图元素，删除稳定自由栅格内的点云，剩下的点云即为离线地图。
本步骤包括以下子步骤：
S5.1、对于经过位姿变换后得到的点云，以第一分辨率L1将点云划分成若干个三维栅格，并初始化叶子节点分辨率L3，在本发明其中一个实施例中，叶子节点分辨率为L3为0.1m，可以理解的是，在其他实施例中，可以根据需要将叶子节点分辨率L3取值确定为其他数值；
S5.2、从点云地图开始，以八叉树数据结构的方式划分栅格。每个节点从三个方向划分为8个子节点，直到节点的长度小于等于叶子节点分辨率L3；
S5.3、将叶子节点的占据概率初始化为一默认值，即栅格的占据状态未知，同时初始化一自由状态计数器C，当栅格被观测到自由时，自由状态计数器加1，占据标记值为0；
S5.4、随点云观测，基于贝叶斯公式更新叶子节点的占据概率，并取所有叶子节点的平均值作为父节点的占据概率；
S5.5、设置最大占据概率阈值Smin和最小占据概率阈值Smax，从八叉树地图中去除占据概率小于等于最小占据概率阈值的稳定自由栅格内的点云，在本发明其中一个实施例中，最大占据概率阈值Smin为0.1，最小占据概率阈值Smax为0.9；
所述基于观测到的栅格占据与否，更新自由状态计数器，并用计数器值作为权重，重新计算占据概率包括以下步骤：
S5.6、若当前栅格为稳定自由栅格，则跳过该步骤S5.7及之后的步骤，输出滤除了非地图元素的地图，若当前栅格不是稳定自由栅格，则从步骤S5.7继续执行；
S5.7、根据观测结果，更新自由状态计数器；其中，当采集到新的激光雷达传感器数据后，执行了S4.5-S5.3的步骤之后得到的，待配准点云的八叉树地图格式表示。从这个格式中可以读取每个栅格在当前帧是否“被占据”。占据的意思是，在该栅格处，雷达光束返回了激光点(表示这个位置有障碍物)。
S5.8、根据观测和自由状态计数器，基于贝叶斯公式更新栅格的占据概率：
式中，St为当前时刻的占据概率值，St-1为上一时刻占据概率值，C为自由状态计数器值，Sz为当前观测所得占据标记值，当观测到占据时为1，否则(即自由时)为0。当栅格为自由时，C＝C+1，否则C＝C-1，但C≥0。
S5.9、判断当前节点是否达到稳定状态：记占据概率到达最大阈值的栅格为稳定占据栅格，记占据概率小于等于最小阈值的为稳定自由栅格；
S5.10、返回步骤S5.5，实时对叶子节点进行修正，得到滤除了非地图元素的高精度地图。
当采集数据处理完毕时，或拼接的地图不再增长时，图2所示的占据概率更新流程终止，输出结果。
本发明实施例利用环境中动态障碍物在连续帧点云观测中的投影不确定性，降低地图中非地图元素占比，在减少建图误差的同时不影响效率，达到同时兼顾准确性和实时性的目的。此方法构建的高精点云地图可作为高精度地图的基础图层来确保后续语义标注精度。
以上所述，仅为本发明选择的实施例，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明所公开的范围内，根据本发明的技术方案及其发明构思加以等同替换或改变，都属于本发明的保护范围。

Claims (8)

1.一种基于非地图元素过滤的高精度地图构建方法，其特征在于，包括以下步骤：
S1、利用激光雷达扫描周围环境，采集点云数据；
S2、对点云数据进行预处理；
S3、根据是否为第一帧点云，进行点云地图初始化；
S4、利用正态分布变换匹配得到位姿变换矩阵后，将该位姿变换矩阵应用到新的观测点云上，配准前后两帧点云以进行局部拼接地图；
S5、利用八叉树地图得到点云地图的栅格化表示并把第一次更新的栅格初始化其占据栅格概率为一默认值；否则基于观测到的栅格占据与否，更新自由状态计数器，并用计数器值作为权重，重新计算占据概率，将占据概率小于等于阈值的栅格设置为稳定自由栅格，并将稳定自由栅格内的点云标记为非地图元素，删除稳定自由栅格内的点云，得到滤除了非地图元素的地图；
其中，S5中将配准点云转化为八叉树地图包括以下步骤：
S5.1、对于经过位姿变换后得到的点云，以第一分辨率L1将点云划分成若干个三维栅格，并初始化叶子节点的分辨率为L3；
S5.2、从点云地图开始，以八叉树数据结构的方式划分栅格，每个节点从三个方向划分为多个子节点，直到节点的长度小于等于叶子节点分辨率L3；
S5.3、将叶子节点的占据概率初始化为一默认值，即栅格的占据状态未知，同时初始化一自由状态计数器，当栅格被观测到自由时，自由状态计数器加一；
S5.4、随点云观测，基于贝叶斯公式更新叶子节点的占据概率，并取所有叶子节点的平均值作为父节点的占据概率；
S5.5、设置最大占据概率阈值Smin和最小占据概率阈值Smax，从八叉树地图中去除占据概率小于等于最小占据概率阈值Smax的稳定自由栅格；
其中，利用观测更新栅格的占据概率，包括以下步骤：
S5.6、若栅格当前为稳定状态栅格，跳过该步骤，否则进行步骤S5.7；
S5.7、根据观测结果，更新自由状态计数器；
S5.8、根据观测和自由状态计数器，基于贝叶斯公式更新栅格的占据概率：
式中，St为当前时刻的占据概率值，St-1为上一时刻占据概率值，C为自由状态计数器值，Sz为当前观测所得占据标记值；
S5.9、判断当前节点是否达到稳定状态：记占据概率到达最大阈值的栅格为稳定占据栅格，记占据概率到达最小阈值的为稳定自由栅格；
S5.10、返回步骤S5.5，实时对叶子节点进行修正，得到滤除了非地图元素的高精度地图。
2.根据权利要求1所述的一种基于非地图元素过滤的高精度地图构建方法，其特征在于，步骤S1具体包括以下步骤：
S1.1、将一机械旋转式多线数激光雷达固定在车辆上，且所固定的位置不会在激光雷达的扫描范围内形成遮挡；
S1.2、准备车载计算单元，在车载计算单元的计算平台中安装Ubuntu操作系统和ROS通讯包，配置激光雷达驱动节点；
S1.3、启动激光雷达驱动节点，设置采集频率和数据格式，采集数据过程中，在预设的时间内切为一个数据包；
S1.4、使用ROS操作系统中的rosbag工具按帧回放录制好的点云数据包。
3.根据权利要求2所述的一种基于非地图元素过滤的高精度地图构建方法，其特征在于，步骤S3中，所述点云地图初始化包括以下步骤：
S3.1、在计算平台的ROS通讯包中，配置GPS/RTK单元驱动节点；
S3.2、启动GPS/RTK单元驱动节点，实时读取、解析全局定位数据，获得自车所在位置经纬度；
S3.3、记录第一帧激光雷达数据到来时，车辆的全局坐标；
S3.4、设置地图原点为该全局坐标和朝向角，后续位姿为相对该原点的偏移量和相对夹角；
S3.5、记录第一帧数据的时间戳，后续观测的时间戳为相对第一帧的偏移时间，并将平均时间差值作为雷达是否正常工作的判断条件，当平均时间大于阈值时停止录制，保存数据。
4.根据权利要求1所述的一种基于非地图元素过滤的高精度地图构建方法，其特征在于，步骤S3中，点云地图初始化的参数包括坐标原点、体素滤波分辨率、最大邻近点数和初始占据概率。
5.根据权利要求1所述的一种基于非地图元素过滤的高精度地图构建方法，其特征在于，步骤S2中，对点云数据进行预处理包括利用统计滤波器去除离群点，具体包括以下步骤：
S2.1、设置最大邻近点数A，针对点云中的某个点，计算它们与周围距离最小的最多A个点的平均距离μ和标准差σ；
S2.2、根据μ和σ构建高斯分布；
S2.3、若某个点在该高斯分布下的标准差大于一倍的标准差σ，则视为离群噪点予以删除；
S2.4、遍历点云中的每个点，执行S2.1-S2.3，直到完成所有点的处理，保存处理后的点云。
6.根据权利要求5所述的一种基于非地图元素过滤的高精度地图构建方法，其特征在于，步骤S2中，所述对点云数据还包括采用体素滤波器对经统计滤波器处理后的点云进行下采样，包括：
S2.5、对于经统计滤波器处理后得到的点云，按照预设的第一分辨率L1将点云划分成若干个三维栅格，划分后，落在每个栅格中的点数大于Nmin；
S2.6、计算栅格中所有点的重心，使用重心代替栅格中的其他点；
S2.7、遍历点云的所有栅格，执行S2.6，得到下采样后的点云。
7.根据权利要求1所述的一种基于非地图元素过滤的高精度地图构建方法，其特征在于，步骤S4中，所述利用正态分布变换匹配得到位姿变换矩阵后，将该位姿变换矩阵应用到新的观测点云上，配准前后两帧点云包括以下步骤：
S4.1、将上一帧点云记为参考点云，当前帧记为待配准点云x；
S4.2、按照预设的第二分辨率L2将参考点云栅格化；
S4.3、对于划分后的栅格，基于栅格内的点云分布计算其均值和协方差；
式中，u是栅格内所有点的平均坐标向量，xi是栅格内某个点的坐标向量，i是点的序号，T是向量的转置、∑是栅格内所有点的坐标协方差，n是栅格内点云的数量；
S4.4、基于均值和协方差，建立多维概率密度函数，所述多维概率密度函数描述了栅格内的点云块的空间位置、方向和平整度；
S4.5、在待配准的新一帧的观测点云q上应用位姿变换矩阵，得到配准点云q′，利用配准点云q′和参考点云构建最大似然函数；
S4.6、求解最大似然函数，输出最终位姿变换矩阵，并将位姿变换矩阵应用在待配准的观测点云q上得到最终配准点云，拼接到点云地图上。
8.根据权利要求7所述的一种基于非地图元素过滤的高精度地图构建方法，其特征在于，S4.5中所述利用配准点云q′和参考点云构建最大似然函数包括以下步骤：
S4.5.1、初始化位姿变换矩阵为一个单位矩阵I；
S4.5.2、将待配准点云q′使用预设的第二分辨率L2栅格化，索引得到同一坐标位置的参考点云栅格，计算相应的概率密度分布函数：
式中，u为栅格内所有点的平均坐标向量；
S4.5.3、遍历所有栅格，针对大于Nmin个点的栅格，返回步骤S4.5.2进行处理；
S4.5.4、令最大似然函数为所有栅格的概率分布函数之和，并作为步骤S4.6的优化目标函数：
式中，score(p)为最大似然函数值，p(x′i)为经过变换后的栅格内点云的概率密度函数，x′i为经位姿变换后的点云三维坐标向量。
CN202110770937.7A 2021-07-07 2021-07-07 一种基于非地图元素过滤的高精度地图构建方法 Active CN113640822B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110770937.7A CN113640822B (zh) 2021-07-07 2021-07-07 一种基于非地图元素过滤的高精度地图构建方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110770937.7A CN113640822B (zh) 2021-07-07 2021-07-07 一种基于非地图元素过滤的高精度地图构建方法

Publications (2)

Publication Number Publication Date
CN113640822A CN113640822A (zh) 2021-11-12
CN113640822B true CN113640822B (zh) 2023-08-18

Family

ID=78416901

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110770937.7A Active CN113640822B (zh) 2021-07-07 2021-07-07 一种基于非地图元素过滤的高精度地图构建方法

Country Status (1)

Country Link
CN (1) CN113640822B (zh)

Families Citing this family (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113239062A (zh) * 2021-06-18 2021-08-10 恒大新能源汽车投资控股集团有限公司 一种停车场地图更新方法、装置、车辆及服务器
CN113920180B (zh) * 2021-12-08 2022-04-01 杭州速玛科技有限公司 一种基于正态分布变换假设校验的点云配准优化方法
CN114659513B (zh) * 2022-03-11 2024-04-09 北京航空航天大学 一种面向非结构化道路的点云地图构建与维护方法
CN117075171B (zh) * 2023-10-18 2024-01-16 新石器慧通（北京）科技有限公司 激光雷达的位姿信息确定方法、装置、设备及存储介质

Citations (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106204705A (zh) * 2016-07-05 2016-12-07 长安大学 一种基于多线激光雷达的3d点云分割方法
US10131446B1 (en) * 2015-07-16 2018-11-20 Near Earth Autonomy, Inc. Addressing multiple time around (MTA) ambiguities, particularly for lidar systems, and particularly for autonomous aircraft
CN110927740A (zh) * 2019-12-06 2020-03-27 合肥科大智能机器人技术有限公司 一种移动机器人定位方法
CN111210475A (zh) * 2020-04-21 2020-05-29 浙江欣奕华智能科技有限公司 一种地图更新方法及装置
CN111598916A (zh) * 2020-05-19 2020-08-28 金华航大北斗应用技术有限公司 一种基于rgb-d信息的室内占据栅格地图的制备方法
CN112014857A (zh) * 2020-08-31 2020-12-01 上海宇航系统工程研究所 用于智能巡检的三维激光雷达定位导航方法及巡检机器人
CN112070770A (zh) * 2020-07-16 2020-12-11 国网安徽省电力有限公司检修分公司 一种高精度三维地图与二维栅格地图同步构建方法
CN112762937A (zh) * 2020-12-24 2021-05-07 哈尔滨工业大学芜湖机器人产业技术研究院 一种基于占据栅格的2d激光序列点云配准方法
CN112785702A (zh) * 2020-12-31 2021-05-11 华南理工大学 一种基于2d激光雷达和双目相机紧耦合的slam方法
CN112859859A (zh) * 2021-01-13 2021-05-28 中南大学 一种基于三维障碍物体素对象映射的动态栅格地图更新方法
CN113009453A (zh) * 2020-03-20 2021-06-22 青岛慧拓智能机器有限公司 矿山路沿检测及建图方法及装置

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108319655B (zh) * 2017-12-29 2021-05-07 百度在线网络技术（北京）有限公司 用于生成栅格地图的方法和装置

Patent Citations (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US10131446B1 (en) * 2015-07-16 2018-11-20 Near Earth Autonomy, Inc. Addressing multiple time around (MTA) ambiguities, particularly for lidar systems, and particularly for autonomous aircraft
CN106204705A (zh) * 2016-07-05 2016-12-07 长安大学 一种基于多线激光雷达的3d点云分割方法
CN110927740A (zh) * 2019-12-06 2020-03-27 合肥科大智能机器人技术有限公司 一种移动机器人定位方法
CN113009453A (zh) * 2020-03-20 2021-06-22 青岛慧拓智能机器有限公司 矿山路沿检测及建图方法及装置
CN111210475A (zh) * 2020-04-21 2020-05-29 浙江欣奕华智能科技有限公司 一种地图更新方法及装置
CN111598916A (zh) * 2020-05-19 2020-08-28 金华航大北斗应用技术有限公司 一种基于rgb-d信息的室内占据栅格地图的制备方法
CN112070770A (zh) * 2020-07-16 2020-12-11 国网安徽省电力有限公司检修分公司 一种高精度三维地图与二维栅格地图同步构建方法
CN112014857A (zh) * 2020-08-31 2020-12-01 上海宇航系统工程研究所 用于智能巡检的三维激光雷达定位导航方法及巡检机器人
CN112762937A (zh) * 2020-12-24 2021-05-07 哈尔滨工业大学芜湖机器人产业技术研究院 一种基于占据栅格的2d激光序列点云配准方法
CN112785702A (zh) * 2020-12-31 2021-05-11 华南理工大学 一种基于2d激光雷达和双目相机紧耦合的slam方法
CN112859859A (zh) * 2021-01-13 2021-05-28 中南大学 一种基于三维障碍物体素对象映射的动态栅格地图更新方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
基于激光雷达的三维高精度地图构建及定位;余睿;《中国优秀硕士学位论文全文数据库工程科技Ⅱ辑(月刊)》(第2期);第C035-347页 *

Also Published As

Publication number Publication date
CN113640822A (zh) 2021-11-12

Similar Documents

Publication Publication Date Title
CN113640822B (zh) 一种基于非地图元素过滤的高精度地图构建方法
CN111220993B (zh) 目标场景定位方法、装置、计算机设备和存储介质
CN112212874B (zh) 车辆轨迹预测方法、装置、电子设备及计算机可读介质
CN111652179A (zh) 基于点线特征融合激光的语义高精地图构建与定位方法
DE112019001657T5 (de) Signalverarbeitungsvorrichtung und signalverarbeitungsverfahren, programm und mobiler körper
JP2021508815A (ja) 妨害物体の検出に基づいて高精細度マップを補正するためのシステムおよび方法
CN108021862A (zh) 道路标志识别
CN113009506B (zh) 一种虚实结合的实时激光雷达数据生成方法、系统及设备
CN112162297B (zh) 一种剔除激光点云地图中动态障碍伪迹的方法
EP4016115A1 (en) Vehicle localization based on radar detections
CN114930401A (zh) 基于点云的三维重建方法、装置和计算机设备
CN111880191B (zh) 基于多智能体激光雷达和视觉信息融合的地图生成方法
JP2022542289A (ja) 地図作成方法、地図作成装置、電子機器、記憶媒体及びコンピュータプログラム製品
CN115249349B (zh) 一种点云去噪方法、电子设备及存储介质
CN111402414A (zh) 一种点云地图构建方法、装置、设备和存储介质
CN114509065B (zh) 地图构建方法、系统、车辆终端、服务器端及存储介质
CN114485698B (zh) 一种交叉路口引导线生成方法及系统
CN113592891B (zh) 无人车可通行域分析方法及导航栅格地图制作方法
CN112740225A (zh) 一种路面要素确定方法及装置
US20230049383A1 (en) Systems and methods for determining road traversability using real time data and a trained model
WO2022047744A1 (zh) 一种用于地图的路面提取方法及装置
CN115176288A (zh) 用于重建道路的环境场景中的特征的方法
CN113724387A (zh) 一种激光与相机融合的地图构建方法
Gressenbuch et al. Mona: The munich motion dataset of natural driving
CN114556419A (zh) 三维点云分割方法和装置、可移动平台

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant