[发明专利]视觉惯性里程计的实现方法及系统

摘要

本发明实施例公开了一种视觉惯性里程计的实现方法，包括以下步骤：第一步，通过相机实时采集图像数据；通过IMU采集陀螺仪和加速度的数据；第二步，场景初始化模块建立视觉惯性里程计系统的初始空间三维地图；第三步，地图扩展模块对空间三维地图进行实时更新；第四步，姿态估计模块接收计算出每一帧图像对应的设备的位置和姿态。本发明通过场景初始化模块实现鲁棒易用的系统初始化过程，通过姿态估计模块实现视觉信息与IMU信息的联合约束进行位置和姿态优化，通过重定位模块实现失败处理和重定位算法，能够得到设备实时的位置和姿态信息。本发明实施例还公开了一种视觉惯性里程计系统。

主权项

1.一种视觉惯性里程计的实现方法，其特征在于，包括以下步骤：第一步，设备通过配备的相机实时采集图像数据，通过配备的IMU实时采集设备的角速度和加速度数据；所述IMU包括陀螺仪和加速度计；第二步，场景初始化模块根据相机实时采集的图像数据和IMU采集的所述角速度和加速度数据，建立视觉惯性里程计系统的初始空间三维地图；第三步，地图扩展模块在场景初始化模块建立起来的空间三维地图的基础上，根据相机实时采集的新的图像数据，对空间三维地图进行实时更新；第四步，姿态估计模块接收相机实时采集的图像数据和IMU采集的所述角速度和加速度数据，利用当前图像特征点与地图扩展模块所维护的三维地图点对应的空间约束关系、图像帧之间特征匹配约束关系和图像帧间IMU的约束信息，计算出每一帧图像对应的设备的位置和姿态，输出设备在当前环境中的位置和姿态信息；所述第二步具体包括以下步骤：在相机所采集的多帧图像上分别提取特征点，进行帧间相应的特征点匹配；根据特征点匹配结果的信息，利用从运动推断结构的方式，恢复出初始空间三维地图和每一帧对应的设备位置和姿态；利用该空间三维地图和每一帧对应的设备位置和姿态以及IMU的所述角速度和加速度数据，估算相机与IMU的相对空间位姿，完成相机坐标系与IMU坐标系的统一；所述第三步中地图扩展模块对空间三维地图进行更新的方法如下：地图扩展模块接收相机所采集的新图像，利用图像上已经提取的特征点；根据提取的特征点与地图点匹配关系不同，分别执行下述操作：当新提取的特征点与初始的空间三维地图上的所有三维地图点均不匹配时，利用三角化方法，从该特征点两个或者多个相机之间的观测计算出该特征点对应的三维空间位置，从而相应在空间三维地图上增加新的地图点；或者当新提取的特征点的位置确定性高于初始的空间三维地图上的地图点的位置确定性，根据对应的二维图像特征点和图像对应设备位置和姿态，更新该地图点的三维空间坐标，从而更新空间三维地图上单个地图点的位置；或者当新提取的或者已有的特征点的三维位置与二维投影关系误差较大，从空间三维地图上删除这些特征点；通过集群调整的方式，同时优化初始的空间三维地图上多个特征点的三维空间位置，使得多个地图点对应相机观测的整体观测误差最小，更新空间三维地图上所有地图点的位置。