CN110030994B - 一种基于单目的鲁棒性视觉惯性紧耦合定位方法 - Google Patents
一种基于单目的鲁棒性视觉惯性紧耦合定位方法 Download PDFInfo
- Publication number
- CN110030994B CN110030994B CN201910216776.XA CN201910216776A CN110030994B CN 110030994 B CN110030994 B CN 110030994B CN 201910216776 A CN201910216776 A CN 201910216776A CN 110030994 B CN110030994 B CN 110030994B
- Authority
- CN
- China
- Prior art keywords
- imu
- time
- coordinate system
- rotation
- bias
- 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
Links
- 230000000007 visual effect Effects 0.000 title claims abstract description 68
- 238000000034 method Methods 0.000 title claims abstract description 41
- 238000010168 coupling process Methods 0.000 title claims abstract description 26
- 238000005859 coupling reaction Methods 0.000 title claims abstract description 26
- 230000008878 coupling Effects 0.000 title claims abstract description 25
- 238000005259 measurement Methods 0.000 claims abstract description 49
- 239000011159 matrix material Substances 0.000 claims abstract description 43
- 230000010354 integration Effects 0.000 claims abstract description 35
- 230000004927 fusion Effects 0.000 claims abstract description 27
- 230000033001 locomotion Effects 0.000 claims abstract description 20
- 230000009466 transformation Effects 0.000 claims abstract description 19
- 230000001133 acceleration Effects 0.000 claims description 54
- 230000005484 gravity Effects 0.000 claims description 39
- 238000013519 translation Methods 0.000 claims description 27
- 239000013598 vector Substances 0.000 claims description 16
- 238000005457 optimization Methods 0.000 claims description 12
- 238000012937 correction Methods 0.000 claims description 7
- 238000006243 chemical reaction Methods 0.000 claims description 6
- 230000000694 effects Effects 0.000 claims description 5
- 230000004807 localization Effects 0.000 claims description 5
- 230000008859 change Effects 0.000 claims description 3
- 230000017105 transposition Effects 0.000 claims description 3
- 238000006073 displacement reaction Methods 0.000 claims description 2
- 238000000926 separation method Methods 0.000 claims description 2
- 238000004364 calculation method Methods 0.000 abstract description 2
- 101000664407 Neisseria meningitidis serogroup B (strain MC58) Surface lipoprotein assembly modifier 2 Proteins 0.000 description 3
- 238000011423 initialization method Methods 0.000 description 3
- 238000005034 decoration Methods 0.000 description 2
- 239000006185 dispersion Substances 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 208000018777 Vulvar intraepithelial neoplasia Diseases 0.000 description 1
- 238000004458 analytical method Methods 0.000 description 1
- 230000003190 augmentative effect Effects 0.000 description 1
- 238000004422 calculation algorithm Methods 0.000 description 1
- 230000001413 cellular effect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 230000008569 process Effects 0.000 description 1
- 238000005070 sampling Methods 0.000 description 1
- 238000007619 statistical method Methods 0.000 description 1
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
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Navigation (AREA)
Abstract
本发明公开了一种基于单目的鲁棒性视觉惯性紧耦合定位方法,步骤包括:通过相机采集视觉数据,通过IMU单元采集惯性数据;进行IMU预积分,并得到IMU先验值;将所述IMU先验值代入视觉惯性联合初始化模型,完成参数的初始化;在所述参数的初始化所需时间内,利用连续关键帧之间的变换矩阵计算运动信息,带入视觉惯性融合定位框架的后端模块,实现紧耦合定位;参数的初始化完成后,转而将所述参数带入视觉惯性融合定位模型,计算得到运动信息,带入视觉惯性融合定位框架的后端模块,实现紧耦合定位。使用本发明提出的方法,初始化时间可缩短至10秒内,相比传统的基于ORB_SLAM2的单目视觉惯性定位系统,定位精度可提高约30%。
Description
技术领域
本发明涉及一种单目鲁棒性视觉惯性紧耦合定位方法,属于SLAM(SimultaneousLocalization and Mapping,同步定位与建图)领域。
背景技术
随着微型飞行器自主飞行、自动驾驶、虚拟现实及增强现实等技术的迅速发展,实现高精度、强鲁棒性定位是完成移动智能体自主导航、探索未知区域等既定任务的重要前提。将视觉传感器和惯性测量单元(IMU)融合,可构建出精度更高、鲁棒性更强的视觉惯性融合定位系统(VIO)。
传统的视觉惯性融合定位框架包括前端和后端两个模块,前端通过IMU和图像估算相邻图像间相机的运动,后端接受前端不同时刻估算的相机运动信息,对其进行局部与全局优化,得到全局一致的轨迹。
现有的VIO包括OKVIS、基于ORB_SLAM2的单目视觉惯性融合定位系统、VINS。OKVIS框架(Leutenegger S,Furgale P,Rabaud V,et al.Keyframe-based visual-inertialslam using nonlinear optimization[J].Proceedings of Robotis Science andSystems(RSS)2013,2013.)提出了视觉惯性融合定位框架的前端模型,该模型实现了视觉和惯性数据的紧耦合,但未恢复出系统尺度和重力加速度等数据,同时不含预积分框架,因此其定位精度和鲁棒性较差;基于ORB_SLAM2(Mur-Artal R,Tardós J D.Visual-inertialmonocular SLAM with map reuse[J].IEEE Robotics and Automation Letters,2017,2(2):796-803.)的单目视觉惯性定位系统,其预积分模型采用流型预积分算法,视觉惯性初始化模型需约15秒,在该阶段无法实现实时鲁棒性定位,同时其跟踪模型采用简单的匀速模型,使相机剧烈运动时无法正确初始化,甚至跟踪失败,最终导致定位效果不佳;VINs(Qin T,Li P,Shen S.Vins-mono:A robust and versatile monocular visual-inertialstate estimator[J].IEEE Transactions on Robotics,2018,34(4):1004-1020.)初始化模型相对较快,但并未对加速度计偏置进行标定,同时初始化精度略低,采用传统的参考帧跟踪模型限制了其定位精度。
发明内容
本发明所要解决的技术问题是:
针对上述现有技术,提出一种包含IMU加权预积分、快速联合初始化及视觉IMU辅助跟踪的面向单目视觉惯性紧耦合定位的方法,提升视觉惯性定位的精度、实时性和鲁棒性。
本发明为解决上述技术问题采用以下技术方案:
本发明提出一种基于单目的鲁棒性视觉惯性紧耦合定位方法,步骤包括:
步骤一、通过相机采集视觉数据,通过IMU单元采集惯性数据;
步骤二、IMU预积分:利用IMU加权预积分模型同步所述视觉数据和所述惯性数据,进行IMU预积分,并得到IMU先验值;
步骤三、初始化:将所述IMU先验值代入视觉惯性联合初始化模型,完成参数的初始化;
步骤四、辅助跟踪定位:在步骤三所述参数的初始化所需时间内,利用连续关键帧之间的变换矩阵计算运动信息,代入视觉惯性融合定位框架的后端模块,实现紧耦合定位;
步骤五、跟踪定位:步骤三所述参数的初始化完成后,转而将所述参数带入视觉惯性融合定位模型,计算得到运动信息,带入视觉惯性融合定位框架的后端模块,实现紧耦合定位。
如前所述的一种基于单目的鲁棒性视觉惯性紧耦合定位方法,进一步地,步骤二所述IMU预积分包括如下具体步骤:
t时刻典型的IMU测量值方程表示为:
其中wB(t)和aW(t)分别是IMU坐标系下的角速度真实值和世界坐标系下加速度真实值,bg(t)和ba(t)分别是陀螺仪和加速度计的偏置,ηg(t)和ηa(t)分别是陀螺仪和加速度计的高斯噪声,gW是世界坐标系下的重力加速度,是t时刻世界坐标向IMU坐标旋转的转置;
t到t+Δt时刻之间真实的加权IMU测量值表示为:
wB(t,t+Δt)=c1(t)wB(t)+c2(t)wB(t+Δt)
aB(t,t+Δt)=c3(t)aB(t)+c4(t)aB(t+Δt) (2)
其中,aB(t)为t时刻IMU坐标系下的加速度;加权系数c1(t),c2(t),c3(t),c4(t)具体表示为:
c1(t)=wB(t)/(wB(t)+wB(t+Δt))
c2(t)=wB(t+Δt)/(wB(t)+wB(t+Δt))
c3(t)=aB(t)/(aB(t)+aB(t+Δt))
c4(t)=aB(t+Δt)/(aB(t)+aB(t+Δt)) (3)
步骤2.2、IMU坐标系下进行加权预积分,求IMU先验值,建立高精度的加权预积分模型:
由t时刻的状态积分得到当前时刻t+Δt的状态,如式(4)所示:
其中RWB(t+Δt)是t+Δt时刻下世界坐标系相对于IMU坐标系的旋转,vW(t+Δt)、pW(t+Δt)分别世界坐标系下的速度和平移;RWB(t)是t时刻下世界坐标系相对于IMU坐标系的旋转,vW(t)、pW(t)分别是世界坐标系下的速度和平移;wW(τ)、aW(τ)和vW(τ)分别为世界坐标系下的角速度变量、加速度变量、速度变量;
其中,在IMU坐标系下,ΔRij为i,j时刻的旋转增量,ΔRik为i,k时刻的旋转增量;Ri为i时刻的旋转,Rj为j时刻的旋转,Rk为k时刻的旋转;为k时刻加速度测量值,为时刻角速度测量值,为k时刻陀螺仪的偏置,为k时刻加速度计的偏置;为k时刻陀螺仪的高斯噪声,为k时刻加速度计的高斯噪声;Δvij为i,j时刻的速度增量,Δvik为i,k时刻的速度增量;vi为i时刻的速度,vj为j时刻的速度;g为重力加速度;Δtij为i,j时刻的时间增量,Δpij为i,j时刻的平移增量;pi为i时刻的平移,pj为j时刻的平移;
分别考虑i,j时刻间的预积分数值受偏置及噪声的影响:先假设偏置不变,仅讨论噪声的影响,再讨论偏置更新的影响,最后由式(6)得到IMU先验值:
其中是旋转对陀螺仪偏置的雅可比矩阵,为i时刻陀螺仪的偏置,为i时刻加速度计的偏置,为i时刻无噪声情况下陀螺仪的偏置,为i时刻无噪声情况下加速度计的偏置,是速度对陀螺仪偏置的雅可比矩阵,是速度对加速度计偏置的雅可比矩阵,和分别是i,j时刻陀螺仪和加速度计偏置的变化量,为,j时刻的速度增量测量值,为,j时刻无噪声情况下的旋转增量测量值,为i,j时刻无噪声情况下的旋转增量,bg为陀螺仪偏置,ba为加速度计偏置;(·)∧表示将向量转换成矩阵;
如前所述的一种基于单目的鲁棒性视觉惯性紧耦合定位方法,进一步地,步骤三所述参数的初始化包括如下具体步骤:
步骤3.1、估计优化的陀螺仪偏置,估计方向修正的重力加速度gw:
分别从陀螺仪测量值和视觉测量值得到旋转,将相对旋转的差异最小化得到优化的陀螺仪偏置bg,所述陀螺仪偏置的优化模型表示为:
其中,N是关键帧的数量,是IMU坐标系到世界坐标系的旋转矩阵,为相机坐标系到世界坐标系的旋转矩阵,由视觉测量值计算得到,是相机和IMU之间的旋转外参;ΔRi,i+1是两个连续关键帧的陀螺仪积分,是重力加速度对相对旋转的雅可比矩阵;为世界坐标系下i时刻的偏置旋转矩阵;
考虑尺度s,估计从IMU坐标系到相机坐标系的变换:
结合(8)和描述了两个连续关键帧的式(4),忽略加速度计及陀螺仪的偏置,得到帧间关系:
估计重力加速度时,同时计算出不准确的尺度s,考虑连续三个关键帧间的(8)、(9)关系,速度项由相对速度表示;分离的变量γ(i)为:
其中[λ(i)β(i)]为i时刻分离出的系数矩阵;
通过求解公式(10)得到重力加速度gw和粗略尺度s;引入重力幅值G=9.8m/s2;
其中δθxy为重力加速度修正量;
结合一阶近似值,理论重力加速度gw表示为:
其中,()×表示将三维向量转换成三维矩阵;
结合(9)和(13)并包含加速度计偏置ba,得到:
对(14)进行变量分离得到:
其中[λ(i) Φ(i) ζ(i)]为i时刻分离出的系数矩阵;
求解公式(15)得到粗略尺度s,重力加速度方向修正δθxy和加速度计偏置ba;
两连续关键帧在(9)中的关系为:
求解此线性最小二乘问题:
得到帧的速度v和优化后尺度s。
如前所述的一种基于单目的鲁棒性视觉惯性紧耦合定位方法,进一步地,步骤三中,步骤3.1还包括:估计了陀螺仪偏置bg及加速度计偏置ba后,对平移、速度和旋转再次预积分,用于提高预积分值精度。
如前所述的一种基于单目的鲁棒性视觉惯性紧耦合定位方法,进一步地,步骤四所述辅助跟踪定位具体地包括步骤:
系统在初始化阶段使用IMU旋转数据;所述旋转数据包括:对于三个连续关键帧F1、F2及F3,相互间的相对旋转分别为ΔT12和ΔT23,且有:
ΔT12=ΔT23 (20)
F1和F2之间的相对变换及F3的变换矩阵分别为:
其中R1、R2为F1、F2的旋转矩阵,T1、T2、T3为F1、F2及F3的变换矩阵,t1、t2、t3为F1、F2及F3的平移向量;
IMU初始的先验旋转数据由旋转矩阵间转换关系得到:
将旋转矩阵R1、R2和R3代入到变换矩阵模型中,得到改进的T3:
再将改进的T3作为初始值代入视觉惯性融合定位框架的后端模块。
如前所述的一种基于单目的鲁棒性视觉惯性紧耦合定位方法,进一步地,步骤五所述跟踪定位具体地包括:
由步骤三得到参数初始化的结果,所述参数包括方向修正的重力加速度gw,tj时刻帧的速度vj,tj时刻帧对应的旋转矩阵Rj,优化后尺度s;
对于三个连续的关键帧F1、F2和F3,有:
其中:v2,v3分别为关键帧F2和F3的速度,t2,t3分别为关键帧F2和F3的时刻,gw为方向修正的重力加速度,R2为关键帧F2的旋转矩阵,a2为关键帧F2的加速度,为关键帧F2的加速度计偏置,为加速度计噪声;又有改进的关键帧F3对应的变换矩阵T3:
最终将改进的T3和v3联合作为初始状态代入视觉惯性融合定位框架的后端模块。
本发明采用以上技术方案与现有技术相比,具有以下技术效果:
本发明在IMU预积分阶段,提出了IMU加权预积分模型,为联合初始化提供IMU先验值,为传统视觉惯性融合定位框架的后端优化部分提供高精度IMU约束,提高了定位精度;
在初始化阶段,提出了视觉惯性联合初始化模型,构建视觉惯性融合状态向量,建立联合初始化模型,为传统视觉惯性融合定位框架的后端优化部分提供绝对尺度、重力加速度及速度信息,使本发明的鲁棒性和定位精度都得到增强;
在初始化阶段,提出了辅助跟踪模型,用于在初始化阶段替代定位,解决了视觉惯性初始化模型需约15秒,在该阶段无法实现实时鲁棒性定位的问题;
总体而言,本发明所提出的方法用于视觉惯性定位,所述方法同步视觉和惯性测量数据,建立高精度的IMU加权预积分模型,为联合初始化和视觉跟踪模型提供帧间运动约束,提高了定位精度;构建视觉惯性融合状态向量,建立联合初始化模型,实现视觉惯性松耦合的快速联合初始化,降低了系统初始化时间;在IMU加权预积分和快速初始化方法的基础上,建立一套视觉惯性辅助跟踪模型,有效提高了系统鲁棒性。使用本发明提出的方法,初始化时间可缩短至10秒内;相比传统的基于ORB_SLAM2的单目视觉惯性定位系统,定位精度可提高约30%。
附图说明
图1是基于单目的视觉惯性紧耦合定位方法流程图;
图2是本方法与现有方法在MH03序列中定位轨迹平面图;
图3是本方法与现有方法在V101序列中定位轨迹平面图;
图4是本方法与现有方法在MH03序列中的轨迹误差曲线图;
图5是本方法与现有方法在MH03序列中的轨迹误差小提琴图。
具体实施方式
下面结合附图对本发明的技术方案做进一步的详细说明:
本技术领域技术人员可以理解的是,除非另外定义,这里使用的所有术语(包括技术术语和科学术语)具有与本发明所属领域中的普通技术人员的一般理解相同的意义。还应该理解的是,诸如通用字典中定义的那些术语应该被理解为具有与现有技术的上下文中的意义一致的意义,并且除非像这里一样定义,不会用理想化或过于正式的含义来解释。
一种基于单目的鲁棒性视觉惯性紧耦合定位方法,首先同步视觉和惯性测量数据,建立高精度的IMU加权预积分模型;其次构建视觉惯性融合状态向量,建立联合初始化模型;最后在IMU加权预积分和快速初始化方法的基础上,建立一套视觉惯性辅助跟踪模型。
如图1,进一步的,一种基于单目的鲁棒性视觉惯性紧耦合定位方法包括如下具体步骤:
步骤1),同步视觉和惯性测量数据,建立高精度的IMU加权预积分模型,具体如下:
a),计算加权IMU测量值。IMU的测量值包含由陀螺仪和加速度计分别提供的角速度和加速度,t时刻典型的测量值方程如式(1)所示。
其中和分别为IMU坐标系下的角速度和加速度测量值,wB(t)和aW(t)分别是IMU坐标系下的角速度真实值和世界坐标系下加速度真实值,bg(t)和ba(t)分别是陀螺仪和加速度计的偏置,ηg(t)和ηa(t)分别是陀螺仪和加速度计的高斯噪声,gW是世界坐标系下的重力加速度,是t时刻世界坐标向IMU坐标旋转的转置。
定义t到t+Δt时刻之间真实的加权IMU测量值为这两个时刻测量值加权描述的常数:
wB(t,t+Δt)=c1(t)wB(t)+c2(t)wB(t+Δt)
aB(t,t+Δt)=c3(t)aB(t)+c4(t)aB(t+Δt) (2)
其中,加权系数c1(t),c2(t),c3(t),c4(t)具体表示如式(3)所示。
c1(t)=wB(t)/(wB(t)+wB(t+Δt))
c2(t)=wB(t+Δt)/(wB(t)+wB(t+Δt))
c3(t)=aB(t)/(aB(t)+aB(t+Δt))
c4(t)=aB(t+Δt)/(aB(t)+aB(t+Δt)) (3)
b),建立加权预积分模型。当前时刻t+Δt的状态可由t时刻的状态积分得到,如式(4)所示:
将式(4)两边同乘可将世界坐标系下的旋转、速度及平移三个状态量转换到IMU相对坐标系下。结合式(1)中的IMU测量值及式(2)中的加权IMU测量表示,在数值积分时,由于IMU有较高的测量频率,相邻两次IMU测量之间的角速度、加速度及旋转可视为定值。故可将式(4)整理得到IMU坐标系下的加权预积分公式(5):
由式(5)知i,j时刻间的预积分数值受偏置和及噪声和的影响,且关系复杂,故需分别考虑。先假设偏置不变,仅讨论噪声的影响,再讨论偏置更新的影响,最后由式(6)得到IMU先验值,其中是旋转对陀螺仪偏置的雅可比矩阵,和是速度对偏置的雅可比矩阵,和是i,j时刻陀螺仪和加速度计偏置的变化量。
步骤2),构建视觉惯性融合状态向量,建立联合初始化模型,包含如下具体步骤:
a),估计偏置及重力加速度。由于噪声的存在,分别从陀螺仪测量值和视觉测量值得到的旋转不相等,故通过最小化相对旋转的差异来优化陀螺仪偏置,优化模型如式(7)所示:
估计从IMU坐标系到相机坐标系的变换时包含了尺度s:
结合(8)和描述了两个连续关键帧的式(4),忽略加速度计及陀螺仪的偏置可得:
估计重力加速度时,同时计算出不准确的尺度s,考虑连续三个关键帧间的(8)、(9)关系,速度项由相对速度表示。分离的变量为:
至少联合四个关键帧可形成测量模型:A3(N-2)×4x4×1=B3(N-1)×1。通过SVD求解模型得到重力加速度向量和粗略尺度由于重力和加速度计偏差很难区分,加入加速度计偏置可能导致病态系统[14]。在优化重力加速度和加速度偏置估计的过程中,为提高可观察性,本文引入重力幅值G=9.8m/s2,重力加速度只需优化方向。考虑参考帧I,其重力方向为且计算出的重力方向为:如下定义旋转和重力向量:
结合一阶近似值,gw可表示为:
结合(9)和(13)并包含加速度偏置,得到:
式(10)进一步转化为含有尺度、重力加速度方向修正及加速度计偏置的表达式:
b),快速恢复尺度。将速度、重力向量及尺度合为一个状态向量:
其中:
通过求解此线性最小二乘问题,可得到相机帧的速度v和优化后尺度s:
步骤3),建立视觉惯性辅助跟踪模型,具体如下:
系统的初始化阶段由于无法获取速度、重力加速度及尺度,故只能使用IMU旋转数据。匀速跟踪模型视相邻关键帧间的相对变换不变,即对于三个连续帧F1、F2及F3,相互间的相对旋转分别为ΔR12和ΔR23,且有:
ΔT12=ΔT23 (20)
F1和F2之间的相对变换及F3的变换矩阵分别为:
本文将IMU初始的先验旋转数据由旋转矩阵间转换关系得到:
将旋转矩阵代入到变换矩阵模型中:
再将改进的T3作为初始值代入优化模型。当系统完成初始化后,可获得准确的重力加速度和IMU速度,因此在视觉优化前可通过IMU获得整个系统的速度信息:
最终将改进的T3和v3联合作为初始状态代入优化模型。
实施例一
本发明提出的方法理论上可应用于现有的传统的视觉惯性融合定位框架(VIO)。现有的传统的视觉惯性融合定位框架包括前端和后端两个模块,前端通过IMU和图像估算相邻图像间相机的运动,后端接受前端不同时刻估算的相机运动信息,对其进行局部与全局优化,得到全局一致的轨迹。
现有的VIO包括OKVIS、基于ORB_SLAM2的单目视觉惯性融合定位系统、VINS。基于视觉惯性ORB_SLAM2(后文记为origin_VIO),并采用EuRoC数据集中的九个数据序列进行测试。该数据集包含了不同的房间和工业环境中,配备有VI-Sensor双目惯性相机的无人机动态运动。其图像采集频率为20Hz,IMU采样频率为200Hz,数据集提供真实轨迹。由于是单目视觉惯性平台,故仅使用数据集中左目相机测量值。本实例使用内存8G的Intel Core i7-7000笔记本,本方法(记为robust_VIO)实验结果与origin_VIO结果作比较。
图2为MH03序列在origin_VIO及robust_VIO中的定位轨迹平面图。由于系统基于关键帧进行定位跟踪,故定位轨迹会存在部分不连续的情况。由图可知,两个系统估计的结果几乎跟踪了实际轨迹,但robust_VIO的轨迹较于origin_VIO的轨迹更连续、稳定且偏离更小。
图3为MH03序列在两种系统中定位的轨迹误差分析图,主要统计分析数据的离散程度和分布密度,robust_VIO误差分布主要集中在0.095m左右,而origin_VIO主要集中在0.194m附近,前者误差的中位数和四分位数都较小,且下侧的离散值表现也更优。
为验证本文提出的快速联合初始化方法的快速性,以V101序列为例作初始化状态图,图4和图5分别为初始化尺度和加速度计偏置的变化情况。由图可知在robust_VIO中四类变量在10秒内稳定收敛,而origin_VIO需要15秒,本文方法加快了初始化速度。
最后给出九种EuRoC数据序列在origin_VIO及robust_VIO两种系统中的定位结果。由表1可知本文定位结果优于origin_VIO的定位结果,定位精度平均提高了约30%。
表1:两种方法在EuRoC数据集上定位结果对比
以上所述仅是本发明的部分实施方式,应当指出,对于本技术领域的普通技术人员来说,在不脱离本发明原理的前提下,还可以做出若干改进和润饰,这些改进和润饰也应视为本发明的保护范围。
Claims (5)
1.一种基于单目的鲁棒性视觉惯性紧耦合定位方法,其特征在于,步骤包括:
步骤一、通过相机采集视觉数据,通过IMU单元采集惯性数据;
步骤二、IMU预积分:利用IMU加权预积分模型同步所述视觉数据和所述惯性数据,进行IMU预积分,并得到IMU先验值;
步骤三、初始化:将所述IMU先验值代入视觉惯性联合初始化模型,完成参数的初始化;
步骤四、辅助跟踪定位:在步骤三所述参数的初始化所需时间内,利用连续关键帧之间的变换矩阵计算运动信息,代入视觉惯性融合定位框架的后端模块,实现紧耦合定位;
步骤五、跟踪定位:步骤三所述参数的初始化完成后,转而将所述参数代入视觉惯性融合定位框架的前端模块,计算得到运动信息后,代入视觉惯性融合定位框架的后端模块进行优化,实现紧耦合定位;
步骤五所述跟踪定位具体地包括:
由步骤三得到参数初始化的结果,所述参数包括方向修正的重力加速度gw,tj时刻帧的速度vj,tj时刻帧对应的旋转矩阵Rj,优化后尺度s;
对于三个连续的关键帧F1、F2和F3,有:
其中:v2,v3分别为关键帧F2和F3的速度,t2,t3分别为关键帧F2和F3的时刻,gw为方向修正的重力加速度,R2为关键帧F2的旋转矩阵,a2为关键帧F2的加速度,为关键帧F2的加速度计偏置,为加速度计噪声;又有改进的关键帧F3对应的变换矩阵T3:
最终将改进的T3和v3联合作为初始状态代入视觉惯性融合定位框架的后端模块。
2.根据权利要求1所述的一种基于单目的鲁棒性视觉惯性紧耦合定位方法,其特征在于,步骤二所述IMU预积分包括如下具体步骤:
t时刻典型的IMU测量值方程表示为:
其中wB(t)和aW(t)分别是IMU坐标系下的角速度真实值和世界坐标系下加速度真实值,bg(t)和ba(t)分别是陀螺仪和加速度计的偏置,ηg(t)和ηa(t)分别是陀螺仪和加速度计的高斯噪声,gW是世界坐标系下的重力加速度,是t时刻世界坐标向IMU坐标旋转的转置;
t到t+Δt时刻之间真实的加权IMU测量值表示为:
wB(t,t+Δt)=c1(t)wB(t)+c2(t)wB(t+Δt)
aB(t,t+Δt)=c3(t)aB(t)+c4(t)aB(t+Δt) (2)
其中,aB(t)为t时刻IMU坐标系下的加速度;加权系数c1(t),c2(t),c3(t),c4(t)具体表示为:
c1(t)=wB(t)/(wB(t)+wB(t+Δt))
c2(t)=wB(t+Δt)/(wB(t)+wB(t+Δt))
c3(t)=aB(t)/(aB(t)+aB(t+Δt))
c4(t)=aB(t+Δt)/(aB(t)+aB(t+Δt)) (3)
步骤2.2、IMU坐标系下进行加权预积分,求IMU先验值,建立高精度的加权预积分模型:
由t时刻的状态积分得到当前时刻t+Δt的状态,如式(4)所示:
其中RWB(t+Δt)是t+Δt时刻下世界坐标系相对于IMU坐标系的旋转,vW(t+Δt)、pW(t+Δt)分别世界坐标系下的速度和平移;RWB(t)是t时刻下世界坐标系相对于IMU坐标系的旋转,vW(t)、pW(t)分别是世界坐标系下的速度和平移;wW(τ)、aW(τ)和vW(τ)分别为世界坐标系下的角速度变量、加速度变量、速度变量;
其中,在IMU坐标系下,ΔRij为i,j时刻的旋转增量,ΔRik为i,k时刻的旋转增量;Ri为i时刻的旋转,Rj为j时刻的旋转,Rk为k时刻的旋转;为k时刻加速度测量值,为k时刻角速度测量值,为k时刻陀螺仪的偏置,为k时刻加速度计的偏置;为k时刻陀螺仪的高斯噪声,为k时刻加速度计的高斯噪声;Δvij为i,j时刻的速度增量,Δvik为i,k时刻的速度增量;vi为i时刻的速度,vj为j时刻的速度;g为重力加速度;Δtij为i,j时刻的时间增量,Δpij为i,j时刻的平移增量;pi为i时刻的平移,pj为j时刻的平移;
分别考虑i,j时刻间的预积分数值受偏置及噪声的影响:先假设偏置不变,仅讨论噪声的影响,再讨论偏置更新的影响,最后由式(6)得到IMU先验值:
3.根据权利要求1所述的一种基于单目的鲁棒性视觉惯性紧耦合定位方法,其特征在于,步骤三所述参数的初始化包括如下具体步骤:
步骤3.1、估计优化的陀螺仪偏置,估计方向修正的重力加速度gw:
分别从陀螺仪测量值和视觉测量值得到旋转,将相对旋转的差异最小化得到优化的陀螺仪偏置bg,所述陀螺仪偏置的优化模型表示为:
其中,N是关键帧的数量,是IMU坐标系到世界坐标系的旋转矩阵,为相机坐标系到世界坐标系的旋转矩阵,由视觉测量值计算得到,是相机和IMU之间的旋转外参;ΔRi,i+1是两个连续关键帧的陀螺仪积分,是重力加速度对相对旋转的雅可比矩阵;为世界坐标系下i时刻的偏置旋转矩阵;
考虑尺度s,估计从IMU坐标系到相机坐标系的变换:
结合(8)和描述了两个连续关键帧的式(4),忽略加速度计及陀螺仪的偏置,得到帧间关系:
估计重力加速度时,同时计算出不准确的尺度s,考虑连续三个关键帧间的(8)、(9)关系,速度项由相对速度表示;分离的变量s和gw表示为:
其中[λ(i) β(i)]为i时刻分离出的系数矩阵;
通过求解公式(10)得到重力加速度gw和粗略尺度s;引入重力幅值G=9.8m/s2;
其中δθxy为重力加速度修正量;
结合一阶近似值,理论重力加速度gw表示为:
其中,( )x表示将三维向量转换成三维矩阵;
结合(9)和(13)并包含加速度计偏置ba,得到:
对(14)进行变量分离得到:
其中[λ(i) Φ(i) ζ(i)]为i时刻分离出的系数矩阵;
求解公式(15)得到粗略尺度s,重力加速度方向修正δθxy和加速度计偏置ba;
两连续关键帧在(9)中的关系为:
求解此线性最小二乘问题:
得到帧的速度v和优化后尺度s。
4.根据权利要求3所述的一种基于单目的鲁棒性视觉惯性紧耦合定位方法,其特征在于,步骤三中,步骤3.1还包括:估计了陀螺仪偏置bg及加速度计偏置ba后,对平移、速度和旋转再次预积分,用于提高预积分值精度。
5.根据权利要求1所述的一种基于单目的鲁棒性视觉惯性紧耦合定位方法,其特征在于,步骤四所述辅助跟踪定位具体地包括步骤:
系统在初始化阶段使用IMU旋转数据;所述旋转数据包括:对于三个连续关键帧F1、F2及F3,相互间的相对旋转分别为ΔT12和ΔT23,且有:
ΔT12=ΔT23 (20)
F1和F2之间的相对变换及F3的变换矩阵分别为:
其中R1、R2为F1、F2的旋转矩阵,T1、T2、T3为F1、F2及F3的变换矩阵,t1、t2、t3为F1、F2及F3的平移向量;
IMU初始的先验旋转数据由旋转矩阵间转换关系得到:
将旋转矩阵R1、R2和R3代入到变换矩阵模型中,得到改进的T3:
再将改进的T3作为初始值代入视觉惯性融合定位框架的后端模块。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910216776.XA CN110030994B (zh) | 2019-03-21 | 2019-03-21 | 一种基于单目的鲁棒性视觉惯性紧耦合定位方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910216776.XA CN110030994B (zh) | 2019-03-21 | 2019-03-21 | 一种基于单目的鲁棒性视觉惯性紧耦合定位方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN110030994A CN110030994A (zh) | 2019-07-19 |
CN110030994B true CN110030994B (zh) | 2022-08-05 |
Family
ID=67236432
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201910216776.XA Active CN110030994B (zh) | 2019-03-21 | 2019-03-21 | 一种基于单目的鲁棒性视觉惯性紧耦合定位方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN110030994B (zh) |
Families Citing this family (16)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110455301A (zh) * | 2019-08-01 | 2019-11-15 | 河北工业大学 | 一种基于惯性测量单元的动态场景slam方法 |
CN112347205B (zh) * | 2019-08-06 | 2023-09-19 | 北京魔门塔科技有限公司 | 一种车辆误差状态的更新方法和装置 |
CN110717927A (zh) * | 2019-10-10 | 2020-01-21 | 桂林电子科技大学 | 基于深度学习和视惯融合的室内机器人运动估计方法 |
CN110986968B (zh) * | 2019-10-12 | 2022-05-24 | 清华大学 | 三维重建中实时全局优化和错误回环判断的方法及装置 |
CN110874569B (zh) * | 2019-10-12 | 2022-04-22 | 西安交通大学 | 一种基于视觉惯性融合的无人机状态参数初始化方法 |
CN110763251B (zh) * | 2019-10-18 | 2021-07-13 | 华东交通大学 | 视觉惯性里程计优化的方法及系统 |
CN111578937B (zh) * | 2020-05-29 | 2024-01-09 | 上海新天策数字科技有限公司 | 同时优化外参数的视觉惯性里程计系统 |
CN111982148B (zh) * | 2020-07-06 | 2022-12-06 | 杭州易现先进科技有限公司 | Vio初始化的处理方法、装置、系统和计算机设备 |
CN112129287B (zh) * | 2020-09-24 | 2022-06-10 | 北京华捷艾米科技有限公司 | 一种基于视觉惯性里程计处理的方法和相关装置 |
CN112649016B (zh) * | 2020-12-09 | 2023-10-03 | 南昌大学 | 一种基于点线初始化的视觉惯性里程计方法 |
CN112633122B (zh) * | 2020-12-17 | 2024-01-23 | 厦门大学 | 一种单目vio系统的前端里程计算法及系统 |
CN112862768B (zh) * | 2021-01-28 | 2022-08-02 | 重庆邮电大学 | 一种基于点线特征的自适应单目vio初始化方法 |
CN113223161B (zh) * | 2021-04-07 | 2022-04-12 | 武汉大学 | 一种基于imu和轮速计紧耦合的鲁棒全景slam系统和方法 |
CN113240597B (zh) * | 2021-05-08 | 2024-04-26 | 西北工业大学 | 基于视觉惯性信息融合的三维软件稳像方法 |
CN113223045B (zh) * | 2021-05-19 | 2024-06-11 | 北京数研科技发展有限公司 | 基于动态物体语义分割的视觉与imu传感器融合定位系统 |
CN113203407B (zh) * | 2021-05-20 | 2024-01-02 | 南昌大学 | 基于关键平面的视觉惯性里程计方法 |
Family Cites Families (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN108731700B (zh) * | 2018-03-22 | 2020-07-31 | 东南大学 | 一种视觉惯性里程计中的加权欧拉预积分方法 |
CN108981693B (zh) * | 2018-03-22 | 2021-10-29 | 东南大学 | 基于单目相机的vio快速联合初始化方法 |
CN109029448B (zh) * | 2018-06-28 | 2021-11-12 | 东南大学 | 单目视觉惯性定位的imu辅助跟踪模型 |
CN109166149B (zh) * | 2018-08-13 | 2021-04-02 | 武汉大学 | 一种融合双目相机与imu的定位与三维线框结构重建方法与系统 |
CN109307508B (zh) * | 2018-08-29 | 2022-04-08 | 中国科学院合肥物质科学研究院 | 一种基于多关键帧的全景惯导slam方法 |
-
2019
- 2019-03-21 CN CN201910216776.XA patent/CN110030994B/zh active Active
Also Published As
Publication number | Publication date |
---|---|
CN110030994A (zh) | 2019-07-19 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110030994B (zh) | 一种基于单目的鲁棒性视觉惯性紧耦合定位方法 | |
CN110375738B (zh) | 一种融合惯性测量单元的单目同步定位与建图位姿解算方法 | |
CN109376785B (zh) | 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法 | |
CN110702107A (zh) | 一种单目视觉惯性组合的定位导航方法 | |
Kaiser et al. | Simultaneous state initialization and gyroscope bias calibration in visual inertial aided navigation | |
Ludwig et al. | Comparison of Euler estimate using extended Kalman filter, Madgwick and Mahony on quadcopter flight data | |
CN108981693B (zh) | 基于单目相机的vio快速联合初始化方法 | |
CN111156987B (zh) | 基于残差补偿多速率ckf的惯性/天文组合导航方法 | |
CN103630137B (zh) | 一种用于导航系统的姿态及航向角的校正方法 | |
CN108036785A (zh) | 一种基于直接法与惯导融合的飞行器位姿估计方法 | |
CN110726406A (zh) | 一种改进的非线性优化单目惯导slam的方法 | |
CN112649016A (zh) | 一种基于点线初始化的视觉惯性里程计方法 | |
CN110702143B (zh) | 基于李群描述的sins捷联惯性导航系统动基座快速初始对准方法 | |
CN112556719B (zh) | 一种基于cnn-ekf的视觉惯性里程计实现方法 | |
Kang et al. | Vins-vehicle: A tightly-coupled vehicle dynamics extension to visual-inertial state estimator | |
Zhang et al. | Vision-aided localization for ground robots | |
CN111707261A (zh) | 一种微型无人机高速感知和定位方法 | |
Hansen et al. | Nonlinear observer design for GNSS-aided inertial navigation systems with time-delayed GNSS measurements | |
CN112985450B (zh) | 一种具有同步时间误差估计的双目视觉惯性里程计方法 | |
CN112857398A (zh) | 一种系泊状态下舰船的快速初始对准方法和装置 | |
CN115574816B (zh) | 仿生视觉多源信息智能感知无人平台 | |
CN114234967A (zh) | 一种基于多传感器融合的六足机器人定位方法 | |
CN113503872B (zh) | 一种基于相机与消费级imu融合的低速无人车定位方法 | |
CN111337056B (zh) | 基于优化的LiDAR运动补偿位置姿态系统对准方法 | |
CN113008229A (zh) | 一种基于低成本车载传感器的分布式自主组合导航方法 |
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 |