位姿优化方法、装置、计算机设备及存储介质与流程

摘要: 本公开涉及建图,具体而言,涉及一种位姿优化方法、装置、计算机设备及存储介质。背景技术:1、目前,传统大场景激光同步定位与地图构建(simultaneousloc...
位姿优化方法、装置、计算机设备及存储介质与流程

本公开涉及建图,具体而言,涉及一种位姿优化方法、装置、计算机设备及存储介质。

背景技术:

1、目前,传统大场景激光同步定位与地图构建(simultaneous localization andmapping,slam)建图路线分为在线紧耦合slam方案、和简单离线地图拼接方案两大类。

2、其中,在线紧耦合方案将全局位姿图优化内嵌于机器人实时激光惯性里程计链路,轨迹闭环检测、图优化随机器人运动在线同步运行。简易离线拼接方案逻辑更加轻量化,其针对单趟采集数据独立建图,依靠刚性坐标变换(平移和旋转)直接拼接多段地图;闭环检测仅采用单次帧间迭代最近点算法(iterative closest point,icp)校验,仅生成单条闭环约束;图优化仅接纳激光里程计配准相对位姿约束,不支持卫星导航等绝对观测值,缺少分层因子模型,只支持一次性批处理优化。

3、两类方案广泛应用于轮式机器人场景,但直接迁移至四足足式机器人长周期、分时段大范围巡检建图场景时,存在无法适用的问题。

技术实现思路

1、本公开实施例至少提供一种位姿优化方法、装置、计算机设备及存储介质。

2、第一方面,本公开实施例提供了一种位姿优化方法,包括:

3、将对目标空间进行多次采集得到的多个录制段中的关键帧的位姿转换至同一地图参考系,得到各关键帧分别在所述地图参考系下的初始位姿;

4、根据各关键帧的初始位姿,确定多个有效闭环帧对,并针对每一有效闭环帧对,基于同录制段内的里程计传播进行邻域扩展,生成额外闭环帧对;所述有效闭环帧对与所述额外闭环帧对共同构成闭环帧对集合;

5、构建分层位姿图,所述分层位姿图包括:与各个关键帧对应的节点,每个节点对应下述边中的至少一种边:全局先验约束边、段内里程计约束边、以及基于所述闭环帧对集合构建的闭环约束边、以及卫星导航平移约束边;

6、以各关键帧对应的初始位姿作为迭代初值,以分层位姿图中的全局先验约束边、段内里程计约束边、闭环约束边作为约束条件,对分层位姿图进行非线性迭代优化,求解得到多个录制段中各关键帧分别对应的目标位姿。

7、可选的,所述将对目标空间进行多次采集得到的多个录制段中的关键帧的位姿转换至同一地图参考系,包括:

8、针对多个录制段中的每个录制段,在该录制段满足大地坐标有效的情况下,基于该录制段的首帧关键帧对应的大地坐标,确定该录制段首帧激光雷达在所述地图参考系下的参考位姿;

9、基于所述参考位姿、以及该录制段内各关键帧在对应雷达坐标系下的雷达位姿,确定该录制段内各关键帧在所述地图参考系下的初始位姿。

10、可选的,所述有效闭环帧对包括:第一关键帧、和第一关键帧之间存在闭环关系的第二关键帧、以及所述第一关键帧和所述第二关键帧之间的配准相对位姿;

11、所述根据各关键帧的初始位姿,确定多个有效闭环帧对,包括:

12、提取各关键帧的初始位姿中的平移分量,将各关键帧投影至平移分量构成的二维平面,得到各个关键帧在所述二维平面中的二维投影点;

13、基于得到的二维投影点构建二维空间索引树,并针对每一个查询关键帧,在预设平面距离阈值范围内,利用所述二维空间索引树查找空间邻近的候选关键帧,并基于所述查询关键帧、和对应的候选关键帧构成候选闭环帧对;

14、对所述候选闭环帧对进行子图配准校验,将通过所述子图配准校验的候选闭环帧对确定为所述有效闭环帧对,并基于子图配准校验过程得所述有效闭环帧对中的两个关键帧之间的配准相对位姿。

15、可选的,所述针对每一个查询关键帧,在预设平面距离阈值范围内,利用所述二维空间索引树查找空间邻近的候选关键帧,包括:

16、从所述关键帧中确定所述查询关键帧;所述查询关键帧包括至少部分所述关键帧;

17、针对各查询关键帧,读取与各查询关键帧对应的二维投影点坐标,并以预设平面距离阈值作为搜索半径,从所述二维空间索引树中,查找位于所述预设平面距离阈值范围内的邻近的候选关键帧。

18、可选的,所述基于所述查询关键帧、和对应的候选关键帧构成候选闭环帧对,包括:

19、由查询关键帧与查找到的候选关键帧生成原始帧对,对原始帧对执行去重处理,剔除互为反向的重复帧对;

20、按照录制段归属以及帧序号间隔对所述原始帧对进行过滤,滤除同一录制段内帧序号间隔小于第一预设阈值的原始帧对,得到所述候选闭环帧对。

21、可选的,所述对所述候选闭环帧对进行子图配准校验,并将通过所述子图配准校验的候选闭环帧对确定为所述有效闭环帧对,包括:

22、针对候选闭环帧对中的两个关键帧,分别在各自所属录制段内截取预设数量的前后相邻关键帧,并分别将所述候选闭环帧对中的两个关键帧分别与对应的前后相邻关键帧进行拼接,得到两组局部子图点云;

23、基于所述候选闭环帧对中的两个关键帧的初始位姿,计算得到所述两组局部子图点云之间的初始相对变换矩阵;

24、以所述初始相对变换矩阵为配准初值,对所述两组局部子图点云执行配准,得到配准后的配准相对位姿、配准内点占比以及配准误差分数;

25、当配准内点占比大于预设内点阈值,且配准误差分数小于预设误差阈值时,判定该候选闭环帧对通过子图配准校验,并将配准后的配准相对位姿,作为所述候选闭环帧对中的两个关键帧之间的配准相对位姿。

26、可选的,所述针对每一有效闭环帧对,基于同录制段内的里程计传播进行邻域扩展,生成额外闭环帧对,包括:

27、在所述有效闭环帧对中的第一关键帧所属的录制段内,选取位于所述第一关键帧的目标邻域范内的邻域关键帧;

28、根据所述有效闭环帧对中第一关键帧和第二关键帧之间的配准相对位姿、所述第一关键帧在地图参考系下的初始位姿、以及所述邻域关键帧在地图参考系下的初始位姿,通过同录制段内里程计传播,计算得到所述第二关键帧与所述邻域关键帧之间的派生相对位姿;

29、基于所述邻域关键帧、所述第二关键帧、以及所述派生相对位姿构成额外闭环帧对。

30、可选的,所述对分层位姿图进行非线性迭代优化,包括:

31、采用非线性最小二乘求解算法,基于分层位姿图进行迭代求解;或者,采用增量平滑与建图方法,基于分层位姿图进行迭代求解。

32、可选的,所述方法还包括:

33、根据各关键帧对应的目标位姿,按照录制段生成优化地图点云。

34、第二方面,本公开实施例还提供一种位姿优化装置,包括:

35、转换模块,用于将对目标空间进行多次采集得到的多个录制段中的关键帧的位姿转换至同一地图参考系,得到各关键帧分别在所述地图参考系下的初始位姿;

36、生成模块,用于根据各关键帧的初始位姿,确定多个有效闭环帧对,并针对每一有效闭环帧对,基于同录制段内的里程计传播进行邻域扩展,生成额外闭环帧对;所述有效闭环帧对与所述额外闭环帧对共同构成闭环帧对集合;

37、构建模块,用于构建分层位姿图,所述分层位姿图包括:与各个关键帧对应的节点,每个节点对应下述边中的至少一种边:全局先验约束边、段内里程计约束边、以及基于所述闭环帧对集合构建的闭环约束边、以及卫星导航平移约束边;

38、优化模块,用于以各关键帧对应的初始位姿作为迭代初值,以分层位姿图中的全局先验约束边、段内里程计约束边、闭环约束边作为约束条件,对分层位姿图进行非线性迭代优化,求解得到多个录制段中各关键帧分别对应的目标位姿。

39、第三方面,本公开可选实现方式还提供一种计算机设备,处理器、存储器,所述存储器存储有所述处理器可执行的机器可读指令,所述处理器用于执行所述存储器中存储的机器可读指令,所述机器可读指令被所述处理器执行时,所述机器可读指令被所述处理器执行时执行上述第一方面,或第一方面中任一种可能的实施方式中的步骤。

40、第四方面,本公开可选实现方式还提供一种计算机可读存储介质,该计算机可读存储介质上存储有计算机程序,该计算机程序被运行时执行上述第一方面,或第一方面中任一种可能的实施方式中的步骤。

41、本公开实施例提供的位姿优化方法,通过多个录制段中个关键帧的在地图参考系下的初始位姿,确定多个有效闭环帧对,这些有效闭环帧对基于初始位姿确定,因而能够跨录制段生成,且针对每一个有效闭环帧对,还会基于同录制段内的里程计传播进行邻域扩展,生成额外闭环帧对,有效帧对的跨录制段生成、以及额外闭环正对的扩展生成,极大的增加了闭环约束的数量,进而极大增强了位姿图对轨迹漂移约束的能力。

42、此外,本公开一个或者多个实施例提供的位姿优化方法,在对多个录制段中各关键帧的位姿进行优化处理时,会首先将对目标空间进行多次采集得到的多个录制段中的关键帧的位姿转换至同一地图参考系,从而能够实现跨批次增量处理,都在线模块与离线后处理进行解耦。

43、本公开一个或者多个实施例提供的位姿优化方法,除了将多个录制段中的关键帧的位姿转换至同一地图参考系下,还会为多个录制段中的关键帧分配全局单调递增的关键帧标识,形成全局关键帧集合,以供后续的闭环帧对生成过程、以及对分层位姿图进行非线性迭代优化使用,消除不同录制段直接合并易产生错位的问题。

44、本公开一个或者多个实施例提供的位姿优化方法,会构建包括多种约束边的分层位姿图,利用约束边的分层次构建,提供了一种能够在融合多种约束的统一分层优化框架。满足在多种约束下对多个录制段进行批量优化,提升优化的效率。

45、为使本公开的上述目的、特征和优点能更明显易懂,下文特举较佳实施例,并配合所附附图,作详细说明如下。