CN116539053A - 一种实时全局点云地图构建方法、系统、设备及存储介质 - Google Patents
一种实时全局点云地图构建方法、系统、设备及存储介质 Download PDFInfo
- Publication number
- CN116539053A CN116539053A CN202310639773.3A CN202310639773A CN116539053A CN 116539053 A CN116539053 A CN 116539053A CN 202310639773 A CN202310639773 A CN 202310639773A CN 116539053 A CN116539053 A CN 116539053A
- Authority
- CN
- China
- Prior art keywords
- point cloud
- frame
- data
- vehicle
- pose data
- 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.)
- Pending
Links
- 238000010276 construction Methods 0.000 title claims description 16
- 238000000034 method Methods 0.000 claims abstract description 50
- 230000003068 static effect Effects 0.000 claims abstract description 37
- 239000011159 matrix material Substances 0.000 claims description 30
- 238000006243 chemical reaction Methods 0.000 claims description 28
- 230000015654 memory Effects 0.000 claims description 26
- 238000012545 processing Methods 0.000 claims description 17
- 238000013519 translation Methods 0.000 claims description 16
- 238000004590 computer program Methods 0.000 claims description 15
- 238000013507 mapping Methods 0.000 claims description 14
- 238000001914 filtration Methods 0.000 claims description 12
- 238000013178 mathematical model Methods 0.000 claims description 8
- 230000009286 beneficial effect Effects 0.000 abstract description 4
- 238000005516 engineering process Methods 0.000 description 12
- 230000006870 function Effects 0.000 description 7
- 238000010586 diagram Methods 0.000 description 6
- 102100031629 COP9 signalosome complex subunit 1 Human genes 0.000 description 5
- 102100022148 G protein pathway suppressor 2 Human genes 0.000 description 5
- 101000940485 Homo sapiens COP9 signalosome complex subunit 1 Proteins 0.000 description 5
- 101000900320 Homo sapiens G protein pathway suppressor 2 Proteins 0.000 description 5
- 238000009826 distribution Methods 0.000 description 5
- 230000000694 effects Effects 0.000 description 4
- 238000012800 visualization Methods 0.000 description 4
- 230000005540 biological transmission Effects 0.000 description 3
- 230000009191 jumping Effects 0.000 description 3
- 230000003287 optical effect Effects 0.000 description 3
- 238000012216 screening Methods 0.000 description 3
- 230000004807 localization Effects 0.000 description 2
- 238000005070 sampling Methods 0.000 description 2
- 230000001360 synchronised effect Effects 0.000 description 2
- 230000000007 visual effect Effects 0.000 description 2
- 238000001514 detection method Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 238000005286 illumination Methods 0.000 description 1
- 239000004973 liquid crystal related substance Substances 0.000 description 1
- 238000009827 uniform distribution Methods 0.000 description 1
Classifications
-
- 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
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Processing Or Creating Images (AREA)
Abstract
本发明公开了一种实时全局点云地图构建方法、系统、设备及存储介质，其中所述方法包括：在车辆行驶之前，根据采集的车辆周围环境的多帧第一点云和车辆的多帧第一位姿数据，构建局部点云地图；当车辆在既定实验道路上绕行一圈时，根据采集的车辆周围环境的多帧第二点云和车辆的多帧第二位姿数据，对所述局部点云地图进行点云填充以得到静态全局点云地图；当车辆在任意道路上行驶时，根据实时采集的车辆周围环境的单帧第三点云和车辆的单帧第三位姿数据，对所述静态全局点云地图进行更新以形成实时全局点云地图。本发明通过结合车辆位姿数据和车辆周围环境的点云可以确定车辆的绝对位置信息，有助于构建出更为可靠的全局点云地图。
Description
技术领域
本发明涉及自动驾驶技术领域，具体是涉及一种实时全局点云地图构建方法、系统、设备及存储介质。
背景技术
SLAM(Simultaneous Localization and Mapping，即时定位与地图构建)技术是指车辆建立当前环境下的全局地图并使用该全局地图在任何时间点导航或推断自身位置的过程。随着自动驾驶技术的快速发展，对于车辆位置信息的精度要求越来越高，SLAM技术也越来越受到人们的关注。通过SLAM技术可以实现对自动驾驶车辆的实时定位，并对车辆周围环境进行实时建模与动态更新，保证车辆位置数据的精确性，从而为车辆的决策与控制提供数据支撑。
目前基于SLAM的定位导航技术主要分为激光SLAM技术和视觉SLAM技术，但这两类SLAM技术均是采用帧间匹配和相对定位的方式来获取车辆的实时位置信息，当车辆行驶在空旷道路上时，由于相邻帧之间的特征变化极不明显或者点云数量过于稀少，会导致车辆无法实现自身定位，从而出现位置丢失的现象，对于车辆实现自动驾驶功能造成极大阻碍。
发明内容
本发明提供一种实时全局点云地图构建方法、系统、设备及存储介质，以解决现有技术中所存在的一个或多个技术问题，至少提供一种有益的选择或创造条件。
第一方面，提供一种实时全局点云地图构建方法，所述方法包括：
步骤100、在车辆行驶之前，根据采集的车辆周围环境的多帧第一点云和车辆的多帧第一位姿数据，构建局部点云地图；
步骤200、当车辆在既定实验道路上绕行一圈时，根据采集的车辆周围环境的多帧第二点云和车辆的多帧第二位姿数据，对所述局部点云地图进行点云填充以得到静态全局点云地图；
步骤300、当车辆在任意道路上行驶时，根据实时采集的车辆周围环境的单帧第三点云和车辆的单帧第三位姿数据，对所述静态全局点云地图进行更新以形成实时全局点云地图。
进一步地，所述步骤100包括：
步骤110、对所述多帧第一点云进行滤波处理；
步骤120、对滤波后的多帧第一点云进行下采样处理，得到若干帧第一点云；
步骤130、所述多帧第一位姿数据包括多帧第一位置数据和多帧第一姿态数据，将所述多帧第一位置数据由大地坐标系转换到UTM坐标系表示，再结合所述多帧第一姿态数据和所述若干帧第一点云进行时间同步处理，得到第一位姿数据集合和第一点云集合；
步骤140、根据经坐标转换后的首帧第一位置数据构建点云地图坐标系，再利用所述第一位姿数据集合将所述第一点云集合转换到所述点云地图坐标系进行叠加，形成局部点云地图。
进一步地，滤波后的每帧第一点云包含若干个点云数据，所述步骤120包括：
将滤波后的多帧第一点云所构成的三维点云空间均匀划分成多个扇形区域，再利用数学模型对落在每一个扇形区域内的所有点云数据进行下采样处理，进而得到若干帧第一点云；所述数学模型为：
其中，Sk为第k个扇形区域，pki为落在第k个扇形区域内的第i个点云数据，其由激光雷达采集且在激光雷达坐标系下的位置信息为(pxki,pyki,pzki)，arctan(pxki,pyki)为X轴正方向与通过点(pxki,pyki)和坐标原点的射线之间的夹角，s为所述多个扇形区域的数量，P为滤波后的多帧第一点云所包含的所有点云数据，为所述三维点云空间。
进一步地，在所述步骤130中，结合经坐标转换后的多帧第一位置数据、所述多帧第一姿态数据和所述若干帧第一点云进行时间同步处理，得到第一位姿数据集合和第一点云集合包括：
步骤131、根据经坐标转换后的多帧第一位置数据和所述多帧第一姿态数据，确定待匹配的多帧第四位姿数据；
步骤132、将所述多帧第四位姿数据以时间戳由小到大的顺序存储至第一预设队列，将所述若干帧第一点云以时间戳由小到大的顺序存储至第二预设队列；
步骤133、从所述第一预设队列中读取排列在最前的单帧第四位姿数据，从所述第二预设队列中读取排列在最前的单帧第一点云；
步骤134、判断所述单帧第四位姿数据的时间戳与所述单帧第一点云的时间戳之间的差值绝对值是否小于等于预设时间阈值；若是，则从所述第一预设队列中提取所述单帧第四位姿数据进行保存，从所述第二预设队列中提取所述单帧第一点云进行保存，再执行步骤135；若否，则将时间戳更小的所述单帧第四位姿数据从所述第一预设队列中剔除，或者将时间戳更小的所述单帧第一点云从所述第二预设队列中剔除，再执行步骤135；
步骤135、判断所述第一预设队列和所述第二预设队列中是否至少有一个为空；若是，则将保存的所有第四位姿数据形成第一位姿数据集合输出，将保存的所有第一点云形成第一点云集合输出；若否，则返回步骤133。
进一步地，在所述步骤140中，利用所述第一位姿数据集合将所述第一点云集合转换到所述点云地图坐标系进行叠加，形成局部点云地图包括：
从所述第一点云集合中获取单帧第一点云，再从所述第一位姿数据集合中获取与所述单帧第一点云存在时间同步关系的单帧第四位姿数据；
所述单帧第四位姿数据包含经坐标转换后的单帧第一位置数据和单帧第一姿态数据，根据所述单帧第一姿态数据来确定旋转矩阵，根据经坐标转换后的首帧第一位置数据和所述经坐标转换后的单帧第一位置数据来确定平移矩阵；
根据所述旋转矩阵和所述平移矩阵，将所述单帧第一点云中所包含的所有点云数据转换到所述点云地图坐标系表示；
循环执行上述各个步骤，直至将所述第一点云集合中的所有第一点云所包含的所有点云数据转换到所述点云地图坐标系表示之后再完成点云叠加，形成局部点云地图。
进一步地，所述步骤200包括：
对所述多帧第二点云进行滤波处理；
对滤波后的多帧第二点云进行下采样处理，得到若干帧第二点云；
所述多帧第二位姿数据包括多帧第二位置数据和多帧第二姿态数据，将所述多帧第二位置数据由大地坐标系转换到UTM坐标系表示，再结合所述多帧第二姿态数据和所述若干帧第二点云进行时间同步处理，得到第二位姿数据集合和第二点云集合；
利用所述第二位姿数据集合将所述第二点云集合转换到所述局部点云地图所在的点云地图坐标系进行点云填充与叠加，得到静态全局点云地图。
进一步地，所述单帧第三点云与所述单帧第三位姿数据存在时间同步关系，所述步骤300包括：
将所述单帧第三点云的中心点映射到所述静态全局点云地图所在的点云地图坐标系，再从所述静态全局点云地图中剔除处于所述中心点所在既定半径范围内的所有点云数据；
对所述单帧第三点云进行滤波处理；
所述单帧第三位姿数据包括单帧第三位置数据和单帧第三姿态数据，将所述单帧第三位置数据由大地坐标系转换到UTM坐标系表示，再结合所述单帧第三姿态数据将滤波后的单帧第三点云转换到更新后的静态全局点云地图所在的点云地图坐标系进行点云填充与叠加，再将当前的静态全局点云地图作为实时全局点云地图输出。
第二方面，提供一种实时全局点云地图构建系统，所述系统包括：
构建模块，用于在车辆行驶之前，根据采集的车辆周围环境的多帧第一点云和车辆的多帧第一位姿数据，构建局部点云地图；
填充模块，用于当车辆在既定实验道路上绕行一圈时，根据采集的车辆周围环境的多帧第二点云和车辆的多帧第二位姿数据，对所述局部点云地图进行点云填充以得到静态全局点云地图；
更新模块，用于当车辆在任意道路上行驶时，根据实时采集的车辆周围环境的单帧第三点云和车辆的单帧第三位姿数据，对所述静态全局点云地图进行更新以形成实时全局点云地图。
第三方面，提供一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，所述处理器执行所述计算机程序以实现如第一方面所述的实时全局点云地图构建方法。
第四方面，提供一种计算机可读存储介质，其上存储计算机程序，所述计算机程序被处理器执行时实现如第一方面所述的实时全局点云地图构建方法。
本发明至少具有以下有益效果：利用车辆的实时位姿数据将存在时间同步关系的车辆周围环境的实时点云转换到既定点云地图坐标系下，由此可以实时获取车辆的绝对位置信息，有助于构建出更为可靠的全局点云地图。相较于现有的仅依靠点云进行相对定位的激光SLAM技术和视觉SLAM技术而言，本发明所提供的实时全局点云地图构建方法可以应用在不同行驶场景中，受到环境影响较小，可以有效解决在建图过程中出现的车辆位置丢失这一问题。
附图说明
附图用来提供对本发明技术方案的进一步理解，并且构成说明书的一部分，与本发明的实施例一起用于解释本发明的技术方案，并不构成对本发明技术方案的限制。
图1是本发明实施例中的一种实时全局点云地图构建方法的流程示意图；
图2是本发明实施例中的一种实时全局点云地图构建系统的组成示意图；
图3是本公开实施例中的计算机设备的硬件结构示意图。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。
需要说明的是，虽然在系统示意图中进行了功能模块划分，在流程图中示出了逻辑顺序，但是在某些情况下，可以以不同于系统中的模块划分，或流程图中的顺序执行所示出或描述的步骤。本申请的说明书及上述附图中的术语“第一”、“第二”、“第三”、“第四”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序，应该理解这样使用的数据在适当情况下可以互换，以便这里描述的本申请的实施例能够以除了在这里图示或描述的那些以外的顺序实施。此外，术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含，例如包含了一系列步骤或单元的过程、方法、系统、产品或装置不必限定于清楚列出的那些步骤或单元，而是可以包含没有清楚列出的对于这些过程、方法、产品或装置固有的其他步骤或单元。
请参考图1，图1是本发明实施例提供的一种实时全局点云地图构建方法的流程示意图，所述方法包括如下步骤：
S110、在车辆行驶之前，根据采集的车辆周围环境的多帧第一点云和车辆的多帧第一位姿数据，构建局部点云地图；
S120、当车辆在既定实验道路上绕行一圈时，根据采集的车辆周围环境的多帧第二点云和车辆的多帧第二位姿数据，对所述局部点云地图进行点云填充以得到静态全局点云地图；
S130、当车辆在任意道路上行驶时，根据实时采集的车辆周围环境的单帧第三点云和车辆的单帧第三位姿数据，对所述静态全局点云地图进行更新以形成实时全局点云地图。
在本发明实施例中，上述步骤S110至上述步骤S130是在车载工控机内部运行的，所述车载工控机所采用的操作系统为Linux；上述步骤S110所提及到的每帧第一点云、上述步骤S120所提及到的每帧第二点云以及上述步骤S130所提及到的所述单帧第三点云均是通过设置在车辆顶部的激光雷达以10Hz扫描速度对车辆周围环境进行扫描采集得到的，并且所述激光雷达可以对采集到的任意每帧点云进行系统授时处理，最后通过其与所述车载工控机之间的有线连接关系来实现点云传输功能。
其中，所述激光雷达受到光照、天气、温度等环境因素的影响较小，探测范围较大，可以获取到可靠稳定且精确的道路环境信息，包括路面信息、周边其他车辆信息以及道路两侧环境信息等等。
在本发明实施例中，上述步骤S110所提及到的每帧第一位姿数据是由满足时间同步要求的每帧第一姿态数据和每帧第一位置数据组成的，上述步骤S120所提及到的每帧第二位姿数据是由满足时间同步要求的每帧第二姿态数据和每帧第二位置数据组成的，上述步骤S130所提及到的所述单帧第三位姿数据是由满足时间同步要求的单帧第三姿态数据和单帧第三位置数据组成的；所述每帧第一姿态数据、所述每帧第二姿态数据以及所述单帧第三姿态数据均是通过车载惯导设备进行采集得到的，并且所述车载惯导设备可以对采集到的任意每帧姿态数据进行系统授时，再通过其与所述车载工控机之间的有线连接关系来实现姿态数据的串口传输功能；所述每帧第一位置数据、所述每帧第二位置数据以及所述单帧第三位置数据均是通过车载GPS(Global Positioning System，全球定位系统)设备进行采集得到的，并且所述车载GPS设备可以对采集到的任意每帧位置数据进行系统授时，再通过其与所述车载工控机之间的有线连接关系来实现位置数据的串口传输功能。
在本发明实施例中，当车辆在一段时间内保持静止状态时(即在车辆行驶之前)，利用所述激光雷达采集获取在这一段时间内车辆周围环境的多帧第一点云，利用所述车载惯导设备采集获取在这一段时间内车辆的多帧第一姿态数据，以及利用所述车载GPS设备采集获取在这一段时间内车辆的多帧第一位置数据，并且所述多帧第一姿态数据对应与所述多帧第一位置数据存在时间同步关系，此时将所述多帧第一姿态数据和所述多帧第一位置数据按照关联时间戳进行捆绑处理以得到多帧第一位姿数据。
在此基础上，上述步骤S110的具体实施过程包括但不仅限于以下步骤：
S111、利用现有的StatisticalOutlierRemoval滤波器对所述多帧第一点云中的每帧第一点云执行滤波操作；
S112、将滤波后的多帧第一点云执行下采样操作，进而从中获取包含点云数据量相对较少的若干帧第一点云；
S113、考虑到所述多帧第一位置数据当前是在大地坐标系下进行表示的，即所述多帧第一位置数据是经纬高格式的，无法直接进行解算，此时将所述多帧第一位置数据转换到UTM(Universal Transverse Mercator Grid System，通用横墨卡托格网系统)坐标系下进行表示，再将所述多帧第一姿态数据和经坐标转换后的多帧第一位置数据按照关联时间戳进行捆绑处理，得到待匹配的多帧第四位姿数据；
S114、将所述若干帧第一点云和所述多帧第四位姿数据在时间维度上执行同步处理操作，得到第一点云集合和第一位姿数据集；
S115、以经坐标转换后的首帧第一位置数据(其是由所述多帧第一位置数据中时间戳最小的单帧第一位置数据进行坐标转换得到的)为坐标原点来建立点云地图坐标系，并且所述点云地图坐标系上存在相互垂直关系的三个坐标轴分别指向东向、北向和天向；
S116、利用所述第一位姿数据集合将所述第一点云集合转换到所述点云地图坐标系并完成点云叠加操作，进而得到局部点云地图。
在上述步骤S111中，利用KD-Tree(欧式聚类)算法对每帧第一点云所包含的所有点云数据进行统计分析，计算每个点云数据到所有邻近点云数据之间的平均距离，当计算得到的所有平均距离可以符合高斯分布(其形状可由均值和标准差来共同决定)时，将处于标准范围(即由该均值和该标准差所确定的距离阈值范围)之外的平均距离所关联的点云数据判定为离群点并对其进行滤除；通过这一点云滤波操作，可以避免在执行上述步骤S112所提及的下采样过程中仍然保留无序干扰点而影响最终建图效果。
在上述步骤S112中，所述滤波后的多帧第一点云可以形成一个三维点云空间，其中滤波后的每帧第一点云均携带有若干个点云数据，并且所述三维点云空间是以所述激光雷达为中心基准的，考虑到所述滤波后的多帧第一点云在所述三维点云空间中的分布是极其不均匀的，也存在很多冗余信息，会造成最终建图后的可视化效果不佳，同时也会加重所述车载工控机的运行负担，因此本发明在保证点云信息完整性的基础上，引入分区域下采样策略对所述滤波后的多帧第一点云执行下采样操作，具体表现如下：
首先将所述三维点云空间按照预设数量进行均匀划分以得到多个扇形区域，并且所述多个扇形区域之间的唯一交叉点为所述三维点云空间的中心点；
其次利用预先搭建的数学模型将处于每个扇形区域内的所有点云数据执行下采样操作，以达到删减每个扇形区域内点云数据量的目的；
最后将所述多个扇形区域内剩下的所有点云数据以时间戳相同为基准进行归类划分，以形成若干帧第一点云；
其中，所述数学模型为：
式中，Sk指代第k个扇形区域，pki指代落在第k个扇形区域Sk内的第i个点云数据，其在激光雷达坐标系下的位置信息为(pxki,pyki,pzki)，获取该激光雷达坐标系的坐标原点与点(pxki,pyki)之间所形成的射线，arctan(pxki,pyki)指代该射线与该激光雷达坐标系的X轴正方向之间所形成的夹角，并且arctan(pyi,pxi)∈(-π,π)，s为所述预设数量(即所述多个扇形区域的具体数量)，k为第k个扇形区域Sk的索引值，P指代所述滤波后的多帧第一点云所携带的所有点云数据，∈为属于的数学符号，指代所述三维点云空间，/>为包含于的数学符号。
需要说明的是，由于在每个扇形区域内执行下采样任务在算法逻辑上是完全独立的，可以借助多个GPU(Graphic Processing Unit，图形处理器)设备同时执行，从而达到降低算力要求和加快运行速度的目的；此外，所述若干帧第一点云可以在所述三维点云空间内呈现出均匀分布状态，并且其携带的每个点云数据的特征表达能力相对较强。
在本发明实施例中，上述步骤S114的具体实施过程包括如下步骤：
S114.1、预先创建长度为10的第一预设队列和第二预设队列，并且所述第一预设队列和所述第二预设队列均是一种特殊的线性表，遵循先进先出的原则；
S114.2、按照时间戳从小到大的存储顺序，将所述多帧第四位姿数据逐帧存储到所述第一预设队列中，以及将所述若干帧第一点云逐帧存储到所述第二预设队列中；
S114.3、读取所述第一预设队列中当前排列在最前(即时间戳最小)的单帧第四位姿数据，以及读取所述第二预设队列中当前排列在最前(即时间戳最小)的单帧第一点云；
S114.4、获取所述单帧第四位姿数据所关联的时间戳并记为TGPS1，以及获取所述单帧第一点云所关联的时间戳并记为Tlidar1，再判断|TGPS1-Tlidar1|≤Tset是否成立，其中Tset为技术人员提前拟定好的非零值的预设时间阈值；若成立，则说明所述单帧第四位姿数据与所述单帧第一点云之间满足时间同步要求，此时继续执行步骤S114.5；若不成立，则说明所述单帧第四位姿数据与所述单帧第一点云之间无法满足时间同步要求，此时跳转执行步骤S114.6；
S114.5、将所述单帧第四位姿数据从所述第一预设队列中直接提取出来并另作保存，以及将所述单帧第一点云从所述第二预设队列中直接提取出来并另作保存，再跳转执行步骤S114.7；
S114.6、根据时间戳TGPS1与时间戳Tlidar1之间的大小关系，从所述第一预设队列和所述第二预设队列中择一完成数据更新操作，再继续执行步骤S114.7；其中，这一数据更新操作过程为：当TGPS1<Tlidar1时，从所述第一预设队列中直接剔除掉所述单帧第四位姿数据；或者当TGPS1>Tlidar1时，从所述第二预设队列中直接剔除掉所述单帧第一点云；
S114.7、判断所述第一预设队列和所述第二预设队列中是否至少有一个处于空数据状态；若是，则直接结束当前的时间同步处理操作，再将原先保存到的所有第四位姿数据整合为第一位姿数据集合输出，以及将原先保存到的所有第一点云整合为第一点云集合输出；若否，则返回执行上述步骤S114.3。
在本发明实施例中，上述步骤S116的具体实施过程包括如下步骤：
S116.1、从所述第一点云集合中提取出第i帧第一点云，以及从所述第一位姿数据集合中提取出第i帧第四位姿数据；其中，所述第i帧第一点云与所述第i帧第四位姿数据之间满足时间同步要求，并且由上述步骤S113可知，所述第i帧第四位姿数据实际包含有第i帧第一姿态数据和经坐标转换后的第i帧第一位置数据；
S116.2、利用第i帧第一姿态数据，计算出第i帧第一点云所关联的旋转矩阵为：
式中，R1i为第i帧第一点云所关联的旋转矩阵，α1i为第i帧第一姿态数据所记载的偏航角，β1i为第i帧第一姿态数据所记载的翻滚角，γ1i为第i帧第一姿态数据所记载的俯仰角；
S116.3、利用经坐标转换后的第i帧第一位置数据以及上述步骤S115所提及到的所述经坐标转换后的首帧第一位置数据，计算出第i帧第一点云所关联的平移矩阵为：
式中，T1i为第i帧第一点云关联的平移矩阵，(X1i,Y1i,Z1i)为经坐标转换后的第i帧第一位置数据，(X0,Y0,Z0)为所述经坐标转换后的首帧第一位置数据；
S116.4、利用第i帧第一点云所关联的旋转矩阵以及第i帧第一点云所关联的平移矩阵，将第i帧第一点云所携带的所有点云数据依次转换至所述点云地图坐标系下进行表示；其中，针对任意一个点云数据的转换公式如下：
式中，(x1ij,y1ij,z1ij)为第i帧第一点云所携带的第j个点云数据的位置信息，为第i帧第一点云所携带的第j个点云数据转换至所述点云地图坐标系下进行表示时的位置信息；
S116.5、判断i＜N1是否成立，其中N1为所述第一点云集合中所包含的所有第一点云的帧数；若成立，则将所述点云地图坐标系下的所有第一点云直接进行叠加处理，由此得到特征更加丰富、范围更加广泛的局部点云地图；若不成立，则将i+1赋值给i，再返回执行上述步骤S116.1；
需要说明的是，上述步骤S116.1是从i＝1开始执行的。
在本发明实施例中，在车辆绕着既定实验道路行驶一圈之后，利用所述激光雷达采集获取在整个行驶过程中车辆周围环境的多帧第二点云，利用所述车载惯导设备采集获取在整个行驶过程中车辆的多帧第二姿态数据，以及利用所述车载GPS设备采集获取在整个行驶过程中车辆的多帧第二位置数据，并且所述多帧第二姿态数据对应与所述多帧第二位置数据存在时间同步关系，此时将所述多帧第二姿态数据和所述多帧第二位置数据按照关联时间戳进行捆绑处理以得到多帧第二位姿数据。
在此基础上，上述步骤S120的具体实施过程包括但不仅限于以下步骤：
S121、利用现有的StatisticalOutlierRemoval滤波器对所述多帧第二点云中的每帧第二点云执行滤波操作；
S122、将滤波后的多帧第二点云执行下采样操作，进而从中获取包含点云数据量相对较少的若干帧第二点云；
S123、考虑到所述多帧第二位置数据当前是在大地坐标系下进行表示的，即所述多帧第二位置数据是经纬高格式的，无法直接进行解算，此时将所述多帧第二位置数据转换到UTM坐标系下进行表示，再将所述多帧第二姿态数据和经坐标转换后的多帧第二位置数据按照关联时间戳进行捆绑处理，得到待匹配的多帧第五位姿数据；
S124、将所述若干帧第二点云和所述多帧第五位姿数据在时间维度上执行同步处理操作，得到第二点云集合和第二位姿数据集合；
S125、利用所述第二位姿数据集合将所述第二点云集合继续转换至所述点云地图坐标系以完成点云填充操作与点云叠加操作，进而将所述局部点云地图更新为静态全局点云地图。
在上述步骤S121中，利用KD-Tree算法对每帧第二点云所包含的所有点云数据进行统计分析，计算每个点云数据到所有邻近点云数据之间的平均距离，当计算得到的所有平均距离可以符合高斯分布(其形状可由均值和标准差来共同决定)时，将处于标准范围(即由该均值和该标准差所确定的距离阈值范围)之外的平均距离所关联的点云数据判定为离群点并对其进行滤除；通过这一点云滤波操作，可以避免在执行上述步骤S122所提及的下采样过程中仍然保留无序干扰点而影响最终建图效果。
在上述步骤S122中，所述滤波后的多帧第二点云可以形成一个三维点云空间(以下表述为第一三维点云空间)，其中滤波后的每帧第二点云均携带有若干个点云数据，并且所述第一三维点云空间同样是以所述激光雷达为中心基准的，考虑到所述滤波后的多帧第二点云在所述第一三维点云空间中的分布是极其不均匀的，也存在很多冗余信息，会造成最终建图后的可视化效果不佳，同时也会加重所述车载工控机的运行负担，因此本发明在保证点云信息完整性的基础上，引入分区域下采样策略对所述滤波后的多帧第二点云执行下采样操作，具体表现如下：
首先将所述第一三维点云空间按照第一预设数量进行均匀划分以得到多个第一扇形区域，并且所述多个第一扇形区域之间的唯一交叉点为所述第一三维点云空间的中心点；
其次利用预先搭建的第一数学模型将处于每个第一扇形区域内的所有点云数据执行下采样操作，以达到删减每个第一扇形区域内点云数据量的目的；
最后将所述多个第一扇形区域内剩下的所有点云数据以时间戳相同为基准进行归类划分，以形成若干帧第二点云；
其中，所述第一数学模型为：
式中，Sp指代第p个第一扇形区域，ppq指代落在第p个第一扇形区域Sp内的第q个点云数据，其在激光雷达坐标系下的位置信息为(pxpq,pypq,pzpq)，获取该激光雷达坐标系的坐标原点与点(pxpq,pypq)之间所形成的射线，arctan(pxpq,pypq)指代该射线与该激光雷达坐标系的X轴正方向之间所形成的夹角，并且arctan(pxpq,pypq)∈(-π,π)，s1为所述第一预设数量(即所述多个第一扇形区域的具体数量)，p为第p个第一扇形区域Sp的索引值，P1指代所述滤波后的多帧第二点云所携带的所有点云数据，∈为属于的数学符号，指代所述第一三维点云空间，/>为包含于的数学符号。
在本发明实施例中，上述步骤S124的具体实施过程包括如下步骤：
S124.1、预先创建长度为10的第三预设队列和第四预设队列，并且所述第三预设队列和所述第四预设队列均是一种特殊的线性表，遵循先进先出的原则；
S124.2、按照时间戳从小到大的存储顺序，将所述多帧第五位姿数据逐帧存储到所述第三预设队列中，以及将所述若干帧第二点云逐帧存储到所述第四预设队列中；
S124.3、读取所述第三预设队列中当前排列在最前(即时间戳最小)的单帧第五位姿数据，以及读取所述第四预设队列中当前排列在最前(即时间戳最小)的单帧第二点云；
S124.4、获取所述单帧第五位姿数据所关联的时间戳并记为TGPS2，以及获取所述单帧第二点云所关联的时间戳并记为Tlidar2，再判断|TGPS2-Tlidar2|≤Tset是否成立；若成立，则说明所述单帧第五位姿数据与所述单帧第二点云之间满足时间同步要求，此时继续执行步骤S124.5；若不成立，则说明所述单帧第五位姿数据与所述单帧第二点云之间无法满足时间同步要求，此时跳转执行步骤S124.6；
S124.5、将所述单帧第五位姿数据从所述第三预设队列中直接提取出来并另作保存，以及将所述单帧第二点云从所述第四预设队列中直接提取出来并另作保存，再跳转执行步骤S124.7；
S124.6、根据时间戳TGPS2与时间戳Tlidar2之间的大小关系，从所述第三预设队列和所述第四预设队列中择一完成数据更新操作，再继续执行步骤S124.7；其中，这一数据更新操作过程为：当TGPS2<Tlidar2时，从所述第三预设队列中直接剔除掉所述单帧第五位姿数据；或者当TGPS2>Tlidar2时，从所述第四预设队列中直接剔除掉所述单帧第二点云；
S124.7、判断所述第三预设队列和所述第四预设队列中是否至少有一个处于空数据状态；若是，则直接结束当前的时间同步处理操作，再将原先保存到的所有第五位姿数据整合为第二位姿数据集合输出，以及将原先保存到的所有第二点云整合为第二点云集合输出；若否，则返回执行上述步骤S124.3。
需要说明的是，本发明也可以不用执行上述步骤S124.1，直接在执行完上述步骤S114.7之后将所述第一预设队列和所述第二预设队列均重新调整为空数据状态，并将所述第一预设队列定义为所述第三预设队列，以及将所述第二预设队定义为所述第四预设队列，再开始执行上述步骤S124.2。
在本发明实施例中，上述步骤S125的具体实施过程包括如下步骤：
S125.1、从所述第二点云集合中提取出第i帧第二点云，以及从所述第二位姿数据集合中提取出第i帧第五位姿数据；其中，所述第i帧第二点云与所述第i帧第五位姿数据之间满足时间同步要求，并且由上述步骤S123可知，所述第i帧第五位姿数据实际包含有第i帧第二姿态数据和经坐标转换后的第i帧第二位置数据；
S125.2、利用第i帧第二姿态数据，计算出第i帧第二点云所关联的旋转矩阵为：
式中，R2i为第i帧第二点云所关联的旋转矩阵，α2i为第i帧第二姿态数据所记载的偏航角，β2i为第i帧第二姿态数据所记载的翻滚角，γ2i为第i帧第二姿态数据所记载的俯仰角；
S125.3、利用经坐标转换后的第i帧第二位置数据以及上述步骤S115所提及到的所述经坐标转换后的首帧第一位置数据，计算出第i帧第二点云所关联的平移矩阵为：
式中，T2i为第i帧第二点云所关联的平移矩阵，(X2i,Y2i,Z2i)为经坐标转换后的第i帧第二位置数据；
S125.4、利用第i帧第二点云所关联的旋转矩阵以及第i帧第二点云所关联的平移矩阵，将第i帧第二点云所携带的所有点云数据依次转换至所述点云地图坐标系下进行表示；其中，针对任意一个点云数据的转换公式如下：
式中，(x2ij,y2ij,z2ij)为第i帧第二点云所携带的第j个点云数据的位置信息，为第i帧第二点云所携带的第j个点云数据转换至所述点云地图坐标系下进行表示时的位置信息；
S125.5、判断i＜N2是否成立，其中N2为所述第二点云集合中所包含的所有第二点云的帧数；若成立，则完成对所述点云地图坐标系的点云填充操作，再将所述点云地图坐标系下的所有点云直接进行叠加处理，由此将所述局部点云地图更新为静态全局点云地图；若不成立，则将i+1赋值给i，再返回执行上述步骤S125.1；
需要说明的是，上述步骤S125.1是从i＝1开始执行的。
在本发明实施例中，当车辆目前行驶在任意道路上时，利用所述激光雷达实时采集获取在这一行驶过程中车辆周围环境的当前帧第三点云，利用所述车载惯导设备实时采集获取在这一行驶过程中车辆的当前帧第三姿态数据，以及利用所述车载GPS设备实时采集获取在这一行驶过程中车辆的当前帧第三位置数据，并且所述当前帧第三姿态数据对应与所述当前帧第三位置数据存在时间同步关系，此时将所述当前帧第三姿态数据和所述当前帧第三位置数据按照关联时间戳进行捆绑处理以得到当前帧第三位姿数据。
在此基础上，上述步骤S130的具体实施过程包括但不仅限于以下步骤：
S131、获取所述当前帧第三位姿数据所关联的时间戳并记为TGPS3，以及获取所述当前帧第三点云所关联的时间戳并记为Tlidar3，再判断|TGPS3-Tlidar3|≤Tset是否成立；若成立，则说明所述当前帧第三位姿数据与所述当前帧第三点云之间满足时间同步要求，此时将所述当前帧第三位姿数据定义为单帧第三位姿数据，以及将所述当前帧第三点云定义为单帧第三点云，再跳转执行步骤S133；若不成立，则说明所述当前帧第三位姿数据与所述当前帧第三点云之间无法满足时间同步要求，此时继续执行步骤S132；
S132、根据时间戳TGPS3与时间戳Tlidar3之间的大小关系，从所述当前帧第三位姿数据和所述当前帧第三点云中择一完成数据更新操作，再返回执行上述步骤S131；其中，这一数据更新操作过程为：当TGPS3<Tlidar3时，获取下一帧第三位姿数据并将其作为新的当前帧第三位姿数据；或者当TGPS3>Tlidar3时，获取下一帧第三点云并将其作为新的当前帧第三点云；
S133、将所述单帧第三点云的中心点映射到所述静态全局点云地图所在的点云地图坐标系，此时根据所述中心点和既定半径(本发明优选为10m)在所述静态全局点云地图上构建出一个筛选范围，再将所述静态全局点云地图中落在所述筛选范围内的所有点云数据进行剔除处理；其中，所述筛选范围实际是一个刚好贯穿所述静态全局点云地图的圆柱体，并且所述圆柱体垂直于所述点云地图坐标系上的东向坐标轴与北向坐标轴所在平面；
S134、利用现有的StatisticalOutlierRemoval滤波器对所述单帧第三点云执行滤波操作；
S135、所述单帧第三位姿数据是由单帧第三位置数据和单帧第三姿态数据所组成的，考虑到所述单帧第三位置数据当前是在大地坐标系下进行表示的，即所述单帧第三位置数据是经纬高格式的，无法直接进行解算，此时将所述单帧第三位置数据转换到UTM坐标系下进行表示，再将所述单帧第三姿态数据和经坐标转换后的单帧第三位置数据按照关联时间戳进行捆绑处理，得到单帧第六位姿数据；
S136、利用所述单帧第六位姿数据将滤波后的单帧第三点云继续转换至经过上述步骤S133更新后的静态全局点云地图所在的点云地图坐标系以完成点云填充操作与点云叠加操作，再将最终呈现的静态全局点云地图作为实时全局点云地图输出。
在上述步骤S134中，利用KD-Tree算法对所述单帧第三点云所携带的所有点云数据进行统计分析，计算每个点云数据到所有邻近点云数据之间的平均距离，当计算得到的所有平均距离可以符合高斯分布(其形状可由均值和标准差来共同决定)时，将处于标准范围(即由该均值和该标准差所确定的距离阈值范围)之外的平均距离所关联的点云数据判定为离群点并对其进行滤除。
在本发明实施例中，上述步骤S136的具体实施过程包括如下步骤：
S136.1、利用所述单帧第三姿态数据，计算出滤波后的单帧第三点云所关联的旋转矩阵为：
式中，R3为滤波后的单帧第三点云所关联的旋转矩阵，α3为所述单帧第三姿态数据所记载的偏航角，β3为所述单帧第三姿态数据所记载的翻滚角，γ3为所述单帧第三姿态数据所记载的俯仰角；
S136.2、利用经坐标转换后的单帧第三位置数据以及上述步骤S115所提及到的所述经坐标转换后的首帧第一位置数据，计算出滤波后的单帧第三点云所关联的平移矩阵为：
式中，T3为滤波后的单帧第三点云所关联的平移矩阵，(X3,Y3,Z3)为经坐标转换后的单帧第三位置数据；
S136.3、利用滤波后的单帧第三点云所关联的旋转矩阵以及滤波后的单帧第三点云所关联的平移矩阵，将滤波后的单帧第三点云所携带的所有点云数据依次转换至经过上述步骤S133更新后的静态全局点云地图所在的点云地图坐标系下进行表示，以完成对所述点云地图坐标系的点云填充操作；其中，针对任意一个点云数据的转换公式如下：
式中，(x3j,y3j,z3j)为滤波后的单帧第三点云所携带的第j个点云数据的位置信息，为滤波后的单帧第三点云所携带的第j个点云数据转换至所述点云地图坐标系下进行表示时的位置信息；
S136.4、将经过上述步骤S136.3处理后的当前点云地图坐标系下的所有点云直接进行叠加处理，再将最终呈现的静态全局点云地图作为实时全局点云地图输出；
需要说明的是，上述步骤S136.1是从i＝1开始执行的。
在本发明实施例中，当车辆目前行驶在任意道路上并且还未停车熄火时，上述步骤S130应当是实时运行的，并以上述步骤S120所构建出的所述静态全局点云地图为基准不断对其进行更新，且每一次更新就会作为实时全局点云地图输出；此外，基于所述车载工控机与后台服务器之间的无线连接关系，在每一次执行完上述步骤S130之后，所述车载工控机调用现有的点云可视化与推流工具将当前输出的实时全局点云地图推送至所述后台服务器中以可视化地呈现在网页上，并且所述后台服务器每隔一小时将保存一次当前推送的实时全局点云地图，以供研究人员通过局域网进行远程访问与查看，其中所述点云可视化与推流工具是由ros3djs、roslibjs和rosbridge_server组成的。
在本发明实施例中，利用车辆的实时位姿数据将存在时间同步关系的车辆周围环境的实时点云转换到既定点云地图坐标系下，由此可以实时获取车辆的绝对位置信息，有助于构建出更为可靠的全局点云地图。相较于现有的仅依靠点云进行相对定位的激光SLAM技术和视觉SLAM技术而言，本发明所提供的实时全局点云地图构建方法可以应用在不同行驶场景中，受到环境影响较小，可以有效解决在建图过程中出现的车辆位置丢失这一问题。
请参考图2，图2是本发明实施例提供的一种实时全局点云地图构建系统的组成示意图，所述系统包括：
构建模块210，用于在车辆行驶之前(即在车辆保持静止时)，根据在静止时间内所采集到的车辆的多帧第一位姿数据以及车辆周围环境的多帧第一点云，构建出局部点云地图；
填充模块220，用于在车辆绕着既定实验道路行驶一圈之后，根据在行驶过程所采集到车辆的多帧第二位姿数据以及采集的车辆周围环境的多帧第二点云，对所述局部点云地图执行点云填充操作，进而得到静态全局点云地图；
更新模块230，用于当车辆在任意道路上行驶时，根据实时采集到的车辆的单帧第三位姿数据以及车辆周围环境的单帧第三点云，对所述静态全局点云地图执行更新操作，进而形成实时全局点云地图。
上述方法实施例中的内容均适用于本系统实施例中，本系统实施例所实现的功能与上述方法实施例相同，并且所达到的有益效果与上述方法实施例相同，在此不再赘述。
此外，本发明实施例还提供一种计算机可读存储介质，所述计算机可读存储介质上存储有计算机程序，所述计算机程序被处理器执行时实现上述实施例中的实时全局点云地图构建方法。其中，所述计算机可读存储介质包括但不限于任何类型的盘(包括软盘、硬盘、光盘、CD-ROM、和磁光盘)、ROM(Read-Only Memory，只读存储器)、RAM(Random AcceSSMemory，随即存储器)、EPROM(EraSable Programmable Read-Only Memory，可擦写可编程只读存储器)、EEPROM(Electrically EraSable ProgrammableRead-Only Memory，电可擦可编程只读存储器)、闪存、磁性卡片或光线卡片。也就是说，存储设备包括由设备(例如计算机、手机等)以可读的形式存储或传输信息的任何介质，可以是只读存储器、磁盘或光盘等。
此外，图3是本发明实施例所提供的计算机设备的硬件结构示意图，所述计算机设备包括处理器320、存储器330、输入单元340和显示单元350等器件。本领域技术人员可以理解，图3示出的设备结构器件并不构成对所有设备的限定，可以包括比图示更多或更少的部件，或者组合某些部件。存储器330可用于存储计算机程序310以及各功能模块，处理器320运行存储在存储器330的计算机程序310，从而执行设备的各种功能应用以及数据处理。存储器可以是内存储器或外存储器，或者包括内存储器和外存储器。内存储器可以包括只读存储器(ROM)、可编程ROM(PROM)、电可编程ROM(EPROM)、电可擦写可编程ROM(EEPROM)、快闪存储器或者随机存储器。外存储器可以包括硬盘、软盘、ZIP盘、U盘、磁带等。本发明实施例所公开的存储器330包括但不限于上述这些类型的存储器。本发明实施例所公开的存储器330只作为例子而非作为限定。
输入单元340用于接收信号的输入，以及接收用户输入的关键字。输入单元340可包括触控面板以及其它输入设备。触控面板可收集用户在其上或附近的触摸操作(比如用户利用手指、触笔等任何适合的物体或附件在触控面板上或在触控面板附近的操作)，并根据预先设定的程序驱动相应的连接装置；其它输入设备可以包括但不限于物理键盘、功能键(比如播放控制按键、开关按键等)、轨迹球、鼠标、操作杆等中的一种或多种。显示单元350可用于显示用户输入的信息或提供给用户的信息以及终端设备的各种菜单。显示单元350可采用液晶显示器、有机发光二极管等形式。处理器320是终端设备的控制中心，利用各种接口和线路连接整个设备的各个部分，通过运行或执行存储在存储器320内的软件程序和/或模块，以及调用存储在存储器内的数据，执行各种功能和处理数据。
作为一个实施例，所述计算机设备包括处理器320、存储器330和计算机程序310，其中所述计算机程序310被存储在所述存储器330中并被配置为由所述处理器320所执行，所述计算机程序310被配置用于执行上述实施例中的实时全局点云地图构建方法。
尽管本申请的描述已经相当详尽且特别对几个所述实施例进行了描述，但其并非旨在局限于任何这些细节或实施例或任何特殊实施例，而是应当将其视作是通过参考所附权利要求，考虑到现有技术为这些权利要求提供广义的可能性解释，从而有效地涵盖本申请的预定范围。此外，上文以发明人可预见的实施例对本申请进行描述，其目的是为了提供有用的描述，而那些目前尚未预见的对本申请的非实质性改动仍可代表本申请的等效改动。
Claims (10)
1.一种实时全局点云地图构建方法，其特征在于，所述方法包括：
步骤100、在车辆行驶之前，根据采集的车辆周围环境的多帧第一点云和车辆的多帧第一位姿数据，构建局部点云地图；
步骤200、当车辆在既定实验道路上绕行一圈时，根据采集的车辆周围环境的多帧第二点云和车辆的多帧第二位姿数据，对所述局部点云地图进行点云填充以得到静态全局点云地图；
步骤300、当车辆在任意道路上行驶时，根据实时采集的车辆周围环境的单帧第三点云和车辆的单帧第三位姿数据，对所述静态全局点云地图进行更新以形成实时全局点云地图。
2.根据权利要求1所述的实时全局点云地图构建方法，其特征在于，所述步骤100包括：
步骤110、对所述多帧第一点云进行滤波处理；
步骤120、对滤波后的多帧第一点云进行下采样处理，得到若干帧第一点云；
步骤130、所述多帧第一位姿数据包括多帧第一位置数据和多帧第一姿态数据，将所述多帧第一位置数据由大地坐标系转换到UTM坐标系表示，再结合所述多帧第一姿态数据和所述若干帧第一点云进行时间同步处理，得到第一位姿数据集合和第一点云集合；
步骤140、根据经坐标转换后的首帧第一位置数据构建点云地图坐标系，再利用所述第一位姿数据集合将所述第一点云集合转换到所述点云地图坐标系进行叠加，形成局部点云地图。
3.根据权利要求2所述的实时全局点云地图构建方法，其特征在于，滤波后的每帧第一点云包含若干个点云数据，所述步骤120包括：
将滤波后的多帧第一点云所构成的三维点云空间均匀划分成多个扇形区域，再利用数学模型对落在每一个扇形区域内的所有点云数据进行下采样处理，进而得到若干帧第一点云；所述数学模型为：
其中，Sk为第k个扇形区域，pki为落在第k个扇形区域内的第i个点云数据，其由激光雷达采集且在激光雷达坐标系下的位置信息为(pxki,pyki,pzki)，arctan(pxki,pyki)为X轴正方向与通过点(pxki,pyki)和坐标原点的射线之间的夹角，s为所述多个扇形区域的数量，P为滤波后的多帧第一点云所包含的所有点云数据，为所述三维点云空间。
4.根据权利要求2所述的实时全局点云地图构建方法，其特征在于，在所述步骤130中，结合经坐标转换后的多帧第一位置数据、所述多帧第一姿态数据和所述若干帧第一点云进行时间同步处理，得到第一位姿数据集合和第一点云集合包括：
步骤131、根据经坐标转换后的多帧第一位置数据和所述多帧第一姿态数据，确定待匹配的多帧第四位姿数据；
步骤132、将所述多帧第四位姿数据以时间戳由小到大的顺序存储至第一预设队列，将所述若干帧第一点云以时间戳由小到大的顺序存储至第二预设队列；
步骤133、从所述第一预设队列中读取排列在最前的单帧第四位姿数据，从所述第二预设队列中读取排列在最前的单帧第一点云；
步骤134、判断所述单帧第四位姿数据的时间戳与所述单帧第一点云的时间戳之间的差值绝对值是否小于等于预设时间阈值；若是，则从所述第一预设队列中提取所述单帧第四位姿数据进行保存，从所述第二预设队列中提取所述单帧第一点云进行保存，再执行步骤135；若否，则将时间戳更小的所述单帧第四位姿数据从所述第一预设队列中剔除，或者将时间戳更小的所述单帧第一点云从所述第二预设队列中剔除，再执行步骤135；
步骤135、判断所述第一预设队列和所述第二预设队列中是否至少有一个为空；若是，则将保存的所有第四位姿数据形成第一位姿数据集合输出，将保存的所有第一点云形成第一点云集合输出；若否，则返回步骤133。
5.根据权利要求4所述的实时全局点云地图构建方法，其特征在于，在所述步骤140中，利用所述第一位姿数据集合将所述第一点云集合转换到所述点云地图坐标系进行叠加，形成局部点云地图包括：
从所述第一点云集合中获取单帧第一点云，再从所述第一位姿数据集合中获取与所述单帧第一点云存在时间同步关系的单帧第四位姿数据；
所述单帧第四位姿数据包含经坐标转换后的单帧第一位置数据和单帧第一姿态数据，根据所述单帧第一姿态数据来确定旋转矩阵，根据经坐标转换后的首帧第一位置数据和所述经坐标转换后的单帧第一位置数据来确定平移矩阵；
根据所述旋转矩阵和所述平移矩阵，将所述单帧第一点云中所包含的所有点云数据转换到所述点云地图坐标系表示；
循环执行上述各个步骤，直至将所述第一点云集合中的所有第一点云所包含的所有点云数据转换到所述点云地图坐标系表示之后再完成点云叠加，形成局部点云地图。
6.根据权利要求1所述的实时全局点云地图构建方法，其特征在于，所述步骤200包括：
对所述多帧第二点云进行滤波处理；
对滤波后的多帧第二点云进行下采样处理，得到若干帧第二点云；
所述多帧第二位姿数据包括多帧第二位置数据和多帧第二姿态数据，将所述多帧第二位置数据由大地坐标系转换到UTM坐标系表示，再结合所述多帧第二姿态数据和所述若干帧第二点云进行时间同步处理，得到第二位姿数据集合和第二点云集合；
利用所述第二位姿数据集合将所述第二点云集合转换到所述局部点云地图所在的点云地图坐标系进行点云填充与叠加，得到静态全局点云地图。
7.根据权利要求1所述的实时全局点云地图构建方法，其特征在于，所述单帧第三点云与所述单帧第三位姿数据存在时间同步关系，所述步骤300包括：
将所述单帧第三点云的中心点映射到所述静态全局点云地图所在的点云地图坐标系，再从所述静态全局点云地图中剔除处于所述中心点所在既定半径范围内的所有点云数据；
对所述单帧第三点云进行滤波处理；
所述单帧第三位姿数据包括单帧第三位置数据和单帧第三姿态数据，将所述单帧第三位置数据由大地坐标系转换到UTM坐标系表示，再结合所述单帧第三姿态数据将滤波后的单帧第三点云转换到更新后的静态全局点云地图所在的点云地图坐标系进行点云填充与叠加，再将当前的静态全局点云地图作为实时全局点云地图输出。
8.一种实时全局点云地图构建系统，其特征在于，所述系统包括：
构建模块，用于在车辆行驶之前，根据采集的车辆周围环境的多帧第一点云和车辆的多帧第一位姿数据，构建局部点云地图；
填充模块，用于当车辆在既定实验道路上绕行一圈时，根据采集的车辆周围环境的多帧第二点云和车辆的多帧第二位姿数据，对所述局部点云地图进行点云填充以得到静态全局点云地图；
更新模块，用于当车辆在任意道路上行驶时，根据实时采集的车辆周围环境的单帧第三点云和车辆的单帧第三位姿数据，对所述静态全局点云地图进行更新以形成实时全局点云地图。
9.一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，其特征在于，所述处理器执行所述计算机程序以实现如权利要求1至7任一项所述的实时全局点云地图构建方法。
10.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1至7中任一项所述的实时全局点云地图构建方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310639773.3A CN116539053A (zh) | 2023-05-31 | 2023-05-31 | 一种实时全局点云地图构建方法、系统、设备及存储介质 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310639773.3A CN116539053A (zh) | 2023-05-31 | 2023-05-31 | 一种实时全局点云地图构建方法、系统、设备及存储介质 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN116539053A true CN116539053A (zh) | 2023-08-04 |
Family
ID=87445318
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310639773.3A Pending CN116539053A (zh) | 2023-05-31 | 2023-05-31 | 一种实时全局点云地图构建方法、系统、设备及存储介质 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116539053A (zh) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20220342086A1 (en) * | 2019-09-19 | 2022-10-27 | Nippon Telegraph And Telephone Corporation | Position measurement system, positioning calculation apparatus, position measurement method and program |
| CN117031443A (zh) * | 2023-10-09 | 2023-11-10 | 天津云圣智能科技有限责任公司 | 点云数据构建方法、系统及电子设备 |
-
2023
- 2023-05-31 CN CN202310639773.3A patent/CN116539053A/zh active Pending
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20220342086A1 (en) * | 2019-09-19 | 2022-10-27 | Nippon Telegraph And Telephone Corporation | Position measurement system, positioning calculation apparatus, position measurement method and program |
| CN117031443A (zh) * | 2023-10-09 | 2023-11-10 | 天津云圣智能科技有限责任公司 | 点云数据构建方法、系统及电子设备 |
| CN117031443B (zh) * | 2023-10-09 | 2024-01-19 | 天津云圣智能科技有限责任公司 | 点云数据构建方法、系统及电子设备 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| EP3505869B1 (en) | Method, apparatus, and computer readable storage medium for updating electronic map | |
| CN108763287B (zh) | 大规模可通行区域驾驶地图的构建方法及其无人驾驶应用方法 | |
| CN116539053A (zh) | 一种实时全局点云地图构建方法、系统、设备及存储介质 | |
| Barnsley et al. | Distinguishing urban land-use categories in fine spatial resolution land-cover data using a graph-based, structural pattern recognition system | |
| JP2018534603A (ja) | 高精度地図データの処理方法、装置、記憶媒体及び機器 | |
| CN111179274B (zh) | 地图地面分割方法、装置、计算机设备和存储介质 | |
| CN113593017A (zh) | 露天矿地表三维模型构建方法、装置、设备及存储介质 | |
| CN111080682B (zh) | 点云数据的配准方法及装置 | |
| CN112305559A (zh) | 基于地面定点激光雷达扫描的输电线距离测量方法、装置、系统和电子设备 | |
| CN114485698B (zh) | 一种交叉路口引导线生成方法及系统 | |
| CN112146682B (zh) | 智能汽车的传感器标定方法、装置、电子设备及介质 | |
| CN111540027B (zh) | 一种检测方法、装置、电子设备及存储介质 | |
| CN113514843A (zh) | 多子图激光雷达定位方法、系统以及终端 | |
| CN116977628A (zh) | 一种应用于动态环境的基于多模态语义框架的slam方法及系统 | |
| CN114821363A (zh) | 一种基于语义信息匹配的无人机定位建图方法及系统 | |
| CN110717141A (zh) | 一种车道线优化方法、装置及存储介质 | |
| CN114187357A (zh) | 一种高精地图的生产方法、装置、电子设备及存储介质 | |
| CN111426316B (zh) | 机器人定位方法、装置、机器人及可读存储介质 | |
| CN116339351B (zh) | 一种基于基因调控网络的智能体集群区域覆盖方法及系统 | |
| CN112733971A (zh) | 扫描设备的位姿确定方法、装置、设备及存储介质 | |
| CN115273068B (zh) | 一种激光点云动态障碍物剔除方法、装置及电子设备 | |
| CN116642490A (zh) | 基于混合地图的视觉定位导航方法、机器人及存储介质 | |
| CN114241083A (zh) | 一种车道线生成方法、装置、电子设备及存储介质 | |
| CN113960614A (zh) | 一种基于帧-地图匹配的高程图构建方法 | |
| Zhou et al. | A Location Algorithm for Autonomous Vehicles in Large-scale Scenarios Using a Multilayer LIDAR |
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 |