CN115631314A - 一种基于多特征和自适应关键帧的点云地图构建方法 - Google Patents

一种基于多特征和自适应关键帧的点云地图构建方法 Download PDF

Info

Publication number
CN115631314A
CN115631314A CN202211629343.5A CN202211629343A CN115631314A CN 115631314 A CN115631314 A CN 115631314A CN 202211629343 A CN202211629343 A CN 202211629343A CN 115631314 A CN115631314 A CN 115631314A
Authority
CN
China
Prior art keywords
frame
point
point cloud
parameter matrix
transformation parameter
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.)
Granted
Application number
CN202211629343.5A
Other languages
English (en)
Other versions
CN115631314B (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.)
China Automotive Technology and Research Center Co Ltd
CATARC Tianjin Automotive Engineering Research Institute Co Ltd
Original Assignee
China Automotive Technology and Research Center Co Ltd
CATARC Tianjin Automotive Engineering Research Institute Co Ltd
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 China Automotive Technology and Research Center Co Ltd, CATARC Tianjin Automotive Engineering Research Institute Co Ltd filed Critical China Automotive Technology and Research Center Co Ltd
Priority to CN202211629343.5A priority Critical patent/CN115631314B/zh
Publication of CN115631314A publication Critical patent/CN115631314A/zh
Application granted granted Critical
Publication of CN115631314B publication Critical patent/CN115631314B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T17/00—Three dimensional [3D] modelling, e.g. data description of 3D objects
    • G06T17/05—Geographic models
    • 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

  • Physics & Mathematics (AREA)
  • Engineering & Computer Science (AREA)
  • Geometry (AREA)
  • Software Systems (AREA)
  • Remote Sensing (AREA)
  • Computer Graphics (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Radar Systems Or Details Thereof (AREA)

Abstract

本发明提供了一种基于多特征和自适应关键帧的点云地图构建方法，基于正态分布变换算法NDT进行帧间点云粗配准，然后基于点云迭代最近邻点配准算法PLICP进行点云配准纠正，实现高精度的lidar slam前端里程计；其次根据动态的欧氏距离及航向角的变化对点云的关键帧进行提取，最后提出基于滑动窗口完成对激光雷达实时定位与建图lidar slam的后端图优化，进行完成实时性点云高精度地图的构建。本发明所述的一种基于多特征和自适应关键帧的点云地图构建方法，不仅提高了点云地图构建的实时性，而且为无人车的导航提供了精确的高精度点云地图。

Description

一种基于多特征和自适应关键帧的点云地图构建方法
技术领域
本发明属于无人驾驶技术领域，尤其是涉及一种基于多特征和自适应关键帧的点云地图构建方法。
背景技术
车载雷达LIDAR能够自动快速地获取高精度、高密度的周围三维坐标信息，并在城市三维重建、地形测绘等领域具有重要的应用。随着无人车技术的发展，雷达LIDAR作为无人车中感知领域重要的传感器之一，在无人车的SLAM(Simultaneous Localization andMapping，即时定位与地图构建)技术中具有重要的研究价值。
激光雷达是无人驾驶技术开发的重要传感器之一，可通过扫描周围环境构建点云地图。但因雷达lidar对点云地图构建的精度低及实时性差，也是一大亟待克服的问题。
发明内容
有鉴于此，本发明旨在提出一种基于多特征和自适应关键帧的点云地图构建方法，以解决雷达lidar对点云地图构建的精度低及实时性差的问题。
为达到上述目的，本发明的技术方案是这样实现的：
一种基于多特征和自适应关键帧的点云地图构建方法，具体步骤包括：
S1、接收第N帧点云集并初始化坐标系，通过变换参数矩阵T将第N+1帧映射到第N帧坐标系中，并计算映射后第N+1帧点云集的概率密度函数，结合牛顿优化算法优化第N帧与第N+1帧间点云粗配准，并获得最优解的坐标变换参数矩阵T’，N≥1；
S2、最优解的坐标变换参数矩阵T’结合点云迭代最近邻点配准算法PLICP进行第N帧与第N+1帧之间的点云配准纠正，从第N+1帧点云集中找到距离第N帧参考表面距离最小的点，将距离最小的点记为第N+1帧点；
S3、步骤S2执行结束后，将N的值迭代加一；
S4、重复步骤S1-S3，实时获取第N+1帧点，并将第N+1帧点与当前关键帧点配合判断第N+1帧点是满足成为关键帧点的要求，若满足则第N+1帧点保留并成为最新关键帧点，若不满足则删除第N+1帧点，当前关键帧点仍为最新关键帧点；
S5、在点云地图中加入最新关键帧点，实时的对点云地图的更新及构建。
进一步的，步骤S1中，接收第N帧点云集并初始化坐标系，通过变换参数矩阵T将第N+1帧映射到第N帧坐标系中，并计算映射后第N+1帧点云集的概率密度函数，结合牛顿优化算法优化第N帧与第N+1帧间点云粗配准，并获得最优解的坐标变换参数矩阵T’，具体方法：
S11、将第N帧雷达扫描的点云集在所占的空间中划分指定大小的网格，并计算每个网格的均值和协方差矩阵，初始化第N帧坐标系；
S12、初始化坐标变换参数矩阵Tr；
S13、将第N+1帧雷达扫描点云集根据变换参数矩阵映射到第N帧坐标系中，获得映射后第N+1帧雷达扫描点云集合X i’；
S14、映射后第N+1帧雷达扫描点云集合X i’通过正态分布变换算法NDT转换为分段连续可微的概率密度函数；
S15、将X i’中每个点的概率密度相加，获得概率之和S(b)；
S16、根据牛顿优化算法优化概率之和S(b)，寻找变换矩阵b使得概率之和S(b)最大，并获得新的变换参数矩阵Tj；
S17、将步骤S16中新的变换参数矩阵Tj替换步骤S13中的变换参数矩阵并重复步骤S13-S17，直至满足收敛条件，收敛时，变换参数矩阵Tj为最优解的坐标变换参数矩阵T’，实现相邻帧之间的点云粗配准。
进一步的，步骤S12中初始化坐标变换参数矩阵Tr的方法为，
Figure 985638DEST_PATH_IMAGE002
其中，R是3*3的旋转矩阵，t为3*1的平移矩阵。
进一步的，步骤S11中，将第N帧雷达扫描的点云集在所占的空间中划分指定大小的网格，并计算每个网格的均值和协方差矩阵，具体方法如下：
将第N帧雷达扫描到的点云集的点云空间分为相同的若干个立方体，并满足立方体内至少有5个点云，分别求出每个立方体内点的均值q和协方差矩阵∑，具体公式如下
Figure 385002DEST_PATH_IMAGE003
其中，X i=1,2…n 为点云集合，n为点云个数；
以概率密度的形式对离散的点云进行分段连续可微表示，则通过NDT算法表示立方体每个点的概率密度函数，具体公式为，
Figure 430318DEST_PATH_IMAGE005
，
步骤S14中映射后第N+1帧雷达扫描点云集合X i’通过正态分布变换算法NDT转换为分段连续可微的概率密度函数与步骤S11中，第N帧雷达扫描到的点云集转换为分段连续可微的概率密度函数的方法相同，则第N+1帧雷达扫描点云集合X i’概率密度函数为，
Figure 513943DEST_PATH_IMAGE007
。
进一步的，步骤S15中，将X i’中每个点的概率密度相加，获得概率之和S(b)，具体公式：
Figure 472190DEST_PATH_IMAGE008
进一步的，步骤S2中，最优解的坐标变换参数矩阵T’结合点云迭代最近邻点配准算法PLICP进行第N帧与第N+1帧之间的点云配准纠正，从第N+1帧点云集中找到距离第N帧参考表面距离最小的点，将所述距离最小的点记为第N+1帧点，具体步骤如下：
令T’=w=(t，θ)，根据第N帧参考表面S ref 和第N+1帧点中的点云集{P i }，通过旋转和平移w=(t，θ)，在参考表面S ref 找到距离最小的点P h ，P h 属于点云集{P i }，公式如下：
Figure 356970DEST_PATH_IMAGE009
其中：n i 是点到参考面的法向量，“⊕”表示旋转运算符，此时，w k 是经过NDT进行点云粗配准得到的初始变换值T’。
进一步的，步骤S4中，将第N+1帧点与当前关键帧点配合判断第N+1帧点是满足成为关键帧点的要求，具体判断方法如下：
人为设置欧氏距离差值的阈值包括直道场景阈值D1，弯道场景阈值D2；
人为设置航向角差值的阈值包括直道场景阈值
Figure 437052DEST_PATH_IMAGE011
，弯道场景阈值
Figure 42477DEST_PATH_IMAGE013
；
计算第N+1帧点与当前关键帧点之间的欧氏距离差值
Figure 459159DEST_PATH_IMAGE015
及航向角差值
Figure 124626DEST_PATH_IMAGE017
；
若
Figure 426426DEST_PATH_IMAGE018
，则第N+1帧点保留并成为最新关键帧点，且无人车为直道场景行驶，并将最新关键帧点加入点云地图的更新构建；
若
Figure 773225DEST_PATH_IMAGE019
，则第N+1帧点保留并成为最新关键帧点，且无人车为弯道场景行驶，并将最新关键帧点加入点云地图的更新构建；
既不满足
Figure 358927DEST_PATH_IMAGE020
，也不满足
Figure 870329DEST_PATH_IMAGE019
时，第N+1帧点删除不能作为最新关键帧点。
进一步的，步骤S5中，在点云地图中加入最新关键帧点，实时的对点云地图的更新及构建，具体方法为：在点云地图中加入最新关键帧点，通过随时间滑动的滑动窗口对最新的多个连续关键帧数据进行优化，实现实时对点云地图的更新及构建。
相对于现有技术，本发明所述的一种基于多特征和自适应关键帧的点云地图构建方法具有以下有益效果：
（1）本发明所述的一种基于多特征和自适应关键帧的点云地图构建方法，通过变换参数矩阵T将第N+1帧映射到第N帧坐标系中，并计算映射后第N+1帧点云集的概率密度函数，结合牛顿优化算法优化第N帧与第N+1帧间点云粗配准，并结合点云迭代最近邻点配准算法PLICP进行第N帧与第N+1帧之间的点云配准纠正，实现高精度的激光雷达实时定位与建图lidar slam前端位姿估计，为lidar slam后端优化做好准备。
（2）本发明所述的一种基于多特征和自适应关键帧的点云地图构建方法，引入欧氏距离及航向角的判断，减少数据冗余，同时准确提取弯道场景关键帧点云。
（3）本发明所述的一种基于多特征和自适应关键帧的点云地图构建方法，引入滑动窗口，解决了位姿累积误差以及提高了计算效率，实现了实时性点云地图的构建。
附图说明
构成本发明的一部分的附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。在附图中：
图1为本发明实施例所述的一种基于多特征和自适应关键帧的点云地图构建方法流程图；
图2为本发明实施例所述的直道场景下的关键帧提取示意图；
图3为本发明实施例所述的弯道场景下的关键帧提取示意图。
具体实施方式
需要说明的是，在不冲突的情况下，本发明中的实施例及实施例中的特征可以相互组合。
在本发明的描述中，需要理解的是，术语“中心”、“纵向”、“横向”、“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。此外，术语“第一”、“第二”等仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”等的特征可以明示或者隐含地包括一个或者更多个该特征。在本发明的描述中，除非另有说明，“多个”的含义是两个或两个以上。
在本发明的描述中，需要说明的是，除非另有明确的规定和限定，术语“安装”、“相连”、“连接”应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以通过具体情况理解上述术语在本发明中的具体含义。
下面将参考附图并结合实施例来详细说明本发明。
如图1所示，一种基于多特征和自适应关键帧的点云地图构建方法，具体步骤包括：
S1、接收第N帧点云集并初始化坐标系，通过变换参数矩阵T将第N+1帧映射到第N帧坐标系中，并计算映射后第N+1帧点云集的概率密度函数，结合牛顿优化算法优化第N帧与第N+1帧间点云粗配准，并获得最优解的坐标变换参数矩阵T’，N≥1；
S2、最优解的坐标变换参数矩阵T’结合点云迭代最近邻点配准算法PLICP进行第N帧与第N+1帧之间的点云配准纠正，从第N+1帧点云集中找到距离第N帧参考表面距离最小的点，将距离最小的点记为第N+1帧点；
S3、步骤S2执行结束后，将N的值迭代加一；
S4、重复步骤S1-S3，实时获取第N+1帧点，并将第N+1帧点与当前关键帧点配合判断第N+1帧点是满足成为关键帧点的要求，若满足则第N+1帧点保留并成为最新关键帧点，若不满足则删除第N+1帧点，当前关键帧点仍为最新关键帧点；
S5、在点云地图中加入最新关键帧点，实时的对点云地图的更新及构建。
如图1所示，步骤S1中，接收第N帧点云集并初始化坐标系，通过变换参数矩阵T将第N+1帧映射到第N帧坐标系中，并计算映射后第N+1帧点云集的概率密度函数，结合牛顿优化算法优化第N帧与第N+1帧间点云粗配准，并获得最优解的坐标变换参数矩阵T’，具体方法：
S11、将第N帧雷达扫描的点云集在所占的空间中划分指定大小的网格，并计算每个网格的均值q和协方差矩阵∑，初始化第N帧坐标系；
S12、初始化坐标变换参数矩阵Tr为相邻两帧点云集匹配提供初始变换矩阵，为4*4的单位矩阵；
S13、将第N+1帧雷达扫描点云集根据变换参数矩阵映射到第N帧坐标系中，获得映射后第N+1帧雷达扫描点云集合X i’；
S14、映射后第N+1帧雷达扫描点云集合X i’通过正态分布变换算法NDT转换为分段连续可微的概率密度函数；
S15、将X i’中每个点的概率密度相加，获得概率之和S(b)；
S16、根据牛顿优化算法优化概率之和S(b)，寻找变换矩阵b使得概率之和S(b)最大，并获得新的变换参数矩阵Tj，其中变换矩阵b为矩阵Tj；
S17、将步骤S16中新的变换参数矩阵Tj替换步骤S13中的变换参数矩阵并重复步骤S13-S17，直至满足收敛条件，收敛时，变换参数矩阵Tj为最优解的坐标变换参数矩阵T’，实现相邻帧之间的点云粗配准。
如图1所示，步骤S12中初始化坐标变换参数矩阵Tr的方法为，
Figure 439851DEST_PATH_IMAGE022
其中，R是3*3的旋转矩阵，t为3*1的平移矩阵。
如图1所示，步骤S11中，将第N帧雷达扫描的点云集在所占的空间中划分指定大小的网格，并计算每个网格的均值和协方差矩阵，具体方法如下：
将第N帧雷达扫描到的点云集的点云空间分为相同的若干个立方体，并满足立方体内至少有5个点云，分别求出每个立方体内点的均值和协方差矩阵，具体公式如下
Figure 528024DEST_PATH_IMAGE003
其中，X i=1,2…n 为点云集合，n为点云个数；
以概率密度的形式对离散的点云进行分段连续可微表示，则通过NDT算法表示立方体每个点的概率密度函数，具体公式为，
Figure 702653DEST_PATH_IMAGE024
，
步骤S14中映射后第N+1帧雷达扫描点云集合X i’通过正态分布变换算法NDT转换为分段连续可微的概率密度函数与步骤S11中，第N帧雷达扫描到的点云集转换为分段连续可微的概率密度函数的方法相同，则第N+1帧雷达扫描点云集合X i’概率密度函数为，
Figure 585290DEST_PATH_IMAGE026
。
如图1所示，步骤S15中，将X i’中每个点的概率密度相加，获得概率之和S(b)，具体公式：
Figure 593172DEST_PATH_IMAGE028
如图1所示，步骤S2中，最优解的坐标变换参数矩阵T’结合点云迭代最近邻点配准算法PLICP进行第N帧与第N+1帧之间的点云配准纠正，从第N+1帧点云集中找到距离第N帧参考表面距离最小的点，将所述距离最小的点记为第N+1帧点，具体步骤如下：
令T’=w=(t，θ)，根据第N帧参考表面S ref 和第N+1帧点中的点云集{P i }，通过旋转和平移w=(t，θ)，在参考表面S ref 找到距离最小的点P h ，P h 属于点云集{P i }，公式如下：
Figure 734304DEST_PATH_IMAGE009
其中：n i 是点到参考面的法向量，“⊕”表示旋转运算符。此时的w k 是经过NDT进行点云粗配准得到的初始变换值T’。
如图1至图3所示，步骤S4中，将第N+1帧点与当前关键帧点配合判断第N+1帧点是满足成为关键帧点的要求，具体判断方法如下：
人为设置欧氏距离差值的阈值包括直道场景阈值D1，弯道场景阈值D2；
人为设置航向角差值的阈值包括直道场景阈值
Figure 310910DEST_PATH_IMAGE030
，弯道场景阈值
Figure 613715DEST_PATH_IMAGE032
；
计算第N+1帧点与当前关键帧点之间的欧氏距离差值
Figure 970878DEST_PATH_IMAGE034
及航向角差值
Figure 666433DEST_PATH_IMAGE036
；
若
Figure 81234DEST_PATH_IMAGE018
，则第N+1帧点保留并成为最新关键帧点，且无人车为直道场景行驶，并将最新关键帧点加入点云地图的更新构建；
若
Figure 839761DEST_PATH_IMAGE019
，则第N+1帧点保留并成为最新关键帧点，且无人车为弯道场景行驶，并将最新关键帧点加入点云地图的更新构建；
既不满足
Figure 340012DEST_PATH_IMAGE020
，也不满足
Figure 573679DEST_PATH_IMAGE019
时，第N+1帧点删除不能作为最新关键帧点。
关键帧的选取对点云地图优化有着重要的意义。在自动驾驶进行即时定位与建图时，由前端进行计算相邻帧之间的位姿变换，后端进行点云地图的构建。但是由于目前计算机本身的计算性能还不能够处理类似SLAM的工程，所以在创建点云地图之前，选取关键帧进行创建点云地图以提高计算效率。本发明提出根据动态的欧氏距离及航向角的变化完成对点云的关键帧提取，场景分为直道（
Figure 374144DEST_PATH_IMAGE038
）和弯道（
Figure 690856DEST_PATH_IMAGE040
），其中
Figure 288191DEST_PATH_IMAGE042
与
Figure 325548DEST_PATH_IMAGE044
分别为直道场景与弯道场景中连续多帧点（几帧如何确定）云之间的欧氏距离阈值，
Figure 449362DEST_PATH_IMAGE046
与
Figure 936975DEST_PATH_IMAGE048
分别为直道场景与弯道场景中连续多帧点云之间的航向角阈值，选取关键帧的策略如下：
(1) 根据无人车的行驶距离和航向角来区分当前的场景是直道：
点云关键帧点序列为：
Figure 159622DEST_PATH_IMAGE050
，计算出
Figure 515517DEST_PATH_IMAGE052
至
Figure 775728DEST_PATH_IMAGE054
之间的欧氏距离
Figure 293297DEST_PATH_IMAGE056
与航向角
Figure 6169DEST_PATH_IMAGE058
，当
Figure 369017DEST_PATH_IMAGE059
时，根据以上策略
Figure 483735DEST_PATH_IMAGE052
与
Figure 641047DEST_PATH_IMAGE054
为这一时刻的点云关键帧参与后端建图。
(2) 根据无人车的行驶距离和航向角来区分当前的场景是弯道：
点云关键帧点序列为：
Figure 133161DEST_PATH_IMAGE050
，计算出
Figure 830859DEST_PATH_IMAGE052
至
Figure 534504DEST_PATH_IMAGE054
之间的欧氏距离
Figure 534821DEST_PATH_IMAGE056
与航向角
Figure 471553DEST_PATH_IMAGE058
，当
Figure 723674DEST_PATH_IMAGE060
时，根据以上策略
Figure 796672DEST_PATH_IMAGE052
与
Figure 577677DEST_PATH_IMAGE054
为这一时刻的点云关键帧参与后端建图。
如图1所示，步骤S5中，在点云地图中加入最新关键帧点，实时的对点云地图的更新及构建，具体方法为：在点云地图中加入最新关键帧点，通过随时间滑动的滑动窗口对最新的多个连续关键帧数据进行优化，实现实时对点云地图的更新及构建。
滑动窗口大小固定，窗口内优化的连续关键帧的数量固定，随时间移动，大量关键帧参与点云地图时，每次仅优化窗口内的变量，并边缘化其余变量，由于优化时所有变量都将被重新线性化，因此线性化累积误差较小，精度得到保证；同时由于窗口大小固定，优化变量个数基本不变，可以满足实时性。
以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (8)

1.一种基于多特征和自适应关键帧的点云地图构建方法，其特征在于：具体步骤包括：
S1、接收第N帧点云集并初始化坐标系，通过变换参数矩阵T将第N+1帧映射到第N帧坐标系中，并计算映射后第N+1帧点云集的概率密度函数，结合牛顿优化算法优化第N帧与第N+1帧间点云粗配准，并获得最优解的坐标变换参数矩阵T’，N≥1；
S2、最优解的坐标变换参数矩阵T’结合点云迭代最近邻点配准算法PLICP进行第N帧与第N+1帧之间的点云配准纠正，从第N+1帧点云集中找到距离第N帧参考表面距离最小的点，将距离最小的点记为第N+1帧点；
S3、步骤S2执行结束后，将N的值迭代加一；
S4、重复步骤S1-S3，实时获取第N+1帧点，并将第N+1帧点与当前关键帧点配合判断第N+1帧点是满足成为关键帧点的要求，若满足则第N+1帧点保留并成为最新关键帧点，若不满足则删除第N+1帧点，当前关键帧点仍为最新关键帧点；
S5、在点云地图中加入最新关键帧点，实时的对点云地图的更新及构建。
2.根据权利要求1所述的一种基于多特征和自适应关键帧的点云地图构建方法，其特征在于：步骤S1中，接收第N帧点云集并初始化坐标系，通过变换参数矩阵T将第N+1帧映射到第N帧坐标系中，并计算映射后第N+1帧点云集的概率密度函数，结合牛顿优化算法优化第N帧与第N+1帧间点云粗配准，并获得最优解的坐标变换参数矩阵T’，具体方法：
S11、将第N帧雷达扫描的点云集在所占的空间中划分指定大小的网格，并计算每个网格的均值和协方差矩阵，初始化第N帧坐标系；
S12、初始化坐标变换参数矩阵Tr；
S13、将第N+1帧雷达扫描点云集根据变换参数矩阵映射到第N帧坐标系中，获得映射后第N+1帧雷达扫描点云集合X i’；
S14、映射后第N+1帧雷达扫描点云集合X i’通过正态分布变换算法NDT转换为分段连续可微的概率密度函数；
S15、将X i’中每个点的概率密度相加，获得概率之和S(b)；
S16、根据牛顿优化算法优化概率之和S(b)，寻找变换矩阵b使得概率之和S(b)最大，并获得新的变换参数矩阵Tj；
S17、将步骤S16中新的变换参数矩阵Tj替换步骤S13中的变换参数矩阵并重复步骤S13-S17，直至满足收敛条件，收敛时，变换参数矩阵Tj为最优解的坐标变换参数矩阵T’，实现相邻帧之间的点云粗配准。
3.根据权利要求2所述的一种基于多特征和自适应关键帧的点云地图构建方法，其特征在于：步骤S12中初始化坐标变换参数矩阵Tr的方法为，
Figure 379132DEST_PATH_IMAGE002
其中，R是3*3的旋转矩阵，t为3*1的平移矩阵。
4.根据权利要求2所述的一种基于多特征和自适应关键帧的点云地图构建方法，其特征在于：步骤S11中，将第N帧雷达扫描的点云集在所占的空间中划分指定大小的网格，并计算每个网格的均值和协方差矩阵，具体方法如下：
将第N帧雷达扫描到的点云集的点云空间分为相同的若干个立方体，并满足立方体内至少有5个点云，分别求出每个立方体内点的均值q和协方差矩阵∑，具体公式如下
Figure 28419DEST_PATH_IMAGE003
其中，X i=1,2…n 为点云集合，n为点云个数；
以概率密度的形式对离散的点云进行分段连续可微表示，则通过NDT算法表示立方体每个点的概率密度函数，具体公式为，
Figure 96869DEST_PATH_IMAGE004
，
步骤S14中映射后第N+1帧雷达扫描点云集合X i’通过正态分布变换算法NDT转换为分段连续可微的概率密度函数与步骤S11中，第N帧雷达扫描到的点云集转换为分段连续可微的概率密度函数的方法相同，则第N+1帧雷达扫描点云集合X i’概率密度函数为，
Figure 579934DEST_PATH_IMAGE005
。
5.根据权利要求3所述的一种基于多特征和自适应关键帧的点云地图构建方法，其特征在于：步骤S15中，将X i’中每个点的概率密度相加，获得概率之和S(b)，具体公式：
Figure 98116DEST_PATH_IMAGE006
。
6.根据权利要求1所述的一种基于多特征和自适应关键帧的点云地图构建方法，其特征在于：步骤S2中，最优解的坐标变换参数矩阵T’结合点云迭代最近邻点配准算法PLICP进行第N帧与第N+1帧之间的点云配准纠正，从第N+1帧点云集中找到距离第N帧参考表面距离最小的点，将所述距离最小的点记为第N+1帧点，具体步骤如下：
令T’=w=(t，θ)，根据第N帧参考表面S ref 和第N+1帧点中的点云集{P i }，通过旋转和平移w=(t，θ)，在参考表面S ref 找到距离最小的点P h ，P h 属于点云集{P i }，公式如下：
Figure 551094DEST_PATH_IMAGE007
其中：n i 是点到参考面的法向量，“⊕”表示旋转运算符，此时，w k 是经过NDT进行点云粗配准得到的初始变换值T’。
7.根据权利要求1所述的一种基于多特征和自适应关键帧的点云地图构建方法，其特征在于：步骤S4中，将第N+1帧点与当前关键帧点配合判断第N+1帧点是满足成为关键帧点的要求，具体判断方法如下：
人为设置欧氏距离差值的阈值包括直道场景阈值D1，弯道场景阈值D2；
人为设置航向角差值的阈值包括直道场景阈值
Figure 474050DEST_PATH_IMAGE008
，弯道场景阈值
Figure 455913DEST_PATH_IMAGE009
；
计算第N+1帧点与当前关键帧点之间的欧氏距离差值
Figure 198741DEST_PATH_IMAGE010
及航向角差值
Figure 189831DEST_PATH_IMAGE011
；
若
Figure 967294DEST_PATH_IMAGE012
，则第N+1帧点保留并成为最新关键帧点，且无人车为直道场景行驶，并将最新关键帧点加入点云地图的更新构建；
若
Figure 916795DEST_PATH_IMAGE013
，则第N+1帧点保留并成为最新关键帧点，且无人车为弯道场景行驶，并将最新关键帧点加入点云地图的更新构建；
既不满足
Figure 415428DEST_PATH_IMAGE014
，也不满足
Figure 944630DEST_PATH_IMAGE013
时，第N+1帧点删除不能作为最新关键帧点。
8.根据权利要求1所述的一种基于多特征和自适应关键帧的点云地图构建方法，其特征在于：步骤S5中，在点云地图中加入最新关键帧点，实时的对点云地图的更新及构建，具体方法为：在点云地图中加入最新关键帧点，通过随时间滑动的滑动窗口对最新的多个连续关键帧数据进行优化，实现实时对点云地图的更新及构建。
CN202211629343.5A 2022-12-19 2022-12-19 一种基于多特征和自适应关键帧的点云地图构建方法 Active CN115631314B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211629343.5A CN115631314B (zh) 2022-12-19 2022-12-19 一种基于多特征和自适应关键帧的点云地图构建方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211629343.5A CN115631314B (zh) 2022-12-19 2022-12-19 一种基于多特征和自适应关键帧的点云地图构建方法

Publications (2)

Publication Number Publication Date
CN115631314A true CN115631314A (zh) 2023-01-20
CN115631314B CN115631314B (zh) 2023-06-09

Family

ID=84910680

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211629343.5A Active CN115631314B (zh) 2022-12-19 2022-12-19 一种基于多特征和自适应关键帧的点云地图构建方法

Country Status (1)

Country Link
CN (1) CN115631314B (zh)

Citations (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106251399A (zh) * 2016-08-30 2016-12-21 广州市绯影信息科技有限公司 一种基于lsd‑slam的实景三维重建方法
CN108648240A (zh) * 2018-05-11 2018-10-12 东南大学 基于点云特征地图配准的无重叠视场相机姿态标定方法
CN109974707A (zh) * 2019-03-19 2019-07-05 重庆邮电大学 一种基于改进点云匹配算法的室内移动机器人视觉导航方法
CN112258600A (zh) * 2020-10-19 2021-01-22 浙江大学 一种基于视觉与激光雷达的同时定位与地图构建方法
CN112950781A (zh) * 2021-03-19 2021-06-11 中山大学 特种场景的多传感器动态加权融合的点云地图构建方法
CN113447949A (zh) * 2021-06-11 2021-09-28 天津大学 一种基于激光雷达和先验地图的实时定位系统及方法
CN113516750A (zh) * 2021-06-30 2021-10-19 同济大学 三维点云地图构建方法、系统、电子设备及存储介质
CN113902860A (zh) * 2021-10-10 2022-01-07 北京工业大学 一种基于多线激光雷达点云的多尺度静态地图构建方法
CN114563000A (zh) * 2022-02-23 2022-05-31 南京理工大学 一种基于改进型激光雷达里程计的室内外slam方法
CN114659514A (zh) * 2022-03-21 2022-06-24 东南大学 一种基于体素化精配准的LiDAR-IMU-GNSS融合定位方法
CN115290065A (zh) * 2022-06-30 2022-11-04 广州文远知行科技有限公司 一种高精地图生成方法、装置、设备及存储介质
CN115372989A (zh) * 2022-08-19 2022-11-22 中国人民解放军陆军工程大学 基于激光雷达的越野自动小车长距离实时定位系统及方法

Patent Citations (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106251399A (zh) * 2016-08-30 2016-12-21 广州市绯影信息科技有限公司 一种基于lsd‑slam的实景三维重建方法
CN108648240A (zh) * 2018-05-11 2018-10-12 东南大学 基于点云特征地图配准的无重叠视场相机姿态标定方法
CN109974707A (zh) * 2019-03-19 2019-07-05 重庆邮电大学 一种基于改进点云匹配算法的室内移动机器人视觉导航方法
CN112258600A (zh) * 2020-10-19 2021-01-22 浙江大学 一种基于视觉与激光雷达的同时定位与地图构建方法
CN112950781A (zh) * 2021-03-19 2021-06-11 中山大学 特种场景的多传感器动态加权融合的点云地图构建方法
CN113447949A (zh) * 2021-06-11 2021-09-28 天津大学 一种基于激光雷达和先验地图的实时定位系统及方法
CN113516750A (zh) * 2021-06-30 2021-10-19 同济大学 三维点云地图构建方法、系统、电子设备及存储介质
CN113902860A (zh) * 2021-10-10 2022-01-07 北京工业大学 一种基于多线激光雷达点云的多尺度静态地图构建方法
CN114563000A (zh) * 2022-02-23 2022-05-31 南京理工大学 一种基于改进型激光雷达里程计的室内外slam方法
CN114659514A (zh) * 2022-03-21 2022-06-24 东南大学 一种基于体素化精配准的LiDAR-IMU-GNSS融合定位方法
CN115290065A (zh) * 2022-06-30 2022-11-04 广州文远知行科技有限公司 一种高精地图生成方法、装置、设备及存储介质
CN115372989A (zh) * 2022-08-19 2022-11-22 中国人民解放军陆军工程大学 基于激光雷达的越野自动小车长距离实时定位系统及方法

Also Published As

Publication number Publication date
CN115631314B (zh) 2023-06-09

Similar Documents

Publication Publication Date Title
CN109978955B (zh) 一种联合激光点云与图像的高效标注方法
CN115372989A (zh) 基于激光雷达的越野自动小车长距离实时定位系统及方法
CN112923933A (zh) 一种激光雷达slam算法与惯导融合定位的方法
CN114862932B (zh) 基于bim全局定位的位姿修正方法及运动畸变矫正方法
CN110645998A (zh) 一种基于激光点云的无动态物体地图分段建立方法
CN115290097B (zh) 基于bim的实时精确地图构建方法、终端及存储介质
CN114332348B (zh) 一种融合激光雷达与图像数据的轨道三维重建方法
CN113108773A (zh) 一种融合激光与视觉传感器的栅格地图构建方法
CN113706710A (zh) 基于fpfh特征差异的虚拟点多源点云融合方法及系统
CN112484746A (zh) 一种基于地平面的单目视觉辅助激光雷达里程计方法
CN112070800B (zh) 一种基于三维点云极化地图表征的智能车定位方法及系统
CN111932614A (zh) 一种基于聚类中心特征的激光雷达即时定位与建图方法
CN112396641A (zh) 一种基于全等二基线匹配的点云全局配准方法
CN114463396B (zh) 一种利用平面形状和拓扑图投票的点云配准方法
CN115908539A (zh) 一种目标体积自动测量方法和装置、存储介质
CN115471619A (zh) 基于立体成像高分辨率卫星影像的城市三维模型构建方法
CN117392268A (zh) 一种基于自适应结合cpd和icp算法的激光扫描建图方法及系统
CN115631314A (zh) 一种基于多特征和自适应关键帧的点云地图构建方法
CN114563000B (zh) 一种基于改进型激光雷达里程计的室内外slam方法
CN115482282A (zh) 自动驾驶场景下具有多目标追踪能力的动态slam方法
CN114648571A (zh) 机器人高精度地图建图中行驶区域内障碍物过滤方法
CN114581519A (zh) 一种越野环境下的四足机器人激光自主定位建图方法
CN114115231A (zh) 适用于医院场景下移动机器人空间位姿点云校正方法及系统
CN111899291A (zh) 基于多源维度分解的城市点云从粗到精的自动配准方法
CN116518992B (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