CN114170280B - 基于双窗口的激光里程计方法、系统、装置 - Google Patents

基于双窗口的激光里程计方法、系统、装置 Download PDF

Info

Publication number
CN114170280B
CN114170280B CN202111498627.0A CN202111498627A CN114170280B CN 114170280 B CN114170280 B CN 114170280B CN 202111498627 A CN202111498627 A CN 202111498627A CN 114170280 B CN114170280 B CN 114170280B
Authority
CN
China
Prior art keywords
coordinate system
features
feature set
feature
pose
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
Application number
CN202111498627.0A
Other languages
English (en)
Other versions
CN114170280A (zh
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.)
Beijing Nengchuang Technology Co ltd
Institute of Automation of Chinese Academy of Science
Original Assignee
Beijing Nengchuang Technology Co ltd
Institute of Automation of Chinese Academy of Science
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 Beijing Nengchuang Technology Co ltd, Institute of Automation of Chinese Academy of Science filed Critical Beijing Nengchuang Technology Co ltd
Priority to CN202111498627.0A priority Critical patent/CN114170280B/zh
Publication of CN114170280A publication Critical patent/CN114170280A/zh
Application granted granted Critical
Publication of CN114170280B publication Critical patent/CN114170280B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/30—Determination of transform parameters for the alignment of images, i.e. image registration
    • G06T7/33—Determination of transform parameters for the alignment of images, i.e. image registration using feature-based methods
    • 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
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Abstract

本发明属于服务机器人技术领域，具体涉及一种基于双窗口的激光里程计方法、系统、装置，旨在解决现有帧图匹配与平滑相结合的激光里程计方法效率较低的问题。本发明方法包括：获取原始三维点云数据；提取特征，并构建第一特征集；构建第1帧的第一环境特征集；构建第二特征集、第二环境特征集；计算候选关联特征，进而确定出最佳关联特征；构建并求解位姿优化函数，更新激光雷达传感器当前帧坐标系到世界坐标系的位姿；利用第一特征集更新第一环境特征集；构建并求解内部小窗口中的关键帧的多帧位姿优化函数，更新内部小窗口中的所有关键帧的坐标系到世界坐标系的位姿。本发明提高了帧图匹配与平滑相结合的激光里程计方法的效率。

Description

基于双窗口的激光里程计方法、系统、装置
技术领域
本发明涉及服务机器人技术领域，具体涉及一种基于双窗口的激光里程计方法、系统、装置。
背景技术
随着机器人技术的发展，服务机器人已经进入人们生产生活的各个方面。为了完成任务，机器人需要首先确定自身在环境中的位置。机器人定位受到国内外研究人员的普遍关注。通过激光雷达传感器实现机器人定位的方法称为激光里程计。激光里程计的常用方法是通过匹配当前帧和地图中的点云数据来优化当前帧位姿，称之为帧图匹配，其中地图一般通过之前的多帧点云数据创建，代表方法包括LOAM(LiDAR Odometry andMapping)、LeGO-LOAM(Lightweight and Ground-Optimized Lidar Odometry andMapping)以及基于有向几何点DGP(Directed Geometry Point)的方法等。上述方法仅优化当前帧位姿，这使得长期的累积误差较大。为了降低累积误差，同时优化多帧位姿的平滑方法被采用。在位姿估计过程中一般需要用到非线性优化算法，代表算法包括GM(Gauss-Newton)、LM(Levenberg-Marquardt)算法。另外，对于激光雷达传感器获取的三维点云数据，研究人员普遍利用PCL点云库进行处理(包括KD-tree算法等)。
近年来，研究人员开始关注帧图匹配与平滑相结合的激光里程计方法，以有效提高里程计的精度，代表方法有BALM(Bundle Adjustment for LiDAR Mapping)等。但是，现有方案大都采用稠密的特征点或者在全局地图上进行帧图匹配，这降低了激光里程计的效率。为此，需要对现有帧图匹配与平滑相结合的激光里程计方法进行更深入的研究，以解决现有技术效率较低的问题。
发明内容
为了解决现有技术中的上述问题，即为了解决现有帧图匹配与平滑相结合的激光里程计方法效率较低的问题，本发明提出一种基于双窗口的激光里程计方法，包括以下步骤：
步骤S100，服务机器人通过激光雷达传感器感知环境，获取周围环境在激光雷达传感器当前帧坐标系下的原始三维点云数据；
步骤S200，从原始三维点云数据中提取特征，并构建特征集，作为第一特征集；
步骤S300，若激光雷达传感器当前帧是第1帧，则将其作为关键帧，并利用第一特征集构建第一环境特征集，构建后，跳转至步骤S100，否则，跳转至步骤S400；所述关键帧为从激光雷达传感器获取的所有帧中按设定的位姿间距阈值筛选的帧；
步骤S400，根据激光雷达传感器当前帧及其外部大窗口中的关键帧对应的坐标系与世界坐标系的坐标转换关系，将第一特征集、第一环境特征集转换到世界坐标系中，分别得到第二特征集、第二环境特征集；
步骤S500，基于预设的特征关联规则，计算第二特征集中的特征在第二环境特征集中的候选关联特征，进而确定出最佳关联特征；
步骤S600，基于第二特征集的特征及其在第二环境特征集中的最佳关联特征，构建位姿优化函数；利用LM算法求解位姿优化函数，得到激光雷达传感器当前帧坐标系到世界坐标系的优化后位姿，并基于优化后的位姿更新激光雷达传感器当前帧坐标系到世界坐标系的位姿；
步骤S700，循环执行步骤S400-步骤S600 K次，循环完成后，计算更新后的激光雷达传感器当前帧坐标系到世界坐标系的位姿、最近关键帧坐标系到世界坐标系的位姿之间的间距，若该间距大于等于设定的位姿间距阈值，则将当前帧作为新的关键帧，并根据第二特征集的特征及其在第二环境特征集中的最佳关联特征，利用第一特征集更新第一环境特征集，跳转至步骤S800，否则，跳转至步骤S100；其中，K为预设的循环次数；
步骤S800，基于第一环境特征集，构建内部小窗口中的关键帧的多帧位姿优化函数；利用LM算法求解多帧位姿优化函数，得到内部小窗口中的所有关键帧的坐标系到世界坐标系的优化后位姿，进而更新内部小窗口中的所有关键帧的坐标系到世界坐标系的位姿，更新后，跳转至步骤S100。
在一些优选的实施方式中，基于预设的特征关联规则，计算第二特征集中的特征在第二环境特征集中的候选关联特征，进而确定出最佳关联特征，其方法为：
对于第二特征集P2中的特征首先利用PCL点云库中的KD-tree算法在第二环境特征集Q2中寻找满足点点距离约束即/>的特征，构成特征集Ωdd，其中/>‖·‖是向量欧式范数，r是第一预设阈值；
在特征集Ωdd中进一步筛选出满足点面距离约束即的特征，构成/>的候选关联特征集Ωc，其中(p，n)∈Ωdd，d为第二预设阈值；
在候选关联特征集Ωc中寻找使得最小的特征，记为其中(p′，n′)∈Ωc，/>称为第二特征集P2中的特征/>在第二环境特征集Q2中的最佳关联特征。
在一些优选的实施方式中，基于第二特征集的特征及其在第二环境特征集中的最佳关联特征，构建位姿优化函数，其方法为：
其中，Rwl和twl分别是Twl的旋转矩阵和平移向量，Twl是激光雷达传感器当前帧Ll的坐标系到世界坐标系W的位姿，是传感器当前帧Ll的坐标系到世界坐标系W的优化后位姿，pa是第二特征集P2中的特征/>在第一特征集P1中对应特征的位置，/>和/>分别是特征/>在第二环境特征集Q2中的最佳关联特征的位置和方向，/>是矩阵转置算子，‖·‖表示向量欧式范数，/>为位姿所在的流形空间，|P2|为第二特征集P2中的特征数量，a表示第二特征集P2中的特征编号。
在一些优选的实施方式中，根据第二特征集的特征及其在第二环境特征集中的最佳关联特征，利用第一特征集更新第一环境特征集，其方法为：
如果当前帧是新的关键帧，记为则根据第二特征集P2中的特征/>及其在第二环境特征集Q2中的最佳关联特征/>将与/>对应的第一特征集P1中的特征(pa，na)加入与/>对应的第一环境特征集Q1中的特征/>所在的第i*组，记为/>其中/>
如果第二特征集P2中的特征在第二环境特征集Q2中没有找到最佳关联特征，则直接用/>对应的第一特征集P1中的特征(pa，na)，在第一环境特征集Q1中创建新的观测特征/> 其中 全部赋值为Null，然后将N1赋值为N1+1；对于第二环境特征集Q2中未选为最佳关联的特征/>找到其对应的第一环境特征集Q1中的特征(pi，j，ni，j)所在的第i组，在第一环境特征集Q1中创建新的观测特征(pi，N+1，ni，N+1)并将它赋值为Null；
如果(N+1)≤Nlarge，将N赋值为N+1；否则删除第一环境特征集Q1中所有特征在外部大窗口中最旧关键帧中的观测特征(pi，1，ni，1)，然后将其余观测特征的观测帧编号减1，即将(pi，j，ni，j)(j＝2，3，...，N+1)的观测帧编号j全部减1，并删除关键帧/>同时将关键帧的编号j也均减1，N为外部大窗口中的关键帧的数量，Nlarge为第一预设数量，即外部大窗口中关键帧的最大数量。
在一些优选的实施方式中，基于第一环境特征集，构建内部小窗口中的关键帧的多帧位姿优化函数，其方法为：
其中， 是关键帧/>的坐标系到世界坐标系W的优化后位姿，N1是第一环境特征集Q1中特征的数量，si是第一环境特征集Q1中的第i个特征在之前N个关键帧中的最旧观测帧编号，即/> 是第一环境特征集Q1中的第i个特征在外部大窗口的第j个关键帧中的观测特征qi，j＝(pi，j，ni，j)在世界坐标系W中的对应特征的位置，即/> 和/>分别是关键帧/>的坐标系到世界坐标系W的位姿/>的旋转矩阵和平移向量，/>和/>分别是第一环境特征集Q1中的/>在世界坐标系W中的对应特征的位置和方向，即 和/>分别是关键帧/>的坐标系到世界坐标系W的位姿/>的旋转矩阵和平移向量，/>是矩阵转置算子，‖·‖表示向量欧式范数，/>为位姿所在的流形空间。
在一些优选的实施方式中，利用LM算法求解多帧位姿优化函数，得到内部小窗口中的所有关键帧的坐标系到世界坐标系的优化后位姿，其方法为：
对于多帧位姿优化函数，用代替/>即用代替/>用/>代替/>从而位姿优化问题转化为欧式空间/>中的等价问题，直接应用LM算法求解得到/>进而获得M个关键帧的坐标系到世界坐标系W的优化后位姿
本发明的第二方面，提出了一种基于双窗口的激光里程计系统，包括：获取模块、特征提取模块、首帧处理模块、特征转换模块、特征关联模块、位姿优化模块、特征更新模块、位姿平滑模块；
所述获取模块，配置为服务机器人通过激光雷达传感器感知环境，获取周围环境在激光雷达传感器当前帧坐标系下的原始三维点云数据；
所述特征提取模块，配置为从原始三维点云数据中提取特征，并构建特征集，作为第一特征集；
所述首帧处理模块，配置为若激光雷达传感器当前帧是第1帧，则将其作为关键帧，并利用第一特征集构建第一环境特征集，构建后，跳转至获取模块，否则，跳转至特征转换模块；所述关键帧为从激光雷达传感器获取的所有帧中按设定的位姿间距阈值筛选的帧；
所述特征转换模块，配置为根据激光雷达传感器当前帧及其外部大窗口中的关键帧对应的坐标系与世界坐标系的坐标转换关系，将第一特征集、第一环境特征集转换到世界坐标系中，分别得到第二特征集、第二环境特征集；
所述特征关联模块，配置为基于预设的特征关联规则，计算第二特征集中的特征在第二环境特征集中的候选关联特征，进而确定出最佳关联特征；
所述位姿优化模块，配置为基于第二特征集的特征及其在第二环境特征集中的最佳关联特征，构建位姿优化函数；利用LM算法求解位姿优化函数，得到激光雷达传感器当前帧坐标系到世界坐标系的优化后位姿，并基于优化后的位姿更新激光雷达传感器当前帧坐标系到世界坐标系的位姿；
所述特征更新模块，配置为循环执行特征转换模块-位姿优化模块K次，循环完成后，计算更新后的激光雷达传感器当前帧坐标系到世界坐标系的位姿、最近关键帧坐标系到世界坐标系的位姿之间的间距，若该间距大于等于设定的位姿间距阈值，则将当前帧作为新的关键帧，并根据第二特征集的特征及其在第二环境特征集中的最佳关联特征，利用第一特征集更新第一环境特征集，跳转至位姿平滑模块，否则，跳转至获取模块；其中，K为预设的循环次数；
所述位姿平滑模块，配置为基于第一环境特征集，构建内部小窗口中的关键帧的多帧位姿优化函数；利用LM算法求解多帧位姿优化函数，得到内部小窗口中的所有关键帧的坐标系到世界坐标系的优化后位姿，进而更新内部小窗口中的所有关键帧的坐标系到世界坐标系的位姿，更新后，跳转至获取模块。
本发明的第三方面，提出了一种存储装置，其中存储有多条程序，其特征在于，所述程序适用于由处理器加载并执行以实现上述的基于双窗口的激光里程计方法。
本发明的第四方面，提出了一种处理装置，包括处理器、存储装置；处理器，适用于执行各条程序；存储装置，适用于存储多条程序；其特征在于，所述程序适用于由处理器加载并执行以实现上述的基于双窗口的激光里程计方法。
本发明的有益效果：
本发明提高了帧图匹配与平滑相结合的激光里程计方法的效率。本发明的双窗口由嵌套的两个窗口组成，其中外部大窗口中所有关键帧的稀疏有向几何点用于构建帧图匹配的局部地图，内部小窗口中所有关键帧的位姿被平滑以减少累积误差。外部大窗口提高了帧图匹配的精度，而内部小窗口保证了平滑的效率。通过将帧图匹配和平滑有机结合起来，实现了高效精确的激光里程计，为服务机器人在家庭、办公等环境下的定位提供技术支持。
附图说明
图1是本发明一种实施例的基于双窗口的激光里程计方法的流程示意图；
图2是本发明一种实施例的基于双窗口的激光里程计系统的框架示意图。
具体实施方式
为使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
下面结合附图和实施例对本申请作进一步的详细说明。可以理解的是，此处所描述的具体实施例仅用于解释相关发明，而非对该发明的限定。另外还需要说明的是，为了便于描述，附图中仅示出了与有关发明相关的部分。
需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。
本发明的一种基于双窗口的激光里程计方法，如图1所示，包括以下步骤：
步骤S100，服务机器人通过激光雷达传感器感知环境，获取周围环境在激光雷达传感器当前帧坐标系下的原始三维点云数据；
步骤S200，从原始三维点云数据中提取特征，并构建特征集，作为第一特征集；
步骤S300，若激光雷达传感器当前帧是第1帧，则将其作为关键帧，并利用第一特征集构建第一环境特征集，构建后，跳转至步骤S100，否则，跳转至步骤S400；所述关键帧为从激光雷达传感器获取的所有帧中按设定的位姿间距阈值筛选的帧；
步骤S400，根据激光雷达传感器当前帧及其外部大窗口中的关键帧对应的坐标系与世界坐标系的坐标转换关系，将第一特征集、第一环境特征集转换到世界坐标系中，分别得到第二特征集、第二环境特征集；
步骤S500，基于预设的特征关联规则，计算第二特征集中的特征在第二环境特征集中的候选关联特征，进而确定出最佳关联特征；
步骤S600，基于第二特征集的特征及其在第二环境特征集中的最佳关联特征，构建位姿优化函数；利用LM算法求解位姿优化函数，得到激光雷达传感器当前帧坐标系到世界坐标系的优化后位姿，并基于优化后的位姿更新激光雷达传感器当前帧坐标系到世界坐标系的位姿；
步骤S700，循环执行步骤S400-步骤S600 K次，循环完成后，计算更新后的激光雷达传感器当前帧坐标系到世界坐标系的位姿、最近关键帧坐标系到世界坐标系的位姿之间的间距，若该间距大于等于设定的位姿间距阈值，则将当前帧作为新的关键帧，并根据第二特征集的特征及其在第二环境特征集中的最佳关联特征，利用第一特征集更新第一环境特征集，跳转至步骤S800，否则，跳转至步骤S100；其中，K为预设的循环次数；
步骤S800，基于第一环境特征集，构建内部小窗口中的关键帧的多帧位姿优化函数；利用LM算法求解多帧位姿优化函数，得到内部小窗口中的所有关键帧的坐标系到世界坐标系的优化后位姿，进而更新内部小窗口中的所有关键帧的坐标系到世界坐标系的位姿，更新后，跳转至步骤S100。
为了更清晰地对本发明基于双窗口的激光里程计方法进行说明，下面结合附图对本发明方法一种实施例中各步骤进行展开详述。
本发明的双窗口由嵌套的两个窗口组成：外部大窗口和内部小窗口。外部大窗口包含当前帧最近的Nlarge个关键帧(关键帧的定义在下文中陈述)，如果当前帧最近的关键帧的数量小于Nlarge，外部大窗口将包含当前帧最近的所有的关键帧，Nlarge为第一预设数量，即外部大窗口中关键帧的最大数量，本发明中Nlarge优选设置为32；内部小窗口由当前帧最近的Msmall个关键帧构成，如果当前帧最近的关键帧的数量小于Msmall，内部小窗口将包含当前帧最近的所有的关键帧，Msmall为第二预设数量，即内部小窗口中关键帧的最大数量，本发明中Msmall优选设置为8。
本发明基于双窗口的激光里程计方法，具体处理过程如下：
步骤S100，服务机器人通过激光雷达传感器感知环境，获取周围环境在激光雷达传感器当前帧坐标系下的原始三维点云数据；
在本实施例中，服务机器人通过安装于自身的激光雷达传感器获取周围环境在激光雷达传感器当前帧Ll的坐标系中的原始三维点云数据Ds，其中l是一个正整数，表示激光雷达传感器当前帧是第l帧，激光雷达传感器优选采用Velodyne HDL64。
步骤S200，从原始三维点云数据中提取特征，并构建特征集，作为第一特征集；
在本实施例中，利用基于有向几何点DGP和稀疏帧的三维激光雷达SLAM的方法中的特征提取策略从原始三维点云Ds中提取特征并构建特征集，作为第一特征集，第一特征集其中/> pa、na分别表示P1中第a个特征的位置和方向，且满足‖na‖＝1，‖·‖表示向量欧式范数，|P1|表示第一特征集P1中的特征数量。
其中，基于有向几何点DGP和稀疏帧的三维激光雷达SLAM的方法中的特征提取策略，具体可参考文献：Shuang Liang,Zhiqiang Cao,Chengpeng Wang,Junzhi Yu.A Novel3D LiDAR SLAM Based on Directed Geometry Point and Sparse Frame.IEEE Roboticsand Automation Letters，2021，6(2)，374-381中“IV.THE PROPOSED 3D LIDAR SLAM”的“A.DGP Extraction”部分。
步骤S300，若激光雷达传感器当前帧是第1帧，则将其作为关键帧，并利用第一特征集构建第一环境特征集，构建后，跳转至步骤S100，否则，跳转至步骤S400；所述关键帧为从激光雷达传感器获取的所有帧中按设定的位姿间距阈值筛选的帧；
在本实施例中，如果激光雷达传感器当前帧是第1帧，将其置为关键帧，并利用第一特征集P1构建第一环境特征集Q1，跳转至步骤S100，否则，跳转至步骤S400；关键帧是从激光雷达传感器获取的所有帧中筛选出来的帧，按照相邻2个关键帧之间的位姿间距不小于df的原则进行选取，其中df为设定的位姿间距阈值，本发明中优选设置为2米；
第一环境特征集Q1定义为Q1＝{qi，j}，1≤i≤N1，1≤j≤N，其中N1为Q1中不同特征的数量，N为外部大窗口中的关键帧的数量，N≤Nlarge，qi，j＝(pi，j，ni，j)表示第i个特征在外部大窗口的第j个关键帧中的观测特征，j即为第i个特征的观测帧编号，pi，j和ni，j分别表示特征qi，j的位置和方向，‖ni，j‖＝1。激光雷达传感器当前帧是第1帧时利用第一特征集P1构建第一环境特征集Q1如下：N1＝|P1|，pi，N＝pi，ni，N＝ni，qi，j(1≤i≤N1，1≤j≤N-1)全部赋值为Null。
步骤S400，根据激光雷达传感器当前帧及其外部大窗口中的关键帧对应的坐标系与世界坐标系的坐标转换关系，将第一特征集、第一环境特征集转换到世界坐标系中，分别得到第二特征集、第二环境特征集；
在本实施例中，根据激光雷达传感器当前帧Ll及其外部大窗口中的关键帧对应的坐标系与世界坐标系W的坐标转换关系，将第一特征集P1、第一环境特征集Q1转换到世界坐标系中，分别得到第二特征集P2、第二环境特征集Q2，其中世界坐标系W是激光雷达传感器第1帧L1的坐标系；
激光雷达传感器当前帧Ll的坐标系到世界坐标系W的特征转换关系如下：其中，/>为第一特征集P1中的特征(pa，na)在世界坐标系W中的表示，Rwl和twl分别为激光雷达传感器当前帧Ll的坐标系到世界坐标系W的位姿Twl的旋转矩阵和平移向量，变换后的特征/>组成第二特征集P2；
外部大窗口中的关键帧的坐标系与世界坐标系W的特征转换关系如下：/>其中/>为第一环境特征集Q1中的特征(pi，j，ni，j)在世界坐标系W中的表示，/>和/>分别是关键帧/>坐标系到世界坐标系W的位姿/>的旋转矩阵和平移向量，变换后的特征/>组成第二环境特征集Q2。需要说明的是，第一环境特征集Q1中赋值为Null的特征经过变换后，在第二环境特征集Q2中仍为Null。
步骤S500，基于预设的特征关联规则，计算第二特征集中的特征在第二环境特征集中的候选关联特征，进而确定出最佳关联特征；
在本实施例中，基于预设的特征关联规则，计算第二特征集P2中的特征在第二环境特征集Q2中的候选关联特征，进而确定出最佳关联特征；
该步骤中，对第二特征集P2中的每个特征，在第二环境特征集Q2中寻找满足特征关联原则的关联特征，即寻找同时满足点点距离约束和点面距离约束的特征，作为候选关联特征；进而选取点面距离最小的候选关联特征作为最佳关联特征；
对于第二特征集P2中的特征首先利用PCL点云库中的KD-tree算法在第二环境特征集Q2中寻找满足点点距离约束即/> 的特征，构成特征集Ωdd，其中/>‖·‖是向量欧式范数，r是第一预设阈值，本发明中优选设置为2米；在特征集Ωdd中进一步筛选出满足点面距离约束即/>的特征，构成的候选关联特征集Ωc，其中(p，n)∈Ωdd，d为第二预设阈值，本发明中优选设置为0.5米；
然后在候选关联特征集Ωc中寻找使得最小的特征，记为其中(p′，n′)∈Ωc。/>称为第二特征集P2中的特征/>在第二环境特征集Q2中的最佳关联特征，‖·‖表示向量欧式范数。
步骤S600，基于第二特征集的特征及其在第二环境特征集中的最佳关联特征，构建位姿优化函数；利用LM算法求解位姿优化函数，得到激光雷达传感器当前帧坐标系到世界坐标系的优化后位姿，并基于优化后的位姿更新激光雷达传感器当前帧坐标系到世界坐标系的位姿；
在本实施例中，基于第二特征集P2的特征及其在第二环境特征集Q2中的最佳关联特征，构建位姿优化函数，利用retraction映射将位姿优化函数从流形空间转换到欧式空间下，并利用LM算法求解以优化激光雷达传感器当前帧Ll的坐标系到世界坐标系W的位姿Twl；
该步骤中，构建位姿优化函数如下：
其中，Rwl和twl分别是Twl的旋转矩阵和平移向量，是传感器当前帧Ll的坐标系到世界坐标系W的优化后位姿，pa是第二特征集P2中的特征/>在第一特征集P1中对应特征的位置，/>和/>分别是特征/>在第二环境特征集Q2中的最佳关联特征的位置和方向，/>是矩阵转置算子，‖·‖表示向量欧式范数，/>为位姿所在的流形空间，|P2|为第二特征集P2中的特征数量；
利用LM算法求解位姿优化函数的过程如下。首先，对于空间中的任一位姿T，定义retraction映射：/>其中，R和t是T的旋转矩阵和平移向量，δφ和δt为位姿增量，/> 为六维欧式空间，Exp(δφ)是δφ的大写指数映射。在式(1)所示的位姿优化函数中，用/>代替Twl，即用RwlExp(δφ)代替Rwl、用Rwlδt+twl代替twl，从而位姿优化问题转化为欧式空间/>中的等价问题： 直接应用LM算法求解得到(δφ*，δt*)，进而获得激光雷达传感器当前帧Ll的坐标系到世界坐标系W的优化后位姿/>用优化后的位姿/>更新激光雷达传感器当前帧Ll的坐标系到世界坐标系W的位姿Twl，即/>
步骤S700，循环执行步骤S400-步骤S600 K次，循环完成后，计算更新后的激光雷达传感器当前帧坐标系到世界坐标系的位姿、最近关键帧坐标系到世界坐标系的位姿之间的间距，若该间距大于等于设定的位姿间距阈值，则将当前帧作为新的关键帧，并根据第二特征集的特征及其在第二环境特征集中的最佳关联特征，利用第一特征集更新第一环境特征集，跳转至步骤S800，否则，跳转至步骤S100；
在本实施例中，循环执行步骤S400-S600共K次，其中K为预设的循环次数，本发明中优选设置为3，循环后基于激光雷达传感器当前帧Ll的坐标系到世界坐标系W的位姿Twl、最近关键帧的坐标系到世界坐标系W的位姿/>判断当前帧是否为新的关键帧，如果是新关键帧，则根据第二特征集P2的特征及其在第二环境特征集Q2中的最佳关联特征，使用第一特征集P1更新第一环境特征集Q1；
该步骤中，当时，当前帧Ll被认为是新的关键帧，其中twl是当前帧Ll的坐标系到世界坐标系W的位姿Twl的平移向量，/>为最近关键帧/>的坐标系到世界坐标系W的位姿/>的平移向量，‖·‖表示向量欧式范数；
该步骤中，如果当前帧是新的关键帧，记为则根据第二特征集P2中的特征及其在第二环境特征集Q2中的最佳关联特征/>将与/>对应的第一特征集P1中的特征(pa，na)加入与/>对应的第一环境特征集Q1中的特征所在的第i*组，记为/>其中/>如果第二特征集P2中的特征/>在第二环境特征集Q2中没有找到最佳关联特征，则直接用/>对应的第一特征集P1中的特征(pa，na)，在第一环境特征集Q1中创建新的观测特征/>其中/> 全部赋值为Null，/>然后将N1赋值为N1+1；对于第二环境特征集Q2中未选为最佳关联的特征/>找到其对应的第一环境特征集Q1中的特征(pi，j，ni，j)所在的第i组，在第一环境特征集Q1中创建新的观测特征(pi，N+1，ni，N+1)并将它赋值为Null；
如果(N+1)≤Nlarge，将N赋值为N+1；否则删除第一环境特征集Q1中所有特征在外部大窗口中最旧关键帧中的观测特征(pi，1，ni，1)，然后将其余观测特征的观测帧编号减1，即将(pi，j，ni，j)(j＝2，3，...，N+1)的观测帧编号j全部减1，并删除关键帧/>同时将关键帧/>的编号j也均减1。
步骤S800，基于第一环境特征集，构建内部小窗口中的关键帧的多帧位姿优化函数；利用LM算法求解多帧位姿优化函数，得到内部小窗口中的所有关键帧的坐标系到世界坐标系的优化后位姿，进而更新内部小窗口中的所有关键帧的坐标系到世界坐标系的位姿，更新后，跳转至步骤S100；
在本实施例中，基于第一环境特征集Q1，构建内部小窗口中M个关键帧的位姿优化函数，其中，M为内部小窗口的关键帧的个数，M≤Msmall，利用retraction映射将位姿优化函数从流形空间转换到欧式空间下，并利用LM算法求解以平滑内部小窗口中的所有关键帧/>的坐标系到世界坐标系W的位姿集合/>
基于第一环境特征集Q1构建多帧位姿优化函数如下：
其中 是关键帧/>的坐标系到世界坐标系W的优化后位姿，N1是第一环境特征集Q1中特征的数量，si是第一环境特征集Q1中的第i个特征在之前N个关键帧中的最旧观测帧编号，即/> 是第一环境特征集Q1中的qi，j＝(pi，j，ni，j)在世界坐标系W中的对应特征的位置，即/> 和/>分别是关键帧/>的坐标系到世界坐标系W的位姿/>的旋转矩阵和平移向量，/>和/>分别是第一环境特征集Q1中的/>在世界坐标系W中的对应特征的位置和方向，即/> 和/>分别是关键帧/>的坐标系到世界坐标系W的位姿/>的旋转矩阵和平移向量，/>是矩阵转置算子，‖·‖表示向量欧式范数，/>为位姿所在的流形空间；
利用LM算法对公式(2)所示的多帧位姿优化函数进行求解。对于式(2)所示的多帧位姿优化函数，用代替即用/>代替/>用/>代替从而位姿优化问题转化为欧式空间/>中的等价问题，直接应用LM算法求解得到/>进而获得M个关键帧的坐标系到世界坐标系W的优化后位姿/>最终用优化后的位姿/>更新内部小窗口中所有关键帧的坐标系到世界坐标系W的位姿/>即
采用本发明的基于双窗口的激光里程计方法，通过帧图匹配和平滑的有效结合，在保证激光里程计精度的同时，提升了激光里程计的效率，为服务机器人在家庭、办公等环境下的定位提供技术支持。
本发明第二实施例的一种基于双窗口的激光里程计系统，如图2所示，包括：获取模块100、特征提取模块200、首帧处理模块300、特征转换模块400、特征关联模块500、位姿优化模块600、特征更新模块700、位姿平滑模块800；
所述获取模块100，配置为服务机器人通过激光雷达传感器感知环境，获取周围环境在激光雷达传感器当前帧坐标系下的原始三维点云数据；
所述特征提取模块200，配置为从原始三维点云数据中提取特征，并构建特征集，作为第一特征集；
所述首帧处理模块300，配置为若激光雷达传感器当前帧是第1帧，则将其作为关键帧，并利用第一特征集构建第一环境特征集，构建后，跳转至获取模块100，否则，跳转至特征转换模块400；所述关键帧为从激光雷达传感器获取的所有帧中按设定的位姿间距阈值筛选的帧；
所述特征转换模块400，配置为根据激光雷达传感器当前帧及其外部大窗口中的关键帧对应的坐标系与世界坐标系的坐标转换关系，将第一特征集、第一环境特征集转换到世界坐标系中，分别得到第二特征集、第二环境特征集；
所述特征关联模块500，配置为基于预设的特征关联规则，计算第二特征集中的特征在第二环境特征集中的候选关联特征，进而确定出最佳关联特征；
所述位姿优化模块600，配置为基于第二特征集的特征及其在第二环境特征集中的最佳关联特征，构建位姿优化函数；利用LM算法求解位姿优化函数，得到激光雷达传感器当前帧坐标系到世界坐标系的优化后位姿，并基于优化后的位姿更新激光雷达传感器当前帧坐标系到世界坐标系的位姿；
所述特征更新模块700，配置为循环执行特征转换模块400-位姿优化模块600K次，循环完成后，计算更新后的激光雷达传感器当前帧坐标系到世界坐标系的位姿、最近关键帧坐标系到世界坐标系的位姿之间的间距，若该间距大于等于设定的位姿间距阈值，则将当前帧作为新的关键帧，并根据第二特征集的特征及其在第二环境特征集中的最佳关联特征，利用第一特征集更新第一环境特征集，跳转至位姿平滑模块800，否则，跳转至获取模块100；
所述位姿平滑模块800，配置为基于第一环境特征集，构建内部小窗口中的关键帧的多帧位姿优化函数；利用LM算法求解多帧位姿优化函数，得到内部小窗口中的所有关键帧的坐标系到世界坐标系的优化后位姿，进而更新内部小窗口中的所有关键帧的坐标系到世界坐标系的位姿，更新后，跳转至获取模块100。
所述技术领域的技术人员可以清楚的了解到，为描述的方便和简洁，上述描述的系统的具体的工作过程及有关说明，可以参考前述方法实施例中的对应过程，在此不再赘述。
需要说明的是，上述实施例提供的基于双窗口的激光里程计系统，仅以上述各功能模块的划分进行举例说明，在实际应用中，可以根据需要而将上述功能分配由不同的功能模块来完成，即将本发明实施例中的模块或者步骤再分解或者组合，例如，上述实施例的模块可以合并为一个模块，也可以进一步拆分成多个子模块，以完成以上描述的全部或者部分功能。对于本发明实施例中涉及的模块、步骤的名称，仅仅是为了区分各个模块或者步骤，不视为对本发明的不当限定。
本发明第三实施例的一种存储装置，其中存储有多条程序，所述程序适用于由处理器加载并实现上述的基于双窗口的激光里程计方法。
本发明第四实施例的一种处理装置，包括处理器、存储装置；处理器，适于执行各条程序；存储装置，适于存储多条程序；所述程序适于由处理器加载并执行以实现上述的基于双窗口的激光里程计方法。
所述技术领域的技术人员可以清楚的了解到，为描述的方便和简洁，上述描述的存储装置、处理装置的具体工作过程及有关说明，可以参考前述方法实例中的对应过程，在此不再赘述。
本领域技术人员应该能够意识到，结合本文中所公开的实施例描述的各示例的模块、方法步骤，能够以电子硬件、计算机软件或者二者的结合来实现，软件模块、方法步骤对应的程序可以置于随机存储器(RAM)、内存、只读存储器(ROM)、电可编程ROM、电可擦除可编程ROM、寄存器、硬盘、可移动磁盘、CD-ROM、或技术领域内所公知的任意其它形式的存储介质中。为了清楚地说明电子硬件和软件的可互换性，在上述说明中已经按照功能一般性地描述了各示例的组成及步骤。这些功能究竟以电子硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。本领域技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本发明的范围。
术语“第一”、“第二”、“第三”等是用于区别类似的对象，而不是用于描述或表示特定的顺序或先后次序。
至此，已经结合附图所示的优选实施方式描述了本发明的技术方案，但是，本领域技术人员容易理解的是，本发明的保护范围显然不局限于这些具体实施方式。在不偏离本发明的原理的前提下，本领域技术人员可以对相关技术特征做出等同的更改或替换，这些更改或替换之后的技术方案都将落入本发明的保护范围之内。

Claims (9)

1.一种基于双窗口的激光里程计方法，其特征在于，该方法包括：
步骤S100，服务机器人通过激光雷达传感器感知环境，获取周围环境在激光雷达传感器当前帧坐标系下的原始三维点云数据；
步骤S200，从原始三维点云数据中提取特征，并构建特征集，作为第一特征集；
步骤S300，若激光雷达传感器当前帧是第1帧，则将其作为关键帧，并利用第一特征集构建第一环境特征集，构建后，跳转至步骤S100，否则，跳转至步骤S400；所述关键帧为从激光雷达传感器获取的所有帧中按设定的位姿间距阈值筛选的帧；
步骤S400，根据激光雷达传感器当前帧及其外部大窗口中的关键帧对应的坐标系与世界坐标系的坐标转换关系，将第一特征集、第一环境特征集转换到世界坐标系中，分别得到第二特征集、第二环境特征集；
步骤S500，基于预设的特征关联规则，计算第二特征集中的特征在第二环境特征集中的候选关联特征，进而确定出最佳关联特征；
步骤S600，基于第二特征集的特征及其在第二环境特征集中的最佳关联特征，构建位姿优化函数；利用LM算法求解位姿优化函数，得到激光雷达传感器当前帧坐标系到世界坐标系的优化后位姿，并基于优化后的位姿更新激光雷达传感器当前帧坐标系到世界坐标系的位姿；
步骤S700，循环执行步骤S400-步骤S600 K次，循环完成后，计算更新后的激光雷达传感器当前帧坐标系到世界坐标系的位姿、最近关键帧坐标系到世界坐标系的位姿之间的间距，若该间距大于等于设定的位姿间距阈值，则将当前帧作为新的关键帧，并根据第二特征集的特征及其在第二环境特征集中的最佳关联特征，利用第一特征集更新第一环境特征集，跳转至步骤S800，否则，跳转至步骤S100；其中，K为预设的循环次数；
步骤S800，基于第一环境特征集，构建内部小窗口中的关键帧的多帧位姿优化函数；利用LM算法求解多帧位姿优化函数，得到内部小窗口中的所有关键帧的坐标系到世界坐标系的优化后位姿，进而更新内部小窗口中的所有关键帧的坐标系到世界坐标系的位姿，更新后，跳转至步骤S100。
2.根据权利要求1所述的基于双窗口的激光里程计方法，其特征在于，基于预设的特征关联规则，计算第二特征集中的特征在第二环境特征集中的候选关联特征，进而确定出最佳关联特征，其方法为：
对于第二特征集P2中的特征首先利用PCL点云库中的KD-tree算法在第二环境特征集Q2中寻找满足点点距离约束即/> 的特征，构成特征集Ωdd，其中‖·‖是向量欧式范数，r是第一预设阈值；/>为第一特征集P1中的特征(pa,na)在世界坐标系W中的表示；pa、na分别表示P1中第a个特征的位置和方向；为第一环境特征集Q1中的特征(pi,j,ni,j)在世界坐标系W中的表示，所述第一环境特征集Q1定义为Q1＝{qi,j}，其中qi,j＝(pi,j,ni,j)表示第i个特征在外部大窗口的第j个关键帧中的观测特征，j即为第i个特征的观测帧编号，pi,j和ni,j分别表示特征qi,j的位置和方向；
在特征集Ωdd中进一步筛选出满足点面距离约束即的特征，构成的候选关联特征集Ωc，其中(p,n)∈Ωdd，d为第二预设阈值；
在候选关联特征集Ωc中寻找使得最小的特征，记为/>其中(p′,n′)∈Ωc，/>为第二特征集P2中的特征/>在第二环境特征集Q2中的最佳关联特征。
3.根据权利要求2所述的基于双窗口的激光里程计方法，其特征在于，基于第二特征集的特征及其在第二环境特征集中的最佳关联特征，构建位姿优化函数，其方法为：
其中，Rwl和twl分别是Twl的旋转矩阵和平移向量，Twl是激光雷达传感器当前帧Ll的坐标系到世界坐标系W的位姿，是传感器当前帧Ll的坐标系到世界坐标系W的优化后位姿，pa是第二特征集P2中的特征/>在第一特征集P1中对应特征的位置，/>和/>分别是特征/>在第二环境特征集Q2中的最佳关联特征的位置和方向，/>是矩阵转置算子，‖·‖表示向量欧式范数，/>为位姿所在的流形空间，|P2|为第二特征集P2中的特征数量，a表示第二特征集P2中的特征编号。
4.根据权利要求3所述的基于双窗口的激光里程计方法，其特征在于，根据第二特征集的特征及其在第二环境特征集中的最佳关联特征，利用第一特征集更新第一环境特征集，其方法为：
如果当前帧是新的关键帧，记为则根据第二特征集P2中的特征/>及其在第二环境特征集Q2中的最佳关联特征/>将与/>对应的第一特征集P1中的特征(pa,na)加入与/>对应的第一环境特征集Q1中的特征/>所在的第i*组，记为/>其中/>
如果第二特征集P2中的特征在第二环境特征集Q2中没有找到最佳关联特征，则直接用/>对应的第一特征集P1中的特征(pa,na)，在第一环境特征集Q1中创建新的观测特征/> 其中全部赋值为Null，然后将N1赋值为N1+1；对于第二环境特征集Q2中未选为最佳关联的特征/>找到其对应的第一环境特征集Q1中的特征(pi,j,ni,j)所在的第i组，在第一环境特征集Q1中创建新的观测特征(pi,N+1,ni,N+1)并将它赋值为Null；
如果(N+1)≤Nlarge，将N赋值为N+1；否则删除第一环境特征集Q1中所有特征在外部大窗口中最旧关键帧中的观测特征(pi,1,ni,1)，然后将其余观测特征的观测帧编号减1，即将(pi,j,ni,j)(j＝2,3,…,N+1)的观测帧编号j全部减1，并删除关键帧/>同时将关键帧的编号j也均减1，N为外部大窗口中的关键帧的数量，Nlarge为第一预设数量，即外部大窗口中关键帧的最大数量。
5.根据权利要求4所述的基于双窗口的激光里程计方法，其特征在于，基于第一环境特征集，构建内部小窗口中的关键帧的多帧位姿优化函数，其方法为：
其中，是关键帧/>的坐标系到世界坐标系W的优化后位姿，N1是第一环境特征集Q1中特征的数量，si是第一环境特征集Q1中的第i个特征在之前N个关键帧中的最旧观测帧编号，即/>是第一环境特征集Q1中的第i个特征在外部大窗口的第j个关键帧中的观测特征qi,j＝(pi,j,ni,j)在世界坐标系W中的对应特征的位置，即/> 和/>分别是关键帧/>的坐标系到世界坐标系W的位姿/>的旋转矩阵和平移向量，/>和/>分别是第一环境特征集Q1中的在世界坐标系W中的对应特征的位置和方向，即和/>分别是关键帧/>的坐标系到世界坐标系W的位姿/>的旋转矩阵和平移向量，(·)T是矩阵转置算子，‖·‖表示向量欧式范数，/>为位姿所在的流形空间。
6.根据权利要求5所述的基于双窗口的激光里程计方法，其特征在于，利用LM算法求解多帧位姿优化函数，得到内部小窗口中的所有关键帧的坐标系到世界坐标系的优化后位姿，其方法为：
对于多帧位姿优化函数，用代替/>即用代替/>用/>代替/>N-M+1≤j≤N，从而位姿优化问题转化为欧式空间/>中的等价问题，直接应用LM算法求解得到/>进而获得M个关键帧的坐标系到世界坐标系W的优化后位姿/>N-M+1≤j≤N。
7.一种基于双窗口的激光里程计系统，其特征在于，该系统包括：获取模块、特征提取模块、首帧处理模块、特征转换模块、特征关联模块、位姿优化模块、特征更新模块、位姿平滑模块；
所述获取模块，配置为服务机器人通过激光雷达传感器感知环境，获取周围环境在激光雷达传感器当前帧坐标系下的原始三维点云数据；
所述特征提取模块，配置为从原始三维点云数据中提取特征，并构建特征集，作为第一特征集；
所述首帧处理模块，配置为若激光雷达传感器当前帧是第1帧，则将其作为关键帧，并利用第一特征集构建第一环境特征集，构建后，跳转至获取模块，否则，跳转至特征转换模块；所述关键帧为从激光雷达传感器获取的所有帧中按设定的位姿间距阈值筛选的帧；
所述特征转换模块，配置为根据激光雷达传感器当前帧及其外部大窗口中的关键帧对应的坐标系与世界坐标系的坐标转换关系，将第一特征集、第一环境特征集转换到世界坐标系中，分别得到第二特征集、第二环境特征集；
所述特征关联模块，配置为基于预设的特征关联规则，计算第二特征集中的特征在第二环境特征集中的候选关联特征，进而确定出最佳关联特征；
所述位姿优化模块，配置为基于第二特征集的特征及其在第二环境特征集中的最佳关联特征，构建位姿优化函数；利用LM算法求解位姿优化函数，得到激光雷达传感器当前帧坐标系到世界坐标系的优化后位姿，并基于优化后的位姿更新激光雷达传感器当前帧坐标系到世界坐标系的位姿；
所述特征更新模块，配置为循环执行特征转换模块-位姿优化模块K次，循环完成后，计算更新后的激光雷达传感器当前帧坐标系到世界坐标系的位姿、最近关键帧坐标系到世界坐标系的位姿之间的间距，若该间距大于等于设定的位姿间距阈值，则将当前帧作为新的关键帧，并根据第二特征集的特征及其在第二环境特征集中的最佳关联特征，利用第一特征集更新第一环境特征集，跳转至位姿平滑模块，否则，跳转至获取模块；其中，K为预设的循环次数；
所述位姿平滑模块，配置为基于第一环境特征集，构建内部小窗口中的关键帧的多帧位姿优化函数；利用LM算法求解多帧位姿优化函数，得到内部小窗口中的所有关键帧的坐标系到世界坐标系的优化后位姿，进而更新内部小窗口中的所有关键帧的坐标系到世界坐标系的位姿，更新后，跳转至获取模块。
8.一种存储装置，其中存储有多条程序，其特征在于，所述程序适用于由处理器加载并执行以实现权利要求1-6任一项所述的基于双窗口的激光里程计方法。
9.一种处理装置，包括处理器、存储装置；处理器，适用于执行各条程序；存储装置，适用于存储多条程序；其特征在于，所述程序适用于由处理器加载并执行以实现权利要求1-6任一项所述的基于双窗口的激光里程计方法。
CN202111498627.0A 2021-12-09 2021-12-09 基于双窗口的激光里程计方法、系统、装置 Active CN114170280B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111498627.0A CN114170280B (zh) 2021-12-09 2021-12-09 基于双窗口的激光里程计方法、系统、装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111498627.0A CN114170280B (zh) 2021-12-09 2021-12-09 基于双窗口的激光里程计方法、系统、装置

Publications (2)

Publication Number Publication Date
CN114170280A CN114170280A (zh) 2022-03-11
CN114170280B true CN114170280B (zh) 2023-11-28

Family

ID=80484890

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111498627.0A Active CN114170280B (zh) 2021-12-09 2021-12-09 基于双窗口的激光里程计方法、系统、装置

Country Status (1)

Country Link
CN (1) CN114170280B (zh)

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US9870624B1 (en) * 2017-01-13 2018-01-16 Otsaw Digital Pte. Ltd. Three-dimensional mapping of an environment
CN107709928A (zh) * 2015-04-10 2018-02-16 欧洲原子能共同体由欧洲委员会代表 用于实时建图与定位的方法和装置
CN111983639A (zh) * 2020-08-25 2020-11-24 浙江光珀智能科技有限公司 一种基于Multi-Camera/Lidar/IMU的多传感器SLAM方法
CN112258600A (zh) * 2020-10-19 2021-01-22 浙江大学 一种基于视觉与激光雷达的同时定位与地图构建方法
CN112305554A (zh) * 2020-11-23 2021-02-02 中国科学院自动化研究所 基于有向几何点和稀疏帧的激光里程计方法、系统、装置
CN112862894A (zh) * 2021-04-12 2021-05-28 中国科学技术大学 一种机器人三维点云地图构建与扩充方法
CN113066105A (zh) * 2021-04-02 2021-07-02 北京理工大学 激光雷达和惯性测量单元融合的定位与建图方法及系统
CN113658337A (zh) * 2021-08-24 2021-11-16 哈尔滨工业大学 一种基于车辙线的多模态里程计方法

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107709928A (zh) * 2015-04-10 2018-02-16 欧洲原子能共同体由欧洲委员会代表 用于实时建图与定位的方法和装置
US9870624B1 (en) * 2017-01-13 2018-01-16 Otsaw Digital Pte. Ltd. Three-dimensional mapping of an environment
CN111983639A (zh) * 2020-08-25 2020-11-24 浙江光珀智能科技有限公司 一种基于Multi-Camera/Lidar/IMU的多传感器SLAM方法
CN112258600A (zh) * 2020-10-19 2021-01-22 浙江大学 一种基于视觉与激光雷达的同时定位与地图构建方法
CN112305554A (zh) * 2020-11-23 2021-02-02 中国科学院自动化研究所 基于有向几何点和稀疏帧的激光里程计方法、系统、装置
CN113066105A (zh) * 2021-04-02 2021-07-02 北京理工大学 激光雷达和惯性测量单元融合的定位与建图方法及系统
CN112862894A (zh) * 2021-04-12 2021-05-28 中国科学技术大学 一种机器人三维点云地图构建与扩充方法
CN113658337A (zh) * 2021-08-24 2021-11-16 哈尔滨工业大学 一种基于车辙线的多模态里程计方法

Non-Patent Citations (5)

* Cited by examiner, † Cited by third party
Title
Robust Odometry and Mapping for Multi-LiDAR Systems with Online Extrinsic Calibration;Jianhao Jiao 等;《arXiv》;1-20 *
Visual-Gyroscope-Wheel Odometry With Ground Plane Constraint for Indoor Robots in Dynamic Environment;Ming Ouyang 等;《IEEE Sensors Letters》;第5卷(第3期);1-4 *
一种基于ST-EKF的惯性／里程计滑动窗口滤波技术;杜学禹 等;《导航与控制》;第20卷(第3期);26-33 *
可变高度激光里程计在室内不平整地面环境下的地图构建;刘子明 等;《宁波大学学报（理工版）》;第33卷(第4期);17-22 *
激光雷达惯导耦合的里程计与建图方法;庞帆 等;《计算机应用研究》;第38卷(第7期);2188-2193, 2199 *

Also Published As

Publication number Publication date
CN114170280A (zh) 2022-03-11

Similar Documents

Publication Publication Date Title
JP7068989B2 (ja) グリッドマップを作成する方法及び装置
CN111536964B (zh) 机器人定位方法及装置、存储介质
US10593110B2 (en) Method and device for computing a path in a game scene
CN110930495A (zh) 基于多无人机协作的icp点云地图融合方法、系统、装置及存储介质
CN111402339B (zh) 一种实时定位方法、装置、系统及存储介质
CN112305554B (zh) 基于有向几何点和稀疏帧的激光里程计方法、系统、装置
CN113223173B (zh) 基于图模型的三维模型重建迁移方法及系统
Alamri et al. A connectivity index for moving objects in an indoor cellular space
EP3559824B1 (en) Methods, systems and apparatus to improve spatial-temporal data management
CN113516682B (zh) 一种激光slam的回环检测方法
CN114170280B (zh) 基于双窗口的激光里程计方法、系统、装置
CN114061586A (zh) 用于生成电子设备的导航路径的方法和产品
CN117213470A (zh) 一种多机碎片地图聚合更新方法及系统
Ye et al. Ec-sfm: Efficient covisibility-based structure-from-motion for both sequential and unordered images
Choi et al. Topological map building based on thinning and its application to localization
CN112129282A (zh) 一种不同导航方式之间定位结果的转换方法、转换装置
CN109561384B (zh) 一种复合噪声条件下的无线传感器网络节点定位方法
JP5991052B2 (ja) 更新装置、更新方法、および更新プログラム
CN115239902A (zh) 移动设备的周边地图建立方法、装置、设备及存储介质
CN115328163A (zh) 一种巡检机器人雷达里程计的速度与精度优化方法
CN101939666B (zh) 利用传感器数据来对对象的运动进行计算机辅助计算的方法
CN111813102A (zh) 一种分布式自主机器人环境地图构建方法
CN117253209B (zh) 自动驾驶点云检测方法、装置、通信设备及存储介质
CN116185044B (zh) 一种机器人集群系统的控制方法、装置、设备及系统
CN116698017B (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