[发明专利]基于双窗口的激光里程计方法、系统、装置

说明书

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

技术领域

本发明涉及服务机器人技术领域，具体涉及一种基于双窗口的激光里程计方法、系统、装置。

背景技术

随着机器人技术的发展，服务机器人已经进入人们生产生活的各个方面。为了完成任务，机器人需要首先确定自身在环境中的位置。机器人定位受到国内外研究人员的普遍关注。通过激光雷达传感器实现机器人定位的方法称为激光里程计。激光里程计的常用方法是通过匹配当前帧和地图中的点云数据来优化当前帧位姿，称之为帧图匹配，其中地图一般通过之前的多帧点云数据创建，代表方法包括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算法求解位姿优化函数，得到激光雷达传感器当前帧坐标系到世界坐标系的优化后位姿，并基于优化后的位姿更新激光雷达传感器当前帧坐标系到世界坐标系的位姿；