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

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

Info

Publication number
CN110517324B
CN110517324B CN201910791980.4A CN201910791980A CN110517324B CN 110517324 B CN110517324 B CN 110517324B CN 201910791980 A CN201910791980 A CN 201910791980A CN 110517324 B CN110517324 B CN 110517324B
Authority
CN
China
Prior art keywords
camera
imu
binocular
state
coordinate system
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201910791980.4A
Other languages
English (en)
Other versions
CN110517324A (zh
Inventor
张铸青
董鹏
孙印帅
沈楷
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Shanghai Jiaotong University
Original Assignee
Shanghai Jiaotong University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Shanghai Jiaotong University filed Critical Shanghai Jiaotong University
Priority to CN201910791980.4A priority Critical patent/CN110517324B/zh
Publication of CN110517324A publication Critical patent/CN110517324A/zh
Application granted granted Critical
Publication of CN110517324B publication Critical patent/CN110517324B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/80—Analysis of captured images to determine intrinsic or extrinsic camera parameters, i.e. camera calibration
    • G06T7/85—Stereo camera calibration

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Theoretical Computer Science (AREA)
  • Image Analysis (AREA)

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前端来获得每一时刻的双目相机观测到的关键点，通过光流法跟踪关键点，得到同一个关键点fj在不同时刻下双目相机xCi的观测值
Figure GDA0003927287840000021
Figure GDA0003927287840000022
和
Figure GDA0003927287840000023
为双目相机中的像素坐标值，下标中的1、2分别代表双目相机的左目相机、右目相机。
优选地，步骤2包括：
当处理完一帧图像后，通过时间戳，将该帧图像和IMU数据进行对应，从而用该帧IMU数据进行状态预测。
优选地，状态向量x由IMU的状态和双目相机的状态组成，
Figure GDA0003927287840000024
T为向量的转置；
IMU的状态xI：
Figure GDA0003927287840000031
其中，
Figure GDA0003927287840000032
表示世界坐标系到IMU坐标系的旋转，三维向量WvI、WpI分别表示IMU在世界坐标系下的速度、位置，三维向量bg、ba分别代表IMU的角速度偏移量、线速度偏移量，
Figure GDA0003927287840000033
代表双目相机坐标系到IMU坐标系的旋转，IpC代表双目相机在IMU坐标系下的位置；
双目相机的状态向量
Figure GDA0003927287840000034
Figure GDA0003927287840000035
其中，
Figure GDA0003927287840000036
分别代表左目相机、右目相机在世界坐标系的旋转，
Figure GDA0003927287840000037
分别代表左目相机、右目相机在世界坐标系的平移；
误差状态向量
Figure GDA0003927287840000038
Figure GDA0003927287840000039
～代表变量的真实值和观测值的差值。
优选地，IMU的运动模型包括：
Figure GDA00039272878400000310
Figure GDA00039272878400000311
Figure GDA00039272878400000312
其中，∧代表变量的观测值，·表示变量的一阶导数，
Figure GDA00039272878400000313
和
Figure GDA00039272878400000314
是通过IMU得到的除去噪声后的线加速度和角速度的观测值，R()表示将旋转由四元数转换成旋转矩阵，
Figure GDA00039272878400000315
下标×代表斜对称矩阵，基于IMU的运动模型，用4阶龙格库塔方法进行IMU状态预测。
优选地，针对IMU运动模型，进行线性化，得到：
Figure GDA00039272878400000316
Figure GDA00039272878400000317
nI为IMU的噪声，由四部分组成，其中三维向量ng、na分别表示IMU的陀螺仪、加速度计的高斯白噪声，nwg和nwa表示偏移bg和ba的随机游走，F、G是IMU运动模型对IMU误差状态变量的雅克比矩阵。
优选地，通过如下公式得到更新后的误差状态协方差：
Figure GDA0003927287840000041
Figure GDA0003927287840000042
Figure GDA0003927287840000043
Figure GDA0003927287840000044
其中，Φk为第k时刻到k+1时刻的状态转移矩阵；
Qk为第k到k+1时刻下的噪声
Figure GDA0003927287840000045
的协方差；
Figure GDA0003927287840000046
为第k时刻与IMU状态相关的协方差；
Figure GDA0003927287840000047
为第k时刻与相机状态相关的协方差；
Figure GDA0003927287840000048
为第k时刻与IMU和相机均相关的协方差；
Pk+1|k为通过状态转移矩阵预测的第k+1时刻的系统状态协方差。
优选地，步骤3包括：
当接收到新的图像时，该新的图像对应的双目相机的状态被增添到状态变量中，而新的相机的状态通过上一时刻的双目相机位姿和当前时刻的IMU数据获得：
Figure GDA0003927287840000049
左侧的上下标C、W、I分别代表双目相机坐标系、世界坐标系、IMU坐标系；
得到增广的协方差矩阵为：
Figure GDA00039272878400000410
其中，J为计算新的双目相机状态的公式对应的雅克比矩阵，I为单位矩阵。
优选地，步骤4包括：
判断是否有特征点或者相机需要删除时，需要满足如下两个条件之一：
实时维护的特征点地图中存在某些点没有被当前帧追踪到；
当状态向量中的双目相机个数达到上限，挑选出需要删除的两个双目相机状态。
优选地，步骤5中：
双目相机的观测模型：
Figure GDA00039272878400000411
其中，
Figure GDA00039272878400000412
为特征点j在第i时刻相机坐标系下的坐标值；
Figure GDA00039272878400000413
为在相机归一化平面的坐标值；*取值1、2分别代表双目相机的左目相机、右目相机，
Figure GDA0003927287840000051
为世界坐标系到相机坐标系的旋转四元数，Wpj，
Figure GDA0003927287840000052
分别为特征点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的状态和相机的状态组成
Figure GDA0003927287840000061
其中IMU的状态定义为，
Figure GDA0003927287840000062
四元数
Figure GDA0003927287840000063
表示世界坐标系到IMU(机体)坐标系的旋转。三维向量WvI和WpI表示IMU在世界坐标系下的速度和位置。三维向量bg和ba代表IMU的角速度和线速度偏移量。
Figure GDA0003927287840000064
和IpC代表相机和IMU的相对位姿。相机状态定义为，
Figure GDA0003927287840000065
Figure GDA0003927287840000066
代表左右目相机的旋转，
Figure GDA0003927287840000067
为左右目相机的平移。因此有误差状态向量：,
Figure GDA0003927287840000068
其中
Figure GDA0003927287840000069
代表变量的误差(真实值和观测值的差值)
IMU具有如下的运动学模型：
Figure GDA00039272878400000610
Figure GDA00039272878400000611
Figure GDA00039272878400000612
其中
Figure GDA00039272878400000613
代表观测值.
Figure GDA00039272878400000614
和
Figure GDA00039272878400000615
是通过IMU得到的除去噪声后的线加速度和角速度的测量值。R将旋转信息由四元数转换成了旋转矩阵。此外,
Figure GDA00039272878400000616
相机的观测模型如下：
假设特征点fj在世界坐标系下的位置为Wpj,它被相机
Figure GDA00039272878400000617
所观测到，且观测值为
Figure GDA00039272878400000618
通过相机的几何投影模型可以得到：
Figure GDA00039272878400000619
有了以上定义现在对本发明的具体步骤进行说明：
步骤S1，通过双目相机获得观测。
UAV搭载的双目相机在每个时刻可以获得场景的图片，利用S-MSCKF前端来获得每一时刻的相机观测到的关键点，通过光流法跟踪这些关键点，从而可以得到同一个关键点fj在不同时刻下相机
Figure GDA0003927287840000071
的观测值
Figure GDA0003927287840000072
将相机观测当的这些特征点放入一个全局维护的地图容器中，这样便于后边进行特征点的删除和滤波融合。其中，相机的状态向量定义为
Figure GDA0003927287840000073
Figure GDA0003927287840000074
为四元数，代表双目相机的左目相机相对于世界坐标系的旋转姿态，
Figure GDA0003927287840000075
为一个三维的向量，代表双目相机的左目相机相对于世界坐标系原点的平移。同理，
Figure GDA0003927287840000076
构成了双目相机右目的位姿。观测值
Figure GDA0003927287840000077
为特征点fj在左右目图像中(相机归一化平面)的像素位置。
步骤S2，通过IMU数据来进行IMU状态预测。
通过IMU数据的时间戳和图像的时间戳来进行IMU数据和图像数据的匹配对。利用IMU数据，结合IMU运动学模型和四阶龙格库塔(Runge-Kutta)方法进行IMU状态(姿态
Figure GDA0003927287840000078
速度WvI，位置WpI)预测。
步骤S3，状态协方差的预测。
针对IMU运动模型，进行线性化，可以得到
Figure GDA0003927287840000079
通过以下公式可以得到更新后的误差状态协方差：
Figure GDA00039272878400000710
Figure GDA00039272878400000711
Figure GDA00039272878400000712
Figure GDA00039272878400000713
其中Q表示连续时间下的噪声
Figure GDA00039272878400000714
的协方差。
步骤S4，状态变量和状态协方差增广。
当UAV的双目相机接受到图像后，该图像对应的此刻相机的位姿应该增加到系统的状态向量x中，而新的相机的状态可以通过上一时刻的相机位姿和当前时刻获取的IMU数据得到：
Figure GDA00039272878400000715
从而可以得到增广的协方差矩阵为：
Figure GDA0003927287840000081
其中，J为计算新的相机状态的公式对应的雅克比矩阵。
步骤S5，滤波融合。
本发明在每一时刻都会判断是否满足进行滤波融合的条件:
·实时维护的特征点地图map中存在某些点没有被当前帧追踪到，则需要删除这些特征点。
·当状态向量中的相机个数达到上限(比如40)。挑选出需要删除的两个相机状态。当满足以上两个条件之一，就对挑选出来的需要删除的特征点和对应的相机进行状态更新步骤。
状态更新的方法如下：
1.假设特征点fj在世界坐标系下的位置为Wpj,它被i时刻相机xCi所观测到，且真实观测值为
Figure GDA0003927287840000082
实际测量值为
Figure GDA0003927287840000083
通过相机观测模型，可以得到线性化后的量测的残差方程，
Figure GDA0003927287840000084
其中
Figure GDA0003927287840000085
是量测噪声；
Figure GDA0003927287840000086
和
Figure GDA0003927287840000087
是雅克比矩阵；。
2.利用UT变化来求解
Figure GDA0003927287840000088
Figure GDA0003927287840000089
Figure GDA00039272878400000810
Figure GDA00039272878400000811
Figure GDA00039272878400000812
Figure GDA00039272878400000813
Figure GDA00039272878400000814
Figure GDA00039272878400000815
Figure GDA00039272878400000816
Figure GDA00039272878400000817
在上式中，n为误差状态向量
Figure GDA00039272878400000818
的维度；α为UT变换的参数，为常数，通常取一个小于1的较小正数；Wc,Ws分别为后续计算y的均值和协方差所用的加权值；P为状态变量的协方差矩阵；col()j为矩阵的第j列；h为系统的观测方程；
Figure GDA0003927287840000091
为所求的观测值的加权平均；
Figure GDA0003927287840000092
为状态误差向量与观测值的协方差。
由于该特征点可以被多个时刻的相机看到，用这些不同时刻对应的相机可以求得许多个残差方程。将这些残差方程进行叠加，
Figure GDA0003927287840000093
其中Hj为将不同时刻的
Figure GDA0003927287840000094
进行矩阵堆叠操作后得到的大的雅克比矩阵。上十中的其他变量的含义类似，均为对
Figure GDA0003927287840000095
进行矩阵堆叠操作后得到的更大的矩阵·j。
为了消除路标点误差的影响，上式可以左乘
Figure GDA0003927287840000096
的零空间V，得到标准的量测方程，
Figure GDA0003927287840000097
其中变量
Figure GDA0003927287840000098
表示对变量·j左乘零空间V后的结果。
对于每一个需要进行滤波融合的特征点j，都能得到上式形式的标准量测方程，将这些方程进行累加，得到最终的残差向量ro和雅克比矩阵Ho，即：
Figure GDA0003927287840000099
3.通过变分贝叶斯估计来进行状态更新，具体如下：
Figure GDA00039272878400000910
Figure GDA00039272878400000911
If nk＜nk-1，
Figure GDA00039272878400000912
Else if nk≥nk-1,
Figure GDA00039272878400000913
Ωk＝(1-a)νk-1Vk-1+Sk
vk＝(1-a)vk-1+a(d-1)+1
Figure GDA00039272878400000914
Figure GDA00039272878400000915
Figure GDA00039272878400000916
Vk＝Ωk/νk
其中，vk和Ωk为观测噪声的模型参数，Vk定义为Ωk/νk。本发明假设观测噪声的协方差服从逆威沙特分布，即R～W-1(Ωk,v)；
Figure GDA0003927287840000101
为k时刻的最终残差向量；eigenvalue为矩阵的特征值；max为对所得到的值取最大值；a为参数，通常为小于1的正数；Kk为计算的卡尔曼增益；·k|k-1为之前状态预测阶段得到的预测值，·k|k为进行变分贝叶斯估计后得到的更新值。
本实施实例基于S-MSCKF用C++语言结合ROS系统对提出的算法进行了测试。测试的数据集为真实场景的双目+IMU无人机数据集Euroc。实验结果与S-MSCKF的结果进行了对比，如表1，图2所示。
表1
算法对比RMSE/m
Figure GDA0003927287840000102
为了进一步说明本发明能够有效地处理观测噪声协方差时变或未知的情况，我们给系统中加入了重尾噪声。我们通过均匀分布产生随机数r∈[0,1]，如果r＜POutlier,
Figure GDA0003927287840000103
其中POutlier代表给系统加入重尾噪声的概率，n是一个标量，用来改变系统原本默认的观测噪声协方差。在实验中POutlier＝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系统的状态；
通过变分贝叶斯估计来进行状态更新的方法包括：
Figure FDA0003927287830000011
Figure FDA0003927287830000012
If nk＜nk-1，
Figure FDA0003927287830000013
Else if nk≥nk-1,
Figure FDA0003927287830000014
Ωk＝(1-a)νk-1Vk-1+Sk
vk＝(1-a)vk-1+a(d-1)+1
Figure FDA0003927287830000015
Figure FDA0003927287830000016
Figure FDA0003927287830000017
Vk＝Ωk/νk
其中，vk和Ωk为观测噪声的模型参数，Vk定义为Ωk/νk，Ho为雅克比矩阵，假设观测噪声的协方差服从逆威沙特分布，即R～W-1(Ωk,v)；
Figure FDA0003927287830000021
为k时刻的最终残差向量；eigenvalue为矩阵的特征值；max为对所得到的值取最大值；a为参数；Kk为计算的卡尔曼增益；P为状态变量的协方差矩阵，·k|k-1为之前状态预测阶段得到的预测值，·k|k为进行变分贝叶斯估计后得到的更新值。
2.根据权利要求1所述的基于变分贝叶斯自适应算法的双目VIO实现方法，其特征在于，步骤1包括：
双目相机在每个时刻获得图像，利用S-MSCKF前端来获得每一时刻的双目相机观测到的关键点，通过光流法跟踪关键点，得到同一个关键点fj在不同时刻下双目相机
Figure FDA0003927287830000022
的观测值
Figure FDA0003927287830000023
Figure FDA0003927287830000024
和
Figure FDA0003927287830000025
为双目相机中的像素坐标值，下标中的1、2分别代表双目相机的左目相机、右目相机。
3.根据权利要求1所述的基于变分贝叶斯自适应算法的双目VIO实现方法，其特征在于，步骤2包括：
当处理完一帧图像后，通过时间戳，将该帧图像和IMU数据进行对应，从而用该帧IMU数据进行状态预测。
4.根据权利要求3所述的基于变分贝叶斯自适应算法的双目VIO实现方法，其特征在于，状态向量x由IMU的状态和双目相机的状态组成，
Figure FDA0003927287830000026
T为向量的转置；
IMU的状态xI：
Figure FDA0003927287830000027
其中，
Figure FDA0003927287830000028
表示世界坐标系到IMU坐标系的旋转，三维向量WvI、WpI分别表示IMU在世界坐标系下的速度、位置，三维向量bg、ba分别代表IMU的角速度偏移量、线速度偏移量，
Figure FDA0003927287830000029
代表双目相机坐标系到IMU坐标系的旋转，IpC代表双目相机在IMU坐标系下的位置；
双目相机的状态向量
Figure FDA00039272878300000210
Figure FDA00039272878300000211
其中，
Figure FDA00039272878300000212
分别代表左目相机、右目相机在世界坐标系的旋转，
Figure FDA00039272878300000213
分别代表左目相机、右目相机在世界坐标系的平移；
误差状态向量
Figure FDA00039272878300000214
Figure FDA00039272878300000215
～代表变量的真实值和观测值的差值。
5.根据权利要求4所述的基于变分贝叶斯自适应算法的双目VIO实现方法，其特征在于，IMU的运动模型包括：
Figure FDA0003927287830000031
Figure FDA0003927287830000032
Figure FDA0003927287830000033
其中，∧代表变量的观测值，·表示变量的一阶导数，
Figure FDA0003927287830000034
和
Figure FDA0003927287830000035
是通过IMU得到的除去噪声后的线加速度和角速度的观测值，R()表示将旋转由四元数转换成旋转矩阵，左侧的上下标C、W、I分别代表双目相机坐标系、世界坐标系、IMU坐标系；
Figure FDA0003927287830000036
下标×代表斜对称矩阵，基于IMU的运动模型，用4阶龙格库塔方法进行IMU状态预测。
6.根据权利要求5所述的基于变分贝叶斯自适应算法的双目VIO实现方法，其特征在于，针对IMU运动模型，进行线性化，得到：
Figure FDA0003927287830000037
Figure FDA0003927287830000038
nI为IMU的噪声，由四部分组成，其中三维向量ng、na分别表示IMU的陀螺仪、加速度计的高斯白噪声，nwg和nwa表示偏移bg和ba的随机游走，F、G是IMU运动模型对IMU误差状态变量的雅克比矩阵。
7.根据权利要求6所述的基于变分贝叶斯自适应算法的双目VIO实现方法，其特征在于，通过如下公式得到更新后的误差状态协方差：
Figure FDA0003927287830000039
Figure FDA00039272878300000310
Figure FDA00039272878300000311
Figure FDA00039272878300000312
其中，Φk为第k时刻到k+1时刻的状态转移矩阵；
Qk为第k到k+1时刻下的噪声
Figure FDA00039272878300000313
的协方差；
Figure FDA00039272878300000314
为第k时刻与IMU状态相关的协方差；
Figure FDA00039272878300000315
为第k时刻与相机状态相关的协方差；
Figure FDA00039272878300000316
为第k时刻与IMU和相机均相关的协方差；
Pk+1|k为通过状态转移矩阵预测的第k+1时刻的系统状态协方差。
8.根据权利要求7所述的基于变分贝叶斯自适应算法的双目VIO实现方法，其特征在于，步骤3包括：
当接收到新的图像时，该新的图像对应的双目相机的状态被增添到状态变量中，而新的相机的状态通过上一时刻的双目相机位姿和当前时刻的IMU数据获得：
Figure FDA0003927287830000041
左侧的上下标C、W、I分别代表双目相机坐标系、世界坐标系、IMU坐标系；
得到增广的协方差矩阵为：
Figure FDA0003927287830000042
其中，J为计算新的双目相机状态的公式对应的雅克比矩阵，I为单位矩阵。
9.根据权利要求8所述的基于变分贝叶斯自适应算法的双目VIO实现方法，其特征在于，步骤4包括：
判断是否有特征点或者相机需要删除时，需要满足如下两个条件之一：
实时维护的特征点地图中存在某些点没有被当前帧追踪到；
当状态向量中的双目相机个数达到上限，挑选出需要删除的两个双目相机状态。
10.根据权利要求9所述的基于变分贝叶斯自适应算法的双目VIO实现方法，其特征在于，步骤5中：
双目相机的观测模型：
Figure FDA0003927287830000043
其中，
Figure FDA0003927287830000044
为特征点j在第i时刻相机坐标系下的坐标值；
Figure FDA0003927287830000045
为在相机归一化平面的坐标值；*取值1、2分别代表双目相机的左目相机、右目相机，
Figure FDA0003927287830000046
为世界坐标系到相机坐标系的旋转四元数，Wpj，
Figure FDA0003927287830000047
分别为特征点j在世界坐标系下的坐标值以及i时刻相机在世界坐标系下的坐标值。
CN201910791980.4A 2019-08-26 2019-08-26 基于变分贝叶斯自适应算法的双目vio实现方法 Active CN110517324B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910791980.4A CN110517324B (zh) 2019-08-26 2019-08-26 基于变分贝叶斯自适应算法的双目vio实现方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910791980.4A CN110517324B (zh) 2019-08-26 2019-08-26 基于变分贝叶斯自适应算法的双目vio实现方法

Publications (2)

Publication Number Publication Date
CN110517324A CN110517324A (zh) 2019-11-29
CN110517324B true CN110517324B (zh) 2023-02-17

Family

ID=68626929

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910791980.4A Active CN110517324B (zh) 2019-08-26 2019-08-26 基于变分贝叶斯自适应算法的双目vio实现方法

Country Status (1)

Country Link
CN (1) CN110517324B (zh)

Families Citing this family (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111160266B (zh) * 2019-12-30 2023-04-18 三一重工股份有限公司 物体跟踪方法和装置
CN115003983A (zh) * 2020-02-13 2022-09-02 Oppo广东移动通信有限公司 用于通过动态调整测量噪声协方差进行视觉slam的误差状态卡尔曼滤波器
CN113532455B (zh) * 2020-04-17 2023-03-31 北京三快在线科技有限公司 视觉融合定位方法、装置、电子设备和可读存储介质
CN111862150A (zh) * 2020-06-19 2020-10-30 杭州易现先进科技有限公司 图像跟踪的方法、装置、ar设备和计算机设备
CN112254728A (zh) * 2020-09-30 2021-01-22 无锡太机脑智能科技有限公司 基于关键路标增强ekf-slam全局优化的方法
CN112330735B (zh) * 2020-10-26 2022-06-17 武汉中海庭数据技术有限公司 一种车体相对位置测量精度置信度评估方法及系统
CN112991400B (zh) * 2021-04-07 2022-02-01 广东工业大学 一种无人艇的多传感器辅助定位方法
CN113701749A (zh) * 2021-07-29 2021-11-26 江苏师范大学 一种基于立体视觉惯性里程计的车辆定位导航方法及系统

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107869989A (zh) * 2017-11-06 2018-04-03 东北大学 一种基于视觉惯导信息融合的定位方法及系统
CN108717712A (zh) * 2018-05-29 2018-10-30 东北大学 一种基于地平面假设的视觉惯导slam方法
CN109376785A (zh) * 2018-10-31 2019-02-22 东南大学 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法
CN109993113A (zh) * 2019-03-29 2019-07-09 东北大学 一种基于rgb-d和imu信息融合的位姿估计方法
CN110118556A (zh) * 2019-04-12 2019-08-13 浙江工业大学 一种基于协方差交叉融合slam的机器人定位方法及装置

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107747941B (zh) * 2017-09-29 2020-05-15 歌尔股份有限公司 一种双目视觉定位方法、装置及系统

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107869989A (zh) * 2017-11-06 2018-04-03 东北大学 一种基于视觉惯导信息融合的定位方法及系统
CN108717712A (zh) * 2018-05-29 2018-10-30 东北大学 一种基于地平面假设的视觉惯导slam方法
CN109376785A (zh) * 2018-10-31 2019-02-22 东南大学 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法
CN109993113A (zh) * 2019-03-29 2019-07-09 东北大学 一种基于rgb-d和imu信息融合的位姿估计方法
CN110118556A (zh) * 2019-04-12 2019-08-13 浙江工业大学 一种基于协方差交叉融合slam的机器人定位方法及装置

Also Published As

Publication number Publication date
CN110517324A (zh) 2019-11-29

Similar Documents

Publication Publication Date Title
CN110517324B (zh) 基于变分贝叶斯自适应算法的双目vio实现方法
CN106679648B (zh) 一种基于遗传算法的视觉惯性组合的slam方法
CN109520497B (zh) 基于视觉和imu的无人机自主定位方法
US20190122378A1 (en) Apparatuses and methods for machine vision systems including creation of a point cloud model and/or three dimensional model based on multiple images from different perspectives and combination of depth cues from camera motion and defocus with various applications including navigation systems, and pattern matching systems as well as estimating relative blur between images for use in depth from defocus or autofocusing applications
CN112304307A (zh) 一种基于多传感器融合的定位方法、装置和存储介质
CN110084832B (zh) 相机位姿的纠正方法、装置、系统、设备和存储介质
CN106873619B (zh) 一种无人机飞行路径的处理方法
JP2019536012A (ja) 可変コントラスト追跡残余を用いる視覚慣性ナビゲーション
Kottas et al. Efficient and consistent vision-aided inertial navigation using line observations
WO2020221307A1 (zh) 一种运动物体的追踪方法和装置
Kneip et al. Closed-form solution for absolute scale velocity determination combining inertial measurements and a single feature correspondence
Michot et al. Bi-objective bundle adjustment with application to multi-sensor slam
WO2018081366A1 (en) Vision-aided inertial navigation with loop closure
CN110793526A (zh) 基于可穿戴单目视觉和惯性传感器融合的行人导航方法及系统
CN110598370B (zh) 基于sip和ekf融合的多旋翼无人机鲁棒姿态估计
CN113066127A (zh) 一种在线标定设备参数的视觉惯性里程计方法和系统
Hashim Exponentially stable observer-based controller for VTOL-UAVs without velocity measurements
CN113721260B (zh) 一种激光雷达、双目相机和惯导的在线联合标定方法
CN112991400B (zh) 一种无人艇的多传感器辅助定位方法
CN108827287B (zh) 一种复杂环境下的鲁棒视觉slam系统
CN113639782A (zh) 车载传感器的外参标定方法和装置、设备和介质
Panahandeh et al. Exploiting ground plane constraints for visual-inertial navigation
Li et al. Estimation analysis in VSLAM for UAV application
Wang et al. Minimum sigma set SR-UKF for quadrifocal tensor-based binocular stereo vision-IMU tightly-coupled system
Keshavan et al. An optical flow-based solution to the problem of range identification in perspective vision systems

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant