CN110031825B - 激光定位初始化方法 - Google Patents
激光定位初始化方法 Download PDFInfo
- Publication number
- CN110031825B CN110031825B CN201910306885.0A CN201910306885A CN110031825B CN 110031825 B CN110031825 B CN 110031825B CN 201910306885 A CN201910306885 A CN 201910306885A CN 110031825 B CN110031825 B CN 110031825B
- Authority
- CN
- China
- Prior art keywords
- point cloud
- frame
- current
- feature
- principal component
- 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
Links
- 238000011423 initialization method Methods 0.000 title abstract description 11
- 238000005070 sampling Methods 0.000 claims abstract description 74
- 238000000513 principal component analysis Methods 0.000 claims abstract description 43
- 239000011159 matrix material Substances 0.000 claims abstract description 41
- 238000002310 reflectometry Methods 0.000 claims abstract description 40
- 238000004364 calculation method Methods 0.000 claims abstract description 18
- 238000000034 method Methods 0.000 claims description 34
- 230000009466 transformation Effects 0.000 claims description 14
- 238000004422 calculation algorithm Methods 0.000 claims description 12
- 241001417527 Pempheridae Species 0.000 description 7
- 238000004590 computer program Methods 0.000 description 6
- 238000010408 sweeping Methods 0.000 description 5
- 238000004140 cleaning Methods 0.000 description 4
- 238000005516 engineering process Methods 0.000 description 4
- 230000007613 environmental effect Effects 0.000 description 4
- 238000009825 accumulation Methods 0.000 description 3
- 238000011156 evaluation Methods 0.000 description 3
- 230000005484 gravity Effects 0.000 description 3
- 230000008569 process Effects 0.000 description 3
- 230000006872 improvement Effects 0.000 description 2
- 238000012545 processing Methods 0.000 description 2
- 230000003252 repetitive effect Effects 0.000 description 2
- 238000013519 translation Methods 0.000 description 2
- OKTJSMMVPCPJKN-UHFFFAOYSA-N Carbon Chemical compound [C] OKTJSMMVPCPJKN-UHFFFAOYSA-N 0.000 description 1
- 230000032683 aging Effects 0.000 description 1
- 238000013473 artificial intelligence Methods 0.000 description 1
- 229910052799 carbon Inorganic materials 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000000605 extraction Methods 0.000 description 1
- 230000006870 function Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
Images
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/02—Systems using the reflection of electromagnetic waves other than radio waves
- G01S17/06—Systems determining position data of a target
-
- 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
- G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
- G01S7/48—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00
- G01S7/497—Means for monitoring or calibrating
Landscapes
- Physics & Mathematics (AREA)
- Engineering & Computer Science (AREA)
- Electromagnetism (AREA)
- Computer Networks & Wireless Communication (AREA)
- General Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Optical Radar Systems And Details Thereof (AREA)
- Length Measuring Devices By Optical Means (AREA)
Abstract
本发明提供了一种激光定位初始化方法，包括：获取多个点云序列的拼接帧点云信息并采样得到多个采样点云；确定点云分布中心并据此进行主成分分析，生成主成分分析特征矩阵并据此构建特征坐标系；根据点云分布中心和特征坐标系，对采样点云进行划分，生成以三维直方图存储相应采样点云的反射率和/或高度的多个象限；根据拼接帧点云的局部位置坐标命名特征文件；获取距离在预设第二距离内的拼接帧点云对应的第一数量个特征文件，进行第一匹配计算，确定第二数量个特征文件；计算当前帧点云的第二数量个实时位置；对第二数量个特征文件进行平移，进行第二匹配计算；当大于预设阈值时，确定完成激光定位的初始化。由此，拓宽激光初始化的可行环境。
Description
技术领域
本发明涉及数据处理技术领域，尤其涉及一种激光定位初始化方法。
背景技术
随着近年来科技水平的不断提升，人工智能飞速发展，其广泛运用到各个领域之中。其中，由于自动驾驶车辆能够高效利用交通资源，缓解交通拥堵、减少碳排放，自动驾驶技术越来越成为人们所关注的焦点，但乘用车自动驾驶距真正商业化还有一定的距离，而限定环境内的小型低速环卫清扫车为自动驾驶技术的落地提供了具体的应用场景。另一方面，由于人口老龄化的加剧，国内劳动力的成本逐年呈上升趋势，且繁重的重复性体力劳动增加了人们的工作负担，例如，对于公园、校园、大型商场、工业园区等场景的环境卫生清扫作业，环卫工人需要长时间进行重复性的体力劳动，作业繁重，故由智能化的无人驾驶自动清扫代替繁重的人工清扫势不可挡。
小型低速环卫清扫车能够实现高精度定位关键在于激光雷达与高精度差分全球导航卫星系统(Global Navigation Satellite System，GNSS)技术，激光雷达定位技术关键在于初始化位置估计与实时位置估计。激光实时定位主要基于初始估计位置进行高精度推算与特征匹配，因此激光初始位置估计是激光实现高精度定位的必要条件。实际应用中，激光雷达特征的高精度匹配需要的初始位置估计要求在规定误差范围内，误差较大的激光初始化将导致激光雷达特征匹配困难甚至直接失败，造成清扫车定位失败。考虑到自动驾驶清扫车对投放场景的适应能力，激光雷达定位初始化要求能适用于多场景。
目前应用较多的激光定位初始化方法有两种：1.基于GNSS位置的初始化方案；2.基于固定位置的初始化方案。
基于GNSS位置的初始化方案依赖无人车配置差分GNSS，GNSS定位和定向状态较好时可给较好的初始位置估计，位置误差20cm内，角度误差1度内。基于固定点位置的初始化基于激光地图的相对位置，受环境干扰较小，在无GNSS环境中同样使用。
基于差分GNSS初始化方案对环境要求较高，受多径效应、卫星信号强度若等影响，在高楼之间、近楼宇处甚至树木下方，GNSS定位均不能保证得到固定解，GNSS位置精度下降，给定初始化位置偏差可能会达到2米甚至10米。受激光干扰等影响，GNSS差分可能收敛不了，给定航向角偏差能超过10度。该方案受环境制约较严重，在清扫车投放中无法避免卫星强度弱等场景。
基于固定点初始化则要求车辆停放在固定位置固定朝向，当车辆远离该固定点时，将无法实现初始化。该方法受初始化位置及停车精度影响，实际应用时无法保证初始化位置。
两种方式收到环境及位置影响，导致激光初始化只能在特定区域或特定环境，实际清扫时要求的全场景初始化要求无法满足。
发明内容
本发明实施例的目的是提供一种激光定位初始化方法，以解决现有技术中的基于GNSS初始化时对环境要求高或者基于固定点初始化时受初始化位置及停车精度影响的问题。
为解决上述问题，第一方面，本发明提供了一种激光定位初始化方法，所述方法包括：
获取多个点云序列的拼接帧点云信息；所述多个点云序列按照预设第一距离依次排列；每个所述点云序列包括获取时间依次递增的第一帧点云、第二帧点云和第三帧点云，所述第一帧点云、第二帧点云和所述第三帧点云拼接得到拼接帧点云；所述拼接帧点云信息包括拼接帧点云的局部位置坐标、拼接帧点云中各点云的位置、反射率和高度；
对所述拼接帧点云进行采样，得到多个采样点云；
根据所述采样点云中各点云的位置，确定点云分布中心；
根据所述点云分布中心，对所述采样点云进行主成分分析，生成主成分分析特征矩阵；
根据所述主成分分析特征矩阵，构建特征坐标系；
根据所述点云分布中心和所述特征坐标系，对所述采样点云进行划分，生成多个象限，在每个所述象限内以三维直方图存储相应采样点云的反射率和/或高度；
根据所述拼接帧点云的局部位置坐标，为包括多个三维直方图的所述采样点云命名，生成所述采样点云的特征文件；多个所述拼接帧的采样点云的特征文件构成特征文件集；
实时获取当前帧点云信息；所述当前帧点云信息包括当前帧点云的位置坐标、当前帧点云中各点云的位置、反射率和高度；
对所述当前帧点云进行采样，得到多个当前采样点云；
根据所述当前采样点云中各点云的位置，确定当前点云分布中心；
根据所述当前点云分布中心，对所述当前采样点云进行主成分分析，生成当前主成分分析特征矩阵；
根据所述当前主成分分析特征矩阵及矩阵的旋转特性，构建四个当前特征坐标系；
根据所述当前点云分布中心和所述四个当前特征坐标系，对所述当前采样点云进行划分，在每个当前特征坐标系下，生成多个当前象限，在每个所述当前象限内以实时三维直方图存储相应当前采样点云的反射率和/或高度；
根据预设的搜索中心与多个所述拼接帧点云的局部位置坐标的距离，获取所述距离在预设第二距离内的第一数量个拼接帧点云对应的第一数量个特征文件；
将所述实时三维直方图与所述第一数量个特征文件中的三维直方图进行第一匹配计算，得到第一匹配结果；
根据第一匹配结果中的分值，确定第二数量个特征文件；所述第二数量小于所述第一数量；
根据所述第二数量个特征文件的局部位置坐标和所述主成分分析特征矩阵以及所述当前主成分分析特征矩阵，计算当前帧点云的第二数量个实时位置；
在多个方位上，对所述第二数量个特征文件进行平移，确定第二数量个特征文件中每个特征文件在每个方位上平移后的位置；
将所述第二数量个实时位置与每个特征文件在每个方位上平移后的位置进行第二匹配计算，得到第二匹配结果；
当所述第二匹配结果中的分值大于预设阈值时，确定完成激光定位的初始化。
在一种可能的实现方式中，所述第一帧点云、第二帧点云和所述第三帧点云拼接得到拼接帧点云，具体包括：
计算第一帧点云相对于第二帧点云的第一相对位姿变换；
计算第三帧点云相对于第二帧点云的第二相对位姿变换；
根据所述第一相对位姿变换、所述第二相对位姿变换和所述第二帧点云的位姿，叠加得到拼接帧点云。
在一种可能的实现方式中，所述根据所述采样点云中各点云的位置，确定点云分布中心，具体包括：
获取所述采样点云中点云的个数；
将所述采样点云中各点云的x坐标、y坐标和z坐标分别相加，得到x坐标相加结果、y坐标相加结果和z坐标相加结果；
将所述x坐标相加结果、y坐标相加结果和z坐标相加结果分别除以点云的个数，得到点云分布中心。
在一种可能的实现方式中，根据所述点云分布中心和所述特征坐标系，对所述采样点云进行划分，生成多个象限，在每个所述象限内以三维直方图存储相应采样点云的反射率和/或高度，具体包括：
以点云分布中心为球心，分别16米和80米为半径做内球与外球；
根据特征坐标系，对内球和外球除去内球的部分进行划分，得到16个象限；其中，所述内球分为8个象限，将外球除去内球的部分分为8个象限；
当在每个所述象限内以三维直方图存储相应采样点云的反射率时，在每个象限内，以反射率作为三维直方图的横轴的0-255，对多个采样点云进行划分，得到16个三维直方图；或者，
当在每个所述象限内以三维直方图存储相应采样点云的高度时，在每个象限内，以高度作为所述三维直方图的横坐标的0-80，对多个采样点云进行划分，得到16个三维直方图；或者，
当在每个所述象限内以三维直方图存储相应采样点云的反射率和高度时，在每个象限内，以反射率作为三维直方图的横轴的0-255，以高度作为所述三维直方图的横坐标的256-335，对多个采样点云进行划分，得到16个三维直方图。
在一种可能的实现方式中，所述将所述实时三维直方图与所述第一数量个特征文件中的三维直方图进行匹配，得到第一匹配结果，具体包括：
计算所述第一数量个特征文件中每个特征文件中的全部三维直方图的反射率的均值和/或高度的均值；
计算所述实时三维直方图的反射率与所述反射率的均值的第一数值，和/或所述实时三维直方图的高度与所述高度的均值的第二数值；
根据所述第一数值和/或所述第二数值，计算所述实时三维直方图与第一数量个特征文件中的每个特征文件的第一匹配结果。
在一种可能的实现方式中，利用卡方分布或者欧氏距离计算第一数值和/或第二数值。
在一种可能的实现方式中，所述根据所述第二数量个特征文件的局部位置坐标和所述主成分分析特征矩阵以及所述当前主成分分析特征矩阵，计算当前帧点云的第二数量个实时位置，具体包括：
将所述第二数量个特征文件中每个特征文件的局部位置坐标和所述主成分分析特征矩阵、所述当前主成分分析特征矩阵的倒数相乘，得到与该特征文件对应的当前帧点云的一个实时位置。
在一种可能的实现方式中，所述将所述第二数量个实时位置与每个特征文件在每个方位上平移后的位置进行第二匹配计算，得到第二匹配结果，具体包括：
利用NDT算法、ICP算法或者混合高斯模型算法进行第二匹配计算。
在一种可能的实现方式中，所述第一距离为2米，所述第二距离为25米。
在一种可能的实现方式中，所述第一数量为10，所述第二数量为10。第二方面，本发明提供了一种设备，包括存储器和处理器，所述存储器用于存储程序，所述处理器用于执行第一方面任一所述的方法。
第二方面，本发明提供了一种一种设备，包括存储器和处理器，存储器用于存储程序，存储器可通过总线与处理器连接，处理器用于执行软件程序，该软件程序被执行时，能够实现本发明实施例一提供的方法。
第三方面，本发明提供了一种包含指令的计算机程序产品，当所述计算机程序产品在计算机上运行时，使得所述计算机执行如第一方面任一所述的方法。
第四方面，本发明提供了一种计算机可读存储介质，所述计算机可读存储介质上存储有计算机程序，所述计算机程序被处理器执行时实现如第一方面任一所述的方法。
通过应用本发明实施例提供的激光定位初始化方法，通过较大范围内直方图相似性评估计算降低了对GNSS精度的要求和对初始位置的依赖，拓宽激光初始化的可行环境。当车辆在GNSS状态不佳环境中工作时，也能够激光初始化成功，保证车辆正常定位运行。当车辆行驶中定位失败后可自动重定位，而不需要人工介入，将车手动遥控至固定初始点。本申请原理简单易实现，逻辑清晰，各场地适应性良好，参数不需随场地调节，解决了小型低速车辆高精度定位中要求实现全覆盖工作场景激光初始化需求。
附图说明
图1为本发明实施例一提供的激光定位初始化方法流程示意图。
具体实施方式
下面结合附图和实施例对本申请作进一步的详细说明。可以理解的是，此处所描述的具体实施例仅仅用于解释相关发明，而非对该发明的限定。另外还需要说明的是，为便于描述，附图中仅示出了与有关发明相关的部分。
需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。下面将参考附图并结合实施例来详细说明本申请。
图1为本发明实施例一提供的激光定位初始化方法流程示意图。该方法应用在车辆中，尤其可以应用在自动驾驶车辆中，比如低速清扫车或者限定配送区域的物流车，下面以将该方法应用在低速清扫车为例，对本申请进行说明。如图1所示，该方法包括以下步骤：
步骤101，获取多个点云序列的拼接帧点云信息。
其中，该方法的执行主体为车辆控制单元，车辆控制单元可以从激光传感器，比如激光雷达获取到多个点云序列。多个点云序列按照预设第一距离依次排列；示例而非限定，第一距离可以是2米，可以将2米内的3帧点云，作为一个点云序列。
每个点云序列包括获取时间依次递增的第一帧点云、第二帧点云和第三帧点云，第一帧点云、第二帧点云和第三帧点云拼接得到拼接帧点云；拼接帧点云信息包括拼接帧点云的局部位置坐标、拼接帧点云中各点云的位置、反射率和高度。
点云点云的位置包括点云的x坐标、y坐标和z坐标，且该坐标是基于激光坐标系的，激光坐标系的原点是激光传感器的中心点，X轴可以是车头朝前方向，Y轴是朝左方向，Z轴是朝上方向。后续，由于点云的产生者-激光传感器，相对于车辆位置的位置是已知的，可以根据两者的相对位置，将激光坐标系转换为车辆坐标系。
具体的，低速清扫车在进行清扫之前，需要采集清扫区域的地图，以便于后续进行路径规划。在地图采集阶段，车辆可以通过以下方法在离线状态下进行地图采集，在地图采集的过程中，车辆可以通过以下方法得到拼接帧点云：
首先，计算第一帧点云相对于第二帧点云的第一相对位姿变换；然后，计算第三帧点云相对于第二帧点云的第二相对位姿变换；最后，根据第一相对位姿变换、第二相对位姿变换和第二帧点云的位姿，叠加得到拼接帧点云。
可以利用相邻两帧点云间匹配求解相对位姿变换。比如，L1和L2是相邻两帧点云，在匹配后，求出的相对位姿变换是基于L1的激光坐标系的，而设定L1的激光坐标系是全局世界坐标系，所以就可以得到L2在全局坐标系下的位姿。
步骤102，对拼接帧点云进行采样，得到多个采样点云。
其中，可以对拼接帧点云进行均匀采样，在本申请中，可以将拼接帧点云每隔0.2米创建一个三维体素栅格(可把体素栅格想象为微小的空间三维立方体的集合)，然后在每个体素(即，三维立方体)内，用体素中所有点的重心来近似显示体素中其他点，这样该体素内所有点就用一个重心点最终表示，对于所有体素处理后得到过滤后的点云，该过滤后的点云，就是采样点云。由此，减少了后续进行主成分分析时，特征提取的计算量，保证了提取的特征矩阵的均匀性。
步骤103，根据采样点云中各点云的位置，确定点云分布中心。
继续接上例，每个采样点云的位置，即为体素的重心位置。
具体的，步骤102包括以下内容：
首先，获取采样点云中点云的个数；然后，将采样点云中各点云的x坐标、y坐标和z坐标分别相加，得到x坐标相加结果、y坐标相加结果和z坐标相加结果；最后，将x坐标相加结果、y坐标相加结果和z坐标相加结果分别除以点云的个数，得到点云分布中心。
步骤104，根据点云分布中心，对采样点云进行主成分分析，生成主成分分析特征矩阵。
步骤105，根据主成分分析特征矩阵，构建特征坐标系。
具体的，为了计算输入点云的主成分方向，输入点云在此处指的是采样点云，可以基于点云分布中心，对所有采样点云进行主成分分析，构建主成分分析特征矩阵，根据该主成分分析特征矩阵，构建该点云主成分坐标系三轴坐标系，并定义主成分坐标系三轴坐标系为特征坐标系。
其中，该特征坐标系与具体的输入点云有关，各处均不一致。
步骤106，根据点云分布中心和特征坐标系，对采样点云进行划分，生成多个象限，在每个象限内以三维直方图存储相应采样点云的反射率和/或高度。
其中，步骤106包括：
首先，以点云分布中心为球心，分别16米和80米为半径做内球与外球；
然后，根据特征坐标系，对内球和外球除去内球的部分进行划分，得到16个象限；其中，内球分为8个象限，将外球除去内球的部分分为8个象限；
当在每个象限内以三维直方图存储相应采样点云的反射率时，在每个象限内，以反射率作为三维直方图的横轴的0-255，对多个采样点云进行划分，得到16个三维直方图；或者，
当在每个象限内以三维直方图存储相应采样点云的高度时，在每个象限内，以高度作为三维直方图的横坐标的0-80，对多个采样点云进行划分，得到16个三维直方图；或者，
当在每个象限内以三维直方图存储相应采样点云的反射率和高度时，在每个象限内，以反射率作为三维直方图的横轴的0-255，以高度作为三维直方图的横坐标的256-335，对多个采样点云进行划分，得到16个三维直方图。
具体的，上述包括三种情况，第一种以反射率作为三维直方图的横轴、第二种以高度作为三维直方图的横轴、第三种以反射率和高度作为三维直方图的横轴。示例而非限定，高度根据采样点云的可能高度进行设定，本申请中，点云的可能高度为-10-70米。
步骤107，根据拼接帧点云的局部位置坐标，为包括多个三维直方图的采样点云命名，生成采样点云的特征文件；多个拼接帧的采样点云的特征文件构成特征文件集。
具体的，在对拼接帧的采样点云划分完象限，得到多个三维直方图后，可以以拼接帧点云的局部位置坐标，为包括多个三维直方图的拼接帧点云命名，并存储。该处的局部位置坐标，即在局部地图中的位置坐标，局部地图可以是针对激光坐标系的地图，该地图可以转换为基于车辆坐标系的地图。比如，对于第一个拼接帧点云，其局部位置坐标为(xx1，yy1)，其对应的特征文件1可以命名为“xx1_yy1.bin”，“xx1”代表局部地图中x向坐标，“yy1”代表局部地图中y向坐标。对于第二个拼接帧点云，其局部位置坐标为(xx2，yy2)，其对应的特征文件2可以命名为“xx2_yy2.bin”，“xx2”代表局部地图中x向坐标，“yy2”代表局部地图中y向坐标。
可以理解的是，一个点云序列针对的是2米范围内的3帧点云，则在一定区域内，可以将该区域按照2米的间隔进行划分，得到该区域对应的多个特征文件，该些特征文件构成特征文件集。
步骤108，实时获取当前帧点云信息；当前帧点云信息包括当前帧点云的位置坐标、当前帧点云中各点云的位置、反射率和高度。
具体的，当清扫车在进行清扫的过程中，可以通过激光雷达，实时的获取到当前帧点云。
步骤109，对当前帧点云进行采样，得到多个当前采样点云。
其中，得到当前采样点云的方法同步骤102相同，此处不再赘述。
步骤110，根据当前采样点云中各点云的位置，确定当前点云分布中心。
其中，确定当前点云分布中心的方法同步骤103相同，此处不再赘述。
步骤111，根据当前点云分布中心，对当前采样点云进行主成分分析，生成当前主成分分析特征矩阵。
其中，该步骤与步骤104相同，此处不再赘述。
步骤112，根据当前主成分分析特征矩阵及矩阵的旋转特性，构建四个当前特征坐标系。
其中，示例而非限定，矩阵的旋转特征，指的是X轴、Y轴共有四种可能性，在确定X轴和Y方向后，Z轴方向由X轴和Y轴方向的矩阵叉乘获取。
根据主成分分析特征矩阵及四种可能的坐标系，得到四个当前特征坐标系。
步骤113，根据当前点云分布中心和四个当前特征坐标系，对当前采样点云进行划分，在每个当前特征坐标系下，生成多个当前象限，在每个当前象限内以实时三维直方图存储相应当前采样点云的反射率和/或高度。
具体的，根据步骤106的描述，可以确定，在每个当前特征坐标系下，对应16个三维直方图，四个当前坐标系，则对应64个三维直方图。
步骤114，根据预设的搜索中心与多个拼接帧点云的局部位置坐标的距离，获取距离在预设第二距离内的第一数量个拼接帧点云对应的第一数量个特征文件。
具体的，在GNSS可用时，该搜索中心可以是GNSS测量到的位置坐标(该位置坐标存在着精度不高的缺陷)；在GNSS不可用时，可以根据实时记忆点和固定点，确定当前帧点云与离线状态时，多个拼接帧点云的局部位置坐标的距离，并获取距离在25米内的特征文件，此时，获取的距离在25米内的特征文件的数量可以是26个。其中，实时记忆点可以是车辆自动驾驶过程中实时记录到存储工具中实时位置，即上一次GNSS定位正常时的位置。固定点是预先设定的几个点。
步骤115，将实时三维直方图与第一数量个特征文件中的三维直方图进行第一匹配计算，得到第一匹配结果。
具体的，步骤115包括：
首先，计算第一数量个特征文件中每个特征文件中的全部三维直方图的反射率的均值和/或高度的均值。
然后，计算实时三维直方图的反射率与反射率的均值的第一数值，和/或实时三维直方图的高度与高度的均值的第二数值。
最后，根据第一数值和/或第二数值，计算实时三维直方图与第一数量个特征文件中的每个特征文件的第一匹配结果。
示例而非限定，以第一数量为26为例，每一个特征文件中包括16个三维直方图，当这16个三维直方图中包括反射率和高度时，计算这16个三维直方图中反射率的均值，并计算16个高度的均值。为了计算直方图的相似性，可以将实时三维直方图中的反射率和反射率均值进行卡方分布累加，得到第一数值，将实时三维直方图中的高度和高度均值进行卡方分布累加，得到第二数值，将第一数值和第二数值进行加权累加，得到评估分数。共计可以得到26*64个分值。
其中，也可以通过欧式距离，计算第一数值和/或第二数值。
步骤116，根据第一匹配结果中的分值，确定第二数量个特征文件；第二数量小于第一数量。
具体的，第二数量可以是10，可以对分值按照从低到高的顺序排序，取分值处于前10的特征文件。
步骤117，根据第二数量个特征文件的局部位置坐标和主成分分析特征矩阵以及当前主成分分析特征矩阵，计算当前帧点云的第二数量个实时位置。
具体的，将第二数量个特征文件中每个特征文件的局部位置坐标和主成分分析特征矩阵、当前主成分分析特征矩阵的倒数相乘，得到与该特征文件对应的当前帧点云的一个实时位置。如当前主成分分析特征矩阵为Ts，第二数量个特征文件中某个特征文件的主成分分析特征矩阵为Tt，第二数量个特征文件中某个特征文件的局部位置坐标为Tf，则当前帧点云的实时位置为Tr＝Tf*Tt*Ts-1。
其中，当第二数量为10时，对应的当前帧点云的实时位置为10个。
步骤118，在多个方位上，对第二数量个特征文件进行平移，确定第二数量个特征文件中每个特征文件在每个方位上平移后的位置。
具体的，为进一步提高平移不变性，可以根据田字型，对第二数量个特征文件中每个特征文件对应的局部位置坐标进行平移，可以将每个特征文件的局部位置坐标作为田字型的中心，上、下、左、右各移动3米，移动到田字形的4个点，然后从4个点，向左或右移动3米至田字型的另外4个顶点，连同特征文件的局部位置坐标，每个特征文件可以得到9个可能的位置。
步骤119，将第二数量个实时位置与每个特征文件在每个方位上平移后的位置进行第二匹配计算，得到第二匹配结果。
具体的，在步骤117中，实时位置的数量为10个，第二数量个特征文件平移后的位置为90个。
其中，可以利用正态分布变换(Normal Distributions Transform，NDT)算法、迭代最近点(Iterative Closest Point，ICP)算法或者混合高斯模型算法，将10个实时位置中每个实时位置对应的当前采样点云与90个可能位置中每个可能位置的采样点云进行第二匹配计算。
步骤120，当第二匹配结果中的分值大于预设阈值时，确定完成激光定位的初始化。
具体的，在利用NDT算法进行第二匹配计算时，每一轮NDT精匹配间时间为0.7秒，当某轮NDT匹配完成后，匹配分值大于当前位置设定阈值时，则认为激光定位初始化成功，并开始执行实时激光定位。该设定阈值，可以通过地图文件的设计而读取。
通过应用本发明实施例提供的激光定位初始化方法，通过较大范围内直方图相似性评估计算降低了对GNSS精度的要求和对初始位置的依赖，拓宽激光初始化的可行环境。当车辆在GNSS状态不佳环境中工作时，也能够激光初始化成功，保证车辆正常定位运行。当车辆行驶中定位失败后可自动重定位，而不需要人工介入，将车手动遥控至固定初始点。本申请原理简单易实现，逻辑清晰，各场地适应性良好，参数不需随场地调节，解决了小型低速车辆高精度定位中要求实现全覆盖工作场景激光初始化需求。
本发明实施例二提供了一种设备，包括存储器和处理器，存储器用于存储程序，存储器可通过总线与处理器连接。存储器可以是非易失存储器，例如硬盘驱动器和闪存，存储器中存储有软件程序和设备驱动程序。软件程序能够执行本发明实施例提供的上述方法的各种功能；设备驱动程序可以是网络和接口驱动程序。处理器用于执行软件程序，该软件程序被执行时，能够实现本发明实施例提供的方法。
本发明实施例三提供了一种包含指令的计算机程序产品，当计算机程序产品在计算机上运行时，使得计算机执行本发明实施例一提供的方法。
本发明实施例四提供了一种计算机可读存储介质，计算机可读存储介质上存储有计算机程序，计算机程序被处理器执行时实现本发明实施例一提供的方法。
专业人员应该还可以进一步意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、计算机软件或者二者的结合来实现，为了清楚地说明硬件和软件的可互换性，在上述说明中已经按照功能一般性地描述了各示例的组成及步骤。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本发明的范围。
结合本文中所公开的实施例描述的方法或算法的步骤可以用硬件、处理器执行的软件模块，或者二者的结合来实施。软件模块可以置于随机存储器(RAM)、内存、只读存储器(ROM)、电可编程ROM、电可擦除可编程ROM、寄存器、硬盘、可移动磁盘、CD-ROM、或技术领域内所公知的任意其它形式的存储介质中。
以上的具体实施方式，对本发明的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上仅为本发明的具体实施方式而已，并不用于限定本发明的保护范围，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。
Claims (9)
1.一种激光定位初始化方法，其特征在于，所述方法包括：
获取多个点云序列的拼接帧点云信息；所述多个点云序列按照预设第一距离依次排列；每个所述点云序列包括获取时间依次递增的第一帧点云、第二帧点云和第三帧点云，所述第一帧点云、第二帧点云和所述第三帧点云拼接得到拼接帧点云；所述拼接帧点云信息包括拼接帧点云的局部位置坐标、拼接帧点云中各点云的位置、反射率和高度；
对所述拼接帧点云进行采样，得到多个采样点云；
根据所述采样点云中各点云的位置，确定点云分布中心；
根据所述点云分布中心，对所述采样点云进行主成分分析，生成主成分分析特征矩阵；
根据所述主成分分析特征矩阵，构建特征坐标系；
根据所述点云分布中心和所述特征坐标系，对所述采样点云进行划分，生成多个象限，在每个所述象限内以三维直方图存储相应采样点云的反射率和/或高度；
根据所述拼接帧点云的局部位置坐标，为包括多个三维直方图的所述采样点云命名，生成所述采样点云的特征文件；多个所述拼接帧的采样点云的特征文件构成特征文件集；
实时获取当前帧点云信息；所述当前帧点云信息包括当前帧点云的位置坐标、当前帧点云中各点云的位置、反射率和高度；
对所述当前帧点云进行采样，得到多个当前采样点云；
根据所述当前采样点云中各点云的位置，确定当前点云分布中心；
根据所述当前点云分布中心，对所述当前采样点云进行主成分分析，生成当前主成分分析特征矩阵；
根据所述当前主成分分析特征矩阵及矩阵的旋转特性，构建四个当前特征坐标系；
根据所述当前点云分布中心和所述四个当前特征坐标系，对所述当前采样点云进行划分，在每个当前特征坐标系下，生成多个当前象限，在每个所述当前象限内以实时三维直方图存储相应当前采样点云的反射率和/或高度；
根据预设的搜索中心与多个所述拼接帧点云的局部位置坐标的距离，获取所述距离在预设第二距离内的第一数量个拼接帧点云对应的第一数量个特征文件；
将所述实时三维直方图与所述第一数量个特征文件中的三维直方图进行第一匹配计算，得到第一匹配结果；
根据第一匹配结果中的分值，确定第二数量个特征文件；所述第二数量小于所述第一数量；
根据所述第二数量个特征文件的局部位置坐标和所述主成分分析特征矩阵以及所述当前主成分分析特征矩阵，计算当前帧点云的第二数量个实时位置；
在多个方位上，对所述第二数量个特征文件进行平移，确定第二数量个特征文件中每个特征文件在每个方位上平移后的位置；
将所述第二数量个实时位置与每个特征文件在每个方位上平移后的位置进行第二匹配计算，得到第二匹配结果；
当所述第二匹配结果中的分值大于预设阈值时，确定完成激光定位的初始化；
其中，所述根据所述第二数量个特征文件的局部位置坐标和所述主成分分析特征矩阵以及所述当前主成分分析特征矩阵，计算当前帧点云的第二数量个实时位置，具体包括：
将所述第二数量个特征文件中每个特征文件的局部位置坐标和所述主成分分析特征矩阵、所述当前主成分分析特征矩阵的倒数相乘，得到与该特征文件对应的当前帧点云的一个实时位置。
2.根据权利要求1所述的方法，其特征在于，所述第一帧点云、第二帧点云和所述第三帧点云拼接得到拼接帧点云，具体包括：
计算第一帧点云相对于第二帧点云的第一相对位姿变换；
计算第三帧点云相对于第二帧点云的第二相对位姿变换；
根据所述第一相对位姿变换、所述第二相对位姿变换和所述第二帧点云的位姿，叠加得到拼接帧点云。
3.根据权利要求1所述的方法，其特征在于，所述根据所述采样点云中各点云的位置，确定点云分布中心，具体包括：
获取所述采样点云中点云的个数；
将所述采样点云中各点云的x坐标、y坐标和z坐标分别相加，得到x坐标相加结果、y坐标相加结果和z坐标相加结果；
将所述x坐标相加结果、y坐标相加结果和z坐标相加结果分别除以点云的个数，得到点云分布中心。
4.根据权利要求1所述的方法，其特征在于，根据所述点云分布中心和所述特征坐标系，对所述采样点云进行划分，生成多个象限，在每个所述象限内以三维直方图存储相应采样点云的反射率和/或高度，具体包括：
以点云分布中心为球心，分别16米和80米为半径做内球与外球；
根据特征坐标系，对内球和外球除去内球的部分进行划分，得到16个象限；其中，所述内球分为8个象限，将外球除去内球的部分分为8个象限；
当在每个所述象限内以三维直方图存储相应采样点云的反射率时，在每个象限内，以反射率作为三维直方图的横轴的0-255，对多个采样点云进行划分，得到16个三维直方图；或者，
当在每个所述象限内以三维直方图存储相应采样点云的高度时，在每个象限内，以高度作为所述三维直方图的横坐标的0-80，对多个采样点云进行划分，得到16个三维直方图；或者，
当在每个所述象限内以三维直方图存储相应采样点云的反射率和高度时，在每个象限内，以反射率作为三维直方图的横轴的0-255，以高度作为所述三维直方图的横坐标的256-335，对多个采样点云进行划分，得到16个三维直方图。
5.根据权利要求1所述的方法，其特征在于，所述将所述实时三维直方图与所述第一数量个特征文件中的三维直方图进行匹配，得到第一匹配结果，具体包括：
计算所述第一数量个特征文件中每个特征文件中的全部三维直方图的反射率的均值和/或高度的均值；
计算所述实时三维直方图的反射率与所述反射率的均值的第一数值，和/或所述实时三维直方图的高度与所述高度的均值的第二数值；
根据所述第一数值和/或所述第二数值，计算所述实时三维直方图与第一数量个特征文件中的每个特征文件的第一匹配结果。
6.根据权利要求5所述的方法，其特征在于，利用卡方分布或者欧氏距离计算第一数值和/或第二数值。
7.根据权利要求1所述的方法，其特征在于，所述将所述第二数量个实时位置与每个特征文件在每个方位上平移后的位置进行第二匹配计算，得到第二匹配结果，具体包括：
利用NDT算法、ICP算法或者混合高斯模型算法进行第二匹配计算。
8.根据权利要求1所述的方法，其特征在于，所述第一距离为2米，所述第二距离为25米。
9.根据权利要求1所述的方法，其特征在于，所述第一数量为10，所述第二数量为10。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910306885.0A CN110031825B (zh) | 2019-04-17 | 2019-04-17 | 激光定位初始化方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910306885.0A CN110031825B (zh) | 2019-04-17 | 2019-04-17 | 激光定位初始化方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN110031825A CN110031825A (zh) | 2019-07-19 |
| CN110031825B true CN110031825B (zh) | 2021-03-16 |
Family
ID=67238654
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201910306885.0A Active CN110031825B (zh) | 2019-04-17 | 2019-04-17 | 激光定位初始化方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN110031825B (zh) |
Families Citing this family (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111461981B (zh) * | 2020-03-30 | 2023-09-01 | 北京百度网讯科技有限公司 | 点云拼接算法的误差估计方法和装置 |
| CN111461980B (zh) * | 2020-03-30 | 2023-08-29 | 北京百度网讯科技有限公司 | 点云拼接算法的性能估计方法和装置 |
| CN111947666B (zh) * | 2020-08-21 | 2022-10-28 | 广州高新兴机器人有限公司 | 一种室外激光导航位置丢失自动找回方法 |
| CN112348893B (zh) | 2020-10-30 | 2021-11-19 | 珠海一微半导体股份有限公司 | 一种局部点云地图构建方法及视觉机器人 |
| CN112731358B (zh) * | 2021-01-08 | 2022-03-01 | 奥特酷智能科技(南京)有限公司 | 一种多激光雷达外参在线标定方法 |
Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107741234A (zh) * | 2017-10-11 | 2018-02-27 | 深圳勇艺达机器人有限公司 | 一种基于视觉的离线地图构建及定位方法 |
| CN108320329A (zh) * | 2018-02-02 | 2018-07-24 | 维坤智能科技（上海）有限公司 | 一种基于3d激光的3d地图创建方法 |
Family Cites Families (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| GB201116959D0 (en) * | 2011-09-30 | 2011-11-16 | Bae Systems Plc | Vehicle localisation with 2d laser scanner and 3d prior scans |
| CN104299260B (zh) * | 2014-09-10 | 2017-05-17 | 西南交通大学 | 一种基于sift和lbp的点云配准的接触网三维重建方法 |
| CN104298998B (zh) * | 2014-09-28 | 2017-09-08 | 北京理工大学 | 一种3d点云的数据处理方法 |
| CN104764457B (zh) * | 2015-04-21 | 2017-11-17 | 北京理工大学 | 一种用于无人车的城市环境构图方法 |
| CN106908775B (zh) * | 2017-03-08 | 2019-10-18 | 同济大学 | 一种基于激光反射强度的无人车实时定位方法 |
| CN108917761B (zh) * | 2018-05-07 | 2021-01-19 | 西安交通大学 | 一种无人车在地下车库中的精确定位方法 |
| CN109064506B (zh) * | 2018-07-04 | 2020-03-13 | 百度在线网络技术（北京）有限公司 | 高精度地图生成方法、装置及存储介质 |
| CN109146976B (zh) * | 2018-08-23 | 2020-06-02 | 百度在线网络技术（北京）有限公司 | 用于定位无人车的方法和装置 |
| CN109559338B (zh) * | 2018-11-20 | 2020-10-27 | 西安交通大学 | 一种基于加权主成分分析法及m估计的三维点云配准方法 |
-
2019
- 2019-04-17 CN CN201910306885.0A patent/CN110031825B/zh active Active
Patent Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107741234A (zh) * | 2017-10-11 | 2018-02-27 | 深圳勇艺达机器人有限公司 | 一种基于视觉的离线地图构建及定位方法 |
| CN108320329A (zh) * | 2018-02-02 | 2018-07-24 | 维坤智能科技（上海）有限公司 | 一种基于3d激光的3d地图创建方法 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN110031825A (zh) | 2019-07-19 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110031825B (zh) | 激光定位初始化方法 | |
| CN110609290B (zh) | 激光雷达匹配定位方法及装置 | |
| CN109887033B (zh) | 定位方法及装置 | |
| CN110673107B (zh) | 基于多线激光雷达的路沿检测方法及装置 | |
| KR20180044279A (ko) | 깊이 맵 샘플링을 위한 시스템 및 방법 | |
| CN109143207A (zh) | 激光雷达内参精度验证方法、装置、设备及介质 | |
| CN110009718A (zh) | 一种三维高精度地图生成方法及装置 | |
| CN111380510B (zh) | 重定位方法及装置、机器人 | |
| CN105786016A (zh) | 无人机以及rgbd图像的处理方法 | |
| CN115290097B (zh) | 基于bim的实时精确地图构建方法、终端及存储介质 | |
| CN111481112B (zh) | 扫地机的回充对准方法、装置及扫地机 | |
| CN111812669B (zh) | 绕机检查装置及其定位方法、存储介质 | |
| CN114972767A (zh) | 一种基于高空无人机视频的车辆轨迹和航向角提取方法 | |
| CN114758063A (zh) | 基于八叉树结构的局部障碍物栅格地图构建方法及系统 | |
| CN114740867A (zh) | 基于双目视觉的智能避障方法、装置、机器人及介质 | |
| CN112597946A (zh) | 障碍物表示方法、装置、电子设备及可读存储介质 | |
| CN116188470B (zh) | 一种基于无人机航拍识别的故障定位方法和系统 | |
| US20220406014A1 (en) | Granularity-flexible existence-based object detection | |
| CN115542301A (zh) | 激光雷达外部参数的校准方法、装置、设备及存储介质 | |
| CN113421306A (zh) | 定位初始化方法、系统及移动工具 | |
| CN114228411A (zh) | 连接控制方法、装置、设备及存储介质 | |
| KR102618951B1 (ko) | 시각적 매핑 방법 및 이를 실행하기 위하여 기록매체에 기록된 컴퓨터 프로그램 | |
| KR102675138B1 (ko) | 복수개의 라이다들의 캘리브레이션 방법 및 이를 실행하기 위하여 기록매체에 기록된 컴퓨터 프로그램 | |
| KR102626574B1 (ko) | 카메라 및 라이다의 캘리브레이션 방법 및 이를 실행하기 위하여 기록매체에 기록된 컴퓨터 프로그램 | |
| CN113777615B (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 | ||
| TR01 | Transfer of patent right |
Effective date of registration: 20220718 Address after: 401122 No.1, 1st floor, building 3, No.21 Yunzhu Road, Yubei District, Chongqing Patentee after: Chongqing landshipu Information Technology Co.,Ltd. Address before: B4-006, maker Plaza, 338 East Street, Huilongguan town, Changping District, Beijing 100096 Patentee before: Beijing Idriverplus Technology Co.,Ltd. |
|
| TR01 | Transfer of patent right |