CN112067007B - 地图生成方法、计算机存储介质及电子设备 - Google Patents

地图生成方法、计算机存储介质及电子设备 Download PDF

Info

Publication number
CN112067007B
CN112067007B CN202011264528.1A CN202011264528A CN112067007B CN 112067007 B CN112067007 B CN 112067007B CN 202011264528 A CN202011264528 A CN 202011264528A CN 112067007 B CN112067007 B CN 112067007B
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.)
Active
Application number
CN202011264528.1A
Other languages
English (en)
Other versions
CN112067007A (zh
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.)
Hubei Ecarx Technology Co Ltd
Original Assignee
Hubei Ecarx Technology Co Ltd
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 Hubei Ecarx Technology Co Ltd filed Critical Hubei Ecarx Technology Co Ltd
Priority to CN202011264528.1A priority Critical patent/CN112067007B/zh
Publication of CN112067007A publication Critical patent/CN112067007A/zh
Application granted granted Critical
Publication of CN112067007B publication Critical patent/CN112067007B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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为相邻两帧中后一帧，也即是当前帧的载体坐标系，
Figure 666112DEST_PATH_IMAGE001
为其在世界坐标系中的位姿；B为相邻两帧中前一帧的载体坐标系，
Figure 858059DEST_PATH_IMAGE002
为其在世界坐标系中的位姿；
Figure 469169DEST_PATH_IMAGE003
为相邻两帧通过最近邻算法搜索到任意一对配对激光点。其中，
Figure 448889DEST_PATH_IMAGE004
，
Figure 388026DEST_PATH_IMAGE005
是相邻两帧对应的需要优化的变量。载体坐标系设置为IMU所在的坐标系，该坐标系是固定在车辆上的，可随着车辆的移动而移动。
对于任意一对配对激光点
Figure 773877DEST_PATH_IMAGE003
，可根据该任意一对配对激光点
Figure 239493DEST_PATH_IMAGE003
对应的激光点云数据对应的位姿
Figure 639382DEST_PATH_IMAGE005
，
Figure 924869DEST_PATH_IMAGE004
，构造关于该任意一对配对激光点
Figure 84718DEST_PATH_IMAGE003
的激光残差函数。具体地，假设将该任意一对配对激光点
Figure 280207DEST_PATH_IMAGE003
的激光残差函数记作
Figure 241209DEST_PATH_IMAGE006
时，可构造激光残差函数如下述公式1至公式3所示：
Figure 404206DEST_PATH_IMAGE007
公式1
其中：
Figure 351434DEST_PATH_IMAGE008
公式2
Figure 526063DEST_PATH_IMAGE009
公式3
上述公式1至公式3中各参数的物理意义分别为：
Figure 284066DEST_PATH_IMAGE010
：残差向量；
Figure 216250DEST_PATH_IMAGE011
：在前一帧中利用
Figure 888540DEST_PATH_IMAGE012
邻域内10个点构造的协方差矩阵；
Figure 730725DEST_PATH_IMAGE013
：在当前帧中利用
Figure 269416DEST_PATH_IMAGE014
邻域内10个点构造的协方差矩阵；
Figure 16792DEST_PATH_IMAGE015
：用于对残差向量
Figure 820669DEST_PATH_IMAGE010
归一化，是
Figure 110836DEST_PATH_IMAGE016
经过三角（LLT）分解后的量，其为3*3的矩阵；
Figure 753518DEST_PATH_IMAGE017
：归一化后的残差向量，其为3*1的列向量。
公式1中的激光残差函数是关于残差向量的二乘形式，可利用列文伯格-马夸尔特算法（Levenberg-Marquarelt，LM算法）对相邻两帧的位姿进行优化。
在一些实施例中，对相邻两帧的位置和姿态进行优化的优化过程可以为，首先定义第一待优化变量，并确定归一化后的残差向量，然后推导该任意一对配对激光点的第一雅克比矩阵。具体地，可基于李代数的右扰动模型推导该任意一对配对激光点的第一雅克比矩阵。
示意性的，在将第一待优化变量定义为
Figure 129136DEST_PATH_IMAGE018
时，可将归一化后的残差向量定义为
Figure 143228DEST_PATH_IMAGE019
。在一些实施例中，若设定
Figure 835372DEST_PATH_IMAGE020
表示相邻两帧间的任意一对配对激光点的第一雅克比矩阵，则基于李代数的右扰动模型推导出的该任意一对配对激光点的第一雅克比矩阵可如公式4所示：
Figure 886505DEST_PATH_IMAGE021
公式4
需要说明的是，第一雅克比矩阵
Figure 733107DEST_PATH_IMAGE020
为3行12列的数组。
同理，可计算出相邻两帧的激光点云的所有配对激光点的第一雅克比矩阵以及对应的激光残差函数。之后，可根据各配对激光点的第一雅克比矩阵和残差函数计算第一待优化变量迭代的第一增量。在一些实施例中，若设定
Figure 895098DEST_PATH_IMAGE022
表示第一增量，则可根据公式5计算第一增量
Figure 379431DEST_PATH_IMAGE022
：
Figure 460519DEST_PATH_IMAGE023
公式5
其中，
Figure 545150DEST_PATH_IMAGE024
为相邻两帧的激光点云间的配对激光点的对数。
Figure 494520DEST_PATH_IMAGE025
为单位矩阵，
Figure 879365DEST_PATH_IMAGE026
为单位矩阵
Figure 491875DEST_PATH_IMAGE025
的系数。
为了进一步加快计算速度，在一些实施例，可通过GPU对每对的配对激光点的第一雅克比矩阵和激光残差函数分别进行加权求和，再计算第一增量。
在计算出第一增量后，可运行步骤S106，根据第一增量对第一待优化变量进行迭代更新。在一些实施例中，可根据公式6对第一待优化变量进行迭代更新，以使激光残差最小，从而得到第一优化变量：
Figure 860539DEST_PATH_IMAGE027
公式6
在一些实施例中，在执行步骤S108之前，可通过惯性测量单元获取惯性测量数据。惯性测量数据可包括3轴的加速度和3轴的角速度。
接下来运行步骤S108，在一些实施例中，在基于获取到的相邻两帧间的惯性测量数据计算惯性测量残差以及第二雅克比矩阵时，可对惯性测量数据进行预积分计算得到惯性测量残差，进而获取惯性测量残差关于第二待优化变量的偏导得到第二雅克比矩阵。通过预积分计算可快速计算出惯性测量残差和对应的第二雅克比矩阵，以便对第二待优化变量进行优化。
接下来运行步骤S110，在一些实施例中，可根据相邻两帧的状态量确定第二待优化变量，进而根据惯性测量残差和第二雅克比矩阵计算第二待优化变量迭代的第二增量。相邻两帧的状态量包括但不限于相邻两帧的位姿（位置和姿态）、偏置（加速度偏置和角速度偏置）和速度。例如，根据相邻两帧的状态量确定出第二待优化变量可包括相邻两帧的位姿、速度以及偏置。
接下来运行步骤S112，根据第二增量对第二待优化变量进行迭代更新，得到第二优化变量。需要说明的是，在根据第二增量对第二待优化变量进行迭代更新时，可使第二增量随第二待优化变量的迭代更新次数的增加而减小。
在一些实施例中，在根据第二增量对第二待优化变量进行迭代更新过程中，可判断第二增量是否小于第二设定阈值，并在第二增量小于第二设定阈值时，停止对第二待优化变量的迭代更新，并记录迭代停止时的位姿、速度、偏置作为第二优化变量。第二设定阈值可根据需要进行合理设置，以使惯性测量残差最小为宜。
下面在一实施例中详细介绍对第二待优化变量的优化过程。
图3是根据本发明一个实施例的相邻两帧的状态量的示意图。假设惯性测量残差记为
Figure 348021DEST_PATH_IMAGE028
，则可基于图3中示出的相邻两帧间的惯性测量数据，并根据公式7构造惯性测量残差：
Figure 852952DEST_PATH_IMAGE029
公式7
其中，
Figure 636362DEST_PATH_IMAGE030
是相邻两帧中前一帧的位置，
Figure 695585DEST_PATH_IMAGE031
是相邻两帧中后一帧的位置；
Figure 986758DEST_PATH_IMAGE032
是相邻两帧中前一帧的姿态的3*3矩阵表示，
Figure 408512DEST_PATH_IMAGE033
是相邻两帧中前一帧的姿态的四元数表示，
Figure 877671DEST_PATH_IMAGE034
是相邻两帧中后一帧的姿态的四元数表示；
Figure 909343DEST_PATH_IMAGE035
是相邻两帧中前一帧的速度，
Figure 613994DEST_PATH_IMAGE036
是相邻两帧中后一帧的速度；
Figure 296779DEST_PATH_IMAGE037
是相邻两帧中前一帧的加速度偏置，
Figure 186106DEST_PATH_IMAGE038
是相邻两帧中后一帧的加速度偏置；
Figure 78976DEST_PATH_IMAGE039
是相邻两帧中前一帧的角度偏置，
Figure 744575DEST_PATH_IMAGE040
是相邻两帧中后一帧的角度偏置；
Figure 547446DEST_PATH_IMAGE041
是相邻两帧间的时间间隔，
Figure 640298DEST_PATH_IMAGE042
是通过预积分计算出的相邻两帧中后一帧和前一帧的姿态变化，
Figure 895830DEST_PATH_IMAGE043
是预积分计算出的相邻两帧中后一帧和前一帧的位置变化，
Figure 676704DEST_PATH_IMAGE044
是预积分计算出的相邻两帧中后一帧和前一帧的速度变化，
Figure 583349DEST_PATH_IMAGE045
是初始时刻的重力加速度值。
其中，第二雅克比矩阵，可通过计算惯性测量残差关于第二待优化变量中各个变量的偏导来计算。在一些实施例中，若设定
Figure 565211DEST_PATH_IMAGE046
表示第二雅克比矩阵，则可根据公式8计算第二雅克比矩阵：
Figure 432673DEST_PATH_IMAGE047
公式8
需要说明的是，对于惯性测量数据而言，第二雅克比矩阵和惯性测量残差只有一项，在实际应用中，可通过CPU直接计算第二雅克比矩阵和惯性测量残差。
在计算出第二雅克比矩阵后，可根据惯性测量残差和第二雅克比矩阵计算第二待优化变量迭代的第二增量。在一些实施例中，可设定
Figure 897197DEST_PATH_IMAGE048
表示第二增量，进而可根据公式9计算第二增量
Figure 409081DEST_PATH_IMAGE048
：
Figure 76692DEST_PATH_IMAGE049
公式9
在计算出第二增量后，可运行步骤S112，将第一优化变量作为初值，并根据第二增量对第二待优化变量进行迭代更新，得到第二优化变量。具体优化过程可通过公式10和公式11实现：
Figure 306816DEST_PATH_IMAGE050
公式10
Figure 586750DEST_PATH_IMAGE051
公式11
由此，通过上述优化过程即可得到相邻两帧中的后一帧和前一帧的位姿、速度和偏置。
在实际应用中，由于各种原因，采集的激光点云数据不可避免存在运动畸变，特别是车辆高速行驶时。为了消除运动畸变，在一些实施例中，还可基于第二优化变量对相邻两帧中后一帧的激光点云数据对应的激光点云进行运动补偿，然后基于第一优化变量计算相邻两帧的激光点云数据分别对应的激光点云间的位姿变换关系（包括位置变换关系和姿态变换关系），之后，将位姿变换关系作为初值，对相邻两帧的激光点云进行精配准，进而基于精配准得到的轨迹以及运动补偿后的相邻两帧中后一帧的激光点云进行地图生成。本实施例使用的是精配准得到的轨迹以及运动补偿后的激光点云进行地图生成，由于轨迹更加精确，并且没有畸变，此时的地图将更加精确。
在一些实施例中，在基于第二优化变量对相邻两帧中后一帧的激光点云数据对应的激光点云进行运动补偿时，首先，可根据第二优化变量和相邻两帧中后一帧的惯性测量数据计算相邻两帧中后一帧期间内的每个惯性测量数据在采集时的惯性坐标系位姿。其次，可依次选取两个相邻的惯性坐标系位姿作为区间端点，从而得到惯性坐标系下相邻两帧中后一帧期间内的多个位姿区间。然后，获取相邻两帧中后一帧的激光点云中各激光点的时间戳，进而根据各时间戳确定各激光点分别所属的目标位姿区间，基于各目标位姿区间估算各激光点对应的激光坐标系位姿，通过对激光坐标系位姿进行坐标变换来对各激光点进行运动补偿。该实施例通过计算激光点在帧开始时在载体系先的位姿，即可实现对激光点云的运动补偿，从而可消除激光点云的运动畸变，有利于提高地图精度。
在一些实施例中，基于各目标位姿区间估算各激光点对应的激光坐标系位姿可包括：获取目标位姿区间的两个区间端点；对获取到的两个区间端点进行插值处理，得到各激光点对应的激光坐标系位姿。
上文实施例提及的目标位姿区间为多个位姿区间中的任一个。多个位姿区间即可构成参考轨迹。
图4是根据本发明一实施例的参考轨迹示意图。如图4所示，图4中示出的横轴为时间轴，两端表示一帧点云的开始时刻和结束时刻，帧间的空白圆圈表示通过中值积分得到的参考位置，对于100hz的惯性测量数据，帧间可以有10个参考位置，在进行运动补偿时，可将相邻参考位置对应之间的激光点标记为待补偿点，先通过插值方式估算待补偿点的位姿，再通过坐标转换计算待补偿点在帧的开始时刻的在载体坐标系中的位姿。
继续参见图4可知，
Figure 77774DEST_PATH_IMAGE052
，
Figure 667019DEST_PATH_IMAGE053
为一个位姿区间的两个区间端点，
Figure 899286DEST_PATH_IMAGE054
表示时间戳，
Figure 920594DEST_PATH_IMAGE055
是待补偿点朝向的四元数表示，
Figure 875911DEST_PATH_IMAGE056
是待补偿点的时间戳，
Figure 495111DEST_PATH_IMAGE057
，
Figure 683516DEST_PATH_IMAGE058
是待补偿点对应的激光坐标系位置和姿态。
对激光点云进行运动补偿的过程可包括如下。
第一步：根据待补偿点的时间戳
Figure 554520DEST_PATH_IMAGE056
找到参考轨迹中目标位姿区间的两个区间端点，例如为
Figure 380656DEST_PATH_IMAGE052
，
Figure 108441DEST_PATH_IMAGE053
。
第二步：利用插值法求解待补偿点对应的激光坐标系位置
Figure 410240DEST_PATH_IMAGE057
。插值公式可如公式12所示：
Figure 616093DEST_PATH_IMAGE059
公式12。
第三步：利用插值算法求解待补偿点对应的激光坐标系姿态
Figure 732954DEST_PATH_IMAGE060
。插值公式可如公式13所示：
Figure 523318DEST_PATH_IMAGE061
公式13
其中，
Figure 328725DEST_PATH_IMAGE062
，
Figure 541532DEST_PATH_IMAGE063
。
第四步：将待补偿点补偿到帧的开始时刻。换言之，将
Figure 716161DEST_PATH_IMAGE057
，
Figure 707120DEST_PATH_IMAGE064
补偿到
Figure 232779DEST_PATH_IMAGE065
，
Figure 718118DEST_PATH_IMAGE066
处。
具体地，可根据
Figure 747254DEST_PATH_IMAGE057
，
Figure 687877DEST_PATH_IMAGE064
，将待补偿点对应的激光坐标系位姿变换到在世界坐标系中的位姿：
Figure 310619DEST_PATH_IMAGE067
公式14
其中，
Figure 724283DEST_PATH_IMAGE068
为待补偿点在世界坐标系中的位姿。
再根据
Figure 998138DEST_PATH_IMAGE069
，
Figure 471845DEST_PATH_IMAGE066
将变换到世界坐标系中的待补偿点的位姿再从世界变换到帧的开始时刻，转换后的待补偿点帧的开始时刻的位姿设为
Figure 581883DEST_PATH_IMAGE070
，则
Figure 159757DEST_PATH_IMAGE071
公式15
其中，
Figure 163485DEST_PATH_IMAGE072
即为对待补偿点运动补偿后的位姿，其为在帧的开始时刻的在载体坐标系中的位姿。
图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中任一项所述的地图生成方法。
CN202011264528.1A 2020-11-12 2020-11-12 地图生成方法、计算机存储介质及电子设备 Active CN112067007B (zh)

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 CN112067007A (zh) 2020-12-11
CN112067007B true 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)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112506200B (zh) * 2020-12-14 2023-12-08 广州视源电子科技股份有限公司 机器人定位方法、装置、机器人及存储介质
CN112945240B (zh) * 2021-03-16 2022-06-07 北京三快在线科技有限公司 特征点位置的确定方法、装置、设备及可读存储介质

Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
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 纵目科技（上海）股份有限公司 一种高精度地图的构建方法、系统、终端和存储介质

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US10489927B2 (en) * 2016-07-18 2019-11-26 King Abdullah University Of Science And Technology System and method for three-dimensional image reconstruction using an absolute orientation sensor

Patent Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
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)

* Cited by examiner, † Cited by third party
Title
LiDAR/INS组合的室内定位与制图（SLAM）算法改进;余彤;《中国优秀硕士学位论文全文数据库信息科技辑》;20170815;I136-145 *
The Millimeter-Wave Radar SLAM Assisted by the RCS Feature of the Target and IMU;Li Yang,etc;《SENSORS》;20200930;第20卷(第18期);第1-27页 *

Also Published As

Publication number Publication date
CN112067007A (zh) 2020-12-11

Similar Documents

Publication Publication Date Title
CN111207774B (zh) 一种用于激光-imu外参标定的方法及系统
CN112347840B (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) 一种基于多传感器融合的移动机器人定位方法
CN115479598A (zh) 基于多传感器融合的定位与建图方法及紧耦合系统
CN114001733A (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) 一种基于双层滤波框架的高精度里程估计方法
CN115164887A (zh) 基于激光雷达与惯性组合的行人导航定位方法和装置
Nitsche et al. Visual-inertial teach & repeat for aerial robot navigation
CN110864685A (zh) 一种基于松耦合的车辆单目视觉轮式里程计定位方法
CN110849349A (zh) 一种基于磁传感器与轮式里程计融合定位方法
Wang et al. Real-time visual odometry estimation based on principal direction detection on ceiling vision
CN111879323B (zh) 一种基于前端融合的航向角计算方法
Shi et al. A multisource heterogeneous data fusion method for pedestrian tracking

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