CN110617813A - 单目视觉信息和imu信息相融合的尺度估计系统及方法 - Google Patents

Abstract

本公开提供一种单目视觉信息和IMU信息相融合的尺度估计系统及方法，所述尺度估计系统包括：单目视觉SLAM模块，包括单目相机，用于采集视觉图像数据；IMU模块，用于获得IMU模块的IMU位姿数据；以及离线尺度估计模块，将所述视觉图像数据和IMU位姿数据进行融合，实现尺度估计和定位建图。所述尺度估计方法，包括：步骤S1：通过单目相机采集视觉图像数据；通过IMU模块获得IMU位姿数据；步骤S2：基于角速度将IMU位姿数据和单目相机位姿数据进行时间空间对齐；以及步骤S3：基于加速度在频域下进行尺度估计，完成基于单目视觉信息和IMU信息相融合的尺度估计。

Description

单目视觉信息和IMU信息相融合的尺度估计系统及方法

技术领域

本公开涉及同步定位与建图技术领域，尤其涉及一种单目视觉信息和IMU信息相融合的尺度估计系统及方法。

背景技术

同步定位与建图技术(Simultaneous Localization and Mapping，简称SLAM)，是指移动机器人在未知环境和未知位置下，根据搭载的特定传感器的输入信息，增量式地构建全局一致性的环境地图的同时进行位置定位的问题。随着在过去几十年间的发展，SLAM技术在机器人、定位导航、计算机视觉等领域都成为了一项至关重要的研究技术，同时在工业界也初步形成了基本的产业化实用能力。

视觉SLAM(Visual SLAM)是使用单目相机、双目相机、RGB-D相机等视觉传感器构建的SLAM系统。视觉SLAM与人眼类似，通过主动或者被动接受环境光的方式实现对周围环境的信息感知，同时基于多视图几何原理对图像中的特征点进行三维空间还原，从而构建一个与真实环境一致的三维地图。基于双目相机、RGB-D相机的SLAM系统由于传感器能获取深度信息，从而可以构建出真实环境的尺度，但是RGB-D相机使用成本较高，同时RGB-D相机和双目相机在体积尺寸上较大，不便携带，限制了其在低成本设备、便携的移动设备上进一步应用。相反，基于单目视觉相机的SLAM系统具有成本低、体积小、实时性较高、可获取的信息丰富可靠等优点，在实用上更具优势。目前基于单目视觉的SLAM已经有许多的解决方案，比如开源方法VisualSfM，OpenMVG，OpenSfM，ORB-SLAM，ORB-SLAM2等等。但是因为单目相机在多视图几何投影理论上从三维空间投影到二维像素成像平面，损失了一个维度的信息，从而缺失了3D点的深度信息，最终构建出的地图不具有尺度信息。因此，纯单目视觉的SLAM系统都无法获得所建图的绝对尺度信息。这一缺陷限制了单目在现实场景下机器人的应用，也限制了定位和建图的应用。

IMU(惯性测量单元，Inertial Measurement Unit)同样具备了成本低、体积小的优点，IMU通过自身的惯性测量值可以对尺度进行估计。视觉惯性系统(visual inertialsystem，VINS)是指结合了视觉和惯性测量单元IMU实现定位、建图和导航的系统。通过低成本惯性测量单元辅助单目视觉系统成为了SLAM的一种增长趋势，这一系统可以借助IMU信息来弥补单目视觉SLAM系统不具有尺度信息的缺陷。

尽管单目VINS系统在技术层面能够在定位、建图和导航上完美的结合，但是实际的结合算法仍然有很大的不足。其中一大难点首先是对硬件提出了很高的要求，比如同步电路，比如硬件对齐设计。目前，包括商用化的视觉惯性里程计的专用硬件设备GoogleTango平板(使用鱼眼镜头相机和高精度IMU)在内，许多方法(比如Leutenegger et al.，2014，OKVIS框架)都是通过使用特殊定制化的硬件(Camera-IMU)来实现精确的尺度估计。尽管如此，也有一些方法利用标准的智能手机传感器进行运动跟踪和重建，这些方法实际的结果并没有达到理想的效果。

单目VINS系统除了对硬件提出特定要求之外，需要同时解决视觉测距和惯性导航两个算法难题，比如严格的初始化，由于缺乏直接距离测量，很难直接将单目视觉结构和惯性测量值进行融合。另外，VINS系统对硬件提出了很高的要求，比如，各个视频帧和IMU传感器时间戳的同步必须相对准确，如何在低精度的移动设备上实现精确的尺度估计和定位是目前的一大难题。目前VINS系统中效果较好的是2017年香港科技大学提出的VINS-mono框架，图7是这一系统的框架图。在这一框架中，视觉和IMU通过离线和松耦合的方式实现初始化尺度估计的过程。但是这一框架中，通过离线的视觉SfM(Struct from Motion)端输出的位姿和定位结果与IMU进行预积分之后的数据进行位置最小化的方式会引入较大误差，因而最后利用带有误差的尺度信息对后续的定位和估计精度产生的误差更为显著。

公开内容

(一)要解决的技术问题

基于上述问题，本公开提供了一种单目视觉信息和IMU信息相融合的尺度估计系统及方法，以缓解现有技术中单目相机采集的图像中没有绝对尺度信息，基于位置的视觉惯性系统的尺度估计精度较低；图像序列和IMU传感器的硬件时间戳同步性必须非常严格；现有VINS系统需要严格初始化等技术问题。

(二)技术方案

本公开的一个方面，提供一种单目视觉信息和IMU信息相融合的尺度估计系统，包括：单目视觉SLAM模块，包括单目相机，用于采集视觉图像数据；IMU模块，用于获得IMU模块的IMU位姿数据；以及尺度估计模块，将所述视觉图像数据和IMU位姿数据进行融合，实现尺度估计和定位建图。

在本公开实施例中，通过对单目相机采集的无绝对尺度信息的视觉图像数据进行处理，获得视觉位姿数据。

在本公开实施例中，所述视觉位姿数据包括：单目相机位置，单目相机旋转矩阵，单目相机旋转四元数，以及单目相机旋转角速度。

在本公开实施例中，所述IMU模块包括陀螺仪、加速度计；所述IMU位姿数据包括：IMU加速度、IMU角速度。

在本公开的另一方面，提供一种单目视觉信息和IMU信息相融合的尺度估计方法，通过以上任一项所述的单目视觉信息和IMU信息相融合的尺度估计系统进行尺度估计，所述尺度估计方，包括：步骤S1：通过单目相机采集视觉图像数据；通过IMU模块获得IMU位姿数据；步骤S2：基于角速度将IMU位姿数据和单目相机位姿数据进行时间空间对齐；以及步骤S3：基于加速度在频域下进行尺度估计，完成基于单目视觉信息和IMU信息相融合的尺度估计。

在本公开实施例中，单目视觉SLAM模块通过单目相机观测路标点，将三维路标点投影到相机的成像平面，得到像素坐标；成像时的模型采用针孔成像模型；单目视觉SLAM模块的单个路标点按照如下方程进行观测：

ZP_uv＝K(RP_W+t)＝KTP_W

其中，P_u，v是图像上的像素坐标，P_W＝(X Y Z)^T是路标点在相机坐标系下的三维坐标，矩阵K是相机内参数矩阵，T是相机从相机坐标系到世界坐标系的变换矩阵，R为相机的旋转矩阵，t是相机从相机坐标系到世界坐标系的平移向量，Z是路标点在相机坐标系下的Z轴坐标，也即路标点在相机坐标系下的深度值；之后将观测结果进行最小化重投影误差，π_(·)是投影函数，从而可以得到视觉观测的单目相机位姿结果，即相机的旋转矩阵R和相机的位置p：

其中，Xⁱ是第i个路标点在相机坐标系下的位置，是第i个路标点在像素平面的投影坐标，ρ()是鲁棒核函数，χ是从单目相机捕获的图像中提取的所有特征点(也即路标点)的集合，∑是求和符，是指使其后面式子达到最小值时的变量R，t的取值，p是相机的位置数据，也即相机的平移向量t。

在本公开实施例中，首先假设视觉角速度和IMU角速度在时间上是对齐的，可以通过采用最小二乘法最小化不含时间偏置的代价函数得到：

其中R_S表示从相机坐标系到IMU体坐标系的旋转矩阵，t_d表示IMU的时间偏置，表示陀螺仪的偏置 为单目相机角速度，为IMU角速度。

在本公开实施例中，因为视觉角速度和IMU角速度满足两组矢量的空间旋转和平移关系，利用如下空间对齐初值估计封闭解算法，首先定义两组量的角速度质心：

其中，分别为视觉角速度质心和IMU角速度质心，n是参与计算的数据个数，从而可以得到两组去质心的角速度：

其中，P_V，Q_I分别为去质心视觉角速度集和去质心IMU角速度集；那么定义角速度累积矩阵W：

对W进行奇异值分解：

W＝U∑V^T；

其中，∑为奇异值diag(σ₁，σ₂，σ₃)组成的对角矩阵，对角线元素依次从大到小排列，U和V为分解出的正交矩阵；从而，空间对齐的旋转矩阵R_S即为：

R_S＝V×C×U^T；

其中，W为满秩矩阵时，C＝diag(1，1，1)；W为秩亏矩阵时，C＝diag(1，1，-1)；在完成旋转矩阵的解算之后，IMU模块中陀螺仪的角速度的偏置根据下式求得：

在本公开实施例中，利用基于黄金分割搜索的时间偏置和角速度偏置的迭代算法，先确定正负两个方向的最大时间偏置变化±(t_d)_max，之后对时间偏置使用黄金分割搜索，同时以空间对齐初值估计封闭解算法求解R_S和之后时间偏置和空间旋转的求解以迭代方式进行，具体如下，首先设黄金分割率为：

时间偏置的初值为：

a＝-(t_d)_max，b＝(t_d)_max；

两个时间偏置的黄金分割试探点分别为：

α＝b-(b-a)/ρ_g，β＝a+(b-a)/ρ_g；

当两个试探点的误差|α-β|小于阈值τ_tolerance时，则其均值为最终的时间偏置值：

t_d＝(α+β)/2；

得到R_S的最优解后，再将时间偏置项t_d包含进目标函数中，求解时间偏置的最优化结果，之后空间对齐和时间对齐优化交替进行，如下：

在本公开实施例中，在获取了相机坐标系下的视觉加速度和滤波之后的IMU加速度之后，执行最小二乘优化，具有重力项的时域下的目标函数如下式：

其中，为视觉加速度在相机坐标系的表达，为IMU加速度在相机坐标系下的表达，g_W为重力加速度，为不同时刻下世界坐标系到相机坐标系的旋转矩阵，为IMU加速度偏置在相机坐标系下的表达，s就是尺度因子，之后再对时域下的视觉加速度和IMU加速度进行频域变换，设运算符表示傅里叶变换，从而频域下的视觉和惯性加速度计算方式如下：

其中，和分别是包含视觉加速度和惯性加速度的傅里叶变换矩阵，同时分开计算三个轴向的傅里叶变换；最终的尺度因子s求解是最小化幅值谱|A^V(f)|和|A^I(f)|，其代价函数定义如下：

其中，b^a是IMU加速度计的偏置，f_max表示最大频率上限。

(三)有益效果

从上述技术方案可以看出，本公开单目视觉信息和IMU信息相融合的尺度估计系统及方法至少具有以下有益效果其中之一或其中一部分：

(1)不需要在有严格的硬件要求下就可以实现较好的视觉和IMU之间的时间偏置对齐和空间对齐，不需要视频序列和IMU传感器的硬件时间戳必须严格同步，尺度估计模块可以用于视觉惯性里程计(VIO)和视觉惯性导航系统(VINS)中的初始化和后端处理部分；

(2)实现了对视觉位姿数据和IMU惯性数据融合，解决了单目相机没有尺度信息的问题，解决了目前基于位置的视觉惯性系统的尺度估计精度较低的问题，在低成本的移动设备上实现较高精确的尺度估计和定位。

附图说明

图1为本公开实施例单目视觉信息和IMU信息相融合的尺度估计系统组成示意图。

图2为本公开实施例单目视觉信息和IMU信息相融合的尺度估计系统的详细框架结构示意图。

图3为本公开实施例单目视觉信息和IMU信息相融合的尺度估计方法流程示意图。

图4为本公开实施例单目视觉信息和IMU信息相融合的尺度估计方法中基于角速度的时间空间对齐算法流程示意图。

图5为本公开实施例单目视觉信息和IMU信息相融合的尺度估计方法中基于黄金分割搜索的时间偏置和空间偏置的迭代求解算法流程示意图。

图6为本公开实施例单目视觉信息和IMU信息相融合的尺度估计方法中频域下基于加速度的尺度估计算法流程示意图。

图7为2017年香港科技大学提出VINS-mono系统的框架示意图。

具体实施方式

本公开提供了一种单目视觉信息和IMU信息相融合的尺度估计系统及方法，所述尺度估计是指利用IMU(Inertial Measurement Unit，惯性测量单元)信息将视觉SLAM(Simultaneous Localization and Mapping，同步定位与建图技术)的结果进行尺度缩放成真实尺度。IMU惯性测量单元能弥补单目SLAM的不足，可以用于估计视觉定位和建图的尺度因子。本公开针对单目相机无法恢复尺度信息的问题，通过结合低成本消费级的IMU，开发出一套松耦合的尺度估计算法，这一算法通过离线的视觉SLAM模块和较低精度的IMU数据实现了精确的尺度恢复。

为使本公开的目的、技术方案和优点更加清楚明白，以下结合具体实施例，并参照附图，对本公开进一步详细说明。

在本公开实施例中，提供一种单目视觉信息和IMU信息相融合的尺度估计系统，结合图1至图2所示，所述单目视觉信息和IMU信息相融合的尺度估计系统，包括：

单目视觉SLAM模块，包括单目相机，用于采集视觉图像数据；

IMU模块，用于获得IMU模块的IMU位姿数据；以及

尺度估计模块，将所述视觉图像数据和IMU位姿数据进行融合，实现尺度估计和定位建图。

通过对单目相机采集的无绝对尺度信息的视觉图像数据进行处理，获得单目相机位姿数据，即视觉位姿数据；

所述视觉位姿数据包括位置数据和姿态数据：位置数据指单目相机位置，通常用平移向量t表示；姿态数据指单目相机的姿态，通常用单目相机(视觉)旋转矩阵R或者单目相机(视觉)旋转四元数q表示。位姿数据也可以用同时包含了位置信息和姿态信息的变换矩阵T表示。从视觉位姿数据中可以根据现有知识推导出其他位姿信息，比如单目相机旋转角速度(视觉角速度)；

所述IMU模块包括陀螺仪、加速度计；

所述IMU位姿数据包括：IMU加速度、IMU角速度；

IMU加速度也可称为惯性加速度，IMU角速度也可称为惯性角速度；

所述尺度估计模块为离线尺度估计模块或在线尺度估计模块，在本公开实施例中，以离线尺度估计模块作为具体实施例进行描述。

所述视觉图像数据为无尺度信息数据，首先由单目视觉SLAM模块进行视觉定位，通过非线性优化的方法对相机位姿和3D点进行局部集束调整，同时采用回环检测方法实现鲁棒的全局定位，从而获得一个鲁棒的定位地图。在获得无尺度信息的定位地图之后，通过离线尺度估计模块对IMU位姿数据采用上采样和频域下最小化加速度代价函数的方式实现鲁棒的尺度估计，从而在数据精度较低的情况下依然能达到良好的尺度估计结果；通过对尺度的有效估计，进而可以实现鲁棒的视觉惯性定位和建图。

在获得单目视觉SLAM模块的信息之后，后续的离线尺度估计模块将实现一种在给定惯性测量值和相机位姿情况下恢复尺度的SLAM前端初始化和后端尺度估计算法。所述离线尺度估计模块采用解耦和离线的方式，结合单目视觉SLAM的定位结果和IMU的位姿数据进行融合。

在本公开的实施例中，还提供一种单目视觉信息和IMU信息相融合的尺度估计方法，所述尺度估计方法融合了视觉图像数据与IMU位姿数据，首先进行单目视觉信息(视觉图像数据)和IMU信息(IMU位姿数据)的时间和空间对齐，同时利用Rauch-Tung-Striebel(RTS)平滑器来处理带噪声的相机位置，利用EKF扩展卡尔曼滤波器来处理带有噪声的IMU角速度和IMU加速度信息，最后在尺度估计中利用基于频域的最小化加速度测量值来实施最终的尺度估计，从而可以在基于单目相机及低成本消费级的IMU的情况下实现低成本和高精度的尺度估计和定位建图；结合图2至图6所示，所述单目视觉信息和IMU信息相融合的尺度估计方法，包括：

步骤S1：通过单目相机采集视觉图像数据；通过IMU模块获得IMU位姿数据；

所述单目视觉SLAM模块通过单目相机观测路标点，将三维路标点投影到相机的成像平面，得到像素坐标；成像时的模型采用针孔成像模型；单目视觉SLAM模块的单个路标点按照如下方程进行观测：

ZP_uv＝K(RP_W+t)＝KTP_W

其中，P_u，v是图像上的像素坐标，P_W＝(X Y Z)^T是路标点在相机坐标系下的三维坐标，矩阵K是相机内参数矩阵，T是相机从相机坐标系到世界坐标系的变换矩阵，R为相机的旋转矩阵，t是相机从相机坐标系到世界坐标系的平移向量，Z是路标点在相机坐标系下的Z轴坐标，也即路标点在相机坐标系下的深度值。

之后将观测结果进行最小化重投影误差，π_(·)是投影函数，从而可以得到视觉观测的单目相机位姿结果，即相机的旋转矩阵R和相机的位置p：

其中，Xⁱ是第i个路标点在相机坐标系下的位置，是第i个路标点在像素平面的投影坐标，ρ()是鲁棒核函数，χ是从单目相机捕获的图像中提取的所有特征点(也即路标点)的集合，∑是求和符，是指使其后面式子达到最小值时的变量R，t的取值，p是相机的位置数据，也即相机的平移向量t。

步骤S2：基于角速度将IMU位姿数据和单目相机位姿数据进行时间空间对齐；

时间对齐是指通过算法将并未严格按照时钟记录的视觉相机帧和IMU位姿数据的时间戳恢复到实际情况的一种方法；空间对齐是通过算法将并未严格参照硬件设计上的相机和IMU坐标系位置关系T_BC矩阵恢复到实际情况的一种技术。

如图4所示，首先基于IMU模块中的陀螺仪读数与(视觉图像数据中)单目相机角速度进行比较，其中单目相机角速度根据相机位姿计算出来。随后的对齐优化步骤使用高斯牛顿法进行迭代单目相机角速和IMU角速度的最小二乘优化的偏置值作为最佳估计值，从而在空间对齐的同时执行时间对齐。具体技术方法如下：

由于在最优化方程中有待求量从相机坐标系到IMU体坐标系的旋转矩阵R_S(即空间对齐)和IMU相对相机的时间偏置t_d(即时间对齐)，以及陀螺仪的偏置无法在时间偏置未知的情况下得到空间对齐的最优结果。为了找到单目相机角速度和IMU角速度之间最优的空间变换关系，首先假设视觉角速度和惯性角速度在时间上是对齐的，可以通过采用最小二乘法最小化不含时间偏置的代价函数得到：

其中，是单目相机的旋转角速度在相机坐标系下的表达，是IMU的角速度在IMU坐标系下的表达，上标V表示视觉量，上标I表示IMU量，下标C表示相机坐标系，下标S表示IMU坐标系；是陀螺仪角速度偏置在相机坐标系下的表达；R_S表示从相机坐标系到IMU体坐标系的旋转矩阵，||C_ω||²代指误差值，∑是求和符，argmin是指使其后面式子达到最小值时下标变量的取值。上述对空间中的视觉角速度和IMU角速度的最小化误差代价函数的方程式中，因为它们之间满足两组矢量的空间旋转和平移关系，所以本公开利用3D点与3D点之间的ICP的思想进行封闭求解；下面是本公开中提出的空间对齐初值估计算法，首先定义两组量的角速度质心：

其中，分别为视觉角速度质心和IMU角速度质心，n是参与计算的数据个数。从而可以得到两组去质心的角速度：

其中，P_V，Q_I分别为去质心视觉角速度集和去质心IMU角速度集。那么定义角速度累积矩阵W：

对W进行奇异值分解(Singular Value Decomposition，SVD)，

W＝U∑V^T

其中，∑为奇异值diag(σ₁，σ₂，σ₃)组成的对角矩阵，对角线元素依次从大到小排列，U和V为分解出的正交矩阵，上标^T为转置符号。从而，空间对齐的旋转矩阵R_S即为：

R_S＝V×C×U^T

其中，若W为满秩矩阵时C＝diag(1，1，1)，若W为秩亏矩阵时C＝diag(1，1，-1)。在完成旋转矩阵的解算之后，IMU陀螺仪的角速度的偏置即可根据下式求得：

针对时间偏置的优化计算中，本公开提出了一种基于黄金分割搜索的时间偏置和角速度偏置的迭代算法。具体时间偏置和空间偏置的迭代求解算法流程如图5所示。

在这一偏置迭代算法中，先确定正负两个方向的最大时间偏置变化±(t_d)_max，之后对时间偏置使用黄金分割搜索，同时以封闭解形式求解R_S和之后时间偏置和空间旋转的求解以迭代方式进行。具体如下，首先设黄金分割率ρ_g为：

时间偏置的初值a，b为：

a＝-(t_d)_max，b＝(t_d)_max；

那么两个时间偏置的黄金分割试探点α，β分别为：

α＝b-(b-a)/ρ_g，β＝a+(b-a)/ρ_g；

当两个试探点的误差|α-β|小于阈值τ_tolerance时，则其均值为最终的时间偏置值t_d：

t_d＝(α+β)/2；

在得到R_S的最优解之后，再将时间偏置项t_d包含进目标函数中，求解时间偏置的最优化结果，之后空间对齐和时间对齐优化交替进行。

在完成时间和空间对齐之后，将使用R_S和t_d的估计结果在时间和空间上将惯性加速度与相机加速度对齐，进而进行尺度估计。

步骤S3：基于加速度在频域下进行尺度估计，完成基于单目视觉信息和IMU信息相融合的尺度估计。

尺度估计的目的是找到一个尺度因子s来确定重建和定位的尺度。而一般IMU中加速度计的测量值包括了地球重力，同时测量值存在噪声干扰和偏置。因此在使用加速度测量数据进行尺度估计时也需估计重力和加速度计偏置。

由于IMU和相机时间戳的偏置不是恒定量，为了解决时域下IMU和相机的时间戳无法完全对齐的问题，同时也为了解决基于位置的尺度估计存在漂移误差的问题，在本公开中提出了基于加速度在频域下的尺度估计算法。具体算法流程如图6所示。

在获取了相机坐标系下的视觉加速度和滤波之后的IMU加速度之后，只需要执行最小二乘优化即可，具有重力项的时域下的目标函数从而可以写成下式：

上式中，为视觉加速度在相机坐标系的表达，为IMU加速度在相机坐标系下的表达，g_W为重力加速度，为不同时刻下世界坐标系到相机坐标系的旋转矩阵，为IMU加速度偏置在相机坐标系下的表达，s就是所要找的尺度因子。之后再对时域下的视觉加速度和IMU加速度进行频域变换，设运算符表示傅里叶变换，从而频域下的视觉和惯性加速度计算方式如下：

其中，和分别是包含视觉加速度和惯性加速度的傅里叶变换矩阵，同时分开计算三个轴向的傅里叶变换，f表示频率。最终的尺度因子s求解是最小化幅值谱|A^V(f)|和|A^I(f)|，其代价函数定义如下：

上式中，∑是求和符，arg min是指使其后面式子达到最小值时下标变量的取值。，b^a是IMU加速度计的偏置，最大频率上限由f_max表示。

至此，已经结合附图对本公开实施例进行了详细描述。需要说明的是，在附图或说明书正文中，未绘示或描述的实现方式，均为所属技术领域中普通技术人员所知的形式，并未进行详细说明。此外，上述对各元件和方法的定义并不仅限于实施例中提到的各种具体结构、形状或方式，本领域普通技术人员可对其进行简单地更改或替换。

依据以上描述，本领域技术人员应当对本公开单目视觉信息和IMU信息相融合的尺度估计系统及方法有了清楚的认识。

综上所述，本公开单目视觉信息和IMU信息相融合的尺度估计系统及方法不需要在有严格的硬件要求下就可以实现较好的视觉和IMU之间的时间偏置对齐和空间对齐，不需要视频序列和IMU传感器的硬件时间戳必须严格同步，同时解决了现有VINS系统需要严格初始化的问题，本公开设计的尺度估计模块，可以用于视觉惯性里程计(VIO)和视觉惯性导航系统(VINS)中的初始化和后端处理部分。本公开重点从单目相机与IMU的测量数据中恢复尺度信息的方法，实现了对视觉位姿数据和IMU惯性数据融合，解决了单目相机没有尺度信息的问题，解决了目前基于位置的视觉惯性系统的尺度估计精度较低的问题，在低成本的移动设备上实现较高精确的尺度估计和定位。

还需要说明的是，实施例中提到的方向用语，例如“上”、“下”、“前”、“后”、“左”、“右”等，仅是参考附图的方向，并非用来限制本公开的保护范围。贯穿附图，相同的元素由相同或相近的附图标记来表示。在可能导致对本公开的理解造成混淆时，将省略常规结构或构造。

并且图中各部件的形状和尺寸不反映真实大小和比例，而仅示意本公开实施例的内容。另外，在权利要求中，不应将位于括号之间的任何参考符号构造成对权利要求的限制。

除非有所知名为相反之意，本说明书及所附权利要求中的数值参数是近似值，能够根据通过本公开的内容所得的所需特性改变。具体而言，所有使用于说明书及权利要求中表示组成的含量、反应条件等等的数字，应理解为在所有情况中是受到「约」的用语所修饰。一般情况下，其表达的含义是指包含由特定数量在一些实施例中±10％的变化、在一些实施例中±5％的变化、在一些实施例中±1％的变化、在一些实施例中±0.5％的变化。

再者，单词“包含”不排除存在未列在权利要求中的元件或步骤。位于元件之前的单词“一”或“一个”不排除存在多个这样的元件。

说明书与权利要求中所使用的序数例如“第一”、“第二”、“第三”等的用词，以修饰相应的元件，其本身并不意味着该元件有任何的序数，也不代表某一元件与另一元件的顺序、或是制造方法上的顺序，该些序数的使用仅用来使具有某命名的一元件得以和另一具有相同命名的元件能做出清楚区分。

此外，除非特别描述或必须依序发生的步骤，上述步骤的顺序并无限制于以上所列，且可根据所需设计而变化或重新安排。并且上述实施例可基于设计及可靠度的考虑，彼此混合搭配使用或与其他实施例混合搭配使用，即不同实施例中的技术特征可以自由组合形成更多的实施例。

本领域那些技术人员可以理解，可以对实施例中的设备中的模块进行自适应性地改变并且把它们设置在与该实施例不同的一个或多个设备中。可以把实施例中的模块或单元或组件组合成一个模块或单元或组件，以及此外可以把它们分成多个子模块或子单元或子组件。除了这样的特征和/或过程或者单元中的至少一些是相互排斥之外，可以采用任何组合对本说明书(包括伴随的权利要求、摘要和附图)中公开的所有特征以及如此公开的任何方法或者设备的所有过程或单元进行组合。除非另外明确陈述，本说明书(包括伴随的权利要求、摘要和附图)中公开的每个特征可以由提供相同、等同或相似目的的替代特征来代替。并且，在列举了若干装置的单元权利要求中，这些装置中的若干个可以是通过同一个硬件项来具体体现。

类似地，应当理解，为了精简本公开并帮助理解各个公开方面中的一个或多个，在上面对本公开的示例性实施例的描述中，本公开的各个特征有时被一起分组到单个实施例、图、或者对其的描述中。然而，并不应将该公开的方法解释成反映如下意图：即所要求保护的本公开要求比在每个权利要求中所明确记载的特征更多的特征。更确切地说，如下面的权利要求书所反映的那样，公开方面在于少于前面公开的单个实施例的所有特征。因此，遵循具体实施方式的权利要求书由此明确地并入该具体实施方式，其中每个权利要求本身都作为本公开的单独实施例。

以上所述的具体实施例，对本公开的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上所述仅为本公开的具体实施例而已，并不用于限制本公开，凡在本公开的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本公开的保护范围之内。

Claims (10)

1.一种单目视觉信息和IMU信息相融合的尺度估计系统，包括：

单目视觉SLAM模块，包括单目相机，用于采集视觉图像数据；

IMU模块，用于获得IMU模块的IMU位姿数据；以及

尺度估计模块，将所述视觉图像数据和IMU位姿数据进行融合，实现尺度估计和定位建图。

2.根据权利要求1所述的单目视觉信息和IMU信息相融合的尺度估计系统，通过对单目相机采集的无绝对尺度信息的视觉图像数据进行处理，获得视觉位姿数据。

3.根据权利要求2所述的单目视觉信息和IMU信息相融合的尺度估计系统，所述视觉位姿数据包括：单目相机位置，单目相机旋转矩阵，单目相机旋转四元数，以及单目相机旋转角速度。

4.根据权利要求1所述的单目视觉信息和IMU信息相融合的尺度估计系统，所述IMU模块包括陀螺仪、加速度计；所述IMU位姿数据包括：IMU加速度、IMU角速度。

5.一种单目视觉信息和IMU信息相融合的尺度估计方法，通过如权利要求1至4任一项所述的单目视觉信息和IMU信息相融合的尺度估计系统进行尺度估计，所述尺度估计方，包括：

步骤S1：通过单目相机采集视觉图像数据；通过IMU模块获得IMU位姿数据；

步骤S2：基于角速度将IMU位姿数据和单目相机位姿数据进行时间空间对齐；以及

步骤S3：基于加速度在频域下进行尺度估计，完成基于单目视觉信息和IMU信息相融合的尺度估计。

6.根据权利要求5所述的单目视觉信息和IMU信息相融合的尺度估计方法，步骤S1中，单目视觉SLAM模块通过单目相机观测路标点，将三维路标点投影到相机的成像平面，得到像素坐标；成像时的模型采用针孔成像模型；单目视觉SLAM模块的单个路标点按照如下方程进行观测：

ZP_uv＝K(R_W+t)＝KTP_W；

其中，P_u，v是图像上的像素坐标，P_W＝(X Y Z)^T是路标点在相机坐标系下的三维坐标，矩阵K是相机内参数矩阵，T是相机从相机坐标系到世界坐标系的变换矩阵，R为相机的旋转矩阵，t是相机从相机坐标系到世界坐标系的平移向量，Z是路标点在相机坐标系下的Z轴坐标，也即路标点在相机坐标系下的深度值；之后将观测结果进行最小化重投影误差，π_(·)是投影函数，从而可以得到视觉观测的单目相机位姿结果，即相机的旋转矩阵R和相机的位置p：

其中，Xⁱ是第i个路标点在相机坐标系下的位置，是第i个路标点在像素平面的投影坐标，ρ()是鲁棒核函数，χ是从单目相机捕获的图像中提取的所有特征点(也即路标点)的集合，∑是求和符，是指使其后面式子达到最小值时的变量R，t的取值，p是相机的位置数据，也即相机的平移向量t。

7.根据权利要求5所述的单目视觉信息和IMU信息相融合的尺度估计方法，步骤S2中，首先假设视觉角速度和IMU角速度在时间上是对齐的，可以通过采用最小二乘法最小化不含时间偏置的代价函数得到：

其中R_S表示从相机坐标系到IMU体坐标系的旋转矩阵，t_d表示IMU的时间偏置，表示陀螺仪的偏置 为单目相机角速度，为IMU角速度。

8.根据权利要求7所述的单目视觉信息和IMU信息相融合的尺度估计方法，因为视觉角速度和IMU角速度满足两组矢量的空间旋转和平移关系，利用如下空间对齐初值估计封闭解算法，首先定义两组量的角速度质心：

其中，分别为视觉角速度质心和IMU角速度质心，n是参与计算的数据个数，从而可以得到两组去质心的角速度：

其中，P_V，Q_I分别为去质心视觉角速度集和去质心IMU角速度集；那么定义角速度累积矩阵W：

对W进行奇异值分解：

W＝U∑V^T；

其中，∑为奇异值diag(σ₁，σ₂，σ₃)组成的对角矩阵，对角线元素依次从大到小排列，U和V为分解出的正交矩阵；从而，空间对齐的旋转矩阵R_S即为：

R_S＝V×C×U^T；

其中，W为满秩矩阵时，C＝diag(1，1，1)；W为秩亏矩阵时，C＝diag(1，1，-1)；在完成旋转矩阵的解算之后，IMU模块中陀螺仪的角速度的偏置根据下式求得：

9.根据权利要求8所述的单目视觉信息和IMU信息相融合的尺度估计方法，利用基于黄金分割搜索的时间偏置和角速度偏置的迭代算法，先确定正负两个方向的最大时间偏置变化±(t_d)_max，之后对时间偏置使用黄金分割搜索，同时以空间对齐初值估计封闭解算法求解R_S和之后时间偏置和空间旋转的求解以迭代方式进行，具体如下，首先设黄金分割率为：

时间偏置的初值为：

a＝-(t_d)_max，b＝(t_d)_max；

两个时间偏置的黄金分割试探点分别为：

α＝b-(b-a)/ρ_g，β＝a+(b-a)/ρ_g；

当两个试探点的误差|α-β|小于阈值τ_tolerance时，则其均值为最终的时间偏置值：

t_d＝(α+β)/2；

得到R_S的最优解后，再将时间偏置项t_d包含进目标函数中，求解时间偏置的最优化结果，之后空间对齐和时间对齐优化交替进行，如下：

10.根据权利要求5所述的单目视觉信息和IMU信息相融合的尺度估计方法，在获取了相机坐标系下的视觉加速度和滤波之后的IMU加速度之后，执行最小二乘优化，具有重力项的时域下的目标函数如下式：

||g_W||²＝9.8；

其中，为视觉加速度在相机坐标系的表达，为IMU加速度在相机坐标系下的表达，g_W为重力加速度，为不同时刻下世界坐标系到相机坐标系的旋转矩阵，为IMU加速度偏置在相机坐标系下的表达，s就是尺度因子，之后再对时域下的视觉加速度和IMU加速度进行频域变换，设运算符表示傅里叶变换，从而频域下的视觉和惯性加速度计算方式如下：

其中，和分别是包含视觉加速度和惯性加速度的傅里叶变换矩阵，同时分开计算三个轴向的傅里叶变换，f表示频率；最终的尺度因子s求解是最小化幅值谱|A^V(f)|和|A^I(f)|，其代价函数定义如下：

||g_W||²＝9.8；

其中，b^a是IMU加速度计的偏置，f_max表示最大频率上限。