CN107869989B

CN107869989B - 一种基于视觉惯导信息融合的定位方法及系统

Info

Publication number: CN107869989B
Application number: CN201711077826.8A
Authority: CN
Inventors: 刘腾飞; 张鹏
Original assignee: Northeastern University China
Current assignee: Northeastern University China
Priority date: 2017-11-06
Filing date: 2017-11-06
Publication date: 2020-02-07
Anticipated expiration: 2037-11-06
Also published as: CN107869989A

Abstract

本发明公开了一种基于视觉惯导信息融合的定位方法及系统，其中，所述方法包括：对采集的传感器信息进行预处理，所述传感器信息包括：深度视觉传感器的RGB图像和深度图像信息，惯性测量单元IMU数据；获取深度视觉传感器和惯性测量单元所属系统的外部参数；采用IMU预积分模型和深度相机模型处理所述预处理后的传感器信息、外部参数，获得位姿信息；基于回环检测方式对所述位姿信息进行校正，获得校正后的全局一致的位姿信息。上述方法在定位过程中鲁棒性好，提升定位准确性能。

Description

一种基于视觉惯导信息融合的定位方法及系统

技术领域

本发明涉及传感器融合技术，具体涉及一种基于视觉惯导信息融合的定位方法及系统。

背景技术

目前，视觉惯导融合技术广泛应用在三维重建、无人车和无人机的定位导航、自动驾驶等领域，旨在提供实时鲁棒精确的位置和姿态。主流的视觉惯导融合技术为基于滤波器方法。基于滤波器的方法主要采用卡尔曼滤波器及其变种，将惯导信息根据惯性测量单元的运动学模型得到系统状态向量的先验分布，在使用视觉传感器的观测模型更新卡尔曼增益，进而得到系统状态向量的后验分布。具体处理中，按照系统状态向量是否包含特征点坐标向量来分类，可分为松耦合和紧耦合两类。松耦合是指惯性测量单元和视觉传感器分别对系统状态向量进行估计，然后在进行融合。这样可以有效减小计算量，方便融合。紧耦合是指系统状态向量不仅包含惯性测量单元的状态量还包含有特征点坐标向量，统一进行状态估计。

基于滤波器的方法，无论采用松耦合或者紧耦合的方式，普遍存在的问题是无法有效消除累积误差，无法处理回环问题。

为此，业内提出一种基于优化的方法，基于优化的方法能够解决上述累积误差和无法处理回环的缺陷，但是，算法的鲁棒性比较低，无法推广使用。

特别地，当前基于优化的方法中的外参标定主要是采用kalibr工具箱，且使用kalibr工具箱只能离线标定且需要一个特制的标定板，不易操作，无法在线直接使用。

发明内容

针对现有技术中的问题，本发明提供一种基于视觉惯导信息融合的定位方法及系统。

第一方面，本发明提供一种基于视觉惯导信息融合的定位方法，包括：

步骤A1、对采集的传感器信息进行预处理，所述传感器信息包括：深度视觉传感器的RGB图像和深度图像信息，惯性测量单元IMU的数据；

步骤A2、获取深度视觉传感器和惯性测量单元所属系统的外部参数；

步骤A3、采用IMU预积分模型和深度相机模型处理所述预处理后的传感器信息、外部参数，获得位姿信息；

步骤A4、基于回环检测方式对所述位姿信息进行校正，获得校正后的位姿信息并输出该位姿信息。

可选地，所述步骤A1包括：

A11、采集所述深度视觉传感器输出的当前帧的RGB图像和深度图像，每一RGB图像对应一个深度图像，所述深度图像中的像素值表示RGB像素点距深度视觉传感器中相机光心的实际距离；

判断当前帧是否是深度视觉传感器输出的第一帧；

若是，则从当前帧的RGB图像中提取harris角点；

对提取的harris角点计算BRISK描述子，得到当前帧的特征点；

否则，采用LK光流跟踪算法在当前帧的RGB图像中查找与上一帧的RGB图像匹配的特征点；

确定查找的匹配的特征点数量是否等于预设阈值，若等于，则将查找的匹配的特征点作为当前帧的特征点，否则，从当前帧的RGB图像中提取新的harris角点，根据提取的harris角点，获取新的特征点，将获取的新的特征点和查找的匹配的特征点共同作为当前帧的特征点；

A12、采集所述惯性测量单元IMU的数据；

在当前帧为关键帧时，使用预积分模型计算方式，获取相邻关键帧之间IMU的状态增量。

可选地，所述A2包括：

查看所述深度视觉传感器和惯性测量单元所属系统的外部参数是否已经设定，若是，则在步骤A3中直接使用设定的外部参数；

否则，根据当前帧的RGB图像的特征点、上一帧的RGB图像的特征点，采用ICP算法，计算当前帧与上一帧之间的相对位姿；

根据所述位姿增量、所述相对位姿、外部参数之间的坐标变换关系、以及在所述设备启动后采集的相关数据构建超定方程，并采用最小二乘方式求解以获取外部参数。

可选地，所述A3包括：

根据IMU预积分模型计算测量值与系统状态的残差项和协方差；

根据深度相机模型计算特征点从前一个关键帧到当前关键帧去畸变后的重投影误差和协方差；

初始化滑动窗口，当滑动窗口为空时，则将所述残差项与所述重投影误差的马氏范数和加到滑动窗口中；

对滑动窗口中的残差范数构造非线性最小二乘式，通过高斯牛顿法迭代求解，得到系统状态向量的最优解；

边缘化滑动窗口中的残差项。

可选地，所述A4包括：

从当前关键帧提取fast角点计算BRISK描述子作为特征点并计算对应的BoW向量；

与词袋中的特征集进行匹配，判断是否存在回环；

若存在回环，计算构成回环的两个关键帧的相对位姿残差以及关键帧库中其他相邻两个关键帧的相对位姿残差，使用残差项马氏范数构造非线性最小二乘式，对关键帧库中的所有位姿进行全局优化；

若不存在回环，则将当前关键帧的位姿和特征点描述子存入关键帧库，并将特征点描述子的BoW向量存入词袋。

可选地，所述边缘化滑动窗口中的残差项，包括：

在当前帧为关键帧时，边缘化掉滑动窗口中最旧的一组残差范数；

在当前帧为非关键帧时，去掉当前帧的相机观测值，保留IMU测量值。

可选地，所述系统的状态向量为其中

表示滑动窗口中第k个状态中系统的机体在世界坐标系下的姿态、

表示机体在世界坐标系下的位置、

表示机体在世界坐标系下的速度、b_a表示惯性测量单元中加速度计的随机游走噪声、b_g表示惯性测量单元中陀螺仪的随机游走噪声；

表示所述深度视觉传感器与所述惯性测量单元的相对位置、

表示所述深度视觉传感器与所述惯性测量单元的相对姿态；

P₁ ^w,P₂ ^w,…,P_l ^w表示局部地图中的特征点在世界坐标系下的坐标。

第二方面，本发明还提供一种基于视觉惯导信息融合的定位系统，包括：

深度视觉传感器、惯性测量单元、处理器和存储器；

其中，所述深度视觉传感器和惯性测量单元均连接所述处理器，所述存储器存储有所述处理器的可执行指令；

所述处理器被配置为：

对采集的传感器信息进行预处理，所述传感器信息包括：深度视觉传感器的RGB图像和深度图像信息，惯性测量单元IMU的数据；

获取深度视觉传感器和惯性测量单元所属系统的外部参数；

采用IMU预积分模型和深度相机模型处理所述预处理后的传感器信息、外部参数，获得位姿信息；

基于回环检测方式对所述位姿信息进行校正，获得校正后的位姿信息。

第三方面，本发明还提供一种机器人，所述机器人包括上述任意的定位系统，其中所述深度视觉传感器位于所述机器人的头部，所述惯性测量单元位于所述机器人的心脏区域。

可选地，所述深度视觉传感器为深度相机；

所述惯性测量单元包括：三轴加速度计、三轴陀螺仪。

本发明具有的有益效果如下：

本发明中，可以在线实时标定系统外参，更加方便、易上手；使用轻量级的深度视觉传感器和惯导融合进行位姿估计，方便组装和拆卸，有效降低成本；

采用基于非线性优化的紧耦合方法，可有效克服基于滤波器方法固有的累积误差不易消除的缺点；

上述方法中增加回环检测模块，通过检测回环增加约束，可对各个时刻的系统状态进行全局优化，得到全局最优估计，有效减小累积误差；具有重定位功能，当某一时刻位姿估计失败，自动启动重定位功能，鲁棒性更好，也极大地提高了安全性能。

附图说明

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

图1为本发明一种实施例一种基于视觉惯导信息融合的定位系统整体框架示意图；

图2为本发明一种实施例的IMU预积分示意图；

图3为本发明一种实施例的深度视觉传感器针孔模型示意图；

图4为本发明一种实施例的初始边缘化操作示意图；

图5为本发明一种实施例的当缓存的图像帧中最旧的一帧不是关键帧时的边缘化操作示意图；

图6为本发明一种实施例的当缓存的图像帧中最旧的一帧不是关键帧时的边缘化操作示意图。

具体实施方式

为了更好的解释本发明，以便于理解，下面结合附图，通过具体实施方式，对本发明作详细描述。

在以下的描述中，将描述本发明的多个不同的方面，然而，对于本领域内的普通技术人员而言，可以仅仅利用本发明的一些或者全部结构或者流程来实施本发明。为了解释的明确性而言，阐述了特定的数目、配置和顺序，但是很明显，在没有这些特定细节的情况下也可以实施本发明。在其它情况下，为了不混淆本发明，对于一些众所周知的特征将不再进行详细阐述。

当前，基于非线性优化的方法则是分别计算惯导的测量残差和视觉传感器的重投影误差，通过最小化二者的残差和来得到系统状态的最优估计。

本申请中对于非线性优化的方法改进重点在于，充分利用深度相机的特性，在室内深度已知的情况下，减少计算量的同时也使系统的尺度估计更为准确；在室外无法测量深度的情况下，依然可以作为单目相机来使用，从而增强了算法的适应性。在上述方法中采用的在线标定外参的方式，计算处理更方便。

此外，采用局部地图的概念，通过增加局部地图与相机位姿之间的约束，有效减小局部累积误差，也可帮助建立更准确的地图。本实施例的局部地图可理解为下述滑动窗口中关键帧所包含特征点的世界坐标系下的坐标。

以下实施例中的系统具体是指深度视觉传感器和惯性测量单元所在的系统，即安装深度视觉传感器和惯性测量单元的机器人系统。在实际应用中，惯性测量单元位于机器人心脏中，深度视觉传感器位于机器人头部，为此在具体使用过程中，需要将机器人头部的深度相机和机器人心脏中的惯性测量单元的坐标系统一，并实时输出机器人的位姿。

本实施例的方法可应用于飞行机器人，可以估计六自由度位姿。

本申请方法估计的系统状态向量为

其中

表示滑动窗口中第k个状态下机体在世界坐标系下的姿态、

表示机体在世界坐标系下的位置、

表示机体在世界坐标系下的速度、b_a表示惯导加速度计的随机游走噪声、b_g表示惯导陀螺仪的随机游走噪声；

表示相机与惯导的相对位置、表示相机与惯导的相对姿态。

表示局部地图中的特征点在世界坐标系下的坐标。

以下实施例中的IMU的数据用于计算IMU的状态增量，状态增量包括位姿增量与速度增量。RGB图像的相对位姿增量可通过RGB图像匹配得到，二者通过外参建立约束关系。

另外，本实施例中的系统状态向量如上，在计算最后输出的位姿信息中使用系统状态向量的各个信息，最后输出校正后的位姿信息。

图1示出了一种基于视觉惯导信息融合的定位系统的整体框架，基于图1所示的定位系统的定位方法包括下述步骤：

本实施例中，深度视觉传感器可理解为指深度相机，每一帧都输出一张RGB图像和对应该RGB图像的一张深度图像，深度图像的像素和RGB图像的像素是一一对应的，该深度图像的像素值表示对应RGB图像中像素点距相机光心的实际距离。

所述惯性测量单元包括三轴加速度计、三轴陀螺仪的组合，即IMU。

第一步、对采集的传感器信息进行预处理

采集传感器输出的图像信息，对采集的图像信息提取特征点并跟踪，采集IMU数据，并对IMU数据进行预积分处理，获取IMU状态增量。

本实施例中传感器信息包括：深度视觉传感器的RGB图像和深度图像信息，惯性测量单元IMU的数据。

第一步主要包括以下子步骤：

S1、采集深度视觉传感器输出的当前帧的RGB图像和深度图像，由此可得到每个像素的像素坐标以及该像素在相机坐标系下的深度信息；

举例来说，本实施例中记第i个关键帧的第l个像素的像素坐标使用齐次坐标表示为

对应的深度信息表示为

S2、判断当前帧是否是深度视觉传感器输出的第一帧，若是第一帧，则直接提取harris角点并计算BRISK描述子作为特征点；

若不是第一帧，则先使用LK光流跟踪算法在当前帧的RGB图像中找出与上一帧的RGB图像匹配的特征点，判断特征点数量若低于阈值，则提取新特征点，以补充特征点数量。

例如，从步骤S1中采集的RGB图像中提取100个关键点，并计算描述子；

举例来说，本实施例中采用harris角点检测算法提取harris角点，并计算BRISK描述子，从而得到一组特征点

可选地、使用LK光流跟踪法在当前帧中跟踪到与上一帧中的特征点匹配的特征点，当匹配到的特征点不足100的时候，在使用步骤S2中的方法提取新的特征点以凑足100个特征点；

举例来说，本实施例中使用LK光流跟踪法跟踪到的一组匹配特征点记为

当n＝100时，则继续下一步；当n<100时，则使用步骤S2中的方法提取100-n个新的特征点。

S3、采集IMU数据，以及在当前帧为关键帧时，使用预积分模型计算方式，获取相邻关键帧之间IMU的状态增量。

举例来说，IMU数据为IMU输出的加速度和角速度信息等，状态增量包括位姿增量和速度增量。

另外，需要说明的判断当前帧是否是关键帧，计算当前帧与上一关键帧的视差，当视差大于阈值时，则认为当前帧为关键帧；或者，判断当前图像帧匹配的特征点数量，是否小于阈值，当小于阈值时，则把当前图像帧记为关键帧。

如图1和图2所示，本实施例中使用的深度相机的采样频率为30HZ,关键帧的采样频率更低，IMU的采样频率为100HZ，IMU的采样间隔记为Δt；则在相邻两个关键帧i，j之间存在多个IMU的采样值,输出的加速度和角速度信息记为

采用预积分模型，计算以i时刻IMU位姿为参考坐标系的IMU状态增量分别为

其中，IMU状态增量包括位姿增量和速度增量，IMU状态量包括位姿、速度以及随机噪声。

将公式(1)中的高斯噪声η_gk、η_ak分离出来，并保持随机噪声b_g、b_a不变，则有下述的公式(2)：

其中，i、j表示相邻的两个关键帧，关键帧之间包含多个IMU数据，k表示i、j之间的IMU数据序号，R表示IMU在世界坐标系下姿态的旋转矩阵，

R_j分别表示i时刻姿态的逆矩阵和j时刻的姿态，Exp表示李代数的指数映射，

为IMU角速度测量值，b_g、η_g分别表示陀螺仪随机噪声与高斯噪声，Δt表示IMU采样时间，Δt_ij表示相机采样时间，v表示速度，g为重力加速度，为IMU加速度测量值，b_a、η_a分别表示加速度计随机噪声与高斯噪声，p表示位置。

第二步、初始化

该步骤主要用于获取深度视觉传感器和惯性测量单元所属系统的外部参数即系统外参。

也就是说，用于计算得到IMU和深度视觉传感器之间的相对位姿，即获得系统外参。

具体步骤如下：

M1、判断系统外参是否已知(可在外部手动设置)，外参可由外部给定初始值，若已知则转到第三步的后端非线性优化；

若初始值未知，则转到步骤M2；

M2、根据当前帧的RGB图像的特征点、上一帧的RGB图像的特征点，采用ICP算法，计算当前帧(图像帧)与上一帧(图像帧)之间的相对位姿；如图3所示。

也就是说，子步骤M2为：由传感器信息预处理得到相邻两个图像帧匹配的特征点，使用ICP算法计算两帧之间的相对位姿。

例如，M21、使用ICP算法计算相邻两个关键帧的相对位姿，本实施例中记为

M22、前述第一步的S3中IMU预积分求得的位姿增量为

并且系统外参(待求解量)表示为

则存在如下坐标变换关系：

M23、展开步骤M22中等式：

M24、将步骤M23中的(3)式使用四元数表示，并利用四元数的性质可转换为：

其中，相对姿态为：

q＝[x y z w]

M25、对已有数据构建如下所示的超定方程组：

对超定方程组求解最小二乘解

即

的最小特征值对应的特征向量；

M26、判断步骤M25中解是否收敛，若未收敛则继续加入新的测量值，直到得到收敛解；

M27、将步骤M26中求得的

转换为旋转矩阵形式代入到步骤M4中的(4)式求得相对位置

如上求解的系统外参，该系统外参包括相对姿态和相对位置。

第三步、后端非线性优化

具体地，采用IMU预积分模型和深度相机模型处理所述预处理后的传感器信息、外部参数，获得位姿信息。

即，用于对传感器测量值和系统状态向量建立最小二乘式，并使用优化方法，迭代求解出系统状态的最优值。

具体步骤如下：

N1、根据IMU预积分模型计算测量值与系统状态的残差项(即测量残差)和协方差。

根据IMU预积分模型计算测量残差和协方差；

在本实施例中，IMU的残差计算公式为：

协方差迭代计算公式为：

N2、根据深度相机模型计算特征点从前一个关键帧到当前关键帧去畸变后的重投影误差和协方差。

具体地，重投影误差计算如下：

3D-2D重投影误差：

表征局部地图点与滑动窗口中的相机位姿之间的约束关系。

其中，n表示滑动窗口中的第n个状态，表示从IMU坐标系到相机坐标系的位姿变换，表示从世界坐标系到当前IMU坐标系的位姿变换，P_l ^w表示局部地图中第l个地图点在世界坐标系下的坐标，表示第l个地图点在当前相机坐标系下的坐标。

其中K为相机内参矩阵，d为该地图点在相机坐标系下的深度。

上述的相机为深度传感器的深度相机，其他实施例的相机含义相同。

2D-2D重投影误差：

表征相邻两个关键帧之间的位姿约束关系。

所述第l个特征点从第i帧到第j帧的重投影误差计算公式为：

协方差矩阵默认为单位矩阵。

单位矩阵就是对角线元素均为1，其他元素均为0的矩阵，单位矩阵的性质是任何矩阵与其的乘积均为原矩阵。

N3、初始化滑动窗口；当滑动窗口为空时，则将由上述两步计算得到的残差项的马氏范数和直接加到滑动窗口中；不空则转到步骤N4。

该步骤中使用的马氏范数形式如下：

也就是说，当滑动窗口为空时，将所述残差项与所述重投影误差(即相机的测量残差)的马氏范数和加到滑动窗口中。

本实施例中的r是残差，p为协方差，且在使用的马氏范数中包含有前面步骤的协方差。

N4、对滑动窗口中的残差范数构造非线性最小二乘式，使用高斯牛顿法求解系统状态变量的最优估计。

系统的状态量在前述已经说明，可包括世界坐标系下的位置、姿态、速度、陀螺仪随机噪声、加速度计随机噪声、外参以及局部地图点的世界坐标下的坐标等。

上述非线性最小二乘式形式如下：

其中为IMU的测量残差，

为深度相机的测量残差，B表示IMU的测量数据集，C表示深度相机的测量数据集，M表示局部地图点数据集，表示在机体坐标系下滑动窗口中从第i个关键帧到第j个关键帧之间的状态变化量，

表示在相机坐标系下滑动窗口中第j个关键帧中第l个特征点对应的状态量。

N5、边缘化滑动窗口中的残差项；具体地，当前帧若为关键帧则边缘化掉滑动窗口中最旧的一组残差范数，否则直接去掉当前帧的相机观测值，但保留IMU测量值。

本实施例中处理的均是关键帧，判断当前帧是否为关键帧，可通过计算当前帧与上一个关键帧的视差，当视差大于阈值则认为当前帧为关键帧。或者，判断当前图像帧匹配的特征点数量，是否小于阈值，当小于阈值时，则把当前图像帧记为关键帧。

即，对于一个图像帧中的特征点在像素坐标系中的坐标为u1,v1；该特征点在另一个图像帧的坐标为u2,v2；则该特征点在两个图像帧中的视差定义为

举例来说，本实施例中采用的滑动窗口中的关键帧的规模为3，缓存的图像帧的规模为3。如图4、图5、图6所示，边缘化分为三种情况。

详细介绍如下：

如图4所示，初始时，当滑动窗口刚好有3个关键帧并且缓存空间有3个图像帧，则将开始3帧的IMU预积分对应的速度和随机噪声项边缘化掉；

如图5所示，当采集到新的图像帧，缓存空间中的最旧的图像帧不是关键帧，则将该帧对应的相机观测值和状态量

以及IMU预积分中的速度和随机噪声项

边缘化掉；

如图6所示，当采集到新的图像帧，缓存空间中的最旧的图像帧是关键帧，则将该帧移入滑动窗口，并将滑动窗口中最旧的关键帧

和在中可见而在最新的关键帧

或当前图像帧

中不可见的特征点以及最新关键帧对应的IMU预积分中的速度和随机噪声项

边缘化掉；

第四步、回环检测

本实施例中基于回环检测方式对所述位姿信息进行校正，获得校正后的位姿信息并输出该位姿信息，即用于检测系统轨迹是否存在闭环，当存在闭环时，则对系统状态向量中的位姿进行全局优化。

具体步骤如下：

Q1、从当前帧提取fast角点并计算BRISK描述子作为特征点；例如在当前帧v中，提取500个fast角点，并计算BRISK描述子以及对应的BoW向量。

应说明的是，前面提取的harris角点与fast角点不同，对环境的适应性更强，一般用作跟踪；fast角点计算快，数量多，可用于回环检测。

Q2、与词袋中的特征集进行匹配，判断是否存在回环；

Q3、若存在回环，计算构成回环的两个关键帧的相对位姿残差以及关键帧库中其他相邻两个关键帧的相对位姿残差，使用残差项L2范数构造非线性最小二乘式，对关键帧库中的所有位姿进行全局优化；

也就是说，若存在回环(即当前帧计算得到的描述子与词袋中的描述子匹配成功)，则根据匹配的描述子从关键帧库中提取对应的关键帧位姿数据，在实施例中和v构成回环的关键帧为m；

Q4、由步骤S3得到当前关键帧的位姿。在本实施例中当前关键帧的位姿以及其回环匹配到的关键帧的位姿分别记为

且满足

本实施例中，相对位姿与位姿增量是等价的概念，一般称相机位姿之间为相对位姿，IMU数据经预积分得到的是位姿增量。

Q5、计算关键帧库位姿残差，建立位姿图，进行位姿全局优化；步骤如下：

构造非线性最小二乘式：

其中L表示所有与回环相关的关键帧集合，S为其他关键帧集合，r为关键帧之间的位姿残差，h为鲁棒核函数，在本实施例中使用huber核函数。

r的计算公式为

使用高斯牛顿法求解上述最小二乘问题。

转到第一步，处理下一帧。

Q6、若不存在回环，则将当前帧的位姿和特征点的描述子存入关键帧库，特征点描述子的BoW向量存入词袋中。转到步骤1，处理下一帧。

为更好的理解回环检测，补充解释如下：

回环检测，采用词袋模型，对每一个关键帧，提取500个fast角点，然后对每一个角点计算BRISK描述子，在利用描述子计算唯一的BOW向量，构建特征词袋。对于最新的关键帧，提取特征点计算描述子BOW向量，然后与特征词袋中的向量进行相似性评分，当评分超过阈值时则认为当前关键帧与匹配关键帧构成回环。

关键帧库，回环检测模块中维护了一个关键帧库，其中包含每一个关键帧优化后的位置和姿态信息、BRISK描述子以及与前一个关键帧的相对位姿。当检测到回环后，通过从特征词袋中匹配的关键帧的描述子信息，可以在关键帧库中找到和该关键帧对应的位姿信息。

全局位姿图优化，将关键帧库中的关键帧分为两类，一类为构成回环的关键帧，记构成回环的两个关键帧对应的位姿分别记为相对位姿记为

I₃为三阶单位矩阵；一类为普通连续关键帧，记相邻两个关键帧i，j对应的位姿为T_i ^w、

保存的ij之间的相对位姿为

计算位姿残差：

构造最小二乘问题优化函数：

通过高斯牛顿法迭代求解得到关键帧库中所有关键帧位姿的最优值。

关键帧，图2示出，相邻两个关键帧之间存在很多个图像帧，图像帧即是相机实时采集的图像数据，由于相机的采集频率比较高(一般为30HZ)，所以相邻两个图像帧之间的匹配特征点比较多，但是这样并没有对计算结果的准确性带来明显的提高，反而增大了计算量，因此我们采用关键帧策略，在保证计算准确性同时，减少计算量。

关键帧的选择方式有：

1)计算当前图像帧与上一个关键帧每个匹配特征点视差，然后计算平均视差，当视差大于阈值时，则把当前图像帧记为关键帧；

2)判断当前图像帧匹配的特征点数量，是否小于阈值，当小于阈值时，则把当前图像帧记为关键帧。

根据本发明的另一方面，本发明还提供一种基于视觉惯导信息融合的定位系统，该定位系统包括：深度视觉传感器、惯性测量单元、处理器和存储器；

所述处理器被配置为：

获取深度视觉传感器和惯性测量单元所属系统的外部参数；

再一方面，本发明还提供一种机器人，所述机器人包括上述任意所述的定位系统，其中所述深度视觉传感器位于所述机器人的头部，所述惯性测量单元位于所述机器人的心脏区域。

本实施例中所述深度视觉传感器为轻量级的深度相机；所述惯性测量单元包括：三轴加速度计、三轴陀螺仪。

采用基于非线性优化的紧耦合方法，可有效克服基于滤波器方法固有的累积误差不可有效抑制的缺点；例如前述的后端优化用于减小局部累积误差、回环检测部分用于减小全局的累积误差。

最后应说明的是：以上所述的各实施例仅用于说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述实施例所记载的技术方案进行修改，或者对其中部分或全部技术特征进行等同替换；而这些修改或替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的范围。

Claims

1.一种基于视觉惯导信息融合的定位方法，其特征在于，包括：

步骤A4、基于回环检测方式对所述位姿信息进行校正，获得校正后的位姿信息并输出；

所述系统的外部参数为惯性测量单元和深度视觉传感器之间的相对位姿；

所述处理后的传感器信息包括：传感器测量值和系统状态向量；

所述A3具体包括：

边缘化滑动窗口中的残差项；

所述传感器测量值为IMU积分得到的位置、姿态和速度以及使用LK光流跟踪法在当前帧中跟踪到与上一帧中的特征点匹配的特征点。

2.根据权利要求1所述的方法，其特征在于，所述步骤A1包括：

判断当前帧是否是深度视觉传感器输出的第一帧；

若是，则从当前帧的RGB图像中提取harris角点；

对提取的harris角点计算BRISK描述子，得到当前帧的特征点；

A12、采集所述惯性测量单元IMU的数据；

3.根据权利要求2所述的方法，其特征在于，所述A2包括：

根据所述位姿增量、所述相对位姿、外部参数之间的坐标变换关系、以及在所述系统启动后采集的相关数据构建超定方程，并采用最小二乘方式求解以获取外部参数；

所述外部参数为惯性测量单元和深度视觉传感器之间的相对位姿；

所述系统启动后采集的相关数据包括：深度视觉传感器的RGB图像和深度图像信息和IMU输出的加速度和角速度信息。

4.根据权利要求3所述的方法，其特征在于，所述A4包括：

与词袋中的特征集进行匹配，判断是否存在回环；

5.根据权利要求4所述的方法，其特征在于，所述边缘化滑动窗口中的残差项，包括：

6.根据权利要求1至5任一所述的方法，其特征在于，所述系统的状态向量为

其中

表示机体在世界坐标系下的位置、

表示所述深度视觉传感器与所述惯性测量单元的相对位置、表示所述深度视觉传感器与所述惯性测量单元的相对姿态；

表示局部地图中的特征点在世界坐标系下的坐标。

7.一种基于视觉惯导信息融合的定位系统，其特征在于，包括：

深度视觉传感器、惯性测量单元、处理器和存储器；

所述处理器被配置为：

获取深度视觉传感器和惯性测量单元所属系统的外部参数；

基于回环检测方式对所述位姿信息进行校正，获得校正后的位姿信息；

所述采用IMU预积分模型和深度相机模型处理所述预处理后的传感器信息、外部参数，获得位姿信息，具体包括：

边缘化滑动窗口中的残差项；

8.一种机器人，其特征在于，所述机器人包括权利要求7所述的定位系统，其中所述深度视觉传感器位于所述机器人的头部，所述惯性测量单元位于所述机器人的心脏区域。

9.根据权利要求8所述的机器人，其特征在于，所述深度视觉传感器为深度相机；

所述惯性测量单元包括：三轴加速度计、三轴陀螺仪。