CN112649016A - 一种基于点线初始化的视觉惯性里程计方法 - Google Patents

一种基于点线初始化的视觉惯性里程计方法 Download PDF

Info

Publication number
CN112649016A
CN112649016A CN202011425027.7A CN202011425027A CN112649016A CN 112649016 A CN112649016 A CN 112649016A CN 202011425027 A CN202011425027 A CN 202011425027A CN 112649016 A CN112649016 A CN 112649016A
Authority
CN
China
Prior art keywords
imu
coordinate system
frame
line
camera
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.)
Granted
Application number
CN202011425027.7A
Other languages
English (en)
Other versions
CN112649016B (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.)
Nanchang University
Original Assignee
Nanchang 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 Nanchang University filed Critical Nanchang University
Priority to CN202011425027.7A priority Critical patent/CN112649016B/zh
Publication of CN112649016A publication Critical patent/CN112649016A/zh
Application granted granted Critical
Publication of CN112649016B publication Critical patent/CN112649016B/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
    • G01C22/00—Measuring distance traversed on the ground by vehicles, persons, animals or other moving solid bodies, e.g. using odometers, using pedometers
    • 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
    • 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)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Navigation (AREA)
  • Image Analysis (AREA)

Abstract

本发明提供了一种基于点线初始化的视觉惯性里程计方法，包括以下步骤：步骤一、通过相机采集视觉数据，通过IMU采集惯性数据；步骤二、图像特征提取跟踪：对任意两帧图像提取并且跟踪点特征和线特征，得到前后帧之间的特征对应关系；步骤三、IMU预积分：对任意两帧图像之间的所有IMU数据进行预积分，得到IMU先验值，若IMU零偏改变则根据新的零偏重新计算IMU预积分测量值；步骤四、初始化：将步骤二提取和跟踪到的特征以及步骤三计算得到的IMU预积分值送入视觉惯性初始化模型，得到初始参数值；步骤五、紧耦合视觉惯性定位：将初始参数送入紧耦合视觉惯性估计器，实现对载体的实时定位；本发明增加了系统在无纹理环境下的初始化成功率，降低了初始化时间。

Description

一种基于点线初始化的视觉惯性里程计方法
技术领域
本发明属于SLAM(Simultaneous Localization and Mapping，同步定位与建图)领域，具体涉及一种基于点线初始化的视觉惯性里程计方法。
背景技术
由于相机和IMU(Inertial Measurement Unit,惯性测量单元)传感器的低成本性以及采集信息能力的互补性，VIO(Visual-Inertial Odometry，视觉惯性里程计)已广泛应用于从机器人导航到自主驾驶等领域。作为VIO的初始步骤，初始化过程可以为后端提供尺度、前几个图像帧的位姿和速度、IMU零偏以及重力加速度方向等关键信息。因此，一个健壮的VIO系统必须要求有精确且快速收敛的初始化部分来提供后端所需要的初始信息。
VINS-Mono是传统经典的VIO方案，其初始化阶段从利用特征点的纯视觉SFM(Structure from Motion,从运动中恢复结构)开始，然后通过将该结构与IMU预积分测量值松耦合对齐来恢复度量尺度、速度、重力加速度方向和IMU零偏。
但是在某些特殊的无纹理环境中，VIO系统无法在场景中提取到足够的特征点，即没有有效的场景结构与IMU预积分测量值对齐，最终导致初始化过程失败甚至整个VIO系统的发散。
发明内容
针对上述VIO存在的问题，提出一种基于点线初始化的视觉惯性里程计方法，提升VIO系统在无纹理环境下初始化的成功率以及定位精度。其初始化过程的主要特征在于在SFM过程中结合特征点和特征线，构建了一个存在点和线的结构。
本发明为解决上述技术问题采取如下技术方案：一种基于点线初始化的视觉惯性里程计方法，包括以下步骤：
步骤一、通过相机采集视觉数据，通过IMU采集惯性数据；
步骤二、图像特征提取跟踪：对任意两帧图像提取并且跟踪点特征和线特征，得到前后帧之间的特征对应关系；
步骤三、IMU预积分：对任意两帧图像之间的所有IMU数据进行预积分，得到IMU先验值，若IMU零偏改变则根据新的零偏重新计算IMU预积分测量值；
步骤四、初始化：将步骤二提取和跟踪到的特征以及步骤三计算得到的IMU预积分值送入视觉惯性初始化模型，得到初始参数值；
步骤五、紧耦合视觉惯性定位：将初始参数送入紧耦合视觉惯性估计器，实现对载体的实时定位。
进一步地，步骤二所述图像特征提取跟踪包括如下具体步骤：
步骤2.1、对新帧快速提取角点，并且将所有的提取到的角点与上一帧的角点通过光流法匹配，得到前后两帧点特征之间的对应关系。
步骤2.2、通过LSD(Line Segment Detector，线段检测器)算法对新帧提取线特征并且使用LBD(Line Binary Descriptor，线二进制描述子)算法计算它们的描述子，随后根据描述子对上一帧和新帧的线特征进行匹配，得到前后两帧线特征之间的对应关系。
进一步地，步骤三所述IMU预积分包括如下具体步骤：
步骤3.1、收集两个图像帧之间的所有IMU测量值，IMU测量值方程表示为：
Figure BDA0002824390290000021
其中，at和ωt分别为t时刻IMU坐标系下加速度真实值和角速度真实值，am和ωm分别为t时刻IMU坐标系下加速度与角速度测量值，
Figure BDA0002824390290000022
和
Figure BDA0002824390290000023
分别为t时刻加速度计与陀螺仪零偏，gw是世界坐标系下的重力加速度，Rtw为世界坐标系到t时刻IMU坐标系的旋转矩阵，na和nω分别为加速度计与陀螺仪的高斯噪声。
步骤3.2、根据步骤3.1得到的若干组IMU数据进行预积分：
Figure BDA0002824390290000024
其中：
Figure BDA0002824390290000025
其中
Figure BDA0002824390290000026
和
Figure BDA0002824390290000027
表示第k个图像帧到从该图像帧开始第i个IMU数据的三个预积分项，δt为相邻两个IMU数据之间的时间值，
Figure BDA0002824390290000028
表示i时刻IMU坐标系到第k个图像帧的IMU坐标系的旋转矩阵，
Figure BDA0002824390290000029
和
Figure BDA00028243902900000210
分别表示i时刻加速度计与陀螺仪的测量值，
Figure BDA00028243902900000211
表示四元数乘法。
步骤3.3、如果IMU零偏改变，则重新计算IMU预积分测量值：
Figure BDA0002824390290000031
其中
Figure BDA0002824390290000032
和
Figure BDA0002824390290000033
表示重新计算之前的三个预积分项，
Figure BDA0002824390290000034
和
Figure BDA0002824390290000035
表示重新计算之后的三个预积分项，
Figure BDA0002824390290000036
和
Figure BDA0002824390290000037
分别为加速计与陀螺仪零偏的变化量。J表示各个预积分项关于IMU零偏的雅克比矩阵。
进一步地，步骤四所述初始化包括如下具体步骤：
步骤4.1、基于滑动窗口构造点线的结构
第一步，当新帧即将进入窗口时，判断窗口中是否存在与新帧的匹配特征点足够多并且视差足够大的帧；第二步，如果不存在该帧，实行边缘化，等待下一帧的到来，关于边缘化详见步骤5.2；如果存在，将该帧视为参考帧c0，用五点法求得窗口中所有的帧到参考帧的旋转与平移，并且三角化窗口中所有的点特征和线特征；第三步，对窗口中所有的状态量做BA(Bundle Adjustment，光束法平差)，窗口中所有的状态量为：
Figure BDA0002824390290000038
其中
Figure BDA0002824390290000039
和
Figure BDA00028243902900000310
分别为第k帧图像帧相机坐标系到参考帧相机坐标系之间平移量和四元数，li和Li分别为参考帧相机坐标系下第i个点特征的三维坐标和线特征的普吕克坐标，k表示从0到n的正整数。
BA通过最小化如下函数：
Figure BDA00028243902900000311
其中Fi和Gi分别是相机观察到点特征和线特征的集合，
Figure BDA00028243902900000312
为第i个点特征从第j帧图像帧投影到参考帧相机坐标系下的二维归一化平面坐标，
Figure BDA00028243902900000313
为第i个线特征从第j帧图像帧投影到参考帧相机坐标系下的普吕克坐标，ρ为鲁棒核函数，rfi表示li取归一化平面坐标后与
Figure BDA00028243902900000314
之间的二维差值，rli表示
Figure BDA00028243902900000315
和Li之间的六维差值。
步骤4.2、估计陀螺仪偏差
给定IMU与相机之间的外参，将状态量从相机坐标系转化到IMU坐标系：
Figure BDA00028243902900000316
其中s为构造点线结构的尺度信息，
Figure BDA00028243902900000317
表示归一化平移量，qbc为相机到IMU的旋转四元数，
Figure BDA00028243902900000318
和
Figure BDA00028243902900000319
分别表示第k个相机的IMU坐标系到参考帧坐标系下的旋转四元数和平移量，qbc和pbc分别表示相机坐标系到IMU坐标系的旋转四元数和平移量。
将步骤4.1得到的结构与IMU预积分对齐，得到：
Figure BDA0002824390290000041
其中，B表示滑动窗口中所有的图像帧，
Figure BDA0002824390290000042
和
Figure BDA0002824390290000043
分别表示第k+1和第k帧IMU坐标系到参考相机坐标系的旋转四元数。
步骤4.3、估计速度、重力加速度与尺度
估计的状态向量表示为：
Figure BDA0002824390290000044
其中v表示各个IMU坐标系下的速度，
Figure BDA0002824390290000045
为参考系下的重力加速度，该状态向量通过求解如下函数得到：
Figure BDA0002824390290000046
其中I为单位矩阵，pbc为相机到IMU的平移量，Δtk表示连续两个图像帧之间的时间间隔。
步骤4.4、重力加速度向量优化
给计算得到的重力加速度加上扰动：
Figure BDA0002824390290000047
其中，g为重力加速度的模长，
Figure BDA0002824390290000048
和g′分别为扰动前和扰动后的重力加速度单位向量，b1和b2为
Figure BDA0002824390290000049
正切空间上的任意一组基，ω1和ω2为待解的两个值。求解以下方程：
Figure BDA00028243902900000410
其中ω＝[ω1,ω2]Τ，b＝[b1,b2]。
步骤4.5、完成初始化
将IMU坐标系的位置和速度量，点特征的位置和线特征的位置按照尺度缩放：
Figure BDA0002824390290000051
其中p，v和l分别为原始位置量、速度值和点特征坐标，p′、v′、l′和L′分别为缩放后的位置量、速度值、点特征坐标和线特征普吕克坐标。原始的线坐标L＝[nΤ,dΤ]，n为坐标系原点与线特征所确定平面的法向量，d为线特征的方向向量。
进一步地，步骤五所述紧耦合视觉惯性定位包括如下具体步骤：
步骤5.1、紧耦合定位
对滑动窗口所有待估计的状态变量变量如下：
Figure BDA0002824390290000052
其中xk为IMU坐标系k时刻的状态，包括该时刻IMU坐标系到世界坐标系的平移、旋转、在世界坐标系下的速度以及零偏，λi表示第i个点特征在第一次被观察到时相机坐标系的逆深度，οi为第i个线特征在世界坐标系下的正交坐标，k表示从0到n的正整数。
最小化以下函数求解状态量：
Figure BDA0002824390290000053
其中
Figure BDA0002824390290000054
为bk和bk+1IMU坐标系之间预积分的测量值，
Figure BDA0002824390290000055
和
Figure BDA0002824390290000056
分别表示在第j个相机坐标系下被观测到的第i个点特征和线特征的观测值，B表示窗口中所有预积分项的集合，F和G分别表示被相机观测到的点特征和线特征的集合。对应的残差表示如下：
Figure BDA0002824390290000057
其中
Figure BDA0002824390290000058
表示四元数的三维误差量，vec表示取四元数的虚部。
Figure BDA0002824390290000061
其中
Figure BDA0002824390290000062
和
Figure BDA0002824390290000063
分别表示第k个特征点在第i个和第j个相机坐标系下的观测值。b1和b2为正切空间中观测值向量任意选择的一组基。
Figure BDA0002824390290000064
其中d(s,l)为点s到线l的距离，
Figure BDA0002824390290000065
和
Figure BDA0002824390290000066
为观测值线段的两个端点，l＝(l1,l2,l3)Τ。
步骤5.2、边缘化
当新帧进入窗口时，计算新帧与次新帧之间匹配到的特征点视差。若视差大于某一阈值，则认为次新帧是关键帧，边缘化最老帧；若视差小于该阈值，则认为次新帧不是关键帧，边缘化次新帧。
本发明的有益效果是：
本发明所提出的方法用于视觉惯性定位，所述方法同步视觉与IMU信息，在初始化阶段构建了点线结构的状态向量，定义了点和线的残差模型，建立了高精度的点线结构，增加了线特征丰富的几何约束，增加了点线结构中对图像帧的定位精度，增加了系统在无纹理环境下的初始化成功率，降低了初始化时间；在紧耦合优化中，改善了线特征重投影误差模型，更加能利用好线特征的几何约束。使用本发明提出的方法，相较于传统的VINS-Mono视觉惯性定位系统，定位精度可提高约11％。
附图说明
图1是基于点线初始化视觉惯性里程计流程图；
图2是本方法线重投影误差示意图；
图3是本方法与现有方法在MH05序列中定位误差随时间变化图；
图4是本方法与现有方法在MH05序列中定位误差分布图；
图5是本方法与现有方法在MH05序列中定位误差箱线图；
图6是本方法与现有方法在MH05序列中定位误差小提琴图。
具体实施方式
下面结合附图对本发明的技术方案做详细说明。
本技术领域技术人员可以理解的是，除非另外定义，这里使用的所有术语(包括技术术语和科学术语)具有与本发明所属领域中的普通技术人员的一般理解相同的意义。还应该理解的是，诸如通用字典中定义的那些术语应该被理解为具有与现有技术的上下文中的意义一致的意义，并且除非像这里一样定义，不会用理想化或过于正式的含义来解释。
如图1，一种基于点线初始化的视觉惯性里程计方法，包括如下步骤：
步骤1)，通过相机采集视觉数据，相机采集频率为20Hz，图片分辨率为752*480；通过IMU采集惯性数据，包括加速度与角速度，IMU采集频率为100Hz。
步骤2)，图像提取跟踪点线特征，具体如下：
a)，对新帧快速提取角点，并且将所有的提取到的角点与上一帧的角点通过光流法匹配，得到前后两帧点特征之间的对应关系，同时通过RANSAC(Random SampleConsensus，随机采样一致性)算法用以剔除离群值。
b)，通过LSD(Line Segment Detector，线段检测器)算法对新帧提取线特征并且使用LBD(Line Binary Descriptor，线二进制描述子)算法计算它们的描述子，随后根据描述子对上一帧和新帧的线特征进行匹配，得到前后两帧线特征之间的对应关系。
步骤3)，IMU预积分：对任意两帧图像之间的所有IMU数据进行预积分，得到IMU先验值，若IMU零偏改变则根据新的零偏重新计算IMU预积分测量值，具体如下：
IMU测量模型如下：
Figure BDA0002824390290000071
其中，at和ωt分别为t时刻IMU坐标系下加速度真实值和角速度真实值，am和ωm分别为t时刻IMU坐标系下加速度与角速度测量值，
Figure BDA0002824390290000072
和
Figure BDA0002824390290000073
分别为t时刻加速度计与陀螺仪零偏，gw是世界坐标系下的重力加速度，Rtw为世界坐标系到t时刻IMU坐标系的旋转矩阵，na和nω分别为加速度计与陀螺仪的高斯噪声。
根据IMU测量模型，计算预积分测量值：
Figure BDA0002824390290000074
其中：
Figure BDA0002824390290000075
其中
Figure BDA0002824390290000076
和
Figure BDA0002824390290000077
表示第k个图像帧到从该图像帧开始第i个IMU数据的三个预积分项，δt为相邻两个IMU数据之间的时间值，
Figure BDA0002824390290000078
表示i时刻IMU坐标系到第k个图像帧的IMU坐标系的旋转矩阵，
Figure BDA0002824390290000081
和
Figure BDA0002824390290000082
分别表示i时刻加速度计与陀螺仪的测量值，
Figure BDA0002824390290000083
表示四元数乘法。
由方程(21)可知，预积分的测量值受到IMU零偏的影响。在优化过程中，零偏被当作状态值一直发生改变。当零偏改变时，不重新计算预积分值，而是通过雅克比矩阵修正预积分值：
Figure BDA0002824390290000084
其中
Figure BDA0002824390290000085
和
Figure BDA0002824390290000086
表示重新计算之前的三个预积分项，
Figure BDA0002824390290000087
和
Figure BDA0002824390290000088
表示重新计算之后的三个预积分项，
Figure BDA0002824390290000089
和
Figure BDA00028243902900000810
分别为加速计与陀螺仪零偏的变化量。J表示各个预积分项关于IMU零偏的雅克比矩阵。
步骤4)，初始化，完成对初始参数的估计
a)，构造点线结构
优化问题算法复杂度需要被关注，为了降低算法复杂度，建立了一个基于滑动窗口的SFM问题，这能提高算法的效率并且能让算法实时运行。第一步，当新帧即将进入窗口时，判断窗口中是否存在与新帧的匹配特征点足够多并且视差足够大的帧；第二步，如果不存在该帧，实行边缘化，等待下一帧的到来，关于边缘化详见步骤5.b；如果存在，将该帧视为参考帧c0，用五点法求得窗口中所有的帧到参考帧的旋转与平移，并且三角化窗口中所有的点特征和线特征；第三步，对窗口中所有的状态量做BA(Bundle Adjustment，光束法平差)，窗口中所有的状态量为：
Figure BDA00028243902900000811
其中
Figure BDA00028243902900000812
和
Figure BDA00028243902900000813
分别为第k帧图像帧相机坐标系到参考帧相机坐标系之间平移量和四元数，li和Li分别为参考帧相机坐标系下第i个点特征的三维坐标和线特征的普吕克坐标，k表示从0到n的正整数。
BA通过最小化如下函数：
Figure BDA00028243902900000814
其中Fi和Gi分别是相机观察到点特征和线特征的集合，
Figure BDA00028243902900000815
为第i个点特征从第j帧图像帧投影到参考帧相机坐标系下的二维归一化平面坐标，
Figure BDA00028243902900000816
为第i个线特征从第j帧图像帧投影到参考帧相机坐标系下的普吕克坐标，ρ为鲁棒核函数，rfi表示li取归一化平面坐标后与
Figure BDA00028243902900000817
之间的二维差值，rli表示
Figure BDA00028243902900000818
和Li之间的六维差值。
b)，估计陀螺仪偏差
给定IMU与相机之间的外参，将状态量从相机坐标系转化到IMU坐标系：
Figure BDA0002824390290000091
其中s为构造点线结构的尺度信息，
Figure BDA0002824390290000092
表示归一化平移量，qbc为相机到IMU的旋转四元数。
Figure BDA0002824390290000093
和
Figure BDA0002824390290000094
分别表示第k个相机的IMU坐标系到参考帧坐标系下的旋转四元数和平移量，qbc和pbc分别表示相机坐标系到IMU坐标系的旋转四元数和平移量。
将步骤a得到的结构与IMU预积分对齐，得到：
Figure BDA0002824390290000095
其中，B表示滑动窗口中所有的图像帧，
Figure BDA0002824390290000096
和
Figure BDA0002824390290000097
分别表示第k+1和第k帧IMU坐标系到参考相机坐标系的旋转四元数。
c)，估计速度、重力加速度与尺度
估计的状态向量表示为：
Figure BDA0002824390290000098
其中v表示各个IMU坐标系下的速度，
Figure BDA0002824390290000099
为参考系下的重力加速度，该状态向量通过求解如下函数得到：
Figure BDA00028243902900000910
其中I为单位矩阵，pbc为相机到IMU的平移量，Δtk表示连续两个图像帧之间的时间间隔。
d)，重力加速度向量优化
根据重力向量方向上的正切空间上计算一组基，然后给计算得到的重力加速度加上来自基上的任意扰动：
Figure BDA00028243902900000911
其中，g为重力加速度的模长，
Figure BDA00028243902900000912
和g′分别为扰动前和扰动后的重力加速度单位向量，b1和b2为
Figure BDA00028243902900000913
正切空间上的任意一组基，ω1和ω2为待解的两个值。将公式(29)代入到公式(28)中迭代四次以确保需要求得解收敛：
Figure BDA0002824390290000101
其中ω＝[ω1,ω2]Τ，b＝[b1,b2]。
e)，完成初始化
将IMU坐标系的位置和速度量，点特征的位置和线特征的位置按照尺度缩放以保证后续步骤的特征是依据真实尺度计算的：
Figure BDA0002824390290000102
其中p，v和l分别为原始位置量、速度值和点特征坐标，p′、v′、l′和L′分别为缩放后的位置量、速度值、点特征坐标和线特征普吕克坐标。原始的线坐标L＝[nΤ,dΤ]，n为坐标系原点与线特征所确定平面的法向量，d为线特征的方向向量。
步骤5)，紧耦合定位
a)，紧耦合优化
对滑动窗口所有待估计的状态变量变量如下：
Figure BDA0002824390290000103
其中xk为IMU坐标系k时刻的状态，包括该时刻IMU坐标系到世界坐标系的平移、旋转、在世界坐标系下的速度以及零偏，λi表示第i个点特征在第一次被观察到时相机坐标系的逆深度，οi为第i个线特征在世界坐标系下的正交坐标，k表示从0到n的正整数。
最小化以下函数求解状态量：
Figure BDA0002824390290000104
其中
Figure BDA0002824390290000105
为bk和bk+1IMU坐标系之间预积分的测量值，
Figure BDA0002824390290000106
和
Figure BDA0002824390290000107
分别表示在第j个相机坐标系下被观测到的第i个点特征和线特征的观测值，B表示窗口中所有预积分项的集合，F和G分别表示被相机观测到的点特征和线特征的集合。对应的残差表示如下：
Figure BDA0002824390290000111
其中
Figure BDA0002824390290000112
表示四元数的三维误差量，vec表示取四元数的虚部。
Figure BDA0002824390290000113
其中
Figure BDA0002824390290000114
和
Figure BDA0002824390290000115
分别表示第k个特征点在第i个和第j个相机坐标系下的观测值。b1和b2为正切空间中观测值向量任意选择的一组基。
Figure BDA0002824390290000116
线重投影示意图如图2所示，其中d(s,l)为点s到线l的距离，
Figure BDA0002824390290000117
和
Figure BDA0002824390290000118
为观测值线段的两个端点，l＝(l1,l2,l3)Τ。
b)，边缘化
执行完步骤a后，窗口中其中一帧必须被边缘化以保证窗口中状态量的数量。当新帧进入窗口时，计算新帧与次新帧之间匹配到的特征点视差。若视差大于某一阈值，则认为次新帧是关键帧，边缘化最老帧；若视差小于该阈值，则认为次新帧不是关键帧，边缘化次新帧。总之，无论边缘化哪一帧，窗口内的帧数保证为一定的数量，即窗口的大小。
实施例一
本发明提出的方法理论上可应用于现有的传统的视觉惯性融合定位框架(VIO)。现有的传统的视觉惯性融合定位框架包括前端和后端两个模块，前端通过IMU和图像估算相邻图像间相机的运动，后端接受前端不同时刻估算的相机运动信息，对其进行局部与全局优化，得到全局一致的轨迹。
基于现有VINS-Mono算法，采用Euroc数据集的序列进行测试。该数据集包含了不同的房间和工业环境中，配备有VI-Sensor双目惯性相机的无人机动态运动。其图像采集频率为20Hz，IMU采样频率为200Hz，数据集提供真实轨迹。本实例使用内存8G的Intel Corei5-8300笔记本，本方法(表格中记为proposed)实验结果与VINS-Mono结果作比较。
图3为本方法及VINS-Mono方法在MH05序列中定位误差随时间变化对比图，本发明提出的方法初始误差小于现有算法，同时，相比于现有算法，本发明的轨迹更加稳定，误差较小。
图4、图5和图6为本方法及VINS-Mono方法在MH05序列中定位误差分布对比图、定位误差箱线对比图和定位误差小提琴对比图。由图可知，本发明的误差较现有算法相比较小且分布较为合理。
最后给出了Euroc序列在本方法和VINS-Mono两种方法的均方根定位误差(单位为米)定位结果。由表1可知本发明定位精度由于现有算法，定位平均精度提高了约11％。
表1
Figure BDA0002824390290000121
以上所述仅表达了本发明的优选实施方式，其描述较为具体和详细，但并不能因此而理解为对本发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形、改进及替代，这些都属于本发明的保护范围。因此，本发明专利的保护范围应以所附权利要求为准。

Claims (5)

1.一种基于点线初始化的视觉惯性里程计方法，其特征在于：包括以下步骤：
步骤一、通过相机采集视觉数据，通过IMU采集惯性数据；
步骤二、图像特征提取跟踪：对任意两帧图像提取并且跟踪点特征和线特征，得到前后帧之间的特征对应关系；
步骤三、IMU预积分：对任意两帧图像之间的所有IMU数据进行预积分，得到IMU先验值，若IMU零偏改变则根据新的零偏重新计算IMU预积分测量值；
步骤四、初始化：将步骤二提取和跟踪到的特征以及步骤三计算得到的IMU预积分值送入视觉惯性初始化模型，得到初始参数值；
步骤五、紧耦合视觉惯性定位：将初始参数送入紧耦合视觉惯性估计器，实现对载体的实时定位。
2.根据权利要求1所述的一种基于点线初始化的视觉惯性里程计方法，其特征在于：步骤二所述图像特征提取跟踪具体包括如下步骤：
步骤2.1、对新帧快速提取角点，并且将所有的提取到的角点与上一帧的角点通过光流法匹配，得到前后两帧点特征之间的对应关系；
步骤2.2、通过LSD算法对新帧提取线特征并且使用LBD算法计算它们的描述子，随后根据描述子对上一帧和新帧的线特征进行匹配，得到前后两帧线特征之间的对应关系。
3.根据权利要求1所述的一种基于点线初始化的视觉惯性里程计方法，其特征在于：步骤三所述IMU预积分具体包括如下步骤：
步骤3.1、收集两个图像帧之间的所有IMU测量值，IMU测量值方程表示为：
Figure FDA0002824390280000011
其中，at和ωt分别为t时刻IMU坐标系下加速度真实值和角速度真实值，am和ωm分别为t时刻IMU坐标系下加速度与角速度测量值，bat和bωt分别为t时刻加速度计与陀螺仪零偏，gw是世界坐标系下的重力加速度，Rtw为世界坐标系到t时刻IMU坐标系的旋转矩阵，na和nω分别为加速度计与陀螺仪的高斯噪声；
步骤3.2、根据步骤3.1得到的若干组IMU数据进行预积分：
Figure FDA0002824390280000021
其中：
Figure FDA0002824390280000022
其中
Figure FDA0002824390280000023
和
Figure FDA0002824390280000024
表示第k个图像帧到从该图像帧开始第i个IMU数据的三个预积分项，δt为相邻两个IMU数据之间的时间值，
Figure FDA0002824390280000025
表示i时刻IMU坐标系到第k个图像帧的IMU坐标系的旋转矩阵，
Figure FDA0002824390280000026
和
Figure FDA0002824390280000027
分别表示i时刻加速度计与陀螺仪的测量值，
Figure FDA0002824390280000028
表示四元数乘法；
步骤3.3、如果IMU零偏改变，则重新计算IMU预积分测量值：
Figure FDA0002824390280000029
其中
Figure FDA00028243902800000210
和
Figure FDA00028243902800000211
表示重新计算之前的三个预积分项，
Figure FDA00028243902800000212
和
Figure FDA00028243902800000213
表示重新计算之后的三个预积分项，
Figure FDA00028243902800000214
和
Figure FDA00028243902800000215
分别为加速计与陀螺仪零偏的变化量，J表示各个预积分项关于IMU零偏的雅克比矩阵。
4.根据权利要求1所述的一种基于点线初始化的视觉惯性里程计方法，其特征在于：步骤四所述初始化具体包括如下步骤：
步骤4.1、基于滑动窗口构造点线的结构
第一步，当新帧即将进入窗口时，判断窗口中是否存在与新帧的匹配特征点足够多并且视差足够大的帧；第二步，如果不存在该帧，实行边缘化，等待下一帧的到来；如果存在，将该帧视为参考帧c0，用五点法求得窗口中所有的帧到参考帧的旋转与平移，并且三角化窗口中所有的点特征和线特征；第三步，对窗口中所有的状态量做BA，窗口中所有的状态量为：
Figure FDA00028243902800000216
其中
Figure FDA0002824390280000031
和
Figure FDA0002824390280000032
分别为第k帧图像帧相机坐标系到参考帧相机坐标系之间平移量和四元数，li和Li分别为参考帧相机坐标系下第i个点特征的三维坐标和线特征的普吕克坐标，k表示从0到n的正整数；
BA通过最小化如下函数：
Figure FDA0002824390280000033
其中Fi和Gi分别是相机观察到点特征和线特征的集合，
Figure FDA0002824390280000034
为第i个点特征从第j帧图像帧投影到参考帧相机坐标系下的二维归一化平面坐标，
Figure FDA0002824390280000035
为第i个线特征从第j帧图像帧投影到参考帧相机坐标系下的普吕克坐标，ρ为鲁棒核函数，rfi表示li取归一化平面坐标后与
Figure FDA0002824390280000036
之间的二维差值，rli表示
Figure FDA0002824390280000037
和Li之间的六维差值；
步骤4.2、估计陀螺仪偏差
给定IMU与相机之间的外参，将状态量从相机坐标系转化到IMU坐标系：
Figure FDA0002824390280000038
其中s为构造点线结构的尺度信息，
Figure FDA0002824390280000039
表示归一化平移量，qbc为相机到IMU的旋转四元数，
Figure FDA00028243902800000310
和
Figure FDA00028243902800000311
分别表示第k个相机的IMU坐标系到参考帧坐标系下的旋转四元数和平移量，qbc和pbc分别表示相机坐标系到IMU坐标系的旋转四元数和平移量；
将步骤4.1得到的结构与IMU预积分对齐，得到：
Figure FDA00028243902800000312
其中，B表示滑动窗口中所有的图像帧，
Figure FDA00028243902800000313
和
Figure FDA00028243902800000314
分别表示第k+1和第k帧IMU坐标系到参考相机坐标系的旋转四元数；
步骤4.3、估计速度、重力加速度与尺度
估计的状态向量表示为：
Figure FDA00028243902800000315
其中v表示各个IMU坐标系下的速度，
Figure FDA00028243902800000316
为参考系下的重力加速度，该状态向量通过求解如下函数得到：
Figure FDA00028243902800000317
其中I为单位矩阵，pbc为相机到IMU的平移量，Δtk表示连续两个图像帧之间的时间间隔；
步骤4.4、重力加速度向量优化
给计算得到的重力加速度加上扰动：
Figure FDA0002824390280000041
其中，g为重力加速度的模长，
Figure FDA0002824390280000042
和g′分别为扰动前和扰动后的重力加速度单位向量，b1和b2为
Figure FDA0002824390280000043
正切空间上的任意一组基，ω1和ω2为待解的两个值；求解以下方程：
Figure FDA0002824390280000044
其中ω＝[ω1,ω2]Τ，b＝[b1,b2]；
步骤4.5、完成初始化
将IMU坐标系的位置和速度量，点特征的位置和线特征的位置按照尺度缩放：
Figure FDA0002824390280000045
其中p，v和l分别为原始位置量、速度值和点特征坐标，p′、v′、l′和L′分别为缩放后的位置量、速度值、点特征坐标和线特征普吕克坐标；原始的线坐标L＝[nΤ,dΤ]，n为坐标系原点与线特征所确定平面的法向量，d为线特征的方向向量。
5.根据权利要求1所述的一种基于点线初始化的视觉惯性里程计方法，其特征在于：步骤五所述紧耦合视觉惯性定位具体包括如下步骤：
步骤5.1、紧耦合定位
对滑动窗口所有待估计的状态变量变量如下：
Figure FDA0002824390280000046
其中xk为IMU坐标系k时刻的状态，包括该时刻IMU坐标系到世界坐标系的平移、旋转、在世界坐标系下的速度以及零偏，λi表示第i个点特征在第一次被观察到时相机坐标系的逆深度，οi为第i个线特征在世界坐标系下的正交坐标，k表示从0到n的正整数；
最小化以下函数求解状态量：
Figure FDA0002824390280000051
其中
Figure FDA0002824390280000052
为bk和bk+1IMU坐标系之间预积分的测量值，
Figure FDA0002824390280000053
和
Figure FDA0002824390280000054
分别表示在第j个相机坐标系下被观测到的第i个点特征和线特征的观测值，B表示窗口中所有预积分项的集合，F和G分别表示被相机观测到的点特征和线特征的集合；对应的残差表示如下：
Figure FDA0002824390280000055
其中
Figure FDA0002824390280000056
表示四元数的三维误差量，vec表示取四元数的虚部；
Figure FDA0002824390280000057
其中
Figure FDA0002824390280000058
和
Figure FDA0002824390280000059
分别表示第k个特征点在第i个和第j个相机坐标系下的观测值；b1和b2为正切空间中观测值向量任意选择的一组基；
Figure FDA00028243902800000510
其中d(s,l)为点s到线l的距离，
Figure FDA0002824390280000061
和
Figure FDA0002824390280000062
为观测值线段的两个端点，l＝(l1,l2,l3)Τ；
步骤5.2、边缘化
当新帧进入窗口时，计算新帧与次新帧之间匹配到的特征点视差；若视差大于某一阈值，则认为次新帧是关键帧，边缘化最老帧；若视差小于该阈值，则认为次新帧不是关键帧，边缘化次新帧。
CN202011425027.7A 2020-12-09 2020-12-09 一种基于点线初始化的视觉惯性里程计方法 Active CN112649016B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011425027.7A CN112649016B (zh) 2020-12-09 2020-12-09 一种基于点线初始化的视觉惯性里程计方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011425027.7A CN112649016B (zh) 2020-12-09 2020-12-09 一种基于点线初始化的视觉惯性里程计方法

Publications (2)

Publication Number Publication Date
CN112649016A true CN112649016A (zh) 2021-04-13
CN112649016B CN112649016B (zh) 2023-10-03

Family

ID=75350395

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011425027.7A Active CN112649016B (zh) 2020-12-09 2020-12-09 一种基于点线初始化的视觉惯性里程计方法

Country Status (1)

Country Link
CN (1) CN112649016B (zh)

Cited By (14)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113124906A (zh) * 2021-05-06 2021-07-16 苏州挚途科技有限公司 基于在线标定的测距方法、装置及电子设备
CN113203407A (zh) * 2021-05-20 2021-08-03 南昌大学 基于关键平面的视觉惯性里程计方法
CN113223045A (zh) * 2021-05-19 2021-08-06 北京数研科技发展有限公司 基于动态物体语义分割的视觉与imu传感器融合定位系统
CN113432595A (zh) * 2021-07-07 2021-09-24 北京三快在线科技有限公司 设备状态获取方法、装置、计算机设备及存储介质
CN113465602A (zh) * 2021-05-26 2021-10-01 北京三快在线科技有限公司 导航方法、装置、电子设备及可读存储介质
CN113503873A (zh) * 2021-07-14 2021-10-15 北京理工大学 一种多传感器融合的视觉定位方法
CN113532420A (zh) * 2021-06-29 2021-10-22 浙江大学 一种融合点线特征的视觉惯性里程计方法
CN113670327A (zh) * 2021-08-11 2021-11-19 影石创新科技股份有限公司 视觉惯性里程计初始化方法、装置、设备和存储介质
CN114137247A (zh) * 2021-11-30 2022-03-04 上海科技大学 基于事件相机的速度感知方法、装置、设备和介质
CN114429500A (zh) * 2021-12-14 2022-05-03 中国科学院深圳先进技术研究院 一种基于点线特征融合的视觉惯性定位方法
CN114440877A (zh) * 2022-01-26 2022-05-06 北京航空航天大学 一种异步多相机视觉惯性里程计定位方法
US11521332B1 (en) 2021-06-29 2022-12-06 Midea Group Co., Ltd. Method and apparatus for optimization of a monocular visual-inertial localization system
WO2023273311A1 (en) * 2021-06-29 2023-01-05 Midea Group Co., Ltd. Method and apparatus for scale calibration and optimization of a monocular visual-inertial localization system
CN117739996A (zh) * 2024-02-21 2024-03-22 西北工业大学 一种基于事件相机惯性紧耦合的自主定位方法

Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20140316698A1 (en) * 2013-02-21 2014-10-23 Regents Of The University Of Minnesota Observability-constrained vision-aided inertial navigation
CN108731700A (zh) * 2018-03-22 2018-11-02 东南大学 一种视觉惯性里程计中的加权欧拉预积分方法
US20190204084A1 (en) * 2017-09-29 2019-07-04 Goertek Inc. Binocular vision localization method, device and system
CN110030994A (zh) * 2019-03-21 2019-07-19 东南大学 一种基于单目的鲁棒性视觉惯性紧耦合定位方法
CN110751123A (zh) * 2019-06-25 2020-02-04 北京机械设备研究所 一种单目视觉惯性里程计系统及方法
CN110763251A (zh) * 2019-10-18 2020-02-07 华东交通大学 视觉惯性里程计优化的方法及系统
CN111272192A (zh) * 2020-02-11 2020-06-12 清华大学 用于滑移转向机器人的联合位姿确定方法及装置
CN111578937A (zh) * 2020-05-29 2020-08-25 天津工业大学 同时优化外参数的视觉惯性里程计系统
CN111609868A (zh) * 2020-05-29 2020-09-01 电子科技大学 一种基于改进光流法的视觉惯性里程计方法
CN111983639A (zh) * 2020-08-25 2020-11-24 浙江光珀智能科技有限公司 一种基于Multi-Camera/Lidar/IMU的多传感器SLAM方法

Patent Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20140316698A1 (en) * 2013-02-21 2014-10-23 Regents Of The University Of Minnesota Observability-constrained vision-aided inertial navigation
US20190204084A1 (en) * 2017-09-29 2019-07-04 Goertek Inc. Binocular vision localization method, device and system
CN108731700A (zh) * 2018-03-22 2018-11-02 东南大学 一种视觉惯性里程计中的加权欧拉预积分方法
CN110030994A (zh) * 2019-03-21 2019-07-19 东南大学 一种基于单目的鲁棒性视觉惯性紧耦合定位方法
CN110751123A (zh) * 2019-06-25 2020-02-04 北京机械设备研究所 一种单目视觉惯性里程计系统及方法
CN110763251A (zh) * 2019-10-18 2020-02-07 华东交通大学 视觉惯性里程计优化的方法及系统
CN111272192A (zh) * 2020-02-11 2020-06-12 清华大学 用于滑移转向机器人的联合位姿确定方法及装置
CN111578937A (zh) * 2020-05-29 2020-08-25 天津工业大学 同时优化外参数的视觉惯性里程计系统
CN111609868A (zh) * 2020-05-29 2020-09-01 电子科技大学 一种基于改进光流法的视觉惯性里程计方法
CN111983639A (zh) * 2020-08-25 2020-11-24 浙江光珀智能科技有限公司 一种基于Multi-Camera/Lidar/IMU的多传感器SLAM方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
吴建峰；郭杭；熊剑: "视觉惯性组合导航系统发展综述", 《卫星导航定位技术文集（2020）》, pages 251 - 254 *
孟庆瑜: "基于点线特征的单目视觉-惯性里程计算法研究", 《中国优秀硕士论文电子期刊》 *

Cited By (21)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113124906A (zh) * 2021-05-06 2021-07-16 苏州挚途科技有限公司 基于在线标定的测距方法、装置及电子设备
CN113223045A (zh) * 2021-05-19 2021-08-06 北京数研科技发展有限公司 基于动态物体语义分割的视觉与imu传感器融合定位系统
CN113203407A (zh) * 2021-05-20 2021-08-03 南昌大学 基于关键平面的视觉惯性里程计方法
CN113203407B (zh) * 2021-05-20 2024-01-02 南昌大学 基于关键平面的视觉惯性里程计方法
CN113465602A (zh) * 2021-05-26 2021-10-01 北京三快在线科技有限公司 导航方法、装置、电子设备及可读存储介质
US11521332B1 (en) 2021-06-29 2022-12-06 Midea Group Co., Ltd. Method and apparatus for optimization of a monocular visual-inertial localization system
CN113532420A (zh) * 2021-06-29 2021-10-22 浙江大学 一种融合点线特征的视觉惯性里程计方法
CN113532420B (zh) * 2021-06-29 2024-01-30 浙江大学 一种融合点线特征的视觉惯性里程计方法
US11756231B2 (en) 2021-06-29 2023-09-12 Midea Group Co., Ltd. Method and apparatus for scale calibration and optimization of a monocular visual-inertial localization system
WO2023273311A1 (en) * 2021-06-29 2023-01-05 Midea Group Co., Ltd. Method and apparatus for scale calibration and optimization of a monocular visual-inertial localization system
CN113432595A (zh) * 2021-07-07 2021-09-24 北京三快在线科技有限公司 设备状态获取方法、装置、计算机设备及存储介质
CN113503873A (zh) * 2021-07-14 2021-10-15 北京理工大学 一种多传感器融合的视觉定位方法
CN113503873B (zh) * 2021-07-14 2024-03-12 北京理工大学 一种多传感器融合的视觉定位方法
CN113670327A (zh) * 2021-08-11 2021-11-19 影石创新科技股份有限公司 视觉惯性里程计初始化方法、装置、设备和存储介质
CN114137247A (zh) * 2021-11-30 2022-03-04 上海科技大学 基于事件相机的速度感知方法、装置、设备和介质
CN114137247B (zh) * 2021-11-30 2024-01-19 上海科技大学 基于事件相机的速度感知方法、装置、设备和介质
CN114429500A (zh) * 2021-12-14 2022-05-03 中国科学院深圳先进技术研究院 一种基于点线特征融合的视觉惯性定位方法
CN114440877A (zh) * 2022-01-26 2022-05-06 北京航空航天大学 一种异步多相机视觉惯性里程计定位方法
CN114440877B (zh) * 2022-01-26 2023-05-23 北京航空航天大学 一种异步多相机视觉惯性里程计定位方法
CN117739996A (zh) * 2024-02-21 2024-03-22 西北工业大学 一种基于事件相机惯性紧耦合的自主定位方法
CN117739996B (zh) * 2024-02-21 2024-04-30 西北工业大学 一种基于事件相机惯性紧耦合的自主定位方法

Also Published As

Publication number Publication date
CN112649016B (zh) 2023-10-03

Similar Documents

Publication Publication Date Title
CN112649016B (zh) 一种基于点线初始化的视觉惯性里程计方法
CN109307508B (zh) 一种基于多关键帧的全景惯导slam方法
CN109029433B (zh) 一种移动平台上基于视觉和惯导融合slam的标定外参和时序的方法
CN109993113B (zh) 一种基于rgb-d和imu信息融合的位姿估计方法
CN111258313B (zh) 多传感器融合slam系统及机器人
CN107564061B (zh) 一种基于图像梯度联合优化的双目视觉里程计算方法
CN107747941B (zh) 一种双目视觉定位方法、装置及系统
CN111156984B (zh) 一种面向动态场景的单目视觉惯性slam方法
CN111275763B (zh) 闭环检测系统、多传感器融合slam系统及机器人
CN109166149A (zh) 一种融合双目相机与imu的定位与三维线框结构重建方法与系统
CN112304307A (zh) 一种基于多传感器融合的定位方法、装置和存储介质
CN110726406A (zh) 一种改进的非线性优化单目惯导slam的方法
CN112749665B (zh) 一种基于图像边缘特征的视觉惯性slam方法
CN112634451A (zh) 一种融合多传感器的室外大场景三维建图方法
CN109029448B (zh) 单目视觉惯性定位的imu辅助跟踪模型
CN112734841B (zh) 一种用轮式里程计-imu和单目相机实现定位的方法
Jiang et al. DVIO: An optimization-based tightly coupled direct visual-inertial odometry
CN111623773B (zh) 一种基于鱼眼视觉和惯性测量的目标定位方法及装置
CN111665512B (zh) 基于3d激光雷达和惯性测量单元的融合的测距和绘图
CN111932674A (zh) 一种线激光视觉惯性系统的优化方法
CN114001733B (zh) 一种基于地图的一致性高效视觉惯性定位算法
CN111609868A (zh) 一种基于改进光流法的视觉惯性里程计方法
CN113516692A (zh) 一种多传感器融合的slam方法和装置
CN114529576A (zh) 一种基于滑动窗口优化的rgbd和imu混合跟踪注册方法
CN114485640A (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