CN115355904A - 一种针对地面移动机器人的Lidar-IMU融合的slam方法 - Google Patents

一种针对地面移动机器人的Lidar-IMU融合的slam方法 Download PDF

Info

Publication number
CN115355904A
CN115355904A CN202210876326.5A CN202210876326A CN115355904A CN 115355904 A CN115355904 A CN 115355904A CN 202210876326 A CN202210876326 A CN 202210876326A CN 115355904 A CN115355904 A CN 115355904A
Authority
CN
China
Prior art keywords
map
frame
sub
pose
key frame
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
Application number
CN202210876326.5A
Other languages
English (en)
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.)
Zhejiang Lab
Original Assignee
Zhejiang Lab
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 Zhejiang Lab filed Critical Zhejiang Lab
Priority to CN202210876326.5A priority Critical patent/CN115355904A/zh
Publication of CN115355904A publication Critical patent/CN115355904A/zh
Pending legal-status Critical Current

Links

Images

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/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • 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
    • 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
    • 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/93—Lidar systems specially adapted for specific applications for anti-collision purposes
    • 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/20—Image preprocessing
    • G06V10/26—Segmentation of patterns in the image field; Cutting or merging of image elements to establish the pattern region, e.g. clustering-based techniques; Detection of occlusion
    • 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/40—Extraction of image or video features
    • G06V10/44—Local feature extraction by analysis of parts of the pattern, e.g. by detecting edges, contours, loops, corners, strokes or intersections; Connectivity analysis, e.g. of connected components
    • 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

Abstract

本发明属于同步定位与建图(slam)领域，公开了一种针对地面移动机器人的Lidar‑IMU融合的slam方法，包括如下步骤：步骤1：数据预处理；步骤2：构建子地图；步骤3：创建地图与优化因子图；步骤4：优化位姿图。本发明能够实现在有起伏路面上为机器人添加地面约束，增加竖直方向的约束，能够很好的减少累积误差，整个实验机器人轨迹长大约1000m，与传统算法相比本发明的方法在精度上有很大的提高，特别是在z方向更为明显。其中loam的平移偏差(均方根误差表示)为18.461m，lio_sam的平移偏差为15.672m，而本发明的算法平移偏差为8.331m。该算法得到的轨迹精度有了很大的提高。

Description

一种针对地面移动机器人的Lidar-IMU融合的slam方法
技术领域
本发明属于同步定位与建图(slam)领域，尤其涉及一种针对地面移动机器人的Lidar-IMU融合的slam方法。
背景技术
同时定位与环境建图是机器人自主导航的前提。近年来随着机器人产业的发展，状态估计、定位和建图是智能移动机器人反馈控制、避障和规划等能力的基本先决条件。在实际应用中，一般采用视觉传感器或激光雷达传感器实现机器人高性能的同步定位与建图功能。基于视觉的方法通常使用单目或立体相机，并在连续图像上对特征进行三角化，以确定相机的运动。尽管基于视觉的方法特别适合于位置识别，但它们对初始化、光照和距离的敏感性使得它们很难单独用于自主导航系统。另一方面，基于激光雷达的方法不受光照变化的影响。特别是最近，随着远程、高分辨率3D激光雷达的出现，以及固态激光雷达的应用，激光雷达变得更适合直接捕捉3D空间中环境的细节。
在过去的二十年中，人们提出了许多基于激光雷达的状态估计和建图的方法。其中，LOAM算法适用于低漂移和实时状态估计和建图的方法，是应用最广泛的方法之一。该方法使用激光雷达和惯性测量单元(IMU)，采用点到线，点到面的约束实现了很好的性能，但IMU只是为了点云去畸变，并没有参与优化。而在Lio_sam中，该算法利用因子图优化实现了IMU和激光雷达的紧融合。这些传统的算法虽然取得了很大的成功，但在大场景并且GPS拒止的情况下，会产生很大的漂移。因为这些大都是没有区分的在点云中提取特征点作为匹配点云，不考虑地面约束。而对于地面移动机器人来说，其特征点主要来自于周围物体的反馈，这些特征点只能约束水方向上的维度，竖直方向只能从地面上提取，这就造成竖直方向的约束不够，在室外大场景下很容易产生漂移。有些算法虽然考虑了地面约束，但只是简单的将地面看成一个平面来添加地面约束。在现实环境中机器人不可能始终运行在同一个地平面上。
不同于一般的6自由度飞行器，地面运动的机器人始终会受到来自地面的约束。如果直接按照传统的SE(3)位姿进行状态估计，由于激光雷达主要采集路面以上的激光点，这就造成点云匹配优化过程中z方向很容易产生较大的漂移。同时，在有起伏的路面上，地面移动机器人虽然受到地面约束，但此时机器人可能会受到不同的地平面约束，如果我们假设机器人始终运行在同一个地平面，并以此添加地面约束，这会影响状态估计的精度。
发明内容
本发明目的在于提供一种针对地面移动机器人的Lidar-IMU融合的slam方法,以解决上述的技术问题。
为解决上述技术问题，本发明的一种针对地面移动机器人的Lidar-IMU融合的slam方法的具体技术方案如下：
一种针对地面移动机器人的Lidar-IMU融合的slam方法，包括如下步骤：
步骤1：数据预处理；包括点云去畸变和特征提取；
步骤2：关键帧选择与子地图构建；引入视觉slam中的关键帧概念，使用两个关键帧之间的激光雷达信息来构造局部子地图；
步骤3：关键帧的因子图优化；通过因子图优化将关键帧与世界坐标系相关联，构建一个因子图来联合优化激光雷达关键帧和IMU的预积分；
步骤4：位姿图优化；通过位姿图优化得到一个全局一致的环境地图。
进一步地，所述步骤1包括点云去畸变和特征提取；
所述点云去畸变：在因子优化过程中，IMU的预积分提供预积分因子，而优化过程IMU的偏差也会作为优化参数参与优化，利用得到的IMU偏差纠正的IMU数据来估计任意时刻载体的位姿，将激光点变换到统一坐标系下，进行点云畸变校正。
所述特征提取：从去畸变的点云中提取特征点云，提取边点和表面点作为匹配的特征点，对于任何点p，从与p同一行扫描中找到10个连续点，用S表示，均分的位于p的两侧，则p的粗糙度可以通过以下公式计算：
Figure BDA0003762688950000021
式中|S|是S中点的个数，取10，ri表示第i个点的深度；
选择粗糙度值较小的点作为平面点，粗糙度值较大的点作为边缘点，则从任一激光雷达扫描得到的特征点表示为
Figure BDA0003762688950000031
Figure BDA0003762688950000032
表示平面点，
Figure BDA0003762688950000033
表示边缘点。
进一步地，所述步骤2包括关键帧的选择和子地图的构建；
所述关键帧的选择：引入了视觉slam中常用的关键帧概念，使用两个关键帧之间的激光雷达信息来构造局部子地图，假设地面移动机器人是一个刚体，相邻激光雷达扫描时刻的机器人本体坐标系的xy平面的变化可以反映地平面的变化，当新的激光雷达扫描到来时，将当前帧地面法线矢量e3转换到前一帧坐标系中e′3i，则相邻激光雷达扫描的地平面之间的角度为：
Figure BDA0003762688950000034
式中
Figure BDA0003762688950000035
是e3的转置矩阵；
如果θ大于设定的阈值θth，则认为地平面的斜率发生了变化，机器人不再与之前的扫描位于同一地平面上，此扫描将被选为关键帧；
所述子地图的构建：如果新的Lidar扫描为关键帧，则创建一个新的子地图，并将该关键帧作为子地图的参考帧，此时机器人的本体坐标系为子地图的坐标系，用Mi表示，然后，构建一个包括激光雷达里程计约束和地面约束的最大后验估计，优化得到后续的普通雷达帧相对于子地图的位姿变换；
当新的激光雷达普通帧Fj到来时，在预处理后获得其特征点
Figure BDA0003762688950000036
将其与所在子地图Mi相匹配，此时，本地子地图具有如下激光雷达帧：
Mi＝{Li,Fi,0,Fi,1…Fi,j-1}
式中Li表示子地图Mi的参考关键帧，Fi,k,k＝1,2,…j-1表示子地图中已有的普通帧，对于扫描匹配方法，使用与loam相同的方法，首先将此普通帧的特征点转换为其所在子地图中得到
Figure BDA0003762688950000037
初始变换是通过使用来自校正后的IMU的预测获得的，对于
Figure BDA0003762688950000038
中的每个特征点，在子地图中找到相应的边缘点和平面点，然后，最小化特征点到其相应边和平面的距离，以获得最最优的变换：
Figure BDA0003762688950000039
式中
Figure BDA0003762688950000041
Figure BDA0003762688950000042
Figure BDA0003762688950000043
Figure BDA0003762688950000044
式中
Figure BDA0003762688950000045
分别是当前帧的边缘特征点和面特征点，
Figure BDA0003762688950000046
是
Figure BDA0003762688950000047
转换到局部坐标系后得到的点，
Figure BDA0003762688950000048
是局部地图中与
Figure BDA0003762688950000049
对应的匹配边缘点，
Figure BDA00037626889500000410
是局部地图中与
Figure BDA00037626889500000411
对应的匹配面点；
子地图中的所有激光雷达帧都位于同一地平面上，因此，使用在SE(3)位姿-SE(2)约束的方法向每个激光雷达普通帧添加平面约束，理想情况下，在子地图中，机器人在平面中移动，将普通帧和子地图之间的变换参数化为SE(2)位姿，其李代数表示为v＝[v1 v2φ]，其中φ是旋转分量，(v1,v2)并表示平移分量；在实际环境中，由于机器人运动中的崎岖地形和机器人自身的晃动，机器人的姿态在SE(2)以外的维度上存在运动干扰，因此，不是直接在SE(2)上参数化机器人姿势，而是通过李代数将姿势从SE(2)恢复到SE(3)：
ξ＝[v1 v2 0 0 0 φ]T
同时，用高斯模型对其他三个维度的运动扰动的误差进行建模，表示
Figure BDA00037626889500000415
其他三维空间中允许的运动扰动振幅，这是由地形条件和机器人结构决定的；
因此，地平面约束的残差定义为：
Figure BDA00037626889500000412
其中，
Figure BDA00037626889500000413
是平面约束的测量值，可以通过将当前姿态估计值投影到SE(2)空间，然后再恢复到SE(3)来获得：
Figure BDA00037626889500000414
平面约束的雅可比矩阵和协方差矩阵为：
Figure BDA0003762688950000051
Figure BDA0003762688950000052
式中
Figure BDA0003762688950000053
分别是z方向，roll方向和pitch方向的机器人状态的方差。
进一步地，所述步骤2采用了一种启发式方法，用于机器人在同一地平面上移动太长时间而无法选择关键帧的情况，与前一帧的状态相比，此时机器人旋转或平移超过了定义的阈值，将此扫描视为关键帧，如果当前扫描不满足上述两个条件，则该扫描为普通帧，关键帧随后将用于与IMU联合优化，而普通帧则用于构建子地图。
进一步地，所述步骤3包括如下具体步骤：
通过因子图优化将关键帧与世界坐标系相关联，构建一个因子图来联合优化激光雷达关键帧和IMU的预积分，该因子图包括以下3种类型的因子：
激光雷达里程计因子：当新的关键帧Li到来时，经过预处理后，得到边点
Figure BDA0003762688950000054
和平面点
Figure BDA0003762688950000055
此时，将此新状态节点添加到因子图中，将此关键帧与前一个子地图Mi-1进行匹配，子地图包含的激光雷达帧如下：
Mi-1＝{Li-1,Fi-1,0,Fi-1,1,Fi-1,2…Fi-1,n}
其中n是子地图Mi-1中普通帧数目，匹配方法与步骤3所述方法相同，通过匹配得到相邻关键帧之间的相对位姿变换，此时新的关键帧位姿会作为节点，相对位姿作为因子加入了因子图中；
IMU预积分因子：IMU的测量模型如下：
Figure BDA0003762688950000056
Figure BDA0003762688950000057
IMU数据是在其本体坐标系的测量，并且会受到加性噪声na，nw、加速度偏差
Figure BDA0003762688950000058
以及陀螺仪偏差
Figure BDA0003762688950000059
的影响，将偏差建模为随机游走，其导数是高斯的，加速度计和陀螺仪的加法噪声被假定为高斯分布；
给出两个相邻关键帧之间的最终预积分形式：
Figure BDA00037626889500000510
Figure BDA0003762688950000061
Figure BDA0003762688950000062
式中
Figure BDA0003762688950000063
Figure BDA0003762688950000064
Figure BDA0003762688950000065
除了效率之外，IMU预积分的应用为因子图提供了一种相对位姿约束的因子；
回环因子：使用基于欧氏距离的回环检测方法，将欧式距离作为先验，将距离当前关键帧的最近且位姿的相对变换小于设定的阈值的关键帧作为候选帧，然后，将候选帧及其前后的n个关键帧及其相应的本地子地图作为当前关键帧的匹配点云，得到当前帧和候选帧之间的相对变换，该变换作为回环因子添加到因子图中。
进一步地，所述步骤4包括如下具体步骤：
基于滑动窗口的位姿优化用于将普通帧固定到世界坐标系下，并校正关键帧位姿以减少误差的累积，在构建局部子地图的过程中，得到普通帧和子地图的相对姿态，通过因子图优化，得到世界坐标系下关键帧的位姿，在滑动窗口中，普通帧的相对姿势和关键帧的绝对姿势，一起执行位姿图优化，在优化过程中，最早的关键帧或回环关键帧的位姿是固定的，边缘化时，直接删除最早进入位姿图优化的子地图中的关键帧和普通帧，最后，通过上述优化我们得到一个全局一致的环境地图。
本发明的一种针对地面移动机器人的Lidar-IMU融合的slam方法具有以下优点：本发明能够实现在有起伏路面上为机器人添加地面约束，增加竖直方向的约束，能够很好的减少累积误差，整个实验机器人轨迹长大约1000m，与传统算法相比本发明的方法在精度上有很大的提高，特别是在z方向更为明显。其中loam的平移偏差(均方根误差表示)为18.461m，lio_sam的平移偏差为15.672m，而本发明的算法平移偏差为8.331m。该算法得到的轨迹精度有了很大的提高。
附图说明
图1为本发明方法中Lidar-IMU的融合框架图；
图2为本发明方法中的三种优化方式的示意图；
图3为各种算法在同一个数据集上得到的轨迹在xy方向上的投影示意图；
图4为各种算法在同一个数据集上得到的轨迹在z方向上的投影示意图。
具体实施方式
为了更好地了解本发明的目的、结构及功能，下面结合附图，对本发明一种针对地面移动机器人的Lidar-IMU融合的slam方法做进一步详细的描述。
如图1所示，一种针对地面移动机器人的Lidar-IMU融合的slam方法分为4个部分。系统接收到激光雷达和IMU的测量数据后，首先要进入点云预处理模块，利用IMU估计的状态对点云去畸变，得到无畸变的点云。再通过投影深度图的方法分割点云，拟合地平面，并从分割后的点云中提取边缘特征点和面特征点作为匹配点云。然后，我们地面的变化将雷达扫描分为关键帧和普通帧，地面的变化我们可以通过IMU测量，一般我们认为地面移动机器人为刚体，可以通过IMU测得的前后两帧姿态变化得到两帧地平面的变化，若地平面的变化超过我们设定的阈值，此激光雷达帧就被选为关键帧否则为普通帧。若如果该扫描为普通帧，则进入局部地图构建模块。在该模块中我们会构建一个包括激光雷达帧间约束以及地面约束最大后验估计，优化得到当前帧与局部地图的变换关系并将该帧点云转换到其所在的局部地图中。若该扫描为关键帧，则进入局部地图创建与因子图优化模块。该模块主要做两件事，一是重新创建一个局部地图，该关键帧为新局部地图的参考帧，其本体坐标系为局部地图的坐标系，后续的普通帧通过最大后延估计转化到此局部地图中。同时，我们启动因子图优化模块，通过因子图优化得到关键帧在世界坐标系下的位姿，普通帧不参与此因子图优化。该因子图包括三种类型的因子：由两关键帧之间的IMU测量通过预积分技术得到的IMU预积分因子、通过点云匹配得到当前帧与前一帧的相对位姿构成的激光雷达里程计因子以及回环检测因子。至此，通过局部地图中的最大后验估计们得到普通帧与局部地图之间的相对位姿。通过因子图优化到关键帧位姿在世界坐标系下的位姿。最后，这两种位姿进入最后一个模块，即基于滑动窗口的位姿图优化模块。该模块利用位姿图优化上述位姿放在滑动窗口中进一步优化将普通帧关联到世界坐标系下得到全局一致的地图。在滑动优化过程中我们始终固定滑动窗口中最旧关键帧位姿。由于我们采用的是位姿优化，每帧位姿只与前后两帧的测量有关，边缘化时，我们可以直接丢掉最老帧的信息。该模块不仅将普通帧的位姿关联到了世界坐标系下，而且纠正了关键帧的位姿，减少误差的累积。
本发明的一种针对地面移动机器人的Lidar-IMU融合的slam方法，包括如下步骤：
1)数据预处理
点云去畸变：点云畸变是由于激光雷达采集数据的过程中，随着载体运动导致一帧点云中的点不是在同一时刻采集的，即不同激光点的坐标系不一致。在因子优化过程中，IMU的预积分提供预积分因子，而优化过程IMU的偏差也会作为优化参数参与优化，可以利用得到的IMU偏差纠正的IMU数据来估计任意时刻载体的位姿，将激光点变换到统一坐标系下，进行点云畸变校正。同时从IMU得到的位姿可以为帧间匹配提供很好的初始值。
特征提取：接下来，我们从去畸变的点云中提取特征点云。我们提取边点和表面点作为匹配的特征点。对于任何点p，我们从与p同一行扫描中找到10个连续点，用S表示，均分的位于p的两侧。则p的粗糙度可以通过以下公式计算：
Figure BDA0003762688950000081
式中|S|是S中点的个数，本发明取10，ri表示第i个点的深度。
我们选择粗糙度值较小的点作为平面点，粗糙度值较大的点作为边缘点。则从任一激光雷达扫描得到的特征点可以表示为
Figure BDA0003762688950000082
Figure BDA0003762688950000083
表示平面点，
Figure BDA0003762688950000084
表示边缘点。
2)关键帧的选择与子地图的构建
关键帧的选择：为了保证算法的实时性，我们引入了视觉slam中常用的关键帧概念。但不同之处在于，我们不是丢弃两个关键帧之间的激光雷达信息，而是使用它们来构造局部子地图。在算法中，我们假设地面移动机器人是一个刚体，所以相邻激光雷达扫描时刻的机器人本体坐标系的xy平面的变化可以反映地平面的变化。当新的激光雷达扫描到来时，我们将当前帧地面法线矢量e3转换到前一帧坐标系中e′3i，则相邻激光雷达扫描的地平面之间的角度为：
Figure BDA0003762688950000085
式中
Figure BDA0003762688950000086
是e3的转置矩阵。
如果θ大于我们设定的阈值θth，则认为地平面的斜率发生了变化。机器人不再与之前的扫描位于同一地平面上。此扫描将被选为关键帧。我们还采用了一种简单但有效的启发式方法，用于机器人在同一地平面上移动太长时间而无法选择关键帧的情况。与前一帧的状态相比，此时机器人旋转或平移超过了我们定义的阈值，我们将此扫描视为关键帧。如果当前扫描不满足上述两个条件，则该扫描为普通帧。如图2所示，关键帧随后将用于与IMU联合优化，而普通帧则用于构建子地图。
子地图的构建：如果新的Lidar扫描为关键帧，则我们需要创建一个新的子地图，并将该关键帧作为子地图的参考帧。此时机器人的本体坐标系为子地图的坐标系，用Mi表示，如图2所示。然后，我们构建一个包括激光雷达里程计约束和地面约束的最大后验估计，优化得到后续的普通雷达帧相对于子地图的位姿变换。
当新的激光雷达普通帧Fj到来时，我们在预处理后获得其特征点
Figure BDA0003762688950000091
我们将其与所在子地图Mi相匹配。此时，本地子地图具有如下激光雷达帧：
Mi＝{Li,Fi,0,Fi,1…Fi,j-1}
式中Li表示子地图Mi的参考关键帧，Fi,k,k＝1,2,…j-1表示子地图中已有的普通帧。对于扫描匹配方法，我们使用与loam相同的方法，因为它在各种具有挑战性的环境中具有计算效率和鲁棒性。我们首先将此普通帧的特征点转换为其所在子地图中得到
Figure BDA0003762688950000092
初始变换是通过使用来自校正后的IMU的预测获得的。对于
Figure BDA0003762688950000093
中的每个特征点，我们在子地图中找到相应的边缘点和平面点。然后，我们最小化特征点到其相应边和平面的距离，以获得最最优的变换：
Figure BDA0003762688950000094
式中
Figure BDA0003762688950000095
Figure BDA0003762688950000096
Figure BDA0003762688950000097
Figure BDA0003762688950000098
式中
Figure BDA0003762688950000099
分别是当前帧的边缘特征点和面特征点。
Figure BDA00037626889500000910
是
Figure BDA00037626889500000911
转换到局部坐标系后得到的点。
Figure BDA00037626889500000912
是局部地图中与
Figure BDA00037626889500000913
对应的匹配边缘点。
Figure BDA00037626889500000914
是局部地图中与
Figure BDA0003762688950000101
对应的匹配面点。
子地图中的所有激光雷达帧都位于同一地平面上。因此，我们使用在SE(3)位姿-SE(2)约束的方法向每个激光雷达普通帧添加平面约束。理想情况下，在子地图中，机器人在平面中移动。我们可以将普通帧和子地图之间的变换参数化为SE(2)位姿，其李代数是可以表示为v＝[v1 v2 φ]，其中φ是旋转分量，(v1,v2)并表示平移分量。然而，在实际环境中，由于机器人运动中的崎岖地形和机器人自身的晃动，机器人的姿态在SE(2)以外的维度上存在运动干扰。因此，我们不是直接在SE(2)上参数化机器人姿势，而是通过李代数将姿势从SE(2)恢复到SE(3)：
ξ＝[v1 v2 0 0 0 φ]T
同时，我们也没有忽略其他三个维度的运动扰动，而是用高斯模型对它们的误差进行建模，表示
Figure BDA0003762688950000102
其他三维空间中允许的运动扰动振幅，这是由地形条件和机器人结构决定的。
因此，地平面约束的残差定义为：
Figure BDA0003762688950000103
其中，
Figure BDA0003762688950000104
是平面约束的测量值，可以通过将当前姿态估计值投影到SE(2)空间，然后再恢复到SE(3)来获得：
Figure BDA0003762688950000105
平面约束的雅可比矩阵和协方差矩阵为：
Figure BDA0003762688950000106
Figure BDA0003762688950000107
式中
Figure BDA0003762688950000108
分别是z方向，roll方向和pitch方向的机器人状态的方差。
3)关键帧的因子图优化
本节通过因子图优化将关键帧与世界坐标系相关联。我们将构建一个因子图来联合优化激光雷达关键帧和IMU的预积分。该因子图包括以下3种类型的因子。
激光雷达里程计因子：当新的关键帧Li到来时，经过预处理后，我们得到边点
Figure BDA0003762688950000109
和平面点
Figure BDA00037626889500001010
此时，我们需要将此新状态节点添加到因子图中。我们将此关键帧与前一个子地图Mi-1(而不是全局地图)进行匹配。子地图包含的激光雷达帧如下：
Mi-1＝{Li-1,Fi-1,0,Fi-1,1,Fi-1,2…Fi-1,n}
其中n是子地图Mi-1中普通帧数目。匹配方法与前述方法相同。通过匹配我们得到相邻关键帧之间的相对位姿变换。此时新的关键帧位姿会作为节点，相对位姿作为因子加入了因子图中。
IMU预积分因子：IMU的测量模型如下：
Figure BDA0003762688950000111
Figure BDA0003762688950000112
IMU数据是在其本体坐标系的测量，并且会受到加性噪声na，nw、加速度偏差
Figure BDA0003762688950000113
以及陀螺仪偏差
Figure BDA0003762688950000114
的影响。一般来说，我们将偏差建模为随机游走，其导数是高斯的，加速度计和陀螺仪的加法噪声被假定为高斯分布。
这里，我们给出了两个相邻关键帧之间的最终预积分形式：
Figure BDA0003762688950000115
Figure BDA0003762688950000116
Figure BDA0003762688950000117
式中
Figure BDA0003762688950000118
Figure BDA0003762688950000119
Figure BDA00037626889500001110
除了效率之外，IMU预积分的应用也自然为因子图提供了一种相对位姿约束的因子。
回环因子：本发明使用基于欧氏距离的回环检测方法。我们将欧式距离作为先验，将距离当前关键帧的最近且位姿的相对变换小于我们设定的阈值的关键帧作为候选帧。然后，我们将候选帧及其前后的n个关键帧及其相应的本地子地图作为当前关键帧的匹配点云。使用前述方法，我们得到当前帧和候选帧之间的相对变换，该变换作为回环因子添加到因子图中。
4)位姿图优化
在本节中，基于滑动窗口的位姿优化用于将普通帧固定到世界坐标系下，并校正关键帧位姿以减少误差的累积。在构建局部子地图的过程中，我们得到普通帧和子地图的相对姿态。通过因子图优化，我们得到世界坐标系下关键帧的位姿。如图1和图2所示，在滑动窗口中，普通帧的相对姿势和关键帧的绝对姿势，一起执行位姿图优化。在优化过程中，最早的关键帧或回环关键帧的位姿是固定的。边缘化时，我们直接删除最早进入位姿图优化的子地图中的关键帧和普通帧，因为位子图优化仅与前后两帧相关。最后，通过上述优化我们得到一个全局一致的环境地图。
本发明能够实现在有起伏路面上为机器人添加地面约束，增加竖直方向的约束，能够很好的减少累积误差，图3，图4所示。整个实验机器人轨迹长大约1000m，与传统算法相比本发明的方法在精度上有很大的提高，特别是在z方向更为明显。其中loam的平移偏差(均方根误差表示)为18.461m，lio_sam的平移偏差为15.672m，而本发明的算法平移偏差为8.331m。该算法得到的轨迹精度有了很大的提高。
可以理解，本发明是通过一些实施例进行描述的，本领域技术人员知悉的，在不脱离本发明的精神和范围的情况下，可以对这些特征和实施例进行各种改变或等效替换。另外，在本发明的教导下，可以对这些特征和实施例进行修改以适应具体的情况及材料而不会脱离本发明的精神和范围。因此，本发明不受此处所公开的具体实施例的限制，所有落入本申请的权利要求范围内的实施例都属于本发明所保护的范围内。

Claims (6)

1.一种针对地面移动机器人的Lidar-IMU融合的slam方法，其特征在于，包括如下步骤：
步骤1：数据预处理；包括点云去畸变和特征提取；
步骤2：关键帧选择与子地图构建；引入视觉slam中的关键帧概念，使用两个关键帧之间的激光雷达信息来构造局部子地图；
步骤3：关键帧的因子图优化；通过因子图优化将关键帧与世界坐标系相关联，构建一个因子图来联合优化激光雷达关键帧和IMU的预积分；
步骤4：位姿图优化；通过位姿图优化得到一个全局一致的环境地图。
2.根据权利要求1所述的针对地面移动机器人的Lidar-IMU融合的slam方法，其特征在于，所述步骤1包括点云去畸变和特征提取；
所述点云去畸变：在因子优化过程中，IMU的预积分提供预积分因子，而优化过程IMU的偏差也会作为优化参数参与优化，利用得到的IMU偏差纠正的IMU数据来估计任意时刻载体的位姿，将激光点变换到统一坐标系下，进行点云畸变校正；
所述特征提取：从去畸变的点云中提取特征点云，提取边点和表面点作为匹配的特征点，对于任何点p，从与p同一行扫描中找到10个连续点，用S表示，均分的位于p的两侧，则p的粗糙度可以通过以下公式计算：
Figure FDA0003762688940000011
式中|S|是S中点的个数，取10，ri表示第i个点的深度；
选择粗糙度值较小的点作为平面点，粗糙度值较大的点作为边缘点，则从任一激光雷达扫描得到的特征点表示为
Figure FDA0003762688940000012
Figure FDA0003762688940000013
表示平面点，
Figure FDA0003762688940000014
表示边缘点。
3.根据权利要求1所述的针对地面移动机器人的Lidar-IMU融合的slam方法，其特征在于，所述步骤2包括关键帧的选择和子地图的构建；
所述关键帧的选择：引入了视觉slam中的关键帧概念，使用两个关键帧之间的激光雷达信息来构造局部子地图，假设地面移动机器人是一个刚体，相邻激光雷达扫描时刻的机器人本体坐标系的xy平面的变化可以反映地平面的变化，当新的激光雷达扫描到来时，将当前帧地面法线矢量e3转换到前一帧坐标系中e′3i，则相邻激光雷达扫描的地平面之间的角度为：
Figure FDA0003762688940000021
式中
Figure FDA0003762688940000022
是e3的转置矩阵；
如果θ大于设定的阈值θth，则认为地平面的斜率发生了变化，机器人不再与之前的扫描位于同一地平面上，此扫描将被选为关键帧；
所述子地图的构建：如果新的Lidar扫描为关键帧，则创建一个新的子地图，并将该关键帧作为子地图的参考帧，此时机器人的本体坐标系为子地图的坐标系，用Mi表示，然后，构建一个包括激光雷达里程计约束和地面约束的最大后验估计，优化得到后续的普通雷达帧相对于子地图的位姿变换；
当新的激光雷达普通帧Fj到来时，在预处理后获得其特征点
Figure FDA0003762688940000023
将其与所在子地图Mi相匹配，此时，本地子地图具有如下激光雷达帧：
Mi＝{Li,Fi,0,Fi,1…Fi,j-1}
式中Li表示子地图Mi的参考关键帧，Fi,k,k＝1,2,…j-1表示子地图中已有的普通帧，对于扫描匹配方法，使用与loam相同的方法，首先将此普通帧的特征点转换为其所在子地图中得到
Figure FDA0003762688940000024
初始变换是通过使用来自校正后的IMU的预测获得的，对于
Figure FDA0003762688940000025
中的每个特征点，在子地图中找到相应的边缘点和平面点，然后，最小化特征点到其相应边和平面的距离，以获得最最优的变换：
Figure FDA0003762688940000026
式中
Figure FDA0003762688940000027
Figure FDA0003762688940000031
Figure FDA0003762688940000032
Figure FDA0003762688940000033
式中
Figure FDA0003762688940000034
分别是当前帧的边缘特征点和面特征点，
Figure FDA0003762688940000035
是
Figure FDA0003762688940000036
转换到局部坐标系后得到的点，
Figure FDA0003762688940000037
是局部地图中与
Figure FDA0003762688940000038
对应的匹配边缘点，
Figure FDA0003762688940000039
是局部地图中与
Figure FDA00037626889400000310
对应的匹配面点；
子地图中的所有激光雷达帧都位于同一地平面上，因此，使用在SE(3)位姿-SE(2)约束的方法向每个激光雷达普通帧添加平面约束，理想情况下，在子地图中，机器人在平面中移动，将普通帧和子地图之间的变换参数化为SE(2)位姿，其李代数表示为v＝[v1 v2 φ]，其中φ是旋转分量，(v1,v2)并表示平移分量；在实际环境中，由于机器人运动中的崎岖地形和机器人自身的晃动，机器人的姿态在SE(2)以外的维度上存在运动干扰，因此，不是直接在SE(2)上参数化机器人姿势，而是通过李代数将姿势从SE(2)恢复到SE(3)：
ξ＝[v1 v2 0 0 0 φ]T
同时，用高斯模型对其他三个维度的运动扰动的误差进行建模，表示
Figure FDA00037626889400000311
其他三维空间中允许的运动扰动振幅，这是由地形条件和机器人结构决定的；
因此，地平面约束的残差定义为：
Figure FDA00037626889400000312
其中，
Figure FDA00037626889400000313
是平面约束的测量值，可以通过将当前姿态估计值投影到SE(2)空间，然后再恢复到SE(3)来获得：
Figure FDA00037626889400000314
平面约束的雅可比矩阵和协方差矩阵为：
Figure FDA0003762688940000041
Figure FDA0003762688940000042
式中
Figure FDA0003762688940000043
分别是z方向，roll方向和pitch方向的机器人状态的方差。
4.根据权利要求3所述的针对地面移动机器人的Lidar-IMU融合的slam方法，其特征在于，所述步骤2采用一种启发式方法，用于机器人在同一地平面上移动太长时间而无法选择关键帧的情况，与前一帧的状态相比，此时机器人旋转或平移超过了定义的阈值，将此扫描视为关键帧，如果当前扫描不满足上述两个条件，则该扫描为普通帧，关键帧随后将用于与IMU联合优化，而普通帧则用于构建子地图。
5.根据权利要求1所述的针对地面移动机器人的Lidar-IMU融合的slam方法，其特征在于，所述步骤3包括如下具体步骤：
通过因子图优化将关键帧与世界坐标系相关联，构建一个因子图来联合优化激光雷达关键帧和IMU的预积分，该因子图包括以下3种类型的因子：
激光雷达里程计因子：当新的关键帧Li到来时，经过预处理后，得到边点
Figure FDA0003762688940000044
和平面点
Figure FDA0003762688940000045
此时，将此新状态节点添加到因子图中，将此关键帧与前一个子地图Mi-1进行匹配，子地图包含的激光雷达帧如下：
Mi-1＝{Li-1,Fi-1,0,Fi-1,1,Fi-1,2…Fi-1,n}
其中n是子地图Mi-1中普通帧数目，匹配方法与步骤3所述方法相同，通过匹配得到相邻关键帧之间的相对位姿变换，此时新的关键帧位姿会作为节点，相对位姿作为因子加入了因子图中；
IMU预积分因子：IMU的测量模型如下：
Figure FDA0003762688940000046
Figure FDA0003762688940000047
IMU数据是在其本体坐标系的测量，并且会受到加性噪声na，nw、加速度偏差
Figure FDA0003762688940000048
以及陀螺仪偏差
Figure FDA0003762688940000049
的影响，将偏差建模为随机游走，其导数是高斯的，加速度计和陀螺仪的加法噪声被假定为高斯分布；
给出两个相邻关键帧之间的最终预积分形式：
Figure FDA0003762688940000051
Figure FDA0003762688940000052
Figure FDA0003762688940000053
式中
Figure FDA0003762688940000054
Figure FDA0003762688940000055
Figure FDA0003762688940000056
除了效率之外，IMU预积分的应用为因子图提供了一种相对位姿约束的因子；
回环因子：使用基于欧氏距离的回环检测方法，将欧式距离作为先验，将距离当前关键帧的最近且位姿的相对变换小于设定的阈值的关键帧作为候选帧，然后，将候选帧及其前后的n个关键帧及其相应的本地子地图作为当前关键帧的匹配点云，得到当前帧和候选帧之间的相对变换，该变换作为回环因子添加到因子图中。
6.根据权利要求1所述的针对地面移动机器人的Lidar-IMU融合的slam方法，其特征在于，所述步骤4包括如下具体步骤：
基于滑动窗口的位姿优化用于将普通帧固定到世界坐标系下，并校正关键帧位姿以减少误差的累积，在构建局部子地图的过程中，得到普通帧和子地图的相对姿态，通过因子图优化，得到世界坐标系下关键帧的位姿，在滑动窗口中，普通帧的相对姿势和关键帧的绝对姿势，一起执行位姿图优化，在优化过程中，最早的关键帧或回环关键帧的位姿是固定的，边缘化时，直接删除最早进入位姿图优化的子地图中的关键帧和普通帧，最后，通过上述优化得到一个全局一致的环境地图。
CN202210876326.5A 2022-07-25 2022-07-25 一种针对地面移动机器人的Lidar-IMU融合的slam方法 Pending CN115355904A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210876326.5A CN115355904A (zh) 2022-07-25 2022-07-25 一种针对地面移动机器人的Lidar-IMU融合的slam方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210876326.5A CN115355904A (zh) 2022-07-25 2022-07-25 一种针对地面移动机器人的Lidar-IMU融合的slam方法

Publications (1)

Publication Number Publication Date
CN115355904A true CN115355904A (zh) 2022-11-18

Family

ID=84032412

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210876326.5A Pending CN115355904A (zh) 2022-07-25 2022-07-25 一种针对地面移动机器人的Lidar-IMU融合的slam方法

Country Status (1)

Country Link
CN (1) CN115355904A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116592897A (zh) * 2023-07-17 2023-08-15 河海大学 基于位姿不确定性的改进orb-slam2定位方法

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116592897A (zh) * 2023-07-17 2023-08-15 河海大学 基于位姿不确定性的改进orb-slam2定位方法
CN116592897B (zh) * 2023-07-17 2023-09-22 河海大学 基于位姿不确定性的改进orb-slam2定位方法

Similar Documents

Publication Publication Date Title
CN111929699B (zh) 一种顾及动态障碍物的激光雷达惯导里程计与建图方法及系统
Kanade et al. Real-time and 3D vision for autonomous small and micro air vehicles
JP2020507072A (ja) 実時間オンライン自己運動推定を備えたレーザスキャナ
CN112304307A (zh) 一种基于多传感器融合的定位方法、装置和存储介质
Rosinol et al. Incremental visual-inertial 3d mesh generation with structural regularities
CN110261870A (zh) 一种用于视觉-惯性-激光融合的同步定位与建图方法
CN110726406A (zh) 一种改进的非线性优化单目惯导slam的方法
CN112556719B (zh) 一种基于cnn-ekf的视觉惯性里程计实现方法
CN112833892B (zh) 一种基于轨迹对齐的语义建图方法
CN114526745A (zh) 一种紧耦合激光雷达和惯性里程计的建图方法及系统
CN115479598A (zh) 基于多传感器融合的定位与建图方法及紧耦合系统
CN115407357A (zh) 基于大场景的低线束激光雷达-imu-rtk定位建图算法
CN116182837A (zh) 基于视觉激光雷达惯性紧耦合的定位建图方法
CN116449384A (zh) 基于固态激光雷达的雷达惯性紧耦合定位建图方法
CN113674412A (zh) 基于位姿融合优化的室内地图构建方法、系统及存储介质
Liu A robust and efficient lidar-inertial-visual fused simultaneous localization and mapping system with loop closure
Zhang LILO: A Novel Lidar–IMU SLAM System With Loop Optimization
CN115355904A (zh) 一种针对地面移动机器人的Lidar-IMU融合的slam方法
CN112945233B (zh) 一种全局无漂移的自主机器人同时定位与地图构建方法
CN112762929B (zh) 一种智能导航方法、装置和设备
Alliez et al. Indoor localization and mapping: Towards tracking resilience through a multi-slam approach
CN116608873A (zh) 一种自动驾驶车辆的多传感器融合定位建图方法
Emter et al. 3D SLAMWith Scan Matching and Factor Graph Optimization
CN115218889A (zh) 一种基于点线特征融合的多传感器室内定位方法
CN112837374B (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