激光里程计的位姿优化方法、装置、介质及设备制造方法及图纸

本发明专利技术公开了一种激光里程计的位姿优化方法，包括：接收激光点云帧；根据所述激光点云帧的第一位姿获取关键帧，将所述关键帧添加进预设的BA约束窗口内；遍历所述BA约束窗口内的关键帧，根据两两关键帧之间的第二位姿及位姿变化量构建代价约束函数；调用GTSAM优化库，根据所述BA约束窗口内构建的代价约束函数进行位姿优化。本发明专利技术打破了现有技术只建立连续激光点云帧间约束的束缚，通过引入BA约束窗口大大增大了激光点云帧之间的约束，有效地提高了激光里程计的精度以及鲁棒性。激光里程计的精度以及鲁棒性。激光里程计的精度以及鲁棒性。

【技术实现步骤摘要】
激光里程计的位姿优化方法、装置、介质及设备


[0001]本专利技术涉及机器人
，尤其涉及一种激光里程计的位姿优化方法、装置、介质及设备。

技术介绍

[0002]移动机器人要实现点到点的自主寻径行走功能，前提是移动机器人具备高精度的同步定位和建图能力(Simultaneous Localization and Mapping，简称SLAM)。由于激光雷达具有高精度三维目标点测量输出的能力，因此激光SLAM技术成为目前移动机器人首选的定位建图方法。激光里程计作为激光SLAM前端部分，对激光SLAM的精度起到关键作用。
[0003]现有室外用的3D激光雷达测距将近100米，第0米位置上的激光点云帧和第100米位置上的激光点云帧存在重叠部分，理论上能够直接通过点云匹配，如此机器人在这100米运动过程中的累计误差可以忽略不计。然而在现有技术中激光里程计是设定为对连续的两帧激光点云进行配准的。随着机器人位移的增加会不可避免的引入较大的累积误差，进行降低了激光里程计的精度。

技术实现思路

[0004]本专利技术实施例提供了一种激光里程计的位姿优化方法、装置、介质及设备，以解决现有激光里程计存在的累计误差大、精度欠佳的问题。
[0005]一种激光里程计的位姿优化方法，所述方法包括：
[0006]接收激光点云帧；
[0007]根据所述激光点云帧的第一位姿获取关键帧，将所述关键帧添加进预设的BA约束窗口内；
[0008]遍历所述BA约束窗口内的关键帧，根据两两关键帧之间的第二位姿及位姿变化量构建代价约束函数；
[0009]调用GTSAM优化库，根据所述BA约束窗口内构建的代价约束函数进行位姿优化。
[0010]可选地，所述根据所述激光点云帧的第一位姿获取关键帧，将所述关键帧添加进预设的BA约束窗口内包括：
[0011]采用Scan To Scan配准法计算每一激光点云帧的第一位姿；
[0012]对于连续的两个激光点云帧，根据所述第一位姿计算所述两个激光点云帧之间的位姿变化量；
[0013]当所述位姿变化量满足第一预设条件时，将后一个激光点云帧作为关键帧，添加进预设的BA约束窗口内。
[0014]可选地，所述第一预设条件包括：
[0015]所述第一位姿的位姿变化量中位移变化量大于或等于预设的第一位移阈值，和/或，所述位姿变化量中的旋转矩阵变化量大于或等于预设的旋转阈值。
[0016]可选地，所述根据两两关键帧之间的第二位姿及位姿变化量构建代价约束函数包
括：
[0017]采用Scan To SupMap配准法计算每一关键帧的第二位姿；
[0018]根据两两关键帧的第二位姿计算位姿变化量；
[0019]当所述位姿变化量满足第二预设条件时，构建所述位姿变化量对应的两个关键帧之间的代价约束函数。
[0020]可选地，所述第二预设条件包括：
[0021]所述第二位姿的位姿变化量中位移变化量大于或等于预设的第二位移阈值；
[0022]所述第二位移阈值为激光雷达最大测距的n倍，0<n≤1。
[0023]可选地，所述代价约束函数的计算公式为：
[0024][0025]在上式中，s表示BA约束窗口内的最大帧数；p
i,j
为待优化量，表示第i时刻和第，p
i
表示根据Scan To Scan配准法计算出来的第i个激光点云帧的第一位姿，p
j
表示根据Scan To Scan配准法计算出来的第j个激光点云帧的第一位姿，p'
i
表示根据Scan To SupMap配准法计算出来的第i个激光点云帧的第二位姿，p'
j
表示根据Scan To SupMap配准法计算出来的第j个激光点云帧的第二位姿，p
ij
表示第i个激光点云帧和第j个激光点云帧的位姿变化量，σ1表示第一信息矩阵，σ2表示第二信息矩阵。
[0026]可选地，在将所述后一个激光点云帧作为关键帧添加进预设的BA约束窗口内之后，所述方法还包括：
[0027]剔除所述BA约束窗口中最先添加的关键帧。
[0028]一种激光里程计的位姿优化装置，所述装置包括：
[0029]接收模块，用于接收激光点云帧；
[0030]获取模块，用于根据所述激光点云帧的第一位姿获取关键帧，将所述关键帧添加进预设的BA约束窗口内；
[0031]构建模块，用于遍历所述BA约束窗口内的关键帧，根据两两关键帧之间的第二位姿及位姿变化量构建代价约束函数；
[0032]优化模块，用于调用GTSAM优化库，根据所述BA约束窗口内构建的代价约束函数进行位姿优化。
[0033]一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器执行时实现如上所述的激光里程计的位姿优化方法。
[0034]一种终端设备，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现如上所述的激光里程计的位姿优化方法。
[0035]本专利技术实施例通过接收激光点云帧，根据所述激光点云帧的第一位姿获取关键帧，以对激光点云帧进行过滤，然后将所述关键帧添加进预设的BA约束窗口内；遍历所述BA约束窗口内的关键帧，根据两两关键帧之间的第二位姿及位姿变化量构建代价约束函数；最后调用GTSAM优化库，根据所述BA约束窗口内构建的代价约束函数进行位姿优化；从而打破了现有技术只建立连续激光点云帧间约束的束缚，通过引入BA约束窗口大大增大了激光点云帧之间的约束，有效地提高了激光里程计的精度以及鲁棒性。
附图说明
[0036]为了更清楚地说明本专利技术实施例的技术方案，下面将对本专利技术实施例的描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本专利技术的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
[0037]图1是本专利技术一实施例提供的激光里程计的位姿优化方法的实现流程图；
[0038]图2是本专利技术一实施例提供的激光里程计的位姿优化方法中步骤S102的实现流程图；
[0039]图3是本专利技术一实施例提供的激光里程计的位姿优化方法中步骤S103的实现流程图；
[0040]图4是本专利技术一实施例提供的激光里程计的位姿优化装置的示意图；
[0041]图5是本专利技术一实施例中计算机设备的一示意图。
具体实施方式
[0042]下面将结合本专利技术实施例中的附图，对本专利技术实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本专利技术一部分实施例，而不是全部的实施例。基于本专利技术中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本专利技术保护的范围。
[0043]现有激光里程计包括融合了惯性传感器IMU的激光本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种激光里程计的位姿优化方法，其特征在于，所述方法包括：接收激光点云帧；根据所述激光点云帧的第一位姿获取关键帧，将所述关键帧添加进预设的BA约束窗口内；遍历所述BA约束窗口内的关键帧，根据两两关键帧之间的第二位姿及位姿变化量构建代价约束函数；调用GTSAM优化库，根据所述BA约束窗口内构建的代价约束函数进行位姿优化。2.如权利要求1所述的激光里程计的位姿优化方法，其特征在于，所述根据所述激光点云帧的第一位姿获取关键帧，将所述关键帧添加进预设的BA约束窗口内包括：采用Scan To Scan配准法计算每一激光点云帧的第一位姿；对于连续的两个激光点云帧，根据所述第一位姿计算所述两个激光点云帧之间的位姿变化量；当所述位姿变化量满足第一预设条件时，将后一个激光点云帧作为关键帧，添加进预设的BA约束窗口内。3.如权利要求2所述的激光里程计的位姿优化方法，其特征在于，所述第一预设条件包括：所述第一位姿的位姿变化量中位移变化量大于或等于预设的第一位移阈值，和/或，所述位姿变化量中的旋转矩阵变化量大于或等于预设的旋转阈值。4.如权利要求1至3任一项所述的激光里程计的位姿优化方法，其特征在于，所述根据两两关键帧之间的第二位姿及位姿变化量构建代价约束函数包括：采用Scan To SupMap配准法计算每一关键帧的第二位姿；根据两两关键帧的第二位姿计算位姿变化量；当所述位姿变化量满足第二预设条件时，构建所述位姿变化量对应的两个关键帧之间的代价约束函数。5.如权利要求4所述的激光里程计的位姿优化方法，其特征在于，所述第二预设条件包括：所述第二位姿的位姿变化量中位移变化量大于或等于预设的第二位移阈值；所述第二位移阈值为激光雷达最大测距的n倍，0<n≤1。6.如权利要求4所述的激光里程计的位姿优化方法，其特征在于，所述代价约束函数的计算公式为：在上式中，s表示BA约束窗口内的最大...

【专利技术属性】
技术研发人员：袁国斌，刘彪，柏林，舒海燕，沈创芸，祝涛剑，王恒华，
申请(专利权)人：广州高新兴机器人有限公司，
类型：发明
国别省市：