[发明专利]一种激光建图定位方法、装置、存储介质及电子设备

说明书

本发明公开了一种激光建图定位方法、装置、存储介质及电子设备，包括采用占据栅格地图来进行环境表示；运动跟踪线程中相邻两帧激光雷达进行扫描匹配，计算优化的帧间位姿运动矩阵；当前帧激光雷达数据与占据栅格地图匹配，调整帧间位姿运动矩阵。建图线程中由新插入的关键帧和关键帧队列构建共视帧约束，检测到闭环时，构建闭环帧约束，采用增量式优化位姿因子图更新关键帧队列位姿，实现全局地图的更新。本发明通过两次匹配，确保机器人运动过程中局部建图准确可靠；对于原始位姿图非线性最小二乘优化问题的求解，将图式结构转换为树状结构，并将非线性优化问题的求解转换为最新帧相关的共视帧的增量式求解，大大提高计算效率，减小资源的耗费。

技术领域

本发明涉及定位和建图技术领域，具体涉及一种激光建图定位方法、装置、存储介质及电子设备。

背景技术

激光SLAM(simultaneous localization and mapping，即时定位与地图构建)一般是指装备平面单线激光雷达的轮式移动机器人，通过运动来使用激光雷达扫描周围室内环境进行建图，同时在此地图中估算机器人位姿的技术。所使用的传感器除了激光雷达，还可以使用轮速里程计，惯导陀螺仪等。现有方案一般通过激光扫描匹配来计算帧间位姿变换，再通过粒子滤波进行状态更新迭代，或通过位姿图优化来消除大范围建图过程中的积累误差。

粒子滤波方法采用一系列的粒子来表示机器人的位姿和地图信息，通过里程计信息作为运动先验和扫描匹配激光到地图上的匹配度来推算概率分布函数(proposaldistribution)。同时对粒子根据激光匹配度来进行权重系数的评估，再根据这些评估权重系统的大小，来对于粒子进行筛选，确保雷达数据与地图匹配好的粒子能够在多次迭代筛选中大概率的存活下来，从而完成建图。该方法不足之处在于无法消除建图过程中的累计误差，且通过激光扫描匹配强依赖于里程计信息。一旦里程计信息失效，如出现轮子打滑，空转，胎压不稳，或者机器人被意外推动，挪移，就会出现建图花掉的情形。

位姿图优化方法在局部通过扫描匹配来计算帧间位姿，使用的是相关度匹配和基于高斯牛顿的最小化地图灰度差的方法；全局通过优化整个位姿图来优化整体轨迹，闭环检测使用分支定界加速的相关度匹配和基于高斯牛顿的最小化地图灰度差的方法来构造位姿图的闭环运动约束。该方案可以单独直接使用雷达数据(主要对于雷达数据源质量较高或机器人运动速度较低情形)，也可以使用轮速里程计及/或惯导来提供运动先验，同时使用雷达数据。不足之处在于一方面帧间匹配依然存在错误估算的情况，对于边建图边导航的探索模式中容易导致运动规划控制无法正常进行；另一方面全局位姿图优化过程中所使用的加速分支定界来根据相关度匹配寻找相似子图并计算帧间运动而构建运动约束的位姿图优化方法，对于计算资源的耗费是非常显著的。就算使用多线程计算的方式，计算负载也很难降下来。

发明内容

有鉴于此，本发明实施例提供了一种激光建图定位方法、装置、存储介质及电子设备，以解决现有技术在激光建图定位过程中机器人的运动跟踪过程抗干扰能力不够强问题，以及位姿图优化过程存在非线性优化问题不够高效耗时过多的问题。

本发明提出的技术方案如下：

本发明实施例第一方面提供一种激光建图定位方法，该方法包括：根据获取的第一帧激光雷达数据构建局部的占据栅格地图；根据运动跟踪线程中当前帧激光雷达数据与上一帧激光雷达数据进行匹配，计算得到优化的帧间位姿运动矩阵；根据当前帧激光雷达数据与占据栅格地图进行匹配，对所述优化的帧间位姿运动矩阵进行调整，得到精确的当前帧位姿；根据当前帧和当前关键帧之间的距离更新局部地图或重新构建局部地图；根据新插入的关键帧构建的运动约束，增量式优化更新关键帧队列中各个关键帧的位姿，得到更新后的全局地图。