[发明专利]基于单目相机的VIO快速联合初始化方法

说明书

本发明公开了一种基于单目相机的VIO快速联合初始化方法，包括如下步骤：(1)用ORB‑SLAM处理几秒钟的视频以获得初始的位姿和几个关键帧；(2)通过步骤(1)中的关键帧与IMU预积分构建代价函数计算陀螺仪偏差；(3)通过解算连续帧间的线性模型求取重力加速度计偏置、重力加速度、重力加速度校准与重力加速度计偏置；(4)通过步骤(3)中的重力加速度校准与参数分离求出尺度与速度信息。本发明的初始化方法具有更快的初始化速度，对定位精度影响很小，本发明提出的方法可以将初始化时间控制在10秒以内；提高联合初始化的速度对于提高实时性具有重要意义。

技术领域

本发明涉及机器人视觉定位导航技术领域，尤其是一种基于单目相机的VIO快速联合初始化方法。

背景技术

智能移动终端的状态估计已经成为计算机视觉和机器人领域的热门话题，因为它可应用于微型飞行器、自动驾驶汽车、虚拟现实和增强现实等新兴技术。传统的智能移动终端定位分为室外定位和室内定位。室外定位包括卫星定位和基站定位，室内定位主要依靠多传感器融合。在不同的传感器系统中，视觉惯性系统由于其体积小、成本低、潜力大而受到了广泛的关注。视觉惯性系统有两种估计位姿的主要方法：非线性优化方法和递归滤波方法。单目视觉系统可以提供足够的环境信息来完成3D建图和定位，但不能恢复尺度信息，因此限制了其在现实世界中在机器人方面的应用。由于IMU传感器提供自身运动信息，并且可以估计重力方向而且可观测绝对俯仰角，这使得单目视觉惯性系统可以恢复尺度信息。

初始状态估计是视觉惯性里程计的一个重要步骤，大多数现有的单目VINS方法在没有良好初始状态估计的情况下是无法进行精确定位的，并且如果运动估计由于缺乏初始条件而失败，它们也不能对运动进行恢复。已有VIO给出的初始化方法为相机姿态、尺度、速度、重力、陀螺仪和加速度计偏差提供了最佳解决方案，但初始化收敛需要10秒以上的时间。港科大提出的VINS介绍的另一种初始化方法提供了一个鲁棒的初始化过程，能够从未知的初始状态引导系统，但是需要大量计算资源来计算重力方向。

发明内容

本发明所要解决的技术问题在于，提供一种基于单目相机的VIO快速联合初始化方法，具有更快的初始化速度，对定位精度影响很小，能够将初始化时间控制在10秒以内。

为解决上述技术问题，本发明提供一种基于单目相机的VIO快速联合初始化方法，包括如下步骤：

(1)用ORB-SLAM处理几秒钟的视频以获得初始的位姿和几个关键帧；

(2)通过步骤(1)中的关键帧与IMU预积分构建代价函数计算陀螺仪偏差；

(3)通过解算连续帧间的线性模型求取重力加速度计偏置、重力加速度、重力加速度校准与重力加速度计偏置；

(4)通过步骤(3)中的重力加速度校准与参数分离求出尺度与速度信息。

优选的，步骤(3)中，重力加速度计偏置估计具体为：两个连续关键帧之间的运动可以用两帧间所测量的预积分ΔR,Δv,Δp来描述，由预积分方程：

i和i+1分别是帧数，和分别是IMU的第i+1帧在世界坐标系下的旋转、速度和平移，ΔR_i,i+1、Δv_i,i+1和Δp_i,i+1分别是IMU的第i和i+1帧间的世界坐标系下的旋转、速度和平移增量，Δt_i,i+1是时间差，和分别是平移相对于角速度和加速度的雅各比矩阵，和分别是速度相对于角速度和加速度的雅各比矩阵，是旋转相对于角速度的雅各比矩阵，和表示第i帧的加速度计和陀螺仪偏置，g_w是世界坐标系中的重力加速度矢量，w是世界表系，b是IMU坐标系，b_i和b_i+1分别是第i和i+1帧的IMU坐标系；在世界参考坐标系中定义IMU测量得到的旋转平移以及速度