[发明专利]一种动态场景下的激光单目视觉融合定位建图方法

说明书

本发明公开了一种动态场景下的激光单目视觉融合定位建图方法，该方法包括：步骤1，检测动态障碍物信息；步骤2，获取相对位姿，并输出满足要求的关键帧信息；步骤3，将关键帧插入到共视图中，生成新地图点，并优化关键帧的位姿和地图点；步骤4，判断每一关键帧的图像数据与当前关键帧的图像数据的相似度是否达到阈值，再在本质图上将当前关键帧与回环关键帧连接，并更新本质图，最后进行全局BA，获得优化后的关键帧位姿、特征点地图和点云地图。本发明能够在动态场景中执行SLAM目的。

技术领域

本发明涉及面向自动驾驶领域的激光单目视觉融合定位建图方法，特别是关于一种动态场景下的激光单目视觉融合定位建图方法。

背景技术

随着自动化、人工智能和机器人等技术的飞速发展，实际生活和工业场景中应用了越来越多具有感知、定位和导航功能的智能无人系统，例如扫地机器人、仓储机器人及无人驾驶出租车等。随着传感器技术及智能算法的不断迭代，这些无人系统的应用场景已经从简单的已知环境，扩展到完全未知的复杂环境。在未知环境中，机器人需要用来自传感器的信息来感知周围环境以及估计自身状态，以保证其能够在复杂未知环境中自主运动。因此，机器人在未知环境中将面临三个问题：1)我在哪？2)我周围都有什么？3)我要去哪里？这三个问题分别对应为：1)机器人的自主定位问题；2)机器人的地图构建问题；3)机器人的实时避障及路径规划问题。

SLAM(英文全称为：Simultaneous Localization and Mapping，中文全称为：定位与建图)是利用机器人自身传感器感知和定位到的周围环境信息构建环境地图的技术，其为前两个问题提供了答案，从而为机器人在未知环境中自主运动提供了基础保证。SLAM可以依据采集信息使用的传感器不同，可分为激光SLAM和视觉SLAM。其中，激光SLAM存在点云数据在垂直方向上稀疏性造成的俯仰角估计不准的问题，同时激光雷达所获取的环境信息也不如视觉传感器丰富，相对而言，在回环检测的效率上不及视觉传感器。视觉SLAM由于具有相对丰富的环境信息以及垂直方向上结构化数据的密集性，可在一定程度弥补上述问题缺陷。目前，绝大多数视觉SLAM问题的研究基于假设周围环境为静态，在动态场景下的激光单目视觉融合定位建图方法仍不成熟，相关理论有待进一步完善。

发明内容

本发明的目的在于提供一种动态场景下的激光单目视觉融合定位建图方法来克服或至少减轻现有技术的上述缺陷中的至少一个。

为实现上述目的，本发明提供一种动态场景下的激光单目视觉融合定位建图方法，该方法包括：

步骤1，根据当前帧点云数据与上一帧视觉里程计预测到的位姿先验信息，以检测动态障碍物信息，并输出；

步骤2，根据步骤1输出的动态障碍物信息在相互标定外参的单目视觉2上形成的图像掩膜，在图像掩膜上提取ORB特征点及其与上一帧ORB特征点的匹配，再估计ORB特征点的深度，通过位姿解算获取相对位姿，并输出满足要求的关键帧信息；

步骤3，将步骤2输出的关键帧插入到共视图中，根据地图点共视程度更新该关键帧与其它关键帧的连接关系、以及生长树和词袋模型，生成新地图点，并根据当前关键帧的本质图找到相邻关键帧，构建非线性优化问题，优化关键帧的位姿和地图点；

步骤4，判断每一关键帧的图像数据与当前关键帧的图像数据的相似度是否达到阈值，如果是，则判定为出现回环，替换或填补当前关键帧与回环关键帧存在冲突的地图点，再在本质图上将当前关键帧与回环关键帧连接，并更新本质图，最后进行全局BA，获得优化后的关键帧位姿、特征点地图和点云地图。

进一步地，步骤1具体包括：

步骤11，将激光雷达输出的点云数据进行栅格化处理，再根据地面点云的分布特征进行地面分割操作，获取非地面点云；

步骤12，将步骤11输出的所有非地面点云聚类为多个点云簇，一个点云簇对应一个障碍物，以区分出各自独立的障碍物点云簇；