[发明专利]一种基于点云地图的视觉定位方法

说明书

本发明提供了一种基于点云地图的视觉定位方法，包括点云地图生成模块、视觉惯性里程计构建模块，以及基于点云地图的视觉匹配定位模块。其中点云地图模块通过融合激光，IMU以及GPS信息建立高精度的点云地图；视觉惯性里程计构建模块先提取视觉特征点，使用光流法追踪前后帧特征点，并对IMU进行预积分，和视觉特征点进行融合，构建视觉惯性里程计，输出每一帧的初始位姿，并恢复特征点的深度；基于已有地图的视觉匹配定位模块根据初始位置从点云地图中提取子图，通过视觉恢复的特征点投影到3D空间地图坐标系下，并在当前子图中查询最近点。对于视觉和地图匹配到的最近点，采用基于对偶四元数的RANSAC算法优化当前帧的位姿。

技术领域

本发明涉及机器人定位领域，特别涉及一种基于点云地图的视觉定位方法。

背景技术

随着智能芯片、5G通信、人工智能技术的快速发展，智能机器人已经广泛应用于各种环境，如工业领域搬运机器人、商业领域送餐机器人、家用扫地机器人。定位是机器人技术的核心，实现机器人自身的准确定位是一项最基本、最重要的功能。定位模块是机器人完成后续感知、规划、抓取等任务的基础。

机器人定位是指通过自身携带的传感器如相机、激光雷达、IMU、轮速计、毫米波雷达等，计算当前在环境中的位置。其中轮速计、IMU等传感器由于器件本身的限制，无法单独进行准确的定位。目前主流的定位方案有两种，基于激光的方案以及基于视觉的方案。基于激光的定位方案较为成熟，精度普遍比视觉高，但是激光的价格昂贵，而且激光在垂直方向的线束有限，获取的信息较少，影响定位精度；基于视觉的定位方案稳定性不如基于激光的方案，在特征纹理较少以及场景变化较大的动态环境鲁棒性较差。

根据上述介绍，当前机器人定位领域仍存在以下问题：1)基于视觉的方案对场景变化敏感，定位精度差，且仅依靠单目相机无法求解尺度。2)基于激光的方案成本较高，难以落地。将视觉和激光融合虽然提高了精度，但是在成本和系统资源占用上存在劣势。本文针对以上问题，提出了基于点云地图的视觉惯性定位，在已知点云地图时，仅使用相机和IMU，提高了定位的精度，降低了定位成本。

发明内容

发明目的：本发明要解决的问题为针对当前定位方案精度不足，成本较高等问题。提出了基于已知地图的视觉惯性定位方法。融合激光、IMU和GPS(全球定位系统， GlobalPositioning System)建立高精度地图，并将地图和视觉惯性里程计恢复的特征点进行匹配，优化机器人位姿，降低硬件成本的同时提高了定位精度，使得系统的鲁棒性得以保证。

为了解决上述技术问题，本发明公开了一种基于点云地图的视觉惯性定位方法，首先建立点云地图构建模块、视觉惯性里程计构建模块和基于点云地图的定位模块。其中点云地图构建模块提供定位所需地图，视觉惯性里程计构建模块为定位模块提供初始位姿。

本发明方法具体包括：建立点云地图构建模块、视觉惯性里程计构建模块和视觉匹配定位模块；

所述点云地图构建模块用于，将惯性测量单元IMU、激光和GPS(全球定位系统) 进行融合，实现多传感器融合激光建图；

所述视觉惯性里程计构建模块基于点云地图的视觉定位提供初始位姿，通过融合单目视觉和惯性测量单元IMU，求解每一帧图像的位姿和特征点深度；

所述视觉匹配定位模块基于不同模态的点云和视觉信息，对地图点构建kd-tree(k 维树，k为数据的维度)，与投影到地图坐标系的特征点匹配后采用基于对偶四元数的粒子滤波器进行定位。

所述点云地图构建模块具体执行如下步骤：

步骤1-1，以激光为基准，将惯性测量单元IMU和GPS数据相对于激光进行对齐，对于每一帧激光点云，使用100Hz的惯性测量单元IMU进行去畸变处理；

步骤1-2，对惯性测量单元IMU进行惯性解算，解算结果作为卡尔曼滤波的预测更新；