[发明专利]基于变分贝叶斯自适应算法的双目VIO实现方法

权利要求书

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

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

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

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

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

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

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

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

通过变分贝叶斯估计来进行状态更新的方法包括：

If n_k＜n_k-1，

Else if n_k≥n_k-1,

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

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

V_k＝Ω_k/ν_k

其中，v_k和Ω_k为观测噪声的模型参数，V_k定义为Ω_k/ν_k，H_o为雅克比矩阵，假设观测噪声的协方差服从逆威沙特分布，即R～W^-1(Ω_k,v)；为k时刻的最终残差向量；eigenvalue为矩阵的特征值；max为对所得到的值取最大值；a为参数；K_k为计算的卡尔曼增益；P为状态变量的协方差矩阵，·_k|k-1为之前状态预测阶段得到的预测值，·_k|k为进行变分贝叶斯估计后得到的更新值。

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坐标系下的位置；

双目相机的状态向量

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

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