CN115355904A - 一种针对地面移动机器人的Lidar-IMU融合的slam方法 - Google Patents
一种针对地面移动机器人的Lidar-IMU融合的slam方法 Download PDFInfo
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 72
- 230000004927 fusion Effects 0.000 title claims abstract description 19
- 238000005457 optimization Methods 0.000 claims abstract description 53
- 238000013519 translation Methods 0.000 claims abstract description 13
- 238000007781 pre-processing Methods 0.000 claims abstract description 6
- 230000009466 transformation Effects 0.000 claims description 23
- 230000010354 integration Effects 0.000 claims description 20
- 230000008569 process Effects 0.000 claims description 16
- 238000005259 measurement Methods 0.000 claims description 13
- 239000011159 matrix material Substances 0.000 claims description 9
- 238000000605 extraction Methods 0.000 claims description 7
- 239000000654 additive Substances 0.000 claims description 6
- 230000000996 additive effect Effects 0.000 claims description 6
- 230000000007 visual effect Effects 0.000 claims description 5
- 238000009825 accumulation Methods 0.000 claims description 4
- 238000001514 detection method Methods 0.000 claims description 4
- 238000010276 construction Methods 0.000 claims description 3
- 238000012937 correction Methods 0.000 claims description 3
- 238000005295 random walk Methods 0.000 claims description 3
- 238000006243 chemical reaction Methods 0.000 claims description 2
- 230000000694 effects Effects 0.000 claims 1
- 230000001360 synchronised effect Effects 0.000 abstract description 3
- 230000008859 change Effects 0.000 description 9
- 238000013507 mapping Methods 0.000 description 5
- 238000010586 diagram Methods 0.000 description 3
- 230000036544 posture Effects 0.000 description 2
- 230000001133 acceleration Effects 0.000 description 1
- 238000013459 approach Methods 0.000 description 1
- 229960001948 caffeine Drugs 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 238000005286 illumination Methods 0.000 description 1
- 239000000463 material Substances 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000035945 sensitivity Effects 0.000 description 1
- 238000000638 solvent extraction Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- RYYVLZVUVIJVGH-UHFFFAOYSA-N trimethylxanthine Natural products CN1C(=O)N(C)C(=O)C2=C1N=CN2C RYYVLZVUVIJVGH-UHFFFAOYSA-N 0.000 description 1
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 description 1
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
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Computer Networks & Wireless Communication (AREA)
- Electromagnetism (AREA)
- Automation & Control Theory (AREA)
- Multimedia (AREA)
- Theoretical Computer Science (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明属于同步定位与建图(slam)领域,公开了一种针对地面移动机器人的Lidar‑IMU融合的slam方法,包括如下步骤:步骤1:数据预处理;步骤2:构建子地图;步骤3:创建地图与优化因子图;步骤4:优化位姿图。本发明能够实现在有起伏路面上为机器人添加地面约束,增加竖直方向的约束,能够很好的减少累积误差,整个实验机器人轨迹长大约1000m,与传统算法相比本发明的方法在精度上有很大的提高,特别是在z方向更为明显。其中loam的平移偏差(均方根误差表示)为18.461m,lio_sam的平移偏差为15.672m,而本发明的算法平移偏差为8.331m。该算法得到的轨迹精度有了很大的提高。
Description
技术领域
本发明属于同步定位与建图(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的粗糙度可以通过以下公式计算:
式中|S|是S中点的个数,取10,ri表示第i个点的深度;
进一步地,所述步骤2包括关键帧的选择和子地图的构建;
所述关键帧的选择:引入了视觉slam中常用的关键帧概念,使用两个关键帧之间的激光雷达信息来构造局部子地图,假设地面移动机器人是一个刚体,相邻激光雷达扫描时刻的机器人本体坐标系的xy平面的变化可以反映地平面的变化,当新的激光雷达扫描到来时,将当前帧地面法线矢量e3转换到前一帧坐标系中e′3i,则相邻激光雷达扫描的地平面之间的角度为:
如果θ大于设定的阈值θth,则认为地平面的斜率发生了变化,机器人不再与之前的扫描位于同一地平面上,此扫描将被选为关键帧;
所述子地图的构建:如果新的Lidar扫描为关键帧,则创建一个新的子地图,并将该关键帧作为子地图的参考帧,此时机器人的本体坐标系为子地图的坐标系,用Mi表示,然后,构建一个包括激光雷达里程计约束和地面约束的最大后验估计,优化得到后续的普通雷达帧相对于子地图的位姿变换;
Mi={Li,Fi,0,Fi,1…Fi,j-1}
式中Li表示子地图Mi的参考关键帧,Fi,k,k=1,2,…j-1表示子地图中已有的普通帧,对于扫描匹配方法,使用与loam相同的方法,首先将此普通帧的特征点转换为其所在子地图中得到初始变换是通过使用来自校正后的IMU的预测获得的,对于中的每个特征点,在子地图中找到相应的边缘点和平面点,然后,最小化特征点到其相应边和平面的距离,以获得最最优的变换:
式中
子地图中的所有激光雷达帧都位于同一地平面上,因此,使用在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
因此,地平面约束的残差定义为:
平面约束的雅可比矩阵和协方差矩阵为:
进一步地,所述步骤2采用了一种启发式方法,用于机器人在同一地平面上移动太长时间而无法选择关键帧的情况,与前一帧的状态相比,此时机器人旋转或平移超过了定义的阈值,将此扫描视为关键帧,如果当前扫描不满足上述两个条件,则该扫描为普通帧,关键帧随后将用于与IMU联合优化,而普通帧则用于构建子地图。
进一步地,所述步骤3包括如下具体步骤:
通过因子图优化将关键帧与世界坐标系相关联,构建一个因子图来联合优化激光雷达关键帧和IMU的预积分,该因子图包括以下3种类型的因子:
Mi-1={Li-1,Fi-1,0,Fi-1,1,Fi-1,2…Fi-1,n}
其中n是子地图Mi-1中普通帧数目,匹配方法与步骤3所述方法相同,通过匹配得到相邻关键帧之间的相对位姿变换,此时新的关键帧位姿会作为节点,相对位姿作为因子加入了因子图中;
IMU预积分因子:IMU的测量模型如下:
给出两个相邻关键帧之间的最终预积分形式:
式中
除了效率之外,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的粗糙度可以通过以下公式计算:
式中|S|是S中点的个数,本发明取10,ri表示第i个点的深度。
2)关键帧的选择与子地图的构建
关键帧的选择:为了保证算法的实时性,我们引入了视觉slam中常用的关键帧概念。但不同之处在于,我们不是丢弃两个关键帧之间的激光雷达信息,而是使用它们来构造局部子地图。在算法中,我们假设地面移动机器人是一个刚体,所以相邻激光雷达扫描时刻的机器人本体坐标系的xy平面的变化可以反映地平面的变化。当新的激光雷达扫描到来时,我们将当前帧地面法线矢量e3转换到前一帧坐标系中e′3i,则相邻激光雷达扫描的地平面之间的角度为:
如果θ大于我们设定的阈值θth,则认为地平面的斜率发生了变化。机器人不再与之前的扫描位于同一地平面上。此扫描将被选为关键帧。我们还采用了一种简单但有效的启发式方法,用于机器人在同一地平面上移动太长时间而无法选择关键帧的情况。与前一帧的状态相比,此时机器人旋转或平移超过了我们定义的阈值,我们将此扫描视为关键帧。如果当前扫描不满足上述两个条件,则该扫描为普通帧。如图2所示,关键帧随后将用于与IMU联合优化,而普通帧则用于构建子地图。
子地图的构建:如果新的Lidar扫描为关键帧,则我们需要创建一个新的子地图,并将该关键帧作为子地图的参考帧。此时机器人的本体坐标系为子地图的坐标系,用Mi表示,如图2所示。然后,我们构建一个包括激光雷达里程计约束和地面约束的最大后验估计,优化得到后续的普通雷达帧相对于子地图的位姿变换。
Mi={Li,Fi,0,Fi,1…Fi,j-1}
式中Li表示子地图Mi的参考关键帧,Fi,k,k=1,2,…j-1表示子地图中已有的普通帧。对于扫描匹配方法,我们使用与loam相同的方法,因为它在各种具有挑战性的环境中具有计算效率和鲁棒性。我们首先将此普通帧的特征点转换为其所在子地图中得到初始变换是通过使用来自校正后的IMU的预测获得的。对于中的每个特征点,我们在子地图中找到相应的边缘点和平面点。然后,我们最小化特征点到其相应边和平面的距离,以获得最最优的变换:
式中
子地图中的所有激光雷达帧都位于同一地平面上。因此,我们使用在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
因此,地平面约束的残差定义为:
平面约束的雅可比矩阵和协方差矩阵为:
3)关键帧的因子图优化
本节通过因子图优化将关键帧与世界坐标系相关联。我们将构建一个因子图来联合优化激光雷达关键帧和IMU的预积分。该因子图包括以下3种类型的因子。
激光雷达里程计因子:当新的关键帧Li到来时,经过预处理后,我们得到边点和平面点此时,我们需要将此新状态节点添加到因子图中。我们将此关键帧与前一个子地图Mi-1(而不是全局地图)进行匹配。子地图包含的激光雷达帧如下:
Mi-1={Li-1,Fi-1,0,Fi-1,1,Fi-1,2…Fi-1,n}
其中n是子地图Mi-1中普通帧数目。匹配方法与前述方法相同。通过匹配我们得到相邻关键帧之间的相对位姿变换。此时新的关键帧位姿会作为节点,相对位姿作为因子加入了因子图中。
IMU预积分因子:IMU的测量模型如下:
这里,我们给出了两个相邻关键帧之间的最终预积分形式:
式中
除了效率之外,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的粗糙度可以通过以下公式计算:
式中|S|是S中点的个数,取10,ri表示第i个点的深度;
3.根据权利要求1所述的针对地面移动机器人的Lidar-IMU融合的slam方法,其特征在于,所述步骤2包括关键帧的选择和子地图的构建;
所述关键帧的选择:引入了视觉slam中的关键帧概念,使用两个关键帧之间的激光雷达信息来构造局部子地图,假设地面移动机器人是一个刚体,相邻激光雷达扫描时刻的机器人本体坐标系的xy平面的变化可以反映地平面的变化,当新的激光雷达扫描到来时,将当前帧地面法线矢量e3转换到前一帧坐标系中e′3i,则相邻激光雷达扫描的地平面之间的角度为:
如果θ大于设定的阈值θth,则认为地平面的斜率发生了变化,机器人不再与之前的扫描位于同一地平面上,此扫描将被选为关键帧;
所述子地图的构建:如果新的Lidar扫描为关键帧,则创建一个新的子地图,并将该关键帧作为子地图的参考帧,此时机器人的本体坐标系为子地图的坐标系,用Mi表示,然后,构建一个包括激光雷达里程计约束和地面约束的最大后验估计,优化得到后续的普通雷达帧相对于子地图的位姿变换;
Mi={Li,Fi,0,Fi,1…Fi,j-1}
式中Li表示子地图Mi的参考关键帧,Fi,k,k=1,2,…j-1表示子地图中已有的普通帧,对于扫描匹配方法,使用与loam相同的方法,首先将此普通帧的特征点转换为其所在子地图中得到初始变换是通过使用来自校正后的IMU的预测获得的,对于中的每个特征点,在子地图中找到相应的边缘点和平面点,然后,最小化特征点到其相应边和平面的距离,以获得最最优的变换:
式中
子地图中的所有激光雷达帧都位于同一地平面上,因此,使用在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
因此,地平面约束的残差定义为:
平面约束的雅可比矩阵和协方差矩阵为:
4.根据权利要求3所述的针对地面移动机器人的Lidar-IMU融合的slam方法,其特征在于,所述步骤2采用一种启发式方法,用于机器人在同一地平面上移动太长时间而无法选择关键帧的情况,与前一帧的状态相比,此时机器人旋转或平移超过了定义的阈值,将此扫描视为关键帧,如果当前扫描不满足上述两个条件,则该扫描为普通帧,关键帧随后将用于与IMU联合优化,而普通帧则用于构建子地图。
5.根据权利要求1所述的针对地面移动机器人的Lidar-IMU融合的slam方法,其特征在于,所述步骤3包括如下具体步骤:
通过因子图优化将关键帧与世界坐标系相关联,构建一个因子图来联合优化激光雷达关键帧和IMU的预积分,该因子图包括以下3种类型的因子:
Mi-1={Li-1,Fi-1,0,Fi-1,1,Fi-1,2…Fi-1,n}
其中n是子地图Mi-1中普通帧数目,匹配方法与步骤3所述方法相同,通过匹配得到相邻关键帧之间的相对位姿变换,此时新的关键帧位姿会作为节点,相对位姿作为因子加入了因子图中;
IMU预积分因子:IMU的测量模型如下:
给出两个相邻关键帧之间的最终预积分形式:
式中
除了效率之外,IMU预积分的应用为因子图提供了一种相对位姿约束的因子;
回环因子:使用基于欧氏距离的回环检测方法,将欧式距离作为先验,将距离当前关键帧的最近且位姿的相对变换小于设定的阈值的关键帧作为候选帧,然后,将候选帧及其前后的n个关键帧及其相应的本地子地图作为当前关键帧的匹配点云,得到当前帧和候选帧之间的相对变换,该变换作为回环因子添加到因子图中。
6.根据权利要求1所述的针对地面移动机器人的Lidar-IMU融合的slam方法,其特征在于,所述步骤4包括如下具体步骤:
基于滑动窗口的位姿优化用于将普通帧固定到世界坐标系下,并校正关键帧位姿以减少误差的累积,在构建局部子地图的过程中,得到普通帧和子地图的相对姿态,通过因子图优化,得到世界坐标系下关键帧的位姿,在滑动窗口中,普通帧的相对姿势和关键帧的绝对姿势,一起执行位姿图优化,在优化过程中,最早的关键帧或回环关键帧的位姿是固定的,边缘化时,直接删除最早进入位姿图优化的子地图中的关键帧和普通帧,最后,通过上述优化得到一个全局一致的环境地图。
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 (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN116592897A (zh) * | 2023-07-17 | 2023-08-15 | 河海大学 | 基于位姿不确定性的改进orb-slam2定位方法 |
CN118067114A (zh) * | 2024-04-24 | 2024-05-24 | 成都赛力斯科技有限公司 | 一种地图构建方法、装置、电子设备及存储介质 |
-
2022
- 2022-07-25 CN CN202210876326.5A patent/CN115355904A/zh active Pending
Cited By (3)
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定位方法 |
CN118067114A (zh) * | 2024-04-24 | 2024-05-24 | 成都赛力斯科技有限公司 | 一种地图构建方法、装置、电子设备及存储介质 |
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) | 実時間オンライン自己運動推定を備えたレーザスキャナ | |
CN110726406A (zh) | 一种改进的非线性优化单目惯导slam的方法 | |
CN112556719B (zh) | 一种基于cnn-ekf的视觉惯性里程计实现方法 | |
CN114526745A (zh) | 一种紧耦合激光雷达和惯性里程计的建图方法及系统 | |
CN115407357A (zh) | 基于大场景的低线束激光雷达-imu-rtk定位建图算法 | |
CN115479598A (zh) | 基于多传感器融合的定位与建图方法及紧耦合系统 | |
CN112833892B (zh) | 一种基于轨迹对齐的语义建图方法 | |
CN116182837A (zh) | 基于视觉激光雷达惯性紧耦合的定位建图方法 | |
CN116449384A (zh) | 基于固态激光雷达的雷达惯性紧耦合定位建图方法 | |
CN114964276B (zh) | 一种融合惯导的动态视觉slam方法 | |
CN115355904A (zh) | 一种针对地面移动机器人的Lidar-IMU融合的slam方法 | |
Zhang | LILO: A novel LiDAR–IMU SLAM system with loop optimization | |
CN118135526B (zh) | 基于双目相机的四旋翼无人机视觉目标识别与定位方法 | |
CN116608873A (zh) | 一种自动驾驶车辆的多传感器融合定位建图方法 | |
CN113674412A (zh) | 基于位姿融合优化的室内地图构建方法、系统及存储介质 | |
CN115218889A (zh) | 一种基于点线特征融合的多传感器室内定位方法 | |
CN116879870A (zh) | 一种适用于低线束3d激光雷达的动态障碍物去除方法 | |
CN116380079A (zh) | 一种融合前视声呐与orb-slam3的水下slam方法 | |
Liu et al. | A multi-sensor fusion with automatic vision-LiDAR calibration based on Factor graph joint optimization for SLAM | |
CN112945233B (zh) | 一种全局无漂移的自主机器人同时定位与地图构建方法 | |
CN112762929B (zh) | 一种智能导航方法、装置和设备 | |
CN115512054B (zh) | 三维时空连续点云地图的构建方法 | |
CN115344033B (zh) | 一种基于单目相机/imu/dvl紧耦合的无人船导航与定位方法 |
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 |