CN113570667A

CN113570667A - 视觉惯导补偿方法、装置及存储介质

Info

Publication number: CN113570667A
Application number: CN202111133280.XA
Authority: CN
Inventors: 刘宁; 节笑晗; 袁超杰; 苏中; 刘福朝; 赵旭; 赵辉
Original assignee: Beijing Information Science and Technology University
Current assignee: Beijing Information Science and Technology University
Priority date: 2021-09-27
Filing date: 2021-09-27
Publication date: 2021-10-29
Anticipated expiration: 2041-09-27
Also published as: CN113570667B

Abstract

本发明公开了一种视觉惯导补偿方法、装置及存储介质。其中，该方法包括：在SIFT算子追踪丢失的情况下，将惯性传感器IMU初始化，获取所述IMU的帧间IMU数据，而无需重新启动相机；基于所述帧间IMU数据进行姿态解算，以获取惯导的位姿估计，并对所述惯导的位姿估计进行融合，得到融合后的姿态；根据所述融合后的姿态更新位姿信息与地图，以对所述视觉惯导进行补偿。本发明解决了由于重启相机而造成的计算量大的技术问题。

Description

视觉惯导补偿方法、装置及存储介质

技术领域

本发明涉及视觉导航领域，具体而言，涉及一种视觉惯导补偿方法、装置及存储介质。

背景技术

基于同步定位与建图（SLAM, Simultaneous Localization and Mapping）的技术被应用于多个机器人等导航领域。通过实时的定位与建图可以提供丰富的环境信息和地图信息，有利于无人车、机器人等对环境的认识与感知。

虽然，现在存在的ORB算法的特征匹配效果还是能够令人满意的，而且ORB算法具有稳定的旋转不变性。然而，ORB算法在尺度方面结果不佳，在增加算法尺度变换后仍然没有取得好的结果，并且ORB算法特征描述是二进制字符串，会造成很严重的不匹配的情况，以及在建图过程中很容易出现追踪丢失的情况。

针对上述的问题，目前尚未提出有效的解决方案。

发明内容

本发明实施例提供了一种视觉惯导补偿方法、装置及存储介质，以至少解决由于重启相机而造成的计算量大的技术问题。

根据本发明实施例的一个方面，提供了一种视觉惯导补偿方法，所述方法包括：在SIFT（尺度不变特征变换）算子追踪丢失的情况下，将惯性传感器IMU初始化，获取所述IMU的帧间IMU数据，而无需重新启动相机；基于所述帧间IMU数据进行姿态解算，以获取惯导的位姿估计，并对所述惯导的位姿估计进行融合，得到融合后的姿态；根据所述融合后的姿态更新位姿信息与地图，以对所述视觉惯导进行补偿；在SIFT算子追踪未丢失的情况下，基于相机读取的帧间图像和惯性传感器读取的帧间IMU数据提取关键帧和特征点，并基于所述关键帧和所述特征点将相应的新的数据加入到地图中，并更新地图，以进行视觉导航。

根据本发明实施例的另一方面，还提供了一种视觉惯导补偿装置，其特征在于，包括：SIFT算子丢失补偿模块，被配置为在SIFT算子追踪丢失的情况下，将惯性传感器IMU初始化，获取所述IMU的帧间IMU数据，而无需重新启动相机；基于所述帧间IMU数据进行姿态解算，以获取惯导的位姿估计，并对所述惯导的位姿估计进行融合，得到融合后的姿态；根据所述融合姿态更新位姿信息与地图，以对所述视觉惯导进行补偿；SIFT算子未丢失补偿模块，被配置为在SIFT算子追踪未丢失的情况下，基于相机读取的帧间图像和惯性传感器读取的帧间IMU数据提取关键帧和特征点，并基于所述关键帧和所述特征点将相应的新的数据加入到地图中，并更新地图，以进行视觉导航。

在本发明实施例中，在sift丢失的情况下，用IMU代替SIFT，不再重启SIFT，从而实现了计算量减小、导航更快速的技术效果，进而解决了由于重启相机而造成的计算量大的技术问题。

附图说明

此处所说明的附图用来提供对本发明的进一步理解，构成本申请的一部分，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。在附图中：

图1是根据本发明实施例的视觉惯导补偿方法的流程图一；

图2是根据本发明实施例的视觉惯导补偿方法的流程图二；

图3是根据本发明实施例的视觉惯导补偿方法的流程图三；

图4是根据本发明实施例的视觉惯导补偿方法的流程图四；

图5是根据本发明实施例的视觉惯导补偿装置的结构示意图；

图6是根据本发明实施例的视觉惯导补偿系统的结构示意图。

具体实施方式

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

需要说明的是，本发明的说明书和权利要求书及上述附图中的术语“第一”、“第二”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的本发明的实施例能够以除了在这里图示或描述的那些以外的顺序实施。此外，术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含，例如，包含了一系列步骤或单元的过程、方法、系统、产品或设备不必限于清楚地列出的那些步骤或单元，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。

实施例1

根据本发明实施例，提供了一种视觉惯导补偿方法，如图1所示，该方法包括：

步骤S102，在SIFT算子追踪丢失的情况下，用IMU代替SIFT，不再重启SIFT。

在SIFT算子追踪丢失的情况下，将惯性传感器IMU初始化，获取所述IMU的帧间IMU数据，而无需重新启动相机；基于所述帧间IMU数据进行姿态解算，以获取惯导的位姿估计，并对所述惯导的位姿估计进行融合，得到融合后的姿态；根据所述融合后的姿态更新位姿信息与地图，以对所述视觉惯导进行补偿。

步骤S104，在SIFT（Scale Invariant Feature Transform，尺度不变特征变换）算子追踪未丢失的情况下，使用SIFT算子确定特征值，以进行视觉导航。

在SIFT算子追踪未丢失的情况下，在一个示例性实施例中，基于相机读取的帧间图像和惯性传感器读取的帧间IMU数据提取关键帧和特征点，并基于所述关键帧和所述特征点将相应的新的数据加入到地图中，并更新地图，以进行视觉导航。例如，基于相机读取的帧间图像和上一帧图像的光流追踪结果，利用SIFT算子提取特征点，以计算所述帧间图像的位姿。

例如，基于相机读取的帧间图像和上一帧图像的光流追踪结果，计算所述帧间图像的位姿；基于惯性传感器读取的帧间IMU数据进行惯性传感器的位姿估计，并将所述帧间图像的位姿与所述惯性传感器的位姿估计进行融合，以提取所述关键帧和特征点。

在一个示例性实施例中，利用SIFT算子提取特征点可以通过如下方式实现：

（1）检测尺度空间极值，基于所述尺度空间极值获取关键点；

将所述帧间图像与每层尺度的高斯滤波器进行卷积确定所述帧间图像的尺度空间；对所述尺度空间中的每层尺度的图像做模糊处理；对每层尺度的图像做降采样，得到一组大小不一的图像；利用DoG（Difference of Gaussians，高斯函数的差分），来比较同组中的两个相邻图像来确定所述关键点。

（2）将所述关键点进行本地化，并为所述关键点分配一个或多个方向；

对尺度空间DoG函数进行曲线拟合，得到极值点的偏移量，在所述偏移量小于偏移阈值的情况下，确定所述关键点为真实关键点；利用高斯差分函数确定所述关键点的曲率，在所述曲率小于曲率阈值的情况下，确定所述关键点为稳定的真实关键点。

确定所述关键点的梯度，使用方向直方图统计邻域内像素的梯度和方向，其中，所述方向直方图的峰值代表所述关键点处邻域梯度的方向；以所述方向直方图中最大值作为所述关键点的主方向。

（3）基于所述一个或多个方向，确定所述关键点的描述符，并基于所述关键点的描述符确定所述特征点。

计算在所述关键点位置周围的区域中的每个图像采样点处的梯度幅度和取向来创建所述关键点的描述符，作为所述SIFT特征描述向量；采用所述SIFT特征描述向量的欧式距离来作为所述关键点的相似性判定度量。

需要说明的是，对于前述的各方法实施例，为了简单描述，故将其都表述为一系列的动作组合，但是本领域技术人员应该知悉，本发明并不受所描述的动作顺序的限制，因为依据本发明，某些步骤可以采用其他顺序或者同时进行。其次，本领域技术人员也应该知悉，说明书中所描述的实施例均属于优选实施例，所涉及的动作和模块并不一定是本发明所必须的。

通过以上的实施方式的描述，本领域的技术人员可以清楚地了解到根据上述实施例的方法可借助软件加必需的通用硬件平台的方式来实现，当然也可以通过硬件，但很多情况下前者是更佳的实施方式。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质（如ROM/RAM、磁碟、光盘）中，包括若干指令用以使得一台终端设备（可以是手机，计算机，服务器，或者网络设备等）执行本发明各个实施例所述的方法。

实施例2

根据本发明实施例，提供了一种视觉惯导补偿方法。

首先，对相机和IMU进行初始化。

插入图像帧，提取图像特征，与上一帧进行光流追踪，通过光流结果计算该帧的位姿。当效果不理想时，会补充新的特征点进行双目左右两个摄像头的匹配，并作三角化，将新的位姿信息加入地图，并触发后端的回环优化。

前端处理的结果将作为后端优化的初始值，后端在拿到处理之后的关键帧和路标点，将其进行检查更新优化，再返回优化结果。控制优化问题的规模在一定范围内，不会随时间一直增长，控制计算量，之后返回更新加入地图。

如果发生追踪丢失的现象，将IMU进行初始化，提取IMU的帧间状态进行姿态演算，不再重新启动相机部分，之后将惯导的位姿估计进行融合，根据融合姿态更新位姿信息与地图。

前后端在分别的线程中处理数据，前端提取了关键帧后，在地图中加入新的数据；后端检测到地图更新时，运行一次优化，把地图中旧的关键帧和地图点去掉，以保持稀疏地图的规模。

在提取特征点的部分用SIFT算法，以下是用于生成图像特征集的计算的主要阶段，包括以下四个步骤：

步骤S202，尺度空间极值检测。

第一阶段的计算主要通过使用高斯差分函数有效地识别不规则尺度和方向的潜在特征点，并且检测所有尺度和图像位置。

步骤S204，关键点本地化。

在每个待选的位置上，建立一个适合确定位置和尺度的详细模型。根据其稳定性的测量来判断关键点。

步骤S206，方向分配。

每个关键点位置可能会分配一个或多个方向，其准则基于局部图像梯度方向。在执行接下来的操作时，每个特征的分布方向、比例和位置都是独一无二的，从而提供不变性的基础条件。

步骤S208，关键点描述符。

用选定的比例尺度度量每个关键点周围的区域的局部图像梯度。这些被转化为代表性，允许显着水平的局部形状失真和光照变化。

实施例3

本发明实施例提供了另一种用SIFT算法提取特征点的方法，如图3所示，该方法包括以下步骤：

步骤S302，确定尺度空间。

将一个被视为尺度的参数引入到图像信息处理模型中，通过连续变化尺度参数获得多尺度下的尺度空间表示序列，对这些序列进行尺度空间主轮廓的提取，并以该主轮廓作为一种特征向量，实现边缘、角点检测和不同分辨率上的特征提取等。

首先检测在SIFT框架中被称为关键点的兴趣点。该图像与每层尺度的高斯滤波器进行卷积，然后采用连续的高斯模糊图像的差异。

一个图像的尺度空间

表示为一个变化尺度的高斯函数

与原图像空间坐标I(x,y)的卷积。其中，（

，

）为空间坐标，p为尺度坐标。

其中，*表示卷积运算，

其中，m，n表示高斯模板的维度(由

确定)，G表示尺度可变高斯函数，大尺度对应于图像的概貌特征，小尺度对应于图像的细节特征。

尺度空间由高斯金字塔表达实现，建立高斯金字塔的步骤如下：

（1）在每层尺度的图像做模糊处理；

（2）对图像做降采样。

影像的高斯金字塔模型是指：将初始图像不断进行降阶采样，得到一组大小不一的图像，由大到小，从上到下构成的塔状模型。原图像为金字塔的初层，每步降采样所得到的新图像为金字塔的一层(每层一张图像)，每个金字塔共n层。金字塔的层数根据图像的初始大小和塔最上层图像的大小一起决定，其计算式子如下：

其中M，N为原图像的大小,t为塔顶图像的最小维数的对数值。例如，一个大小为216*216的图像，金字塔上各层图像的大小如下表所示，当塔顶图像为8*8时，n=6，当塔顶图像为4*4时，n=7。

如下所示的表1示出了 216*216图像金字塔顶层图像大小与层数的关系。

图像大小	216	128	64	16	8	4	2	1
									金字塔层数	2	3	4	5	6	7	8	9

表1

关键点是由DoG空间的局部极值点组成。关键点的初步探索是通过比较同组中的两个相邻图像来完成的。为了找到DoG功能的极限点，将每个像素与其所有相邻点进行比较，以查看其是否大于或小于图像和比例场中的相邻点。初始图像与高斯增量卷积，以产生由尺度空间中的常数因子k分隔的图像。选择将尺度空间的每个角度（即，p的加倍）划分为整数s的间隔。我们必须在每个八度的模糊图像堆栈中生成s+3图像，以便最终的极值检测覆盖一个完整的角度。相邻的图像尺度被减去以产生高斯差分图像。一旦完成了一个完整的角度，我们重新取样高斯图像，它的两倍于初始值p（它将是来自堆栈顶部的2个图像），取每行和每列的第二个像素。

当然，由于某些极端点响应较弱，所以极限点并不是所有稳定的特征点，而且DoG运算符将具有较强的边缘响应。

步骤S304，关键点本地化。

对位置，尺度和主曲率的比率进行详细的分析。拒绝具有低对比度的点或者沿着边缘不太局部化的点。关键点的位置和尺度通过拟合三维二次函数精确确定，低对比度和不稳定边缘响应点的关键点被去除，DoG算子将具有强边缘响应，以增强匹配稳定性，提高抗噪声能力。

为了提高关键点的稳定性，需要对尺度空间DoG函数进行曲线拟合。利用DoG函数在尺度空间的Taylor展开式(拟合函数)为：

其中，

，

为三维矢量，表示相对于插值中心点的偏移量，D表示尺度空间函数,T是数学运算符号。求导并让方程等于零，可以得到极值点的偏移量为：

其中，

表示X的极值。对应极值点，方程的值为：

其中，

代表相对插值中心的偏移量，当它在任一维度上的偏移量大于0.5时（即x或y或

），意味着插值中心已经偏移到它的邻近点上，所以必须改变当前关键点的位置。同时在新的位置上反复插值直到收敛；也有可能超出所设定的迭代次数或者超出图像边界的范围，此时这样的点应该删除，在一些示例中，可能需要进行5次迭代。另外，

过小的点易受噪声的干扰而变得不稳定，所以将

小于某个经验值（在一个示例中，可使用0.03，或0.04/S）的极值点删除。同时，在此过程中获取特征点的精确位置（原位置加上拟合的偏移量）以及尺度

和

。

为了稳定性，拒绝低对比度的关键点是不够的。高斯差分函数将沿着边缘具有很强的响应，即使沿着边缘的位置确定不良，因此对于少量的噪声是不稳定的。高斯差分函数中的一个不太明确的峰值将在边缘上具有较大的主曲率，而在垂直方向上将具有较小的主曲率。主曲率可以从2×2的Hessian矩阵H计算出来，该矩阵在关键点的位置和尺度上计算：

H的特征值

和

代表x和y方向的梯度，H表示Hessian矩阵，

表示X方向的二阶偏导数，

表示XY方向的二阶导数，

表示Y方向的二阶偏导数。

表示矩阵H对角线元素之和，

表示矩阵H的行列式，H的特征值

和

代表x和y方向的梯度。假设是

较大的特征值，而是

较小的特征值，令

，其中，

表示较大特征值与较小特征值的比值，则

D的主曲率和H的特征值成正比，令α为最大特征值，β为最小的特征值，则公式

的值在两个特征值相等时最小，随着的增大而增大。值越大，说明两个特征值的比值越大，即在某一个方向的梯度值越大，而在另一个方向的梯度值越小，而边缘恰恰就是这种情况。这仅取决于特征值的比率而不是它们的个体值。当两个特征值相等时，数量（r +1）2/r为最小值，并且随r增加。因此，为了检查主曲率的比例低于某个阈值，r需要检查

上式成立则关键点为真实，不成立就舍弃这个点。

步骤S306，方向分配。

通过基于局部图像特性为每个关键点指定一致的方向，关键点描述符可以相对于该方向被表示，并且因此可以用于描述变换。

梯度的模值和方向如下：

其中，（

，

）为坐标，

为梯度的方向，L为关键点所在的尺度空间值，梯度的模值m(x,y)按

=1.5

的高斯分布加成，按尺度采样的3p原则，邻域窗口半径为3

1.5

。

在完成关键点的梯度计算后，使用直方图统计邻域内像素的梯度和方向。梯度直方图将0~360度的方向范围分为36个柱(bins)，其中每柱10度。

方向直方图的峰值则代表了该特征点处邻域梯度的方向，以直方图中最大值作为该关键点的主方向。方向直方图中的峰值对应于局部梯度的主要方向。因此，对于多个像素的几何像素，几何像素的位置和尺度不同。只有约15％的点被分配多个方向，但这些对匹配的稳定性有重要作用。最后，抛物线不适用于最近距离较近的3个直方图值，以确保更好的准确度。在一个优选的实施例中，把该关键点复制成多份关键点，并将方向值分别赋给这些复制后的关键点，并且，离散的梯度方向直方图要进行插值拟合处理，来求得更精确的方向角度值。

至此，将检测出的含有位置、尺度和方向的关键点即是该图像的SIFT特征点。

步骤S308，生成关键点描述符。

首先计算在关键点位置周围的区域中的每个图像采样点处的梯度幅度和取向来创建关键点描述符。它们由高斯窗口加权，由覆盖的圆圈表示。然后将这些样本累积到总结4x4子区域内容的定向直方图，每个箭头的长度对应于该区域内该方向附近的梯度大小之和。

在一个实施例中，描述子使用在关键点尺度空间内4*4的窗口中计算的8个方向的梯度信息，共4*4*8=128维向量表征。

至此，SIFT特征描述向量生成。

接下来就是SIFT之间的匹配过程。

为了使SIFT运算量减少，针对不同图像把局部极值的阈值进行分块自适应，从而将特征点和匹配点控制在既能保证准确度，又能减少计算量的自适应改进办法，具体实施办法如下：

其中，

是自适应条件下产生的全新阈值，

是最初的阈值，

为自适应系数，初始阈值设为0.02，Q为在最初的阈值条件下产生的特征点或匹配点的数量，

是满足要求下特征点与匹配点的数量最大值与最小值，

设为0.15，

设为1.1，

设为10。

比如有2幅图A和B，并生成了两幅图的描述子（分别是k1128维和k2128维），就将两图中各个描述子进行匹配，匹配上128维即可表示两个特征点匹配上了。

当两幅图像的SIFT特征向量生成后，采用关键点特征向量的欧式距离来作为两幅图像中关键点的相似性判定度量。取图像A中的某个关键点，并找出其与图像B中欧式距离最近的前两个关键点，在这两个关键点中，如果最近的距离除以次近的距离少于某个比例阈值，则接受这一对匹配点。降低这个比例阈值，SIFT匹配点数目会减少，但更加稳定。为了排除因为图像遮挡和背景混乱而产生的无匹配关系的关键点,可以采用比较最近邻距离与次近邻距离的方法，距离比率ratio小于某个阈值，则认为是正确匹配。因为对于错误匹配，由于特征空间的高维性,相似的距离可能有大量其他的错误匹配,从而它的ratio值比较高。

在一个示例性实施例中，ratio的取值原则如下:

ratio=0.4对于准确度要求高的匹配；

ratio=0.6对于匹配点数目要求比较多的匹配；

ratio=0.5一般情况下。

也可按如下原则：当最近邻距离<200时ratio=0.6，反之ratio=0.4，ratio的取值策略能排分错误匹配点。

实施例4

参见图4，其是根据本发明实施例的视觉惯导补偿方法的流程示意图，如图4所示，包括如下步骤：

步骤S402，对相机和IMU（惯性传感器）进行初始化。

相机（也称为摄像头）读取帧间图像，IMU读取帧间IMU数据。

步骤S404，前端处理。

插入相机读取的图像帧，提取图像特征，与上一帧进行光流追踪，通过光流结果计算该帧的位姿。

同时，提取IMU的帧间状态进行姿态演算，之后将图像与惯导的位姿估计进行融合，根据融合姿态更新位姿信息与地图。

当效果不理想时，可以补充新的特征点进行双目左右两个摄像头的匹配，并作三角化，将新的位姿信息加入地图，并触发后端的回环优化。

步骤S406，后端处理。

前端处理的结果将作为后端优化的初始值，后端在获取到处理之后的关键帧和路标点，将其进行检查更新优化，再返回优化结果。控制优化问题的规模在一定范围内，不会随时间一直增长，控制计算量，之后返回更新加入地图。

如果发生追踪丢失的现象，立即重置相机与IMU进行初始化，并重复上述流程。

其中，提取特征点的方法可以有多种。例如，在提取特征点的部分可以用ORB算法，即快速特征点提取和描述。FAST关键点（特征点）是通过以下步骤获取的：

（1）首先选择像素p，并且假设它的亮度为；

（2）设定1个阈值T（比如，的百分二十）；

（3）把像素p当作中心，在半径为三个像素的圆上选择十六个像素;

（4）假设所选圆圈具有亮度大于+T或小于-T的连续N个点，则将像素p看做成特征点（N一般设定为十二，也就是FAST-12）；

（5）对所有的像素将上面的四个步骤不断地循环。

在一个优选的示例中，为了使FAST-12算法的效率更高，应当加入预测试操作来很迅速的删掉很大一部分不是角点的像素，角点就是极值点，即在某方面属性特别突出的点，曲线上局部曲率最大的点。ORB为FAST角点没有方向性和缩放的薄弱点添加缩放和旋转的描述。而且特征的旋转是由如下的灰度质心法（Intensity Centroid）实现的。

（1）在小图像块B中，将图像块的矩定义为：

（2）接下来，采用以下公式确定质心：

（3）描述方向向量OC，即几何中心点O（小图像是一个矩形，对角线的交点就是几何中心点）与质心C可以在图像块中被连线），则可以定义特征点的方向为如下所示：

FAST角点对尺度和选择进行描述，极大地增强了不同图像之间表达的鲁棒性，把这种改进后的FAST称为Oriented FAST。

而BRIEF是一种向量由多个1和0组成的二进制描述符，0和1对关键点附近的两个像素（例如p和q）的大小关系进行编码：如果p比q大，则取值为1，反之，若p比q小，则取值为0。如果我们取128这样的p和q，我们最终将得到一个由0，1组成的128维向量。

在运动估计部分以地标位置

的样子利用传感器输入，预测机器人在状态

和

之间的运动。可视化功能简化了地标的数据关联，它们的外观通过特征描述符来量化，并且可以定义这些描述符的相似性度量。然后通过计算在描述符空间中的距离来匹配关键点描述符对

。然而，距离本身并不是作为关联的标准相应描述符之间的距离可能差别很大。因此，本公开实施方式利用距离最近邻居（

）和到第二个最近邻居（

）的距离空间。对于SIFT和SURF（Speeded Up Robust Features，加速稳健特征），这是

其中，r为在描述符空间中的距离，

为关键点描述符对，

为关键点描述符对，

为距离关键点最近邻居的距离，

为距离关键点第二近邻居的距离。

假设1个关键点仅可以和另外一个图像中的另外一个关键点来进行匹配，那么到第2个距离最短的邻居的距离应该要大得多。为了使最近的邻居快速搜索，使用在OpenCV库中实现的快速近似最近邻搜索FLANN的库。特征检测器和描述符的选择极大地影响了系统的准确性和运行时性能。在系统中使用OpenCV库的实现，这可以从大量的关键点检测器和特征提取器中进行选择。对于ORB，使用汉明距离。由于它本身之间的距离并非关联标准，因为匹配描述符的距离可能变化很大。由于特征空间的高维度，学习拒绝阈值的映射通常是不可行的。

在每一个递归步骤中，降低内部测定的阈值。结合有效估计的最小匹配特征数阈值。随着地图面积的增加，室内环境带来了额外的挑战，人造环境通常包含重复结构，例如：相同类型的椅子、窗户或重复壁纸。通过这样的相同实例给出足够的相似特征，两个图像之间的对应特征匹配导致估计假冒变换。最小匹配数的阈值减少了随机相似性和重复性较差的对象的错误估计数。

将阈值设置得足够高以排除系统性错误关联的估计值会导致在没有提到含糊不清的情况下的性能损失。因此，提出的替代鲁棒性措施对于具有挑战性的场景非常有利。为了考虑测量的强各向异性不确定性，可以通过最小化平方马氏距离而不是平方欧氏距离来改善变换估计，称之为两帧稀疏束调整。

对更早的帧（即闭环）进行成功的转换估计可以大大减少累积误差。为了找到一个大的闭环，可以从一组指定的关键帧中随机采样一个帧。该组关键帧用第一帧初始化。任何无法与最近关键帧匹配的新帧都会作为关键帧添加到该集合中。这样，用于采样的帧数大大减少，而关键帧覆盖的视野包含大部分感知区域。

实施例5

根据本发明实施例，还提供了一种用于实施上述视觉惯导补偿装置，如图5所示，该装置包括：

SIFT算子丢失补偿模块52，被配置为在SIFT算子追踪丢失的情况下，将惯性传感器IMU初始化，获取所述IMU的帧间IMU数据，而无需重新启动相机；基于所述帧间IMU数据进行姿态解算，以获取惯导的位姿估计，并对所述惯导的位姿估计进行融合，得到融合后的姿态；根据所述融合姿态更新位姿信息与地图，以对所述视觉惯导进行补偿。

SIFT算子未丢失补偿模块54，被配置为在SIFT算子追踪未丢失的情况下，基于相机读取的帧间图像和惯性传感器读取的帧间IMU数据提取关键帧和特征点，并基于所述关键帧和所述特征点将相应的新的数据加入到地图中，并更新地图，以进行视觉导航。

SIFT算子未丢失补偿模块54还被配置为基于相机读取的帧间图像和上一帧图像的光流追踪结果，计算所述帧间图像的位姿；基于惯性传感器读取的帧间IMU数据进行惯性传感器的位姿估计，并将所述帧间图像的位姿与所述惯性传感器的位姿估计进行融合，以提取所述关键帧和特征点。

例如，基于相机读取的帧间图像和上一帧图像的光流追踪结果，利用SIFT算子提取特征点，以计算所述帧间图像的位姿。

其中，SIFT算子未丢失补偿模块54包括特征点提取单元，特征点提取单元采用SIFT算子来提取特征点。例如，检测尺度空间极值，基于所述尺度空间极值获取关键点；将所述关键点进行本地化，并为所述关键点分配一个或多个方向；基于所述一个或多个方向，确定所述关键点的描述符，并基于所述关键点的描述符确定所述特征点。

具体地，特征点提取单元将所述帧间图像与每层尺度的高斯滤波器进行卷积确定所述帧间图像的尺度空间；对所述尺度空间中的每层尺度的图像做模糊处理；对每层尺度的图像做降采样，得到一组大小不一的图像；通过比较同组中的两个相邻图像来确定所述关键点。

特征点提取单元对尺度空间DoG函数进行曲线拟合，得到极值点的偏移量，在所述偏移量小于偏移阈值的情况下，确定所述关键点为真实关键点；利用高斯差分函数确定所述关键点的曲率，在所述曲率小于曲率阈值的情况下，确定所述关键点为稳定的真实关键点。

特征点提取单元确定所述关键点的梯度，使用方向直方图统计邻域内像素的梯度和方向，其中，所述方向直方图的峰值代表所述关键点处邻域梯度的方向；然后，以所述方向直方图中最大值作为所述关键点的主方向。

例如，计算在所述关键点位置周围的区域中的每个图像采样点处的梯度幅度和取向来创建所述关键点的描述符，作为所述SIFT特征描述向量；采用所述SIFT特征描述向量的欧式距离来作为所述关键点的相似性判定度量。

实施例6

参见图6，图6是根据本发明实施例的基于视觉惯导补偿装置的导航系统的结构示意图，其中，视觉惯导补偿装置是通过硬件实现平台来实现的。

该导航系统包括双目摄像头45和IMU传感器46以及视觉惯导补偿装置50。

视觉惯导补偿装置50所用硬件平台集成了两个ARM A9双核CPU 40-1和40-2、一个25K可编程逻辑单元41和一个85K可编程逻辑单元42，同时具备了硬件编程和软件编程功能。硬件平台拥有专用的摄像头接口47可以连接双目摄像头45，以及专用的传感器接口48可以连接IMU传感器46，将实时系数视觉里程计在此硬件平台上实现可以大大加速算法的运行速率。另外，此平台拥有FLASH输出接口43以及其他各种高速输出接口44，能够将输出结果很好的直接传递给其他平台。

所用硬件平台主要特色是：

（1）核心板集成电源管理：底板从核心板取电，节省底板电源芯片，降低底板硬件设计成本。

（2）核心板+底板设计：设计灵活，用户基于核心板设计功能底板，简化了底板硬件设计难度，适合项目应用，方便二次开发。

（3）尺寸紧凑：有利于设计更小的功能底板。

（4）资源丰富：

高性能接口：四个摄像头接口、一个HDMI、一个千兆网口、一个SD接口、一个USB-232接口、一个USB-OTG接口、两个FEP接口；

GPIO/差分对：7010/mini7010核心板可用102个IO/48对差分（其中PS端2个IO，PL端100个IO/48对差分），7020核心板具备127个IO/60对差分（其中PS端2个IO，PL端125个IO/60对差分）。底板FEPX2接口具备48个GPIO/24对差分对。

FEP接口：高速通信接口，可外接子卡，实现功能扩展。

丰富的DEMO：图像采集、HLS图像算法设计、双目/四路摄像头拼接+字幕叠加显示；Linux开发；千兆网络通信等。

其中，硬件平台芯片核心板搭载了一颗XILINX ZYNQ可编程FPGA芯片,其中一块核心板使用主芯片型号ZYNQXC7Z020CLG400-2I，另一块核心板使用主芯片型号7Z010CLG400-1C，还有一块核心板使用主芯片型号7Z010CLG400-1C。XC7Z010-CLG400-1C集成了ARM A9双核CPU和25K可编程逻辑单元。同时具备了硬件编程和软件编程功能。XC7Z020-CLG400-2I集成了ARM A9双核的CPU和85K可编程逻辑单元，同时具备了硬件编程和软件编程功能。

核心板具有一片4bit SPI FLASH。FLASH可用于保存数据和代码，初始化PL和PS部分子系统。其主要技术参数如下所示：

• 128Mbit

• x1, x2, and x4 支持

•最高时钟104 MHz, MZ7XA rates @ 100 MHz 4bit 模式下可以达到400Mbs

•工作于3.3V

平台板载了一路HDMI接口，HDMI部分采用了IO模拟HDMI信号。输出可以到1080P @60HZ高清传输，输入可以达到 720P @60HZ，优选地，使用HDMI子卡做输入。

实施例7

本公开的实施例还提供了一种存储介质。可选地，在本实施例中，上述存储介质可以实施上述实施例1至4中所描述的方法。

可选地，在本实施例中，上述存储介质可以位于惯性导航系统的网络中的多个网络设备中的至少一个网络设备。

可选地，在本实施例中，上述存储介质可以包括但不限于：U盘、只读存储器（ROM，Read-Only Memory）、随机存取存储器（RAM，Random Access Memory）、移动硬盘、磁碟或者光盘等各种可以存储程序代码的介质。

可选地，在本实施例中，处理器根据存储介质中已存储的程序代码执行上述实施例1至4中的方法。

可选地，本实施例中的具体示例可以参考上述实施例1至4中所描述的示例，本实施例在此不再赘述。

上述本发明实施例序号仅仅为了描述，不代表实施例的优劣。

上述实施例中的集成的单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在上述计算机可读取的存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在存储介质中，包括若干指令用以使得一台或多台计算机设备（可为个人计算机、服务器或者网络设备等）执行本发明各个实施例所述方法的全部或部分步骤。

在本发明的上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详述的部分，可以参见其他实施例的相关描述。

在本申请所提供的几个实施例中，应该理解到，所揭露的客户端，可通过其它的方式实现。其中，以上所描述的装置实施例仅仅是示意性的，例如所述单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，单元或模块的间接耦合或通信连接，可以是电性或其它的形式。

所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。

另外，在本发明各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims

1.一种视觉惯导补偿方法，其特征在于，所述方法包括：

在尺度不变特征变换SIFT算子追踪丢失的情况下，将惯性传感器IMU初始化，获取所述IMU的帧间IMU数据，而无需重新启动相机；基于所述帧间IMU数据进行姿态解算，以获取惯导的位姿估计，并对所述惯导的位姿估计进行融合，得到融合后的姿态；根据所述融合后的姿态更新位姿信息与地图，以对所述视觉惯导进行补偿；

在SIFT算子追踪未丢失的情况下，基于相机读取的帧间图像和惯性传感器读取的帧间IMU数据提取关键帧和特征点，并基于所述关键帧和所述特征点将相应的新的数据加入到地图中，并更新地图，以进行视觉导航。

2.根据权利要求1所述的方法，其特征在于，基于相机读取的帧间图像和惯性传感器读取的帧间IMU数据提取关键帧和特征点包括：

基于相机读取的帧间图像和上一帧图像的光流追踪结果，计算所述帧间图像的位姿；

基于惯性传感器读取的帧间IMU数据进行惯性传感器的位姿估计，并将所述帧间图像的位姿与所述惯性传感器的位姿估计进行融合，以提取所述关键帧和特征点。

3.根据权利要求2所述的方法，其特征在于，基于相机读取的帧间图像和上一帧图像的光流追踪结果，计算所述帧间图像的位姿包括：基于相机读取的帧间图像和上一帧图像的光流追踪结果，利用SIFT算子提取特征点，以计算所述帧间图像的位姿。

4.根据权利要求3所述的方法，其特征在于，利用SIFT算子提取特征点包括：

检测尺度空间极值，基于所述尺度空间极值获取关键点；

将所述关键点进行本地化，并为所述关键点分配一个或多个方向；

基于所述一个或多个方向，确定所述关键点的描述符，并基于所述关键点的描述符确定所述特征点。

5.根据权利要求4所述的方法，其特征在于，检测尺度空间极值，基于所述尺度空间极值获取关键点包括：

将所述帧间图像与每层尺度的高斯滤波器进行卷积确定所述帧间图像的尺度空间；

对所述尺度空间中的每层尺度的图像做模糊处理；

对每层尺度的图像做降采样，得到一组大小不一的图像；

通过比较同组中的两个相邻图像来确定所述关键点。

6.根据权利要求4所述的方法，其特征在于，将所述关键点进行本地化包括：

对尺度空间DoG函数进行曲线拟合，得到极值点的偏移量，在所述偏移量小于偏移阈值的情况下，确定所述关键点为真实关键点；

利用高斯差分函数确定所述关键点的曲率，在所述曲率小于曲率阈值的情况下，确定所述关键点为稳定的真实关键点。

7.根据权利要求4所述的方法，其特征在于，为所述关键点分配一个或多个方向包括：

确定所述关键点的梯度，使用方向直方图统计邻域内像素的梯度和方向，其中，所述方向直方图的峰值代表所述关键点处邻域梯度的方向；

以所述方向直方图中最大值作为所述关键点的主方向。

8.根据权利要求4所述的方法，其特征在于，基于所述一个或多个方向，确定所述关键点的描述符，并基于所述关键点的描述符确定所述特征点包括：

计算在所述关键点位置周围的区域中的每个图像采样点处的梯度幅度和取向来创建所述关键点的描述符，作为SIFT特征描述向量；

采用所述SIFT特征描述向量的欧式距离来作为所述关键点的相似性判定度量。

9.一种视觉惯导补偿装置，其特征在于，包括：

SIFT算子丢失补偿模块，被配置为在SIFT算子追踪丢失的情况下，将惯性传感器IMU初始化，获取所述IMU的帧间IMU数据，而无需重新启动相机；基于所述帧间IMU数据进行姿态解算，以获取惯导的位姿估计，并对所述惯导的位姿估计进行融合，得到融合后的姿态；根据所述融合后的姿态更新位姿信息与地图，以对所述视觉惯导进行补偿；

SIFT算子未丢失补偿模块，被配置为在SIFT算子追踪未丢失的情况下，基于相机读取的帧间图像和惯性传感器读取的帧间IMU数据提取关键帧和特征点，并基于所述关键帧和所述特征点将相应的新的数据加入到地图中，并更新地图，以进行视觉导航。

10.一种计算机可读存储介质，其上存储有程序，所述程序在被执行时，使得所述计算机执行如权利要求1至8中任一项所述的方法。