CN113358134A - 一种视觉惯性里程计系统 - Google Patents

Abstract

本发明提供一种视觉惯性里程计系统，包括数据采集模块和数据处理模块，数据采集模块用关于采集图像信息、角速度和加速度；数据处理模块用于执行以下预处理和后端优化算法；本发明算法使用视觉/惯性紧组合导航技术，对比传统视觉里程计，该视觉惯性里程计系统可以在高动态情况下稳定运行且有较好的性能水平；相比于基于松组合的视觉惯性里程计，本发明能够充分传感器的信息，在使用相同水平的传感器时发挥出更好的性能；定位精度可以达到分米级，无需依赖卫星信号。

Description

一种视觉惯性里程计系统

技术领域

本发明涉及视觉惯性导航技术领域，尤其涉及一种视觉惯性里程计系统。

背景技术

随着无人机、无人车以及移动机器人的井喷式发展，导航技术成为了制约无人平台广泛应用的瓶颈技术之一。SLAM技术一直被广泛用于无人机、无人车以及移动机器人的导航，即自主导航领域，SLAM是“Simultaneous Localization And Mapping”的缩写，译为同步定位与地图构建。经过了三十多年的发展，视觉SLAM——即使用视觉传感器作为主要信息输入的SLAM技术，由于其成本的低廉且图像信息更为丰富，逐渐成为了SLAM研究热点之一。

视觉SLAM算法框架主要分为视觉里程计和地图构建两大模块，视觉里程计输出系统的位置姿态信息，地图构建模块通过结合视觉里程计的信息和图像信息完成地图构建。其中，单目视觉SLAM是使用单目相机作为主要传感器的SLAM技术，拥有系统结构简单、成本较低的优势，但是由于单目相机不能得到绝对深度信息，因而实现难度也是最大的；且只使用相机的视觉里程计对于高动态运动情况(即有较大的旋转运动)，容易丢失特征点而导致里程计失效。

故而提高定位精度，特别是高动态情况下的定位精度，成为了视觉里程计的重要研究方向。在应用需求的牵引下，使用视觉/惯性组合导航技术的视觉惯性里程计，特别是视觉与微惯性传感器的组合，逐渐发展成为当前自主导航及机器人领域的研究热点。视觉/惯性组合导航具有显著的优点：(1)微惯性器件和视觉传感器具有体积小、成本低的优点，随着制造技术的不断进步，器件越来越小，且成本越来越低。(2)不同于卫星和无线电导航，视觉和惯性导航均不依赖外部设施支撑，可以实现自主导航。(3)惯性器件和视觉器件具有很好的互补性，惯性导航误差随时间累积，但是在短时间内可以很好地跟踪载体快速运动，保证短时间的导航精度；而视觉导航在低动态运动中具有很高的估计精度，且引入了视觉闭环矫正可以极大地抑制组合导航误差，两者的组合可以更好地估计导航参数。

视觉/惯性组合导航技术以组合方式可以分为松组合和紧组合：松组合把视觉传感器和惯性传感器分别作为独立的模块进行组合，两个模块分别使用视觉导航算法和惯性导航算法来获得系统的导航信息，然后再对两个信息进行融合滤波，来达到更好的导航结果；紧组合则是使用两个传感器的原始数据，然后构建一个基于最小二乘法的优化框架，把所有数据都放入框架内进行优化，最后得到导航结果。松组合的优点是结构简单，实现难度较低，但缺点是性能较低，达不到很好的精度；紧组合可以达到很好的性能水平，但是结构复杂，实现难度大。

发明内容

为了在保证导航精度的同时，降低算法复杂度，提高处理效率，本发明提供一种视觉惯性里程计系统，包括数据采集模块和数据处理模块，所述数据采集模块用关于采集图像信息、角速度和加速度；所述数据处理模块用于执行以下预处理和后端优化算法：

预处理：

对采集的所述图像信息、角速度和加速度进行时间同步对齐；

针对当前图像信息，提取图像Harris角点，跟踪相邻帧与所述当前图像信息的图像Harris角点匹配的特征点；

利用所述采集的角速度和加速度进行积分，得到当前时刻的位置速度旋转矩阵PVQ；

计算所述相邻帧的预积分增量，以及误差传递矩阵；

后端优化：

判断所述当前图像信息的特征点，与前一帧图像匹配的特征点之间的相对位移是否超过阈值，如是，判定所述当前图像信息为关键帧，边缘化滑动窗口内最老的图像帧，并将该关键帧存入所述滑动窗的最新位置；若否，则丢弃前一图像帧，并将所述当前图像信息和前一图像帧之间的预积分增量，转换为所述当前图像信息与前二图像帧之间的预积分增量；

构建由边缘化的先验信息、测量残差、视觉重投影误差构成的目标函数，通过优化目标函数来得到系统状态向量的最优解，所述系统状态向量包括系统的位置、姿态、速度、加速度计误差和、陀螺仪误差；基于最优解对应的系统状态向量得到导航信息。

进一步的，所述数据采集模块包括相机和惯性传感器，所述相机用于所述采集图像信息，所述惯性传感器用于所述采集角速度和加速度。

进一步的，所述利用所述角速度和加速度进行积分，得到当前时刻的位置速度旋转矩阵，包括通过如下积分运算得到：

所述

为k+1时刻世界坐标系下的的位置，所述

为k时刻的世界坐标系下的位置，所述

为k时刻世界坐标系下的的速度，所述

为k+1时刻世界坐标系下的的速度，所述Δt_k为k时刻到k+1时刻的时间差，所述

为Body自身坐标系(body-axiscoordinate system)到Word世界坐标系的旋转矩阵，所述

和

分别为所述采集的加速度和角速度，所述g^w为重力向量，所述

和所述

分别为加速度偏差和角速度偏差。

进一步的，所述计算所述相邻帧的预积分增量，通过如下预积分增量函数得到：

式中，所述

所述

所述

分别为式(1)中位置速度旋转矩阵的预积分增量真值；所述

所述

所述

分别为所述数据采集模块测量值得到的预积分增量；所述

所述

为位置P矩阵的误差预积分增量；所述

为速度V矩阵的误差预积分增量；所述

为转移Q矩阵的误差预积分增量。

进一步的，所述目标函数包括：

所述‖r_p-J_pX‖²为边缘化的先验信息，所述

为测量残差，所述

为视觉重投影误差。

进一步的，所述通过优化目标函数来得到系统状态向量的最优解包括：

当目标函数中的系统状态向量X有一个增量ΔX后，目标函数有最小值，构造一个关于增量ΔX的增量公式：

上式中，

为预积分误差项的协方差，

为误差项r_B关于所有状态向量X的Jacobian矩阵，

为视觉观测的误差协方差矩阵；

对上述增量公式进行简化：

(Λ_p+Λ_B+Λ_C)ΔX＝b_p+b_B+b_C (5)

其中，Λ_p，Λ_B，Λ_C为Hessian矩阵；

通过最小二乘法求解上述简化后的增量公式，可得当前时刻系统状态向量X的最优解。

进一步的，所述方法还包括：

在进行首次预处理之后，且在首次进行后端优化处理之前，还需要进行一次初始化处理：首先利用Global SFM算法，求得当前收到的所有帧的位姿和所有路标点在世界坐标系下的位置，求解重力方向、尺度因子、每一帧对应的速度及陀螺仪偏差，并根据计算得到的陀螺仪偏差，更新所述首次预处理计算得到的相邻帧的预积分增量。

进一步的，所述重力方向、尺度因子、每一帧对应的速度通过如下函数，利用LDLT分解方法求解得到：

进一步的，所述陀螺仪偏差通过构造如下函数，采用LDLT分解方法计算得到：

本发明的有益效果是：

根据本发明提供的一种视觉惯性里程计系统，包括数据采集模块和数据处理模块，数据采集模块用关于采集图像信息、角速度和加速度；数据处理模块用于执行以下预处理和后端优化算法：对采集的所述图像信息、角速度和加速度进行时间同步对齐；针对当前图像信息，提取图像Harris角点，跟踪相邻帧与当前图像信息的图像Harris角点匹配的特征点；利用采集的角速度和加速度进行积分，得到当前时刻的位置速度旋转矩阵PVQ；计算相邻帧的预积分增量，以及误差传递矩阵；判断当前图像信息的特征点，与前一帧图像匹配的特征点之间的相对位移是否超过阈值，如是，判定当前图像信息为关键帧，边缘化滑动窗口内最老的图像帧，并将该关键帧存入所述滑动窗的最新位置；若否，则丢弃前一图像帧，并将当前图像信息和前一图像帧之间的预积分增量，转换为当前图像信息与前二图像帧之间的预积分增量；构建由边缘化的先验信息、测量残差、视觉重投影误差构成的目标函数，通过优化目标函数来得到系统状态向量的最优解，系统状态向量包括系统的位置、姿态、速度、加速度计误差和、陀螺仪误差；基于最优解对应的系统状态向量得到导航信息。本发明算法使用视觉/惯性紧组合导航技术，对比传统视觉里程计，该视觉惯性里程计系统可以在高动态情况下稳定运行且有较好的性能水平；相比于基于松组合的视觉惯性里程计，本发明能够充分传感器的信息，在使用相同水平的传感器时发挥出更好的性能；定位精度可以达到分米级，无需依赖卫星信号。

附图说明

图1为本发明实施例一的视觉惯性里程计系统结构示意图；

图2为本发明实施例一的视觉惯性导航算法流程示意图；

图3为本发明实施例一的本系统在仓库大空间环境下的运行效果图；

图4为本发明实施例一的本系统在室内小空间环境下的运行效果图；

图5为本发明实施例一的本系统在室内房间-走廊环境下的运行效果图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，下面通过具体实施方式结合附图对本发明作进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

实施例一：

本实施例提供一种视觉惯性里程计系统，PVQ矩阵，即位置速度旋转矩阵，在世界坐标系下的真值即为算法最终需要得到的结果。但是，真值是不可能直接得到的，本方案构建了一个关于包含IMU得到的PVQ矩阵误差以及视觉重投影误差的目标函数(视觉重投影误差可以理解为相机输出的定位信息(也为PVQ矩阵)的误差)，之后，通过最小二乘法优化，使得该目标函数近似为0时，即可得到当前时刻的误差最小的PVQ矩阵，也就得到了导航结果。

基于以上逻辑还存在以下问题，就是如果直接使用0时刻到当前时刻的IMU输出数据积分得到PVQ矩阵，会导致数据量过于庞大，降低运行效率，故在信息损失最小且效率最优的需求下，本方案使用两帧图像之间的IMU数据进行积分，得到预积分(即IMU在两帧图像之间的PVQ矩阵)，再根据预积分得到预积分误差，用这个预积分误差代替前述的PVQ矩阵误差，可以得到一个计算量较小，且包含信息足够的目标函数，当目标函数趋近于0时，就得到了最终需要的PVQ矩阵的误差，再由测量值加上误差，便可以得到足够接近真值的最终结果。

请参见图1-5，包括数据采集模块10和数据处理模块20，其中数据采集模块10包括相机11(具体可采用120度广角相机)和惯性传感器12(简称IMU)，相机11用于采集图像信息，惯性传感器12用于采集角速度和加速度。数据处理模块20可以为嵌入式计算板，主要包括前端数据预处理模块21、参数初始化模块22和后端优化模块23，并执行以下算法流程：

首先，系统上电，算法启动，单目相机获取图像信息，IMU获得角速度与加速度信息，之后嵌入式计算板的驱动程序读取两个传感器数据和板上晶振时间数据，将两种数据同步到晶振时间后输出至视觉/惯性紧组合导航算法，其中，相机的输出频率是20Hz(相机频率从5Hz-100Hz，视场角FOV从10°到360°)，IMU的频率是200Hz(IMU频率从100Hz-1000Hz)，算法对相机图像信息和IMU信息进行对齐和配对。

系统通过底层驱动对两种传感器数据进行了硬件时间同步，减少了算法对齐两种数据的计算耗时，提高了算法的实时性。

之后开始对两种数据进行预处理：对于图像信息，数据预处理模块提取图像Harris角点，利用金字塔光流法跟踪相邻帧，通过随机抽样一致算法(RANSAC算法)去除异常点，最后将跟踪到的特征点推送到图像队列中，并通知后端进行处理，但是对于第一帧图像不进行以上处理，直接储存；对于IMU信息，数据预处理模块提将IMU数据进行积分，得到当前时刻的位置速度旋转矩阵(PVQ矩阵)(这里旋转矩阵采用四元数)，作为相机的初始化位置、速度姿态，积分公式如下：

其中，

为k+1时刻世界坐标系下的的位置，

为k时刻的世界坐标系下的位置，

为k时刻世界坐标系下的的速度，

为k+1时刻世界坐标系下的的速度，

为k时刻世界坐标系下的旋转四元数，

为k+1时刻世界坐标系下的旋转四元数，Δt_k为k时刻到k+1时刻的时间差，

为Body自身坐标系到Word世界坐标系的旋转矩阵，

和

分别为所述采集的加速度和角速度，g^w为重力向量，

和

分别为加速度偏差和角速度偏差。

为

其中

为

的反对称阵，

为

的转置。

之后计算在后端优化中将用到的相邻帧的预积分增量，假设预积分的变化量与IMU偏差是线性关系，则可以将预积分增量函数简化且精度损失很小，由积分函数式(8)可得如下预积分增量函数：

式中

分别为式(8)中PVQ矩阵的预积分增量真值；

分别为IMU测量值得到的预积分增量；

为P矩阵的误差预积分增量；

为V矩阵的误差预积分增量；

为Q矩阵的误差预积分增量。计算完预积分增量之后还需要计算误差传递矩阵，误差传递矩阵如下所示：

上式中，

以上处理得到的预积分增量将用于后端优化模块中每帧图像间的计算约束，在滑动窗口优化过程中会使用这个约束来优化变量；得到的误差传递矩阵将会用于后端优化模块中IMU残差项的优化计算。

对于图像信息，算法对图像提取特征点并追踪前一相邻帧的特征点，特征点提取方法是通过检测每个点与周围点灰度值梯度来判断是否为特征点；对于IMU信息，算法计算相邻两个图像帧之间的预积分，通过预积分来约束两个图像帧信息，提高系统稳定性。

数据预处理完成之后，进行是否初始化判断，若已执行初始化，则进入后端优化模块进行计算；若未执行初始化，则开始初始化进程：首先用Global SFM算法(全局式SFM，一种通过提取图像特征点并匹配图像间的特征点来进行三维重建的算法)求得当前收到的所有帧的位姿和所有路标点(路标点为已进行匹配追踪的特征点)的3D位置(即世界坐标系下的位置)，然后跟IMU预积分的值对齐，求解重力方向、尺度因子、每一帧对应的速度及陀螺仪偏差。整个系统一次运行过程中，初始化只进行一次。前面预处理阶段得到的是IMU在世界坐标系下的速度(位置和姿态)，而在初始化阶段，把IMU在世界坐标系下的速度(位置和姿态)作为相机的初始化速度(位置和姿态)。

求解重力方向

尺度因子s、每一帧对应的速度

的函数如下：

采用LDLT分解方法(若A为一对称矩阵且其任意一k阶主子阵均不为零，则A有如下惟一的分解形式：A＝LDL^T，其中L为一下三角形单位矩阵(即主对角线元素皆为1)，D为一对角矩阵(只在主对角线上有元素，其余皆为零)，L^T为L的转置矩阵)求解上式，就能得到相应变量。

构造如下函数，可以求解陀螺仪偏差b_ω：

上式形如Ax＝B，故也可以采用LDLT分解求得陀螺仪偏差b_ω。计算得到陀螺仪偏差之后，需重新按照式(9)计算IMU预积分。

初始化完成后，图像数据进入一个大小为10帧图像数据的滑动窗口，后端优化模块算法开始，首先判断当前帧图像特征点与上一帧时的特征点的相对位移是否超出阈值：若超出，则该图像帧为关键帧，边缘化滑动窗口内最老的图像帧，该帧存入第10个关键帧；若未超出，则该帧不是关键帧，丢弃上一帧数据，这一帧数据顶替上一帧的位置，同时把当前帧和前一帧之间的IMU预积分转换为当前帧和前二帧之间的IMU预积分。

之后构建一个关于误差项的目标函数，通过优化目标函数来得到系统状态向量的最优解，系统状态向量X包含：系统的位置、姿态、速度、加速度计误差和、陀螺仪误差、相机到IMU的外参、路标点的逆深度。加速度计误差和陀螺仪误差包含白噪声，是属于时变的变量，没有确定解，只能将其优化为最小，当式(3)中的加速度计误差增量和陀螺仪误差增量为0时，加速度计误差和陀螺仪误差便是最小。路标点为已进行匹配追踪的特征点，将路标点投影至世界坐标系下，该点距离相机的长度即为深度，逆深度为深度的倒数。相机到IMU的外参为相机坐标系转换到IMU坐标系的旋转平移矩阵，在初始化阶段，将路标点的3D位置同IMU预积分的值对齐，便可求得相机到IMU的外参。

目标函数为：

这三项分别为边缘化的先验信息、IMU的测量残差、视觉的重投影误差，根据求解最小二乘法的Levenberg-Marquardt算法，想要求解目标函数的最小值，可以理解为，当目标函数中的状态向量X有一个增量ΔX后，目标函数有最小值，故可以构造一个关于增量ΔX的增量公式，令增量的导数为0，可得如下式所示的增量公式，使用最小二乘法迭代求解出状态量X的最优解：

上式中，

＝为IMU预积分误差项的协方差，

为误差项r_B关于所有状态向量(即优化变量)X的Jacobian矩阵，

为视觉观测的误差协方差矩阵。

可将上式继续简化为：

(Λ_p+Λ_B+Λ_C)ΔX＝b_p+b_B+b_C (16)

其中，Λ_p，Λ_B，Λ_C为Hessian矩阵。

通过最小二乘法(Levenberg-Marquardt法)求解上述目标函数，可得当前时刻系统状态向量X的最优解，然后把系统的位置、姿态、速度保存下来，算法又开始接收下一时刻的数据，开始一轮新的迭代。

通过构建大小为5-20帧的滑动窗口，来减少冗余数据对算法影响，同时，通过IMU预积分的约束，使得当前有效数据不被丢失，保证了系统运行的稳定性；把边缘化的先验信息、IMU的测量残差和视觉的重投影误差同时纳入目标函数进行最小二乘优化，实现了视觉与惯性的紧组合，提高了系统的性能。

本发明算法使用视觉/惯性紧组合导航技术，对比传统视觉里程计，该视觉惯性里程计系统可以在高动态情况下稳定运行且有较好的性能水平；相比于基于松组合的视觉惯性里程计，本发明能够充分传感器的信息，在使用相同水平的传感器时发挥出更好的性能；定位精度可以达到分米级，无需依赖卫星信号。

本发明使用的单目相机特征点提取方法可以快速提取特征点，且不局限于可见光相机、红外相机、热成像相机等，只要可以提供灰度值，就能够有效提取特征点；同时使用单目相机，能有效降低系统成本。

本发明使用滑动窗口框架进行后端优化，可以有效降低因长时间运行导致冗余信息过多而造成的计算耗时增加问题，同时因为使用了IMU预积分作为图像帧之间的约束，不会丢失有效信息，提高了系统稳定性。

显然，本领域的技术人员应该明白，上述本发明的各模块或各步骤可以用通用的计算装置来实现，它们可以集中在单个的计算装置上，或者分布在多个计算装置所组成的网络上，可选地，它们可以用计算装置可执行的程序代码来实现，从而，可以将它们存储在计算机存储介质(ROM/RAM、磁碟、光盘)中由计算装置来执行，并且在某些情况下，可以以不同于此处的顺序执行所示出或描述的步骤，或者将它们分别制作成各个集成电路模块，或者将它们中的多个模块或步骤制作成单个集成电路模块来实现。所以，本发明不限制于任何特定的硬件和软件结合。

以上内容是结合具体的实施方式对本发明所作的进一步详细说明，不能认定本发明的具体实施只局限于这些说明。对于本发明所属技术领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干简单推演或替换，都应当视为属于本发明的保护范围。

Claims (9)

1.一种视觉惯性里程计系统，其特征在于，包括数据采集模块和数据处理模块，所述数据采集模块用关于采集图像信息、角速度和加速度；所述数据处理模块用于执行以下预处理和后端优化算法：

预处理：

对采集的所述图像信息、角速度和加速度进行时间同步对齐；

针对当前图像信息，提取图像Harris角点，跟踪相邻帧与所述当前图像信息的图像Harris角点匹配的特征点；

利用所述采集的角速度和加速度进行积分，得到当前时刻的位置速度旋转矩阵PVQ；

计算所述相邻帧的预积分增量，以及误差传递矩阵；

后端优化：

判断所述当前图像信息的特征点，与前一帧图像匹配的特征点之间的相对位移是否超过阈值，如是，判定所述当前图像信息为关键帧，边缘化滑动窗口内最老的图像帧，并将该关键帧存入所述滑动窗的最新位置；若否，则丢弃前一图像帧，并将所述当前图像信息和前一图像帧之间的预积分增量，转换为所述当前图像信息与前二图像帧之间的预积分增量；

构建由边缘化的先验信息、测量残差、视觉重投影误差构成的目标函数，通过优化目标函数来得到系统状态向量的最优解，所述系统状态向量包括系统的位置、姿态、速度、加速度计误差和、陀螺仪误差；基于最优解对应的系统状态向量得到导航信息。

2.如权利要求1所述的视觉惯性里程计系统，其特征在于，所述数据采集模块包括相机和惯性传感器，所述相机用于所述采集图像信息，所述惯性传感器用于所述采集角速度和加速度。

3.如权利要求1所述的视觉惯性里程计系统，其特征在于，所述利用所述角速度和加速度进行积分，得到当前时刻的位置速度旋转矩阵，包括通过如下积分运算得到：

所述

为k+1时刻世界坐标系下的的位置，所述

为k时刻的世界坐标系下的位置，所述

为k时刻世界坐标系下的的速度，所述

为k+1时刻世界坐标系下的的速度，所述Δt_k为k时刻到k+1时刻的时间差，所述

为Body自身坐标系到Word世界坐标系的旋转矩阵，所述

和

分别为所述采集的加速度和角速度，所述g^w为重力向量，所述

和所述

分别为加速度偏差和角速度偏差。

4.如权利要求1所述的视觉惯性里程计系统，其特征在于，所述计算所述相邻帧的预积分增量，通过如下预积分增量函数得到：

式中，所述

所述

所述

分别为式(1)中位置速度旋转矩阵的预积分增量真值；所述

所述

所述

分别为所述数据采集模块测量值得到的预积分增量；所述

所述

为位置P矩阵的误差预积分增量；所述

为速度V矩阵的误差预积分增量；所述

为转移Q矩阵的误差预积分增量。

5.如权利要求1-4任一项所述的视觉惯性里程计系统，其特征在于，所述目标函数包括：

所述‖r_p-J_pX‖²为边缘化的先验信息，所述

测量残差，所述

为视觉重投影误差。

6.如权利要求5所述的视觉惯性里程计系统，其特征在于，所述通过优化目标函数来得到系统状态向量的最优解包括：

当目标函数中的系统状态向量X有一个增量ΔX后，目标函数有最小值，构造一个关于增量ΔX的增量公式：

上式中，

为预积分误差项的协方差，

为误差项r_B关于所有状态向量X的Jacobian矩阵，

为视觉观测的误差协方差矩阵；

对上述增量公式进行简化：

(Λ_p+Λ_B+Λ_C)ΔX＝b_p+b_B+b_C (5)

其中，Λ_p，Λ_B，Λ_C为Hessian矩阵；

通过最小二乘法求解上述简化后的增量公式，可得当前时刻系统状态向量X的最优解。

7.如权利要求6所述的视觉惯性里程计系统，其特征在于，所述方法还包括：

在进行首次预处理之后，且在首次进行后端优化处理之前，还需要进行一次初始化处理：首先利用Global SFM算法，求得当前收到的所有帧的位姿和所有路标点在世界坐标系下的位置，求解重力方向、尺度因子、每一帧对应的速度及陀螺仪偏差，并根据计算得到的陀螺仪偏差，更新所述首次预处理计算得到的相邻帧的预积分增量。

8.如权利要求7所述的视觉惯性里程计系统，其特征在于，所述重力方向、尺度因子、每一帧对应的速度通过如下函数，利用LDLT分解方法求解得到：

9.如权利要求7所述的视觉惯性里程计系统，其特征在于，所述陀螺仪偏差通过构造如下函数，采用LDLT分解方法计算得到：

Images