CN114279434B - 一种建图方法、装置、电子设备和存储介质 - Google Patents
一种建图方法、装置、电子设备和存储介质 Download PDFInfo
- Publication number
- CN114279434B CN114279434B CN202111615213.1A CN202111615213A CN114279434B CN 114279434 B CN114279434 B CN 114279434B CN 202111615213 A CN202111615213 A CN 202111615213A CN 114279434 B CN114279434 B CN 114279434B
- Authority
- CN
- China
- Prior art keywords
- pose
- key frame
- data
- target
- preset
- 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 19
- 238000012545 processing Methods 0.000 claims abstract description 164
- 238000005457 optimization Methods 0.000 claims abstract description 109
- 238000000034 method Methods 0.000 claims abstract description 82
- 238000013507 mapping Methods 0.000 claims description 55
- 230000001360 synchronised effect Effects 0.000 claims description 18
- 238000010586 diagram Methods 0.000 claims description 17
- 238000001514 detection method Methods 0.000 claims description 16
- 230000008569 process Effects 0.000 claims description 15
- 238000001914 filtration Methods 0.000 claims description 14
- 238000004590 computer program Methods 0.000 claims description 8
- 238000005259 measurement Methods 0.000 claims description 5
- 230000001131 transforming effect Effects 0.000 claims description 5
- 230000008859 change Effects 0.000 claims description 4
- 238000009499 grossing Methods 0.000 claims description 4
- 230000006870 function Effects 0.000 description 13
- 230000003287 optical effect Effects 0.000 description 5
- 238000005516 engineering process Methods 0.000 description 3
- AYFVYJQAPQTCCC-GBXIJSLDSA-N L-threonine Chemical compound C[C@@H](O)[C@H](N)C(O)=O AYFVYJQAPQTCCC-GBXIJSLDSA-N 0.000 description 2
- 238000012937 correction Methods 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 230000010354 integration Effects 0.000 description 2
- 239000013307 optical fiber Substances 0.000 description 2
- 230000000644 propagated effect Effects 0.000 description 2
- 239000004065 semiconductor Substances 0.000 description 2
- 230000002159 abnormal effect Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 239000000835 fiber Substances 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 239000011159 matrix material Substances 0.000 description 1
Landscapes
- Navigation (AREA)
Abstract
本公开实施例公开了一种建图方法、装置、电子设备和存储介质,该方法包括:对预设传感器数据进行处理;其中,所述预设传感器数据是在预设时间段内采集到的全部数据,所述预设时间段是目标车辆在预设场景中从运行开始到运行结束的时间段;当对所述预设传感器数据处理完成时,基于获得的处理结果以及原始GPS数据进行第一位姿图优化处理,获得关键帧的第一目标位姿;基于所述关键帧的第一目标位姿,以及与所述关键帧对应的原始数据构建所述预设场景的地图。本公开提高了建图精度和效率。
Description
技术领域
本公开涉及自动驾驶技术领域,尤其涉及一种建图方法、装置、电子设备和存储介质。
背景技术
建图与定位技术是自动驾驶领域的核心技术之一。在无人驾驶场景,通常首先需要对环境进行建图,然后在建好的地图中实现定位功能。建图的精度会很大程度地影响定位的精度,因此想要实现高精度定位首先要建立高精度地图。
目前常用的建图方案主要以GPS(Global Positioning System,全球定位***)+SLAM(Simultaneous Localization And Mapping,同步定位与建图)的方案为主。
发明人在实际应用中发现常用的建图方案存在如下问题:建图方案不灵活,针对每帧传感器数据分别依次地执行位姿图优化的步骤,如此,假如出现低精度状态的GPS数据,则需要工程人员介入进行大量的调参工作,并反复建图,导致建图效率较低。
发明内容
为了解决上述技术问题或者至少部分地解决上述技术问题,本公开实施例提供了一种建图方法、装置、电子设备和存储介质,提高了建图效率。
第一方面,本公开实施例提供了一种建图方法,该方法包括:
对预设传感器数据进行处理;其中,所述预设传感器数据是在预设时间段内采集到的全部数据,所述预设时间段是目标车辆在预设场景中从运行开始到运行结束的时间段;
当对所述预设传感器数据处理完成时,基于获得的处理结果以及原始GPS数据进行第一位姿图优化处理,获得关键帧的第一目标位姿;
基于所述关键帧的第一目标位姿,以及与所述关键帧对应的原始数据构建所述预设场景的地图。
第二方面,本公开实施例还提供了一种建图装置,该装置包括:
里程计模块和GPS数据优化模块;
其中,所述里程计模块用于:对预设传感器数据进行处理;其中,所述预设传感器数据是在预设时间段内采集到的全部数据,所述预设时间段是目标车辆在预设场景中从运行开始到运行结束的时间段;
所述GPS数据优化模块包括位姿图优化处理单元和建图单元;
所述位姿图优化处理单元用于当对所述预设传感器数据处理完成时,基于获得的处理结果以及原始GPS数据进行第一位姿图优化处理,获得关键帧的第一目标位姿;
所述建图单元用于基于所述关键帧的第一目标位姿,以及与所述关键帧对应的原始数据构建所述预设场景的地图。
第三方面,本公开实施例还提供了一种电子设备,所述电子设备包括:一个或多个处理器;存储装置,用于存储一个或多个程序;当所述一个或多个程序被所述一个或多个处理器执行,使得所述一个或多个处理器实现如上所述的建图方法。
第四方面,本公开实施例还提供了一种计算机可读存储介质,其上存储有计算机程序,该程序被处理器执行时实现如上所述的建图方法。
本公开实施例提供的建图方法,在对建图所需的全部传感器数据处理完成时,才执行位姿图优化处理的步骤,即在一次完整的建图流程中只执行一次位姿图优化处理的步骤,而不是针对每帧传感器数据处理完成时分别依次地执行位姿图优化处理的步骤,如此可较灵活地对位姿图优化处理环节进行参数调节,从而达到提高建图效率的目的。具体的,所述建图方法包括:对预设传感器数据进行处理;其中,所述预设传感器数据是在预设时间段内采集到的全部数据,所述预设时间段是目标车辆在预设场景中从运行开始到运行结束的时间段;当对所述预设传感器数据处理完成时,基于获得的处理结果以及原始GPS数据进行第一位姿图优化处理,获得关键帧的第一目标位姿;基于所述关键帧的第一目标位姿,以及与所述关键帧对应的原始数据构建所述预设场景的地图。
附图说明
结合附图并参考以下具体实施方式,本公开各实施例的上述和其他特征、优点及方面将变得更加明显。贯穿附图中,相同或相似的附图标记表示相同或相似的元素。应当理解附图是示意性的,原件和元素不一定按照比例绘制。
图1为本公开实施例中的一种建图方法的流程图;
图2为本公开实施例中的一种对预设传感器数据进行处理的流程示意图;
图3为本公开实施例中的一种对预设传感器数据进行处理的流程示意图;
图4为本公开实施例中的一种建图结果的示意图;
图5为本公开实施例中的另一种建图结果的示意图;
图6为本公开实施例中的一种基于获得的处理结果以及原始GPS数据进行第一位姿图优化处理的流程示意图;
图7为本公开实施例中的一种回环检测的流程示意图;
图8本公开实施例中的一种建图方法的流程图;
图9为本公开实施例中的一种建图装置的结构示意图;
图10为本公开实施例中的一种电子设备的结构示意图。
具体实施方式
下面将参照附图更详细地描述本公开的实施例。虽然附图中显示了本公开的某些实施例,然而应当理解的是,本公开可以通过各种形式来实现,而且不应该被解释为限于这里阐述的实施例,相反提供这些实施例是为了更加透彻和完整地理解本公开。应当理解的是,本公开的附图及实施例仅用于示例性作用,并非用于限制本公开的保护范围。
需要注意,本公开中提及的“第一”、“第二”等概念仅用于对不同的装置、模块或单元进行区分,并非用于限定这些装置、模块或单元所执行的功能的顺序或者相互依存关系。
本公开实施方式中的多个装置之间所交互的消息或者信息的名称仅用于说明性的目的,而并不是用于对这些消息或信息的范围进行限制。
本公开实施例提供了一种建图方法,旨在提高建图效率和建图精度。本公开实施例提供的建图方法不仅适用于对室外场景进行建图,还适用于对室内场景进行建图。下面结合具体的实施例对该方法进行介绍。图1为本公开实施例中的一种建图方法的流程图。该方法可以由建图装置执行,该装置可以采用软件和/或硬件的方式实现,该装置可配置于电子设备中,例如服务器。
如图1所示,针对室外场景,该方法具体可以包括如下步骤:
步骤110、对预设传感器数据进行处理;其中,所述预设传感器数据是在预设时间段内采集到的全部数据,所述预设时间段是目标车辆在预设场景中从运行开始到运行结束的时间段。
其中,预设时间段指目标车辆在预设场景中运行一圈所花费的时间段。例如预设场景是从地点A沿着路线1到达地点B,则预设时间段指目标车辆从地点A出发,沿着路线1行驶到达地点B所花费的时间段,假如目标车辆从地点A出发时是上午9点,沿着路线1行驶到达地点B时是上午9点半,则预设时间段指从9点-9点半的这半个小时。
在目标车辆上装载有多种不同类型的预设传感器,该预设传感器用于在目标车辆行驶的过程中采集相关数据。具体的,所述预设传感器数据包括在所述预设时间段内通过GPS获得的GPS数据、在所述预设时间段内通过激光雷达采集的点云、在所述预设时间段内通过摄像头采集的图像、在所述预设时间段内通过IMU(Inertial Measurement Unit,惯性测量单元)采集的数据以及在所述预设时间段内通过轮速计采集的数据。
假如在所述预设时间段内通过激光雷达采集的点云一共为1000帧,在所述预设时间段内通过摄像头采集的图像一共为1000帧,在所述预设时间段内通过IMU采集的数据一共为1000帧,在所述预设时间段内通过轮速计采集的数据一共为1000帧,则所述预设传感器数据为上述1000+1000+1000+1000=4000帧的数据。
示例性的,参考如图2所示的一种对预设传感器数据进行处理的流程示意图,所述对预设传感器数据进行处理包括如下步骤:
步骤210、对所述预设传感器数据进行数据同步处理,获得同步后的数据。
可以理解的是,由于误差的存在,不同类型的传感器(例如激光雷达和摄像头)采集数据的触发时刻和采集频率不严格一致,因此采集数据的时间戳不同,故首先需要对不同传感器采集的数据进行同步处理,以将不同传感器采集的数据对齐。
示例性的,对所述预设传感器数据进行数据同步处理,获得同步后的数据,包括:以激光雷达采集点云的时间戳为参考标准,将所述图像、所述IMU采集的数据、所述轮速计采集的数据与所述点云进行配对,获得多组预设传感器数据,每组预设传感器数据为同一时间戳下的数据。
具体的,可基于如下规则对预设传感器数据进行数据同步处理:
ifabs(tlidar-tothers)<tthre
tlidar=tothers
上式中,tlidar表示激光雷达采集的某关键帧中的点云的时间戳,tothers表示除点云之外的其它传感器数据的时间戳,假设是图像数据的时间戳,如果两者之差的绝对值小于阈值tthre,则将上述点云和上述图像数据确定为时间同步的传感器数据,其实质是在同一时刻(该时刻为tlidar)被采集的数据。tthre为预设的数据同步最大时间差阈值。
换言之,数据同步是指:对以硬件脉冲同步触发的方式采集的多源传感器数据进行数据关联。
步骤220、针对当前关键帧中的点云,基于所述同步后的数据对当前关键帧进行点云配准,获得与所述当前关键帧对应的多个初始位姿。
其中,所述当前关键帧是任意一个关键帧。即针对每个关键帧均执行步骤220的操作。其中,关键帧具体指关键的预设传感器数据帧,关键帧包括点云、图像数据、轮速计数据等多种传感器数据。
关键帧是从全部帧中确定的某些帧,例如全部帧包括第一帧、第二帧、第三帧……第十帧,而关键帧包括其中的第一帧、第三帧、第五帧、第七帧和第九帧。通常是基于目标车辆的行驶距离确定关键帧,例如目标车辆启动时获得的第一帧数据(该第一帧数据包括多种传感器数据,例如点云数据、图像数据、轮速计数据和imu数据等)为第一个关键帧,当目标车辆行驶5米时获取的数据帧(同样的,该数据帧包括多种传感器数据,例如包括点云数据、图像数据、轮速计数据和imu数据等)为第二个关键帧。
其中,点云配准的实质是将点云从激光雷达的坐标系转换至世界坐标系的过程。
为了提高建图方法的鲁棒性,提高建图精度,在对关键帧进行点云配准时,可通过多种不同的配准算法实现,进而从获得的多个配准结果中选取最优的配准结果。示例性的,基于所述同步后的数据对当前关键帧进行点云配准,获得与所述当前关键帧对应的多个初始位姿,包括:
根据同一时间戳下的预设传感器数据,分别通过多种配准算法基于预设局部地图对所述当前关键帧进行配准,获得所述当前关键帧的多个初始位姿。
其中,多种配准算法例如可以是p2d NDT、d2d NDT、p2p ICP、p2l ICP、p2pl ICP、GICP、color ICP等,多种配准算法可被集成在配准算法库,并给配准算法库预留数据接口,方便未来进行其它配准算法的集成与扩展。
假设配准算法库中集成有10种不同的配准算法,则针对当前关键帧会获得对应的10个初始位姿,即通过每种配准算法均会获得一个初始位姿。然后,从该10个初始位姿中选取最优的一个作为当前关键帧对应的最终位姿(在本实施例中,将该最终位姿标记为第二目标位姿)。
可以理解的是,不同的配准算法所适用的场景可能不同,通过使用多种配准算法获得多个初始位姿,而后选取其中最优的一个作为第二目标位姿,可提高第二目标位姿的确定精度,进而提高建图精度,同时还可提高建图方法的鲁棒性。
步骤230、对所述当前关键帧对应的多个初始位姿进行最优估计,获得与所述当前关键帧对应的第二目标位姿。
示例性的,对所述当前关键帧对应的多个初始位姿进行最优估计,获得与所述当前关键帧对应的第二目标位姿,包括:针对所述多个初始位姿中的每个初始位姿,基于预设局部地图分别确定每个初始位姿的配准置信度得分,获得多个配准置信度得分;将所述多个配准置信度得分中的最小值确定为目标配准置信度得分;将获得所述目标配准置信度得分的初始位姿确定为与所述当前关键帧对应的第二目标位姿。假设当前关键帧对应有3个初始位姿,分别记为第一初始位姿、第二初始位姿和第三初始位姿;第一初始位姿的配准置信度得分为1,第二初始位姿的配准置信度得分为2,第三初始位姿的配准置信度得分为3,则将第一初始位姿确定为当前关键帧对应的第二目标位姿。
确定其它关键帧对应的第二目标位姿的方式与上述确定当前关键帧对应的第二目标位姿的方式类似,不再一一说明。
需要说明的是,第二目标位姿具体可以是在世界坐标系下所述目标车辆的位姿。
通过上述步骤220,针对每个关键帧均对应有一个位姿集合TN,该位姿集合中包括与同一关键帧对应的多个初始位姿,N表示初始位姿的数量,或者说是配准算法库中集成的配准算法的数量。进一步的,所述针对所述多个初始位姿中的每个初始位姿,基于预设局部地图分别确定每个初始位姿的配准置信度得分,包括:针对所述多个初始位姿中的当前初始位姿,确定目标点通过所述当前初始位姿变换后获得的参考点,其中,所述目标点为所述当前关键帧中的点云中的一个点;确定所述预设局部地图中与所述参考点距离最近的对象点,并确定所述对象点与所述参考点之间的距离,将所述距离确定为所述目标点对应的距离;根据所述当前关键帧中点云中点的总数,以及所述当前关键帧中各点分别对应的所述距离之和确定所述当前初始位姿的配准置信度得分。
具体的,针对当前关键帧对应的位姿集合TN中的每个初始位姿t,通过下述算式确定其配准置信度得分:
其中,s表示初始位姿t(即上述当前初始位姿)的配准置信度得分,M表示当前关键帧中的点云总数,J表示所述预设局部地图的点云,j为点云J中距离参考点最近的点,即j表示上述的对象点,参考点为当前关键帧中的第i个点(即上述的目标点)经过初始位姿t变换后得到的点。dt(i,j)表示点j与通过点i获得的参考点之间的距离。s越小表示初始位姿t的配准精度越高。
根据上述算式可以获得配准置信度得分集合S,取集合S中的最小值作为最优位姿tbest,即将集合S中的最小值确定为当前关键帧对应的第二目标位姿。
概括性的,tbest=minS→TN
需要说明的是,所述预设局部地图是基于历史关键帧的第二目标位姿以及与所述历史关键帧对应的原始数据构建的。故针对第一个关键帧,因为不存在历史关键帧,因此还不存在预设局部地图,故不对第一个关键帧进行配准,且第一个关键帧是在目标车辆刚启动时获取到的第一个传感器数据帧,因此可将第一个关键帧确定为预设原点,具体是所述预设局部地图的原点,当对第二个关键帧进行点云配准时,则可基于所述预设原点进行点云配准。其中,与所述历史关键帧对应的原始数据可以指与所述历史关键帧的时间戳对齐的图像数据和点云数据。所述预设局部地图的原点是在UTM(UniversalTransverseMercator Grid System,通用横墨卡托格网***)坐标系下的原点,该原点与世界坐标系的原点相差一个偏移量,该偏移量为第一个关键帧中的GPS数据的坐标值,故基于第一个关键帧中的GPS数据的坐标值以及第一个关键帧的位姿可以确定世界坐标系的原点。
进一步的,所述对预设传感器数据进行处理,还包括:
若所述当前关键帧的第二目标位姿与所述历史关键帧的第二目标位姿之间的欧式距离小于距离阈值,则将所述当前关键帧标记为历史关键帧,以对所述预设局部地图进行更新。其中,所述历史关键帧的数量可以为多个,当所述历史关键帧的数量为多个时,所述欧式距离为当前关键帧的第二目标位姿与每个历史关键帧的第二目标位姿之间的欧氏距离的和。将所述当前关键帧标记为历史关键帧的实质是对原来的历史关键帧进行扩充,而非替代,因此,历史关键帧的数量可能会越来越多。当有新的历史关键帧加入时,对所述预设局部地图进行更新一次,更新的实质是基于历史关键帧的第二目标位姿以及对应的原始数据创建地图,将创建的地图确定为更新后的预设局部地图。
示例性的,参考如图3所示的一种对预设传感器数据进行处理的流程示意图,获取到传感器数据后,首先进行数据同步处理,获得同一时间戳下的激光雷达数据(即点云数据)、图像数据、imu数据、轮速计数据。进一步的,imu数据、轮速计数据作为点云配准的初始值,与激光雷达数据和图像数据一起输入到点云配准算法集成库,通过点云配准算法集成库中的多种点云配准算法基于所述点云配准的初始值,结合激光雷达数据和图像数据进行点云配准,获得多个初始位姿组成的位姿集合。对位姿集合中的多个初始位姿进行最优估计,获得当前关键帧对应的第二目标位姿,并将该第二目标位姿存储到数据库,同时利用当前关键帧对预设局部地图进行更新。最后判断数据是否已经全部处理完成,如果全部处理完,则结束处理流程,否则获取下一帧传感器数据重复上述处理流程。
步骤120、当对所述预设传感器数据处理完成时,基于获得的处理结果以及原始GPS数据进行第一位姿图优化处理,获得关键帧的第一目标位姿。
其中,所述处理结果包括关键帧的第二目标位姿,以及与所述关键帧对应的所述原始数据。原始数据可以指与关键帧对应的图像数据和点云数据。对所述预设传感器数据处理完成的实质是对全部的关键帧处理完成,换言之,是获得建图所需的全部关键帧的第二目标位姿,而不是一个或者少量的几个关键帧的第二目标位姿。
其中,第二目标位姿具体可以是基于点云、图像数据、轮速计数据以及IMU数据等在车体坐标下的多源传感器数据获得的在世界坐标系下目标车辆的位姿。因此可以理解的是,第二目标位姿存在累积误差,为了消除或者减少累积误差,需结合GPS数据对第二目标位姿进行校对调整。所述第一目标位姿即为结合GPS数据对第二目标位姿进行校对调整后获得的优化位姿。
其中,结合GPS数据对第二目标位姿进行校对调整的方式可以是在世界坐标系下,利用GPS数据对所述第二目标位姿进行校对调整。还可以是在车体坐标系下进行,即第二目标位姿是基于点云、图像数据、轮速计数据以及IMU数据等多源传感器数据获得的在车体坐标系下的车辆位姿,并将GPS数据也转换至车体坐标系下,利用车体坐标系下的GPS数据对第二目标位姿进行校对调整,将获得的校对调整后的车辆位姿再转换回世界坐标系,从而获得优化后的第一目标位姿。上述原始GPS数据指在所述预设时间段内获得的GPS数据,即在目标车辆在预设场景中从运行开始到运行结束的时间段内获得的GPS数据。
进一步的,考虑到在真实复杂的驾驶场景中,GPS的状态并不能保证一直是高精度状态,比如城市道路中的高架桥、林荫道、隧道等场景会使得GPS信号出现不稳定的现象,导致其定位精度下降。甚至还会出现GPS自认为定位状态是高精度状态,但其真实定位状态却是低精度状态,将该状态定义为假阳性状态。为了避免低精度状态和/或假阳性状态的GPS数据导致建图精度下降,甚至导致建图失败的情况发生,在本实施例的技术方案中,针对原始GPS数据进行过滤处理,以将低精度状态和/或假阳性状态的GPS数据从原始GPS数据中删除,保留高精度状态的GPS数据,以达到提高建图精度和效率的目的。
示例性的,所述当对所述预设传感器数据处理完成时,基于获得的处理结果以及原始GPS数据进行第一位姿图优化处理,获得关键帧的第一目标位姿,包括:
对与所述关键帧对应的原始GPS数据进行过滤,以将不符合预设条件的原始GPS数据删除,获得符合预设条件的、与所述关键帧对应的目标GPS数据;根据所述处理结果,以及与关键帧对应的目标GPS数据进行第一位姿图优化处理,获得所述关键帧的第一目标位姿。
其中,与所述关键帧对应的原始GPS数据指:与关键帧的时间戳对齐的原始GPS数据。每个关键帧对应有时间戳(假设该时间戳被标记为参考时间戳),若GPS数据的时间戳与该参考时间戳之差的绝对值小于阈值,则可认为该GPS数据与关键帧是同一时刻被采集到的。
示例性的,所述对与所述关键帧对应的原始GPS数据进行过滤,以将不符合预设条件的原始GPS数据删除,获得符合预设条件的、与所述关键帧对应的目标GPS数据,包括:
将第一预设状态(即低精度状态)的原始GPS数据删除,获得剩余的第二预设状态(即高精度状态)的原始GPS数据;针对所述第二预设状态的原始GPS数据进行航向角分析,若相邻两帧的所述第二预设状态的原始GPS数据的航向角之间的变化度超过第一阈值(此类GPS数据即为假阳性状态的数据),则将所述相邻两帧的所述第二预设状态的原始GPS数据删除,获得第二预设状态的剩余原始GPS数据;对所述剩余原始GPS数据的航向角以及全局坐标通过设定滤波器进行曲线平滑处理,获得平滑后的目标曲线;将所述剩余原始GPS数据中与所述目标曲线之间的欧式距离超过第二阈值的GPS数据删除,获得所述目标GPS数据。其中,所述设定滤波器可以是Savitzky-Golay滤波器。
示例性的,可以参考如图4和图5所示的两种建图结果的示意图,其中,图4所示的建图结果是在天桥的行驶场景下,未对原始GPS数据进行过滤时的建图结果,图5所示的建图结果是在天桥的行驶场景下,对原始GPS数据进行过滤后的建图结果,可以看出由于未对原始GPS数据进行过滤导致建图结果中的天桥存在扭曲的现象,这是由于天桥遮挡导致出现了假阳性状态的GPS数据;而对原始GPS数据进行过滤可以有效地踢除假阳性状态的GPS数据,从而使得建图结果中的天桥呈现较好的一致性,不存在所建天桥扭曲的现象。因此,通过对原始GPS数据进行过滤,可以实现提高建图精度和建图效果的目的。
具体的,所述根据所述处理结果,以及与所述关键帧对应的目标GPS数据进行第一位姿图优化处理,获得所述关键帧的第一目标位姿,包括:根据与同一关键帧对应的第二目标位姿和第四目标位姿确定与同一关键帧对应的位姿误差,其中,所述第二目标位姿基于所述关键帧确定,所述第四目标位姿根据所述目标GPS数据确定。即第二目标位姿是基于点云、图像数据、轮速计数据以及IMU数据等多源传感器数据获得的在世界坐标系下目标车辆的位姿;第四目标位姿是基于GPS数据获得的在世界坐标系下目标车辆的位姿。基于所述关键帧对应的所述位姿误差构建位姿图误差函数;基于所述位姿图误差函数确定所述关键帧的第一目标位姿,从而减少第二目标位姿存在的累积误差。
概括性的,定义ti表示基于第i个关键帧获得的第二目标位姿,Ti表示基于与第i个关键帧对应的目标GPS数据获得的第四目标位姿,与第i个关键帧对应的位姿误差为ei=ti-Ti。基于关键帧对应的位姿误差构建位姿图误差函数其中,Ωi表示与第i个关键帧对应的预设信息矩阵,k表示关键帧中点云的数量。对上述的位姿图误差函数进行优化,优化的变量是第二目标位姿,优化的目标是使F(x)最小。上述优化问题的实质是非线性最小二乘问题,可以通过列文伯格或者高斯牛顿法来求解该问题,最终得到优化后的位姿,即所述第一目标位姿,再根据第一目标位姿以及与关键帧对应的原始数据构建所述预设场景的地图。
步骤130、基于所述关键帧的第一目标位姿,以及与所述关键帧对应的原始数据构建所述预设场景的地图。
其中,第一目标位姿的实质为在世界坐标系下目标车辆的位姿,或者理解为是在世界坐标系下目标车辆相对于预设原点的相对位姿,预设原点可以是在所述目标车辆刚启动时(即目标车辆在起始位置处)通过传感器采集到的第一帧数据所指示的位置。原始数据具体可以是点云和图像数据,基于目标车辆相对于预设原点的相对位姿,以及点云和图像与目标车辆之间的相对位姿确定点云和图像相对于预设原点的位姿,从而获得所述预设场景的地图。可以理解的是所述预设场景的地图是在世界坐标系下的地图。
示例性的,参考如图6所示的一种基于获得的处理结果以及原始GPS数据进行第一位姿图优化处理的流程示意图,将通过步骤110获得的关键帧的第二目标位姿、关键帧原始数据、GPS原始数据进行数据关联处理,然后将无效GPS原始数据删除,无效GPS原始数据包括低精度状态的原始GPS数据和假阳性状态的原始GPS数据。接着进行位姿图优化处理,如果优化成功,则输出高精度地图,如果优化失败,则进行参数优化调整重新进行位姿图优化。
本实施例提供的建图方法,在对建图所需的全部传感器数据处理完成时,才执行位姿图优化处理的步骤,即整个建图流程只需执行一次位姿图优化处理的步骤;如此可较灵活地对位姿图优化处理环节进行参数调节,从而达到提高建图效率的目的。且本实施例的方案,通过对原始GPS数据进行过滤,可提高GPS数据的质量,进而提高建图精度和建图效果。在对关键帧中的点云进行配准时,通过采用多种不同的配准算法对同一关键帧中的点云进行多次配准,获得多个初始位姿,并从多个初始位姿中选取最优的一个作为第二目标位姿,提高了第二目标位姿的确定精度,进而达到提高建图精度的目的。
在上述实施例的基础上,针对预设场景是室内场景的情况,为了进一步提高建图精度,所述基于所述处理结果以及原始GPS数据进行第一位姿图优化处理之前,所述方法还包括:当对所述预设传感器数据处理完成时,基于所述处理结果进行回环检测;若检测到回环,则进行第二位姿图优化处理,获得优化结果。
对应的,所述基于获得的处理结果以及原始GPS数据进行第一位姿图优化处理,获得关键帧的第一目标位姿,包括:
基于所述优化结果以及原始GPS数据进行第一位姿图优化处理,获得关键帧的第一目标位姿。
所述基于所述处理结果进行回环检测,包括:
将与所述关键帧对应的所述第二目标位姿以及与所述关键帧对应的原始数据输入至场景识别算法库,以通过所述场景识别算法库中的多种场景识别算法确定回环帧索引;基于所述回环帧索引确定是否检测到回环。其中,所述场景识别算法库集成有多种场景识别算法,例如词袋识别算法、netvlad识别算法、pointnetvlad识别算法、scan context识别算法以及人工设置回环等识别算法,并预留有数据接口,方便未来算法的集成与扩展,从而达到提高回环检测鲁棒性的目的。若确定检测到回环,则将所述回环帧索引指示的关键帧的第二目标位姿之间的回环约束关系作为第二位姿图优化处理的约束条件;结合所述约束条件进行第二位姿图优化处理,获得与关键帧对应的第三目标位姿,所述优化结果包括所述第三目标位姿。通过将回环帧索引指示的关键帧的第二目标位姿之间的约束条件作为第二位姿图优化处理的约束条件,可进一步减少累积误差,获得优化后的第三目标位姿,进而可提高建图精度。
对应的,参考如图7所示的一种回环检测的流程示意图,将通过上述步骤110获得的关键帧位姿(即第二目标位姿)、关键帧原始数据输入至场景识别算法集成库,通过场景识别算法集成库中的多种场景识别算法进行回环检测,若检测到回环,则结合回环帧之间的约束关系进行位姿图优化,获得第三目标位姿,最后可基于第三目标位姿以及关键帧的原始数据构建高精度地图,或者为了进一步提高建图精度,可通过GPS数据对所述第三目标位姿进行校对调整,获得优化后的第一目标位姿,最终基于第一目标位姿以及关键帧的原始数据构建高精度地图。
进一步的,参考如图8所示的一种建图方法的流程示意图,其包括:通过SLAM前端对获取到的传感器数据进行处理,以获得关键帧的第二目标位姿。换言之,SLAM前端的输入是在预设时间段内采集的传感器数据,输出是关键帧的第二目标位姿。当确定所有的传感器数据都处理完时,进行回环检测,如果检测到回环,则进行基于回环的位姿图优化处理,该位姿图优化处理的输入是关键帧的第二目标位姿以及与关键帧对应的原始数据,输出是关键帧的第三目标位姿,即对第二目标位姿进行优化所获得的优化结果。接着将第三目标位姿输入至GPS数据关联模块。GPS数据关联模块首先对GPS数据进行过滤处理,获得与关键帧对应的目标GPS数据,然后根据目标GPS数据获得第四目标位姿,基于第四目标位姿以及第三目标位姿进行位姿图优化处理(即基于第四目标位姿对第三目标位姿进行校对调整),获得第一目标位姿,最后基于第一目标位姿和关键帧的原始数据构建地图。
可以理解的是,在不包括回环检测分支的建图流程中,上述第三目标位姿被上述第二目标位姿代替,即通过第二目标位姿与所述第四目标位姿确定与同一关键帧对应的位姿误差,以进行位姿图优化处理,获得所述第一目标位姿。
图9为本公开实施例中的一种建图装置的结构示意图。如图9所示:该装置包括:里程计模块910和GPS数据优化模块920。
其中,里程计模块910用于:对预设传感器数据进行处理;其中,所述预设传感器数据是在预设时间段内采集到的全部数据,所述预设时间段是目标车辆在预设场景中从运行开始到运行结束的时间段。
GPS数据优化模块920包括位姿图优化处理单元和建图单元;所述位姿图优化处理单元用于当对所述预设传感器数据处理完成时,基于获得的处理结果以及原始GPS数据进行第一位姿图优化处理,获得关键帧的第一目标位姿;所述建图单元用于基于所述关键帧的第一目标位姿,以及与所述关键帧对应的原始数据构建所述预设场景的地图。
可选的,所述预设传感器数据包括通过激光雷达采集的点云、通过摄像头采集的图像、通过惯性测量单元IMU采集的数据以及通过轮速计采集的数据;所述原始数据包括所述点云和所述图像。
可选的,所述处理结果包括所述关键帧的第二目标位姿,以及与所述关键帧对应的所述原始数据。
可选的,里程计模块910包括:同步单元、配准单元和最优估计单元;其中,同步单元用于:对所述预设传感器数据进行数据同步处理,获得同步后的数据;配准单元用于:针对当前关键帧中的点云,基于所述同步后的数据对当前关键帧进行点云配准,获得与所述当前关键帧对应的多个初始位姿;最优估计单元,用于对所述当前关键帧对应的多个初始位姿进行最优估计,获得与所述当前关键帧对应的第二目标位姿。
可选的,所述同步单元具体用于:以激光雷达采集点云的时间戳为参考标准,将所述图像、所述IMU采集的数据、所述轮速计采集的数据与所述点云进行配对,获得多组同一时间戳下的预设传感器数据。
可选的,所述配准单元具体用于:根据同一时间戳下的预设传感器数据,分别通过多种配准算法基于预设局部地图对所述当前关键帧进行点云配准,获得所述当前关键帧的多个初始位姿。
可选的,所述最优估计单元包括:第一确定子单元,用于针对所述多个初始位姿中的每个初始位姿,基于预设局部地图分别确定每个初始位姿的配准置信度得分,获得多个配准置信度得分;第二确定子单元,用于将所述多个配准置信度得分中的最小值确定为目标配准置信度得分;第三确定子单元,用于将获得所述目标配准置信度得分的初始位姿确定为与所述当前关键帧对应的第二目标位姿。
可选的,所述第一确定子单元具体用于:针对所述多个初始位姿中的当前初始位姿,确定目标点通过所述当前初始位姿变换后获得的参考点,其中,所述目标点为所述当前关键帧中的点云中的一个点;确定所述预设局部地图中与所述参考点距离最近的对象点,并确定所述对象点与所述参考点之间的距离,将所述距离确定为所述目标点对应的距离;根据所述当前关键帧中点云中点的总数,以及所述当前关键帧中各点分别对应的所述距离之和确定所述当前初始位姿的配准置信度得分。
可选的,所述预设局部地图是基于历史关键帧的第二目标位姿以及与所述历史关键帧对应的原始数据构建的;里程计模块910还包括更新模块,用于若所述当前关键帧的第二目标位姿与所述历史关键帧的第二目标位姿之间的欧式距离小于距离阈值,则将所述当前关键帧标记为历史关键帧,以对所述预设局部地图进行更新。
可选的,所述装置还包括回环检测模块,用于在基于所述处理结果以及原始GPS数据进行第一位姿图优化处理之前,当对所述预设传感器数据处理完成时,基于所述处理结果进行回环检测;回环优化模块,用于若检测到回环,则进行第二位姿图优化处理,获得优化结果。对应的,所述位姿图优化处理单元用于基于所述优化结果以及原始GPS数据进行第一位姿图优化处理,获得关键帧的第一目标位姿。
可选的,所述回环检测模块具体用于:将与所述关键帧对应的所述第二目标位姿以及与所述关键帧对应的原始数据输入至场景识别算法库,以通过所述场景识别算法库中的多种场景识别算法确定回环帧索引;基于所述回环帧索引确定是否检测到回环。所述回环优化模块具体用于:若确定检测到回环,则将所述回环帧索引指示的关键帧的第二目标位姿之间的回环约束关系作为第二位姿图优化处理的约束条件;结合所述约束条件进行第二位姿图优化处理,获得与关键帧对应的第三目标位姿,所述优化结果包括所述第三目标位姿。
可选的,GPS数据优化模块920还包括:过滤单元,用于对与所述关键帧对应的原始GPS数据进行过滤,以将不符合预设条件的原始GPS数据删除,获得符合预设条件的、与所述关键帧对应的目标GPS数据;对应的,所述位姿图优化处理单元具体用于:根据所述处理结果,以及与关键帧对应的目标GPS数据进行第一位姿图优化处理,获得所述关键帧的第一目标位姿。
可选的,所述过滤单元具体用于:将第一预设状态的原始GPS数据删除,获得剩余的第二预设状态的原始GPS数据;针对所述第二预设状态的原始GPS数据进行航向角分析,若相邻两帧的所述第二预设状态的原始GPS数据的航向角之间的变化度超过第一阈值,则将所述相邻两帧的所述第二预设状态的原始GPS数据删除,获得第二预设状态的剩余原始GPS数据;对所述剩余原始GPS数据的航向角以及全局坐标通过设定滤波器进行曲线平滑处理,获得平滑后的目标曲线;将所述剩余原始GPS数据中与所述目标曲线之间的欧式距离超过第二阈值的GPS数据删除,获得所述目标GPS数据。
可选的,所述位姿图优化处理单元包括:位姿误差确定子单元,用于根据与同一关键帧对应的第二目标位姿和第四目标位姿确定与同一关键帧对应的位姿误差,其中,所述第二目标位姿基于所述关键帧确定,所述第四目标位姿根据所述目标GPS数据确定;误差函数确定子单元,用于基于所述关键帧对应的所述位姿误差构建位姿图误差函数;优化子单元用于:基于所述位姿图误差函数确定所述关键帧的第一目标位姿。
本公开实施例提供的建图装置是一种模块化的高精度建图框架,包括里程计模块、GPS数据优化模块以及回环优化模块,每个模块均可以独立运行,极大地提高了建图的灵活性。且在出现建图问题时,只需在问题所在模型进行单独地调整优化,而不影响其它模块,如此可以提高建图的质量和效率,使得建图装置便于维护与扩展。该建图方法具备更好的鲁棒性,可以支持更多驾驶场景的高精度建图。本案涉及离线的GPS数据优化处理,可以踢除异常的GPS数据,并且可以独立运行,方便调参优化,提升建图效率,且本案可以消除里程计存在的累积误差,得到全局一致性的高精度地图。同时,本案可以兼容多种场景识别算法,并保留扩展接口,方便算法扩展。
本公开实施例提供的建图装置,可执行本公开方法实施例所提供的建图方法中的步骤,具备执行步骤和有益效果此处不再赘述。
图10为本公开实施例中的一种电子设备的结构示意图。下面具体参考图10,其示出了适于用来实现本公开实施例中的电子设备500的结构示意图。图10示出的电子设备仅仅是一个示例,不应对本公开实施例的功能和使用范围带来任何限制。
如图10所示,电子设备500可以包括处理装置(例如中央处理器、图形处理器等)501,其可以根据存储在只读存储器(ROM)502中的程序或者从存储装置508加载到随机访问存储器(RAM)503中的程序而执行各种适当的动作和处理以实现如本公开所述的实施例的方法。在RAM 503中,还存储有电子设备500操作所需的各种程序和数据。处理装置501、ROM502以及RAM 503通过总线504彼此相连。输入/输出(I/O)接口505也连接至总线504。
特别地,根据本公开的实施例,上文参考流程图描述的过程可以被实现为计算机软件程序。例如,本公开的实施例包括一种计算机程序产品,其包括承载在非暂态计算机可读介质上的计算机程序,该计算机程序包含用于执行流程图所示的方法的程序代码,从而实现如上所述的建图方法。在这样的实施例中,该计算机程序可以通过通信装置509从网络上被下载和安装,或者从存储装置508被安装,或者从ROM 502被安装。在该计算机程序被处理装置501执行时,执行本公开实施例的方法中限定的上述功能。
需要说明的是,本公开上述的计算机可读介质可以是计算机可读信号介质或者计算机可读存储介质或者是上述两者的任意组合。计算机可读存储介质例如可以是——但不限于——电、磁、光、电磁、红外线、或半导体的***、装置或器件,或者任意以上的组合。计算机可读存储介质的更具体的例子可以包括但不限于:具有一个或多个导线的电连接、便携式计算机磁盘、硬盘、随机访问存储器(RAM)、只读存储器(ROM)、可擦式可编程只读存储器(EPROM或闪存)、光纤、便携式紧凑磁盘只读存储器(CD-ROM)、光存储器件、磁存储器件、或者上述的任意合适的组合。在本公开中,计算机可读存储介质可以是任何包含或存储程序的有形介质,该程序可以被指令执行***、装置或者器件使用或者与其结合使用。而在本公开中,计算机可读信号介质可以包括在基带中或者作为载波一部分传播的数据信号,其中承载了计算机可读的程序代码。这种传播的数据信号可以采用多种形式,包括但不限于电磁信号、光信号或上述的任意合适的组合。计算机可读信号介质还可以是计算机可读存储介质以外的任何计算机可读介质,该计算机可读信号介质可以发送、传播或者传输用于由指令执行***、装置或者器件使用或者与其结合使用的程序。计算机可读介质上包含的程序代码可以用任何适当的介质传输,包括但不限于:电线、光缆、RF(射频)等等,或者上述的任意合适的组合。
上述计算机可读介质可以是上述电子设备中所包含的;也可以是单独存在,而未装配入该电子设备中。上述计算机可读介质承载有一个或者多个程序,当上述一个或者多个程序被该电子设备执行时,使得该电子设备:对预设传感器数据进行处理;其中,所述预设传感器数据是在预设时间段内采集到的全部数据,所述预设时间段是目标车辆在预设场景中从运行开始到运行结束的时间段;当对所述预设传感器数据处理完成时,基于获得的处理结果以及原始GPS数据进行第一位姿图优化处理,获得关键帧的第一目标位姿;基于所述关键帧的第一目标位姿,以及与所述关键帧对应的原始数据构建所述预设场景的地图。
可选的,当上述一个或者多个程序被该电子设备执行时,该电子设备还可以执行上述实施例所述的其他步骤。
在本公开的上下文中,机器可读介质可以是有形的介质,其可以包含或存储以供指令执行***、装置或设备使用或与指令执行***、装置或设备结合地使用的程序。机器可读介质可以是机器可读信号介质或机器可读储存介质。机器可读介质可以包括但不限于电子的、磁性的、光学的、电磁的、红外的、或半导体***、装置或设备,或者上述内容的任何合适组合。机器可读存储介质的更具体示例会包括基于一个或多个线的电气连接、便携式计算机盘、硬盘、随机存取存储器(RAM)、只读存储器(ROM)、可擦除可编程只读存储器(EPROM或快闪存储器)、光纤、便捷式紧凑盘只读存储器(CD-ROM)、光学储存设备、磁储存设备、或上述内容的任何合适组合。
方案1、一种建图方法,所述方法包括:
对预设传感器数据进行处理;其中,所述预设传感器数据是在预设时间段内采集到的全部数据,所述预设时间段是目标车辆在预设场景中从运行开始到运行结束的时间段;
当对所述预设传感器数据处理完成时,基于获得的处理结果以及原始GPS数据进行第一位姿图优化处理,获得关键帧的第一目标位姿;
基于所述关键帧的第一目标位姿,以及与所述关键帧对应的原始数据构建所述预设场景的地图。
方案2、根据方案1所述的方法,所述预设传感器数据包括通过激光雷达采集的点云、通过摄像头采集的图像、通过惯性测量单元IMU采集的数据以及通过轮速计采集的数据;
所述原始数据包括所述点云和所述图像。
方案3、根据方案2所述的方法,所述处理结果包括所述关键帧的第二目标位姿,以及与所述关键帧对应的所述原始数据;
所述对预设传感器数据进行处理,包括:
对所述预设传感器数据进行数据同步处理,获得同步后的数据;
针对当前关键帧中的点云,基于所述同步后的数据对当前关键帧进行点云配准,获得与所述当前关键帧对应的多个初始位姿;
对所述当前关键帧对应的多个初始位姿进行最优估计,获得与所述当前关键帧对应的第二目标位姿。
方案4、根据方案3所述的方法,所述对所述预设传感器数据进行数据同步处理,获得同步后的数据,包括:
以激光雷达采集点云的时间戳为参考标准,将所述图像、所述IMU采集的数据、所述轮速计采集的数据与所述点云进行配对,获得多组同一时间戳下的预设传感器数据。
方案5、根据方案3所述的方法,所述基于所述同步后的数据对当前关键帧进行点云配准,获得与所述当前关键帧对应的多个初始位姿,包括:
根据同一时间戳下的预设传感器数据,分别通过多种配准算法基于预设局部地图对所述当前关键帧进行点云配准,获得所述当前关键帧的多个初始位姿。
方案6、根据方案3所述的方法,所述对所述当前关键帧对应的多个初始位姿进行最优估计,获得与所述当前关键帧对应的第二目标位姿,包括:
针对所述多个初始位姿中的每个初始位姿,基于预设局部地图分别确定每个初始位姿的配准置信度得分,获得多个配准置信度得分;
将所述多个配准置信度得分中的最小值确定为目标配准置信度得分;
将获得所述目标配准置信度得分的初始位姿确定为与所述当前关键帧对应的第二目标位姿。
方案7、根据方案6所述的方法,所述针对所述多个初始位姿中的每个初始位姿,基于预设局部地图分别确定每个初始位姿的配准置信度得分,包括:
针对所述多个初始位姿中的当前初始位姿,确定目标点通过所述当前初始位姿变换后获得的参考点,其中,所述目标点为所述当前关键帧中的点云中的一个点;
确定所述预设局部地图中与所述参考点距离最近的对象点,并确定所述对象点与所述参考点之间的距离,将所述距离确定为所述目标点对应的距离;
根据所述当前关键帧中点云中点的总数,以及所述当前关键帧中各点分别对应的所述距离之和确定所述当前初始位姿的配准置信度得分。
方案8、根据方案7所述的方法,所述预设局部地图是基于历史关键帧的第二目标位姿以及与所述历史关键帧对应的原始数据构建的;
所述对预设传感器数据进行处理,还包括:
若所述当前关键帧的第二目标位姿与所述历史关键帧的第二目标位姿之间的欧式距离小于距离阈值,则将所述当前关键帧标记为历史关键帧,以对所述预设局部地图进行更新。
方案9、根据方案3所述的方法,所述基于所述处理结果以及原始GPS数据进行第一位姿图优化处理之前,所述方法还包括:
当对所述预设传感器数据处理完成时,基于所述处理结果进行回环检测;
若检测到回环,则进行第二位姿图优化处理,获得优化结果;
对应的,所述基于获得的处理结果以及原始GPS数据进行第一位姿图优化处理,获得关键帧的第一目标位姿,包括:
基于所述优化结果以及原始GPS数据进行第一位姿图优化处理,获得关键帧的第一目标位姿。
方案10、根据方案9所述的方法,所述基于所述处理结果进行回环检测,包括:
将与所述关键帧对应的所述第二目标位姿以及与所述关键帧对应的原始数据输入至场景识别算法库,以通过所述场景识别算法库中的多种场景识别算法确定回环帧索引;
基于所述回环帧索引确定是否检测到回环。
方案11、根据方案10所述的方法,所述若检测到回环,则进行第二位姿图优化处理,获得优化结果,包括:
若确定检测到回环,则将所述回环帧索引指示的关键帧的第二目标位姿之间的回环约束关系作为第二位姿图优化处理的约束条件;
结合所述约束条件进行第二位姿图优化处理,获得与所述关键帧对应的第三目标位姿,所述优化结果包括所述第三目标位姿。
方案12、根据方案1-8任意一项所述的方法,所述当对所述预设传感器数据处理完成时,基于获得的处理结果以及原始GPS数据进行第一位姿图优化处理,获得关键帧的第一目标位姿,包括:
对与所述关键帧对应的原始GPS数据进行过滤,以将不符合预设条件的原始GPS数据删除,获得符合预设条件的、与所述关键帧对应的目标GPS数据;
根据所述处理结果,以及与关键帧对应的目标GPS数据进行第一位姿图优化处理,获得所述关键帧的第一目标位姿。
方案13、根据方案12所述的方法,所述对与所述关键帧对应的原始GPS数据进行过滤,以将不符合预设条件的原始GPS数据删除,获得符合预设条件的、与所述关键帧对应的目标GPS数据,包括:
将第一预设状态的原始GPS数据删除,获得剩余的第二预设状态的原始GPS数据;
针对所述第二预设状态的原始GPS数据进行航向角分析,若相邻两帧的所述第二预设状态的原始GPS数据的航向角之间的变化度超过第一阈值,则将所述相邻两帧的所述第二预设状态的原始GPS数据删除,获得第二预设状态的剩余原始GPS数据;
对所述剩余原始GPS数据的航向角以及全局坐标通过设定滤波器进行曲线平滑处理,获得平滑后的目标曲线;
将所述剩余原始GPS数据中与所述目标曲线之间的欧式距离超过第二阈值的GPS数据删除,获得所述目标GPS数据。
方案14、根据方案12所述的方法,所述根据所述优化结果,以及与所述关键帧对应的目标GPS数据进行第一位姿图优化处理,获得所述关键帧的第一目标位姿,包括:
根据与同一关键帧对应的第二目标位姿和第四目标位姿确定与所述同一关键帧对应的位姿误差,其中,所述第二目标位姿基于所述关键帧确定,所述第四目标位姿根据所述目标GPS数据确定;
基于所述关键帧对应的所述位姿误差构建位姿图误差函数;
基于所述位姿图误差函数确定所述关键帧的第一目标位姿。
方案15、一种建图装置,所述装置包括:里程计模块和GPS数据优化模块;
其中,所述里程计模块用于:对预设传感器数据进行处理;其中,所述预设传感器数据是在预设时间段内采集到的全部数据,所述预设时间段是目标车辆在预设场景中从运行开始到运行结束的时间段;
所述GPS数据优化模块包括位姿图优化处理单元和建图单元;
所述位姿图优化处理单元用于当对所述预设传感器数据处理完成时,基于获得的处理结果以及原始GPS数据进行第一位姿图优化处理,获得关键帧的第一目标位姿;
所述建图单元用于基于所述关键帧的第一目标位姿,以及与所述关键帧对应的原始数据构建所述预设场景的地图。
方案16、一种电子设备,所述电子设备包括:
一个或多个处理器;
存储装置,用于存储一个或多个程序;
当所述一个或多个程序被所述一个或多个处理器执行,使得所述一个或多个处理器实现如方案1-14中任一项所述的方法。
方案17、一种计算机可读存储介质,其上存储有计算机程序,该程序被处理器执行时实现如方案1-14中任一项所述的方法。
以上描述仅为本公开的较佳实施例以及对所运用技术原理的说明。本领域技术人员应当理解,本公开中所涉及的公开范围,并不限于上述技术特征的特定组合而成的技术方案,同时也应涵盖在不脱离上述公开构思的情况下,由上述技术特征或其等同特征进行任意组合而形成的其它技术方案。例如上述特征与本公开中公开的(但不限于)具有类似功能的技术特征进行互相替换而形成的技术方案。
Claims (15)
1.一种建图方法,其特征在于,所述方法包括:
对预设传感器数据进行处理;其中,所述预设传感器数据是在预设时间段内采集到的全部数据,所述预设时间段是目标车辆在预设场景中从运行开始到运行结束的时间段;
当对所述预设传感器数据处理完成时,基于获得的处理结果以及原始GPS数据进行第一位姿图优化处理,获得关键帧的第一目标位姿;
基于所述关键帧的第一目标位姿,以及与所述关键帧对应的原始数据构建所述预设场景的地图;
其中,所述预设传感器数据包括通过激光雷达采集的点云、通过摄像头采集的图像、通过惯性测量单元IMU采集的数据以及通过轮速计采集的数据;所述原始数据包括所述点云和所述图像;
所述处理结果包括所述关键帧的第二目标位姿,以及与所述关键帧对应的所述原始数据;所述对预设传感器数据进行处理,包括:
对所述预设传感器数据进行数据同步处理,获得同步后的数据;
针对当前关键帧中的点云,基于所述同步后的数据对当前关键帧进行点云配准,获得与所述当前关键帧对应的多个初始位姿;
对所述当前关键帧对应的多个初始位姿进行最优估计,获得与所述当前关键帧对应的第二目标位姿。
2.根据权利要求1所述的方法,其特征在于,所述对所述预设传感器数据进行数据同步处理,获得同步后的数据,包括:
以激光雷达采集点云的时间戳为参考标准,将所述图像、所述IMU采集的数据、所述轮速计采集的数据与所述点云进行配对,获得多组同一时间戳下的预设传感器数据。
3.根据权利要求1所述的方法,其特征在于,所述基于所述同步后的数据对当前关键帧进行点云配准,获得与所述当前关键帧对应的多个初始位姿,包括:
根据同一时间戳下的预设传感器数据,分别通过多种配准算法基于预设局部地图对所述当前关键帧进行点云配准,获得所述当前关键帧的多个初始位姿。
4.根据权利要求1所述的方法,其特征在于,所述对所述当前关键帧对应的多个初始位姿进行最优估计,获得与所述当前关键帧对应的第二目标位姿,包括:
针对所述多个初始位姿中的每个初始位姿,基于预设局部地图分别确定每个初始位姿的配准置信度得分,获得多个配准置信度得分;
将所述多个配准置信度得分中的最小值确定为目标配准置信度得分;
将获得所述目标配准置信度得分的初始位姿确定为与所述当前关键帧对应的第二目标位姿。
5.根据权利要求4所述的方法,其特征在于,所述针对所述多个初始位姿中的每个初始位姿,基于预设局部地图分别确定每个初始位姿的配准置信度得分,包括:
针对所述多个初始位姿中的当前初始位姿,确定目标点通过所述当前初始位姿变换后获得的参考点,其中,所述目标点为所述当前关键帧中的点云中的一个点;
确定所述预设局部地图中与所述参考点距离最近的对象点,并确定所述对象点与所述参考点之间的距离,将所述距离确定为所述目标点对应的距离;
根据所述当前关键帧中点云中点的总数,以及所述当前关键帧中各点分别对应的所述距离之和确定所述当前初始位姿的配准置信度得分。
6.根据权利要求5所述的方法,其特征在于,所述预设局部地图是基于历史关键帧的第二目标位姿以及与所述历史关键帧对应的原始数据构建的;
所述对预设传感器数据进行处理,还包括:
若所述当前关键帧的第二目标位姿与所述历史关键帧的第二目标位姿之间的欧式距离小于距离阈值,则将所述当前关键帧标记为历史关键帧,以对所述预设局部地图进行更新。
7.根据权利要求1所述的方法,其特征在于,所述基于所述处理结果以及原始GPS数据进行第一位姿图优化处理之前,所述方法还包括:
当对所述预设传感器数据处理完成时,基于所述处理结果进行回环检测;
若检测到回环,则进行第二位姿图优化处理,获得优化结果;
对应的,所述基于获得的处理结果以及原始GPS数据进行第一位姿图优化处理,获得关键帧的第一目标位姿,包括:
基于所述优化结果以及原始GPS数据进行第一位姿图优化处理,获得关键帧的第一目标位姿。
8.根据权利要求7所述的方法,其特征在于,所述基于所述处理结果进行回环检测,包括:
将与所述关键帧对应的所述第二目标位姿以及与所述关键帧对应的原始数据输入至场景识别算法库,以通过所述场景识别算法库中的多种场景识别算法确定回环帧索引;
基于所述回环帧索引确定是否检测到回环。
9.根据权利要求8所述的方法,其特征在于,所述若检测到回环,则进行第二位姿图优化处理,获得优化结果,包括:
若确定检测到回环,则将所述回环帧索引指示的关键帧的第二目标位姿之间的回环约束关系作为第二位姿图优化处理的约束条件;
结合所述约束条件进行第二位姿图优化处理,获得与所述关键帧对应的第三目标位姿,所述优化结果包括所述第三目标位姿。
10.根据权利要求1-6任意一项所述的方法,其特征在于,所述当对所述预设传感器数据处理完成时,基于获得的处理结果以及原始GPS数据进行第一位姿图优化处理,获得关键帧的第一目标位姿,包括:
对与所述关键帧对应的原始GPS数据进行过滤,以将不符合预设条件的原始GPS数据删除,获得符合预设条件的、与所述关键帧对应的目标GPS数据;
根据所述处理结果,以及与关键帧对应的目标GPS数据进行第一位姿图优化处理,获得所述关键帧的第一目标位姿。
11.根据权利要求10所述的方法,其特征在于,所述对与所述关键帧对应的原始GPS数据进行过滤,以将不符合预设条件的原始GPS数据删除,获得符合预设条件的、与所述关键帧对应的目标GPS数据,包括:
将第一预设状态的原始GPS数据删除,获得剩余的第二预设状态的原始GPS数据;
针对所述第二预设状态的原始GPS数据进行航向角分析,若相邻两帧的所述第二预设状态的原始GPS数据的航向角之间的变化度超过第一阈值,则将所述相邻两帧的所述第二预设状态的原始GPS数据删除,获得第二预设状态的剩余原始GPS数据;
对所述剩余原始GPS数据的航向角以及全局坐标通过设定滤波器进行曲线平滑处理,获得平滑后的目标曲线;
将所述剩余原始GPS数据中与所述目标曲线之间的欧式距离超过第二阈值的GPS数据删除,获得所述目标GPS数据。
12.根据权利要求10所述的方法,其特征在于,所述根据所述处理结果,以及与所述关键帧对应的目标GPS数据进行第一位姿图优化处理,获得所述关键帧的第一目标位姿,包括:
根据与同一关键帧对应的第二目标位姿和第四目标位姿确定与所述同一关键帧对应的位姿误差,其中,所述第二目标位姿基于所述关键帧确定,所述第四目标位姿根据所述目标GPS数据确定;
基于所述关键帧对应的所述位姿误差构建位姿图误差函数;
基于所述位姿图误差函数确定所述关键帧的第一目标位姿。
13.一种建图装置,其特征在于,所述装置包括:里程计模块和GPS数据优化模块;
其中,所述里程计模块用于:对预设传感器数据进行处理;其中,所述预设传感器数据是在预设时间段内采集到的全部数据,所述预设时间段是目标车辆在预设场景中从运行开始到运行结束的时间段;
所述GPS数据优化模块包括位姿图优化处理单元和建图单元;
所述位姿图优化处理单元用于当对所述预设传感器数据处理完成时,基于获得的处理结果以及原始GPS数据进行第一位姿图优化处理,获得关键帧的第一目标位姿;
所述建图单元用于基于所述关键帧的第一目标位姿,以及与所述关键帧对应的原始数据构建所述预设场景的地图;
其中,所述预设传感器数据包括通过激光雷达采集的点云、通过摄像头采集的图像、通过惯性测量单元IMU采集的数据以及通过轮速计采集的数据;所述原始数据包括所述点云和所述图像;
所述处理结果包括所述关键帧的第二目标位姿,以及与所述关键帧对应的所述原始数据;所述里程计模块包括:
同步单元,用于:对所述预设传感器数据进行数据同步处理,获得同步后的数据;
配准单元,用于:针对当前关键帧中的点云,基于所述同步后的数据对当前关键帧进行点云配准,获得与所述当前关键帧对应的多个初始位姿;
最优估计单元,用于对所述当前关键帧对应的多个初始位姿进行最优估计,获得与所述当前关键帧对应的第二目标位姿。
14.一种电子设备,其特征在于,所述电子设备包括:
一个或多个处理器;
存储装置,用于存储一个或多个程序;
当所述一个或多个程序被所述一个或多个处理器执行,使得所述一个或多个处理器实现如权利要求1-12中任一项所述的方法。
15.一种计算机可读存储介质,其上存储有计算机程序,其特征在于,该程序被处理器执行时实现如权利要求1-12中任一项所述的方法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111615213.1A CN114279434B (zh) | 2021-12-27 | 2021-12-27 | 一种建图方法、装置、电子设备和存储介质 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111615213.1A CN114279434B (zh) | 2021-12-27 | 2021-12-27 | 一种建图方法、装置、电子设备和存储介质 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN114279434A CN114279434A (zh) | 2022-04-05 |
CN114279434B true CN114279434B (zh) | 2024-06-14 |
Family
ID=80876353
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202111615213.1A Active CN114279434B (zh) | 2021-12-27 | 2021-12-27 | 一种建图方法、装置、电子设备和存储介质 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN114279434B (zh) |
Families Citing this family (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN116500638B (zh) * | 2023-06-25 | 2023-10-10 | 江苏大学 | 一种基于slam技术的收割机耕道自动导航方法及*** |
CN116539026B (zh) * | 2023-07-06 | 2023-09-29 | 杭州华橙软件技术有限公司 | 地图构建方法、装置、设备及存储介质 |
Citations (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110261870A (zh) * | 2019-04-15 | 2019-09-20 | 浙江工业大学 | 一种用于视觉-惯性-激光融合的同步定位与建图方法 |
CN113269878A (zh) * | 2021-05-26 | 2021-08-17 | 上海新纪元机器人有限公司 | 一种基于多传感器的建图方法及*** |
Family Cites Families (11)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN112292582A (zh) * | 2018-04-20 | 2021-01-29 | 文远知行有限公司 | 用于生成高清晰度地图的方法和*** |
CN109974712A (zh) * | 2019-04-22 | 2019-07-05 | 广东亿嘉和科技有限公司 | 一种基于图优化的变电站巡检机器人建图方法 |
CN111044069B (zh) * | 2019-12-16 | 2022-04-29 | 驭势科技(北京)有限公司 | 一种车辆定位方法、车载设备及存储介质 |
CN111693043B (zh) * | 2020-06-18 | 2023-04-07 | 北京四维图新科技股份有限公司 | 地图数据处理方法以及设备 |
CN111735446B (zh) * | 2020-07-09 | 2020-11-13 | 上海思岚科技有限公司 | 一种激光、视觉定位融合的方法及设备 |
CN112068154B (zh) * | 2020-09-14 | 2022-12-13 | 中科院软件研究所南京软件技术研究院 | 一种激光建图定位方法、装置、存储介质及电子设备 |
CN112634451B (zh) * | 2021-01-11 | 2022-08-23 | 福州大学 | 一种融合多传感器的室外大场景三维建图方法 |
CN113091771B (zh) * | 2021-04-13 | 2022-09-23 | 清华大学 | 一种激光雷达-相机-惯导联合标定方法及*** |
CN113514843A (zh) * | 2021-07-09 | 2021-10-19 | 深圳华芯信息技术股份有限公司 | 多子图激光雷达定位方法、***以及终端 |
CN113587933B (zh) * | 2021-07-29 | 2024-02-02 | 山东山速机器人科技有限公司 | 一种基于分支定界算法的室内移动机器人定位方法 |
CN113654555A (zh) * | 2021-09-14 | 2021-11-16 | 上海智驾汽车科技有限公司 | 一种基于多传感器数据融合的自动驾驶车辆高精定位方法 |
-
2021
- 2021-12-27 CN CN202111615213.1A patent/CN114279434B/zh active Active
Patent Citations (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110261870A (zh) * | 2019-04-15 | 2019-09-20 | 浙江工业大学 | 一种用于视觉-惯性-激光融合的同步定位与建图方法 |
CN113269878A (zh) * | 2021-05-26 | 2021-08-17 | 上海新纪元机器人有限公司 | 一种基于多传感器的建图方法及*** |
Also Published As
Publication number | Publication date |
---|---|
CN114279434A (zh) | 2022-04-05 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN108230379B (zh) | 用于融合点云数据的方法和装置 | |
CN111354042B (zh) | 机器人视觉图像的特征提取方法、装置、机器人及介质 | |
CN108732584B (zh) | 用于更新地图的方法和装置 | |
CN107481292B (zh) | 车载摄像头的姿态误差估计方法和装置 | |
CN110160542B (zh) | 车道线的定位方法和装置、存储介质、电子装置 | |
CN110849362B (zh) | 一种基于车载惯性的激光雷达与视觉组合导航算法 | |
CN110033489B (zh) | 一种车辆定位准确性的评估方法、装置及设备 | |
CN114279434B (zh) | 一种建图方法、装置、电子设备和存储介质 | |
KR102664900B1 (ko) | 드론을 이용한 지상기준점 측량장치 및 그 방법 | |
WO2018142900A1 (ja) | 情報処理装置、データ管理装置、データ管理システム、方法、及びプログラム | |
CN109710724B (zh) | 一种构建点云地图的方法和设备 | |
CN107167826B (zh) | 一种自动驾驶中基于可变网格的图像特征检测的车辆纵向定位***及方法 | |
WO2011120141A1 (en) | Dynamic network adjustment for rigorous integration of passive and active imaging observations into trajectory determination | |
CN116255992A (zh) | 一种同时定位与建图的方法和装置 | |
CN103411587A (zh) | 定位定姿方法及*** | |
CN113066127A (zh) | 一种在线标定设备参数的视觉惯性里程计方法和*** | |
KR20150095365A (ko) | 비전 센서 데이터베이스를 이용한 거리 측정 방법 | |
CN113311905B (zh) | 一种数据处理*** | |
CN114111791B (zh) | 一种智能机器人室内自主导航方法、***及存储介质 | |
CN115376109A (zh) | 障碍物检测方法、障碍物检测装置以及存储介质 | |
CN113587934B (zh) | 一种机器人、室内定位方法、装置和可读存储介质 | |
CN113959437A (zh) | 一种用于移动测量设备的测量方法及*** | |
CN117388830A (zh) | 激光雷达与惯性导航的外参标定方法、装置、设备及介质 | |
CN112689234A (zh) | 室内车辆定位方法、装置、计算机设备和存储介质 | |
CN113190564A (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 |