位姿获取方法、系统和可移动平台
文献发布时间:2023-06-19 09:29:07
技术领域
本申请实施例涉及无人机技术领域,尤其涉及一种位姿获取方法、系统和可移动平台。
背景技术
导航电子地图,可用于辅助人工驾驶员在驾驶汽车时做导航使用,这种导航电子地图的绝对坐标精度大约在10米左右。但是,随着无人驾驶技术的发展,无人驾驶汽车会成为一种出行趋势,而无人驾驶汽车在道路上行驶时需要精确的知道自己在路上的位置,往往车辆离马路牙子和旁边的车道也就几十厘米左右,所以需要更高精度的地图,这种地图的绝对精度一般都会在亚米级,也就是1米以内的精度,而且横向的相对精度(比如,车道和车道,车道和车道线的相对位置精度)往往还要更高。这种高精度的地图不仅有高精度的坐标,同时还有准确的道路形状,并且每个车道的坡度、曲率、航向、高程、侧倾的数据也都含有,以保证无人驾驶汽车在道路上行驶的安全性和准确性。
因此,高精度的地图对无人驾驶汽车至关重要,高精度的地图目前是根据车辆在行驶过程中拍摄装置采集到的数据重建而成。但是由于车辆本身的位姿数据存在误差,这会影响高精度的地图的精度。
发明内容
本申请实施例提供一种位姿获取方法、系统和可移动平台,用于提高获取位姿的准确性,以提高建立的地图的精度。
第一方面,本申请实施例提供一种位姿获取方法,应用于可移动平台,所述可移动平台设有第一探测装置,用于获取激光点云数据,还设有第二探测装置,用于获取位姿信息;该方法包括:
获取可移动平台在目标区域内移动的过程中获取的各时刻的激光点云数据和所述位姿信息;
根据相邻两时刻的激光点云数据,获得相邻两时刻的激光点云的相对位姿关系;
根据各相邻两时刻的激光点云的相对位姿关系和各时刻的位姿信息,修正所述各时刻的激光点云的位姿,以获得所述各时刻的激光点云的目标位姿。
第二方面,本申请实施例提供一种位姿获取系统,应用于可移动平台中,所述系统包括:
第一探测装置,用于获取激光点云数据;
第二探测装置,用于获取位姿信息;
处理器,用于获取可移动平台在目标区域内移动的过程中所述第一探测装置获取的各时刻的激光点云数据和所述第二探测装置获取的各时刻的所述位姿信息;根据相邻两时刻的激光点云数据,获得相邻两时刻的激光点云的相对位姿关系;根据各相邻两时刻的激光点云的相对位姿关系和各时刻的位姿信息,修正所述各时刻的激光点云的位姿,以获得所述各时刻的激光点云的目标位姿。
第三方面,本申请实施例提供一种可移动平台,包括:
第一探测装置,用于获取激光点云数据;
第二探测装置,用于获取位姿信息;
处理器,用于获取可移动平台在目标区域内移动的过程中所述第一探测装置获取的各时刻的激光点云数据和所述第二探测装置获取的各时刻的所述位姿信息;根据相邻两时刻的激光点云数据,获得相邻两时刻的激光点云的相对位姿关系;根据各相邻两时刻的激光点云的相对位姿关系和各时刻的位姿信息,修正所述各时刻的激光点云的位姿,以获得所述各时刻的激光点云的目标位姿。
第四方面,本申请实施例提供一种可读存储介质,所述可读存储介质上存储有计算机程序;所述计算机程序在被执行时,实现如第一方面本申请实施例所述的位姿获取方法。
第五方面,本申请实施例提供一种程序产品,所述程序产品包括计算机程序,所述计算机程序存储在可读存储介质中,可移动平台的至少一个处理器可以从所述可读存储介质读取所述计算机程序,所述至少一个处理器执行所述计算机程序使得可移动平台实施如第一方面本申请实施例所述的位姿获取方法。
本申请实施例提供的位姿获取方法、系统和可移动平台,通过获取可移动平台在目标区域内移动的过程中获取的各时刻的激光点云数据和位姿信息;根据相邻两时刻的激光点云数据,获得相邻两时刻的激光点云的相对位姿关系;根据各相邻两时刻的激光点云的相对位姿关系和各时刻的位姿信息,修正所述各时刻的激光点云的位姿,以获得所述各时刻的激光点云的目标位姿。从而使得激光点云的目标位姿可以更加准确地反映出可移动平台的实际位姿,以便在根据激光点云的目标位姿建立地图时,提高了地图的精度。
附图说明
为了更清楚地说明本申请实施例或现有技术中的技术方案,下面将对实施例或现有技术描述中所需要使用的附图作一简单地介绍,显而易见地,下面描述中的附图是本申请的一些实施例,对于本领域普通技术人员来讲,在不付出创造性劳动的前提下,还可以根据这些附图获得其他的附图。
图1是根据本申请的实施例的自动驾驶车辆100的示意性架构图;
图2为本申请一实施例提供的应用场景示意图;
图3为本申请一实施例提供的位姿获取方法的流程图;
图4为本申请一实施例提供的各时刻间的激光点云的相对位姿关系的一种示意图;
图5为本申请一实施例提供的位姿获取系统的结构示意图;
图6为本申请另一实施例提供的位姿获取系统的结构示意图;
图7为本申请一实施例提供的可移动平台的结构示意图;
图8为本申请另一实施例提供的可移动平台的结构示意图。
具体实施方式
为使本申请实施例的目的、技术方案和优点更加清楚,下面将结合本申请实施例中的附图,对本申请实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例是本申请一部分实施例,而不是全部的实施例。基于本申请中的实施例,本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例,都属于本申请保护的范围。
需要说明的是,当组件被称为“固定于”另一个组件,它可以直接在另一个组件上或者也可以存在居中的组件。当一个组件被认为是“连接”另一个组件,它可以是直接连接到另一个组件或者可能同时存在居中组件。
除非另有定义,本文所使用的所有的技术和科学术语与属于本申请的技术领域的技术人员通常理解的含义相同。本文中在本申请的说明书中所使用的术语只是为了描述具体的实施例的目的,不是旨在于限制本申请。本文所使用的术语“及/或”包括一个或多个相关的所列项目的任意的和所有的组合。
下面结合附图,对本申请的一些实施方式作详细说明。在不冲突的情况下,下述的实施例及实施例中的特征可以相互组合。
本申请的实施例提供了位姿获取方法、系统和可移动平台,其中,可移动平台可以是手持电话、手持云台、无人机、无人车、无人船、机器人或自动驾驶汽车等。
下面对本申请可移动平台的描述使用自动驾驶车辆作为示例。图1是根据本申请的实施例的自动驾驶车辆100的示意性架构图。
自动驾驶车辆100可以包括感知系统110、控制系统120和机械系统130。
其中,感知系统110用于测量自动驾驶车辆100的状态信息,即自动驾驶车辆100的感知数据,感知数据可以表示自动驾驶车辆100的位置信息和/或状态信息,例如,位置、角度、速度、加速度和角速度等。感知系统110例如可以包括视觉传感器(例如包括多个单目或双目视觉装置)、激光雷达、毫米波雷达、惯性测量单元(Inertial Measurement Unit,IMU)、全球导航卫星系统、陀螺仪、超声传感器、电子罗盘、和气压计等传感器中的至少一种。例如,全球导航卫星系统可以是全球定位系统(Global Positioning System,GPS)。
感知系统110获取到感知数据后,可以将感知数据传输给控制系统120。其中,控制系统120用于根据感知数据做出用于控制自动驾驶车辆100如何行驶的决策,例如:以多少的速度行驶,或者,以多少的刹车加速度刹车,或者,是否变道行驶,或者,左/右转行驶等。控制系统120例如可以包括:计算平台,例如车载超算平台,或者中央处理器、分布式处理单元等具有处理功能器件的至少一种。控制系统120还可以包括车辆上各种数据传输的通信链路。
控制系统120可以根据确定的决策向机械系统130输出一个或多个控制指令。其中,机械系统130用于响应来自控制系统120的一个或多个控制指令对自动驾驶车辆100进行控制,以完成上述决策,例如:机械系统130可以驱动自动驾驶车辆100的车轮转动,从而为自动驾驶车辆100的行驶提供动力,其中,车轮的转动速度可以影响到无人驾驶车辆的速度。其中,机械系统130例如可以包括:机械的车身发动机/电动机、控制的线控系统等等中的至少一种。
应理解,上述对于无人驾驶车辆各组成部分的命名仅是出于标识的目的,并不应理解为对本申请的实施例的限制。
其中,图2为本申请一实施例提供的应用场景示意图,如图2所示,自动驾驶车辆可以地面上行驶,并且自动驾驶车辆在目标区域的地面行驶的过程中,可以(例如通过上述的感知系统110)采集感知数据,该感知数据可以包括激光点云数据、位姿信息和Vins位姿数据等,然后对激光点云进行位姿修正,具体如何处理可以参见本申请下述各实施例所述。
图3为本申请一实施例提供的位姿获取方法的流程图,如图3所示,本实施例的方法可以应用于可移动平台中,也可应用于该可移动平台之外的其它电子设备。该方法包括:
S301、获取可移动平台在目标区域内移动的过程中获取的各时刻的激光点云数据和位姿信息。
S302、根据相邻两时刻的激光点云数据,获得相邻两时刻的激光点云的相对位姿关系。
S303、根据各相邻两时刻的激光点云的相对位姿关系和各时刻的位姿信息,修正所述各时刻的激光点云的位姿,以获得所述各时刻的激光点云的目标位姿。
本实施例以应用于可移动平台为例,可移动平台设备第一探测装置和第二探测装置,其中,第一探测装置用于获取激光点云数据,第二探测装置用于获取位姿信息。位姿信息可以是第二探测装置所在位置的位姿信息。第二探测装置可以设置于可移动平台,因此位姿信息可以为可移动平台的位姿信息。其中,当可移动平台在目标区域内移动的过程中,第一探测装置获取各时刻的激光点云数据,第二探测装置获取各时刻的位姿信息。相应地,可移动平台获取第一探测装置获取到的各时刻的激光点云数据以及第二探测装置获取到的各时刻的位姿信息。第一探测装置例如为激光传感器,第二探测装置例如为GPS。
若本实施例应用于该可移动平台之外的其它电子设备,则电子设备可以从可移动平台处获取可移动平台在目标区域内移动的过程中获取的各时刻的激光点云数据和位姿信息,例如接收可移动平台发送的上述信息,或者,从可移动平台的存储装置中读取上述信息。
本实施例的可移动平台获取各时刻的激光点云数据和各时刻的位姿信息之后,根据相邻两时刻的激光点云数据,获得相邻两时刻的激光点云的相对位姿关系,从而可以获得各相邻两时刻的激光点云的相对位姿关系。以各时刻分别为时刻1至时刻10为例,相应地可以获得9个相邻两时刻的激光点云的相对位姿关系。可选地,该相对位姿关系包括以下至少一项:旋转矩阵和位移矩阵。
在获得各相邻两时刻的激光点云的相对位姿关系之后,根据各相邻两时刻的激光点云的相对位姿关系以及各时刻的位姿信息,对各时刻的激光点云的位姿进行修正,从而获得各时刻的激光点云的目标位姿。
本实施例提供的位姿获取方法,通过获取可移动平台在目标区域内移动的过程中获取的各时刻的激光点云数据和位姿信息;根据相邻两时刻的激光点云数据,获得相邻两时刻的激光点云的相对位姿关系;根据各相邻两时刻的激光点云的相对位姿关系和各时刻的位姿信息,修正所述各时刻的激光点云的位姿,以获得所述各时刻的激光点云的目标位姿。从而使得激光点云的目标位姿可以更加准确地反映出可移动平台的实际位姿,以便在根据激光点云的目标位姿建立地图时,提高了地图的精度。
在一些实施例中,在执行S303之后,还根据所述各时刻的激光点云的目标位姿,建立地图。由于各时刻的激光点云的目标位姿与实际位姿非常接近,所以据此建立的地图的精度更高。
在一些实施例中,修正的激光点云的位姿包括如下至少一项:高度信息和航向信息。本实施例中,根据各相邻两时刻的激光点云的相对位姿关系和各时刻的位姿信息,修正所述各时刻的激光点云的高度信息,使得修正后的激光点云的高度更准确;或者,根据各相邻两时刻的激光点云的相对位姿关系和各时刻的位姿信息,修正所述各时刻的激光点云的航向信息,使得修正后的激光点云的航向更准确;或者,根据各相邻两时刻的激光点云的相对位姿关系和各时刻的位姿信息,修正所述各时刻的激光点云的高度信息和航向信息,使得修正后的激光点云的高度和航向更准确。
在一些实施例中,上述S302的一种可能的实现方式可以包括S3021和S3022:
S3021、获取相邻两时刻的激光点云的估计相对位姿关系。
S3022、根据所述估计相对位姿关系和所述相邻两时刻的激光点云数据,获得相邻两时刻的激光点云的相对位姿关系。
本实施例中,获取相邻两时刻的激光点云的估计相对位姿关系,然后根据该相邻两时刻的激光点云的估计相对位姿关系和该相邻两时刻的激光点云数据,获得相邻两时刻的激光点云的相对位姿关系。由于获取的相对估计位姿关系误差较大,因此再根据激光点云数据,对估计相对位姿关系进行修正,以获得更加准确的相对位姿关系。
在一些实施例中,上述S3021的一种可能的实现方式可以包括A1和A2:
A1、获取所述可移动平台在相邻两时刻获取的vins位姿数据;
A2、根据相邻两时刻的vins位姿数据,获取相邻两时刻的激光点云的估计相对位姿关系。
本实施例中,可移动平台还设有第三探测装置,第三探测装置可以获取vins位姿数据。当可移动平台在目标区域内移动的过程中,第三探测装置可以获取各时刻的vins位姿数据,相应地,可移动平台可以获取第三探测装置获取的各时刻的vins位姿数据。然后,可移动平台从中获取相邻两时刻的vins位姿数据,每时刻的vins位姿数据可以表示每时刻的激光点云的估计位姿,并根据相邻两时刻的vins位姿数据,获取两时刻的激光点云的估计相对位姿关系。
在一些实施例中,上述S3021的一种可能的实现方式可以包括B1和B2:
B1、根据相邻两时刻中前一时刻的激光点云数据和所述估计相对位姿关系,估计所述相邻两时刻中后一时刻的激光点云数据。
B2、根据估计的后一时刻的激光点云数据、第一探测装置获取的后一时刻的激光点云数据以及所述估计相对位姿关系,获得相邻两时刻的激光点云的相对位姿关系。
本实施例中,以相邻两时刻为时刻2和时刻3为例,根据时刻2的激光点云数据的时刻2与时刻3的激光点云数据的估计相对位姿关系,估计时刻3的激光点云数据,估计出的时刻3的激光点云数据与第一探测装置获取的时刻3的激光点云数据可能不同。然后再根据估计的时刻3的激光点云数据、第一探测装置获取的时刻3的激光点云数据以及上述的估计相对位姿关系,获得时刻2与时刻3的激光点云的相对位姿关系。
可选地,上述B2的一种可能的实现方式为:根据估计的后一时刻的激光点云数据和所述第一探测装置获取的后一时刻的激光点云数据,确定相对位姿关系偏差;根据所述相对位姿关系偏差和所述估计相对位姿关系,获得两时刻的激光点云的相对位姿关系。
根据估计的时刻3的激光点云数据以及第一探测装置获取的时刻3的激光点云数据,确定估计的与第一探测装置获取的时刻3的激光点云数据的相对位姿关系,称为相对位姿关系偏差,然后再根据该相对位姿关系偏差以及上述时刻2与时刻3的激光点云的估计相对位姿关系,获取时刻2与时刻3的激光点云的相对位姿关系。
在一些实施例中,上述根据相邻两时刻的激光点云数据,获得相邻两时刻的激光点云的相对位姿关系的一种可能的实现方式可以:根据相邻两时刻的激光点云数据,确定相邻两时刻的激光点云数据是否匹配。若匹配,则根据相邻两时刻的激光点云数据,获得相邻两时刻的激光点云的相对位姿关系。
本实施例中,根据相邻两时刻的激光点云数据,确定相邻两时刻的激光点云数据是否匹配,如果匹配,则执行上述根据相邻两时刻的激光点云数据,获得该相邻两时刻的相对位姿关系的方案,如果不匹配,则不获取该相邻两时刻的相对位位姿关系。这样获取的是相匹配的相邻两时刻的激光点云的相对位姿关系,这些相对位姿关系更加准确,以利于后续修正激光点云。
可选地,判断相邻两时刻的激光点云数据是否匹配,例如:获取相邻两时刻的激光点云数据间的法向量的距离,如果该法向量的距离小于预设值,则确定相邻两时刻的激光点云数据匹配,如果该法向量的距离大于等于预设值,则确定相邻两时刻的激光点云数据不匹配。
在一些实施例中,上述S303的一种可能的实现方式可以包括S3031-S3033:
S3031、针对任一时刻i,根据各时刻的位姿信息,确定位姿与时刻i的位姿相距预设距离内的K个时刻,K、i为大于等于1的整数。
S3032、根据所述K个时刻的激光点云数据和所述时刻i的激光点云数据,获取时刻i与K个时刻中每一时刻的激光点云的相对位姿关系。
S3033、根据各时刻i与K个时刻中每一时刻的激光点云的相对位姿关系、各相邻两时刻的激光点云的相对位姿关系,修正各时刻的激光点云的位姿。
本实施例中,以各时刻中的任一时刻为例,例如时刻i,i为大于等于1的整数,根据各时刻的位姿信息,确定位姿与时刻i的位姿相距预设距离内的K个时刻。可选地,位姿信息包括GPS数据,相应地,确定的是位置与时刻i的位置相距预设距离内的K个时刻。然后根据该K个时刻的激光点云数据和时刻i的激光点云数据,获取时刻i与K个时刻中每一时刻的激光点云的相对位姿关系,从而获得各个时刻中每个时刻分别与位姿相距预设距离内的至少一个时刻的激光点云的位姿。
其中,获取时刻i与K个时刻中每一时刻的激光点云的相对位姿关系的方式可以参见上述实施例中获取相邻两时刻的激光点云的相对位姿关系的方式,此处不再赘述。
然后,根据各个时刻中每个时刻分别与位姿相距预设距离内的至少一个时刻的激光点云的相对位姿关系、上述S302中获得的各相邻两时刻的激光点云的相对位姿关系,修正各时刻的激光点云的位姿。提高了修正后的激光点云的位姿的准确性。
在一些实施例中,上述S3033的一种可能的实现方式可以为C1:
C1、根据时刻i与K个时刻中时刻j的激光点云的相对位姿关系,以及时刻i与时刻j之间的各相邻时刻的激光点云的相对位姿关系,修正时刻i到时刻j的激光点云的位姿。
本实施例中,以K个时刻中的任一时刻为例,该任一时刻为时刻j,假设时刻i为时刻1,时刻j为时刻4,如图4所示,根据时刻1与时刻4的激光点云的相对位姿关系(通过S3032获得的),以及时刻1与时刻2的激光点云的相对位姿关系、时刻2与时刻3的激光点云的相对位姿关系、时刻3与时刻4的激光点云的相对位姿关系,可以修正时刻1、时刻2、时刻3、时刻4的激光点云的位姿。
在一些实施例中,上述C1的一种可能的实现方式可以包括C11-C13:
C11、根据时刻i到时刻j中的各相邻时刻的激光点云的相对位姿关系,以及时刻i与时刻j的激光点云的相对位姿关系,确定时刻i与时刻j的激光点云的相对位姿关系误差。
C12、根据所述相对位姿关系误差,修正时刻i到时刻j中的各相邻时刻的激光点云的相对位姿关系。
C13、根据修正后的时刻i到时刻j中的各相邻时刻的激光点云的相对位姿关系,修正时刻i到时刻j的激光点云数据。
本实施例中,以时刻i为时刻1,时刻j为时刻4为例,根据时刻1与时刻2的激光点云的相对位姿关系、时刻2与时刻3的激光点云的位姿关系、时刻3与时刻4的激光点云的位姿关系以及时刻1与时刻4的激光点云的位姿关系,确定时刻i与时刻j的激光点云的相对位姿关系误差。例如:以相对位姿关系包括如下至少一项:旋转矩阵和位移矩阵为例,将时刻1与时刻2的激光点云的相对位姿关系、时刻2与时刻3的激光点云的相对位姿关系、时刻3与时刻4的激光点云的相对位姿关系相乘,获得一个时刻1与时刻4的激光点云的计算相对位姿关系,再获取时刻1与时刻4的激光点云的位姿关系与时刻1与时刻4的激光点云的计算相对位姿关系的差值,该差值称为时刻1与时刻4的激光点云的相对位姿关系误差。
然后根据时刻1与时刻4的激光点云的相对位姿关系误差,修正时刻1与时刻2的激光点云的相对位姿关系、时刻2与时刻3的激光点云的相对位姿关系、时刻3与时刻4的激光点云的相对位姿关系。然后,根据修正后的时刻1与时刻2的激光点云的相对位姿关系、修正后的时刻2与时刻3的激光点云的相对位姿关系、修正后的时刻3与时刻4的激光点云的相对位姿关系,修正时刻1、时刻2、时刻3、时刻4的激光点云的位姿。使得修正后的激光点云数据根据修正后的相对位姿关系变换后,可以尽可能与相应的激光点云数据相同。
可选地,时刻i与时刻j的激光点云的相对位姿关系误差与,时刻i和时刻j的激光点云的相对位姿关系、时刻i到时刻j中各相邻两时刻的激光点云的相对位姿关系有关,因此,令时刻i与时刻j的激光点云的相对位姿关系误差最小,修正时刻i到时刻j中各相邻时刻的激光点云数据的相对位姿关系。或者,
可选地,可以令时刻i与K个时刻中每个时刻的激光点云的相对位姿关系误差之和最小,修正时刻i到K个时刻中每个时刻中各相邻时刻的激光点云数据的相对位姿关系。或者,
可选地,可以令各个时刻i与对应的K个时刻中每个时刻的激光点云的相对位姿关系误差之和最小,修正各相邻时刻的激光点云的相对位姿关系。
在一些实施例中,S3031的一种可能的实现方式为:根据各时刻的位姿信息,确定位姿与时刻i的位姿相距预设距离内的所有时刻,该所有时刻为K个时刻,需要说明的是,i的取值不,K的值取也可能不同。或者,K的数值是预先设定好的,根据各时刻的位姿信息,确定位姿与时刻i的位姿相距预设距离内的K个时刻,例如:K预先取值为2,若位姿与时刻i的位姿相距预设距离内的所有时刻大于2个时刻,则本实施例获取其中的2个时刻,以节省处理资源。
在一些实施例中,S3031的另一种可能的实现方式为:根据各时刻的位姿信息,确定位姿与时刻i的位姿相距预设距离内的时刻为M个时刻,所述M为大于等于K的整数;根据时刻i的激光点云数据和所述M个时刻的激光点云数据,确定M个时刻中激光点云数据分别与时刻i的激光点云数据匹配的K个时刻。
本实施例中,根据各时刻的位姿信息,确定位姿与时刻i的位姿相距预设距离内的所有时刻,该所有时刻为M个时刻。然后根据M个时刻的激光点云数据和时刻i的激光点云数据,确定激光点云数据与上述时刻i的激光点云数据匹配的上述K个时刻。例如:将激光点云数据与时刻i的激光点云数据匹配的所有时刻确定为K个时刻。或者,K的数值是预先设定好的,从M个时刻中激光点云数据与时刻i的激光点云数据匹配的K个时刻,例如:K预先取值为2,若M个激光点云数据中激光点云数据与时刻i的激光点云数据匹配的时刻为3个,则本实施例获取其中的2个时刻,以节省处理资源。
其中,如何确定两个时刻的激光点云数据匹配的方式可以参见确定相邻两时刻的激光点云数据匹配的方式,此处不再赘述。
本申请实施例中还提供了一种计算机存储介质,该计算机存储介质中存储有程序指令,所述程序执行时可包括如图3及其对应实施例中的位姿获取方法的部分或全部步骤。
图5为本申请一实施例提供的位姿获取系统的结构示意图,如图5所示,本实施例的位姿获取系统500可以包括:第一探测装置501、第二探测装置502和处理器503。其中,第一探测装置501、第二探测装置502和处理器503可以通过总线连接。可选地,位姿获取系统500还可以包括第三探测装置504,第三探测装置504可以通过总线与上述部件连接。
第一探测装置501,用于获取激光点云数据。
第二探测装置502,用于获取位姿信息。
处理器503,用于获取可移动平台在目标区域内移动的过程中所述第一探测装置获取的各时刻的激光点云数据和所述第二探测装置获取的各时刻的所述位姿信息;根据相邻两时刻的激光点云数据,获得相邻两时刻的激光点云的相对位姿关系;根据各相邻两时刻的激光点云的相对位姿关系和各时刻的位姿信息,修正所述各时刻的激光点云的位姿,以获得所述各时刻的激光点云的目标位姿。
在一些实施例中,所述处理器503,具体用于:
针对任一时刻i,根据各时刻的位姿信息,确定位姿与时刻i的位姿相距预设距离内的K个时刻,K、i为大于等于1的整数;
根据所述K个时刻的激光点云数据和所述时刻i的激光点云数据,获取时刻i与K个时刻中每一时刻的激光点云的相对位姿关系;
根据各时刻i与K个时刻中每一时刻的激光点云的相对位姿关系、各相邻两时刻的激光点云的相对位姿关系,修正各时刻的激光点云的位姿。
在一些实施例中,所述处理器503,具体用于:
根据时刻i与K个时刻中时刻j的激光点云的相对位姿关系,以及时刻i与时刻j之间的各相邻时刻的激光点云的相对位姿关系,修正时刻i到时刻j的激光点云的位姿。
在一些实施例中,所述处理器503,具体用于:
根据时刻i到时刻j中的各相邻时刻的激光点云的相对位姿关系,以及时刻i与时刻j的激光点云的相对位姿关系,确定时刻i与时刻j的激光点云的相对位姿关系误差;
根据所述相对位姿关系误差,修正时刻i到时刻j中的各相邻时刻的激光点云的相对位姿关系;
根据修正后的时刻i到时刻j中的各相邻时刻的激光点云的相对位姿关系,修正时刻i到时刻j的激光点云数据。
在一些实施例中,所述处理器503,具体用于:
根据所各时刻i对应的相对位姿关系误差之和最小,修正各相邻时刻的激光点云的相对位姿关系。
在一些实施例中,修正的激光点云的位姿包括如下至少一种:高度信息和航向信息。
在一些实施例中,所述处理器503,具体用于:
根据相邻两时刻的激光点云数据,确定相邻两时刻的激光点云数据是否匹配;
若匹配,则根据相邻两时刻的激光点云数据,获得相邻两时刻的激光点云的相对位姿关系。
在一些实施例中,所述处理器503,具体用于:
根据各时刻的位姿信息,确定位姿与时刻i的位姿相距预设距离内的时刻为M个时刻,所述M为大于等于K的整数;
根据时刻i的激光点云数据和所述M个时刻的激光点云数据,确定M个时刻中激光点云数据分别与时刻i的激光点云数据匹配的K个时刻。
在一些实施例中,若两时刻的激光点云数据间的法向量的距离小于预设值,则该两时刻的激光点云数据匹配。
在一些实施例中,所述处理器503,具体用于:
获取两时刻的激光点云的估计相对位姿关系;
根据所述估计相对位姿关系和所述两时刻的激光点云数据,获得两时刻的激光点云的相对位姿关系。
在一些实施例中,所述第三探测装置504,用于获取vins位姿数据;
所述处理器503,具体用于:获取所述第三探测装置504在两时刻获取的vins位姿数据;根据两时刻的vins位姿数据,获取两时刻的激光点云的估计相对位姿关系。
在一些实施例中,所述处理器503,具体用于:
根据两时刻中前一时刻的激光点云数据和所述估计相对位姿关系,估计所述两时刻中后一时刻的激光点云数据;
根据估计的后一时刻的激光点云数据、所述第一探测装置获取的后一时刻的激光点云数据以及所述估计相对位姿关系,获得两时刻的激光点云的相对位姿关系。
在一些实施例中,所述处理器503,具体用于:
根据估计的后一时刻的激光点云数据和所述第一探测装置获取的后一时刻的激光点云数据,确定相对位姿关系偏差;
根据所述相对位姿关系偏差和所述估计相对位姿关系,获得两时刻的激光点云的相对位姿关系。
在一些实施例中,所述位姿信息包括GPS数据。
在一些实施例中,所述相对位姿关系包括以下至少一项:旋转矩阵和位移矩阵。
在一些实施例中,所述处理器503,还用于根据所述各时刻的激光点云的目标位姿,建立地图。
可选地,本实施例的位姿获取系统500还可以包括:用于存储程序代码的存储器(图中未示出),存储器用于存储程序代码,当程序代码被执行时,所述位姿获取系统500可以实现上述的技术方案。
本实施例的位姿获取系统,可以用于执行图3及对应方法实施例的技术方案,其实现原理和技术效果类似,此处不再赘述。
图6为本申请另一实施例提供的位姿获取系统的结构示意图,如图6所示,本实施例的位姿获取系统600可以包括:存储器601和处理器602。其中,存储器601和处理器602可以通过总线连接。
存储器601,用于存储程序代码;
处理器602,用于当程序代码被调用时,执行:
获取可移动平台在目标区域内移动的过程中获取的各时刻的激光点云数据和所述位姿信息;根据相邻两时刻的激光点云数据,获得相邻两时刻的激光点云的相对位姿关系;根据各相邻两时刻的激光点云的相对位姿关系和各时刻的位姿信息,修正所述各时刻的激光点云的位姿,以获得所述各时刻的激光点云的目标位姿。
在一些实施例中,所述处理器602,具体用于:
针对任一时刻i,根据各时刻的位姿信息,确定位姿与时刻i的位姿相距预设距离内的K个时刻,K、i为大于等于1的整数;
根据所述K个时刻的激光点云数据和所述时刻i的激光点云数据,获取时刻i与K个时刻中每一时刻的激光点云的相对位姿关系;
根据各时刻i与K个时刻中每一时刻的激光点云的相对位姿关系、各相邻两时刻的激光点云的相对位姿关系,修正各时刻的激光点云的位姿。
在一些实施例中,所述处理器602,具体用于:
根据时刻i与K个时刻中时刻j的激光点云的相对位姿关系,以及时刻i与时刻j之间的各相邻时刻的激光点云的相对位姿关系,修正时刻i到时刻j的激光点云的位姿。
在一些实施例中,所述处理器602,具体用于:
根据时刻i到时刻j中的各相邻时刻的激光点云的相对位姿关系,以及时刻i与时刻j的激光点云的相对位姿关系,确定时刻i与时刻j的激光点云的相对位姿关系误差;
根据所述相对位姿关系误差,修正时刻i到时刻j中的各相邻时刻的激光点云的相对位姿关系;
根据修正后的时刻i到时刻j中的各相邻时刻的激光点云的相对位姿关系,修正时刻i到时刻j的激光点云数据。
在一些实施例中,所述处理器602,具体用于:
根据所各时刻i对应的相对位姿关系误差之和最小,修正各相邻时刻的激光点云的相对位姿关系。
在一些实施例中,修正的激光点云的位姿包括如下至少一种:高度信息和航向信息。
在一些实施例中,所述处理器602,具体用于:
根据相邻两时刻的激光点云数据,确定相邻两时刻的激光点云数据是否匹配;
若匹配,则根据相邻两时刻的激光点云数据,获得相邻两时刻的激光点云的相对位姿关系。
在一些实施例中,所述处理器602,具体用于:
根据各时刻的位姿信息,确定位姿与时刻i的位姿相距预设距离内的时刻为M个时刻,所述M为大于等于K的整数;
根据时刻i的激光点云数据和所述M个时刻的激光点云数据,确定M个时刻中激光点云数据分别与时刻i的激光点云数据匹配的K个时刻。
在一些实施例中,若两时刻的激光点云数据间的法向量的距离小于预设值,则该两时刻的激光点云数据匹配。
在一些实施例中,所述处理器602,具体用于:
获取两时刻的激光点云的估计相对位姿关系;
根据所述估计相对位姿关系和所述两时刻的激光点云数据,获得两时刻的激光点云的相对位姿关系。
在一些实施例中,所述处理器602,具体用于:获取所述第三探测装置在两时刻获取的vins位姿数据;根据两时刻的vins位姿数据,获取两时刻的激光点云的估计相对位姿关系。
在一些实施例中,所述处理器602,具体用于:
根据两时刻中前一时刻的激光点云数据和所述估计相对位姿关系,估计所述两时刻中后一时刻的激光点云数据;
根据估计的后一时刻的激光点云数据、所述第一探测装置获取的后一时刻的激光点云数据以及所述估计相对位姿关系,获得两时刻的激光点云的相对位姿关系。
在一些实施例中,所述处理器602,具体用于:
根据估计的后一时刻的激光点云数据和所述第一探测装置获取的后一时刻的激光点云数据,确定相对位姿关系偏差;
根据所述相对位姿关系偏差和所述估计相对位姿关系,获得两时刻的激光点云的相对位姿关系。
在一些实施例中,所述位姿信息包括GPS数据。
在一些实施例中,所述相对位姿关系包括以下至少一项:旋转矩阵和位移矩阵。
在一些实施例中,所述处理器602,还用于根据所述各时刻的激光点云的目标位姿,建立地图。
本实施例的位姿获取系统,可以用于执行图3及对应方法实施例的技术方案,其实现原理和技术效果类似,此处不再赘述。
图7为本申请一实施例提供的可移动平台的结构示意图,如图7所示,本实施例的可移动平台700可以包括:第一探测装置701、第二探测装置702和处理器703。其中,第一探测装置701、第二探测装置702和处理器703可以通过总线连接。可选地,可移动平台700还可以包括第三探测装置704,第三探测装置704可以通过总线与上述部件连接。
第一探测装置701,用于获取激光点云数据。
第二探测装置702,用于获取位姿信息。
处理器703,用于获取可移动平台700在目标区域内移动的过程中所述第一探测装置701获取的各时刻的激光点云数据和所述第二探测装置702获取的各时刻的所述位姿信息;根据相邻两时刻的激光点云数据,获得相邻两时刻的激光点云的相对位姿关系;根据各相邻两时刻的激光点云的相对位姿关系和各时刻的位姿信息,修正所述各时刻的激光点云的位姿,以获得所述各时刻的激光点云的目标位姿。
在一些实施例中,所述处理器703,具体用于:
针对任一时刻i,根据各时刻的位姿信息,确定位姿与时刻i的位姿相距预设距离内的K个时刻,K、i为大于等于1的整数;
根据所述K个时刻的激光点云数据和所述时刻i的激光点云数据,获取时刻i与K个时刻中每一时刻的激光点云的相对位姿关系;
根据各时刻i与K个时刻中每一时刻的激光点云的相对位姿关系、各相邻两时刻的激光点云的相对位姿关系,修正各时刻的激光点云的位姿。
在一些实施例中,所述处理器703,具体用于:
根据时刻i与K个时刻中时刻j的激光点云的相对位姿关系,以及时刻i与时刻j之间的各相邻时刻的激光点云的相对位姿关系,修正时刻i到时刻j的激光点云的位姿。
在一些实施例中,所述处理器703,具体用于:
根据时刻i到时刻j中的各相邻时刻的激光点云的相对位姿关系,以及时刻i与时刻j的激光点云的相对位姿关系,确定时刻i与时刻j的激光点云的相对位姿关系误差;
根据所述相对位姿关系误差,修正时刻i到时刻j中的各相邻时刻的激光点云的相对位姿关系;
根据修正后的时刻i到时刻j中的各相邻时刻的激光点云的相对位姿关系,修正时刻i到时刻j的激光点云数据。
在一些实施例中,所述处理器703,具体用于:
根据所各时刻i对应的相对位姿关系误差之和最小,修正各相邻时刻的激光点云的相对位姿关系。
在一些实施例中,修正的激光点云的位姿包括如下至少一种:高度信息和航向信息。
在一些实施例中,所述处理器703,具体用于:
根据相邻两时刻的激光点云数据,确定相邻两时刻的激光点云数据是否匹配;
若匹配,则根据相邻两时刻的激光点云数据,获得相邻两时刻的激光点云的相对位姿关系。
在一些实施例中,所述处理器703,具体用于:
根据各时刻的位姿信息,确定位姿与时刻i的位姿相距预设距离内的时刻为M个时刻,所述M为大于等于K的整数;
根据时刻i的激光点云数据和所述M个时刻的激光点云数据,确定M个时刻中激光点云数据分别与时刻i的激光点云数据匹配的K个时刻。
在一些实施例中,若两时刻的激光点云数据间的法向量的距离小于预设值,则该两时刻的激光点云数据匹配。
在一些实施例中,所述处理器703,具体用于:
获取两时刻的激光点云的估计相对位姿关系;
根据所述估计相对位姿关系和所述两时刻的激光点云数据,获得两时刻的激光点云的相对位姿关系。
在一些实施例中,所述第三探测装置704,用于获取vins位姿数据;
所述处理器703,具体用于:获取所述第三探测装置704在两时刻获取的vins位姿数据;根据两时刻的vins位姿数据,获取两时刻的激光点云的估计相对位姿关系。
在一些实施例中,所述处理器703,具体用于:
根据两时刻中前一时刻的激光点云数据和所述估计相对位姿关系,估计所述两时刻中后一时刻的激光点云数据;
根据估计的后一时刻的激光点云数据、所述第一探测装置获取的后一时刻的激光点云数据以及所述估计相对位姿关系,获得两时刻的激光点云的相对位姿关系。
在一些实施例中,所述处理器703,具体用于:
根据估计的后一时刻的激光点云数据和所述第一探测装置获取的后一时刻的激光点云数据,确定相对位姿关系偏差;
根据所述相对位姿关系偏差和所述估计相对位姿关系,获得两时刻的激光点云的相对位姿关系。
在一些实施例中,所述位姿信息包括GPS数据。
在一些实施例中,所述相对位姿关系包括以下至少一项:旋转矩阵和位移矩阵。
在一些实施例中,所述处理器703,还用于根据所述各时刻的激光点云的目标位姿,建立地图。
可选地,本实施例的可移动平台700还可以包括:用于存储程序代码的存储器(图中未示出),存储器用于存储程序代码,当程序代码被执行时,所述可移动平台700可以实现上述的技术方案。
本实施例的可移动平台,可以用于执行图3及对应方法实施例的技术方案,其实现原理和技术效果类似,此处不再赘述。
图8为本申请另一实施例提供的可移动平台的结构示意图,如图8所示,本实施例的可移动平台800可以包括:可移动平台本体801以及位姿获取系统802。
其中,所述位姿获取系统802安装于所述可移动平台本体801上。位姿获取系统802可以是独立于可移动平台本体801的设备。
其中,位姿获取系统802可以采用图5或图6所示装置实施例的结构,其对应地,可以执行图3及其对应方法实施例的技术方案,其实现原理和技术效果类似,此处不再赘述。
可选地,所述可移动平台800包括手持电话、手持云台、无人机、无人车、无人船、机器人或自动驾驶汽车。
本领域普通技术人员可以理解:实现上述方法实施例的全部或部分步骤可以通过程序指令相关的硬件来完成,前述的程序可以存储于一计算机可读取存储介质中,该程序在执行时,执行包括上述方法实施例的步骤;而前述的存储介质包括:只读内存(Read-OnlyMemory,ROM)、随机存取存储器(Random Access Memory,RAM)、磁碟或者光盘等各种可以存储程序代码的介质。
最后应说明的是:以上各实施例仅用以说明本申请的技术方案,而非对其限制;尽管参照前述各实施例对本申请进行了详细的说明,本领域的普通技术人员应当理解:其依然可以对前述各实施例所记载的技术方案进行修改,或者对其中部分或者全部技术特征进行等同替换;而这些修改或者替换,并不使相应技术方案的本质脱离本申请各实施例技术方案的范围。
- 位姿获取方法、系统和可移动平台
- 一种用于移动平台下视觉着舰导航的位姿估计系统及方法