[发明专利]一种提高视觉惯导系统鲁棒性的方法、装置和机器人设备

说明书

本发明涉及视觉和惯导融合的同时定位与地图构建领域，特别是涉及一种提高视觉惯导系统鲁棒性的方法、装置和机器人设备，其中所述提供视觉惯导系统鲁棒性的方法，通过确定参考帧；对所述参考帧进行修正；根据最老帧和修正后的参考帧，对除了所述参考帧和最老帧之外的帧进行非线性优化，完成对图形在静止或匀速状态下，将新的视觉约束引入到系统中去，优化了现有的视觉约束和惯性约束机制，很好的解决了轨迹飘走的问题，显著提高了系统的鲁棒性。

技术领域

本发明实施例涉及视觉和惯导融合的同时定位与地图构建领域，特别是涉及一种提高视觉惯导系统鲁棒性的方法、装置和机器人设备。

背景技术

同时定位与地图构建系统(SLAM,Simultaneous localization and mapping,以下简称SLAM)是指搭载特定传感器的主体，如机器人、无人机、AR设备等，在没有环境先验信息的情况下，于运动过程中建立环境的模型，同时估计自己运动的过程。SLAM中的传感器可以为视觉相机、IMU、GPS、激光雷达等，本文中讨论的传感器主要为视觉相机和IMU，通常称这种基于视觉和观星测量单元IMU(Inertial Measurement Unit)融合的SLAM系统为VINS(visual-inertial system)，或VIO(visual-inertial odometry)，即视觉惯导系统。

VIO按照视觉和IMU的状态向量是否放在一起优化，可分为松耦合和紧耦合，在紧耦合VIO系统中，通过将视觉提供的重投影误差的视觉约束，和IMU提供的帧间约束，放到一个整体的目标函数中进行优化。但是，当出现快速旋转、快速移动、相机遮挡或无纹理等情况，会导致视觉约束丢失，这种情况在AR、机器人等中是经常会出现的，以手机等AR设备为例，当快速运动时，因为运动模糊等原因，会导致图像质量发生模糊，此时想在这种图像上进行视觉的特征点检测和跟踪将变得不现实。

当视觉约束丢失时，VIO系统的整体目标函数中仅有IMU约束起作用，但是又因为IMU的累积误差，将导致计算的状态向量出现错误，当视觉丢失后静止不动时，相机轨迹仍会沿某个方向加速飘走。

发明内容

鉴于上述问题，提出了本发明实施例以便提供一种克服上述问题的方法、装置、机器人设备及计算机存储介质。

为解决上述技术问题，本发明实施例采用的一个技术方案是：提供一种提高视觉惯导系统鲁棒性的方法，包括

确定参考帧；

对所述参考帧进行修正；

根据最老帧和修正后的参考帧，对除了所述参考帧和最老帧之外的帧进行非线性优化。

所述参考帧为相同帧的次新帧。

所述确定参考帧，进一步包括确定需要边缘化的帧，包括：

根据当前帧，判断最老帧是否是需要边缘化的帧；

如果是，则将最老帧确定为需要边缘化的帧；

如果不是，则进一步判断当前帧和次新帧是否相同，如果相同，则将次新帧确定为参考帧，并将最老帧确定为需要边缘化的帧。

如果当前帧和次新帧不相同，则将次新帧确定为需要边缘化的帧，并在后续不对参考帧进行修正。

所述判断当前帧和次新帧是否相同，具体为：

判断当前帧和次新帧跟踪成功的点数是否小于预先设定的阈值，如果不小于，则认为当前帧和次新帧相同；如果小于，则认为当前帧和次新帧不同。

所述修正参考帧，具体为对参考帧的速度和旋转进行修正。

所述修正参考帧，具体为：

将参考帧的速度设置为：

将参考帧的旋转设置为：