CN114526745B - 一种紧耦合激光雷达和惯性里程计的建图方法及系统 - Google Patents
一种紧耦合激光雷达和惯性里程计的建图方法及系统 Download PDFInfo
- Publication number
- CN114526745B CN114526745B CN202210150666.XA CN202210150666A CN114526745B CN 114526745 B CN114526745 B CN 114526745B CN 202210150666 A CN202210150666 A CN 202210150666A CN 114526745 B CN114526745 B CN 114526745B
- Authority
- CN
- China
- Prior art keywords
- frame
- matching
- pose
- imu
- laser
- 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
- 238000010276 construction Methods 0.000 title description 7
- 238000000034 method Methods 0.000 claims abstract description 58
- 238000005457 optimization Methods 0.000 claims abstract description 43
- 238000001514 detection method Methods 0.000 claims abstract description 17
- 238000007781 pre-processing Methods 0.000 claims abstract description 12
- 206010034719 Personality change Diseases 0.000 claims abstract description 6
- 238000012545 processing Methods 0.000 claims abstract description 5
- 238000013507 mapping Methods 0.000 claims description 23
- 239000011159 matrix material Substances 0.000 claims description 20
- 230000008859 change Effects 0.000 claims description 14
- 238000012937 correction Methods 0.000 claims description 14
- 230000009466 transformation Effects 0.000 claims description 14
- 230000010354 integration Effects 0.000 claims description 12
- 238000005259 measurement Methods 0.000 claims description 10
- 238000004364 calculation method Methods 0.000 claims description 6
- 238000009826 distribution Methods 0.000 claims description 4
- 238000000605 extraction Methods 0.000 claims description 4
- 230000001360 synchronised effect Effects 0.000 claims description 3
- 238000009966 trimming Methods 0.000 claims description 2
- 238000010168 coupling process Methods 0.000 abstract description 8
- 238000005859 coupling reaction Methods 0.000 abstract description 8
- 230000008878 coupling Effects 0.000 abstract description 4
- 238000005516 engineering process Methods 0.000 description 6
- 230000008569 process Effects 0.000 description 6
- 230000007613 environmental effect Effects 0.000 description 5
- 238000011161 development Methods 0.000 description 4
- 230000006870 function Effects 0.000 description 4
- 230000001133 acceleration Effects 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 2
- 238000006243 chemical reaction Methods 0.000 description 2
- 230000004927 fusion Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000013519 translation Methods 0.000 description 2
- 230000002776 aggregation Effects 0.000 description 1
- 238000004220 aggregation Methods 0.000 description 1
- 238000004458 analytical method Methods 0.000 description 1
- 238000004140 cleaning Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 238000007500 overflow downdraw method Methods 0.000 description 1
- 230000008447 perception Effects 0.000 description 1
- 238000012887 quadratic function Methods 0.000 description 1
- 230000017105 transposition Effects 0.000 description 1
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/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
- G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
- G01C21/30—Map- or contour-matching
- G01C21/32—Structuring or formatting of map data
-
- 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
- G01C21/1652—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 with ranging devices, e.g. LIDAR or RADAR
-
- 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/89—Lidar systems specially adapted for specific applications for mapping or imaging
-
- 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)
- Automation & Control Theory (AREA)
- Computer Networks & Wireless Communication (AREA)
- Electromagnetism (AREA)
- Navigation (AREA)
- Traffic Control Systems (AREA)
Abstract
本发明涉及一种紧耦合激光雷达和惯性里程计的建图方法及系统，方法包括：获取IMU数据和激光点云数据，对IMU数据进行预处理操作，得到当前激光帧姿态变化，对激光点云数据进行预处理操作，得到当前关键帧对应局部地图；对当前激光帧姿态变化和当前关键帧对应局部地图进行降采样处理，建立位姿优化方程进行扫描匹配优化，根据匹配的结果对之前获取的所有点云数据进行矫正；最后进行回环检测线程，对位姿进行矫正后保存当前帧和匹配帧之间的关系，实现闭环检测，构建出可视化的全局地图。本发明通过使用关键帧结合局部地图的方式使得帧图匹配的效率大大提高。
Description
技术领域
本发明涉及无人驾驶地图实时构建和车辆系统技术领域技术领域，特别是涉及一种紧耦合激光雷达和惯性里程计的建图方法及系统。
背景技术
无人驾驶技术是近年来智能交通发展的重要前沿领域，其中包含了建图定位，融合感知，路径规划，决策控制等部分。无人驾驶车辆需要提供精确的信息来实现安全正确的行驶。无人驾驶技术主要通过集成GNSS，IMU和激光雷达等传感器来获得车辆的厘米级位置信息。近年来，随着同步与地图构建技术使用的传感器正不断迭代发展，更为精确的实时导航越来越受到人们的普遍关注，稳定性更强、精度更高和可靠性更好的技术已成为人们越来越强烈的需求。
当前主流的无人驾驶技术主要由两种：一种是基于GNSS(Global NavigationSatellite System，全球导航卫星系统)和激光雷达特征匹配的融合方法。但在某些特殊场景下的目标，由于目标的卫星信号容易受到遮挡、干扰等复杂的环境因素影响，难以为无人车提供目标持续、稳定的导航数据，尤其是隧道、城市峡谷、树冠茂密场景，环境遮挡造成可视卫星数目急剧降低。此时需要无人车进行自适应局部，找到自身车体所处道路的空间位置以及相对位姿，从而使得车辆能够在弱信号区域内依靠高精度的导航数据满足安全需求。通过以GNSS信息作为载体的位置约束，使用最小二乘方法对GNSS和激光扫描帧间匹配得到的位置进行融合。但是由于不同时空下的GNSS误差大小不一致，使用普通最小二乘法进行求解相当于将不同时空下的GNSS误差无差别引入系统中，最终导致载体位姿求解不准确、按载体位姿拼接得到的点云地图局部细节不够精细化的问题。
另一种是基于车载惯性传感器IMU(Inertial Measurement Unit，惯性测量单元)、里程计以及激光雷达特征匹配的融合航迹推算方法，主要是根据上一时刻的位姿推断得到下一时刻的位姿信息，通过构建的精准高精度地图与之进行特征匹配，从而得到精确的局部信息。该方法优点明显，能够实现获取厘米级的信息。通过对不同时刻两组采集到的分散物体点云的匹配与比对，得到这段时间内物体移动的距离和转动的角度，即位姿的变化；一连串位姿变化的集合，便可以组成移动物体走过的路径。
在智能驾驶蓬勃发展的今天，新的技术和新的应用前景正在逐渐释放，虽然融合IMU和激光雷达进行建图的方法已经开始普及，但现有方法及其应用发展也面临着诸多的挑战。以感知为基础的建图定位功能作为智能驾驶的第一环，后续环节路径规划和底层控制亟需其能够提供更为精确的环境地图和定位的工具和方法。
发明内容
本发明提供一种紧耦合激光雷达和惯性里程计的建图方法及系统，以解决上述现有技术中存在的问题。
为实现上述目的，本发明提供了如下方案：
一种紧耦合激光雷达和惯性里程计的建图方法，包括：
获取IMU数据和激光点云数据，对所述IMU数据进行预处理操作，得到当前激光帧姿态变化，对所述激光点云数据进行预处理操作，得到当前关键帧对应局部地图；
对所述当前激光帧姿态变化和所述当前关键帧对应局部地图进行降采样处理，建立位姿优化方程进行扫描匹配优化，根据匹配的结果对之前获取的所有点云数据进行矫正；
最后进行回环检测线程，将角点和平面点特征作为当前帧特征集，将关键帧特征点作为匹配帧特征集，对所述当前帧特征集和所述匹配帧特征集进行匹配并计算匹配得分，根据匹配结果计算所述当前帧和匹配帧之间的欧式距离，对位姿进行矫正后保存当前帧和匹配帧之间的关系，实现闭环检测，构建出可视化的全局地图。
优选地，对所述IMU数据进行预处理的过程包括：首先修剪IMU数据队列保证IMU时间戳和激光雷达时间戳同步，从而将IMU数据与扫描的时间戳进行对齐，对激光雷达初始帧到结束帧时间段内的所述IMU数据进行姿态预积分，获得IMU姿态的变化，找到距离激光起始时刻最近的IMU数据，并将其欧拉角作为激光起始时刻的初始值。
优选地，对所述IMU数据进行姿态预积分的过程包括：通过检测所述IMU数据队列在当前帧激光里程计时刻之前是否存在数据来判断系统是否进行初始化，若未进行初始化，则对所述IMU预积分器进行重置，用前一帧的状态、偏置，施加IMU预计分量，得到当前时刻预估的数据，并对变量节点赋予初值并执行优化，通过优化后的状态对IMU预积分器进行初始化，更新IMU的偏置，用最新的偏置重新计算当前激光里程计时刻之后的IMU预积分以计算每一时刻的位姿，将结果存放在所述IMU预积分器中，得到当前激光帧姿态变化。
优选地，对所述激光点云数据进行预处理操作的过程包括：对激光雷达点云数据进行畸变矫正，通过对点云数据的每一帧进行线性插值，得到当前时刻对应的IMU位姿，再根据IMU预积分得到所述当前帧姿态变化，实现点云偏斜矫正；然后计算曲率，根据曲率提取角点和平面点进行点云特征提取，得到当前关键帧对应局部地图。
优选地，通过评估局部区域上的点曲率提取所述角点和平面特征，在获取所述当前激光帧姿态变化和所述关键帧对应局部地图后进行降采样剔除噪声数据，建立特征点集合并进行角点、平面点匹配优化，通过获取激光里程计数据，得到激光里程计位姿的时间戳以及位姿变换矩阵，通过滑动窗口创建点云地图，利用点-线-面之间的几何关系，建立位姿求解方程，然后通过标签匹配和LM优化方法进行点云匹配，基于传感器视域和图优化方法进行位姿优化。
优选地，建立所述位姿求解方程的过程包括：通过提取边缘特征和平面特征，建立关键帧特征集合，利用位姿估计构建局部地图，针对六自由度的LM优化方法，优化计算点和线特征的对应点约束，得到相应的位姿信息。
优选地，所述进行回环检测线程的过程包括：将角点和平面点特征作为当前帧的特征集，将前后相邻的5个关键帧特征点作为匹配帧特征集，将空间离散为立方体，采用正态分布变换进行两个特征集的匹配并计算匹配得分，根据匹配结果计算所述当前帧和匹配帧帧之间的欧氏距离，对位姿进行矫正后保存当前帧和匹配帧之间的关系，实现闭环检测，构建出可视化的全局地图。
一种应用于紧耦合激光雷达和惯性里程计的建图方法的系统，包括：
位姿获取模块：用于基于纠正零偏后的IMU测量数据，对相邻两个时刻间的IMU测量数据进行预积分，得到相邻两个时刻间移动物体的位姿变化；
位姿匹配模块：用于利用得到的所述位姿变化作为激光点云匹配的初始位姿，基于所述初始位姿，将激光点云中的当前帧和匹配帧进行匹配；
位姿输出模块：用于获取匹配结果，并输出对应的匹配位姿。
一种应用于紧耦合激光雷达和惯性里程计的建图系统的电子设备，包括存储器和处理器，所述存储器上存储有可在所述处理器上运行的紧耦合激光雷达惯性里程计建图及程序，所述建图程序被所述处理器运行时，执行所述的紧耦合激光雷达和惯性里程计的建图方法。
本发明的有益效果为：
(1)本发明通过使用关键帧结合局部地图的方式使得帧图匹配的效率大大提高；最后进行回环检测线程，保存当前帧和匹配帧之间的关系，计算帧之间的欧氏距离，实现闭环检测，从而构建出可视化的全局地图；
(2)本发明方法避免了在卫星信号受到遮挡、干扰等复杂的环境因素影响难以为无人车提供持续、稳定的导航数据和由于不同时空下的GNSS误差大小不一致最终导致载体位姿求解不准确、按载体位姿拼接得到的点云地图局部细节不够精细化的问题。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
图1为本发明实施例的紧耦合激光雷达和惯性里程计的建图方法流程图；
图2为本发明实施例的紧耦合激光雷达和惯性里程计的建图方法的构建地图过程示意图；
图3为本发明实施例的紧耦合激光雷达和惯性里程计的建图方法的最终结果侧视图；
图4为本发明实施例的紧耦合激光雷达和惯性里程计的建图方法的最终结果俯视图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。
本发明提出了一种紧耦合激光雷达和惯性里程计的建图方法及系统，用于全局地图构建和优化。包括以下步骤(参照附图1)：
首先对激光雷达初始帧到结束帧之间的IMU数据进行姿态积分，并将距离激光起始时刻最近的IMU数据的欧拉角作为激光起始时刻初始值并执行优化，更新上一时刻的姿态和偏置；通过对点云数据的每一帧进行线性插值，再根据IMU预积分得到的姿态的变化，从而实现点云偏斜矫正；然后计算曲率并标记遮挡点和平行光束点，提取角点和平面点进行点云特征提取。其次通过地图优化对获取到的当前激光帧位姿估计和对应的局部地图进行降采样剔除噪声数据，建立特征点集合并进行角点、平面点匹配优化，利用高斯牛顿迭代算法建立相应的位姿优化方程进行扫描匹配优化，之后优化因子图和矫正位姿。使用关键帧结合局部地图的方式使得帧图匹配的效率大大提高。最后进行回环检测线程，保存当前帧和匹配帧之间的关系，计算帧之间的欧氏距离，实现闭环检测，从而构建出可视化的全局地图(如图2)。
1、IMU因子处理,流程如下：
1.1、对点云偏斜矫正所需的IMU数据进行预处理。
对当前雷达帧起始扫描时间戳到扫描结束时间戳之间的IMU数据进行姿态积分，获得这段时间内IMU姿态的变化，用于后面偏斜矫正时使用。首先修剪IMU数据队列保证IMU时间戳和激光雷达时间戳同步，从而将IMU数据与扫描的时间戳进行对齐。之后对当前雷达帧起始扫描时间戳到扫描结束时间戳之间的IMU数据进行姿态积分，将角加速度乘时间求得旋转的角速度，从而找到距离激光起始时刻最近的IMU数据，并将其欧拉角作为激光起始时刻的初始值。
1.2、IMU预积分。基本流程如下：
1.2.1、通过检测IMU队列在当前帧激光里程计时刻之前是否存在数据来判断系统是否进行初始化。若未进行初始化，先移除IMU队列中时间戳早于当前帧激光里程计时刻的所有数据，并将当前帧激光里程计时刻的激光里程计位姿转换到IMU坐标系下作为起始状态因子图的先验位姿、速度和噪声等，并执行一次因子图优化，最后重置两个IMU积分器。其中，一个用来对两顿之间的IMU数据进行积分，另一个用来对最新一帧激光里程计后收到的IMU数据进行积分。
1.2.2、每接收到100帧激光里程计数据，重置一次优化器。在重置时，当前位姿与速度采用上一次优化得到的位姿和速度，噪声模型也采用上一次优化后模型的边缘分布，在重置赋值之后再进行一次优化。
1.2.3、添加IMU因子:对两激光帧对应的IMU线加速度、角速度数据进行积分，将此数据作为X(key-1)X(key)之间的约束，然后将已经处理的IMU数据从队列中剔除。添加IMUbias因子，提供角度和平移的变换关系，并添加噪声之间的偏置关系；添加当前接收到的激光里程计数据作为X(key)的先验因子，用前一帧的状态、偏置，施加IMU预计分量，得到当前时刻预估的数据。
1.2.4、对速度和噪声等几个变量节点赋予初值并执行优化，更新上一次时刻的姿态和偏置，并判断优化得到的位姿和速度是否过大。利用当前优化后的状态来对IMU预积分器进行初始化，更新了IMU的偏置，用最新的偏置重新计算当前激光里程计时刻之后的IMU预积分以计算每一时刻的位姿，结果存放在IMU积分器中。
2、激光里程计因子处理
2.1、畸变矫正
2.1.1、首先利用scan-to-map得到当前的位置，从而检索出IMU的坐标和索引值，然后对激光帧中的每个扫描点进行线性插值，得到当前点时间戳对应的IMU位姿，进行当前点对应的IMU位姿和第一个点对应的IMU位姿之间的相对变换，从而完成点的偏斜矫正。
2.1.2、投影到深度图中并记录完整点云。计算当前点深度得到其在深度图中的行索引，然后计算当前点的水平倾角和水平角分辨率，得到其在深度图中的列索引。在前面的点云偏斜矫正后填充深度图并记录完整点云。
2.1.3、确定每根线的起始和结束点索引，并提取出偏斜矫正后点云及对应的点云信息。
2.2、特征提取
2.2.1、计算曲率并标记遮挡点和平行光束点
取当前点的前五个点以及后五个点，计算其到中间点的距离差之和的平方作为曲率的度量，并缓存点的曲率信息，便于后面依据曲率对点进行排序。标记出遮挡点和平行光束点，避免后面进行错误的特征提取
2.2.2、提取角点和平面点。将一帧点云数据分成6段，计算点云的起始和结束索引，每段都提取一定数量的角点和平面点特征。对段内的所有点先按照计算的平滑度进行排序。从低到高标记满足未被处理且曲率大于阈值的点为角点，并同时将其前后五个点都标记为已处理避免角点聚集，每段最多提取20个角点；同理，再次从高到低标记满足未被处理且曲率小于阈值的点为平面点，并将一帧点云数据中未处理的点也都归类为平面点，然后对平面点云集合进行降采样。
3、图优化
3.1、利用IMU数据结合变换矩阵来预估当前激光对应的位姿数据。主要流程如下：
3.1.1、保存上一次经过优化的变换矩阵数据，变换矩阵数据在优化前存储IMU预估的优化位姿，变换矩阵在经过因子图优化后又存储经过优化后的位姿。
3.1.2、判断关键帧集合中有无数据，判断条件是当前帧被选定为关键帧且经过了因子图优化。当关键帧集合为空时当前帧默认为关键帧。此处的目的就是接收激光的第一帧数据。
3.1.3、如果是第一帧数据，利用畸变矫正中激光起始时刻对应IMU姿态作为变换矩阵的姿态数据，其中位置数据初始化为0。
3.1.4、当前帧数据不是激光的第一帧数据且IMU里程计信息可获得时，判断上一时刻IMU预处理数据是否可获得，然后计算当前帧起始时刻对应的IMU里程计的数据与上一帧激光起始时刻对应的IMU里程计数据，计算两帧IMU里程计之间的增量，结合上一帧起始时刻的经过优化后的变换矩阵数据，预估当前时刻激光帧里程计的位姿。
3.2、局部地图特征点选定函数针对当前关键帧(此处的关键帧不是点云数据而是xyz坐标信息)
3.2.1、获取当前关键帧的位置，并利用历史关键帧的位置构建KD-Tree，指定搜索半径来搜索邻近的关键帧集合进行降采样，剔除不符合要求的关键帧
3.2.2、针对关键帧集合重点数据，找寻在该位置下的角点和平面点，构成特征点集合，来用于扫描匹配
3.2.3、对得到特征点数据继续进行降采样，判断容器是否过大效率是否过低，定期进行数据清除。
3.3、扫描匹配优化
由以上分析可知，当从前端传递过来当前雷达的特征点时，通过估计的雷达位姿，将特征点转换到地图坐标系下，再通过最近邻搜索查找到转换后的特征点在地图中对应的特征，然后计算转换后特征点相对于对应特征的距离，并将此距离作为残差项进行优化.主要步骤包括角点匹配优化、平面点匹配优化和高斯牛顿迭代算法。
3.3.1、角点匹配优化采用点到线的匹配方式
(1)将当前帧的角点数据从雷达坐标系转换到世界坐标系；
式中表示在时刻k+1时世界坐标系下第i个特征点的位置(通过估计的雷达位姿转换的)，/>表示在时刻k+1时雷达坐标系下第i个角特征点和面特征点的位置，表示在时刻k+1时雷达坐标系相对于世界坐标系的变换，也即求解的目标，t＝(tx,ty,tz)T，R是通过欧拉角转换成的旋转矩阵，转换关系为：
式中，旋转部分r＝[rx,ry,rz]T，分别表示在XYZ参考坐标系中的分量式。
(2)利用Kd-tree搜索距离当前角点数据最近的5个点，并计算5个样本的几何中心以及协方差矩阵；
(3)对协方差矩阵讲行特征值分解，得到特征值以及特征向量；
a、若最大的特征值比次大的特征值大较多就认为构成了线，即5个点构成了一条直线；
b、利用特征向量以及前面计算出的几何中心点沿着特征向量前后位置各取一个点，用这两个点形成一条直线；
(4)套用角点匹配的残差构造公式构造方程：
式中表示在k+1时刻第i个特征点通过估计的雷达位姿转换到世界坐标系下的特征点坐标，/>和/>分别表示第i个角点特征点在地图中对应的边线特征上的两个坐标点。
(5)同时针对每一个角点匹配时形成的残差都赋予一个权值，残差小的权值大，残差大的权值小。
(6)为了之后残差方程J的计算，需要求解角点特征点的定义公式(3)中的变量/>然后将公式(3)进行展开
从而可以求出角点特征点的
3.3.2、平面点匹配优化采用点到平面的匹配方式
(1)将当前坐标系的平面点从激光雷达坐标系转换到世界坐标系；
(2)从局部地图中利用KDTree找寻距离当前平面点最近的5个点，判断距离当前匹配平面点最远的点的距离是否大于阈值；
(3)对于选出的五个点，p为线上一点，a、b、c、d分别为面上的四个点，利用线性最小二乘的方式拟合一个平面，套用平面点匹配的残差构造公式来构造方程：
式中x＝[x,y,z]T表示雷达坐标系下的面特征点通过估计的雷达位姿转换到世界坐标系下的坐标,pa、pb、pc、pd为线段距离
(4)为了之后残差方程J的计算，需要求解面点特征点的令得：
(5)针对每一个平面点匹配时形成的残差都赋予一个权值，残差小的权值大，残差大的权值小。
(6)判断平面点到匹配平面的距离是否大于一定范围，大于一定范围则认为平面构造不合理。
3.3.3、非线性求解构造了高斯牛顿迭代算法对残差方程进行优化，获得最小二乘解。
(1)取出当前帧匹配的平面点和角点个数之和，若匹配点数小于50则认为数目过少不进行优化，遍历所有的匹配的平面点和角点，计算雅可比矩阵并存储残差；
(2)构造高斯牛顿方程，迭代求解，将雅克比矩阵进行转置计算海森矩阵，判断海森矩阵是否奇异，根据能否跟踪二次函数判断海森矩阵是否退化。其中，迭代法迭代公式如下:
JTJ·ΔT＝-JTf (8)
式中J为残差方程相对于优化变量的雅克比矩阵，ΔT为优化变量增量，f为残差方程结果，求解如下
式中m为构建的局部关键帧地图.
根据公式(9)的残差方程定义，可采用链式法则求得雅克比矩阵J，计算公式如下:
将J分成两部分J＝[Jt,Jr]T，分别为残差方程相对于优化变量平移部分t＝[tx,ty,tz]T的和旋转部分r＝[rx,ry,rz]T的雅克比矩阵。
于是可以将J拆分成下面两部分:
而式(12)中的Jr又可以进一步分解为Jr＝[Jrx,Jry,Jrz]T三部分，Jrx、Jry、Jry分别表示在不同参考坐标系中的分量式，对应公式如下:
通过前面求得的角点和面点的特征点残差方程求得的各自对应雅克比矩阵中的项，以及残差方程f计算的残差结果，从而利用高斯牛顿迭代方法求解出关键帧位姿。
(3)更新位姿得到优化后得到的增量，分别求出旋转矩阵和平移矩阵的模，计算增量是否已经小于一定范围，当迭代次数达到或者增量较小，则迭代停止则完成优化。
3.3.4、因子图优化。添加因子里程计因子、GPS因子，当检测到回环，执行因子图优化，更新当前里程计的位姿以及当前关键帧的位姿，而没有对之前多个关健帧的位姿和点云的位姿进行更新，最后将优化后的位姿、接收到的角点和平面点数据分别进行存储；
3.3.5、矫正位姿，首先判断历史关键帧集合是否为空，检测闭环并已经经过图优化，则清空容器中存储的角点、平面点和全局路径，遍历当前帧中所有数据，更新全局路径中的数据和所有历史关键帧的位姿并进行存储；
3.4、回环检测将空间离散为立方体，采用正态分布变换进行两个特征集的匹配并计算匹配得分，根据匹配的结果对位姿矫正后保存当前帧和匹配帧之间的关系，计算帧之间的欧氏距离，实现闭环检测。
如图3为构建的地图结果的侧视图，图4为构建的地图结果的俯视图。
本发明还提供一种应用于紧耦合激光雷达和惯性里程计的建图方法的系统，包括：
位姿获取模块：用于基于纠正零偏后的IMU测量数据，对相邻两个时刻间的IMU测量数据进行预积分，得到相邻两个时刻间移动物体的位姿变化；
位姿匹配模块：用于利用得到的所述位姿变化作为激光点云匹配的初始位姿，基于所述初始位姿，将激光点云中的当前帧和匹配帧进行匹配；
位姿输出模块：用于获取匹配结果，并输出对应的匹配位姿。
一种应用于紧耦合激光雷达和惯性里程计的建图系统的电子设备包括存储器和处理器，所述存储器上存储有可在所述处理器上运行的紧耦合激光雷达惯性里程计建图及程序，所述建图程序被所述处理器运行时，执行紧耦合激光雷达和惯性里程计的建图方法。
本发明的有益效果为：
首先相比有融合GNSS和激光雷达的建图方法，本发明方法避免了在卫星信号受到遮挡、干扰等复杂的环境因素影响难以为无人车提供持续、稳定的导航数据和由于不同时空下的GNSS误差大小不一致最终导致载体位姿求解不准确、按载体位姿拼接得到的点云地图局部细节不够精细化的问题。
其次现有方法大多是直接存储全局体素地图而不是局部地图，因而很难执行回环检测以修正漂移或者组合GPS等测量进行位姿修正，并且体素地图的使用效率会随时间降低。故本发明方法使用帧-局部地图匹配代替LOAM的帧-全局地图匹配，只独立地存储每个关键帧的特征，而不是在位姿估计完成后就将特征加入到全局地图中，提高了帧图匹配的效率。
最后现有方法的后端大多是基于易用的开源优化库(如ceres-solver,g2o,gtsam等)进行残差方程的优化，虽然方便调用一些自动求导函数，但本发明方法仅基于相对更简单的Eigen函数库构造了高斯牛顿迭代算法对残差方程进行优化来获得最小二乘解，使得代码的可读性和开发速度都得到了提升。
以上所述的实施例仅是对本发明优选方式进行的描述，并非对本发明的范围进行限定，在不脱离本发明设计精神的前提下，本领域普通技术人员对本发明的技术方案做出的各种变形和改进，均应落入本发明权利要求书确定的保护范围内。
Claims (8)
1.一种紧耦合激光雷达和惯性里程计的建图方法，其特征在于，包括：
获取IMU数据和激光点云数据，对所述IMU数据进行预处理操作，得到当前激光帧姿态变化，对所述激光点云数据进行预处理操作，得到当前关键帧对应局部地图；
对所述当前激光帧姿态变化和所述当前关键帧对应局部地图进行降采样处理，建立位姿优化方程进行扫描匹配优化，根据匹配的结果对之前获取的所有点云数据进行矫正；
建立所述位姿优化方程进行扫描匹配优化，包括：
通过评估局部区域上的点曲率提取角点和平面特征，在获取所述当前激光帧姿态变化和所述关键帧对应局部地图后进行降采样剔除噪声数据，建立特征点集合并进行角点、平面点匹配优化，通过获取激光里程计数据，得到激光里程计位姿的时间戳以及位姿变换矩阵，通过滑动窗口创建点云地图，利用点-线-面之间的几何关系，建立位姿求解方程，然后通过标签匹配和LM优化方法进行点云匹配，基于传感器视域和图优化方法进行位姿优化；
最后进行回环检测线程，将角点和平面点特征作为当前帧特征集，将关键帧特征点作为匹配帧特征集，对所述当前帧特征集和所述匹配帧特征集进行匹配并计算匹配得分，根据匹配结果计算所述当前帧和匹配帧之间的欧式距离，对位姿进行矫正后保存当前帧和匹配帧之间的关系，实现闭环检测，构建出可视化的全局地图。
2.根据权利要求1所述的紧耦合激光雷达和惯性里程计的建图方法，其特征在于，对所述IMU数据进行预处理的过程包括：首先修剪IMU数据队列保证IMU时间戳和激光雷达时间戳同步，从而将IMU数据与扫描的时间戳进行对齐，对激光雷达初始帧到结束帧时间段内的所述IMU数据进行姿态预积分，获得IMU姿态的变化，找到距离激光起始时刻最近的IMU数据，并将其欧拉角作为激光起始时刻的初始值。
3.根据权利要求2所述的紧耦合激光雷达和惯性里程计的建图方法，其特征在于，对所述IMU数据进行姿态预积分的过程包括：通过检测所述IMU数据队列在当前帧激光里程计时刻之前是否存在数据来判断系统是否进行初始化，若未进行初始化，则对IMU预积分器进行重置，用前一帧的状态、偏置，施加IMU预积分量，得到当前时刻预估的数据，并对变量节点赋予初值并执行优化，通过优化后的状态对IMU预积分器进行初始化，更新IMU的偏置，用最新的偏置重新计算当前激光里程计时刻之后的IMU预积分以计算每一时刻的位姿，将结果存放在所述IMU预积分器中，得到当前激光帧姿态变化。
4.根据权利要求3所述的紧耦合激光雷达和惯性里程计的建图方法，其特征在于，对所述激光点云数据进行预处理操作的过程包括：对激光雷达点云数据进行畸变矫正，通过对点云数据的每一帧进行线性插值，得到当前时刻对应的IMU位姿，再根据IMU预积分得到所述当前帧姿态变化，实现点云偏斜矫正；然后计算曲率，根据曲率提取角点和平面点进行点云特征提取，得到当前关键帧对应局部地图。
5.根据权利要求4所述的紧耦合激光雷达和惯性里程计的建图方法，其特征在于，建立所述位姿求解方程的过程包括：通过提取边缘特征和平面特征，建立关键帧特征集合，利用位姿估计构建局部地图，针对六自由度的LM优化方法，优化计算点和线特征的对应点约束，得到相应的位姿信息。
6.根据权利要求1所述的紧耦合激光雷达和惯性里程计的建图方法，其特征在于，所述进行回环检测线程的过程包括：将角点和平面点特征作为当前帧的特征集，将前后相邻的5个关键帧特征点作为匹配帧特征集，将空间离散为立方体，采用正态分布变换进行两个特征集的匹配并计算匹配得分，根据匹配结果计算所述当前帧和匹配帧帧之间的欧氏距离，对位姿进行矫正后保存当前帧和匹配帧之间的关系，实现闭环检测，构建出可视化的全局地图。
7.应用于权利要求1-6任一项所述的紧耦合激光雷达和惯性里程计的建图方法的系统，其特征在于，包括：
位姿获取模块：用于基于纠正零偏后的IMU测量数据，对相邻两个时刻间的IMU测量数据进行预积分，得到相邻两个时刻间移动物体的位姿变化；
位姿匹配模块：用于利用得到的所述位姿变化作为激光点云匹配的初始位姿，基于所述初始位姿，将激光点云中的当前帧和匹配帧进行匹配；
位姿输出模块：用于获取匹配结果，并输出对应的匹配位姿。
8.一种应用于紧耦合激光雷达和惯性里程计的建图系统的电子设备，其特征在于，包括存储器和处理器，所述存储器上存储有可在所述处理器上运行的紧耦合激光雷达惯性里程计建图程序，所述建图程序被所述处理器运行时，执行如权利要求1至6中任一项所述的紧耦合激光雷达和惯性里程计的建图方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210150666.XA CN114526745B (zh) | 2022-02-18 | 2022-02-18 | 一种紧耦合激光雷达和惯性里程计的建图方法及系统 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210150666.XA CN114526745B (zh) | 2022-02-18 | 2022-02-18 | 一种紧耦合激光雷达和惯性里程计的建图方法及系统 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN114526745A CN114526745A (zh) | 2022-05-24 |
| CN114526745B true CN114526745B (zh) | 2024-04-12 |
Family
ID=81623657
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210150666.XA Active CN114526745B (zh) | 2022-02-18 | 2022-02-18 | 一种紧耦合激光雷达和惯性里程计的建图方法及系统 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114526745B (zh) |
Families Citing this family (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115183778A (zh) * | 2022-07-01 | 2022-10-14 | 北京斯年智驾科技有限公司 | 一种基于码头石墩的建图方法、装置、设备以及介质 |
| CN114842084B (zh) * | 2022-07-04 | 2022-09-30 | 矿冶科技集团有限公司 | 一种地图构建方法、装置及移动探测设备 |
| CN115560744A (zh) * | 2022-08-22 | 2023-01-03 | 深圳市普渡科技有限公司 | 机器人以及基于多传感器的三维建图方法、存储介质 |
| CN115371662B (zh) * | 2022-08-22 | 2024-06-25 | 北京化工大学 | 一种基于概率栅格移除动态对象的静态地图构建方法 |
| CN115328163B (zh) * | 2022-09-16 | 2023-03-28 | 西南交通大学 | 一种巡检机器人雷达里程计的速度与精度优化方法 |
| CN115218892A (zh) * | 2022-09-20 | 2022-10-21 | 成都睿芯行科技有限公司 | 一种基于多传感器融合的轻量级定位与建图方法 |
| CN115265523B (zh) * | 2022-09-27 | 2023-01-03 | 泉州装备制造研究所 | 机器人同时定位与建图方法、装置及可读介质 |
| CN116337072A (zh) * | 2023-03-24 | 2023-06-27 | 华侨大学 | 一种工程机械的建图、方法、设备、及可读存储介质 |
| CN117739968B (zh) * | 2023-12-22 | 2024-07-09 | 武汉大学 | 适用于复杂林下环境的鲁棒实时定位与建图方法及系统 |
| CN117974926B (zh) * | 2024-03-28 | 2024-06-18 | 苏州艾吉威机器人有限公司 | 一种基于机械旋转式三维激光的定位与建图方法及系统 |
Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| EP3078935A1 (en) * | 2015-04-10 | 2016-10-12 | The European Atomic Energy Community (EURATOM), represented by the European Commission | Method and device for real-time mapping and localization |
| CN109658449A (zh) * | 2018-12-03 | 2019-04-19 | 华中科技大学 | 一种基于rgb-d图像的室内场景三维重建方法 |
| CN109870157A (zh) * | 2019-02-20 | 2019-06-11 | 苏州风图智能科技有限公司 | 确定车体位姿的方法及装置、制图方法 |
| CN109945856A (zh) * | 2019-02-18 | 2019-06-28 | 天津大学 | 基于惯性/雷达的无人机自主定位与建图方法 |
| CN111273312A (zh) * | 2020-01-15 | 2020-06-12 | 吉林大学 | 一种智能车辆定位与回环检测方法 |
| CN111929699A (zh) * | 2020-07-21 | 2020-11-13 | 北京建筑大学 | 一种顾及动态障碍物的激光雷达惯导里程计与建图方法及系统 |
| CN112967392A (zh) * | 2021-03-05 | 2021-06-15 | 武汉理工大学 | 一种基于多传感器触合的大规模园区建图定位方法 |
| CN113066105A (zh) * | 2021-04-02 | 2021-07-02 | 北京理工大学 | 激光雷达和惯性测量单元融合的定位与建图方法及系统 |
| CN114018248A (zh) * | 2021-10-29 | 2022-02-08 | 同济大学 | 一种融合码盘和激光雷达的里程计方法与建图方法 |
-
2022
- 2022-02-18 CN CN202210150666.XA patent/CN114526745B/zh active Active
Patent Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| EP3078935A1 (en) * | 2015-04-10 | 2016-10-12 | The European Atomic Energy Community (EURATOM), represented by the European Commission | Method and device for real-time mapping and localization |
| CN109658449A (zh) * | 2018-12-03 | 2019-04-19 | 华中科技大学 | 一种基于rgb-d图像的室内场景三维重建方法 |
| CN109945856A (zh) * | 2019-02-18 | 2019-06-28 | 天津大学 | 基于惯性/雷达的无人机自主定位与建图方法 |
| CN109870157A (zh) * | 2019-02-20 | 2019-06-11 | 苏州风图智能科技有限公司 | 确定车体位姿的方法及装置、制图方法 |
| CN111273312A (zh) * | 2020-01-15 | 2020-06-12 | 吉林大学 | 一种智能车辆定位与回环检测方法 |
| CN111929699A (zh) * | 2020-07-21 | 2020-11-13 | 北京建筑大学 | 一种顾及动态障碍物的激光雷达惯导里程计与建图方法及系统 |
| CN112967392A (zh) * | 2021-03-05 | 2021-06-15 | 武汉理工大学 | 一种基于多传感器触合的大规模园区建图定位方法 |
| CN113066105A (zh) * | 2021-04-02 | 2021-07-02 | 北京理工大学 | 激光雷达和惯性测量单元融合的定位与建图方法及系统 |
| CN114018248A (zh) * | 2021-10-29 | 2022-02-08 | 同济大学 | 一种融合码盘和激光雷达的里程计方法与建图方法 |
Non-Patent Citations (2)
| Title |
|---|
| LOAM: Lidar Odometry and Mapping in Real-time;Ji Zhang等;《2014 Robotics: Science and Systems X》;20151010;第1-10页 * |
| 基于LiDAR/INS的野外移动机器人组合导航方法;宋锐等;《智能系统学报》;20200731;第15卷(第4期);第804-810页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN114526745A (zh) | 2022-05-24 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN114526745B (zh) | 一种紧耦合激光雷达和惯性里程计的建图方法及系统 | |
| CN109934920B (zh) | 基于低成本设备的高精度三维点云地图构建方法 | |
| CN113781582A (zh) | 基于激光雷达和惯导联合标定的同步定位与地图创建方法 | |
| CN110361027A (zh) | 基于单线激光雷达与双目相机数据融合的机器人路径规划方法 | |
| CN113654555A (zh) | 一种基于多传感器数据融合的自动驾驶车辆高精定位方法 | |
| CN114323033B (zh) | 基于车道线和特征点的定位方法、设备及自动驾驶车辆 | |
| CN114019552A (zh) | 一种基于贝叶斯多传感器误差约束的定位置信度优化方法 | |
| CN115407357A (zh) | 基于大场景的低线束激光雷达-imu-rtk定位建图算法 | |
| CN110412596A (zh) | 一种基于图像信息和激光点云的机器人定位方法 | |
| CN114325634A (zh) | 一种基于激光雷达的高鲁棒性野外环境下可通行区域提取方法 | |
| Wen et al. | TM 3 Loc: Tightly-coupled monocular map matching for high precision vehicle localization | |
| CN113920198B (zh) | 一种基于语义边缘对齐的由粗到精的多传感器融合定位方法 | |
| CN116608873A (zh) | 一种自动驾驶车辆的多传感器融合定位建图方法 | |
| CN115183762A (zh) | 一种机场仓库内外建图方法、系统、电子设备及介质 | |
| CN115930977A (zh) | 特征退化场景的定位方法、系统、电子设备和可读存介质 | |
| CN115574816A (zh) | 仿生视觉多源信息智能感知无人平台 | |
| CN113379915B (zh) | 一种基于点云融合的行车场景构建方法 | |
| CN113838129B (zh) | 一种获得位姿信息的方法、装置以及系统 | |
| CN117387604A (zh) | 基于4d毫米波雷达和imu融合的定位与建图方法及系统 | |
| CN117075158A (zh) | 基于激光雷达的无人变形运动平台的位姿估计方法及系统 | |
| CN115562076B (zh) | 用于无人驾驶矿车的仿真系统、方法以及存储介质 | |
| CN113227713A (zh) | 生成用于定位的环境模型的方法和系统 | |
| CN115468576A (zh) | 一种基于多模态数据融合的自动驾驶定位方法及系统 | |
| CN115963508A (zh) | 激光雷达和imu的紧耦合slam方法及系统 | |
| Zhou et al. | Localization for unmanned vehicle |
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 |