CN116342661A - 利用路标点偏置位姿修正的双目视觉惯性里程计方法 - Google Patents

利用路标点偏置位姿修正的双目视觉惯性里程计方法 Download PDF

Info

Publication number
CN116342661A
CN116342661A CN202310391308.2A CN202310391308A CN116342661A CN 116342661 A CN116342661 A CN 116342661A CN 202310391308 A CN202310391308 A CN 202310391308A CN 116342661 A CN116342661 A CN 116342661A
Authority
CN
China
Prior art keywords
pose
camera
imu
bias
matrix
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.)
Pending
Application number
CN202310391308.2A
Other languages
English (en)
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 CN202310391308.2A priority Critical patent/CN116342661A/zh
Publication of CN116342661A publication Critical patent/CN116342661A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/20—Analysis of motion
    • G06T7/269—Analysis of motion using gradient-based methods
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/20—Analysis of motion
    • G06T7/246—Analysis of motion using feature-based methods, e.g. the tracking of corners or segments
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/50—Depth or shape recovery
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • 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
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V20/00—Scenes; Scene-specific elements
    • G06V20/40—Scenes; Scene-specific elements in video content
    • G06V20/46—Extracting features or characteristics from the video content, e.g. video fingerprints, representative shots or key frames
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10016—Video; Image sequence
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/30—Subject of image; Context of image processing
    • G06T2207/30236—Traffic on road, railway or crossing
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/30—Subject of image; Context of image processing
    • G06T2207/30241—Trajectory
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/30—Subject of image; Context of image processing
    • G06T2207/30248—Vehicle exterior or interior
    • G06T2207/30252—Vehicle exterior; Vicinity of vehicle
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/30—Subject of image; Context of image processing
    • G06T2207/30248—Vehicle exterior or interior
    • G06T2207/30252—Vehicle exterior; Vicinity of vehicle
    • G06T2207/30256—Lane; Road marking
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Multimedia (AREA)
  • Mathematical Physics (AREA)
  • Mathematical Analysis (AREA)
  • Mathematical Optimization (AREA)
  • Computational Mathematics (AREA)
  • Pure & Applied Mathematics (AREA)
  • Data Mining & Analysis (AREA)
  • Computing Systems (AREA)
  • Algebra (AREA)
  • Databases & Information Systems (AREA)
  • Software Systems (AREA)
  • General Engineering & Computer Science (AREA)
  • Image Analysis (AREA)

Abstract

本发明公开了一种利用路标点偏置位姿修正的双目视觉惯性里程计方法，首先对输入的双目图像进行包括提取特征点并进行光流追踪、IMU预积分以及关键帧筛选的数据预处理，得到筛选后的关键帧；再采用视觉和IMU的松耦合进行视觉惯性里程计快速初始化；根据BA优化目标函数、位姿变化的雅可比矩阵函数推导出路标点偏置与位姿估计误差关联关系，求出光流跟踪特征点过程中路标点的逆深度偏置，利用关联关系对位姿估计进行修正；最后基于紧耦合的后端非线性优化方法，构建优化误差函数迭代求解，实现对深度信息误差以及位姿估计误差的消除，减少轨迹误差。

Description

利用路标点偏置位姿修正的双目视觉惯性里程计方法
技术领域
本发明属于计算机技术领域，涉及SLAM(即时定位与地图构建)技术领域，主要涉及了一种利用路标点偏置位姿修正的双目视觉惯性里程计方法。
背景技术
对于协同SLAM系统，回环检测不仅包括单个机器人非相邻帧之间的回环测量(intra-loop closures)，还需要包括不同机器人的帧间的相对位姿变换，称之为机器人之间的回环测量(inter-loop closures)。正是由于回环检测的引入，里程计带来的累积误差才得以消除，保证了轨迹与地图长时间(long term)的正确性和稳定性。然而，异常回环的存在会使得优化器的求解结果难以收敛，因此在后端优化过程中减少或者剔除假阳性回环的影响，可以降低优化失败的概率，对SLAM系统的鲁棒性提升至关重要。
综上，针对单机器人的假阳性回环剔除方法，大多不适合在线地增量式地剔除假阳性回环，而针对多个机器人之间假阳性回环剔除相关研究较少，且这两类方法均具有对人为设置参数敏感的特点。面对上述现状与问题，需要考虑如何在协同SLAM系统中首先能够剔除单个机器人的假阳性回环测量，为多机器人之间的相对位姿变换提供可靠。
发明内容
本发明正是针对现有技术中视觉惯性里程计位姿估计精度不足的问题，提供一种利用路标点偏置位姿修正的双目视觉惯性里程计方法，首先对输入的双目图像进行包括提取特征点并进行光流追踪、IMU预积分以及关键帧筛选的数据预处理，得到筛选后的关键帧；再采用视觉和IMU的松耦合进行视觉惯性里程计快速初始化；根据BA优化目标函数、位姿变化的雅可比矩阵函数推导出路标点偏置与位姿估计误差关联关系，求出光流跟踪特征点过程中路标点的逆深度偏置，利用关联关系对位姿估计进行修正；最后基于紧耦合的后端非线性优化方法，构建优化误差函数迭代求解，实现对深度信息误差以及位姿估计误差的消除，减少轨迹误差。
为了实现上述目的，本发明采取的技术方案是：利用路标点偏置位姿修正的双目视觉惯性里程计方法，包括以下步骤：
S1，数据预处理：对输入的双目图像进行数据预处理，得到筛选后的关键帧；所述数据预处理至少包括提取特征点并进行光流追踪、I MU预积分以及关键帧筛选；
S2，视觉惯性里程计快速初始化：采用视觉和I MU的松耦合进行视觉惯性里程计快速初始化，双目相机通过三角测量恢复特征点深度，利用PnP算法进行纯视觉的相机位姿估计，再将其与I MU预积分结果进行对齐，恢复尺度因子、重力、陀螺仪偏置和每一帧的速度；所述初始化至少包括陀螺仪偏置校正、初始化速度、重力向量和尺度因子；
S3，位姿估计修正：根据BA优化目标函数、双目立体视觉相机观测方程推导出路标点偏置与位姿估计误差关联关系，推导出光流跟踪特征点过程中路标点的逆深度偏置，利用关联关系对位姿估计进行修正；
S4，后端非线性优化：基于紧耦合的后端非线性优化方法，构建优化误差函数迭代求解，实现对系统状态量进行紧耦合的非线性优化以得到更精确的定位数据；所述系统状态量包括相机位姿、I MU加速度计偏置、陀螺仪偏置、相机到I MU的外参、三维路标点的逆深度。
作为本发明的一种改进，所述步骤S1具体包括：
S11：采用特征点法估计相机位姿，提取特征点，并采用光流跟踪法匹配特征点；
S12：采用I MU预积分方法对测量值之间的相对状态量进行处理，给出预积分状态的协方差矩阵以及雅可比矩阵，实现与绝对位姿的解耦，构建误差函数对关键帧位姿的迭代优化；所述相对状态量至少包括当前相对于上一时刻的姿态和速度、位移；
S13：关键帧筛选：满足以下两个条件之一就判定为关键帧：
(a)根据视差判断，通过计算光流跟踪的Shi-Tomasi角点的视差来进行关键帧筛选，若视差大于设定阈值，则将其判定为关键帧；
(b)根据特征点数量判断，若图像帧跟踪的特征点数量没有超过设定阈值，将其设定为关键帧。
作为本发明的一种改进，所述步骤S12中，通过IMU预积分方法，给出预积分状态的协方差矩阵以及雅可比矩阵，即推导出t+δt时刻雅可比矩阵Jt+δt的迭代关系：
Figure BDA0004175849730000031
其中，J代表IMU测量值的误差的雅可比矩阵，I代表单位矩阵，F代表线性误差系数。
作为本发明的一种改进，所述步骤S2中，对陀螺仪偏置校正，得出陀螺仪偏置δbω：
Figure BDA0004175849730000032
其中，
Figure BDA0004175849730000033
是bω是对应雅可比矩阵/>
Figure BDA0004175849730000034
对/>
Figure BDA0004175849730000035
的偏导，/>
Figure BDA0004175849730000036
是前者的转置矩阵，/>
Figure BDA0004175849730000037
代表克罗内克积，/>
Figure BDA0004175849730000038
代表k-1帧到k+1帧的旋转矩阵，/>
Figure BDA0004175849730000039
代表第k帧对应IMU坐标系到上一帧的旋转矩阵，/>
Figure BDA00041758497300000310
代表第k+1帧对应IMU坐标系到帧的旋转矩阵，vec代表矩阵向量化；
利用Cholesky分解求解下式，恢复出待优化变量
Figure BDA00041758497300000311
即可得到初始化后的速度、重力向量和尺度因子：
Figure BDA00041758497300000312
其中，I代表单位矩阵，
Figure BDA00041758497300000313
代表参考帧下的重力向量，/>
Figure BDA00041758497300000314
代表k帧对应速度，/>
Figure BDA00041758497300000315
代表k+1帧对应速度，s代表尺度因子，/>
Figure BDA00041758497300000316
和/>
Figure BDA00041758497300000317
代表相邻两IMU的预积分项，/>
Figure BDA0004175849730000041
代表参考帧到第k帧IMU的旋转矩阵，/>
Figure BDA0004175849730000042
代表第k帧相机到参考帧的旋转矩阵，Δtk代表间隔时间，/>
Figure BDA0004175849730000043
代表IMU到相机的旋转矩阵。
作为本发明的另一种改进，所述步骤S3中推导路标点偏置与位姿估计误差关联关系具体为：将相机位姿以及三维点同时作为优化变量，非线性最小二乘问题被建模为最小化重投影误差，即集束调整(Bundle Adjustment,BA)，BA优化的目标函数为：
Figure BDA0004175849730000044
其中x为相机位姿，将n个世界坐标系下的三维路标点pi转化到像素坐标系下的特征点zi的转化关系表示为h(x,pi)；根据双目立体视觉相机模型，观测方程h(x,pi)为非线性方程，因此上述最小二乘优化问题需要通过迭代优化算法求解，对h(x,pi)在初始值x(k)处进行一阶泰勒展开得到：
h(x,pi)＝h(x(k),pi)+Jx(x-x(k))
Jx为h(x,pi)关于位姿x的雅克比矩阵；对h(x(k),pi)在初始值
Figure BDA0004175849730000045
处进行一阶泰勒展开：
Figure BDA0004175849730000046
Figure BDA0004175849730000047
为h(x(k),pi)关于三维路标点pi的雅克比矩阵，对h(x,pi)的位姿更新利用高斯牛顿迭代求解，表示为：
Figure BDA0004175849730000048
式中
Figure BDA0004175849730000049
为Jx的伪逆矩阵，和上面的一阶泰勒展开式，进行联合推导，可以得到：
Figure BDA00041758497300000410
迭代过程中位姿估计误差x(k+1)-x(k)表示为∈x，
Figure BDA00041758497300000411
表示像素坐标系中的测量误差，/>
Figure BDA00041758497300000412
则表示三维路标点误差；
由n个三维路标点偏置确定的位姿估计误差的期望可以表示为：
Figure BDA0004175849730000051
其中矩阵I3n×3n形式如下：
Figure BDA0004175849730000052
其中，Jx为h(x(k),pi)关于位姿x的雅可比矩阵，
Figure BDA0004175849730000053
为Jx的逆矩阵，Jp为h(x(k),pi)关于三维路标点p的雅可比矩阵，∈p是三位路标点误差，μp是测量误差偏置。
作为本发明的另一种改进，所述步骤S3中双目视觉提取路标点逆深度偏置推导，平均逆深度偏置μ(dL)表示为：
Figure BDA0004175849730000054
Figure BDA0004175849730000055
其中，
Figure BDA0004175849730000056
是逆深度值，/>
Figure BDA0004175849730000057
是是平均逆深度偏置，/>
Figure BDA0004175849730000058
是某一帧的深度偏置。作为本发明的又一种改进，所述步骤S3还包括路标点筛选，根据相机运动角速度进行筛选的策略如下：
Figure BDA0004175849730000059
其中，||ωt||2表示由IMU给出的相机运动角速度的l2范数，当计算出的路标点偏置大于对应情况下设定的阈值时即对其进行舍弃。
作为本发明的又一种改进，所述步骤S4改进双目视觉重投影误差项的紧耦合后端非线性优化，构建后端的目标函数包含三项误差分别为单目视觉重投影误差，IMU测量误差和边缘化信息，使用Levenberg-Marquardt(LM)法对目标函数进行迭代优化求解；
将双目相机重投影误差用一个三维向量表示：
Figure BDA0004175849730000061
其中，
Figure BDA0004175849730000062
和/>
Figure BDA0004175849730000063
分别表示世界坐标系经过外参变换后，相机坐标系下的三维坐标点，f、cx、cy为左右目相机内参。
与现有技术相比，本发明提出了一种利用路标点偏置进行位姿修正的视觉惯性里程计算法，通过对位姿估计误差期望与三维路标点偏置关联进行分析，利用推导出的三维路标点在光流跟踪中的逆深度偏置完成对深度信息误差以及位姿估计误差的消除。本方法可方便地与现有的综合表现较优秀的视觉惯性里程计算法相结合，本算法的绝对轨迹误差的RMSE指标实现了有效降低，MAE指标也大大降低，根据两项不同指标计算的定位精度均有明显提升。同时，单张图像帧的平均计算时间实验证明，增加位姿修正步骤尽管耗时略有提高，可以满足嵌入式设备上的实时计算的要求。
附图说明
图1为本发明方法的步骤流程图；
图2为本发明方法步骤S1中双目图像特征提取与点对匹配流程图；
图3为本发明方法步骤S2中松耦合视觉惯性初始化效果示意图；
图4为针孔相机模型原理图；
图5为双目相机模型原理图；
图6为本发明方法与VINS-Mono轨迹对比与x、y、z三方向轨迹对比图，其中：
图6(a)为MH_02_easy序列轨迹对比图；
图6(b)为V2_03_difficu lt序列轨迹对比图；
图6(c)为MH_02_easy三方向轨迹对比图；
图6(d)为V2_03_difficu lt三方向轨迹对比图。
具体实施方式
下面结合附图和具体实施方式，进一步阐明本发明，应理解下述具体实施方式仅用于说明本发明而不用于限制本发明的范围。
实施例1
一种利用路标点偏置位姿修正的双目视觉惯性里程计方法，如图1所示，包括以下步骤：
步骤S1：数据预处理包括对输入的双目图像提取特征点并进行光流追踪、IMU预积分以及关键帧筛选；
(11)提取特征点、光流跟踪法匹配特征点
采用特征点法估计相机位姿，提取并对其进行光流跟踪，以达到平衡视觉惯性里程计前端的速度与精度的效果，双目图像特征提取与光流跟踪的流程如图2所示，采用双目相机，先特征提取，然后光流跟踪；其中，左目图像之间的特征点光流追踪可以是为了求解帧间对应关系，而左右目图像之间的特征点光流追踪与反向光流跟踪目的是进行三角测量为利用PnP算法进行运动估计以及位姿修正等步骤做准备。其中，左目图像之间的特征点光流追踪可以是为了求解帧间对应关系，而左右目图像之间的特征点光流追踪与反向光流跟踪目的是进行三角测量为利用PnP算法进行运动估计以及位姿修正等步骤做准备。
视觉定位采用Shi-Tomasi特征点提取算法，Shi-Tomasi特征点是对Harris特征点提取的改进，可以计算适合跟踪的优质特征(Good Features to Track)，使得特征分布更均匀，达到亚像素级别的检测精度。
(12)IMU预积分、预积分项误差分析
为了解决后端优化算法中状态量重复积分的问题，IMU预积分技术对测量值之间的相对状态量进行处理，实现与绝对位姿的解耦，构建误差函数对关键帧位姿的迭代优化。推导出IMU的预积分项的连续形式和基于中值法的离散形式，并且为了将IMU预积分运用到非线性优化中，对其中的误差项进行分析，并给出预积分状态的协方差矩阵以及雅可比矩阵。
考虑相邻两图像帧ck和ck+1，IMU坐标系分别对应bk和bk+1，由于IMU采集频率远远高于相机图像采集频率，那么bk+1帧位置，速度和方向三个状态量需要根据[tk,tk+1]时间间隔内的所有IMU测量值，在世界坐标系中通过对测量值积分进行传递，推导如下：
Figure BDA0004175849730000081
上式中
Figure BDA0004175849730000082
等式右边的四元数右乘符号表示为以下矩阵形式，[ω]×为以下形式的反对称矩阵：
Figure BDA0004175849730000083
公式(1)中，Δtk为[tk,tk+1]之间的时间间隔，
Figure BDA0004175849730000084
表示从IMU坐标系到世界坐标系的转换，/>
Figure BDA0004175849730000085
为四元数表示的当前时刻IMU坐标系到帧的旋转量。
对预积分项误差分析并推导其雅可比以及表示误差项权重的协方差矩阵：下一个时刻的IMU测量值的误差和上一时刻测量值误差为线性关系，根据当前时刻的值预测下一时刻的均值和协方差，根据误差传播公式，t+δt时刻的协方差矩阵可由t时刻协方差矩阵推导：
Figure BDA0004175849730000086
上式中Q表示bk帧时的噪声项初始协方差，误差项的协方差矩阵初始化值和矩阵Q分别为：
Figure BDA0004175849730000091
同时可以根据公式(3)计算t+δt时刻雅可比矩阵Jt+δt有迭代关系：
Figure BDA0004175849730000092
以上是推导出的预积分项的协方差矩阵以及雅可比矩阵。
(13)关键帧筛选
在预处理模块中，进行关键帧筛选(Key Frame,KF)提高系统运行效率，关键帧筛选可以剔除信息冗余度较高的图像帧。相机输入的图像关键帧数量不小于设定的滑动窗口大小(将滑动窗口大小设定为10)时，认为达到了进行初始化要求，关键帧筛选剔除信息冗余度较高的图像帧，根据以下两个条件进行判定：(a)通过计算光流跟踪的Shi-Tomasi角点的视差，若视差大于设定阈值，则说明对此帧图像的跟踪质量较好，将其判定为关键帧；(b)若图像帧跟踪的特征点数量没有超过设定阈值，说明此图像帧信息冗余度低，同样将其设定为关键帧。考虑光流跟踪过程中可能会出现特征点的误匹配，采取RANSAC算法来提高匹配准确率。
步骤S2：视觉和IMU的松耦合进行视觉惯性里程计快速初始化；
采用视觉和IMU的松耦合进行快速初始化方案，双目相机首先通过三角测量恢复特征点深度，然后利用PnP算法进行纯视觉的相机位姿估计，即滑动窗口中所有帧的位姿和所有路标点在三维空间中的位置，然后将其与IMU预积分结果进行对齐，恢复尺度因子、重力、陀螺仪偏置和每一帧的速度。相机运动与IMU预积分对齐效果如图3所示：
(21)陀螺仪偏置校正
将加速度计偏置值和重力加速度耦合到一起，因为在初始化时间内加速度计偏置一般比较小，且难以直接计算，因此忽略加速度计偏置的影响，在初始化中不再计算而仅对陀螺仪偏置进行校正，对于相邻图像帧，视觉运动估计给出的相邻帧之间的旋转应该等于IMU预积分的旋转值，构建目标函数为：
Figure BDA0004175849730000101
其中：
Figure BDA0004175849730000102
b表示预积分旋转值增加的微小扰动，
Figure BDA0004175849730000103
为式(5)中bω对应的雅可比矩阵/>
Figure BDA0004175849730000104
对
Figure BDA0004175849730000105
的偏导，式(6)目标函数的最小值为单位四元数，进一步写为：
Figure BDA0004175849730000106
以()vec符号表示四元数虚部，可以推导出：
Figure BDA0004175849730000107
将等式(9)左侧转为正定阵，即可直接用Cholesky进行分解求解出陀螺仪偏置δbω：
Figure BDA0004175849730000108
(22)初始化速度、重力向量和尺度因子
初始化速度、重力向量和尺度因子的待优化变量定义为：
Figure BDA0004175849730000109
其中
Figure BDA00041758497300001010
为滑窗中n+1个图像帧的速度，s为尺度因子，/>
Figure BDA00041758497300001011
为相机参考帧下的重力向量。将残差定义为相邻两I MU数据的预积分项/>
Figure BDA00041758497300001012
和/>
Figure BDA00041758497300001013
和与预测值之间的误差
Figure BDA00041758497300001014
和/>
Figure BDA00041758497300001015
Figure BDA0004175849730000111
其中Δt为相邻两图像帧间的时间，将帧位姿转换公式带入上式中的
Figure BDA0004175849730000112
项，可以得到：
Figure BDA0004175849730000113
为了利用Cholesky分解，将上式转化为Hx＝b的形式：
Figure BDA0004175849730000114
表示为矩阵形式为：
Figure BDA0004175849730000115
同样也将
Figure BDA0004175849730000116
转为矩阵矩阵形式，综合式(15)可以写成：
Figure BDA0004175849730000117
利用Cholesky分解求解公式(16)，恢复出待优化变量
Figure BDA0004175849730000118
即可得到初始化后的速度、重力向量和尺度因子。
步骤S3：利用推导出的路标点偏置与位姿估计误差关联关系，求出光流跟踪特征点过程中路标点的逆深度偏置，利用关联关系对位姿估计进行修正；
(31)位姿估计误差与三维路标点偏置关联分析
将相机位姿以及三维点同时作为优化变量，非线性最小二乘问题被建模为最小化重投影误差，这类问题统称为集束调整(Bundle Adjustment,BA)，BA优化的目标函数为：
Figure BDA0004175849730000121
式中x为相机位姿，将n个世界坐标系下的三维路标点pi转化到像素坐标系下的特征点zi的转化关系表示为h(x,pi)。h(x,pi)为非线性方程，因此上述最小二乘优化问题需要通过迭代优化算法求解，对h(x,pi)在初始值x(k)处进行一阶泰勒展开得到：
h(x,pi)＝h(x(k),pi)+Jx(x-x(k)) (18)
Jx为h(x,pi)关于位姿x的雅克比矩阵。同样地，对h(x(k),pi)在初始值
Figure BDA0004175849730000122
处进行一阶泰勒展开：
Figure BDA0004175849730000123
Jpi为h(x(k),pi)关于三维路标点pi的雅克比矩阵，对公式(18)的位姿更新利用高斯牛顿迭代求解，表示为：
x(k+1)＝x(k)-Jx +(h(x(k),pi)-zi) (20)
式中Jx +为Jx的伪逆矩阵，和式(19)进行联合推导，可以得到：
Figure BDA0004175849730000124
迭代过程中位姿估计误差x(k+1)-x(k)表示为∈x，
Figure BDA0004175849730000125
表示像素坐标系中的测量误差，
Figure BDA0004175849730000126
则表示三维路标点误差。可见高斯牛顿法对位姿估计的迭代结果主要受到/>
Figure BDA0004175849730000127
和/>
Figure BDA0004175849730000128
的影响。对于测量误差，设其满足零均值高斯分布/>
Figure BDA0004175849730000129
并设路标点满足非零均值的高斯分布/>
Figure BDA00041758497300001210
其偏置/>
Figure BDA00041758497300001211
受到图像匹配精度和视差夹角的影响，也有可能通过BA优化最小化重投影误引入。根据上述条件，由n个三维路标点偏置确定的位姿估计误差的期望可以表示为：
Figure BDA00041758497300001212
其中矩阵I3n×3n形式如下：
Figure BDA0004175849730000131
至此推导出位姿估计误差期望与三维路标点偏置关系，即可依据路标点偏置对位姿误差进行修正。
(32)双目视觉提取路标点逆深度偏置推导
用vt＝[vx,vy,vz]T和ωt＝[ωx,ωy,ωz]T分别表示相机t时刻运动线速度和角速度，使用光流法进行特征点跟踪，用zi(xi,yi)表示像素坐标系下提取的特征点像素坐标，其水平和垂直方向的速度分别为：
Figure BDA0004175849730000132
式中，f为相机焦距(focal length)，(xFOE,yFOE)为光流场延伸焦点(Focus ofExpansion，FOE)坐标，d(xi,yi)为缩放逆深度vz/Zi。设光流跟踪特征点个数为n，并对焦距进行归一化。为了方便推导，本实施例定义符号及其含义如表1所示：
表1路标点逆深度偏置推导符号表
Figure BDA0004175849730000133
Figure BDA0004175849730000141
注：diag符号表示对角线矩阵
根据公式(24)联立光流跟踪的n个特征点，得到矩阵形式如下：
Figure BDA0004175849730000142
对公式(25)进行分析，各时刻角速度[ωt]3n×1可由视觉惯性里程计中IMU传感器直接给出，上式可进一步写为：
Ad＝b (26)
其中：
Figure BDA0004175849730000143
根据最小二乘法求解上述超正定方程，得到：
Figure BDA0004175849730000144
其中
Figure BDA0004175849730000145
计算出的zi(xi,yi)的逆深度估计/>
Figure BDA0004175849730000146
偏置/>
Figure BDA0004175849730000147
为：
Figure BDA0004175849730000151
其中
Figure BDA0004175849730000152
表示像素坐标系下的特征点的噪声协方差，噪声协方差与标定相机内参时的重投影误差，也就是偏离几个像素值有关，实现中固定取1.5个像素值，rix和six分别表示ri和si对xi偏导，riy和siy则表示ri和si对yi偏导。采用双目相机估计特征点深度，设共有L帧双目图像对上述n个光流跟踪点进行深度估计，将L个双目帧计算出的逆深度值dL以及公式(29)计算出的平均逆深度偏置μ(dL)表示为：
Figure BDA0004175849730000153
用
Figure BDA0004175849730000154
符号表示三维点的逆深度真值，那么双目帧计算出的逆深度期望满足
Figure BDA0004175849730000155
若du＝dL-μ(dL)，那么满足/>
Figure BDA0004175849730000156
因此称du为校正逆深度。至此根据μ(dL)，结合公式(22)中位姿估计误差与路标点偏置的关联关系并计算求得的逆深度的倒数，即可完成对位姿估计的矫正。
有关采用双目相机估计特征点深度，通过对双目相机成像模型的分析，能够更好地研究图像对位姿估计以及点云数据提取的影响。
通过针孔相机模型这一广泛使用的基础模型来解释三维点到相机成像平面上的成像过程，针孔相机模型如图4所示，Oc-Zc轴指向相机前方，相机焦距为f。根据小孔成像原理以及相似三角形定理可得到空间点P(X,Y,Z)到成像平面上的点P'(x',y')存在如下转化关系：
Figure BDA0004175849730000161
需要注意，公式(31)中负号表示倒像，实际中相机会对成像平面的图像进行翻转，因此可以等价地将成像平面以相机平面为中心对称地放到相机前方。式(31)成为：
Figure BDA0004175849730000162
化简后得到空间点在图像坐标系中的坐标为：
Figure BDA0004175849730000163
通过平移和缩放将图像坐标系转化为原点定义在图像左上角的像素坐标O-u-v，设原点到像素坐标O-u轴缩放的倍数为α，轴缩放倍数为β，平移量为[Cx,Cy]T，则像素坐标可表示为
Figure BDA0004175849730000164
和公式(33)联立得到：
Figure BDA0004175849730000165
式中f为相机焦距，单位为米；α为O-x轴单位尺寸，单位为像素/米；β为O-y轴单位尺寸，单位为像素/米；fx和fy的单位为像素。整理成矩阵形式后可以得到:
Figure BDA0004175849730000166
等式左侧由齐次坐标表示像素坐标点，上式中P的坐标是其在相机坐标系下的坐标表示，考虑相机在空间中的运动，点P在相机坐标系下的坐标可由其世界坐标系下的坐标根据相机位姿进行转化得到。设相机在世界坐标系的位置与姿态可分别由平移向量tcam与旋转矩阵Rcam描述，则有：
Figure BDA0004175849730000171
其中Pw表示世界坐标系下的位置，平移向量和旋转矩阵即视觉惯性里程计算法进行位姿估计的待估计值，表示相机的轨迹。左右两个单目相机沿着一条水平线水平放置组成一个双目相机，如图5所示，左目相机和右目相机光心之间的距离为双目相机基线。通过同时采集左右两幅图像，并且已知双目相机基线长度，即可计算空间三维点的特征信息。
图5中，OL、OR分别为左右目相机的光心，f为焦距，uL、uR分别为像素坐标u轴上坐标，b为光心OL到OR之间的基线长度。根据以上双目相机模型可以计算三维点P到相机的距离为:
Figure BDA0004175849730000172
其中d为左右图像的像素坐标之差，称为视差。
(33)路标点筛选策略
特征点的错误匹配会使得三维路标点偏置μp变得非常大，因此直接利用估计出的逆深度偏置对位姿进行修正可能会使得SLAM算法的稳定性降低。为了避免上述问题，需要确定哪些路标点偏置可以用于位姿修正中。本实施例根据相机运动角速度进行筛选的策略如下：
Figure BDA0004175849730000173
其中||ωt||2表示由IMU给出的相机运动角速度的l2范数，基本原则是相机运动越快，特征点视差越大，对特征点的深度估计越精确。如公式(39)所示，当计算出的路标点偏置大于对应情况下设定的阈值时即对其进行舍弃，以避免特征点错误匹配带来的影响，提高视觉惯性里程计系统的稳定性。
步骤S4：基于紧耦合的后端非线性优化方法，构建优化误差函数迭代求解。
基于视觉惯性里程计初始化以及位姿修正步骤后，在后端对系统状态量X进行紧耦合的非线性优化以得到更精确的定位数据。系统定义的状态量包括：经过位姿修正后的滑动窗口中n+1个相机位姿
Figure BDA0004175849730000181
IMU加速度计偏置ba和陀螺仪偏置bω，以及相机到IMU的外参/>
Figure BDA0004175849730000182
(包括位置向量/>
Figure BDA0004175849730000183
和旋转/>
Figure BDA0004175849730000184
)和m+1个三维路标点的逆深度dj(0≤j≤m)，因此可以表示为：
Figure BDA0004175849730000185
为了求解最优的状态变量X*，构建后端的目标函数为：
Figure BDA0004175849730000186
公式(41)中，构建的目标函数包含的三项误差分别为单目视觉重投影误差，IMU测量误差和边缘化信息，使用Levenberg-Marquardt(LM)法对目标函数进行迭代优化求解。IMU测量误差和边缘化信息的构建与VINS-Mono一致，相机输入传感器类型为双目，需要对其中的视觉重投影误差进行改进，降低双目视觉误差的构建复杂度。根据相机投影模型将任意世界坐标系三维路标点投影到像素坐标系中，左侧相机的重投影误差可表示为：
Figure BDA0004175849730000187
/>
其中，
Figure BDA0004175849730000188
表示左图像中的像素坐标，设/>
Figure BDA0004175849730000189
和/>
Figure BDA00041758497300001810
为其中的横纵坐标，/>
Figure BDA00041758497300001811
为世界坐标系到左目相机像素坐标系的投影关系。如果对左右目相机均构建重投影误差，会产生一个四自由度的误差向量。进行简化，利用双目相机视觉模型，设校正后的双目相机左右图像中的纵坐标相同。因此只需要计算右图像中路标点投影的横坐标/>
Figure BDA00041758497300001812
将双目相机重投影误差用一个三维向量表示：
Figure BDA00041758497300001813
其中
Figure BDA0004175849730000191
和/>
Figure BDA0004175849730000192
分别表示世界坐标系经过外参变换后，相机坐标系下的三维坐标点。f、cx、cy为左右目相机内参，使用式(43)代替式(42)中的视觉重投影误差，即可降低求解误差向量维度，降低后端优化的计算复杂度，提高计算速度。
实验测试例
本设计是对原始的VINS-Mono视觉惯性里程计的改进，通过充分考虑并推导了双目相机提取三维路标点过程中的偏置信息以及对位姿估计误差与三维路标点偏置关联进行分析，完成视觉惯性里程计的位姿定位结果修正。为验证提出方法对应的运算速度和定位精度，在嵌入式设备上利用面向视觉惯性SLAM算法的EuRoC数据集进行了定位精度与单张图像帧平均处理时间对比实验。
实验中运行单机客户端视觉惯性里程计的嵌入式硬件设备为NVIDIA JetsonXavier NX，适用于无人机、小型商业机器人和其他IoT嵌入式系统：在公开数据集中进行对比实验，采用EuRoc飞行器数据集。
选用绝对轨迹误差(Absolute Trajectory Error，ATE)中的均方根误差(RootMean Square Error，RMSE)以及平均绝对误差(Mean Absolute Error，MAE)两个指标衡量SLAM系统的位姿估计精度。绝对轨迹误差用于衡量估计轨迹与真实轨迹的一致程度，即轨迹准确度。其将真实轨迹与估计轨迹进行配准后，计算二者位置差值，可以用来评价SLAM算法的位姿估计结果。而平均绝对误差和均方根误差均可以降低噪声或偶发性对结果评估的影响。因此本实验记录RMSE以及MAE两个指标对算法性能进行综合分析。设本体的真实运动轨迹为x＝{x1,x2,…xn}，系统估计的相机运动轨迹为x′＝{x′1,x′2,…x′n}，则有：
Figure BDA0004175849730000193
Figure BDA0004175849730000194
其中tran表示位姿的平移向量。为了进行更直观地展示定位精度提升效果，对比实验中还涉及到性能提升的计算：
Figure BDA0004175849730000201
/>
其中θ表示算法定位精度的提升程度，β为改进算法的轨迹误差，α为基础算法的轨迹误差。同时，为了评估改进方法对时间的影响，还在不同数据集序列上进行了计算改进算法对每个图像帧的平均处理时间，证明了本算法可以满足实时计算要求。
一旦视觉惯性里程计完成基于松耦合的视觉惯性里程计初始化之后，即进行位姿修正步骤，将修正后的滑动窗口中的相机位姿以及逆深度数据代替原始VIN-Mono中的待优化变量进行非线性优化。本算法可通过为后端算法提供更优的初值来提高里程计的定位精。注意在对比实验中，本算法除了使用路标筛选策略外，还将每一关键帧中参与位姿修正的路标点个数限制在50以内。此参数的设定依据为：无论是在原始VINS-Mono系统还是本章提出的改进系统，视觉里程计前端每一帧图像最多提取的特征点数量均不超过150个。通过大量实际测试证明，对参与位姿修正的路标点个数限制在三分之一，可以较好地平衡位姿修正后的精度与里程计系统的计算效率。
同时，为了使测试结果更加地准确可靠，在每个数据序列上均进行了10次实验，并相应地计算出10次实验结果的RMSE和MAE指标平均值，在Jetson Xavier NX设备上对比原始VINS-Mono中的视觉惯性里程计和本方法的结果如下表所示(RMSE和MAE精度保留至保留小数点后三位)
表定位精度测试RMSE指标对比
Figure BDA0004175849730000202
Figure BDA0004175849730000211
表定位精度测试MAE指标对比
Figure BDA0004175849730000212
从以上结果可以看出，在十个测试序列中，本方法的两项精度指标均比原始的视觉惯性里程计方法表现更加出色。最好的RMSE和MAE指标算法定位精度提升均在测试序列为V2_03_difficu lt，分别为34.22％和30.93％。在平均的轨迹长度为82.045m的EuRoc飞行器数据集上，平均的RMSE指标的算法定位精度提升为12.22％，平均的MAE指标的算法定位精度提升为13.63％，为了更直观地表示定位精度的提升，分别绘制本算法与原始VINS-Mono在数据序列MH_02_easy和V2_03_difficu lt两种不同条件场景中的轨迹对比图以及随着时间增长在三个方向上与轨迹真值的对比结果，具体如图6所示，其中图6(a)为MH_02_easy序列轨迹对比图；图6(b)为V2_03_difficu lt序列轨迹对比图；图6(c)为MH_02_easy三方向轨迹对比图；图6(d)为V2_03_difficu lt三方向轨迹对比图，图中虚线代表Euroc数据集给出的轨迹真值，标记处两个实线分别代表原始的VINS-Mono系统计算出的轨迹及本方法计算出的轨迹，从下图中的框中部分可以看出，在各个数据序列中，本方法标记的实线更接近轨迹真值，证明了本算法在对位姿进行修正后可以提升视觉惯性里程计的定位精度。
同时，为了计算本方法中引入位姿修正步骤对时间耗时的影响，还与原始的VINS-Mono系统进行了单张图像帧在基于ROS框架中vi ns_est imator节点的平均计算时间对比，计算时间包含了位姿修正与后端优化两部分时间。这是因为两种算法的feature_tracker节点均只执行特征提取与光流跟踪，并发布特征点消息供vi ns_est imator订阅并输出关键帧位姿，而本方法是将前端的单目相机传感器输入改为双目，经过在嵌入式设备上实际的测试比较(利用了Jetson Xavier NX上的GPU进行加速)，相差时间可以忽略不计，时间测试如下表所示，其中计算差值为本算法的平均计算时间减去VIN-Mono算法的平均计算时间，同样地，为了使结果更加可信，在每个数据序列上均进行了10次实验，并计算出10次实验结果的平均耗时：
表单张图像帧在vins_estimator节点中平均计算时间
Figure BDA0004175849730000221
从上表结果可以看出，在V1_02_medium、V1_03_d ifficu lt、V2_01_easy和V2_03_difficu lt四个数据序列上的时间差值明显比其它序列低，说明加入误差修正步骤后对原始的视觉惯性里程计算算法计算时间影响程度较低。其原因在于，本算法中路标点的偏置计算占据了大部分增加的时间耗时，而上述四个数据集均为Vicon Room场景，大部分为白色墙体，场景中的纹理信息不如MH_01～05的Machine Hal l场景丰富，因此需要计算的路标点偏置数量较少，并且从图6(b)可以看出，由于在室内场景Vicon Room中飞行器的位姿变化较快，参与位姿修正的路标点较少，因此位姿修正步骤的时间影响程度较低。经过计算，在十个数据集序列中的平均增加时间耗时为8.829ms，平均增加时间占比为原始视觉惯性里程计的19.26％。经过时间耗时实验验证，本方法可以实现在嵌入式设备上进行实时计算。
综上，本算法实现了对深度信息误差以及位姿估计误差的消除，提高了视觉惯性里程计的定位精度。
需要说明的是，以上内容仅仅说明了本发明的技术思想，不能以此限定本发明的保护范围，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰均落入本发明权利要求书的保护范围之内。

Claims (8)

1.利用路标点偏置位姿修正的双目视觉惯性里程计方法，其特征在于，包括以下步骤：
S1，数据预处理：对输入的双目图像进行数据预处理，得到筛选后的关键帧；所述数据预处理至少包括提取特征点并进行光流追踪、IMU预积分以及关键帧筛选；
S2，视觉惯性里程计快速初始化：采用视觉和IMU的松耦合进行视觉惯性里程计快速初始化，双目相机通过三角测量恢复特征点深度，利用PnP算法进行纯视觉的相机位姿估计，再将其与IMU预积分结果进行对齐，恢复尺度因子、重力、陀螺仪偏置和每一帧的速度；所述初始化至少包括陀螺仪偏置校正、初始化速度、重力向量和尺度因子；
S3，位姿估计修正：根据BA优化的目标函数、双目立体视觉相机观测方程推导出路标点偏置与位姿估计误差关联关系，推导出光流跟踪特征点过程中路标点的逆深度偏置，利用关联关系对位姿估计进行修正；
S4，后端非线性优化：基于紧耦合的后端非线性优化方法，构建优化误差函数迭代求解，实现对系统状态量进行紧耦合的非线性优化以得到更精确的定位数据；所述系统状态量包括相机位姿、IMU加速度计偏置、陀螺仪偏置、相机到IMU的外参、三维路标点的逆深度。
2.如权利要求1所述利用路标点偏置位姿修正的双目视觉惯性里程计方法，其特征在于：所述步骤S1具体包括：
S11：采用特征点法估计相机位姿，提取特征点，并采用光流跟踪法匹配特征点；
S12：采用IMU预积分方法对测量值之间的相对状态量进行处理，给出预积分状态的协方差矩阵以及雅可比矩阵，实现与绝对位姿的解耦，构建误差函数对关键帧位姿的迭代优化；所述相对状态量至少包括当前相对于上一时刻的姿态和速度、位移；
S13：关键帧筛选：满足以下两个条件之一就判定为关键帧：
(a)根据视差判断，通过计算光流跟踪的Shi-Tomasi角点的视差来进行关键帧筛选，若视差大于设定阈值，则将其判定为关键帧；
(b)根据特征点数量判断，若图像帧跟踪的特征点数量没有超过设定阈值，将其设定为关键帧。
3.如权利要求2所述利用路标点偏置位姿修正的双目视觉惯性里程计方法，其特征在于：所述步骤S12中，通过IMU预积分方法，给出预积分状态的协方差矩阵以及雅可比矩阵，即推导出t+δt时刻雅可比矩阵Jt+δt的迭代关系：
Figure FDA0004175849720000021
其中，J代表IMU测量值的误差的雅可比矩阵；I代表单位矩阵；F代表线性误差系数。
4.如权利要求2所述利用路标点偏置位姿修正的双目视觉惯性里程计方法，其特征在于：所述步骤S2中，对陀螺仪偏置校正，得出陀螺仪偏置bω：
Figure FDA0004175849720000022
其中，
Figure FDA0004175849720000023
是bω是对应雅可比矩阵/>
Figure FDA0004175849720000024
对/>
Figure FDA0004175849720000025
的偏导；/>
Figure FDA0004175849720000026
是前者的转置矩阵，/>
Figure FDA0004175849720000027
代表克罗内克积；/>
Figure FDA0004175849720000028
代表k-1帧到k+1帧的旋转矩阵；/>
Figure FDA0004175849720000029
代表第k帧对应IMU坐标系到上一帧的旋转矩阵；/>
Figure FDA00041758497200000210
代表第k+1帧对应IMU坐标系到帧的旋转矩阵，vec代表矩阵向量化；
利用Cholesky分解求解下式，恢复出待优化变量
Figure FDA00041758497200000211
即可得到初始化后的速度、重力向量和尺度因子：
Figure FDA0004175849720000031
其中，I代表单位矩阵；
Figure FDA0004175849720000032
代表参考帧下的重力向量；/>
Figure FDA0004175849720000033
代表k帧对应速度；/>
Figure FDA0004175849720000034
代表k+1帧对应速度；s代表尺度因子；/>
Figure FDA0004175849720000035
和/>
Figure FDA0004175849720000036
代表相邻两IMU的预积分项；/>
Figure FDA0004175849720000037
代表参考帧到第k帧IMU的旋转矩阵；/>
Figure FDA0004175849720000038
代表第k帧相机到参考帧的旋转矩阵；Δtk代表间隔时间；
Figure FDA0004175849720000039
代表IMU到相机的旋转矩阵。
5.如权利要求3或4所述利用路标点偏置位姿修正的双目视觉惯性里程计方法，其特征在于：所述步骤S3中推导路标点偏置与位姿估计误差关联关系具体为：将相机位姿以及三维点同时作为优化变量，非线性最小二乘问题被建模为最小化重投影误差，即集束调整(Bundle Adjustment,BA)，BA优化的目标函数为：
Figure FDA00041758497200000310
其中x为相机位姿，将n个世界坐标系下的三维路标点pi转化到像素坐标系下的特征点zi的转化关系表示为h(x,pi)；根据双目立体视觉相机模型，观测方程h(x,pi)为非线性方程，因此上述最小二乘优化问题需要通过迭代优化算法求解，对h(x,pi)在初始值x(k)处进行一阶泰勒展开得到：
h(x，pi)＝h(x(k)，pi)+Jx(x-x(k))
Jx为h(x,pi)关于位姿x的雅克比矩阵；对h(x(k),pi)在初始值
Figure FDA00041758497200000311
处进行一阶泰勒展开：
Figure FDA00041758497200000312
Figure FDA00041758497200000314
为h(x(k),pi)关于三维路标点pi的雅克比矩阵，对h(x,pi)的位姿更新利用高斯牛顿迭代求解，表示为：
Figure FDA00041758497200000313
式中
Figure FDA0004175849720000041
为Jx的伪逆矩阵，和上面的一阶泰勒展开式，进行联合推导，可以得到：
Figure FDA0004175849720000042
迭代过程中位姿估计误差x(k+1)-x(k)表示为∈x，
Figure FDA0004175849720000043
表示像素坐标系中的测量误差，/>
Figure FDA0004175849720000044
则表示三维路标点误差；由n个三维路标点偏置确定的位姿估计误差的期望可以表示为：
Figure FDA0004175849720000045
其中矩阵I3n×3n形式如下：
Figure FDA0004175849720000046
其中，Jx为h(x(k),pi)关于位姿x的雅可比矩阵，
Figure FDA0004175849720000047
为Jx的逆矩阵，Jp为h(x(k),pi)关于三维路标点p的雅可比矩阵，∈p是三位路标点误差，μp是测量误差偏置。
6.如权利要求5所述利用路标点偏置位姿修正的双目视觉惯性里程计方法，其特征在于：所述步骤S3中推导出了双目视觉提取路标点逆深度偏置，平均逆深度偏置
Figure FDA0004175849720000048
表示为：
Figure FDA0004175849720000049
Figure FDA00041758497200000410
其中，
Figure FDA00041758497200000411
是逆深度值，/>
Figure FDA00041758497200000412
是是平均逆深度偏置，/>
Figure FDA00041758497200000413
是某一帧的深度偏置。
7.如权利要求5所述利用路标点偏置位姿修正的双目视觉惯性里程计方法，其特征在于：所述步骤S3还包括路标点筛选，根据相机运动角速度进行筛选的策略如下：
Figure FDA0004175849720000051
其中，||ωt||2表示由IMU给出的相机运动角速度的l2范数，当计算出的路标点偏置大于对应情况下设定的阈值时即对其进行舍弃。
8.如权利要求6或7所述利用路标点偏置位姿修正的双目视觉惯性里程计方法，其特征在于：所述步骤S4改进双目视觉重投影误差项的紧耦合后端非线性优化，构建后端的目标函数包含三项误差分别为单目视觉重投影误差，IMU测量误差和边缘化信息，使用Levenberg-Marquardt(LM)法对目标函数进行迭代优化求解；
将双目相机重投影误差用一个三维向量表示：
Figure FDA0004175849720000052
其中，
Figure FDA0004175849720000053
和/>
Figure FDA0004175849720000054
分别表示世界坐标系经过外参变换后，相机坐标系下的三维坐标点，f、cx、cy为左右目相机内参。
CN202310391308.2A 2023-04-13 2023-04-13 利用路标点偏置位姿修正的双目视觉惯性里程计方法 Pending CN116342661A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310391308.2A CN116342661A (zh) 2023-04-13 2023-04-13 利用路标点偏置位姿修正的双目视觉惯性里程计方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310391308.2A CN116342661A (zh) 2023-04-13 2023-04-13 利用路标点偏置位姿修正的双目视觉惯性里程计方法

Publications (1)

Publication Number Publication Date
CN116342661A true CN116342661A (zh) 2023-06-27

Family

ID=86884127

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310391308.2A Pending CN116342661A (zh) 2023-04-13 2023-04-13 利用路标点偏置位姿修正的双目视觉惯性里程计方法

Country Status (1)

Country Link
CN (1) CN116342661A (zh)

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116871709A (zh) * 2023-09-08 2023-10-13 常州市鹏瑞自动化设备有限公司 激光切割异形圆管件的方法
CN116883502A (zh) * 2023-09-05 2023-10-13 深圳市智绘科技有限公司 相机位姿和路标点位置的确定方法、装置、介质及设备
CN117532604A (zh) * 2023-11-08 2024-02-09 哈尔滨工业大学 一种基于立体视觉的目标位姿及高阶运动信息观测方法
CN117760428A (zh) * 2024-02-22 2024-03-26 西北工业大学 一种基于多立体视觉惯性紧耦合的自主定位方法

Cited By (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116883502A (zh) * 2023-09-05 2023-10-13 深圳市智绘科技有限公司 相机位姿和路标点位置的确定方法、装置、介质及设备
CN116871709A (zh) * 2023-09-08 2023-10-13 常州市鹏瑞自动化设备有限公司 激光切割异形圆管件的方法
CN116871709B (zh) * 2023-09-08 2023-12-08 常州市鹏瑞自动化设备有限公司 激光切割异形圆管件的方法
CN117532604A (zh) * 2023-11-08 2024-02-09 哈尔滨工业大学 一种基于立体视觉的目标位姿及高阶运动信息观测方法
CN117532604B (zh) * 2023-11-08 2024-05-10 哈尔滨工业大学 一种基于立体视觉的目标位姿及高阶运动信息观测方法
CN117760428A (zh) * 2024-02-22 2024-03-26 西北工业大学 一种基于多立体视觉惯性紧耦合的自主定位方法
CN117760428B (zh) * 2024-02-22 2024-04-30 西北工业大学 一种基于多立体视觉惯性紧耦合的自主定位方法

Similar Documents

Publication Publication Date Title
CN109993113B (zh) 一种基于rgb-d和imu信息融合的位姿估计方法
CN107564061B (zh) 一种基于图像梯度联合优化的双目视觉里程计算方法
CN116342661A (zh) 利用路标点偏置位姿修正的双目视觉惯性里程计方法
CN111024066B (zh) 一种无人机视觉-惯性融合室内定位方法
CN112649016B (zh) 一种基于点线初始化的视觉惯性里程计方法
CN107341814B (zh) 基于稀疏直接法的四旋翼无人机单目视觉测程方法
CN112304307A (zh) 一种基于多传感器融合的定位方法、装置和存储介质
CN110726406A (zh) 一种改进的非线性优化单目惯导slam的方法
Liu et al. Direct visual odometry for a fisheye-stereo camera
CN107590827A (zh) 一种基于Kinect的室内移动机器人视觉SLAM方法
CN109166149A (zh) 一种融合双目相机与imu的定位与三维线框结构重建方法与系统
EP3379202B1 (en) A method for determining bias in an inertial measurement unit of an image acquisition device
CN110986968B (zh) 三维重建中实时全局优化和错误回环判断的方法及装置
CN111354043A (zh) 一种基于多传感器融合的三维姿态估计方法及装置
CN114001733B (zh) 一种基于地图的一致性高效视觉惯性定位算法
CN114693754B (zh) 一种基于单目视觉惯导融合的无人机自主定位方法与系统
Yu et al. Vision-aided inertial navigation with line features and a rolling-shutter camera
Eichhardt et al. Affine correspondences between central cameras for rapid relative pose estimation
CN114013449A (zh) 针对自动驾驶车辆的数据处理方法、装置和自动驾驶车辆
CN114485640A (zh) 基于点线特征的单目视觉惯性同步定位与建图方法及系统
CN112907633B (zh) 动态特征点识别方法及其应用
Mu et al. Visual navigation features selection algorithm based on instance segmentation in dynamic environment
CN111476842B (zh) 一种像机相对位姿估计方法及系统
Schill et al. Estimating ego-motion in panoramic image sequences with inertial measurements
CN111145267A (zh) 基于imu辅助的360度全景视图多相机标定方法

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