CN110030994B - 一种基于单目的鲁棒性视觉惯性紧耦合定位方法 - Google Patents

一种基于单目的鲁棒性视觉惯性紧耦合定位方法 Download PDF

Info

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
Application number
CN201910216776.XA
Other languages
English (en)
Other versions
CN110030994A (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.)
Southeast University
Original Assignee
Southeast 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 Southeast University filed Critical Southeast University
Priority to CN201910216776.XA priority Critical patent/CN110030994B/zh
Publication of CN110030994A publication Critical patent/CN110030994A/zh
Application granted granted Critical
Publication of CN110030994B publication Critical patent/CN110030994B/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
    • 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预积分包括如下具体步骤：
步骤2.1、计算加权IMU测量值，得到IMU坐标系下的角速度测量值
Figure BDA0002002319950000021
和加速度测量值
Figure BDA0002002319950000022
t时刻典型的IMU测量值方程表示为：
Figure BDA0002002319950000023
Figure BDA0002002319950000024
其中wB(t)和aW(t)分别是IMU坐标系下的角速度真实值和世界坐标系下加速度真实值，bg(t)和ba(t)分别是陀螺仪和加速度计的偏置，ηg(t)和ηa(t)分别是陀螺仪和加速度计的高斯噪声，gW是世界坐标系下的重力加速度，
Figure BDA0002002319950000025
是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)所示：
Figure BDA0002002319950000031
其中RWB(t+Δt)是t+Δt时刻下世界坐标系相对于IMU坐标系的旋转,vW(t+Δt)、pW(t+Δt)分别世界坐标系下的速度和平移；RWB(t)是t时刻下世界坐标系相对于IMU坐标系的旋转,vW(t)、pW(t)分别是世界坐标系下的速度和平移；wW(τ)、aW(τ)和vW(τ)分别为世界坐标系下的角速度变量、加速度变量、速度变量；
将式(4)两边同乘
Figure BDA0002002319950000032
将世界坐标系下的旋转、速度及平移三个状态量转换到IMU相对坐标系下；将相邻两次IMU测量之间的角速度、加速度及旋转视为定值，进行数值积分得到IMU坐标系下的加权预积分公式(5)：
Figure BDA0002002319950000033
Figure BDA0002002319950000034
Figure BDA0002002319950000035
其中，在IMU坐标系下，ΔRij为i,j时刻的旋转增量，ΔRik为i,k时刻的旋转增量；Ri为i时刻的旋转，Rj为j时刻的旋转，Rk为k时刻的旋转；
Figure BDA0002002319950000036
为k时刻加速度测量值，
Figure BDA0002002319950000037
为时刻角速度测量值，
Figure BDA0002002319950000038
为k时刻陀螺仪的偏置，
Figure BDA0002002319950000039
为k时刻加速度计的偏置；
Figure BDA00020023199500000310
为k时刻陀螺仪的高斯噪声，
Figure BDA00020023199500000311
为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先验值：
Figure BDA0002002319950000041
其中
Figure BDA0002002319950000042
是旋转对陀螺仪偏置的雅可比矩阵，
Figure BDA0002002319950000043
为i时刻陀螺仪的偏置，
Figure BDA0002002319950000044
为i时刻加速度计的偏置，
Figure BDA0002002319950000045
为i时刻无噪声情况下陀螺仪的偏置，
Figure BDA0002002319950000046
为i时刻无噪声情况下加速度计的偏置，
Figure BDA0002002319950000047
是速度对陀螺仪偏置的雅可比矩阵，
Figure BDA0002002319950000048
是速度对加速度计偏置的雅可比矩阵，
Figure BDA0002002319950000049
和
Figure BDA00020023199500000410
分别是i,j时刻陀螺仪和加速度计偏置的变化量，
Figure BDA00020023199500000411
为,j时刻的速度增量测量值，
Figure BDA00020023199500000412
为,j时刻无噪声情况下的旋转增量测量值，
Figure BDA00020023199500000413
为i,j时刻无噪声情况下的旋转增量，bg为陀螺仪偏置，ba为加速度计偏置；(·)∧表示将向量转换成矩阵；
如前所述的一种基于单目的鲁棒性视觉惯性紧耦合定位方法，进一步地，步骤三所述参数的初始化包括如下具体步骤：
步骤3.1、估计优化的陀螺仪偏置，估计方向修正的重力加速度gw：
分别从陀螺仪测量值和视觉测量值得到旋转，将相对旋转的差异最小化得到优化的陀螺仪偏置bg，所述陀螺仪偏置的优化模型表示为：
Figure BDA00020023199500000414
其中，N是关键帧的数量，
Figure BDA00020023199500000415
是IMU坐标系到世界坐标系的旋转矩阵，
Figure BDA00020023199500000416
为相机坐标系到世界坐标系的旋转矩阵，由视觉测量值计算得到，
Figure BDA00020023199500000417
是相机和IMU之间的旋转外参；ΔRi,i+1是两个连续关键帧的陀螺仪积分，
Figure BDA00020023199500000418
是重力加速度对相对旋转的雅可比矩阵；
Figure BDA00020023199500000419
为世界坐标系下i时刻的偏置旋转矩阵；
考虑尺度s，估计从IMU坐标系到相机坐标系的变换：
Figure BDA00020023199500000420
其中，
Figure BDA00020023199500000421
为IMU坐标系到世界坐标系的平移；
Figure BDA00020023199500000422
为相机坐标系到世界坐标系的平移；
Figure BDA00020023199500000423
为IMU坐标系到相机坐标系的平移；
结合(8)和描述了两个连续关键帧的式(4)，忽略加速度计及陀螺仪的偏置，得到帧间关系：
Figure BDA0002002319950000051
其中，
Figure BDA0002002319950000052
为某时刻相机坐标系到世界坐标系的平移；
Figure BDA0002002319950000053
为某时刻IMU坐标系在世界坐标系下的速度；Δti,i+1为i时刻与i+1时刻间的时间增量，Δpi,i+1为i时刻与i+1时刻间的位移增量；
估计重力加速度时，同时计算出不准确的尺度s，考虑连续三个关键帧间的(8)、(9)关系，速度项由相对速度表示；分离的变量γ(i)为：
Figure BDA0002002319950000054
其中[λ(i)β(i)]为i时刻分离出的系数矩阵；
通过求解公式(10)得到重力加速度gw和粗略尺度s；引入重力幅值G＝9.8m/s2；
考虑参考帧I，其重力方向为
Figure BDA0002002319950000055
且计算出的重力方向为：
Figure BDA0002002319950000056
如下定义旋转
Figure BDA0002002319950000057
和理论重力加速度gw：
Figure BDA0002002319950000058
通过扰动δθ优化旋转；其中，
Figure BDA00020023199500000513
在参考帧I中围绕x轴和y轴旋转两次进行参数化，而z轴上的旋转与重力对齐，对gw没有影响；优化旋转如下所示：
Figure BDA00020023199500000510
Figure BDA00020023199500000511
其中δθxy为重力加速度修正量；
结合一阶近似值,理论重力加速度gw表示为：
Figure BDA00020023199500000512
其中，()×表示将三维向量转换成三维矩阵；
结合(9)和(13)并包含加速度计偏置ba，得到：
Figure BDA0002002319950000061
对(14)进行变量分离得到：
Figure BDA0002002319950000062
其中[λ(i) Φ(i) ζ(i)]为i时刻分离出的系数矩阵；
求解公式(15)得到粗略尺度s,重力加速度方向修正δθxy和加速度计偏置ba；
步骤3.2、快速恢复尺度：将速度
Figure BDA0002002319950000063
重力加速度gw及尺度s合为一个状态向量XI：
Figure BDA0002002319950000064
Figure BDA0002002319950000065
是第i帧图像下IMU坐标系中的速度；
两连续关键帧在(9)中的关系为:
Figure BDA0002002319950000066
其中Δpi,i+1和Δvi,i+1分别是两连续关键帧的速度增量和平移增量；
Figure BDA0002002319950000067
为某时刻IMU在自身坐标系下的速度；
分离变量并得到对应的含噪声项
Figure BDA0002002319950000068
的线性测量模型：
Figure BDA0002002319950000069
其中
Figure BDA00020023199500000610
和
Figure BDA00020023199500000611
为实际观测值；
Figure BDA00020023199500000612
求解此线性最小二乘问题：
Figure BDA00020023199500000613
得到帧的速度v和优化后尺度s。
如前所述的一种基于单目的鲁棒性视觉惯性紧耦合定位方法，进一步地，步骤三中，步骤3.1还包括：估计了陀螺仪偏置bg及加速度计偏置ba后，对平移、速度和旋转再次预积分，用于提高预积分值精度。
如前所述的一种基于单目的鲁棒性视觉惯性紧耦合定位方法，进一步地，步骤四所述辅助跟踪定位具体地包括步骤：
***在初始化阶段使用IMU旋转数据；所述旋转数据包括：对于三个连续关键帧F1、F2及F3，相互间的相对旋转分别为ΔT12和ΔT23，且有：
ΔT12＝ΔT23 (20)
F1和F2之间的相对变换及F3的变换矩阵分别为：
Figure BDA0002002319950000071
Figure BDA0002002319950000072
其中R1、R2为F1、F2的旋转矩阵，T1、T2、T3为F1、F2及F3的变换矩阵，t1、t2、t3为F1、F2及F3的平移向量；
IMU初始的先验旋转数据由旋转矩阵间转换关系得到：
Figure BDA0002002319950000073
其中R3为F3的旋转矩阵，
Figure BDA0002002319950000076
是相机和IMU之间的旋转外参，ΔR23是关键帧F2与关键帧F3间的相对旋转，由IMU先验数据提供；
将旋转矩阵R1、R2和R3代入到变换矩阵模型中，得到改进的T3：
Figure BDA0002002319950000075
再将改进的T3作为初始值代入视觉惯性融合定位框架的后端模块。
如前所述的一种基于单目的鲁棒性视觉惯性紧耦合定位方法，进一步地，步骤五所述跟踪定位具体地包括：
由步骤三得到参数初始化的结果，所述参数包括方向修正的重力加速度gw，tj时刻帧的速度vj，tj时刻帧对应的旋转矩阵Rj，优化后尺度s；
对于三个连续的关键帧F1、F2和F3，有：
Figure BDA0002002319950000081
其中：v2，v3分别为关键帧F2和F3的速度，t2，t3分别为关键帧F2和F3的时刻，gw为方向修正的重力加速度，R2为关键帧F2的旋转矩阵，a2为关键帧F2的加速度，
Figure BDA0002002319950000082
为关键帧F2的加速度计偏置，
Figure BDA0002002319950000083
为加速度计噪声；又有改进的关键帧F3对应的变换矩阵T3：
Figure BDA0002002319950000084
其中，
Figure BDA0002002319950000085
是相机和IMU之间的旋转外参，R1、R2和R3分别为关键帧F1、F2和F3的旋转矩阵，ΔR23为关键帧F2与关键帧F3间的相对旋转；
最终将改进的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)所示。
Figure BDA0002002319950000091
Figure BDA0002002319950000092
其中
Figure BDA0002002319950000096
和
Figure BDA0002002319950000094
分别为IMU坐标系下的角速度和加速度测量值，wB(t)和aW(t)分别是IMU坐标系下的角速度真实值和世界坐标系下加速度真实值，bg(t)和ba(t)分别是陀螺仪和加速度计的偏置，ηg(t)和ηa(t)分别是陀螺仪和加速度计的高斯噪声，gW是世界坐标系下的重力加速度，
Figure BDA0002002319950000095
是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)所示：
Figure BDA0002002319950000101
Figure BDA0002002319950000102
Figure BDA0002002319950000103
将式(4)两边同乘
Figure BDA0002002319950000104
可将世界坐标系下的旋转、速度及平移三个状态量转换到IMU相对坐标系下。结合式(1)中的IMU测量值及式(2)中的加权IMU测量表示，在数值积分时，由于IMU有较高的测量频率，相邻两次IMU测量之间的角速度、加速度及旋转可视为定值。故可将式(4)整理得到IMU坐标系下的加权预积分公式(5)：
Figure BDA0002002319950000105
Figure BDA0002002319950000106
Figure BDA0002002319950000107
由式(5)知i,j时刻间的预积分数值受偏置
Figure BDA0002002319950000108
和
Figure BDA0002002319950000109
及噪声
Figure BDA00020023199500001010
和
Figure BDA00020023199500001011
的影响，且关系复杂，故需分别考虑。先假设偏置不变，仅讨论噪声的影响，再讨论偏置更新的影响，最后由式(6)得到IMU先验值，其中
Figure BDA00020023199500001012
是旋转对陀螺仪偏置的雅可比矩阵，
Figure BDA00020023199500001013
和
Figure BDA00020023199500001014
是速度对偏置的雅可比矩阵，
Figure BDA00020023199500001015
和
Figure BDA00020023199500001022
是i,j时刻陀螺仪和加速度计偏置的变化量。
Figure BDA00020023199500001017
Figure BDA00020023199500001018
Figure BDA00020023199500001019
步骤2)，构建视觉惯性融合状态向量，建立联合初始化模型，包含如下具体步骤：
a)，估计偏置及重力加速度。由于噪声的存在，分别从陀螺仪测量值和视觉测量值得到的旋转不相等，故通过最小化相对旋转的差异来优化陀螺仪偏置，优化模型如式(7)所示：
Figure BDA00020023199500001020
N是关键帧的数量，
Figure BDA00020023199500001021
是IMU坐标系到世界坐标系的旋转，相机坐标系到世界坐标系的旋转
Figure BDA0002002319950000111
由视觉测量值计算得到，ΔRi,i+1是两个连续关键帧的陀螺仪积分。
估计从IMU坐标系到相机坐标系的变换时包含了尺度s：
Figure BDA0002002319950000112
结合(8)和描述了两个连续关键帧的式(4)，忽略加速度计及陀螺仪的偏置可得：
Figure BDA0002002319950000113
估计重力加速度时，同时计算出不准确的尺度s，考虑连续三个关键帧间的(8)、(9)关系，速度项由相对速度表示。分离的变量为：
Figure BDA0002002319950000114
至少联合四个关键帧可形成测量模型：A3(N-2)×4x4×1＝B3(N-1)×1。通过SVD求解模型得到重力加速度向量
Figure BDA0002002319950000115
和粗略尺度
Figure BDA0002002319950000116
由于重力和加速度计偏差很难区分，加入加速度计偏置可能导致病态***[14]。在优化重力加速度和加速度偏置估计的过程中，为提高可观察性，本文引入重力幅值G＝9.8m/s2，重力加速度只需优化方向。考虑参考帧I，其重力方向为
Figure BDA0002002319950000117
且计算出的重力方向为：
Figure BDA0002002319950000118
如下定义旋转
Figure BDA0002002319950000119
和重力向量：
Figure BDA00020023199500001110
Figure BDA00020023199500001111
可在参考帧I中围绕x轴和y轴旋转两次进行参数化，而z轴上的旋转与重力对齐，对gw没有影响，故可通过扰动δθ优化旋转，如下所示：
Figure BDA00020023199500001112
Figure BDA00020023199500001113
结合一阶近似值,gw可表示为：
Figure BDA00020023199500001114
结合(9)和(13)并包含加速度偏置，得到：
Figure BDA0002002319950000121
式(10)进一步转化为含有尺度、重力加速度方向修正及加速度计偏置的表达式：
Figure BDA0002002319950000122
b)，快速恢复尺度。将速度、重力向量及尺度合为一个状态向量：
Figure BDA0002002319950000123
Figure BDA0002002319950000124
是第i帧图像下IMU坐标系中的速度，两连续关键帧在(9)中的关系为:
Figure BDA0002002319950000125
分离变量并得到对应的含噪声项
Figure BDA0002002319950000126
的线性测量模型：
Figure BDA0002002319950000127
其中：
Figure BDA0002002319950000128
通过求解此线性最小二乘问题，可得到相机帧的速度v和优化后尺度s：
Figure BDA0002002319950000129
步骤3)，建立视觉惯性辅助跟踪模型，具体如下：
***的初始化阶段由于无法获取速度、重力加速度及尺度，故只能使用IMU旋转数据。匀速跟踪模型视相邻关键帧间的相对变换不变，即对于三个连续帧F1、F2及F3，相互间的相对旋转分别为ΔR12和ΔR23，且有：
ΔT12＝ΔT23 (20)
F1和F2之间的相对变换及F3的变换矩阵分别为：
Figure BDA0002002319950000131
Figure BDA0002002319950000132
本文将IMU初始的先验旋转数据由旋转矩阵间转换关系得到：
Figure BDA0002002319950000133
其中
Figure BDA0002002319950000134
是相机和IMU之间的旋转外参，ΔR23是关键帧F2与关键帧F3间的相对旋转，由IMU先验数据提供。
将旋转矩阵代入到变换矩阵模型中：
Figure BDA0002002319950000135
再将改进的T3作为初始值代入优化模型。当***完成初始化后，可获得准确的重力加速度和IMU速度，因此在视觉优化前可通过IMU获得整个***的速度信息：
Figure BDA0002002319950000136
最终将改进的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数据集上定位结果对比
Figure BDA0002002319950000141
以上所述仅是本发明的部分实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (5)

1.一种基于单目的鲁棒性视觉惯性紧耦合定位方法，其特征在于，步骤包括：
步骤一、通过相机采集视觉数据，通过IMU单元采集惯性数据；
步骤二、IMU预积分：利用IMU加权预积分模型同步所述视觉数据和所述惯性数据，进行IMU预积分，并得到IMU先验值；
步骤三、初始化：将所述IMU先验值代入视觉惯性联合初始化模型，完成参数的初始化；
步骤四、辅助跟踪定位：在步骤三所述参数的初始化所需时间内，利用连续关键帧之间的变换矩阵计算运动信息，代入视觉惯性融合定位框架的后端模块，实现紧耦合定位；
步骤五、跟踪定位：步骤三所述参数的初始化完成后，转而将所述参数代入视觉惯性融合定位框架的前端模块，计算得到运动信息后，代入视觉惯性融合定位框架的后端模块进行优化，实现紧耦合定位；
步骤五所述跟踪定位具体地包括：
由步骤三得到参数初始化的结果，所述参数包括方向修正的重力加速度gw，tj时刻帧的速度vj，tj时刻帧对应的旋转矩阵Rj，优化后尺度s；
对于三个连续的关键帧F1、F2和F3，有：
Figure FDA0003678809170000011
其中：v2，v3分别为关键帧F2和F3的速度，t2，t3分别为关键帧F2和F3的时刻，gw为方向修正的重力加速度，R2为关键帧F2的旋转矩阵，a2为关键帧F2的加速度，
Figure FDA0003678809170000012
为关键帧F2的加速度计偏置，
Figure FDA0003678809170000013
为加速度计噪声；又有改进的关键帧F3对应的变换矩阵T3：
Figure FDA0003678809170000014
其中，
Figure FDA0003678809170000015
是相机和IMU之间的旋转外参，R1、R2和R3分别为关键帧F1、F2和F3的旋转矩阵，ΔR23为关键帧F2与关键帧F3间的相对旋转；
最终将改进的T3和v3联合作为初始状态代入视觉惯性融合定位框架的后端模块。
2.根据权利要求1所述的一种基于单目的鲁棒性视觉惯性紧耦合定位方法，其特征在于，步骤二所述IMU预积分包括如下具体步骤：
步骤2.1、计算加权IMU测量值，得到IMU坐标系下的角速度测量值
Figure FDA0003678809170000016
和加速度测量值
Figure FDA0003678809170000017
t时刻典型的IMU测量值方程表示为：
Figure FDA0003678809170000021
Figure FDA0003678809170000022
其中wB(t)和aW(t)分别是IMU坐标系下的角速度真实值和世界坐标系下加速度真实值，bg(t)和ba(t)分别是陀螺仪和加速度计的偏置，ηg(t)和ηa(t)分别是陀螺仪和加速度计的高斯噪声，gW是世界坐标系下的重力加速度，
Figure FDA0003678809170000023
是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)所示：
Figure FDA0003678809170000024
其中RWB(t+Δt)是t+Δt时刻下世界坐标系相对于IMU坐标系的旋转，vW(t+Δt)、pW(t+Δt)分别世界坐标系下的速度和平移；RWB(t)是t时刻下世界坐标系相对于IMU坐标系的旋转，vW(t)、pW(t)分别是世界坐标系下的速度和平移；wW(τ)、aW(τ)和vW(τ)分别为世界坐标系下的角速度变量、加速度变量、速度变量；
将式(4)两边同乘
Figure FDA0003678809170000025
将世界坐标系下的旋转、速度及平移三个状态量转换到IMU相对坐标系下；将相邻两次IMU测量之间的角速度、加速度及旋转视为定值，进行数值积分得到IMU坐标系下的加权预积分公式(5)：
Figure FDA0003678809170000031
Figure FDA0003678809170000032
Figure FDA0003678809170000033
其中，在IMU坐标系下，ΔRij为i，j时刻的旋转增量，ΔRik为i，k时刻的旋转增量；Ri为i时刻的旋转，Rj为j时刻的旋转，Rk为k时刻的旋转；
Figure FDA0003678809170000034
为k时刻加速度测量值，
Figure FDA0003678809170000035
为k时刻角速度测量值，
Figure FDA0003678809170000036
为k时刻陀螺仪的偏置，
Figure FDA0003678809170000037
为k时刻加速度计的偏置；
Figure FDA0003678809170000038
为k时刻陀螺仪的高斯噪声，
Figure FDA0003678809170000039
为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先验值：
Figure FDA00036788091700000310
其中
Figure FDA00036788091700000311
是旋转对陀螺仪偏置的雅可比矩阵，
Figure FDA00036788091700000312
为i时刻陀螺仪的偏置，
Figure FDA00036788091700000313
为i时刻加速度计的偏置，
Figure FDA00036788091700000314
为i时刻无噪声情况下陀螺仪的偏置，
Figure FDA00036788091700000315
为i时刻无噪声情况下加速度计的偏置，
Figure FDA00036788091700000316
是速度对陀螺仪偏置的雅可比矩阵，
Figure FDA00036788091700000317
是速度对加速度计偏置的雅可比矩阵，
Figure FDA00036788091700000318
和
Figure FDA00036788091700000319
分别是i，j时刻陀螺仪和加速度计偏置的变化量，
Figure FDA00036788091700000320
为i，j时刻的速度增量测量值，
Figure FDA00036788091700000321
为i，j时刻无噪声情况下的旋转增量测量值，
Figure FDA00036788091700000322
为i，j时刻无噪声情况下的旋转增量，bg为陀螺仪偏置，ba为加速度计偏置；(·)^表示将向量转换成矩阵。
3.根据权利要求1所述的一种基于单目的鲁棒性视觉惯性紧耦合定位方法，其特征在于，步骤三所述参数的初始化包括如下具体步骤：
步骤3.1、估计优化的陀螺仪偏置，估计方向修正的重力加速度gw：
分别从陀螺仪测量值和视觉测量值得到旋转，将相对旋转的差异最小化得到优化的陀螺仪偏置bg，所述陀螺仪偏置的优化模型表示为：
Figure FDA0003678809170000041
其中，N是关键帧的数量，
Figure FDA0003678809170000042
是IMU坐标系到世界坐标系的旋转矩阵，
Figure FDA0003678809170000043
为相机坐标系到世界坐标系的旋转矩阵，由视觉测量值计算得到，
Figure FDA0003678809170000044
是相机和IMU之间的旋转外参；ΔRi，i+1是两个连续关键帧的陀螺仪积分，
Figure FDA0003678809170000045
是重力加速度对相对旋转的雅可比矩阵；
Figure FDA0003678809170000046
为世界坐标系下i时刻的偏置旋转矩阵；
考虑尺度s，估计从IMU坐标系到相机坐标系的变换：
Figure FDA0003678809170000047
其中，
Figure FDA0003678809170000048
为IMU坐标系到世界坐标系的平移；
Figure FDA0003678809170000049
为相机坐标系到世界坐标系的平移；
Figure FDA00036788091700000410
为IMU坐标系到相机坐标系的平移；
结合(8)和描述了两个连续关键帧的式(4)，忽略加速度计及陀螺仪的偏置，得到帧间关系：
Figure FDA00036788091700000411
其中，
Figure FDA00036788091700000412
为某时刻相机坐标系到世界坐标系的平移；
Figure FDA00036788091700000413
为某时刻IMU坐标系在世界坐标系下的速度；Δti，i+1为i时刻与i+1时刻间的时间增量，Δpi，i+1为i时刻与i+1时刻间的位移增量；
估计重力加速度时，同时计算出不准确的尺度s，考虑连续三个关键帧间的(8)、(9)关系，速度项由相对速度表示；分离的变量s和gw表示为：
Figure FDA00036788091700000414
其中[λ(i) β(i)]为i时刻分离出的系数矩阵；
通过求解公式(10)得到重力加速度gw和粗略尺度s；引入重力幅值G＝9.8m/s2；
考虑参考帧I，其重力方向为
Figure FDA00036788091700000415
且计算出的重力方向为：
Figure FDA00036788091700000416
如下定义旋转
Figure FDA00036788091700000417
和理论重力加速度gw：
Figure FDA0003678809170000051
通过扰动δθ优化旋转；其中，
Figure FDA0003678809170000052
在参考帧I中围绕x轴和y轴旋转两次进行参数化，而z轴上的旋转与重力对齐，对gw没有影响；优化旋转如下所示：
Figure FDA0003678809170000053
Figure FDA0003678809170000054
其中δθxy为重力加速度修正量；
结合一阶近似值，理论重力加速度gw表示为：
Figure FDA0003678809170000055
其中，( )x表示将三维向量转换成三维矩阵；
结合(9)和(13)并包含加速度计偏置ba，得到：
Figure FDA0003678809170000056
对(14)进行变量分离得到：
Figure FDA0003678809170000057
其中[λ(i) Φ(i) ζ(i)]为i时刻分离出的系数矩阵；
求解公式(15)得到粗略尺度s，重力加速度方向修正δθxy和加速度计偏置ba；
步骤3.2、快速恢复尺度：将速度
Figure FDA0003678809170000058
重力加速度gw及尺度s合为一个状态向量X1：
Figure FDA0003678809170000059
Figure FDA00036788091700000510
是第i帧图像下IMU坐标系中的速度；
两连续关键帧在(9)中的关系为：
Figure FDA00036788091700000511
Figure FDA0003678809170000061
其中Δpi，i+1和Δvi，i+1分别是两连续关键帧的速度增量和平移增量；
Figure FDA0003678809170000062
为某时刻IMU在自身坐标系下的速度；
分离变量并得到对应的含噪声项
Figure FDA0003678809170000063
的线性测量模型：
Figure FDA0003678809170000064
其中
Figure FDA0003678809170000065
和
Figure FDA0003678809170000066
为实际观测值；
Figure FDA0003678809170000067
求解此线性最小二乘问题：
Figure FDA0003678809170000068
得到帧的速度v和优化后尺度s。
4.根据权利要求3所述的一种基于单目的鲁棒性视觉惯性紧耦合定位方法，其特征在于，步骤三中，步骤3.1还包括：估计了陀螺仪偏置bg及加速度计偏置ba后，对平移、速度和旋转再次预积分，用于提高预积分值精度。
5.根据权利要求1所述的一种基于单目的鲁棒性视觉惯性紧耦合定位方法，其特征在于，步骤四所述辅助跟踪定位具体地包括步骤：
***在初始化阶段使用IMU旋转数据；所述旋转数据包括：对于三个连续关键帧F1、F2及F3，相互间的相对旋转分别为ΔT12和ΔT23，且有：
ΔT12＝ΔT23 (20)
F1和F2之间的相对变换及F3的变换矩阵分别为：
Figure FDA0003678809170000069
Figure FDA00036788091700000610
其中R1、R2为F1、F2的旋转矩阵，T1、T2、T3为F1、F2及F3的变换矩阵，t1、t2、t3为F1、F2及F3的平移向量；
IMU初始的先验旋转数据由旋转矩阵间转换关系得到：
Figure FDA00036788091700000611
其中R3为F3的旋转矩阵，
Figure FDA0003678809170000071
是相机和IMU之间的旋转外参，ΔR23是关键帧F2与关键帧F3间的相对旋转，由IMU先验数据提供；
将旋转矩阵R1、R2和R3代入到变换矩阵模型中，得到改进的T3：
Figure FDA0003678809170000072
再将改进的T3作为初始值代入视觉惯性融合定位框架的后端模块。
CN201910216776.XA 2019-03-21 2019-03-21 一种基于单目的鲁棒性视觉惯性紧耦合定位方法 Active CN110030994B (zh)

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)

* Cited by examiner, † Cited by third party
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 桂林电子科技大学 基于深度学习和视惯融合的室内机器人运动估计方法
CN110874569B (zh) * 2019-10-12 2022-04-22 西安交通大学 一种基于视觉惯性融合的无人机状态参数初始化方法
CN110986968B (zh) * 2019-10-12 2022-05-24 清华大学 三维重建中实时全局优化和错误回环判断的方法及装置
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)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108981693B (zh) * 2018-03-22 2021-10-29 东南大学 基于单目相机的vio快速联合初始化方法
CN108731700B (zh) * 2018-03-22 2020-07-31 东南大学 一种视觉惯性里程计中的加权欧拉预积分方法
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方法

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
Weiss et al. Real-time onboard visual-inertial state estimation and self-calibration of MAVs in unknown environments
CN108981693B (zh) 基于单目相机的vio快速联合初始化方法
CN108036785A (zh) 一种基于直接法与惯导融合的飞行器位姿估计方法
CN110726406A (zh) 一种改进的非线性优化单目惯导slam的方法
CN112649016A (zh) 一种基于点线初始化的视觉惯性里程计方法
CN111156987A (zh) 基于残差补偿多速率ckf的惯性/天文组合导航方法
CN112556719B (zh) 一种基于cnn-ekf的视觉惯性里程计实现方法
Kang et al. Vins-vehicle: A tightly-coupled vehicle dynamics extension to visual-inertial state estimator
Hansen et al. Nonlinear observer design for GNSS-aided inertial navigation systems with time-delayed GNSS measurements
Zhang et al. Vision-aided localization for ground robots
Bloesch et al. Fusion of optical flow and inertial measurements for robust egomotion estimation
CN112985450B (zh) 一种具有同步时间误差估计的双目视觉惯性里程计方法
CN112857398A (zh) 一种系泊状态下舰船的快速初始对准方法和装置
CN115574816B (zh) 仿生视觉多源信息智能感知无人平台
CN111340851A (zh) 基于双目视觉与imu融合的slam方法
CN111337056B (zh) 基于优化的LiDAR运动补偿位置姿态***对准方法
CN113008229A (zh) 一种基于低成本车载传感器的分布式自主组合导航方法
CN117073720A (zh) 弱环境与弱动作控制下快速视觉惯性标定与初始化方法及设备
CN114993338B (zh) 基于多段独立地图序列的一致性高效视觉惯性里程计算法
CN113503872B (zh) 一种基于相机与消费级imu融合的低速无人车定位方法

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