基于双窗口的激光里程计方法、系统、装置制造方法及图纸

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

【技术实现步骤摘要】
基于双窗口的激光里程计方法、系统、装置


[0001]本专利技术涉及服务机器人
，具体涉及一种基于双窗口的激光里程计方法、系统、装置。

技术介绍

[0001]随着机器人技术的发展，服务机器人已经进入人们生产生活的各个方面。为了完成任务，机器人需要首先确定自身在环境中的位置。机器人定位受到国内外研究人员的普遍关注。通过激光雷达传感器实现机器人定位的方法称为激光里程计。激光里程计的常用方法是通过匹配当前帧和地图中的点云数据来优化当前帧位姿，称之为帧图匹配，其中地图一般通过之前的多帧点云数据创建，代表方法包括LOAM(LiDAR Odometry and Mapping)、LeGO
‑
LOAM(Lightweight and Ground
‑
Optimized Lidar Odometry and Mapping)以及基于有向几何点DGP(Directed Geometry Point)的方法等。上述方法仅优化当前帧位姿，这使得长期的累积误差较大。为了降低累积误差，同时优化多帧位姿的平滑方法被采用。在位姿估计过程中一般需要用到非线性优化算法，代表算法包括GM(Gauss
‑
Newton)、LM(Levenberg
‑
Marquardt)算法。另外，对于激光雷达传感器获取的三维点云数据，研究人员普遍利用PCL点云库进行处理(包括KD
‑
tree算法等)。
[0002]近年来，研究人员开始关注帧图匹配与平滑相结合的激光里程计方法，以有效提高里程计的精度，代表方法有BALM(Bundle Adjustment for LiDAR Mapping)等。但是，现有方案大都采用稠密的特征点或者在全局地图上进行帧图匹配，这降低了激光里程计的效率。为此，需要对现有帧图匹配与平滑相结合的激光里程计方法进行更深入的研究，以解决现有技术效率较低的问题。

技术实现思路

[0003]为了解决现有技术中的上述问题，即为了解决现有帧图匹配与平滑相结合的激光里程计方法效率较低的问题，本专利技术提出一种基于双窗口的激光里程计方法，包括以下步骤：
[0004]步骤S100，服务机器人通过激光雷达传感器感知环境，获取周围环境在激光雷达传感器当前帧坐标系下的原始三维点云数据；
[0005]步骤S200，从原始三维点云数据中提取特征，并构建特征集，作为第一特征集；
[0006]步骤S300，若激光雷达传感器当前帧是第1帧，则将其作为关键帧，并利用第一特征集构建第一环境特征集，构建后，跳转至步骤S100，否则，跳转至步骤S400；所述关键帧为从激光雷达传感器获取的所有帧中按设定的位姿间距阈值筛选的帧；
[0007]步骤S400，根据激光雷达传感器当前帧及其外部大窗口中的关键帧对应的坐标系与世界坐标系的坐标转换关系，将第一特征集、第一环境特征集转换到世界坐标系中，分别得到第二特征集、第二环境特征集；
[0008]步骤S500，基于预设的特征关联规则，计算第二特征集中的特征在第二环境特征
集中的候选关联特征，进而确定出最佳关联特征；
[0009]步骤S600，基于第二特征集的特征及其在第二环境特征集中的最佳关联特征，构建位姿优化函数；利用LM算法求解位姿优化函数，得到激光雷达传感器当前帧坐标系到世界坐标系的优化后位姿，并基于优化后的位姿更新激光雷达传感器当前帧坐标系到世界坐标系的位姿；
[0010]步骤S700，循环执行步骤S400
‑
步骤S600 K次，循环完成后，计算更新后的激光雷达传感器当前帧坐标系到世界坐标系的位姿、最近关键帧坐标系到世界坐标系的位姿之间的间距，若该间距大于等于设定的位姿间距阈值，则将当前帧作为新的关键帧，并根据第二特征集的特征及其在第二环境特征集中的最佳关联特征，利用第一特征集更新第一环境特征集，跳转至步骤S800，否则，跳转至步骤S100；其中，K为预设的循环次数；
[0011]步骤S800，基于第一环境特征集，构建内部小窗口中的关键帧的多帧位姿优化函数；利用LM算法求解多帧位姿优化函数，得到内部小窗口中的所有关键帧的坐标系到世界坐标系的优化后位姿，进而更新内部小窗口中的所有关键帧的坐标系到世界坐标系的位姿，更新后，跳转至步骤S100。
[0012]在一些优选的实施方式中，基于预设的特征关联规则，计算第二特征集中的特征在第二环境特征集中的候选关联特征，进而确定出最佳关联特征，其方法为：
[0013]对于第二特征集P2中的特征首先利用PCL点云库中的KD
‑
tree算法在第二环境特征集Q2中寻找满足点点距离约束即的特征，构成特征集Ω
dd
，其中‖
·
‖是向量欧式范数，r是第一预设阈值；
[0014]在特征集Ω
dd
中进一步筛选出满足点面距离约束即的特征，构成的候选关联特征集Ω
c
，其中(p，n)∈Ω
dd
，d为第二预设阈值；
[0015]在候选关联特征集Ω
c
中寻找使得最小的特征，记为其中(p
′
，n
′
)∈Ω
c
，称为第二特征集P2中的特征在第二环境特征集Q2中的最佳关联特征。
[0016]在一些优选的实施方式中，基于第二特征集的特征及其在第二环境特征集中的最佳关联特征，构建位姿优化函数，其方法为：
[0017][0018]其中，R
wl
和t
wl
分别是T
wl
的旋转矩阵和平移向量，T
wl
是激光雷达传感器当前帧L
l
的坐标系到世界坐标系W的位姿，是传感器当前帧L
l
的坐标系到世界坐标系W的优化后位姿，p
a
是第二特征集P2中的特征在第一特征集P1中对应特征的位置，和分别是特征在第二环境特征集Q2中的最佳关联特征的位置和方向，是矩阵转置算子，‖
·
‖表示向量欧式范数，为位姿所在的流形空间，|P2|为第二特征集P2中的特征数量，a表示第二特征集P2中的特征编号。
[0019]在一些优选的实施方式中，根据第二特征集的特征及其在第二环境特征集中的最佳关联特征，利用第一特征集更新第一环境特征集，其方法为：
[0020]如果当前帧是新的关键帧，记为则根据第二特征集P2中的特征及其在第二环境特征集Q2中的最佳关联特征将与对应的第一特征集P1中的特征(p
a
，n
a
)加入与对应的第一环境特征集Q1中的特征所在的第i*组，记为其中
[0021]如果第二特征集P2中的特征在第二环境特征集Q2中没有找到最佳关联特征，则直接用对应的第一特征集P1中的特征(p
a
，n
a
)，在第一环境特征集Q1中创建新的观测特征的观测特征其中其中其中全部赋值本文档来自技高网...

【技术保护点】

【技术特征摘要】
中的最佳关联特征。3.根据权利要求2所述的基于双窗口的激光里程计方法，其特征在于，基于第二特征集的特征及其在第二环境特征集中的最佳关联特征，构建位姿优化函数，其方法为：其中，R
wl
和t
wl
分别是T
wl
的旋转矩阵和平移向量，T
wl
是激光雷达传感器当前帧L
l
的坐标系到世界坐标系W的位姿，是传感器当前帧L
l
的坐标系到世界坐标系W的优化后位姿，p
a
是第二特征集P2中的特征在第一特征集P1中对应特征的位置，和分别是特征在第二环境特征集Q2中的最佳关联特征的位置和方向，是矩阵转置算子，‖
·
‖表示向量欧式范数，为位姿所在的流形空间，|P2|为第二特征集P2中的特征数量，a表示第二特征集P2中的特征编号。4.根据权利要求3所述的基于双窗口的激光里程计方法，其特征在于，根据第二特征集的特征及其在第二环境特征集中的最佳关联特征，利用第一特征集更新第一环境特征集，其方法为：如果当前帧是新的关键帧，记为则根据第二特征集P2中的特征及其在第二环境特征集Q2中的最佳关联特征将与对应的第一特征集P1中的特征(p
a
，n
a
)加入与对应的第一环境特征集Q1中的特征所在的第i
*
组，记为其中如果第二特征集P2中的特征在第二环境特征集Q2中没有找到最佳关联特征，则直接用对应的第一特征集P1中的特征(p
a
，n
a
)，在第一环境特征集Q1中创建新的观测特征观测特征其中全部赋值为Null，然后将N1赋值为N1+1；对于第二环境特征集Q2中未选为最佳关联的特征找到其对应的第一环境特征集Q1中的特征(p
i，j
，n
i，j
)所在的第i组，在第一环境特征集Q1中创建新的观测特征(p
i，N+1
，n
i，N+1
)并将它赋值为Null；如果(N+1)≤N
large
，将N赋值为N+1；否则删除第一环境特征集Q1中所有特征在外部大窗口中最旧关键帧中的观测特征(p
i，1
，n
i，1
)，然后将其余观测特征的观测帧编号减1，即将(p
i，j
，n
i，j
)(j＝2，3，...，N+1)的观测帧编号j全部减1，并删除关键帧同时将关键帧的编号j也均减1，N为外部大窗口中的关键帧的数量，N
large
为第一预设数量，即外部大窗口中关键帧的最大数量。
5.根据权利要求1所述的基于双窗口的激光里程计方法，其特征在于，基于第一环境特征集，构建内部小窗口中的关键帧的多帧位姿优化函数，其方法为：其中，其中，是关键帧的坐标系到世界坐标系W的优化后位姿，N1是第一环境特征集Q1中特征的数量，s
i
是第一环境特征集Q1中的第i个特征在之前N个关键帧中的最旧观测帧编号，即关键帧中的最旧观测帧编号，即是第一环境特征集Q1中的第i个特征在外部大窗口的第j个关键帧中的观测特征q
i，j
＝(p
i，j
，n
i，j
)在世界坐标系W中的对应特征的位置，即对应特征的位...

【专利技术属性】
技术研发人员：李忠辉，曹志强，谭民，梁爽，亢晋立，
申请(专利权)人：中国科学院自动化研究所，
类型：发明
国别省市：