CN110517324A - 基于变分贝叶斯自适应算法的双目vio实现方法 - Google Patents

Abstract

本发明提供了一种基于变分贝叶斯自适应算法的双目VIO实现方法，包括：通过双目相机获得图像，对图像进行特征点提取，将提取的特征点加入全局维护的地图容器中；通过IMU数据进行IMU状态预测；当新接收到一帧图像，将该帧图像加入到状态向量中进行状态向量及状态协方差的增广；判断是否有特征点或者相机需要删除，如果有，则进行滤波融合；判断为有，则基于双目相机的观测模型进行UT变换，计算观测模型对应的雅克比矩阵；将多个雅克比矩阵叠加起来后，进行零空间投影，得到最终的标准观测方程；对得到的标准观测方程应用变分贝叶斯估计，更新VIO系统的状态。能够很好地处理系统观测噪声时变的情况，在提升精度的同时，增加鲁棒性。

Description

基于变分贝叶斯自适应算法的双目VIO实现方法

技术领域

本发明涉及机器人定位领域，具体地，涉及一种基于变分贝叶斯(variationalBayesian，VB)自适应非线性滤波的双目视觉惯性里程计(Visual Inertial Odometry，VIO)实现方法。

背景技术

机器人定位与建图(simultaneous localization and mapping，SLAM)问题对于实现一个完全自主的机器人是至关重要的一环。当我们不考虑建图问题，SLAM问题就被简化成了里程计的定位问题。而基于视觉的定位算法由于可以提供丰富的环境信息以及其较低的成本，越来越受人青睐。然而，仅仅依靠视觉信息来完成机器人的定位，在很多时候系统是不够鲁棒的(比如说，当环境的纹理特征不明显，环境的光照变化大，机器人的运动过快导致相机模糊等)。这就需要我们通过融合其他传感器的观测数据来提高系统的鲁棒性。IMU(Inertial measurement unit,惯性测量单元)被认为与视觉信息有较好的互补，比如说IMU长时间会有较严重的漂移，但是相机是不存在漂移的，而相机有的时候会因为机器人运动过快而使观测到的图片模糊，这时候可以依赖IMU数据。此外IMU也是一种轻量级的传感器，便于移动机器人携带，所以视觉与IMU的融合受到了机器人定位领域的广泛关注。

针对如何融合视觉与IMU的信息，学术界有两大方法。一个是基于滤波的信息融合方法，一个是基于非线性优化的信息融合方法。其中，基于滤波的算法运行效率更高，且能达到和基于优化相似的精度。A.I.Mourikis and S.I.Roumeliotis在2007年提出了一种基于EKF滤波的视觉与IMU的融合算法，命名为MSCKF。在2018年K.Sun等人对MSCKF进行了改进，提出了基于双目的MSCKF(S-MSCKF)。基于滤波的信息融合方法如：专利文献CN110032201A公开了一种基于卡尔曼滤波的IMU机载视觉姿态融合的方法，主要包括步骤S300：基于卡尔曼滤波的IMU/机载视觉姿态融合；采集IMU实验数据并经过四元数解算得到姿态角数据，同时对图像采集的图片进行处理得到相机的内、外参数；采用卡尔曼滤波融合姿态角数据和外参数。

绝大多数的VIO系统都是假定观测噪声是均值和方差都已知且恒定的高斯分布，而实际情况下，量测噪声的方差很可能是不知道且是时变的，此外，可能因为传感器的某些瞬时故障而使量测产生较大的不确定度。这种情况下，观测噪声就不再符合高斯分布了，这就会使得机器人定位不准确，甚至发散。

发明内容

针对现有技术中的缺陷，本发明的目的是提供一种基于变分贝叶斯自适应算法的双目VIO实现方法。

根据本发明提供的一种基于变分贝叶斯自适应算法的双目VIO实现方法，包括：

步骤1：通过双目相机获得图像，对图像进行特征点提取，将提取的特征点加入全局维护的地图容器中；

步骤2：通过IMU数据进行IMU状态预测；

步骤3：当新接收到一帧图像，将该帧图像加入到状态向量中进行状态向量及状态协方差的增广；

步骤4：判断是否有特征点或者相机需要删除，如果有，则进行滤波融合；

步骤5：当步骤4的判断为有，则基于双目相机的观测模型进行UT变换，计算观测模型对应的雅克比矩阵；

步骤6：将多个雅克比矩阵叠加起来后，进行零空间投影，得到最终的标准观测方程；

步骤7：对步骤6得到的标准观测方程应用变分贝叶斯估计，更新VIO系统的状态。

优选地，步骤1包括：

双目相机在每个时刻获得图像，利用S-MSCKF前端来获得每一时刻的双目相机观测到的关键点，通过光流法跟踪关键点，得到同一个关键点f_j在不同时刻下双目相机x_Ci的观测值和为双目相机中的像素坐标值，下标中的1、2分别代表双目相机的左目相机、右目相机。

优选地，步骤2包括：

当处理完一帧图像后，通过时间戳，将该帧图像和IMU数据进行对应，从而用该帧IMU数据进行状态预测。

优选地，状态向量x由IMU的状态和双目相机的状态组成，T为向量的转置；

IMU的状态x_I：

其中，表示世界坐标系到IMU坐标系的旋转，三维向量分别表示IMU在世界坐标系下的速度、位置，三维向量b_g、b_a分别代表IMU的角速度偏移量、线速度偏移量，代表双目相机坐标系到IMU坐标系的旋转，代表双目相机在IMU坐标系下的位置；

双目相机的状态向量

其中，分别代表左目相机、右目相机在世界坐标系的旋转，分别代表左目相机、右目相机在世界坐标系的平移；

误差状态向量～代表变量的真实值和观测值的差值。

优选地，IMU的运动模型包括：

其中，∧代表变量的观测值，·表示变量的一阶导数，和是通过IMU得到的除去噪声后的线加速度和角速度的观测值，R()表示将旋转由四元数转换成旋转矩阵，

下标×代表斜对称矩阵，基于IMU的运动模型，用4阶龙格库塔方法进行IMU状态预测。

优选地，针对IMU运动模型，进行线性化，得到：

n_I为IMU的噪声，由四部分组成，其中三维向量n_g、n_a分别表示IMU的陀螺仪、加速度计的高斯白噪声，n_wg和n_wa表示偏移b_g和b_a的随机游走，F、G是IMU运动模型对IMU误差状态变量的雅克比矩阵。

优选地，通过如下公式得到更新后的误差状态协方差：

其中，Φ_k为第k时刻到k+1时刻的状态转移矩阵；

Q_k为第k到k+1时刻下的噪声的协方差；

为第k时刻与IMU状态相关的协方差；为第k时刻与相机状态相关的协方差；

为第k时刻与IMU和相机均相关的协方差；

P_k+1|k为通过状态转移矩阵预测的第k+1时刻的系统状态协方差。

优选地，步骤3包括：

当接收到新的图像时，该新的图像对应的双目相机的状态被增添到状态变量中，而新的相机的状态通过上一时刻的双目相机位姿和当前时刻的IMU数据获得：

左侧的上下标C、W、I分别代表双目相机坐标系、世界坐标系、IMU坐标系；

得到增广的协方差矩阵为：

其中，J为计算新的双目相机状态的公式对应的雅克比矩阵，I为单位矩阵。

优选地，步骤4包括：

判断是否有特征点或者相机需要删除时，需要满足如下两个条件之一：

实时维护的特征点地图中存在某些点没有被当前帧追踪到；

当状态向量中的双目相机个数达到上限，挑选出需要删除的两个双目相机状态。

优选地，步骤5中：

双目相机的观测模型：

其中，为特征点j在第i时刻相机坐标系下的坐标值；为在相机归一化平面的坐标值；*取值1、2分别代表双目相机的左目相机、右目相机，为世界坐标系到相机坐标系的旋转四元数，分别为特征点j在世界坐标系下的坐标值以及i时刻相机在世界坐标系下的坐标值。

与现有技术相比，本发明具有如下的有益效果：

对于SLAM这样的非线性较强的系统，利用了UT变换来求解观测方程的雅可比矩阵，与原来通过一阶泰勒展开相比，在相似的计算开销下实现了三阶近似精度。此外，该发明将噪声的协方差时变的情况的考虑了进去，用逆威沙特分布来构建观测噪声的协方差模型，利用变分贝叶斯近似来更新系统的状态，这使得系统在精度和鲁棒性方面都表现更佳。本发明可以作为一种通用的算法用在基于滤波的VIO系统中，使系统具有更好的精度和鲁棒性。

附图说明

通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：

图1为本发明的工作流程图；

图2为S-MSCKF和本发明在Euroc数据集上的精度对比(均方根误差，RMSE)示意图；

图3为在加入了重尾噪声后S-MSCKF和本发明在Euroc数据集上的精度对比(均方根误差，RMSE)示意图。

具体实施方式

下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变化和改进。这些都属于本发明的保护范围。

本发明针对S-MSCKF系统，提出了一种基于变分贝叶斯的自适应算法的VIO，这使得VIO系统能够处理观测噪声协方差未知且时变的情况，此外还能有效处理观测的离群点对系统的影响，从而提高了系统的鲁棒性。另外，通常情况下，VIO系统具有较强的非线性性，所以该发明引入了无痕变换(unscented transform,UT)来处理非线性系统带来的问题。通过以上两个方面的改进，提升了系统的鲁棒性和精度。

以配备有IMU和双目相机的无人机(UAV)为平台进行本发明基于IMU和双目相机信息融合定位的具体操作。整体的流程如图1所示。

系统的状态向量由IMU的状态和相机的状态组成其中IMU的状态定义为，

四元数表示世界坐标系到IMU(机体)坐标系的旋转。三维向量和表示IMU在世界坐标系下的速度和位置。三维向量b_g和b_a代表IMU的角速度和线速度偏移量。和代表相机和IMU的相对位姿。相机状态定义为，

代表左右目相机的旋转，为左右目相机的平移。因此有误差状态向量：,

其中代表变量的误差(真实值和观测值的差值)

IMU具有如下的运动学模型：

其中代表观测值.和是通过IMU得到的除去噪声后的线加速度和角速度的测量值。R将旋转信息由四元数转换成了旋转矩阵。此外,

相机的观测模型如下：

假设特征点f_j在世界坐标系下的位置为它被相机所观测到，且观测值为通过相机的几何投影模型可以得到：

有了以上定义现在对本发明的具体步骤进行说明：

步骤S1，通过双目相机获得观测。

UAV搭载的双目相机在每个时刻可以获得场景的图片，利用S-MSCKF前端来获得每一时刻的相机观测到的关键点，通过光流法跟踪这些关键点，从而可以得到同一个关键点f_j在不同时刻下相机的观测值将相机观测当的这些特征点放入一个全局维护的地图容器中，这样便于后边进行特征点的删除和滤波融合。其中，相机的状态向量定义为为四元数，代表双目相机的左目相机相对于世界坐标系的旋转姿态，为一个三维的向量，代表双目相机的左目相机相对于世界坐标系原点的平移。同理，构成了双目相机右目的位姿。观测值为特征点f_j在左右目图像中(相机归一化平面)的像素位置。

步骤S2，通过IMU数据来进行IMU状态预测。

通过IMU数据的时间戳和图像的时间戳来进行IMU数据和图像数据的匹配对。利用IMU数据，结合IMU运动学模型和四阶龙格库塔(Runge-Kutta)方法进行IMU状态(姿态速度位置)预测。

步骤S3，状态协方差的预测。

针对IMU运动模型，进行线性化，可以得到

通过以下公式可以得到更新后的误差状态协方差：

其中Q表示连续时间下的噪声的协方差。

步骤S4，状态变量和状态协方差增广。

当UAV的双目相机接受到图像后，该图像对应的此刻相机的位姿应该增加到系统的状态向量x中，而新的相机的状态可以通过上一时刻的相机位姿和当前时刻获取的IMU数据得到：

从而可以得到增广的协方差矩阵为：

其中，J为计算新的相机状态的公式对应的雅克比矩阵。

步骤S5，滤波融合。

本发明在每一时刻都会判断是否满足进行滤波融合的条件:

·实时维护的特征点地图map中存在某些点没有被当前帧追踪到，则需要删除这些特征点。

·当状态向量中的相机个数达到上限(比如40)。挑选出需要删除的两个相机状态。当满足以上两个条件之一，就对挑选出来的需要删除的特征点和对应的相机进行状态更新步骤。

状态更新的方法如下：

1.假设特征点f_j在世界坐标系下的位置为它被i时刻相机所观测到，且真实观测值为实际测量值为通过相机观测模型，可以得到线性化后的量测的残差方程，

其中是量测噪声；和是雅克比矩阵；。

2.利用UT变化来求解

在上式中，n为误差状态向量的维度；α为UT变换的参数，为常数，通常取一个小于1的较小正数；W_c,W_s分别为后续计算y的均值和协方差所用的加权值；P为状态变量的协方差矩阵；col()_j为矩阵的第j列；h为系统的观测方程；为所求的观测值的加权平均；为状态误差向量与观测值的协方差。

由于该特征点可以被多个时刻的相机看到，用这些不同时刻对应的相机可以求得许多个残差方程。将这些残差方程进行叠加，

其中H^j为将不同时刻的进行矩阵堆叠操作后得到的大的雅克比矩阵。上十中的其他变量的含义类似，均为对进行矩阵堆叠操作后得到的更大的矩阵·j。

为了消除路标点误差的影响，上式可以左乘的零空间V，得到标准的量测方程，

其中变量表示对变量·^j左乘零空间V后的结果。

对于每一个需要进行滤波融合的特征点j，都能得到上式形式的标准量测方程，将这些方程进行累加，得到最终的残差向量r_o和雅克比矩阵H_o，即：

3.通过变分贝叶斯估计来进行状态更新，具体如下：

Else if n_k≥n_k-1,

Ω_k＝(1-a)v_k-1V_k-1+S_k

v_k＝(1-a)v_k-1+a(d-1)+1

V_k＝Ω_k/v_k

其中，v_k和Ω_k为观测噪声的模型参数，V_k定义为Ω_k/v_k。本发明假设观测噪声的协方差服从逆威沙特分布，即为k时刻的最终残差向量；eigenvalue为矩阵的特征值；max为对所得到的值取最大值；a为参数，通常为小于1的正数；K_k为计算的卡尔曼增益；·_k|k-1为之前状态预测阶段得到的预测值，·_k|k为进行变分贝叶斯估计后得到的更新值。

本实施实例基于S-MSCKF用C++语言结合ROS系统对提出的算法进行了测试。测试的数据集为真实场景的双目+IMU无人机数据集Euroc。实验结果与S-MSCKF的结果进行了对比，如表1，图2所示。

表1

算法对比RMSE/m

为了进一步说明本发明能够有效地处理观测噪声协方差时变或未知的情况，我们给系统中加入了重尾噪声。我们通过均匀分布产生随机数r∈[0,1]，如果r＜P_Outlier,其中P_Outlier代表给系统加入重尾噪声的概率，n是一个标量，用来改变系统原本默认的观测噪声协方差。在实验中P_Outlier＝0.1，n＝10。实验结果如图3所示。

综上，本发明提出的算方法能够有效地处理系统的观测噪声为非高斯(方差时变)的情况，从而有效提升了VIO系统在定位时的精度和鲁棒性。

在算法中，将观测模型的测量噪声考虑为时变的，具体来说，测量噪声的协方差被构建成逆威沙特分布。通过变分贝叶斯估计的方法，推导出了在系统需要进行状态更新时的更新步骤。此外，由于系统具有较强的非线性性，故在计算观测模型的雅克比矩阵时，没有采用一阶泰勒展开，而是采用无痕变换(unscented transform，UT)，基于概率采样的方法来求解雅克比矩阵，其精度能到达三阶。实验结果表明，本发明能够很好地处理系统观测噪声时变的情况，在提升精度的同时，增加了系统的鲁棒性。本发明可以被用于将惯性测量单元(IMU)和双目相机作为传感器的机器人定位问题中。

以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变化或修改，这并不影响本发明的实质内容。在不冲突的情况下，本申请的实施例和实施例中的特征可以任意相互组合。

Claims (10)

1.一种基于变分贝叶斯自适应算法的双目VIO实现方法，其特征在于，包括：

步骤1：通过双目相机获得图像，对图像进行特征点提取，将提取的特征点加入全局维护的地图容器中；

步骤2：通过IMU数据进行IMU状态预测；

步骤3：当新接收到一帧图像，将该帧图像加入到状态向量中进行状态向量及状态协方差的增广；

步骤4：判断是否有特征点或者相机需要删除，如果有，则进行滤波融合；

步骤5：当步骤4的判断为有，则基于双目相机的观测模型进行UT变换，计算观测模型对应的雅克比矩阵；

步骤6：将多个雅克比矩阵叠加起来后，进行零空间投影，得到最终的标准观测方程；

步骤7：对步骤6得到的标准观测方程应用变分贝叶斯估计，更新VIO系统的状态。

2.根据权利要求1所述的基于变分贝叶斯自适应算法的双目VIO实现方法，其特征在于，步骤1包括：

双目相机在每个时刻获得图像，利用S-MSCKF前端来获得每一时刻的双目相机观测到的关键点，通过光流法跟踪关键点，得到同一个关键点f_j在不同时刻下双目相机的观测值 和为双目相机中的像素坐标值，下标中的1、2分别代表双目相机的左目相机、右目相机。

3.根据权利要求1所述的基于变分贝叶斯自适应算法的双目VIO实现方法，其特征在于，步骤2包括：

当处理完一帧图像后，通过时间戳，将该帧图像和IMU数据进行对应，从而用该帧IMU数据进行状态预测。

4.根据权利要求3所述的基于变分贝叶斯自适应算法的双目VIO实现方法，其特征在于，状态向量x由IMU的状态和双目相机的状态组成，T为向量的转置；

IMU的状态x_I：

其中，表示世界坐标系到IMU坐标系的旋转，三维向量^Wv_I、^Wp_I分别表示IMU在世界坐标系下的速度、位置，三维向量b_g、b_a分别代表IMU的角速度偏移量、线速度偏移量，代表双目相机坐标系到IMU坐标系的旋转，^Ip_C代表双目相机在IMU坐标系下的位置；

双目相机的状态向量

其中，分别代表左目相机、右目相机在世界坐标系的旋转，分别代表左目相机、右目相机在世界坐标系的平移；

误差状态向量 ～代表变量的真实值和观测值的差值。

5.根据权利要求4所述的基于变分贝叶斯自适应算法的双目VIO实现方法，其特征在于，IMU的运动模型包括：

其中，∧代表变量的观测值，·表示变量的一阶导数，和是通过IMU得到的除去噪声后的线加速度和角速度的观测值，R()表示将旋转由四元数转换成旋转矩阵，

下标×代表斜对称矩阵，基于IMU的运动模型，用4阶龙格库塔方法进行IMU状态预测。

6.根据权利要求5所述的基于变分贝叶斯自适应算法的双目VIO实现方法，其特征在于，针对IMU运动模型，进行线性化，得到：

n_I为IMU的噪声，由四部分组成，其中三维向量n_g、n_a分别表示IMU的陀螺仪、加速度计的高斯白噪声，n_wg和n_wa表示偏移b_g和b_a的随机游走，F、G是IMU运动模型对IMU误差状态变量的雅克比矩阵。

7.根据权利要求6所述的基于变分贝叶斯自适应算法的双目VIO实现方法，其特征在于，通过如下公式得到更新后的误差状态协方差：

其中，Φ_k为第k时刻到k+1时刻的状态转移矩阵；

Q_k为第k到k+1时刻下的噪声的协方差；

为第k时刻与IMU状态相关的协方差；为第k时刻与相机状态相关的协方差；

为第k时刻与IMU和相机均相关的协方差；

P_k+1|k为通过状态转移矩阵预测的第k+1时刻的系统状态协方差。

8.根据权利要求7所述的基于变分贝叶斯自适应算法的双目VIO实现方法，其特征在于，步骤3包括：

当接收到新的图像时，该新的图像对应的双目相机的状态被增添到状态变量中，而新的相机的状态通过上一时刻的双目相机位姿和当前时刻的IMU数据获得：

左侧的上下标C、W、I分别代表双目相机坐标系、世界坐标系、IMU坐标系；

得到增广的协方差矩阵为：

其中，J为计算新的双目相机状态的公式对应的雅克比矩阵，I为单位矩阵。

9.根据权利要求8所述的基于变分贝叶斯自适应算法的双目VIO实现方法，其特征在于，步骤4包括：

判断是否有特征点或者相机需要删除时，需要满足如下两个条件之一：

实时维护的特征点地图中存在某些点没有被当前帧追踪到；

当状态向量中的双目相机个数达到上限，挑选出需要删除的两个双目相机状态。

10.根据权利要求9所述的基于变分贝叶斯自适应算法的双目VIO实现方法，其特征在于，步骤5中：

双目相机的观测模型：

其中，为特征点j在第i时刻相机坐标系下的坐标值；为在相机归一化平面的坐标值；*取值1、2分别代表双目相机的左目相机、右目相机，为世界坐标系到相机坐标系的旋转四元数，分别为特征点j在世界坐标系下的坐标值以及i时刻相机在世界坐标系下的坐标值。