CN114723917A - 激光里程计的位姿优化方法、装置、介质及设备 - Google Patents

激光里程计的位姿优化方法、装置、介质及设备 Download PDF

Info

Publication number
CN114723917A
CN114723917A CN202210367762.XA CN202210367762A CN114723917A CN 114723917 A CN114723917 A CN 114723917A CN 202210367762 A CN202210367762 A CN 202210367762A CN 114723917 A CN114723917 A CN 114723917A
Authority
CN
China
Prior art keywords
pose
point cloud
laser
laser point
variation
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.)
Pending
Application number
CN202210367762.XA
Other languages
English (en)
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.)
Guangzhou Gosuncn Robot Co Ltd
Original Assignee
Guangzhou Gosuncn Robot 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 Guangzhou Gosuncn Robot Co Ltd filed Critical Guangzhou Gosuncn Robot Co Ltd
Priority to CN202210367762.XA priority Critical patent/CN114723917A/zh
Publication of CN114723917A publication Critical patent/CN114723917A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T17/00—Three dimensional [3D] modelling, e.g. data description of 3D objects
    • G06T17/20—Finite element generation, e.g. wire-frame surface description, tesselation
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10028—Range image; Depth image; 3D point clouds
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/30—Subject of image; Context of image processing
    • G06T2207/30244—Camera pose

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Computer Graphics (AREA)
  • Geometry (AREA)
  • Software Systems (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Length Measuring Devices By Optical Means (AREA)

Abstract

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

Description

激光里程计的位姿优化方法、装置、介质及设备
技术领域
本发明涉及机器人技术领域，尤其涉及一种激光里程计的位姿优化方法、装置、介质及设备。
背景技术
移动机器人要实现点到点的自主寻径行走功能，前提是移动机器人具备高精度的同步定位和建图能力(Simultaneous Localization and Mapping，简称SLAM)。由于激光雷达具有高精度三维目标点测量输出的能力，因此激光SLAM技术成为目前移动机器人首选的定位建图方法。激光里程计作为激光SLAM前端部分，对激光SLAM的精度起到关键作用。
现有室外用的3D激光雷达测距将近100米，第0米位置上的激光点云帧和第100米位置上的激光点云帧存在重叠部分，理论上能够直接通过点云匹配，如此机器人在这100米运动过程中的累计误差可以忽略不计。然而在现有技术中激光里程计是设定为对连续的两帧激光点云进行配准的。随着机器人位移的增加会不可避免的引入较大的累积误差，进行降低了激光里程计的精度。
发明内容
本发明实施例提供了一种激光里程计的位姿优化方法、装置、介质及设备，以解决现有激光里程计存在的累计误差大、精度欠佳的问题。
一种激光里程计的位姿优化方法，所述方法包括：
接收激光点云帧；
根据所述激光点云帧的第一位姿获取关键帧，将所述关键帧添加进预设的BA约束窗口内；
遍历所述BA约束窗口内的关键帧，根据两两关键帧之间的第二位姿及位姿变化量构建代价约束函数；
调用GTSAM优化库，根据所述BA约束窗口内构建的代价约束函数进行位姿优化。
可选地，所述根据所述激光点云帧的第一位姿获取关键帧，将所述关键帧添加进预设的BA约束窗口内包括：
采用Scan To Scan配准法计算每一激光点云帧的第一位姿；
对于连续的两个激光点云帧，根据所述第一位姿计算所述两个激光点云帧之间的位姿变化量；
当所述位姿变化量满足第一预设条件时，将后一个激光点云帧作为关键帧，添加进预设的BA约束窗口内。
可选地，所述第一预设条件包括：
所述第一位姿的位姿变化量中位移变化量大于或等于预设的第一位移阈值，和/或，所述位姿变化量中的旋转矩阵变化量大于或等于预设的旋转阈值。
可选地，所述根据两两关键帧之间的第二位姿及位姿变化量构建代价约束函数包括：
采用Scan To SupMap配准法计算每一关键帧的第二位姿；
根据两两关键帧的第二位姿计算位姿变化量；
当所述位姿变化量满足第二预设条件时，构建所述位姿变化量对应的两个关键帧之间的代价约束函数。
可选地，所述第二预设条件包括：
所述第二位姿的位姿变化量中位移变化量大于或等于预设的第二位移阈值；
所述第二位移阈值为激光雷达最大测距的n倍，0<n≤1。
可选地，所述代价约束函数的计算公式为：
Figure BDA0003587816200000031
在上式中，s表示BA约束窗口内的最大帧数；pi,j为待优化量，表示第i时刻和第，pi表示根据Scan To Scan配准法计算出来的第i个激光点云帧的第一位姿，pj表示根据ScanTo Scan配准法计算出来的第j个激光点云帧的第一位姿，p'i表示根据Scan To SupMap配准法计算出来的第i个激光点云帧的第二位姿，p'j表示根据Scan To SupMap配准法计算出来的第j个激光点云帧的第二位姿，pij表示第i个激光点云帧和第j个激光点云帧的位姿变化量，σ1表示第一信息矩阵，σ2表示第二信息矩阵。
可选地，在将所述后一个激光点云帧作为关键帧添加进预设的BA约束窗口内之后，所述方法还包括：
剔除所述BA约束窗口中最先添加的关键帧。
一种激光里程计的位姿优化装置，所述装置包括：
接收模块，用于接收激光点云帧；
获取模块，用于根据所述激光点云帧的第一位姿获取关键帧，将所述关键帧添加进预设的BA约束窗口内；
构建模块，用于遍历所述BA约束窗口内的关键帧，根据两两关键帧之间的第二位姿及位姿变化量构建代价约束函数；
优化模块，用于调用GTSAM优化库，根据所述BA约束窗口内构建的代价约束函数进行位姿优化。
一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器执行时实现如上所述的激光里程计的位姿优化方法。
一种终端设备，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现如上所述的激光里程计的位姿优化方法。
本发明实施例通过接收激光点云帧，根据所述激光点云帧的第一位姿获取关键帧，以对激光点云帧进行过滤，然后将所述关键帧添加进预设的BA约束窗口内；遍历所述BA约束窗口内的关键帧，根据两两关键帧之间的第二位姿及位姿变化量构建代价约束函数；最后调用GTSAM优化库，根据所述BA约束窗口内构建的代价约束函数进行位姿优化；从而打破了现有技术只建立连续激光点云帧间约束的束缚，通过引入BA约束窗口大大增大了激光点云帧之间的约束，有效地提高了激光里程计的精度以及鲁棒性。
附图说明
为了更清楚地说明本发明实施例的技术方案，下面将对本发明实施例的描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
图1是本发明一实施例提供的激光里程计的位姿优化方法的实现流程图；
图2是本发明一实施例提供的激光里程计的位姿优化方法中步骤S102的实现流程图；
图3是本发明一实施例提供的激光里程计的位姿优化方法中步骤S103的实现流程图；
图4是本发明一实施例提供的激光里程计的位姿优化装置的示意图；
图5是本发明一实施例中计算机设备的一示意图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
现有激光里程计包括融合了惯性传感器IMU的激光惯性里程计，主要采用Scan ToScan点云匹配法和Scan To SubMap点云匹配法，此两种匹配方法均会随着机器人位移的增大引入较大的累计误差，进而影响到激光里程计的精度。为解决现有激光里程计存在的累计误差大、精度欠佳的问题，本发明实施例对激光里程计的位姿计算方式进行了优化，通过引入BA约束窗口增大激光点云帧之间的约束，打破现有技术只建立连续激光点云帧间约束的束缚，有效地抑制了累计误差，提高了激光里程计的精度以及鲁棒性。
以下对本发明实施例提供的激光里程计的位姿优化方法进行详细的描述。图1为本发明实施例提供的激光里程计的位姿优化方法。如图1所示，所述激光里程计的位姿优化方法包括：
在步骤S101中，接收激光点云帧。
在这里，在进行激光里程计的位姿计算时，本发明实施例首先启动第一线程，通过第一线程接收激光点云帧。应当理解，由于机器人的移动是持续进行的，因此第一线程也是持续地接收激光点云帧。可选地，作为本发明的一个优选示例，在接收到激光点云帧之后，还可对每一激光点云帧进行体素滤波处理，以在不影响配准精度的同时，通过体素滤波处理减少点云匹配过程中的点数，提高配准的速度。
在步骤S102中，根据所述激光点云帧的第一位姿获取关键帧，将所述关键帧添加进预设的BA约束窗口内。
在接收到激光点云帧之后，从所述激光点云帧中获取关键帧。在这里，所述关键帧是指与前一激光点云帧相比，位姿变化量超过预设阈值的激光点云帧。将所述关键帧添加进BA约束窗口。其中，BA(Bundle Adjustment)源于视觉重建和视觉SLAM，物理意义为在多个不同相机位姿下能观测到同一个视觉特征点，因此通过这个视觉特征点可以把多个不同的相机位姿关联一起，达到互相约束的效果。
可选地，作为本发明的一个优选示例，图2为本发明实施例提供的激光里程计的位姿优化方法中步骤S102的实现流程图。如图2所示，步骤S102包括：
在步骤S1021中，采用Scan To Scan配准法计算每一激光点云帧的第一位姿。
在这里，所述第一位姿为经过Scan To Scan配准法计算计算得到的位姿。Scan ToScan配准法也称帧与帧配准法。由于机器人是一直在运动的，其定位输出也是连续的，步骤S101持续不断地接收到激光点云帧。每当接收到一个激光点云帧Ki，i＝0,1,2,3……，本发明实施例通过Scan To Scan配准法计算该激光点云帧的第一位姿p'i(R,T)。其中，所述第一位姿p'(R,T)包括旋转矩阵R和位移信息T。
在步骤S1022中，对于连续的两个激光点云帧，根据所述第一位姿计算所述两个激光点云帧之间的位姿变化量。
其中，所述连续的两个激光点云帧是指前后接收到的相邻的两个激光点云帧。对于当前接收到的激光点云帧p'i，获取前一激光点云帧的位姿，通过计算当前激光点云帧与前一激光点云帧之间的位姿差值，作为所述当前激光点云帧p'i的位姿变化量。所述位姿变化量包括旋转矩阵变化量和位移变化量。
在步骤S1023中，当所述位姿变化量满足第一预设条件时，将后一个激光点云帧作为关键帧，添加进预设的BA约束窗口内。
在这里，所述第一预设条件包括：所述第一位姿的位姿变化量中位移变化量大于或等于预设的第一位移阈值，和/或，所述位姿变化量中的旋转矩阵变化量大于或等于预设的旋转阈值。
本发明实施例将所述第一位姿的位姿变化量中的位移变化量与第一位移阈值进行比较，将所述旋转矩阵变化量与旋转阈值进行比较。当所述位姿变化量中的位移变化量大于或等于预设的第一位移阈值，和/或，所述位姿变化量中的旋转矩阵变化量大于或等于预设的旋转阈值，则以当前的激光点云帧Ki作为关键帧，进行保存。在保存关键帧时，一方面记录所述激光点云帧Ki对应的通过Scan To Scan配准法计算得到的第一位姿p'i(R,T)，另一方面记录激光雷达扫描的点云数据。
同时添加进预设的BA约束窗口内。在这里，BA约束窗口内的关键帧有多个，优选为20个。所述BA约束窗口内的关键帧用于进行位姿的BA优化。可选地，为保证优化运行的实时性，在每添加一针关键帧之后，本发明实施例同步剔除所述BA约束窗口中最先添加的关键帧。可见，在所述BA约束窗口内的关键帧并非都是相邻连续的关键帧，关键帧与关键帧之间可以相隔至少一个激光点云帧。
在步骤S103中，遍历所述BA约束窗口内的关键帧，根据两两关键帧之间的第二位姿及位姿变化量构建代价约束函数。
对于所述BA约束窗口内的关键帧，本发明实施例进一步计算所述关键帧的第二位姿。由于通过步骤S102得到的BA约束窗口内的关键帧并非连续的，因此任意两两关键帧之间的配准是不连续的帧间配准，本发明实施例通过对BA约束窗口中的不连续的关键帧构建代价约束函数，从而可以大大增加对激光点云帧之间的约束，拥有更多的激光点云帧的约束。
可选地，作为本发明的一个优选示例，图3为本发明实施例提供的激光里程计的位姿优化方法中步骤S103的实现流程图。如图3所示，步骤S103包括：
在步骤S1031中，采用Scan To SupMap配准法计算每一关键帧的第二位姿。
在这里本发明实施例启动第二线程，采用Scan To SubMap配准法，遍历每一关键帧，计算所述关键帧的第二位姿。Scan To SupMap配准法也称帧与子图批准法。应当理解，每一个关键帧对应一个通过Scan To Scan配准法得到的第一位姿p'i(R,T)和一个通过Scan To SubMap配准法得到的第二位姿pi(R,T)，在这里，下标i为关键帧对应的原激光点云帧的下标。
在步骤S1032中，根据两两关键帧的第二位姿计算位姿变化量。
如前所述，所述关键帧的第二位姿通过Scan To SupMap配准法计算得到。在这里，本发明实施例通过计算所述BA约束窗口内的两两关键帧的第二位姿之间的位姿差值，作为所述两两关键帧之间的位姿变化量。
在步骤S1033中，当所述位姿变化量满足第二预设条件时，构建所述位姿变化量对应的两个关键帧之间的代价约束函数。
其中，所述第二预设条件包括：所述第二位姿的位姿变化量中位移变化量大于或等于预设的第二位移阈值。作为本发明的一个优选示例，所述第二位移阈值为激光雷达最大测距的n倍，0<n≤1。示例性地，所述第二位移阈值可以为激光雷达最大测距的0.8倍。
当且仅当两个关键帧(这里假设第i个激光点云帧和第j个激光点云帧被添加进BA约束窗口内，分别记为激光点云帧Ki、Kj)之间的第二位姿的位移变化量大于或等于第二位移阈值时，本发明实施例根据所述两个关键帧(激光点云帧Ki、Kj)的第一位姿和第二位姿构建代价约束函数。可选地，作为本发明的一个优选示例，所述代价约束函数的计算公式为：
Figure BDA0003587816200000091
在上式中，s表示BA约束窗口内的最大帧数；pi,j为待优化量，表示第i时刻和第，pi表示根据Scan To Scan配准法计算出来的第i个激光点云帧的第一位姿，pj表示根据ScanTo Scan配准法计算出来的第j个激光点云帧的第一位姿，p'i表示根据Scan To SupMap配准法计算出来的第i个激光点云帧的第二位姿，p'j表示根据Scan To SupMap配准法计算出来的第j个激光点云帧的第二位姿，pij表示第i个激光点云帧和第j个激光点云帧的位姿变化量，σ1表示第一信息矩阵，σ2表示第二信息矩阵。
其中，所述第一信息矩阵σ1和第二信息矩阵σ2均为协方差矩阵求逆得到，其中第一信息矩阵σ1表征Scan To SupMap配准计算的准确程度，由点云配准得分给出，第二信息矩阵σ2表征Scan To Scan配准计算的准确程度，由点云配准得分给出。
遍历所述BA约束窗口中，第二位姿的位移变化量大于或等于第二位移阈值的两个关键帧，构建对应的代价约束函数。
在步骤S104中，调用GTSAM优化库，根据所述BA约束窗口内构建的代价约束函数进行位姿优化。
在得到所述BA约束窗口内的所有代价约束函数之后，本发明实施例通过调用GTSAM优化库，对BA约束窗口内的代价约束函数进行位姿整体优化，并在优化结束后输出每一激光点云帧Ki的位姿，i＝0,1,2,3……，从而实现激光里程计。其中，所述GTSAM优化库是Georgia Tech Smoothing and Mapping的简称，是基于因子图的C++库。
综上所述，普通激光里程计只建立了连续激光点云帧之间的位姿约束，假定其中有某一帧位姿计算有误差，则误差将在整个后续里程计进行传播，大大引起了激光里程计的累积误差，甚至某一帧位姿计算出错，将有可能导致整个激光里程计发散，本发明实施例引入BA思想，打破原先只建立连续激光点云帧间(i，j)约束，即(j-i＝1)，将拥有更多激光点云帧的约束，即j-i>1，有效地抑制了累计误差，提高了激光里程计的精度以及鲁棒性。
应理解，上述实施例中各步骤的序号的大小并不意味着执行顺序的先后，各过程的执行顺序应以其功能和内在逻辑确定，而不应对本发明实施例的实施过程构成任何限定。
在一实施例中，本发明还提供一种激光里程计的位姿优化装置，该激光里程计的位姿优化装置与上述实施例中激光里程计的位姿优化方法一一对应。如图4所示，该激光里程计的位姿优化装置包括接收模块41、获取模块42、构建模块43、优化模块44。各功能模块详细说明如下：
接收模块41，用于接收激光点云帧；
获取模块42，用于根据所述激光点云帧的第一位姿获取关键帧，将所述关键帧添加进预设的BA约束窗口内；
构建模块43，用于遍历所述BA约束窗口内的关键帧，根据两两关键帧之间的第二位姿及位姿变化量构建代价约束函数；
优化模块44，用于调用GTSAM优化库，根据所述BA约束窗口内构建的代价约束函数进行位姿优化。
可选地，所述获取模块42包括：
第一位姿计算单元，用于采用Scan To Scan配准法计算每一激光点云帧的第一位姿；
第一变化量计算单元，用于对于连续的两个激光点云帧，根据所述第一位姿计算所述两个激光点云帧之间的位姿变化量；
添加单元，用于当所述位姿变化量满足第一预设条件时，将后一个激光点云帧作为关键帧，添加进预设的BA约束窗口内。
可选地，所述第一预设条件包括：
所述第一位姿的位姿变化量中位移变化量大于或等于预设的第一位移阈值，和/或，所述位姿变化量中的旋转矩阵变化量大于或等于预设的旋转阈值。
可选地，所述构建模块43包括：
第二位姿计算单元，用于采用Scan To SupMap配准法计算每一关键帧的第二位姿；
第二变化量计算单元，用于根据两两关键帧的第二位姿计算位姿变化量；
构建单元，用于当所述位姿变化量满足第二预设条件时，构建所述位姿变化量对应的两个关键帧之间的代价约束函数。
可选地，所述第二预设条件包括：
所述第二位姿的位姿变化量中位移变化量大于或等于预设的第二位移阈值；
所述第二位移阈值为激光雷达最大测距的n倍，0<n≤1。
可选地，所述代价约束函数的计算公式为：
Figure BDA0003587816200000121
在上式中，s表示BA约束窗口内的最大帧数；pi,j为待优化量，表示第i时刻和第，pi表示根据Scan To Scan配准法计算出来的第i个激光点云帧的第一位姿，pj表示根据ScanTo Scan配准法计算出来的第j个激光点云帧的第一位姿，p'i表示根据Scan To SupMap配准法计算出来的第i个激光点云帧的第二位姿，p'j表示根据Scan To SupMap配准法计算出来的第j个激光点云帧的第二位姿，pij表示第i个激光点云帧和第j个激光点云帧的位姿变化量，σ1表示第一信息矩阵，σ2表示第二信息矩阵。
可选地，所述获取模块42还包括：
剔除单元，用于剔除所述BA约束窗口中最先添加的关键帧。
关于激光里程计的位姿优化装置的具体限定可以参见上文中对于激光里程计的位姿优化方法的限定，在此不再赘述。上述激光里程计的位姿优化装置中的各个模块可全部或部分通过软件、硬件及其组合来实现。上述各模块可以硬件形式内嵌于或独立于计算机设备中的处理器中，也可以以软件形式存储于存储器中，以便于处理器调用执行以上各个模块对应的操作。
在一个实施例中，提供了一种计算机设备，该计算机设备可以是服务器，其内部结构图可以如图5所示。该计算机设备包括通过系统总线连接的处理器、存储器、网络接口和数据库。其中，该计算机设备的处理器用于提供计算和控制能力。该计算机设备的存储器包括非易失性存储介质、内存储器。该非易失性存储介质存储有操作系统、计算机程序和数据库。该内存储器为非易失性存储介质中的操作系统和计算机程序的运行提供环境。该计算机设备的网络接口用于与外部的终端通过网络连接通信。该计算机程序被处理器执行时以实现一种激光里程计的位姿优化方法。
在一个实施例中，提供了一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，处理器执行计算机程序时实现以下步骤：
接收激光点云帧；
根据所述激光点云帧的第一位姿获取关键帧，将所述关键帧添加进预设的BA约束窗口内；
遍历所述BA约束窗口内的关键帧，根据两两关键帧之间的第二位姿及位姿变化量构建代价约束函数；
调用GTSAM优化库，根据所述BA约束窗口内构建的代价约束函数进行位姿优化。
本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一非易失性计算机可读取存储介质中，该计算机程序在执行时，可包括如上述各方法的实施例的流程。其中，本发明所提供的各实施例中所使用的对存储器、存储、数据库或其它介质的任何引用，均可包括非易失性和/或易失性存储器。非易失性存储器可包括只读存储器(ROM)、可编程ROM(PROM)、电可编程ROM(EPROM)、电可擦除可编程ROM(EEPROM)或闪存。易失性存储器可包括随机存取存储器(RAM)或者外部高速缓冲存储器。作为说明而非局限，RAM以多种形式可得，诸如静态RAM(SRAM)、动态RAM(DRAM)、同步DRAM(SDRAM)、双数据率SDRAM(DDRSDRAM)、增强型SDRAM(ESDRAM)、同步链路(Synchlink)DRAM(SLDRAM)、存储器总线(Rambus)直接RAM(RDRAM)、直接存储器总线动态RAM(DRDRAM)、以及存储器总线动态RAM(RDRAM)等。
所属领域的技术人员可以清楚地了解到，为了描述的方便和简洁，仅以上述各功能单元、模块的划分进行举例说明，实际应用中，可以根据需要而将上述功能分配由不同的功能单元、模块完成，即将所述装置的内部结构划分成不同的功能单元或模块，以完成以上描述的全部或者部分功能。
以上所述实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围，均应包含在本发明的保护范围之内。

Claims (10)

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所述的激光里程计的位姿优化方法，其特征在于，所述代价约束函数的计算公式为：
Figure FDA0003587816190000021
在上式中，s表示BA约束窗口内的最大帧数；pi,j为待优化量，表示第i时刻和第，pi表示根据Scan To Scan配准法计算出来的第i个激光点云帧的第一位姿，pj表示根据Scan ToScan配准法计算出来的第j个激光点云帧的第一位姿，p'i表示根据Scan To SupMap配准法计算出来的第i个激光点云帧的第二位姿，p'j表示根据Scan To SupMap配准法计算出来的第j个激光点云帧的第二位姿，pij表示第i个激光点云帧和第j个激光点云帧的位姿变化量，σ1表示第一信息矩阵，σ2表示第二信息矩阵。
7.如权利要求2所述的激光里程计的位姿优化方法，其特征在于，在将所述后一个激光点云帧作为关键帧添加进预设的BA约束窗口内之后，所述方法还包括：
剔除所述BA约束窗口中最先添加的关键帧。
8.一种激光里程计的位姿优化装置，其特征在于，所述装置包括：
接收模块，用于接收激光点云帧；
获取模块，用于根据所述激光点云帧的第一位姿获取关键帧，将所述关键帧添加进预设的BA约束窗口内；
构建模块，用于遍历所述BA约束窗口内的关键帧，根据两两关键帧之间的第二位姿及位姿变化量构建代价约束函数；
优化模块，用于调用GTSAM优化库，根据所述BA约束窗口内构建的代价约束函数进行位姿优化。
9.一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1至7任一项所述的激光里程计的位姿优化方法。
10.一种终端设备，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现如权利要求1至7任一项所述的激光里程计的位姿优化方法。
CN202210367762.XA 2022-04-08 2022-04-08 激光里程计的位姿优化方法、装置、介质及设备 Pending CN114723917A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210367762.XA CN114723917A (zh) 2022-04-08 2022-04-08 激光里程计的位姿优化方法、装置、介质及设备

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210367762.XA CN114723917A (zh) 2022-04-08 2022-04-08 激光里程计的位姿优化方法、装置、介质及设备

Publications (1)

Publication Number Publication Date
CN114723917A true CN114723917A (zh) 2022-07-08

Family

ID=82240948

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210367762.XA Pending CN114723917A (zh) 2022-04-08 2022-04-08 激光里程计的位姿优化方法、装置、介质及设备

Country Status (1)

Country Link
CN (1) CN114723917A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115164887A (zh) * 2022-08-30 2022-10-11 中国人民解放军国防科技大学 基于激光雷达与惯性组合的行人导航定位方法和装置

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115164887A (zh) * 2022-08-30 2022-10-11 中国人民解放军国防科技大学 基于激光雷达与惯性组合的行人导航定位方法和装置

Similar Documents

Publication Publication Date Title
CN111536964B (zh) 机器人定位方法及装置、存储介质
US10852139B2 (en) Positioning method, positioning device, and robot
CN111709975B (zh) 多目标跟踪方法、装置、电子设备及存储介质
CN111060125B (zh) 碰撞检测方法、装置、计算机设备和存储介质
CN110969649A (zh) 一种激光点云与地图的匹配评价方法、介质、终端和装置
CN111666797B (zh) 车辆定位方法、装置和计算机设备
CN114723917A (zh) 激光里程计的位姿优化方法、装置、介质及设备
CN111898682A (zh) 基于多个源模型修正新模型的方法、装置以及计算机设备
CN111709984B (zh) 位姿深度预测方法、视觉里程计方法、装置、设备及介质
CN113514843A (zh) 多子图激光雷达定位方法、系统以及终端
CN111145634B (zh) 一种校正地图的方法及装置
CN110146855B (zh) 雷达间断干扰抑制门限计算方法及装置
CN112097772B (zh) 机器人及其地图构建方法和装置
CN113189541B (zh) 一种定位方法、装置及设备
CN113256683A (zh) 目标跟踪方法及相关设备
CN113609097A (zh) 指纹库的生成方法、装置、计算机设备和存储介质
CN110887490A (zh) 一种激光定位导航的关键帧选取方法、介质、终端和装置
WO2020119747A1 (zh) 一种定位方法、终端和计算机和存储介质
CN116630442A (zh) 一种视觉slam位姿估计精度评估方法及装置
CN114463429B (zh) 机器人、地图创建方法、定位方法及介质
CN115147497A (zh) 一种标定方法、装置及电子设备
CN112873209B (zh) 定位传感器时延标定方法、装置、计算机设备及存储介质
CN115290065A (zh) 一种高精地图生成方法、装置、设备及存储介质
CN115063461A (zh) 一种误差消除方法、装置及电子设备
CN113724384A (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