CN112067007A - 地图生成方法、计算机存储介质及电子设备 - Google Patents
地图生成方法、计算机存储介质及电子设备 Download PDFInfo
- Publication number
- CN112067007A CN112067007A CN202011264528.1A CN202011264528A CN112067007A CN 112067007 A CN112067007 A CN 112067007A CN 202011264528 A CN202011264528 A CN 202011264528A CN 112067007 A CN112067007 A CN 112067007A
- Authority
- CN
- China
- Prior art keywords
- variable
- laser
- laser point
- adjacent frames
- pose
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
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/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
-
- 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
Abstract
本发明提供了一种地图生成方法、计算机存储介质及电子设备。该方法通过计算相邻两帧的激光点云数据的激光残差和第一雅克比矩阵,进而计算第一优化变量迭代的第一增量,并根据第一增量对第一待优化变量进行迭代更新得到第一优化变量;通过计算相邻两帧的惯性测量数据的惯性测量残差和第二雅克比矩阵,进而计算第二优化变量迭代的第二增量,并将第一优化变量作为初值,并根据第二增量对第二待优化变量进行迭代更新得到第二优化变量;通过第二优化变量可生成精度更高的地图。本发明的方案能够联合相邻两帧的激光点云数据和惯性测量数据对相邻两帧的状态量进行联合优化,并能够对下一帧的位姿进行预测,在满实时性要求的同时能够保证地图的精度。
Description
技术领域
本发明涉及无人驾驶技术领域,特别是涉及一种地图生成方法、计算机存储介质及电子设备。
背景技术
在无人驾驶领域,为了实现对车辆的定位,需要构造高精地图。然而,针对室外自然场景的建图,利用的都是一些传统算法,比如LOAM、LeGO、LOAM、segmatch等算法,而且这些传统算法都面临精度和速度的挑战,无法直接用于室外自然场景的建图。目前,通常对相邻两帧的激光数据使用帧间配准算法以辅助生成高精度地图。但现有的帧间配准算法不能很好地适用于车速较快的情况,这是因为车速过快时,比如90km/h,如果仅用相邻两帧的激光数据进行配准,此时没有初值,很容易配准失败,不利于地图构造。此外,当车辆高速行驶时,采集到的激光数据存在运动畸变,若基于帧间配准结果直接拼接点云,那么得到的地图轮廓将是错乱的,地图的精度不能得到有效保证,用户体验不佳。
发明内容
鉴于上述问题,提出了本发明以便提供一种克服上述问题或者至少部分地解决上述问题的地图生成方法、计算机存储介质及电子设备。
本发明的一个目的是要联合相邻两帧的激光点云数据和惯性测量数据对该相邻两帧的位姿、速度、加速度等进行优化,提高了帧间配准的精度,有利于生成更高精度地图。
本发明的一个进一步的目的是要基于当前帧以及当前帧的前一帧的联合优化的结果,再结合当前帧期间的惯性测量数据,对当前帧的激光点云数据进行运动补偿,使得生成的地图更加精确。
本发明的另一个进一步的目的是要采用Cuda对联合优化过程进行加速,提高了联合优化过程中的算法速度,满足了实时性的要求。
特别地,本发明提供了一种地图生成方法,其包括:
基于获取到的相邻两帧的激光点云数据计算激光残差以及第一雅克比矩阵;
确定第一待优化变量,并根据激光残差和所述第一雅克比矩阵计算第一待优化变量迭代的第一增量;
根据第一增量对第一待优化变量进行迭代更新,得到第一优化变量;
基于获取到的相邻两帧间的惯性测量数据计算惯性测量残差以及第二雅克比矩阵;
确定第二待优化变量,并根据惯性测量残差和第二雅克比矩阵计算第二待优化变量迭代的第二增量;
将第一优化变量作为初值,并根据第二增量对第二待优化变量进行迭代更新,得到第二优化变量;
基于第二优化变量进行地图生成。
可选地,基于第二优化变量进行地图生成包括:
基于第二优化变量对相邻两帧中后一帧的激光点云数据对应的激光点云进行运动补偿;
基于第一优化变量计算相邻两帧的激光点云数据分别对应的激光点云间的位姿变换关系;
将位姿变换关系作为初值,对相邻两帧的激光点云进行精配准;
基于精配准得到的轨迹以及运动补偿后的后一帧的激光点云进行地图生成。
可选地,基于第二优化变量对相邻两帧中后一帧的激光点云数据对应的激光点云进行运动补偿包括:
根据第二优化变量和所述后一帧的惯性测量数据计算相邻两帧中后一帧期间内的每个惯性测量数据在采集时的惯性坐标系位姿;
依次选取两个相邻的惯性坐标系位姿作为区间端点,得到惯性坐标系下相邻两帧中后一帧期间内的多个位姿区间;
获取相邻两帧中后一帧的激光点云中各激光点的时间戳;
根据各时间戳确定各激光点分别所属的目标位姿区间,目标位姿区间为多个位姿区间中的任一个;
基于各目标位姿区间估算各激光点对应的激光坐标系位姿;
通过对激光坐标系位姿进行坐标变换来对各激光点进行运动补偿。
可选地,基于各目标位姿区间估算各激光点对应的激光坐标系位姿包括:
获取目标位姿区间的两个区间端点;
对获取到的两个区间端点进行插值处理,得到各激光点对应的激光坐标系位姿。
可选地,基于获取到的相邻两帧的激光点云数据计算激光残差以及第一雅克比矩阵包括:
通过最近邻算法搜索相邻两帧的激光点云间所有配对激光点;
根据各配对激光点对应的激光点云数据,构造关于各配对激光点的激光残差函数;
根据各配对激光点的激光残差函数计算对应的各配对激光点的第一雅克比矩阵;
按照预设权重分别对各配对激光点的激光残差函数和各配对激光点的第一雅克比矩阵进行加权求和,得到激光残差以及第一雅克比矩阵。
可选地,第一待优化变量包括相邻两帧的位姿,第一增量随第一待优化变量的迭代更新次数的增加而减小,并且根据第一增量对第一待优化变量进行迭代更新包括:
判断第一增量是否小于第一设定阈值;
若是,停止对第一待优化变量的迭代更新,并记录迭代停止时的位姿作为第一优化变量。
可选地,第二待优化变量包括相邻两帧的位姿、速度、加速度偏置以及角速度偏置,第二增量随第二待优化变量的迭代更新次数的增加而减小,并且根据第二增量对第二待优化变量进行迭代更新包括:
判断第二增量是否小于第二设定阈值;
若是,停止对第二待优化变量的迭代更新,并记录迭代停止时的位姿、速度、加速度偏置以及角速度偏置作为第二优化变量。
可选地,基于获取到的相邻两帧间的惯性测量数据计算惯性测量残差以及第二雅克比矩阵包括:
对惯性测量数据进行预积分计算,得到惯性测量残差;
获取惯性测量残差关于第二优化变量的偏导,得到第二雅克比矩阵。
特别地,本发明还提供了一种计算机存储介质,计算机存储介质存储有计算机程序代码,当计算机程序代码在计算设备上运行时,导致计算设备执行上述任一的地图生成方法。
特别地,本发明还提供了一种电子设备,其包括:
处理器;
存储器,存储有计算机程序代码,当计算机程序代码被处理器运行时,导致电子设备执行上述任一的地图生成方法。
本发明通过计算相邻两帧的激光点云数据的激光残差以及第一雅克比矩阵,进而计算第一优化变量迭代的第一增量,之后根据第一增量对第一待优化变量进行迭代更新得到第一优化变量,同时通过计算相邻两帧的惯性测量数据的惯性测量残差以及第二雅克比矩阵,进而计算第二优化变量迭代的第二增量,然后将第一优化变量作为初值,并根据第二增量对第二待优化变量进行迭代更新得到第二优化变量,之后,基于第二优化变量进行地图生成。采用本发明的方案可以基于相邻两帧的激光点云数据和惯性测量数据对该相邻两帧的状态量进行联合优化,并能够对下一帧的位姿进行预测,提高了帧间配准的精度,从而能够有效保证地图的精度。
进一步地,本发明能够基于联合优化的结果,再结合一帧期间的惯性测量数据,对当前帧的激光点云数据进行运动补偿,使得生成的地图更加精确。
更进一步地,本发明能够对联合优化过程进行加速,提高了联合优化过程中的算法速度,能够满足实时性的要求。
根据下文结合附图对本发明具体实施例的详细描述,本领域技术人员将会更加明了本发明的上述以及其他目的、优点和特征。
附图说明
后文将参照附图以示例性而非限制性的方式详细描述本发明的一些具体实施例。附图中相同的附图标记标示了相同或类似的部件或部分。本领域技术人员应该理解,这些附图未必是按比例绘制的。附图中:
图1是根据本发明一个实施例的地图生成方法示意性流程图;
图2是根据本发明一实施例的相邻两帧的激光点云间的配对激光点示意图;
图3是根据本发明一个实施例的相邻两帧的状态量的示意图;
图4是根据本发明一实施例的参考轨迹示意图;
图5是根据本发明另一个实施例的地图生成方法流程图;
图6是根据本发明一个实施例的电子设备的示意性结构图。
具体实施方式
下面将参照附图更详细地描述本发明的示例性实施例。虽然附图中显示了本发明的示例性实施例,然而应当理解,可以以各种形式实现本发明而不应被这里阐述的实施例所限制。相反,提供这些实施例是为了能够更透彻地理解本发明,并且能够将本发明的范围完整的传达给本领域的技术人员。
图1是根据本发明一个实施例的地图生成方法示意性流程图。如图1所示,该实施例的地图生成方法至少包括步骤S102至步骤S114。
步骤S102,基于获取到的相邻两帧的激光点云数据计算激光残差以及第一雅克比矩阵。
步骤S104,确定第一待优化变量,并根据激光残差和第一雅克比矩阵计算第一待优化变量迭代的第一增量。
步骤S106,根据第一增量对第一待优化变量进行迭代更新,得到第一优化变量。
步骤S108,基于获取到的相邻两帧间的惯性测量数据计算惯性测量残差以及第二雅克比矩阵。
步骤S110,确定第二待优化变量,并根据惯性测量残差和第二雅克比矩阵计算第二待优化变量迭代的第二增量。
步骤S112,将第一优化变量作为初值,并根据第二增量对第二待优化变量进行迭代更新,得到第二优化变量。
步骤S114,基于第二优化变量进行地图生成。
采用本发明实施例可以基于相邻两帧的激光点云数据和惯性测量数据对该相邻两帧的状态量进行联合优化,并能够对下一帧的位姿进行预测,提高了帧间配准的精度,从而能够有效保证地图的精度。
在一些实施例中,在执行步骤S102之前,可通过激光雷达获取相邻两帧的激光点云数据。激光点云数据可配置成激光雷达测量到的障碍物在激光雷达坐标系下的坐标。此外,通过激光雷达获取的激光点云数据的频率可以很高,比如,10hz。
接下来运行步骤S102,在一些实施例中,可基于迭代最近点算法(IterativeClosest Point,ICP算法)或者各种变种的ICP算法计算激光残差和对应的第一雅克比矩阵。优选使用GICP算法(Generalized-ICP)计算激光残差和对应的第一雅克比矩阵。在另一些实施例中,步骤S102具体可包括如下步骤。
步骤1:通过最近邻算法搜索相邻两帧的激光点云间所有配对激光点。
步骤2:根据各配对激光点对应的激光点云数据,构造关于各配对激光点的激光残差函数。
步骤3:根据各配对激光点的激光残差函数计算对应的各所述配对激光点的第一雅克比矩阵。
步骤4:按照预设权重分别对各配对激光点的激光残差函数和各配对激光点的第一雅克比矩阵进行加权求和,得到激光残差以及所述第一雅克比矩阵。
通过上述步骤1至步骤4即可计算出关于相邻两帧的激光点云数据的激光残差和第一雅克比矩阵,然而在该计算时,对相邻两帧的激光点云数据对应的每对激光点,都需要计算激光残差和第一雅克比矩阵,这使得整个计算过程非常耗时。针对这种情况,在一些实施例中,可通过图形处理器(Graphics Processing Unit,GPU)计算激光残差和第一雅克比矩阵。本实施例通过在GPU上计算激光残差和第一雅克比矩阵,可加快计算速度,减少耗时,能够满足实时性的要求。
接下来运行步骤S104,在一些实施例中,可根据相邻两帧的状态量确定第一待优化变量,进而根据激光残差和第一雅克比矩阵计算第一待优化变量迭代的第一增量。相邻两帧的状态量包括但不限于相邻两帧的位姿(位置和姿态)、偏置(加速度偏置和角速度偏置)、速度。例如,根据相邻两帧的状态量确定出第一待优化变量可包括相邻两帧的位姿。
接下来运行步骤S106,根据第一增量对第一待优化变量进行迭代更新,得到第一优化变量。需要说明的是,在根据第一增量对第一待优化变量进行迭代更新时,可使第一增量随第一待优化变量的迭代更新次数的增加而减小。
在一些实施例中,在根据第一增量对第一待优化变量进行迭代更新过程中,可判断第一增量是否小于第一设定阈值,并在第一增量小于第一设定阈值时,停止对第一待优化变量的迭代更新,并记录迭代停止时的位姿作为第一优化变量。第一设定阈值可根据需要进行合理设置,以使激光测量残差最小为宜。
下面在一实施例中详细介绍激光残差和第一雅克比矩阵的计算过程。
图2是根据本发明一实施例的相邻两帧的激光点云间的配对激光点示意图。如图2所示,A为相邻两帧中后一帧,也即是当前帧的载体坐标系,为其在世界坐标系中的位姿;B为相邻两帧中前一帧的载体坐标系,为其在世界坐标系中的位姿;为相邻两帧通过最近邻算法搜索到任意一对配对激光点。其中,,是相邻两帧对应的需要优化的变量。载体坐标系设置为IMU所在的坐标系,该坐标系是固定在车辆上的,可随着车辆的移动而移动。
对于任意一对配对激光点,可根据该任意一对配对激光点对应的激光点云数据对应的位姿,,构造关于该任意一对配对激光点的激光残差函数。具体地,假设将该任意一对配对激光点的激光残差函数记作时,可构造激光残差函数如下述公式1至公式3所示:
其中:
上述公式1至公式3中各参数的物理意义分别为:
公式1中的激光残差函数是关于残差向量的二乘形式,可利用列文伯格-马夸尔特算法(Levenberg-Marquarelt,LM算法)对相邻两帧的位姿进行优化。
在一些实施例中,对相邻两帧的位置和姿态进行优化的优化过程可以为,首先定义第一待优化变量,并确定归一化后的残差向量,然后推导该任意一对配对激光点的第一雅克比矩阵。具体地,可基于李代数的右扰动模型推导该任意一对配对激光点的第一雅克比矩阵。
示意性的,在将第一待优化变量定义为时,可将归一化后的残差向量定义为。在一些实施例中,若设定表示相邻两帧间的任意一对配对激光点的第一雅克比矩阵,则基于李代数的右扰动模型推导出的该任意一对配对激光点的第一雅克比矩阵可如公式4所示:
同理,可计算出相邻两帧的激光点云的所有配对激光点的第一雅克比矩阵以及对应的激光残差函数。之后,可根据各配对激光点的第一雅克比矩阵和残差函数计算第一待优化变量迭代的第一增量。在一些实施例中,若设定表示第一增量,则可根据公式5计算第一增量:
为了进一步加快计算速度,在一些实施例,可通过GPU对每对的配对激光点的第一雅克比矩阵和激光残差函数分别进行加权求和,再计算第一增量。
在计算出第一增量后,可运行步骤S106,根据第一增量对第一待优化变量进行迭代更新。在一些实施例中,可根据公式6对第一待优化变量进行迭代更新,以使激光残差最小,从而得到第一优化变量:
在一些实施例中,在执行步骤S108之前,可通过惯性测量单元获取惯性测量数据。惯性测量数据可包括3轴的加速度和3轴的角速度。
接下来运行步骤S108,在一些实施例中,在基于获取到的相邻两帧间的惯性测量数据计算惯性测量残差以及第二雅克比矩阵时,可对惯性测量数据进行预积分计算得到惯性测量残差,进而获取惯性测量残差关于第二待优化变量的偏导得到第二雅克比矩阵。通过预积分计算可快速计算出惯性测量残差和对应的第二雅克比矩阵,以便对第二待优化变量进行优化。
接下来运行步骤S110,在一些实施例中,可根据相邻两帧的状态量确定第二待优化变量,进而根据惯性测量残差和第二雅克比矩阵计算第二待优化变量迭代的第二增量。相邻两帧的状态量包括但不限于相邻两帧的位姿(位置和姿态)、偏置(加速度偏置和角速度偏置)和速度。例如,根据相邻两帧的状态量确定出第二待优化变量可包括相邻两帧的位姿、速度以及偏置。
接下来运行步骤S112,根据第二增量对第一待优化变量进行迭代更新,得到第二优化变量。需要说明的是,在根据第二增量对第二待优化变量进行迭代更新时,可使第二增量随第二待优化变量的迭代更新次数的增加而减小。
在一些实施例中,在根据第二增量对第二待优化变量进行迭代更新过程中,可判断第二增量是否小于第二设定阈值,并在第二增量小于第二设定阈值时,停止对第二待优化变量的迭代更新,并记录迭代停止时的位姿、速度、偏置作为第二优化变量。第二设定阈值可根据需要进行合理设置,以使惯性测量残差最小为宜。
下面在一实施例中详细介绍对第二待优化变量的优化过程。
其中,是相邻两帧中前一帧的位置,是相邻两帧中后一帧的位置;是相邻两帧中前一帧的姿态的3*3矩阵表示,是相邻两帧中前一帧的姿态的四元数表示,是相邻两帧中后一帧的姿态的四元数表示;是相邻两帧中前一帧的速度,是相邻两帧中后一帧的速度;是相邻两帧中前一帧的加速度偏置,是相邻两帧中后一帧的加速度偏置;是相邻两帧中前一帧的角度偏置,是相邻两帧中后一帧的角度偏置;是相邻两帧间的时间间隔,是通过预积分计算出的相邻两帧中后一帧和前一帧的姿态变化,是预积分计算出的相邻两帧中后一帧和前一帧的位置变化,是预积分计算出的相邻两帧中后一帧和前一帧的速度变化,是初始时刻的重力加速度值。
需要说明的是,对于惯性测量数据而言,第二雅克比矩阵和惯性测量残差只有一项,在实际应用中,可通过CPU直接计算第二雅克比矩阵和惯性测量残差。
在计算出第二增量后,可运行步骤S112,将第一优化变量作为初值,并根据第二增量对第二待优化变量进行迭代更新,得到第二优化变量。具体优化过程可通过公式10和公式11实现:
由此,通过上述优化过程即可得到相邻两帧中的后一帧和前一帧的位姿、速度和偏置。
在实际应用中,由于各种原因,采集的激光点云数据不可避免存在运动畸变,特别是车辆高速行驶时。为了消除运动畸变,在一些实施例中,还可基于第二优化变量对相邻两帧中后一帧的激光点云数据对应的激光点云进行运动补偿,然后基于第一优化变量计算相邻两帧的激光点云数据分别对应的激光点云间的位姿变换关系(包括位置变换关系和姿态变换关系),之后,将位姿变换关系作为初值,对相邻两帧的激光点云进行精配准,进而基于精配准得到的轨迹以及运动补偿后的相邻两帧中后一帧的激光点云进行地图生成。本实施例使用的是精配准得到的轨迹以及运动补偿后的激光点云进行地图生成,由于轨迹更加精确,并且没有畸变,此时的地图将更加精确。
在一些实施例中,在基于第二优化变量对相邻两帧中后一帧的激光点云数据对应的激光点云进行运动补偿时,首先,可根据第二优化变量和相邻两帧中后一帧的惯性测量数据计算相邻两帧中后一帧期间内的每个惯性测量数据在采集时的惯性坐标系位姿。其次,可依次选取两个相邻的惯性坐标系位姿作为区间端点,从而得到惯性坐标系下相邻两帧中后一帧期间内的多个位姿区间。然后,获取相邻两帧中后一帧的激光点云中各激光点的时间戳,进而根据各时间戳确定各激光点分别所属的目标位姿区间,基于各目标位姿区间估算各激光点对应的激光坐标系位姿,通过对激光坐标系位姿进行坐标变换来对各激光点进行运动补偿。该实施例通过计算激光点在帧开始时在载体系先的位姿,即可实现对激光点云的运动补偿,从而可消除激光点云的运动畸变,有利于提高地图精度。
在一些实施例中,基于各目标位姿区间估算各激光点对应的激光坐标系位姿可包括:获取目标位姿区间的两个区间端点;对获取到的两个区间端点进行插值处理,得到各激光点对应的激光坐标系位姿。
上文实施例提及的目标位姿区间为多个位姿区间中的任一个。多个位姿区间即可构成参考轨迹。
图4是根据本发明一实施例的参考轨迹示意图。如图4所示,图4中示出的横轴为时间轴,两端表示一帧点云的开始时刻和结束时刻,帧间的空白圆圈表示通过中值积分得到的参考位置,对于100hz的惯性测量数据,帧间可以有10个参考位置,在进行运动补偿时,可将相邻参考位置对应之间的激光点标记为待补偿点,先通过插值方式估算待补偿点的位姿,再通过坐标转换计算待补偿点在帧的开始时刻的在载体坐标系中的位姿。
对激光点云进行运动补偿的过程可包括如下。
图5是根据本发明另一个实施例的地图生成方法流程图。参见图5所示,该实施例的地图生成方法可包括如下步骤S502至步骤S530。
步骤S502,获取并记录相邻两帧的激光点云数据以及惯性测量数据。
步骤S504,基于激光点云数据计算激光残差以及第一雅克比矩阵。
步骤S506,根据相邻两帧的位姿确定第一待优化变量,并根据激光残差和第一雅克比矩阵计算第一待优化变量迭代的第一增量。
步骤S508,根据第一增量对第一待优化变量进行迭代更新。
步骤S510,判断第一增量是否小于第一设定阈值。若是,执行步骤S512;若否,返回步骤S508。
步骤S512,停止对第一待优化变量的迭代更新,并记录迭代停止时的位姿作为第一优化变量。
步骤S514,基于惯性测量数据计算惯性测量残差以及第二雅克比矩阵。
步骤S516,根据相邻两帧的位姿、速度、偏置确定第二待优化变量,并根据惯性测量残差和第二雅克比矩阵计算第二待优化变量迭代的第二增量。
步骤S518,将第一优化变量作为初值,并根据第二增量对第二待优化变量进行迭代更新。
步骤S520,判断第二增量是否小于第二设定阈值。若是,执行步骤S522;若否,返回步骤S518。
步骤S522,停止对第二待优化变量的迭代更新,并记录迭代停止时的位姿、速度、加速度偏置以及角速度偏置作为第二优化变量。
步骤S524,基于第二优化变量对相邻两帧中后一帧的激光点云数据对应的激光点云进行运动补偿。
步骤S526,基于第一优化变量计算相邻两帧的激光点云数据分别对应的激光点云间的位姿变换关系。
步骤S528,将位姿变换关系作为初值,对相邻两帧的激光点云进行精配准。
步骤S530,基于精配准得到的轨迹以及运动补偿后的后一帧的激光点云进行地图生成。
基于同一发明构思,在本发明一些实施例中,还提供了计算机存储介质。其中,计算机存储介质存储有计算机程序代码,当计算机程序代码在计算设备上运行时,导致计算设备执行上述任一实施例中的基于样条插值的曲线绘制方法。
图6是根据本发明一个实施例的电子设备600的示意性结构图。参见图6所示,本实施例中还提供了一种电子设备600。该电子设备600可包括处理器610以及存储器620。
存储器620存储有计算机程序代码,当计算机程序代码被处理器610运行时,导致电子设备600执行上述任一实施例中的地图生成方法。
联合相邻两帧的激光点云数据和惯性测量数据对该相邻两帧的状态量进行联合优化,能够对下一帧的位姿进行预测,提高了帧间配准的精度,从而有效提高了地图的精确性。
进一步地,本发明能够基于联合优化的结果,再结合一帧期间的惯性测量数据,对当前帧的激光点云数据进行运动补偿,使得生成的地图更加精确。
更进一步地,本发明能够对联合优化过程进行加速,提高了联合优化过程中的算法速度,能够满足实时性的要求。
至此,本领域技术人员应认识到,虽然本文已详尽示出和描述了本发明的多个示例性实施例,但是,在不脱离本发明精神和范围的情况下,仍可根据本发明公开的内容直接确定或推导出符合本发明原理的许多其他变型或修改。因此,本发明的范围应被理解和认定为覆盖了所有这些其他变型或修改。
Claims (10)
1.一种地图生成方法,包括:
基于获取到的相邻两帧的激光点云数据计算激光残差以及第一雅克比矩阵;
确定第一待优化变量,并根据所述激光残差和所述第一雅克比矩阵计算所述第一待优化变量迭代的第一增量;
根据所述第一增量对所述第一待优化变量进行迭代更新,得到第一优化变量;
基于获取到的所述相邻两帧间的惯性测量数据计算惯性测量残差以及第二雅克比矩阵;
确定第二待优化变量,并根据所述惯性测量残差和所述第二雅克比矩阵计算所述第二待优化变量迭代的第二增量;
将所述第一优化变量作为初值,并根据所述第二增量对所述第二待优化变量进行迭代更新,得到第二优化变量;
基于所述第二优化变量进行地图生成。
2.根据权利要求1所述的地图生成方法,其特征在于,所述基于所述第二优化变量进行地图生成包括:
基于所述第二优化变量对所述相邻两帧中后一帧的激光点云数据对应的激光点云进行运动补偿;
基于所述第一优化变量计算所述相邻两帧的激光点云数据分别对应的激光点云间的位姿变换关系;
将所述位姿变换关系作为初值,对所述相邻两帧的激光点云进行精配准;
基于精配准得到的轨迹以及运动补偿后的所述后一帧的激光点云进行地图生成。
3.根据权利要求2所述的地图生成方法,其特征在于,所述基于所述第二优化变量对所述相邻两帧中后一帧的激光点云数据对应的激光点云进行运动补偿包括:
根据所述第二优化变量和所述后一帧的惯性测量数据计算所述后一帧期间内的每个所述惯性测量数据在采集时的惯性坐标系位姿;
依次选取两个相邻的所述惯性坐标系位姿作为区间端点,得到惯性坐标系下所述后一帧期间内的多个位姿区间;
获取所述后一帧的所述激光点云中各激光点的时间戳;
根据各所述时间戳确定各所述激光点分别所属的目标位姿区间,所述目标位姿区间为所述多个位姿区间中的任一个;
基于各所述目标位姿区间估算各所述激光点对应的激光坐标系位姿;
通过对所述激光坐标系位姿进行坐标变换来对各所述激光点进行运动补偿。
4.根据权利要求3所述的地图生成方法,其特征在于,所述基于各所述目标位姿区间估算各所述激光点对应的激光坐标系位姿包括:
获取所述目标位姿区间的两个区间端点;
对获取到的所述两个区间端点进行插值处理,得到各所述激光点对应的激光坐标系位姿。
5.根据权利要求1所述的地图生成方法,其特征在于,所述基于获取到的相邻两帧的激光点云数据计算激光残差以及第一雅克比矩阵包括:
通过最近邻算法搜索所述相邻两帧的激光点云间所有配对激光点;
根据各所述配对激光点对应的激光点云数据,构造关于各所述配对激光点的激光残差函数;
根据各所述配对激光点的激光残差函数计算对应的各所述配对激光点的第一雅克比矩阵;
按照预设权重分别对各所述配对激光点的激光残差函数和各所述配对激光点的第一雅克比矩阵进行加权求和,得到所述激光残差以及所述第一雅克比矩阵。
6.根据权利要求1所述的地图生成方法,其特征在于,所述第一待优化变量包括所述相邻两帧的位姿,所述第一增量随所述第一待优化变量的迭代更新次数的增加而减小,并且所述根据所述第一增量对所述第一待优化变量进行迭代更新包括:
判断所述第一增量是否小于第一设定阈值;
若是,停止对所述第一待优化变量的迭代更新,并记录迭代停止时的位姿作为所述第一优化变量。
7.根据权利要求1所述的地图生成方法,其特征在于,所述第二待优化变量包括所述相邻两帧的位姿、速度、加速度偏置以及角速度偏置,所述第二增量随所述第二待优化变量的迭代更新次数的增加而减小,并且所述根据所述第二增量对所述第二待优化变量进行迭代更新包括:
判断所述第二增量是否小于第二设定阈值;
若是,停止对所述第二待优化变量的迭代更新,并记录迭代停止时的位姿、速度、加速度偏置以及角速度偏置作为所述第二优化变量。
8.根据权利要求1所述的地图生成方法,其特征在于,所述基于获取到的所述相邻两帧间的惯性测量数据计算惯性测量残差以及第二雅克比矩阵包括:
对所述惯性测量数据进行预积分计算,得到所述惯性测量残差;
获取所述惯性测量残差关于所述第二优化变量的偏导,得到所述第二雅克比矩阵。
9.一种计算机存储介质,所述计算机存储介质存储有计算机程序代码,当所述计算机程序代码在计算设备上运行时,导致所述计算设备执行权利要求1-8中任一项所述的地图生成方法。
10.一种电子设备,包括:
处理器;
存储器,存储有计算机程序代码,当所述计算机程序代码被所述处理器运行时,导致所述电子设备执行权利要求1-8中任一项所述的地图生成方法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202011264528.1A CN112067007B (zh) | 2020-11-12 | 2020-11-12 | 地图生成方法、计算机存储介质及电子设备 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202011264528.1A CN112067007B (zh) | 2020-11-12 | 2020-11-12 | 地图生成方法、计算机存储介质及电子设备 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN112067007A true CN112067007A (zh) | 2020-12-11 |
CN112067007B CN112067007B (zh) | 2021-01-29 |
Family
ID=73655107
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202011264528.1A Active CN112067007B (zh) | 2020-11-12 | 2020-11-12 | 地图生成方法、计算机存储介质及电子设备 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN112067007B (zh) |
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN112506200A (zh) * | 2020-12-14 | 2021-03-16 | 广州视源电子科技股份有限公司 | 机器人定位方法、装置、机器人及存储介质 |
CN112945240A (zh) * | 2021-03-16 | 2021-06-11 | 北京三快在线科技有限公司 | 特征点位置的确定方法、装置、设备及可读存储介质 |
Citations (11)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20190066318A1 (en) * | 2016-07-18 | 2019-02-28 | King Abdullah University Of Science And Technology | System and method for three-dimensional image reconstruction using an absolute orientation sensor |
CN109934920A (zh) * | 2019-05-20 | 2019-06-25 | 奥特酷智能科技(南京)有限公司 | 基于低成本设备的高精度三维点云地图构建方法 |
CN110060202A (zh) * | 2019-04-19 | 2019-07-26 | 湖北亿咖通科技有限公司 | 一种单目slam算法的初始化方法及系统 |
CN110118554A (zh) * | 2019-05-16 | 2019-08-13 | 深圳前海达闼云端智能科技有限公司 | 基于视觉惯性的slam方法、装置、存储介质和设备 |
CN110261870A (zh) * | 2019-04-15 | 2019-09-20 | 浙江工业大学 | 一种用于视觉-惯性-激光融合的同步定位与建图方法 |
WO2020024144A1 (zh) * | 2018-08-01 | 2020-02-06 | 广东朗呈医疗器械科技有限公司 | 一种三维成像方法、装置及终端设备 |
CN110879400A (zh) * | 2019-11-27 | 2020-03-13 | 炬星科技(深圳)有限公司 | 激光雷达与imu融合定位的方法、设备及存储介质 |
CN111402339A (zh) * | 2020-06-01 | 2020-07-10 | 深圳市智绘科技有限公司 | 一种实时定位方法、装置、系统及存储介质 |
CN111650598A (zh) * | 2019-02-19 | 2020-09-11 | 北京京东尚科信息技术有限公司 | 一种车载激光扫描系统外参标定方法和装置 |
CN111665512A (zh) * | 2019-02-21 | 2020-09-15 | 香港科技大学 | 基于3d激光雷达和惯性测量单元的融合的测距和绘图 |
CN111710040A (zh) * | 2020-06-03 | 2020-09-25 | 纵目科技(上海)股份有限公司 | 一种高精度地图的构建方法、系统、终端和存储介质 |
-
2020
- 2020-11-12 CN CN202011264528.1A patent/CN112067007B/zh active Active
Patent Citations (11)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20190066318A1 (en) * | 2016-07-18 | 2019-02-28 | King Abdullah University Of Science And Technology | System and method for three-dimensional image reconstruction using an absolute orientation sensor |
WO2020024144A1 (zh) * | 2018-08-01 | 2020-02-06 | 广东朗呈医疗器械科技有限公司 | 一种三维成像方法、装置及终端设备 |
CN111650598A (zh) * | 2019-02-19 | 2020-09-11 | 北京京东尚科信息技术有限公司 | 一种车载激光扫描系统外参标定方法和装置 |
CN111665512A (zh) * | 2019-02-21 | 2020-09-15 | 香港科技大学 | 基于3d激光雷达和惯性测量单元的融合的测距和绘图 |
CN110261870A (zh) * | 2019-04-15 | 2019-09-20 | 浙江工业大学 | 一种用于视觉-惯性-激光融合的同步定位与建图方法 |
CN110060202A (zh) * | 2019-04-19 | 2019-07-26 | 湖北亿咖通科技有限公司 | 一种单目slam算法的初始化方法及系统 |
CN110118554A (zh) * | 2019-05-16 | 2019-08-13 | 深圳前海达闼云端智能科技有限公司 | 基于视觉惯性的slam方法、装置、存储介质和设备 |
CN109934920A (zh) * | 2019-05-20 | 2019-06-25 | 奥特酷智能科技(南京)有限公司 | 基于低成本设备的高精度三维点云地图构建方法 |
CN110879400A (zh) * | 2019-11-27 | 2020-03-13 | 炬星科技(深圳)有限公司 | 激光雷达与imu融合定位的方法、设备及存储介质 |
CN111402339A (zh) * | 2020-06-01 | 2020-07-10 | 深圳市智绘科技有限公司 | 一种实时定位方法、装置、系统及存储介质 |
CN111710040A (zh) * | 2020-06-03 | 2020-09-25 | 纵目科技(上海)股份有限公司 | 一种高精度地图的构建方法、系统、终端和存储介质 |
Non-Patent Citations (2)
Title |
---|
LI YANG,ETC: "The Millimeter-Wave Radar SLAM Assisted by the RCS Feature of the Target and IMU", 《SENSORS》 * |
余彤: "LiDAR/INS组合的室内定位与制图(SLAM)算法改进", 《中国优秀硕士学位论文全文数据库信息科技辑》 * |
Cited By (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN112506200A (zh) * | 2020-12-14 | 2021-03-16 | 广州视源电子科技股份有限公司 | 机器人定位方法、装置、机器人及存储介质 |
CN112506200B (zh) * | 2020-12-14 | 2023-12-08 | 广州视源电子科技股份有限公司 | 机器人定位方法、装置、机器人及存储介质 |
CN112945240A (zh) * | 2021-03-16 | 2021-06-11 | 北京三快在线科技有限公司 | 特征点位置的确定方法、装置、设备及可读存储介质 |
CN112945240B (zh) * | 2021-03-16 | 2022-06-07 | 北京三快在线科技有限公司 | 特征点位置的确定方法、装置、设备及可读存储介质 |
Also Published As
Publication number | Publication date |
---|---|
CN112067007B (zh) | 2021-01-29 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN111207774B (zh) | 一种用于激光-imu外参标定的方法及系统 | |
CN112347840B (zh) | 视觉传感器激光雷达融合无人机定位与建图装置和方法 | |
CN110243358B (zh) | 多源融合的无人车室内外定位方法及系统 | |
CN107160395B (zh) | 地图构建方法及机器人控制系统 | |
CN109211251B (zh) | 一种基于激光和二维码融合的即时定位与地图构建方法 | |
CN112985416B (zh) | 激光与视觉信息融合的鲁棒定位和建图方法及系统 | |
JP6760114B2 (ja) | 情報処理装置、データ管理装置、データ管理システム、方法、及びプログラム | |
CN109059907B (zh) | 轨迹数据处理方法、装置、计算机设备和存储介质 | |
US20130162785A1 (en) | Method and system for fusing data arising from image sensors and from motion or position sensors | |
JP7131994B2 (ja) | 自己位置推定装置、自己位置推定方法、自己位置推定プログラム、学習装置、学習方法及び学習プログラム | |
CN112067007B (zh) | 地图生成方法、计算机存储介质及电子设备 | |
CN110726406A (zh) | 一种改进的非线性优化单目惯导slam的方法 | |
CN112254729A (zh) | 一种基于多传感器融合的移动机器人定位方法 | |
CN112444246B (zh) | 高精度的数字孪生场景中的激光融合定位方法 | |
CN115479598A (zh) | 基于多传感器融合的定位与建图方法及紧耦合系统 | |
CN115218906A (zh) | 面向室内slam的视觉惯性融合定位方法及系统 | |
CN113674412B (zh) | 基于位姿融合优化的室内地图构建方法、系统及存储介质 | |
Jiang et al. | 3D SLAM based on NDT matching and ground constraints for ground robots in complex environments | |
CN114046800B (zh) | 一种基于双层滤波框架的高精度里程估计方法 | |
Nitsche et al. | Visual-inertial teach & repeat for aerial robot navigation | |
CN114812601A (zh) | 视觉惯性里程计的状态估计方法、装置、电子设备 | |
CN110864685A (zh) | 一种基于松耦合的车辆单目视觉轮式里程计定位方法 | |
CN110849349A (zh) | 一种基于磁传感器与轮式里程计融合定位方法 | |
CN107796417B (zh) | 一种自适应估计景象匹配与惯导安装误差的方法 | |
CN111879323B (zh) | 一种基于前端融合的航向角计算方法 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |