CN115131514A - 一种同时定位建图的方法、装置、系统及存储介质 - Google Patents
一种同时定位建图的方法、装置、系统及存储介质 Download PDFInfo
- Publication number
- CN115131514A CN115131514A CN202210807129.8A CN202210807129A CN115131514A CN 115131514 A CN115131514 A CN 115131514A CN 202210807129 A CN202210807129 A CN 202210807129A CN 115131514 A CN115131514 A CN 115131514A
- Authority
- CN
- China
- Prior art keywords
- point cloud
- cloud data
- point
- target
- map
- 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
- 238000000034 method Methods 0.000 title claims abstract description 59
- 230000010354 integration Effects 0.000 claims abstract description 26
- 238000005516 engineering process Methods 0.000 claims abstract description 16
- 238000007781 pre-processing Methods 0.000 claims abstract description 5
- 238000013507 mapping Methods 0.000 claims description 50
- 238000004422 calculation algorithm Methods 0.000 claims description 25
- 230000011218 segmentation Effects 0.000 claims description 19
- 238000012545 processing Methods 0.000 claims description 18
- 230000004807 localization Effects 0.000 claims description 16
- 238000005457 optimization Methods 0.000 claims description 12
- 238000004590 computer program Methods 0.000 claims description 9
- 238000000638 solvent extraction Methods 0.000 claims description 6
- 238000010276 construction Methods 0.000 claims description 5
- 230000001133 acceleration Effects 0.000 description 9
- 230000008859 change Effects 0.000 description 7
- 238000004364 calculation method Methods 0.000 description 6
- 230000000694 effects Effects 0.000 description 6
- 238000005070 sampling Methods 0.000 description 6
- 238000005259 measurement Methods 0.000 description 5
- 230000009466 transformation Effects 0.000 description 5
- 238000006243 chemical reaction Methods 0.000 description 4
- 238000010586 diagram Methods 0.000 description 4
- 238000001514 detection method Methods 0.000 description 3
- 230000006870 function Effects 0.000 description 3
- 238000009616 inductively coupled plasma Methods 0.000 description 3
- 230000008569 process Effects 0.000 description 3
- 238000013135 deep learning Methods 0.000 description 2
- 230000006872 improvement Effects 0.000 description 2
- 230000002159 abnormal effect Effects 0.000 description 1
- 230000002411 adverse Effects 0.000 description 1
- 150000001875 compounds Chemical class 0.000 description 1
- 238000002996 descriptor matching method Methods 0.000 description 1
- 238000000605 extraction Methods 0.000 description 1
- 238000013178 mathematical model Methods 0.000 description 1
- 239000011159 matrix material Substances 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000008520 organization Effects 0.000 description 1
- 230000008447 perception Effects 0.000 description 1
- 230000000644 propagated effect Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 239000004575 stone Substances 0.000 description 1
- 239000000126 substance Substances 0.000 description 1
- 238000013519 translation Methods 0.000 description 1
- 238000012800 visualization Methods 0.000 description 1
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
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T15/00—3D [Three Dimensional] image rendering
- G06T15/06—Ray-tracing
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T3/00—Geometric image transformations in the plane of the image
- G06T3/06—Topological mapping of higher dimensional structures onto lower dimensional surfaces
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/30—Determination of transform parameters for the alignment of images, i.e. image registration
- G06T7/33—Determination of transform parameters for the alignment of images, i.e. image registration using feature-based methods
- G06T7/344—Determination of transform parameters for the alignment of images, i.e. image registration using feature-based methods involving models
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/50—Depth or shape recovery
- G06T7/55—Depth or shape recovery from multiple images
- G06T7/593—Depth or shape recovery from multiple images from stereo images
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/70—Determining position or orientation of objects or cameras
- G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
- G06T7/75—Determining position or orientation of objects or cameras using feature-based methods involving models
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
- G06V10/00—Arrangements for image or video recognition or understanding
- G06V10/70—Arrangements for image or video recognition or understanding using pattern recognition or machine learning
- G06V10/74—Image or video pattern matching; Proximity measures in feature spaces
- G06V10/75—Organisation of the matching processes, e.g. simultaneous or sequential comparisons of image or video features; Coarse-fine approaches, e.g. multi-scale approaches; using context analysis; Selection of dictionaries
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
- G06V10/00—Arrangements for image or video recognition or understanding
- G06V10/70—Arrangements for image or video recognition or understanding using pattern recognition or machine learning
- G06V10/762—Arrangements for image or video recognition or understanding using pattern recognition or machine learning using clustering, e.g. of similar faces in social networks
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/10—Image acquisition modality
- G06T2207/10028—Range image; Depth image; 3D point clouds
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Theoretical Computer Science (AREA)
- General Physics & Mathematics (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Software Systems (AREA)
- Artificial Intelligence (AREA)
- Computing Systems (AREA)
- Databases & Information Systems (AREA)
- Evolutionary Computation (AREA)
- General Health & Medical Sciences (AREA)
- Medical Informatics (AREA)
- Health & Medical Sciences (AREA)
- Multimedia (AREA)
- Computer Graphics (AREA)
- Geometry (AREA)
- Remote Sensing (AREA)
- Optical Radar Systems And Details Thereof (AREA)
Abstract
本发明公开了一种同时定位建图方法、装置、系统，方法包括以下步骤：根据激光雷达发送点云数据的频率对IMU传感器接收到的IMU数据进行预处理，得到IMU预积分数据；将激光雷达接收到的三维的点云数据转换为二维的深度图像；利用IMU预积分数据与深度图像，获得相邻两帧点云数据之间目标的移动位姿信息；以目标的移动位姿信息为初始值，将当前帧的点云数据与三角网格地图进行匹配，得到目标在所述三角网格地图中的当前位姿信息，完成定位；基于当前帧的点云数据和目标的当前位姿信息，利用三维点云重建技术生成与目标的位置周围预设范围对应的三角形网格，并更新到三角网格地图中，完成建图。
Description
技术领域
本发明涉及无人驾驶技术领域，具体涉及一种同时定位建图的方法、装置、系统及存储介质。
背景技术
无人驾驶技术中同时定位与建图技术是构成无人驾驶技术的基石。如何在未知环境中同时实现对自身位置的估计以及对周围环境的感知是同时定位与建图技术要解决的问题。在无人驾驶领域中，定位算法的准确性在很大程度上依赖于地图的精度。如何在大场景中实时构建高精度地图成为了研究的热点。
目前，现有的基于激光雷达的同时定位与建图方法在机器人发生旋转时定位精度差、建图质量低，当机器人快速旋转时会导致系统崩溃。这是由于激光雷达里程计提供的位姿信息对于旋转信息不敏感，无法正确的描述由于旋转引起的位姿变化，导致产生错误的定位结果，使得系统进入非收敛态并引起系统崩溃。
为了解决这一问题，研究人员引入了额外的IMU(Inertial Measurement Unit，惯性测量单元)传感器，IMU传感器是测量物体三轴姿态角或角速度以及加速度的装置，其可以直接检测机器人的角加速度信息及重力加速度信息，通过计算即可间接得到机器人的位姿信息。激光雷达与惯性测量单元进行融合的同时定位与建图方法，可以解决纯激光雷达算法在旋转时无法正确定位建图的问题。这正是由于IMU传感器提供了正确的位姿变化信息，保证了系统的定位准确。
通过将IMU传感器采集到的角加速度数据进行积分可计算机器人的位姿，为保证位姿准确性，要求传感器具有较高的采样频率，将产生大量低变化率的位姿信息。大量的位姿信息将导致因子图中节点数目过多使得因子图膨胀，因子图膨胀导致优化问题过于复杂，使得系统无法实时的进行定位与建图。
为了解决这一问题，可以使用IMU预积分技术，将连续的低变化率的位姿信息通过预先积分转变为离散的高变化率的位姿信息，从而避免因子图膨胀问题，提升算法的实时运算性能。上述使用预积分技术的激光雷达与IMU融合的SLAM(SimultaneousLocalization and Mapping，同步定位与建图)算法使用点云数据存储地图信息，并且通过激光雷达扫描点云与地图点云匹配生成激光雷达里程计信息。但是，现有的激光雷达与惯性测量单元进行融合的同时定位与建图方法存在以下问题：
(1)由于点云地图本质上是三维空间中离散的点的集合，无法区分噪声点以及有效特征点；在进行通过激光雷达扫描点云与地图点云匹配生成激光雷达里程计信息时，噪声点将导致错误的匹配结果；
(2)点云特征无法直接描述空间中物体的几何形状。本质上点云是对空间中物体进行了采样，无法直接表示物体的几何信息；
(3)对点云的处理以及存储需要消耗大量的运算资源以及存储空间。
发明内容
针对上述技术问题，本发明针提供了一种同时定位建图的方法、装置、系统及存储介质。
本发明解决上述技术问题的技术方案如下：
本发明的技术方案提供一种同时定位建图方法，包括以下步骤：
步骤1.根据激光雷达发送的点云数据的频率对IMU传感器接收到的IMU数据进行预处理，得到IMU预积分数据；其中，激光雷达和IMU传感器均安装在目标上；
步骤2.将激光雷达接收到的三维的点云数据转换为二维的深度图像；
步骤3.利用所述IMU预积分数据与所述深度图像，获得相邻两帧点云数据之间所述目标的移动位姿信息；
步骤4.以所述目标的移动位姿信息为初始位姿，将当前帧的点云数据与三角网格地图进行匹配，得到目标在所述三角网格地图中的当前位姿信息，完成定位；
步骤5.基于当前帧的点云数据和目标的当前位姿信息，利用三维点云重建技术生成与所述目标的当前位置周围预设范围对应的三角形网格，并更新到所述三角网格地图中，完成建图。
进一步地，所述步骤5之后，还包括步骤6：采用多约束因子图算法，将所述IMU预积分数据和所述点云数据作为因子，添加至因子图中，并执行因子图优化，以对所述目标的当前位姿信息和更新后的所述三角网格地图进行全局优化。
进一步地，所述步骤6之后，还包括步骤7：将每一帧点云数据中目标的当前位姿信息与更新后的所述三角网格地图对应存储，并生成贝叶斯树。
进一步地，所述步骤3包括：
对所述深度图像中的每帧点云数据进行聚类分割处理；
从聚类分割处理后的每帧点云数据中提取特征点信息；
以所述IMU预积分数据为初始值，将从相邻两帧点云数据中提取的特征点信息进行匹配，获得相邻两帧点云数据之间所述目标的移动位姿信息。
进一步地，所述步骤3中对所述深度图像中的每帧点云数据进行聚类分割处理的步骤包括：
对于任意两个相邻的点云数据，计算对应两个相邻激光点构成的直线与激光雷达和其中一个激光点构成的连线之间的夹角β；
若β值大于设定的角度阈值，则认为相邻的两个点云数据处于同一物体上，否则认为是处于两个物体；
扫描点云数据中所有相邻点云数据，将点云数据进行聚类分割，对于提取物体，若打在该物体上的点云数据的数目小于设定的阈值，则认为该物体为噪声，将其剔除。
进一步地，所述步骤3中从聚类分割处理后的每帧点云数据中提取特征点信息的步骤包括：
从聚类分割处理后的每帧点云数据中提取特征点，并根据各点云数据在三维空间中的曲度，将聚类分割处理后每帧点云数据中的特征点分为点特征点和面特征点；
其中，对于任一特征点，当对应点云数据的曲度大于等于预设的平滑度阈值时，将该特征点作为点特征点；当对应点云数据的曲度小于预设的平滑度阈值时，将该特征点作为面特征点；所述点特征点和面特征点构成了聚类分割处理后每帧点云数据中的特征点信息。
进一步地，所述步骤3中相邻两帧点云数据中提取的特征点信息进行匹配步骤包括：采用ICP(Iterative Closest Point，最近邻匹配)算法或NDT(Normal DistributionsTransform，正态分布变换)算法或RANSAC(RANdom SAmple Consensus，随机抽样一致)方法。
进一步地，所述步骤4中将当前帧的点云数据与三角网格地图进行匹配，得到目标在所述三角网格地图中的当前位姿信息步骤包括：
根据所述目标的当前位置从三角网格地图中按照距离约束取出预设范围内的三角形网格，并将所述目标的当前位置转换为地图坐标系下，以利用所述地图坐标系计算出所述激光雷达发射出的激光的方向；
计算所述激光雷达发射出的激光是否与取出的三角形网格相交，如果相交则记录下对应的交点位置，构成与所述三角网格地图匹配的约束点云数据；
对所述约束点云数据进行匹配，得到所述目标在所述三角网格地图中的当前位姿信息。
本发明还提供了一种同时定位建图装置，包括处理器以及存储器，所述存储器上存储有计算机程序，所述计算机程序被所述处理器执行时，实现同时定位建图方法。
本发明还提供了一种同时定位建图系统，包括同时定位建图装置，还包括激光雷达、IMU传感器；
所述同时定位建图装置、激光雷达、IMU传感器均安装于目标上，所述激光雷达、IMU传感器分别与所述同时定位建图装置电连接；
所述激光雷达用于采集三维的点云数据，并将所述点云数据发送至所述同时定位建图装置；
所述IMU传感器用于采集目标的IMU数据，并将所述IMU数据发送至所述同时定位建图装置。
本发明还提供了一种计算机存储介质，其上存储有计算机程序，所述计算机该程序被处理器执行时，实现一种同时定位建图方法。
与现有技术相比，本发明具有如下技术效果：
(1)使用三角网格地图建立地图，能够更加直接的展示空间中物体的形状以及曲面信息，对不同物体的特征信息都可以得到统一的提取；相比与传统的基于点云的地图存储方法，基于三角网格的地图存储方法具有空间分辨率高、占用空间少、易于可视化等优势；
(2)相比与传统的点云地图，基于三角网格的地图存储空间消耗更少，理论上可将存储空间减少一半以上；并且，通过三维曲面重建过程可以有效的滤除原始点云数据中的噪声点，提高了对有用信息的提取率，从而节省存储空间；
(3)通过基于贝叶斯树的三角网格地图局部更新方法，可以从全局地图中动态的加载当前局部地图；节省算法运行时占用的内存空间，加速运行速度；
(4)使用光线追踪进行点云与建图匹配可以更好地利用空间中的平面信息，并且光线追踪技术可以通过利用GPU中的SHADER进行加速，在激光雷达里程计的精度以及运行速度两个方面都具有显著性的优势。
附图说明
图1为本发明方法的流程图；
图2为本发明系统的示意性框图；
图3为本发明激光雷达与IMU传感器关系示意图；
图4为本发明激光雷达点云与深度图的关系；
图5中(a)(b)为本发明聚类分割算法示意图；
图6为本发明因子图中变量与约束组织形式示意图。
具体实施方式
以下结合附图对本发明的原理和特征进行描述，所举实例只用于解释本发明，并非用于限定本发明的范围。
实施例1
图1是本发明的同时定位建图方法的流程图。如图1所示，本发明的第一实施例提供的一种同时定位建图方法，包括以下步骤：
步骤1.根据激光雷达发送点云数据的频率对IMU传感器接收到的IMU数据进行预处理，得到IMU预积分数据；其中，激光雷达和IMU传感器均安装在目标上。
对IMU传感器接收到的IMU数据进行预处理，即对接收到的IMU数据中的角速度以及线加速度通过流形积分处理，通过积分给定时间段的IMU数据，获得目标的位姿变化，利用得到的所述位姿变化作为初始位姿。
IMU数据与激光雷达发送的点云数据的对应关系如图3所示，由于IMU传感器发送IMU数据的频率相对激光雷达发送的点云数据的频率高出10倍，例如，IMU传感器的工作频率为100Hz，而常见的激光雷达的工作频率为10Hz，通过对IMU数据进行预积分处理可避免因子数量过多的问题，可提高系统运行速度。
步骤2.将激光雷达接收到的三维的点云数据转换为二维的深度图像。
可以通过现有技术实现将接收到的三维点云数据转换为二维深度图像这一过程，下面做个简单介绍。首先对激光雷达数据利用体素滤波器进行下采样，然后基于激光雷达的工作模式，可以将由激光雷达获取的三维点云数据转换为二维深度图像。例如，16线激光雷达在xy平面上，间隔一定角度发射一系列激光束，对上述xy平面的一系列激光束按照固定的间隔，向不同的方向发射。其中第i系列激光束中的第j个激光束对应为二维深度图中第[i,j]个值，其值等于该束激光打到的点到激光雷达原点的距离。激光雷达的扫描点与深度图关系可以参照图4所示，其中每一行代表激光扫描一圈的点云，每一列代表激光在垂直地面方向上采集到的点，深度图的值代表该点到原点的距离值。通过深度图可以存储点云之间的相对位置关系，为后续算法提取曲度信息提供邻近点的相关信息。采用深度图的方法可以提供对邻居节点信息的直接访问。深度图的行数为2π/激光雷达横向解析率，列数为2π/激光雷达纵向解析率。
步骤3.利用所述IMU预积分数据与所述深度图像，获得相邻两帧点云数据之间所述目标的移动位姿信息。
这一步骤包括以下具体实施过程：
步骤3-1.对所述深度图像中的每帧点云数据进行聚类分割处理；
步骤3-2.从聚类分割处理后的每帧点云数据中提取特征点信息；
步骤3-3.以所述IMU预积分数据为初始值，将从相邻两帧点云数据中提取的特征点信息进行匹配，获得相邻两帧点云数据之间所述目标的移动位姿信息。
其中，对于步骤3-1中对所述深度图像中的每帧点云数据进行聚类分割处理的步骤包括：
对于任意两个相邻的点云数据，计算对应两个相邻激光点构成的直线与激光雷达和其中一个激光点构成的连线之间的夹角β；
若β值大于设定的角度阈值，则认为相邻的两个点云数据处于同一物体上，否则认为是处于两个物体上；
扫描点云数据中所有相邻点云数据，将点云数据进行聚类分割，对于提取物体，若打在该物体上的点云数据的数目小于设定的阈值，则认为该物体为噪声，将其剔除。
具体而言，对于任意两个相邻的点云数据，计算夹角β：
其中，点A和点B为相邻的两个激光点，β为点A和点B点构成平面法向量与以激光雷达O为圆心在A点的切线方向的夹角，若β值大于设定的角度阈值，则认为相邻的两个点云数据处于同一物体上，否则认为是处于两个物体，遍历扫描所述激光雷达获取到的点云数据集合中所有点云数据，当处于一个物体上的点云数据的数目小于预设的阈值，则认为该物体为噪声而将该物体剔除，进而提取出地面点和物体，由此实现点云数据的聚类分割。将点云数据进行聚类，可以剔除噪声，减少噪声的影响。
举例来说，对于任意两个相邻的激光点云数据，首先假设如果相邻的两个激光点云数据打在同一物体表面上，则其两点构成线段的切线方向应指向激光雷达。如附图5所示，其中点A，点B为相邻的两个激光点云，点O为激光雷达，计算夹角β。
若夹角β值大于设定的角度阈值，则认为相邻的两个激光点云数据处于同一物体上，否则认为相邻的两个激光点云数据是处于两个物体上。当该夹角值较小时，表示这个物体的平面是垂直于激光雷达的，其相对激光雷达的投影面积很小，此时激光雷达正好扫描到该平面的概率很低，可以认为这是两个点处在不同未知的物体导致的结果，即这两个点处于不同的物体上。该角度阈值一般根据激光雷达的扫描精度(角度间隔)进行确定，例如16线激光雷达的角度阈值一般设定为5-10°。
通过上述方法，扫描点云数据集合中所有相邻点云数据，将点云数据进行聚类分割。对于提取物体，若打在该物体上的激光点云的数目小于预设的阈值，则认为该物体为噪声，将其剔除，从而实现对点云的聚类分割的目的。基于实践经验，所述预设的阈值为5个点，打在某一物体上的激光点云的数目小于5，可以认为该物体为噪声，这种聚类物体可能是树叶、鸟、或者其他一些小物体或者噪声产生的结果，这些物体在下一帧的点云中并不稳定，即不一定能稳定的观察到，因此不能依靠这些物体来估计位姿，需要将这些物体剔除掉，这样一来可以减少噪声带来的不利影响。
所述步骤3-2中从聚类分割处理后的每帧点云数据中提取特征点信息的步骤包括：
从聚类分割处理后的每帧点云数据中提取特征点，并根据各点云数据在三维空间中的曲度，将聚类分割处理后点云数据中的特征点分为点特征点和面特征点；
其中，对于任一特征点，当对应点云数据的曲度大于等于预设的平滑度阈值时，将该特征点作为点特征点；当对应点云数据的曲度小于预设的平滑度阈值时，将该特征点作为面特征点；所述点特征点和面特征点构成了聚类分割处理后点云数据中的特征点信息。
具体的，曲度C的计算公式如下：
其中，ri为t时刻的点云中的一个点，rj为在点ri的沿着同一激光束方向的与该点ri前后相邻的点，j∈(0，n)组成集合S，其中|S|为集合中点云的数量，|ri|代表第i个点云的2范数，即该第i个点云到原点的欧式距离。
预设一个平滑度阈值Cth，当C≥Cth时，该点ri是点特征点；当C＜Cth时，该点ri是面特征点；所述点特征点和面特征点构成了聚类分割处理后点云数据中的特征点信息。
为了进一步加速算法计算，提高定位精度，本发明采用从转换的深度图像中提取特征点的方法来实现提高定位精度。针对该提取特征点的方法，具体而言，根据点云数据在三维空间中的曲度大小，可以按照曲度将点云进行排序，并将聚类分割处理后每帧点云数据中的特征点分为点特征点和面特征点。根据上述用曲度C进行判断的方法，将聚类分割处理后每帧点云数据中的特征点分解为点特征点和面特征点，加快了算法计算速度。
为了提供连续的里程计信息，步骤3-3中，以IMU预积分数据为初始值，角速度以及线加速度流形积分处理后的数据为初始值，将两帧提取出来的特征点信息进行匹配，以获得两帧点云数据之间目标的移动位姿信息，这里所述的目标的移动位姿信息指的是装载有IMU传感器的目标本体在当前坐标系中的坐标，可采用ICP算法或NDT算法或RANSAC方法进行两帧提取出来的特征点信息进行匹配。
ICP方法称为最近邻匹配算法，该方法通过对当前帧点云中每一个点寻找在上一帧中距离该点最近的点作为邻居点。计算当前点和邻居点的距离作为残差，构建最小二乘问题通过优化两帧点云之间的相对位姿关系使得当前点与邻居点的距离最小。
NDT方法称为正态分布变换算法，是基于正交概率分布变换的点云匹配算法，该方法首先通过正交概率分布变换算法将上一帧点云转变为由正态分布描述的概率分布集合，其中每个正态分布代表该位置上存在点(物体)的概率，通过优化两帧点云之间的相对位姿使得当前帧所有点落在物体上的概率最大，从而计算相对位姿。该NDT方法实际是在ICP方法上改良而来的，拥有更好的定位精度以及旋转容忍度，由于其要对点云进行NDT变换，如果上一帧点云保持不变时，算法运行速度相较ICP有显著的提升。但是如果上一帧点云持续改变时，由于其要不断地进行NDT方法，运行速度会大大降低。
RANSAC为随机抽样一致算法，它是根据一组包含异常数据的样本数据集，计算出数据的数学模型参数，得到有效样本数据的算法。
步骤4.以所述目标的移动位姿信息为初始值，将当前帧的点云数据与三角网格地图进行匹配，得到目标在所述三角网格地图中的当前位姿信息，完成定位。
三角网格地图是由第一帧点云数据生成的，后续随着每一帧不断完善和更新这个地图。由于地图为三角网格的形式，可以通过采用光束追踪的方法进行匹配。这一步骤包括以下具体实施过程：
步骤4-1.根据所述目标的当前位置从三角网格地图中按照距离约束取出预设范围内的三角形网格，并将所述目标的当前位置转换为地图坐标系下，以利用所述地图坐标系计算出所述激光雷达发射出的激光的方向；在这里，所述的一定范围可以是例如50米，即以所获得的目标的移动位姿信息作为初始值，在三角网格地图中按照距离约束取出离该移动目标位置50米范围内的三角网格数据。
所述位置坐标的转换过程是根据当前位置的里程计坐标系(ODOM坐标系)和地图坐标系(MAP坐标系)的坐标转换关系，通过矩阵和四元数运算可得。其中ODOM和MAP的相对位置关系可以通过基于深度学习的重定位方法或者基于全球导航卫星系统(GNSS)的定位系统提供的初始值来确定。可以将初始值设置为原点，此时建立的地图坐标系原点就是机器人启动时所处的位置。
为了在真实地图与建立的地图之间建立联系，需要获得机器人启动时在真实地图中的位置，该位置称为初始重定位。
在可以使用GPS时，初始位置可以通过GPS获得的信息来确定。
当无法使用GPS时，可以通过描述子匹配或者深度学习的方法获得机器人的位置信息。
步骤4-2.计算所述激光雷达发射出的激光是否与取出的三角形网格相交，如果相交则记录下对应的交点位置，构成与所述三角网格地图匹配的约束点云数据。
根据激光雷达的横向解析率和纵向解析率计算出激光雷达发射出的激光的方向，计算如下：
其中r(t)表示一束激光射线的传播方向，
为激光雷达在地图坐标系中的初始位置，t为激光已经传播的时间，
为激光的方向。
其中，
由纵向和横向的解析率计算而得。激光雷达的纵向解析率和横向解析率是由激光雷达厂家中控制激光源转动速度和发射频率决定的，属于出厂已知量。
对所取出的一定范围内的三角网格数据计算激光雷达发射出的点是否与三角网格相交，计算如下：
其中，p0、p1、p2为三角形三个顶点的坐标，通过解该方程组即可求出t、b1、b2，且仅当满足下述条件时求出的点为交点：
通过带入
求出交点位置，记录所有交点的位置，构成了与三角网格地图匹配的约束点云数据。
步骤4-3.对所述约束点云数据进行匹配，得到所述目标在所述三角网格地图中的当前位姿信息。
使用ICP的方法对计算出的所有约束点云数据进行匹配，优化出所述目标在所述三角网格地图中的当前位姿信息。
步骤5.基于当前帧的点云数据和目标的当前位姿信息，利用三维点云重建技术生成与所述目标的当前位置周围预设范围对应的三角形网格，并更新到所述三角网格地图中，完成建图。
三维点云重建技术可采用泊松曲面重建技术。泊松曲面重建技术的核心思想是通过将物体表面的离散样本点信息转化到连续(即可积分)表面函数上，从而构造出连续的隐式表面。这里预设的三角形网格范围可以是50米，利用三维点云重建技术生成与目标的当前位置周围50米范围对应的三角形网格，并以当前帧的点云数据和目标的当前位姿为基准，与预设的50米三角网格进行匹配，将重建的三角网格更新到三角网格地图中。对于在三角网格地图中重复存在的部分，利用网格正则化技术，对三维网格进行重新优化，重新优化结果要使得网格顶点的度数尽量均匀，网格顶点分布光滑，使重建的新网格更加逼近原网格，并记录更新的三角网格地图，完成建图。
步骤6：采用多约束因子图算法，将所述IMU预积分数据和所述点云数据作为因子，添加至因子图中，并执行因子图优化，以对所述目标的当前位姿信息和更新后的所述三角网格地图进行全局优化。
因子图是一种用于描述基于最大后验的最小二乘问题的图模型。其将最小二乘问题中的残差项描述为因子图中的因子，待优化的变量描述为节点。这样最终求解的问题转为求最大后验概率估计XMAP的解。
构建的因子图如图6所示，将从点云数据中提取的点、面特征以及IMU预积分构建的残差放到同一个因子图中进行优化。
对于t时刻采集到的点特征点集合
中的点
在上一帧特征点集合
中寻找距离最近的点
以及与
不在同一列的最近点
构建点到线的ICP约束：
对于t时刻采集到面特征点集合
中的点
在上一帧特征点集合
中寻找距离最近的两个点
在找到距离点
最近的不同列点
构建点到面的ICP约束：
设机器人位姿待优化变量为式T＝[R,p]中旋转矩阵和平移，将变换后位姿记为
变换关系为：
则对应约束量为T的函数：
则非线性优化的目标函数为：
IMU预积分构建的残差：
其中，对机器人的位姿估计来自于根据预积分结果，计算公式为：
式中，
为k时刻采样得到的角加速度测量值，
为k时刻采样得到的线加速度测量值，
和
为传感器的偏置，
和
为传感器噪声，g为重力加速度常量。
因此，最大后验概率估计XMAP：
其中φi(Xi)为约束因子，包括预积分约束因子和帧间匹配约束因子。帧间匹配约束因子由点云匹配约束残差构成，预积分约束因子由IMU预积分残差构成，记为：
通过使用高斯牛顿法或L-M(Levenberg-Marquardt，列文伯格-马夸尔特)方法求解最小二乘问题即可估计目标的相对位姿，根据计算出来的相对位姿，以对所述目标的当前位姿信息和更新后的所述三角网格地图进行全局优化。
为了克服累计误差,可添加回环检测约束，参照图6。具体的，为了消除累计误差，当目标移动到曾经经过的场景时提供额外的回环检测约束；可使用基于几何信息的激光点云回环检测方法获得目标在地图中的近似位姿，从地图中提取周围10平方米的点云数据与当前激光雷达点云数据使用ICP方式进行配准，获得额外的优化约束。
步骤7：将每一帧点云数据中目标的当前位姿信息与更新后的所述三角网格地图对应存储，并生成贝叶斯树。
当某些位姿信息发生改变时，通过所述贝叶斯树结构找到需要更改的节点，将与该节点邻近的三角网格地图转变为点云地图，通过体素滤波器对该点云地图上的点云数据进行下采样，然后再对下采样后的点云数据重新进行三角网格化处理，实现三角网格地图的局部更新，从而不需要加载所有地图，加快对地图的更新速度。
实施例2
本发明实施例2提供了一种同时定位建图装置，包括处理器以及存储器，所述存储器上存储有计算机程序，所述计算机程序被所述处理器执行时，实现实施例1提供的同时定位建图方法。
本发明实施例提供的同时定位建图装置，用于实现同时定位建图方法，因此，同时定位建图方法所具备的技术效果，同时定位建图装置同样具备，在此不再赘述。
实施例3
本发明还提供了一种同时定位建图系统，包括实施例2提供的同时定位建图装置，还包括激光雷达、IMU传感器；所述同时定位建图装置、激光雷达、IMU传感器均安装于目标上，所述激光雷达、IMU传感器分别与所述同时定位建图装置电连接；
所述激光雷达用于采集三维的点云数据，并将所述点云数据发送至所述同时定位建图装置；
所述IMU传感器用于采集目标的惯性数据，并将所述惯性数据发送至所述同时定位建图装置。
实施例4
本发明的实施例4提供了一种计算机存储介质，其上存储有计算机程序，其特征在于，所述计算机该程序被处理器执行时，实现实施例1同时定位建图方法。
本发明实施例提供的计算机存储介质，用于实现同时定位建图方法，因此，同时定位建图方法所具备的技术效果，计算机存储介质同样具备，在此不再赘述。
以上所述仅为本发明的较佳实施例，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。
Claims (11)
1.一种同时定位建图方法，其特征在于，包括以下步骤：
步骤1.根据激光雷达发送点云数据的频率对IMU传感器接收到的IMU数据进行预处理，得到IMU预积分数据；其中，激光雷达和IMU传感器均安装在目标上；
步骤2.将激光雷达接收到的三维的点云数据转换为二维的深度图像；
步骤3.利用所述IMU预积分数据与所述深度图像，获得相邻两帧点云数据之间目标的移动位姿信息；
步骤4.以所述目标的移动位姿信息为初始值，将当前帧的点云数据与三角网格地图进行匹配，得到目标在所述三角网格地图中的当前位姿信息，完成定位；
步骤5.基于当前帧的点云数据和目标的当前位姿信息，利用三维点云重建技术生成与所述目标的当前位置周围预设范围对应的三角形网格，并更新到所述三角网格地图中，完成建图。
2.根据权利要求1所述的一种同时定位建图方法，其特征在于，所述步骤5之后，还包括步骤6：采用多约束因子图算法，将所述IMU预积分数据和所述点云数据作为因子，添加至因子图中，并执行因子图优化，以对所述目标的当前位姿信息和更新后的所述三角网格地图进行全局优化。
3.根据权利要求2所述的一种同时定位建图方法，其特征在于，所述步骤6之后，还包括步骤7：将每一帧点云数据中目标的当前位姿信息与更新后的所述三角网格地图对应存储，并生成贝叶斯树。
4.根据权利要求1所述的一种同时定位建图方法，其特征在于，所述步骤3包括：
对所述深度图像中的每帧点云数据进行聚类分割处理；
从聚类分割处理后的每帧点云数据中提取特征点信息；
以所述IMU预积分数据为初始值，将从相邻两帧点云数据中提取的特征点信息进行匹配，获得相邻两帧点云数据之间目标的移动位姿信息。
5.根据权利要求4所述的一种同时定位建图方法，其特征在于，所述步骤3中对所述深度图像中的每帧点云数据进行聚类分割处理的步骤包括：
对于任意两个相邻的点云数据，计算对应两个相邻激光点构成的直线与激光雷达和其中一个激光点构成的连线之间的夹角β；
若β值大于设定的角度阈值，则认为相邻的两个点云数据处于同一物体上，否则认为是处于两个物体；
扫描点云数据中所有相邻点云数据，将点云数据进行聚类分割，对于提取物体，若打在该物体上的点云数据的数目小于设定的阈值，则认为该物体为噪声，将其剔除。
6.根据权利要求5所述的一种同时定位建图方法，其特征在于，所述步骤3中从聚类分割处理后的每帧点云数据中提取特征点信息的步骤包括：
从聚类分割处理后的每帧点云数据中提取特征点，并根据各点云数据在三维空间中的曲度，将聚类分割处理后每帧点云数据中的特征点分为点特征点和面特征点；
其中，对于任一特征点，当对应点云数据的曲度大于等于预设的平滑度阈值时，将该特征点作为点特征点；当对应点云数据的曲度小于预设的平滑度阈值时，将该特征点作为面特征点；所述点特征点和面特征点构成了聚类分割处理后每帧点云数据中的特征点信息。
7.根据权利要求6所述的一种同时定位建图方法，其特征在于，所述步骤3中相邻两帧点云数据中提取的特征点信息进行匹配步骤包括：采用ICP算法或NDT算法或RANSAC方法。
8.根据权利要求1所述的一种同时定位建图方法，其特征在于，所述步骤4中将当前帧的点云数据与三角网格地图进行匹配，得到目标在所述三角网格地图中的当前位姿信息步骤包括：
根据所述目标的当前位置从三角网格地图中按照距离约束取出预设范围内的三角形网格，并将所述目标的当前位置转换为地图坐标系下，以利用所述地图坐标系计算出所述激光雷达发射出的激光的方向；
计算所述激光雷达发射出的激光是否与取出的三角形网格相交，如果相交则记录下对应的交点位置，构成与所述三角网格地图匹配的约束点云数据；
对所述约束点云数据进行匹配，得到所述目标在所述三角网格地图中的当前位姿信息。
9.一种同时定位建图装置，其特征在于，包括处理器以及存储器，所述存储器上存储有计算机程序，所述计算机程序被所述处理器执行时，实现如权利要求1-8任一所述的同时定位建图方法。
10.一种同时定位建图系统，其特征在于，包括如权利要求9所述的同时定位建图装置，还包括激光雷达、IMU传感器；
所述同时定位建图装置、激光雷达、IMU传感器均安装于目标上，所述激光雷达、IMU传感器分别与所述同时定位建图装置电连接；
所述激光雷达用于采集三维的点云数据，并将所述点云数据发送至所述同时定位建图装置；
所述IMU传感器用于采集目标的IMU数据，并将所述IMU数据发送至所述同时定位建图装置。
11.一种计算机存储介质，其特征在于，其上存储有计算机程序，所述计算机该程序被处理器执行时，实现如权利要求1-8任一所述的一种同时定位建图方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210807129.8A CN115131514A (zh) | 2022-07-12 | 2022-07-12 | 一种同时定位建图的方法、装置、系统及存储介质 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210807129.8A CN115131514A (zh) | 2022-07-12 | 2022-07-12 | 一种同时定位建图的方法、装置、系统及存储介质 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN115131514A true CN115131514A (zh) | 2022-09-30 |
Family
ID=83382827
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210807129.8A Pending CN115131514A (zh) | 2022-07-12 | 2022-07-12 | 一种同时定位建图的方法、装置、系统及存储介质 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115131514A (zh) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115575923A (zh) * | 2022-12-08 | 2023-01-06 | 千巡科技(深圳)有限公司 | 一种地面机器人静止判断方法、系统、装置以及存储介质 |
| CN116152151A (zh) * | 2022-11-12 | 2023-05-23 | 重庆数字城市科技有限公司 | 一种建筑变形信息提取方法及系统 |
-
2022
- 2022-07-12 CN CN202210807129.8A patent/CN115131514A/zh active Pending
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116152151A (zh) * | 2022-11-12 | 2023-05-23 | 重庆数字城市科技有限公司 | 一种建筑变形信息提取方法及系统 |
| CN115575923A (zh) * | 2022-12-08 | 2023-01-06 | 千巡科技(深圳)有限公司 | 一种地面机器人静止判断方法、系统、装置以及存储介质 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109186608B (zh) | 一种面向重定位的稀疏化三维点云地图生成方法 | |
| CN107301654B (zh) | 一种多传感器的高精度即时定位与建图方法 | |
| CN108564616B (zh) | 快速鲁棒的rgb-d室内三维场景重建方法 | |
| CN113436260B (zh) | 基于多传感器紧耦合的移动机器人位姿估计方法和系统 | |
| Hähnel et al. | Learning compact 3D models of indoor and outdoor environments with a mobile robot | |
| CN115131514A (zh) | 一种同时定位建图的方法、装置、系统及存储介质 | |
| CN109522832B (zh) | 基于点云片段匹配约束和轨迹漂移优化的回环检测方法 | |
| CN113432600A (zh) | 基于多信息源的机器人即时定位与地图构建方法及系统 | |
| CN112923933A (zh) | 一种激光雷达slam算法与惯导融合定位的方法 | |
| CN113781582A (zh) | 基于激光雷达和惯导联合标定的同步定位与地图创建方法 | |
| CN115372989A (zh) | 基于激光雷达的越野自动小车长距离实时定位系统及方法 | |
| WO2021021862A1 (en) | Mapping and localization system for autonomous vehicles | |
| CN113674412B (zh) | 基于位姿融合优化的室内地图构建方法、系统及存储介质 | |
| CN112327326A (zh) | 带有障碍物三维信息的二维地图生成方法、系统以及终端 | |
| CN113587933A (zh) | 一种基于分支定界算法的室内移动机器人定位方法 | |
| CN111739066B (zh) | 一种基于高斯过程的视觉定位方法、系统及存储介质 | |
| CN114066773B (zh) | 一种基于点云特征与蒙特卡洛扩展法的动态物体去除 | |
| CN113741523B (zh) | 一种基于边界和采样的混合无人机自主探测方法 | |
| JP2022080303A (ja) | オプティカルフローを用いたライダー位置推定 | |
| CN113487631A (zh) | 基于lego-loam的可调式大角度探测感知及控制方法 | |
| CN117053779A (zh) | 一种基于冗余关键帧去除的紧耦合激光slam方法及装置 | |
| CN116679307A (zh) | 基于三维激光雷达的城市轨道交通巡检机器人定位方法 | |
| Li et al. | An SLAM algorithm based on laser radar and vision fusion with loop detection optimization | |
| CN114998539A (zh) | 一种智慧城市传感器地形定位建图方法 | |
| Wu et al. | Robust Map Merging Method for Collaborative LiDAR-based SLAM Using GPS Sensor |
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 |