CN112284381B - 视觉惯性实时初始化对准方法及系统 - Google Patents

Abstract

本发明公开了一种视觉惯性实时初始化对准方法及系统，获取当前视觉帧，并获取至当前视觉帧的若干个连续视觉帧以及对应的惯性测量单元的测量数据；计算视觉帧位姿，并对相邻两个视觉帧之间的惯性测量单元的测量数据进行预积分得到积分数据；获得视觉帧对应的本体帧位姿，利用积分数据和本体帧位姿建立线性方程组，并对其系数矩阵和常数项进行迭代更新；然后求解，得到当前本体帧速度和初始视觉帧重力加速度；若初始视觉帧重力加速度值的模值与当地重力加速度值的绝对误差小于预设值，且n的值大于预设值，判定视觉惯性初始对准成功。本发明的线性方程组的维度较小，求解未知量计算量更少，效率高，提升了实时性。

Description

视觉惯性实时初始化对准方法及系统

技术领域

本发明涉及信息处理技术领域，特别是涉及一种视觉惯性实时i对准方法及系统。

背景技术

近年来，视觉惯性组合导航系统越来越多地应用到AR、机器人导航、自动驾驶等领域。视觉惯性初始化对准是视觉惯性组合导航必不可少的初始化步骤。

现有的视觉惯性初始化对准方法，通常综合利用多帧的视觉位姿和IMU(InertialMeasurement Unit，惯性测量单元)预积分信息，通过建立线性方程组并求解得到视觉惯性初始化对准状态向量(本体帧的速度以及初始视觉帧的重力加速度矢量等)信息。这种方法融合了多帧视觉惯性信息，可以提升精度，但是实时性较差，并且该方法除了求解当前帧的速度信息，也求解了其他所有帧的速度信息，其他所有帧的速度信息对于视觉惯性导航来说是冗余信息，是不必要计算的量，因此，增加了计算量。

发明内容

针对于上述问题，本发明提供一种视觉惯性实时初始化对准方法及系统，实现了减少计算量，提升算法实时性的目的。

为了实现上述目的，本发明提供了如下技术方案：

一种视觉惯性实时初始化对准方法，包括：

获取当前视觉帧，并获取第k个视觉帧至所述当前视觉帧的连续n+1个视觉帧以及所述第k个视觉帧至所述当前视觉帧之间的惯性测量单元的测量数据，所述当前视觉帧为第k+n个视觉帧，k为自然数，n为正整数；所述视觉帧为相机拍摄的图像帧；

基于视觉里程计方法计算视觉帧位姿，并对相邻两个视觉帧之间的惯性测量单元的测量数据进行预积分得到积分数据；

根据视觉帧位姿以及相机与惯性测量单元的外参获得视觉帧对应的本体帧位姿；所述相机与惯性测量单元的外参为事先标定得到的相机与惯性测量单元的旋转与平移关系参数；

利用所述积分数据和本体帧位姿建立线性方程组，并对所述线性方程组的系数矩阵和常数项进行迭代更新，其中，所述线性方程组的未知量为当前本体帧速度和初始视觉帧重力加速度；所述当前本体帧为当前视觉帧对应的本体帧；所述初始视觉帧为第0个视觉帧；

对迭代更新后的线性方程组进行求解，得到当前本体帧速度和初始视觉帧重力加速度；

若所述初始视觉帧重力加速度值的模值与当地重力加速度值的绝对误差小于预设值，且n的值大于预设值，判定视觉惯性初始对准成功。

可选地，所述基于视觉里程计方法计算视觉帧位姿，包括：

对视觉帧进行特征提取；

对相邻的视觉帧进行特征匹配，获得特征匹配信息；

根据所述特征匹配信息对相机进行运动估计，得到视觉帧位姿。

可选地，所述对相邻两个视觉帧之间的惯性测量单元的测量数据进行预积分得到积分数据，包括：

获取两个连续的视觉帧以及与所述视觉帧对应的时间戳；

确定与所述时间戳对应的时间段内的多个惯性测量单元的测量值；

对所述测量值进行预积分计算，得到积分数据。

可选地，所述对所述线性方程组的系数矩阵和常数项进行迭代更新，包括：

根据第k+n帧的线性方程组的系数矩阵和常数项，生成k+n+1帧的线性方程组的系数矩阵和常数项，所述第k+n帧包括第k+n个视觉帧以及所述第k+n个视觉帧对应的第k+n个本体帧，所述第k+n+1帧包括第k+n+1个视觉帧以及所述第k+n+1个视觉帧对应的第k+n+1个本体帧；

对所述系数矩阵和所述常数项进行迭代更新。

一种视觉惯性实时初始化对准系统，包括：

第一获取单元，用于获取当前视觉帧，并获取第k个视觉帧至所述当前视觉帧的连续n+1个视觉帧以及所述第k个视觉帧至所述当前视觉帧之间的惯性测量单元的测量数据，所述当前视觉帧为第k+n个视觉帧，k为自然数，n为正整数；所述视觉帧为相机拍摄的图像帧；

计算单元，用于基于视觉里程计方法计算视觉帧位姿，并对相邻两个视觉帧之间的惯性测量单元的测量数据进行预积分得到积分数据；

第二获取单元，用于根据视觉帧位姿以及相机与惯性测量单元的外参获得视觉帧对应的本体帧位姿；所述相机与惯性测量单元的外参为事先标定得到的相机与惯性测量单元的旋转与平移关系参数；

更新单元，用于利用所述积分数据和本体帧位姿建立线性方程组，并对所述线性方程组的系数矩阵和常数项进行迭代更新，其中，所述线性方程组的未知量为当前本体帧速度和初始视觉帧重力加速度；所述当前本体帧为当前视觉帧对应的本体帧；所述初始视觉帧为第0个视觉帧；

求解单元，用于对迭代更新后的线性方程组进行求解，得到当前本体帧速度和初始视觉帧重力加速度；

判定单元，用于若所述初始视觉帧重力加速度值的模值与当地重力加速度值的绝对误差小于预设值，且n的值大于预设值，判定视觉惯性初始对准成功。

可选地，所述计算单元包括：

提取子单元，用于对视觉帧进行特征提取；

匹配子单元，用于对相邻的视觉帧进行特征匹配，获得特征匹配信息；

估计子单元，用于根据所述特征匹配信息对相机进行运动估计，得到视觉帧位姿。

可选地，所述计算单元还包括：

第一获取子单元，用于获取两个连续的视觉帧以及与所述视觉帧对应的时间戳；

确定子单元，用于确定与所述时间戳对应的时间段内的多个惯性测量单元的测量值；

积分计算子单元，用于对所述测量值进行预积分计算，得到积分数据。

可选地，所述更新单元包括：

生成子单元，用于根据第k+n帧的线性方程组的系数矩阵和常数项，生成k+n+1帧的线性方程组的系数矩阵和常数项，所述第k+n帧包括第k+n个视觉帧以及所述第k+n个视觉帧对应的第k+n个本体帧，所述第k+n+1帧包括第k+n+1个视觉帧以及所述第k+n+1个视觉帧对应的第k+n+1个本体帧；

更新子单元，用于对所述系数矩阵和所述常数项进行迭代更新。

一种计算机存储介质，所述计算机存储介质上存储有可执行指令，所述指令被处理器执行时实现如上任一项所述的视觉惯性实时初始化对准方法。

一种电子设备，包括：

存储器，用于存储程序；

处理器，用于执行所述程序，所述程序具体用于：

获取当前视觉帧，并获取第k个视觉帧至所述当前视觉帧的连续n+1个视觉帧以及所述第k个视觉帧至所述当前视觉帧之间的惯性测量单元的测量数据，所述当前视觉帧为第k+n个视觉帧，k为自然数，n为正整数；所述视觉帧为相机拍摄的图像帧；

基于视觉里程计方法计算视觉帧位姿，并对相邻两个视觉帧之间的惯性测量单元的测量数据进行预积分得到积分数据；

根据视觉帧位姿以及相机与惯性测量单元的外参获得视觉帧对应的本体帧位姿；所述相机与惯性测量单元的外参为事先标定得到的相机与惯性测量单元的旋转与平移关系参数；

利用所述积分数据和本体帧位姿建立线性方程组，并对所述线性方程组的系数矩阵和常数项进行迭代更新，其中，所述线性方程组的未知量为当前本体帧速度和初始视觉帧重力加速度；所述当前本体帧为当前视觉帧对应的本体帧；所述初始视觉帧为第0个视觉帧；

对迭代更新后的线性方程组进行求解，得到当前本体帧速度和初始视觉帧重力加速度；

若所述初始视觉帧重力加速度值的模值与当地重力加速度值的绝对误差小于预设值，且n的值大于预设值，判定视觉惯性初始对准成功。

相较于现有技术，本发明提供了一种视觉惯性实时初始化对准方法及系统，获取当前视觉帧，并获取第k个视觉帧至当前视觉帧的连续n+1个视觉帧以及对应的惯性测量单元的测量数据，基于视觉里程计方法计算视觉帧位姿，并对相邻两个视觉帧之间的惯性测量单元的测量数据进行预积分得到积分数据；获得视觉帧对应的本体帧位姿，利用积分数据和本体帧位姿建立线性方程组，并对线性方程组的系数矩阵和常数项进行迭代更新。并对迭代更新的线性方程求解，得到当前本体帧速度和初始视觉帧重力加速度；若初始视觉帧重力加速度值的模值与当地重力加速度值的绝对误差小于预设值，且n的值大于预设值，判定视觉惯性初始对准成功。本发明的线性方程组的维度较小，求解未知量计算量更少，效率高，提升了实时性。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据提供的附图获得其他的附图。

图1为本发明实施例提供的一种视觉惯性实时初始化对准系统的示意图；

图2为本发明实施例提供的一种视觉惯性实时初始化对准方法的流程示意图；

图3为本发明实施例提供的一种视觉惯性实时初始化对准系统的结构示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本发明的说明书和权利要求书及上述附图中的术语“第一”和“第二”等是用于区别不同的对象，而不是用于描述特定的顺序。此外术语“包括”和“具有”以及他们任何变形，意图在于覆盖不排他的包含。例如包含了一系列步骤或单元的过程、方法、系统、产品或设备没有设定于已列出的步骤或单元，而是可包括没有列出的步骤或单元。

本发明实施例提供了一种视觉惯性实时初始化对准方法，是针对多帧视觉惯性信息进行视觉惯性初始化对准应用而提出，视觉惯性初始化对准的目标是求取本体帧的速度和初始视觉帧的重力加速度。

为了便于对发明实施例进行说明，现将本发明应用到的相关术语进行解析。

SLAM：(Simultaneous Localization and Mapping，同时定位与建图)。

IMU：(Inertial Measurement Unit，惯性测量单元)。

本发明使用的标号说明：

ci表示第i个视觉帧；

bi表示第i个视觉帧对应的IMU本体帧，以下简称第i个本体帧；

表示第i个视觉帧坐标系下的量；

表示第i个本体帧坐标系下的量；

表示从第j个视觉帧到第i个本体帧的旋转，

表示从第j个本体帧到第i个视觉帧的旋转，

表示从第j个视觉帧到第i个视觉帧的旋转，

表示从第j个本体帧到第i个本体帧的旋转；

表示从第j个视觉帧到第i个本体帧的平移，

表示从第j个本体帧到第i个视觉帧的平移，

表示从第j个视觉帧到第i个视觉帧的平移，

表示从第j个本体帧到第i个本体帧的平移；

表示第i个本体帧的速度；

表示本体帧坐标系到视觉帧坐标系的旋转；

表示本体帧坐标系到视觉帧坐标系的平移；

表示一个确定量的有噪量测值；

Δt表示视觉帧间隔。

另外本领域技术人员应当熟知以下恒等变换关系式：

由于现有的视觉惯性初始化对准方法大多不能兼顾精度和实时性要求，本发明提出了一种递推式视觉惯性初始化对准方法，能够融合多帧视觉位姿和IMU预积分信息求取本体帧的速度和初始视觉帧的重力加速度(这两项同城视觉惯性初始化对准状态向量)，具有精度高和实时性好等优点。设视觉惯性初始化对准融合的帧数为n，则VINS方法建立的线性方程组的系数矩阵有(6n-6)×(3n+3)维，待求解未知量是(3n+3)维向量；本发明公开的方法也同样融合了多帧视觉惯性信息以提高精度，但建立的线性方程组的系数矩阵只有(3n-3)×6维，待求解未知量则恒为6维向量。由此可见，本发明所公开的方法系数矩阵与未知量的维度均小于现有方法，尤其是当n足够大是，本发明所提出的方法相对于VINS方法计算复杂度显著减少。所以本发明公开的方法能够在不损失精度的情况下显著减少计算量，提升算法的实时性。

参见图1，其示出了本发明实施例提供的一种视觉实时初始对准系统示意图。采用相机拍摄场景，相机可以是单目、双目或RGBD相机，相机输出视觉图像。IMU为6轴或9轴惯性传感器，IMU输出IMU测量数据。相机与IMU共同组成视觉惯性传感器，相机与IMU相对位置及角度固定。IMU测量数据与视觉图像输送至计算设备。计算设备包含一个视觉惯性处理单元，运行本发明视觉惯性实时初始化对准方法。视觉惯性传感器可以位于计算设备外部，也可以置于计算设备内部，视觉惯性传感器与计算设备共同组成定位设备。

在本发明的一个实施例中，一台手机就是一个定位设备，手机包含了相机、IMU、处理器和存储器。在本实施例中，视觉惯性处理单元运行于手机处理器中，处理器可以是CPU、GPU或者DSP等。可选地，一个VR头盔就是一个定位设备，VR头盔内部包含了相机、IMU、处理器和存储器。在本实施例中，视觉惯性处理单元运行于VR头盔处理器中，处理器可以是CPU、GPU、专用DSP或嵌入式微处理器等。

参见图2，其示出了本发明实施例提供的一种视觉惯性实时初始化对准方法的流程示意图，该方法可以包括以下步骤：

S101、获取当前视觉帧，并获取第k个视觉帧至所述当前视觉帧的连续n+1个视觉帧以及所述第k个视觉帧至所述当前视觉帧之间的惯性测量单元的测量数据。

所述当前视觉帧为第k+n帧，k为自然数，n为正整数。假设视觉惯性初始对准使用第k帧至第k+n帧的连续n+1个视觉帧及相应本体帧的信息。对于第k+n个视觉帧及其相应的本体帧，视觉惯性初始对准问题的目标是求解相应的本体帧的速度

以及初始视觉帧的重力加速度

已知量为第0帧至第k+n帧总共k+n+1个视觉帧位姿

与

以及相应k+n+1个本体帧两两相邻帧预积分得到的k+n个本体帧预积分信息

与

另外，

与

为事先标定的相机与IMU的外参，它们也是已知量。

本发明所述的视觉帧应当理解为除IMU之外所有能通过对传感器数据做一定的运算实现对传感器本身进行定位定姿的数据源的统称，例如双目立体图像、RGBD深度相机图像、激光雷达图像等。

S102、基于视觉里程计方法计算视觉帧位姿，并对相邻两个视觉帧之间的惯性测量单元的测量数据进行预积分得到积分数据。

在本发明的一种可能的实现方式中，所述基于视觉里程计方法计算视觉帧位姿，包括：对视觉帧进行特征提取；对相邻的视觉帧进行特征匹配，获得特征匹配信息；根据所述特征匹配信息对相机进行运动估计，得到视觉帧位姿。

具体的，在本发明的一个实施例中，视觉帧的位姿可通过视觉里程计(VO)方法得到，视觉里程计方法可以选择常用的基于单目、双目或者RGBD传感器的方法，例如SFM、ORB-SLAM、Kinect-Fusion、RGBD-SLAM等。

通常现有的视觉里程计算法都是基于下面的步骤实现的：

图像获取：单目相机、双目相机或者RGBD相机；

图像校正：使用图像处理技术来去除透镜畸变；

特征检测：确定感兴趣的描述符，在帧与帧之间进行特征匹配并构建光流场，包括：使用相关性来度量两幅图像见的一致性(不进行长时间的特征跟踪)；特征提取、匹配(Lucas–Kanade，ORB等)；构建光流场。

检查光流场向量是否存在潜在的跟踪误差，移除外点；

由光流场估计照相机的运动，得到相机的位姿，可以采用的方式包括：使用使用卡尔曼滤波进行状态估计(相机位姿估计)；查找特征的几何与3D属性，最小化相邻两帧之间特征点的重投影误差，通过最优化方法(例如非线性优化)或随机采样方法(例如RANSAC)求解相机位姿；

周期性的重定位跟踪点以减少跟踪累计误差。

对应的，在本发明实施例中，

所述对相邻两个视觉帧之间的惯性测量单元的测量数据进行预积分得到积分数据，包括：获取两个连续的视觉帧以及与所述视觉帧对应的时间戳；确定与所述时间戳对应的时间段内的多个惯性测量单元的测量值；对所述测量值进行预积分计算，得到积分数据。

具体的，对于两个连续的本体帧i和i+1，设其时间戳分别为ti和ti+1。对于时间段[ti,ti+1]内的多个IMU量测值，可按下式进行预积分得到

与

上式中，

是加速度计测量值，

为加速度计偏置值，

为t时刻本体帧到bi帧的旋转。该式是连续时间积分公式。在本发明的一个实施例中，针对离散时间的IMU数据做预积分时，有多种不同的数值积分方法可供选择，例如欧拉积分法、中点积分法、RK4积分法等，本发明对此不作限定。

S103、根据视觉帧位姿以及相机与惯性测量单元的外参获得视觉帧对应的本体帧位姿。

S104、利用所述积分数据和本体帧位姿建立线性方程组，并对所述线性方程组的系数矩阵和常数项进行迭代更新。

其中，所述相机与惯性测量单元的外参为事先标定得到的相机与惯性测量单元的旋转与平移关系参数；所述线性方程组的未知量为当前本体帧速度和初始视觉帧重力加速度；所述当前本体帧为当前视觉帧对应的本体帧；所述初始视觉帧为第0个视觉帧。

S105、对迭代更新后的线性方程组进行求解，得到当前本体帧速度和初始视觉帧重力加速度。

为了求解第k+n个本体帧的速度

以及初始视觉帧的重力加速度

建立线性方程组H(k+n)χ(k+n)＝z(k+n)。其中，H(k+n)为系数矩阵，χ(k+n)为待求解未知量，z(k+n)为已知量。它们的表达式为：

H(k+n)为3n×6维矩阵，z(k+n)为3n维列向量，χ(k+n)为6维列向量。

由此看来，对于第k+n帧，只要建立了线性方程组H(k+n)χ(k+n)＝z(k+n)，就可以解出χ(k+n)中的本体帧的速度

以及初始视觉帧的重力加速度

从而实现视觉惯性初始对准目标。

本发明不必针对每一帧单独重新建立线性方程。本发明通过一种递推式线性方程组建立方法。简而言之就是第k+n+1帧的线性方程组可以在第k+n帧的线性方程组基础上递推得到，而不用重新建立。

为了便于线性方程组的生成，在本发明实施例中对所述第一线性方程组以m行m列为单位进行分块，并对所述第一线性方程组中的已知量以m行为单位进行分块，得到m个子块；依据所述子块获取第二线性方程组的已知信息；基于所述已知信息，生成所述第二线性方程组。

为了便于描述，将H(k+n)以3行3列为单位进行分块，共可分为n行2列个子块，则可将H(k+n)记为：

其中，

同理，将z(k+n)以3行为单位进行分块，共可分为n个子块，则可将z(k+n)记为：

其中，

对于第k+n+1帧，H(k+n+1)为(3n+3)×6维矩阵，将H(k+n+1)以3行3列为单位进行分块，共可分为n+1行2列个子块，则可将H(k+n+1)记为：

对于第k+n+1帧，z(k+n+1)为3n+3维列向量，同理可将z(k+n+1)以3行为单位进行分块，共可分为n+1个子块，则可将z(k+n+1)记为：

第k+n+1帧线性方程组的各元素可以根据第k+n帧递推得到，具体关系式为：

根据关系式(6)～(11)，可由第k+n帧的线性方程组递推得到第k+n+1帧的线性方程组。

对线性方程组进行求解，可得到当前本体帧速度

和初始视觉帧重力加速度

的值。

S106、若所述初始视觉帧重力加速度值的模值与当地重力加速度值的绝对误差小于预设值，且n的值大于预设值，判定视觉惯性初始对准成功。

例如，如果重力加速度

值的模值与理论重力加速度模值的插值小于预设值，且n的值大于预设值，则判定初始化对准成功。否则初始化对准失败。

本发明提出的方法融合多帧视觉惯性信息建立线性方程组，可以提高求解的初始化对准状态向量(包括本体帧的速度和初始视觉帧的重力加速度)的精度。设融合的帧数为n，则传统的VINS方法建立的线性方程组的系数矩阵有(6n-6)×(3n+3)维，待求解未知量是(3n+3)维向量；而本发明公开的方法也融合了同样多帧视觉惯性信息以提高精度，但该方法建立的线性方程组的系数矩阵只有(3n-3)×6维，待求解未知量则恒为6维向量。线性方程组系数矩阵与未知量维度都有显著下降，因此整体求解时间也会显著下降，所以本发明的方法能够显著提高效率，具有更好的实时性。

在本发明的一些实施例中，用第j个视觉帧到第0个视觉帧的平移

与旋转

表示第j帧的位姿，同理也可以反过来使用第0个视觉帧到第j个视觉帧的平移

与旋转

表示第j帧的位置姿态，这两种位置姿态的表达式之间具有确定的转换关系：

根据该转换关系可以实现两种位置姿态表达式的等效替换。

本发明的方法应用到水平地面运行的无人车上，则z方向的维度可以舍弃，三维问题简化为二维。本领域技术人员利用本发明公开的针对三维空间的方法稍加推导与修改就能应用到二维空间上。

当视觉帧为单目图像时，求解得到的位置

是不准确的，包含一个未知尺度s参数。此时，z(k+n)_j中的

表达式变为

z(k+n)中因包含未知数故而不再是已知量，此时方程组无法直接求解。为了求解未知量，需要对方程组做少许调整。调整方法是将z(k+n)中含有未知数的项

中的已知部分

移至方程组左边，即置于H(k+n)中，并将未知部分s放入χ(k+n)，即在χ(k+n)中新增s。

经过调整后，公式(1)～(3)可以变为：

H(k+n)与z(k+n)各元素为：

第k+n帧到第k+n+1帧的递推关系式为：

公式(6)～(10)不变。

H(k+n+1)_j,3＝H(k+n)_j,3,j＝1,2,...,n (12)

在本发明实施例中也可以用恒等变换关系将方程组H(k+n)χ(k+n)＝z(k+n)的各个旋转矩阵和速度向量变换至初始视觉坐标系，此时：

H(k+n)_j,1＝IΔt，H(k+n)_j,2＝(n-j+0.5)IΔt²，j＝1,2,...,n (4变)

H(k+n+1)_j,1＝H(k+n)_j,1,j＝1,2,...,n (6变)

H(k+n+1)_n+1,1＝IΔt (8变)

H(k+n+1)_n+1,2＝0.5IΔt² (9变)

在本发明实施了中，可以融合多帧视觉惯性信息进行初始化对准，精度高。线性方程组系数矩阵和未知量维度更少，求解未知量计算量更少，效率更高，实时性更好。线性方程组通过递推关系建立，进一步降低计算量提高效率。

参见图3，在本发明实施例提供了另一种视觉惯性实时初始化对准系统，包括：

第一获取单元10，用于获取当前视觉帧，并获取第k个视觉帧至所述当前视觉帧的连续n+1个视觉帧以及所述第k个视觉帧至所述当前视觉帧之间的惯性测量单元的测量数据，所述当前视觉帧为第k+n个视觉帧，k为自然数，n为正整数；所述视觉帧为相机拍摄的图像帧；

计算单元20，用于基于视觉里程计方法计算视觉帧位姿，并对相邻两个视觉帧之间的惯性测量单元的测量数据进行预积分得到积分数据；

第二获取单元30，用于根据视觉帧位姿以及相机与惯性测量单元的外参获得视觉帧对应的本体帧位姿；所述相机与惯性测量单元的外参为事先标定得到的相机与惯性测量单元的旋转与平移关系参数；

更新单元40，用于利用所述积分数据和本体帧位姿建立线性方程组，并对所述线性方程组的系数矩阵和常数项进行迭代更新，其中，所述线性方程组的未知量为当前本体帧速度和初始视觉帧重力加速度；所述当前本体帧为当前视觉帧对应的本体帧；所述初始视觉帧为第0个视觉帧；

求解单元50，用于对迭代更新后的线性方程组进行求解，得到当前本体帧速度和初始视觉帧重力加速度；

判定单元60，用于若所述初始视觉帧重力加速度值的模值与当地重力加速度值的绝对误差小于预设值，且n的值大于预设值，判定视觉惯性初始对准成功。

在上述实施例的基础上，所述计算单元包括：

提取子单元，用于对视觉帧进行特征提取；

匹配子单元，用于对相邻的视觉帧进行特征匹配，获得特征匹配信息；

估计子单元，用于根据所述特征匹配信息对相机进行运动估计，得到视觉帧位姿。

在上述实施例的基础上，所述计算单元还包括：

第一获取子单元，用于获取两个连续的视觉帧以及与所述视觉帧对应的时间戳；

确定子单元，用于确定与所述时间戳对应的时间段内的多个惯性测量单元的测量值；

积分计算子单元，用于对所述测量值进行预积分计算，得到积分数据。

在上述实施例的基础上，所述更新单元包括：

生成子单元，用于根据第k+n帧的线性方程组的系数矩阵和常数项，生成k+n+1帧的线性方程组的系数矩阵和常数项，所述第k+n帧包括第k+n个视觉帧以及所述第k+n个视觉帧对应的第k+n个本体帧，所述第k+n+1帧包括第k+n+1个视觉帧以及所述第k+n+1个视觉帧对应的第k+n+1个本体帧；

更新子单元，用于对所述系数矩阵和所述常数项进行迭代更新。

本发明提供了一种视觉惯性实时初始化对准系统，获取当前视觉帧，并获取第k个视觉帧至当前视觉帧的连续n+1个视觉帧以及对应的惯性测量单元的测量数据，基于视觉里程计方法计算视觉帧位姿，并对相邻两个视觉帧之间的惯性测量单元的测量数据进行预积分得到积分数据；获得视觉帧对应的本体帧位姿，利用积分数据和本体帧位姿建立线性方程组，并对线性方程组的系数矩阵和常数项进行迭代更新。并对迭代更新的线性方程求解，得到当前本体帧速度和初始视觉帧重力加速度；若初始视觉帧重力加速度值的模值与当地重力加速度值的绝对误差小于预设值，且n的值大于预设值，判定视觉惯性初始对准成功。本发明的线性方程组的维度较小，求解未知量计算量更少，效率高，提升了实时性。

在本发明实施例中还提供了一种计算机存储介质，所述计算机存储介质上存储有可执行指令，所述指令被处理器执行时实现如上任一项所述的视觉惯性实时初始化对准方法。

本发明实施例还提供了一种电子设备，包括：

存储器，用于存储程序；

处理器，用于执行所述程序，所述程序具体用于：

获取当前视觉帧，并获取第k个视觉帧至所述当前视觉帧的连续n+1个视觉帧以及所述第k个视觉帧至所述当前视觉帧之间的惯性测量单元的测量数据，所述当前视觉帧为第k+n个视觉帧，k为自然数，n为正整数；所述视觉帧为相机拍摄的图像帧；

基于视觉里程计方法计算视觉帧位姿，并对相邻两个视觉帧之间的惯性测量单元的测量数据进行预积分得到积分数据；

根据视觉帧位姿以及相机与惯性测量单元的外参获得视觉帧对应的本体帧位姿；所述相机与惯性测量单元的外参为事先标定得到的相机与惯性测量单元的旋转与平移关系参数；

利用所述积分数据和本体帧位姿建立线性方程组，并对所述线性方程组的系数矩阵和常数项进行迭代更新，其中，所述线性方程组的未知量为当前本体帧速度和初始视觉帧重力加速度；所述当前本体帧为当前视觉帧对应的本体帧；所述初始视觉帧为第0个视觉帧；

对迭代更新后的线性方程组进行求解，得到当前本体帧速度和初始视觉帧重力加速度；

若所述初始视觉帧重力加速度值的模值与当地重力加速度值的绝对误差小于预设值，且n的值大于预设值，判定视觉惯性初始对准成功。

处理器，用于执行所述程序，所述程序还用于执行视觉惯性实时初始化对准方法的任意步骤及相关子步骤的方法。

本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的装置而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。

对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本发明。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明将不会被限制于本文所示的这些实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。

Claims (10)

1.一种视觉惯性实时初始化对准方法，其特征在于，包括：

获取当前视觉帧，并获取第k个视觉帧至所述当前视觉帧的连续n+1个视觉帧以及所述第k个视觉帧至所述当前视觉帧之间的惯性测量单元的测量数据，所述当前视觉帧为第k+n个视觉帧，k为自然数，n为正整数；所述视觉帧为相机拍摄的图像帧；

基于视觉里程计方法计算视觉帧位姿，并对相邻两个视觉帧之间的惯性测量单元的测量数据进行预积分得到积分数据；

根据视觉帧位姿以及相机与惯性测量单元的外参获得视觉帧对应的本体帧位姿；所述相机与惯性测量单元的外参为事先标定得到的相机与惯性测量单元的旋转与平移关系参数；

利用所述积分数据和本体帧位姿建立线性方程组，并对所述线性方程组的系数矩阵和常数项进行迭代更新，其中，所述线性方程组的未知量为当前本体帧速度和初始视觉帧重力加速度；所述当前本体帧为当前视觉帧对应的本体帧；所述初始视觉帧为第0个视觉帧；

对迭代更新后的线性方程组进行求解，得到当前本体帧速度和初始视觉帧重力加速度；

若所述初始视觉帧重力加速度值的模值与当地重力加速度值的绝对误差小于预设值，且n的值大于预设值，判定视觉惯性初始对准成功。

2.根据权利要求1所述的方法，其特征在于，所述基于视觉里程计方法计算视觉帧位姿，包括：

对视觉帧进行特征提取；

对相邻的视觉帧进行特征匹配，获得特征匹配信息；

根据所述特征匹配信息对相机进行运动估计，得到视觉帧位姿。

3.根据权利要求1所述的方法，其特征在于，所述对相邻两个视觉帧之间的惯性测量单元的测量数据进行预积分得到积分数据，包括：

获取两个连续的视觉帧以及与所述视觉帧对应的时间戳；

确定与所述时间戳对应的时间段内的多个惯性测量单元的测量值；

对所述测量值进行预积分计算，得到积分数据。

4.根据权利要求1所述的方法，其特征在于，所述对所述线性方程组的系数矩阵和常数项进行迭代更新，包括：

根据第k+n帧的线性方程组的系数矩阵和常数项，生成第k+n+1帧的线性方程组的系数矩阵和常数项，所述第k+n帧包括第k+n个视觉帧以及所述第k+n个视觉帧对应的第k+n个本体帧，所述第k+n+1帧包括第k+n+1个视觉帧以及所述第k+n+1个视觉帧对应的第k+n+1个本体帧；

对所述系数矩阵和所述常数项进行迭代更新。

5.一种视觉惯性实时初始化对准系统，其特征在于，包括：

第一获取单元，用于获取当前视觉帧，并获取第k个视觉帧至所述当前视觉帧的连续n+1个视觉帧以及所述第k个视觉帧至所述当前视觉帧之间的惯性测量单元的测量数据，所述当前视觉帧为第k+n个视觉帧，k为自然数，n为正整数；所述视觉帧为相机拍摄的图像帧；

计算单元，用于基于视觉里程计方法计算视觉帧位姿，并对相邻两个视觉帧之间的惯性测量单元的测量数据进行预积分得到积分数据；

第二获取单元，用于根据视觉帧位姿以及相机与惯性测量单元的外参获得视觉帧对应的本体帧位姿；所述相机与惯性测量单元的外参为事先标定得到的相机与惯性测量单元的旋转与平移关系参数；

更新单元，用于利用所述积分数据和本体帧位姿建立线性方程组，并对所述线性方程组的系数矩阵和常数项进行迭代更新，其中，所述线性方程组的未知量为当前本体帧速度和初始视觉帧重力加速度；所述当前本体帧为当前视觉帧对应的本体帧；所述初始视觉帧为第0个视觉帧；

求解单元，用于对迭代更新后的线性方程组进行求解，得到当前本体帧速度和初始视觉帧重力加速度；

判定单元，用于若所述初始视觉帧重力加速度值的模值与当地重力加速度值的绝对误差小于预设值，且n的值大于预设值，判定视觉惯性初始对准成功。

6.根据权利要求5所述的系统，其特征在于，所述计算单元包括：

提取子单元，用于对视觉帧进行特征提取；

匹配子单元，用于对相邻的视觉帧进行特征匹配，获得特征匹配信息；

估计子单元，用于根据所述特征匹配信息对相机进行运动估计，得到视觉帧位姿。

7.根据权利要求5所述的系统，其特征在于，所述计算单元还包括：

第一获取子单元，用于获取两个连续的视觉帧以及与所述视觉帧对应的时间戳；

确定子单元，用于确定与所述时间戳对应的时间段内的多个惯性测量单元的测量值；

积分计算子单元，用于对所述测量值进行预积分计算，得到积分数据。

8.根据权利要求5所述的系统，其特征在于，所述更新单元包括：

生成子单元，用于根据第k+n帧的线性方程组的系数矩阵和常数项，生成第k+n+1帧的线性方程组的系数矩阵和常数项，所述第k+n帧包括第k+n个视觉帧以及所述第k+n个视觉帧对应的第k+n个本体帧，所述第k+n+1帧包括第k+n+1个视觉帧以及所述第k+n+1个视觉帧对应的第k+n+1个本体帧；

更新子单元，用于对所述系数矩阵和所述常数项进行迭代更新。

9.一种计算机存储介质，其特征在于，所述计算机存储介质上存储有可执行指令，所述指令被处理器执行时实现如权利要求1-4中任意一项所述的视觉惯性实时初始化对准方法。

10.一种电子设备，其特征在于，包括：

存储器，用于存储程序；

处理器，用于执行所述程序，所述程序具体用于：

获取当前视觉帧，并获取第k个视觉帧至所述当前视觉帧的连续n+1个视觉帧以及所述第k个视觉帧至所述当前视觉帧之间的惯性测量单元的测量数据，所述当前视觉帧为第k+n个视觉帧，k为自然数，n为正整数；所述视觉帧为相机拍摄的图像帧；

基于视觉里程计方法计算视觉帧位姿，并对相邻两个视觉帧之间的惯性测量单元的测量数据进行预积分得到积分数据；

根据视觉帧位姿以及相机与惯性测量单元的外参获得视觉帧对应的本体帧位姿；所述相机与惯性测量单元的外参为事先标定得到的相机与惯性测量单元的旋转与平移关系参数；

利用所述积分数据和本体帧位姿建立线性方程组，并对所述线性方程组的系数矩阵和常数项进行迭代更新，其中，所述线性方程组的未知量为当前本体帧速度和初始视觉帧重力加速度；所述当前本体帧为当前视觉帧对应的本体帧；所述初始视觉帧为第0个视觉帧；

对迭代更新后的线性方程组进行求解，得到当前本体帧速度和初始视觉帧重力加速度；

若所述初始视觉帧重力加速度值的模值与当地重力加速度值的绝对误差小于预设值，且n的值大于预设值，判定视觉惯性初始对准成功。

Images