CN112683305B - 一种基于点线特征的视觉-惯性里程计状态估计方法 - Google Patents

一种基于点线特征的视觉-惯性里程计状态估计方法 Download PDF

Info

Publication number
CN112683305B
CN112683305B CN202011401030.5A CN202011401030A CN112683305B CN 112683305 B CN112683305 B CN 112683305B CN 202011401030 A CN202011401030 A CN 202011401030A CN 112683305 B CN112683305 B CN 112683305B
Authority
CN
China
Prior art keywords
feature
frame
point
characteristic
module
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
CN202011401030.5A
Other languages
English (en)
Other versions
CN112683305A (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.)
National University of Defense Technology
Original Assignee
National University of Defense Technology
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 National University of Defense Technology filed Critical National University of Defense Technology
Priority to CN202011401030.5A priority Critical patent/CN112683305B/zh
Publication of CN112683305A publication Critical patent/CN112683305A/zh
Application granted granted Critical
Publication of CN112683305B publication Critical patent/CN112683305B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Landscapes

  • Image Analysis (AREA)

Abstract

本发明公开了一种基于点线特征的视觉‑惯性里程计状态估计方法，目的是解决现有状态估计方法耗时长、精度不高的缺点。技术方案是先构建由数据获取模块、视觉特征追踪模块、IMU预积分模块、特征重建模块、闭环检测与重定位模块、非线性优化模块组成的基于特征点和特征线的视觉‑惯性里程计系统。数据获取模块获取单目相机图像帧和IMU数据；IMU预积分模块对IMU数据进行预积分处理；视觉特征追踪模块使用特征点和特征线之间的约束对特征点线进行追踪；特征重建模块通过点线约束进行特征三维重建；闭环检测与重定位模块基于特征点信息进行闭环检测；非线性优化模块构建非线性优化损失函数并求解，获取机器人状态。本发明可降低处理时间，提升精度。

Description

一种基于点线特征的视觉-惯性里程计状态估计方法
技术领域
本发明涉及计算机图像处理、多传感融合领域，具体涉及运用“单目相机-IMU(Inertial Measurement Unit，中文名为惯性测量单元)传感套件”完成对机器人的状态估计的方法。
背景技术
SLAM(Simultaneous Localization And Mapping，即同时定位与建图)作为机器人领域一个重要分支，近年来得到了大家的广泛研究。SLAM试图解决这样的问题：一个机器人在未知的环境中运动，如何通过对环境的观测确定自身的运动轨迹，同时构建出环境的地图。 SLAM技术正是为了实现这个目标涉及到的诸多技术的总和。一个完整的SLAM系统主要包括视觉里程计部分和优化建图部分。视觉里程计部分粗略估计机器人状态，估计方法主要分为两种方法：特征点法和直接法。特征点法是目前机器人状态估计的主流方法，即首先在图像中提取特征点，对不同帧之间的特征点进行匹配，进而对匹配的特征点对做相关操作，估算相机的位姿。常用的点特征包括Harris角点、SIFT、SURF、ORB、HOG特征。不同于特征点法，直接法可以省去提取特征点的过程，直接利用图像中的灰度信息来估计机器人状态，但该方法尚不成熟，鲁棒性较差。优化建图部分根据全局信息优化机器人状态，并根据传感信息和机器人状态对机器人周围环境进行建图。
如果不需要对环境进行建图，视觉里程计就可以对机器人进行较为高效且准确的状态估计。但是仅基于一台单目相机，视觉里程计无法估计出周围环境的真实尺度，同时也会因为光线变化、运动模糊而产生较大的状态估计误差。近年来，IMU与单目相机的组合“单目相机-IMU传感套件”被更多的用在视觉里程计中，被称作视觉-惯性里程计。IMU可以很好的与单目相机形成互补，提供尺度信息以及极短时间内准确的状态估计，同时单目相机可以修正IMU长时间下的漂移问题。视觉-惯性里程计已经成为SLAM研究中一个重要领域了。
目前，大部分视觉-惯性里程计使用特征法进行特征追踪，并配合IMU数据对机器人的状态进行估计。视觉-惯性里程计的典型代表工作为VINS-Mono(VisualInertialNavigation System-Monocular，中文名为单目视觉-惯性导航系统)，2018年由TongQin等人在期刊IEEE Transactions on Robotics第34卷，第4期，第1004-1020页上发表的文章《VINS- Mono:Arobustandversatilemonocular visual-inertial state estimator》即《单目视觉-惯性导航系统：一个鲁棒且多功能的单目视觉-惯性状态估计器》提出。VINS-Mono基于单目相机对特征点进行追踪，利用IMU预积分方法对IMU测量数据进行处理，并通过将处理后的视觉信息和IMU 信息融合，估计机器人状态。
但是特征点能提供的环境结构信息较少，在一些纹理少、特征弱的场景，工作效果并不理想。基于此，2018年Yijia He等人在期刊Sensors第18卷，第4期，第1159-1179页上发表的文章《PL-VIO:Tightly-coupled monocular visual–inertial odometry usingpoint and line features》即《点线视觉-惯性里程计：利用特征点和特征线的紧耦合单目视觉-惯性里程计》提出了一种基于特征点和特征线的视觉-惯性里程计PL-VIO(Point andLine Visual Inertial Odometry，中文名为点线视觉-惯性里程计)。PL-VIO由于在特征追踪过程中加入了线特征信息，展现出了优于仅基于特征点的视觉-惯性里程计的精度和鲁棒性。但是目前基于特征线的视觉-惯性里程计，在处理线特征时，往往使用LSD(LineSegment Detector，中文名为线段检测器，2008年由Rafael Grompone von Gioi等人在期刊IEEE transactions on pattern analysis and machine intelligence第32卷，第4期，第722-732页上发表的文章《LSD:A fast line segment detector with a falsedetection control》即《线段检测器：一种具有错误检测控制的快速线段检测器》中提出)对视觉图像中的特征线段进行提取，再利用LBD(Line Band Descriptor，中文名为带状直线区域描述符，2013年由ZhangLilian等人在期刊Journal of Visual Communication andImage Representation第24卷，第7期，第794-805页上发表的文章《An efficient androbust line segment matching approach basedonLBD descriptor and pairwisegeometric consistency》即《基于带状直线区域描述符和成对几何一致性的高效鲁棒线段匹配方法》中提出)对特征线段进行描述，最后基于对特征线段的描述对不同图像中的特征线进行匹配，这个过程耗时是特征点追踪耗时的十余倍，同时错误的特征线匹配也会对视觉-惯性里程计精度造成影响。
因此，现有的基于特征点的视觉-惯性里程计状态估计方法具有状态估计精度不高的缺点，现有的基于特征点和特征线的视觉-惯性里程计状态估计方法具有特征追踪耗时较长的缺点。
发明内容
本发明要解决的技术问题是提供一种基于特征点和特征线的视觉-惯性里程计状态估计方法，以解决基于特征点和特征线的视觉-惯性里程计状态估计方法耗时长的缺点，同时解决仅基于特征点的视觉-惯性里程计状态估计方法精度不高的缺点。
为解决此问题，本发明提出了一种快速的基于特征点和特征线的视觉-惯性里程计状态估计方法，本发明在特征追踪和特征重建过程中，使用EDLines算法(EdgeDrawingLines，中文名为边缘绘制直线，2011年由CuneytAkinlar等人在期刊PatternRecognition Letters第32 卷，第13期，第1633-1642页上发表的文章《EDLines:A real-time line segment detector with a false detection control》即《边缘绘制直线：一种具有错误测控制的实时线段检测器》中提出) 提取特征线，使用特征点和特征线之间的约束对特征线进行追踪，通过点线约束进行特征线的三维重建，一方面压缩特征处理耗时，另一方面提升状态估计精度。
具体技术方案是：
第一步，构建基于特征点和特征线的视觉-惯性里程计系统。基于特征点和特征线的视觉 -惯性里程计系统由数据获取模块、视觉特征追踪模块、IMU预积分模块、特征重建模块、非线性优化模块、闭环检测与重定位模块组成。
数据获取模块与视觉特征追踪模块、IMU预积分模块相连。数据获取模块从苏黎世联邦理工学院的公开视觉-惯性里程计数据集“EuRoC MAV Datasets”(European RoboticsChallenge Micro Aerial Vehicle Datasets，中文名为欧洲机器人挑战赛微型飞行器数据集，该数据集中包括双目图像帧和IMU数据)下载数据，获取双目图像帧中左目相机图像帧和IMU数据，将双目图像帧中左目相机图像帧作为单目相机图像帧，将单目相机图像帧发送给视觉特征追踪模块，将IMU数据发送给IMU预积分模块。
视觉特征追踪模块与数据获取模块、特征重建模块、闭环检测与重定位模块相连。视觉特征追踪模块从数据获取模块接收单目相机图像帧，从单目相机图像帧中提取特征点和特征线信息，对连续单目相机图像帧中的特征进行匹配和追踪，将特征点和特征线信息以及特征追踪信息发送给特征重建模块，将特征点信息发送给闭环检测与重定位模块。
IMU预积分模块与数据获取模块、非线性优化模块相连。IMU预积分模块从数据获取模块接收IMU数据，利用IMU预积分算法对连续两帧单目相机图像帧中的IMU数据进行预积分处理，将预积分处理后的IMU预积分数据发送给非线性优化模块。
特征重建模块与视觉特征追踪模块、非线性优化模块相连。特征重建模块从视觉特征追踪模块获取特征点和特征线信息以及特征追踪信息，分别构建特征点管理和特征线管理数据结构，对第一次出现的特征进行三维重建，更新特征点管理和特征线管理数据结构，将更新后的特征点管理和特征线管理数据结构发送给非线性优化模块。
闭环检测与重定位模块与视觉特征追踪模块、非线性优化模块相连。闭环检测与重定位模块从视觉特征追踪模块获取每一帧单目相机图像帧中的特征点信息并进行储存，通过特征点信息匹配，对每一帧单目相机图像帧中的特征点信息进行闭环检测，将闭环检测结果发送给非线性优化模块。
非线性优化模块与特征重建模块、IMU预积分模块、闭环检测与重定位模块相连。非线性优化模块从特征重建模块获取最新的特征点管理数据结构和特征线管理数据结构，从IMU 预积分模块获取最新两帧图像间的IMU预积分数据，从闭环检测与重定位模块获取闭环检测结果，基于特征点管理数据结构和特征线管理数据结构、IMU预积分数据、系统状态估计历史数据、闭环检测结果构建非线性优化损失函数并进行求解，获取机器人状态。
第二步，令变量k＝1，数据获取模块从视觉-惯性里程计数据集“EuRoC MAVDatasets”中获取双目图像帧中左目相机图像帧和IMU数据，将双目图像帧中左目相机图像帧作为单目相机图像帧。令数据获取模块接收到第一帧左目相机图像帧时的时刻为t0时刻；
2.1，t0时刻数据获取模块将第一帧左目相机图像帧发送给视觉特征追踪模块；
2.2，视觉特征追踪模块从数据获取模块得到第一帧左目相机图像帧作为第一帧单目相机图像帧，视觉特征追踪模块利用FAST算法(中文名为快速，2008年由EdwardRosten等人在期刊IEEE transactions on pattern analysis and machineintelligence第32卷，第1期，第105- 119页上发表的文章《Faster and better：A machinelearning approach to corner detection》即《更快更好：用于角点检测的机器学习方法》中提出)提取第一帧单目相机图像帧中的MAXpoint个特征点，利用EDLines算法提取第一帧单目相机图像帧中MAXline条特征线，根据特征点坐标与特征线坐标关系获取第一帧单目相机图像帧中特征线上特征点信息，并将特征点和特征线信息发送至特征重建模块，将特征点信息发送至闭环检测与重定位模块，MAXpoint、MAXline均为正整数，本发明中MAXpoint设置为200、MAXline设置为200；
2.3，特征重建模块从视觉特征追踪模块得到第一帧单目相机图像帧中的特征点信息与特征线信息，特征重建模块构建第一帧特征点管理数据结构实例，该实例包括第一帧单目相机图像中每个特征点的图像坐标系坐标、每个特征点的相机坐标系坐标，特征重建模块构建第一帧特征线管理数据结构实例，该实例包括第一帧单目相机图像中每条特征线段起始点的图像坐标系坐标、每条特征线段起始点的相机坐标系坐标、每条特征线段上的特征点图像坐标系坐标、每条特征线段上所有特征点相机坐标系坐标集合；
2.4，闭环检测与重定位模块从视觉特征追踪模块得到第一帧单目相机图像帧中的特征点信息，并基于DBoW2算法(Distributed Bag of Words2算法，中文名分布式词袋2算法，2012 年由DorianGalvez-Lopez等人在期刊IEEE Transactions onRobotics第28卷，第5期，第1188- 1197页上发表的文章《Bags ofbinarywords for fastplace recognitionin image sequences》即《应用于图像序列中快速场景识别的二进制单词词袋》中提出)对第一帧单目相机图像中的特征点信息进行转化、存储。
第三步，数据获取模块从视觉-惯性里程计数据集“EuRoC MAV Datasets”中获取双目图像帧中左目相机图像帧和IMU数据，将双目图像帧中左目相机图像帧作为单目相机图像帧。数据获取模块在tk时刻从视觉-惯性里程计数据集“EuRoC MAVDatasets”中接收到第k+1帧单目相机图像帧。数据获取模块将第k+1帧单目相机图像帧发送给视觉特征追踪模块，将 [tk-1，tk]时间区间内从视觉-惯性里程计数据集“EuRoC MAV Datasets”中接收到IMU数据发送给IMU预积分模块。
第四步，视觉特征追踪模块对第k+1帧单目相机图像进行特征提取与特征追踪，过程如下：
4.1，视觉特征追踪模块利用KLT稀疏光流算法(KLT为该方法发明者BruceD.Lucas与 Takeo Kanade姓名缩写，1981年由Bruce D.Lucas等人在会议The InternationalJoint Conference on Artificial Intelligence上发表的文章《An iterative imageregistration technique with an application to stereo vision》即《迭代图像配准技术及其在双目视觉中的应用》中提出)，在第 k+1帧单目相机图像中跟踪第k帧单目相机图像中的特征点，得到第k+1帧单目相机图像中的特征点集合Pk+1，Pk+1中的元素为特征点，每个特征点由其在第k+1帧单目相机图像中的坐标表示；
4.2，视觉特征追踪模块利用EDLines算法提取第k+1帧单目相机图像中的MAXline条特征线段，得到第k+1帧单目相机图像中的特征线段集合Lk+1，Lk+1中的元素为特征线段la， a＝1，2，...，A，A表示Lk+1中特征线段的总数，la表示为la(sa，ea)，sa为特征线段la的起点， ea为特征线段la的终点，
Figure GDA0003483993470000051
表示sa在单目图像坐标系的x轴坐标，
Figure GDA0003483993470000052
表示sa在单目图像坐标系的y轴坐标。
4.3，视觉特征追踪模块对Lk+1中的特征线段进行整合，删除误检测线段，合并属于同一条直线的多条线段，得到整合后的新特征线段集合Lk+1，具体步骤如下：
4.3.1，对Lk+1中的特征线段由长至短进行排序，并对特征线段进行编号；
4.3.2，选取Lk+1中的特征线段la，对la与lb(lb∈la+1，la+2，...，lA)进行整合匹配：
4.3.2.1，若
Figure GDA0003483993470000053
交换lb起点与终点，即令lb(sb，eb)＝(eb，sb)；
4.3.2.2，若lb起点sb到la的距离
Figure GDA0003483993470000054
小于距离阈值Td，Td大小为1个像素，且终点eb到la的距离
Figure GDA0003483993470000055
大于阈值Td，且lb所在直线与la所在直线夹角小于角度阈值Tθ，Tθ大小为5度，将 la与lb合并为la(sa，eb)，转4.3.3；
4.3.2.3，若lb起点sb到la的距离
Figure GDA0003483993470000056
大于Td，且终点eb到la的距离
Figure GDA0003483993470000057
小于Td，且lb所在直线与la所在直线夹角小于Tθ，将la与lb合并为la(sb，ea)，转4.3.3；
4.3.2.4，若lb起点sb到la的距离
Figure GDA0003483993470000064
小于Td，且终点eb到la的距离
Figure GDA0003483993470000061
小于Td，且lb所在直线与la所在直线夹角小于Tθ，将la与lb合并为la(sa，ea)，转4.3.3；
4.3.2.5，若lb起点sb到la的距离
Figure GDA0003483993470000062
大于Td，且终点eb到la的距离
Figure GDA0003483993470000063
大于Td，或lb所在直线与la所在直线夹角大于Tθ，则la与lb无法合并，转4.3.4；
4.3.3，此时la与lb合并成功，将合并后的la存入Lk+1，并将lb从Lk+1中删除，若a＜A，转 4.3.2，令a＝a+1；若a＝A，转4.3.5；
4.3.4，此时la与lb合并不成功，若b＜A，转4.3.2，令b＝b+1；若b＝A，且a＜A，则令a＝a+1，转4.3.2；若b＝A，且a＝A，转4.3.5；
4.3.5，得到整合后的新特征线段集合Lk+1；
4.4，视觉追踪模块根据特征点与特征线关系，对第k+1帧单目相机图像中的特征点进行增补，输出新的特征点集合Pk+1，具体步骤如下：
4.4.1，根据2.2中的设置，单目相机图像帧中特征点数最大值MAXpoint＝200，设置单目图像帧中特征线段上特征点最大值MAXpoint_inline，令MAXpoint_inline＝150，并统计第k+1帧单目图像中的特征点数npoint，和第k+1帧单目图像中特征线段上的特征点数npoint_inline；
4.4.2，基于第k+1帧单目图像制作相对应的掩模图像Mat1，Mat1与第k+1帧单目图像尺寸相同，Mat1所有像素为黑，只有特征线段(Lk+1中所有线段)附近像素(距离特征线段10像素以内区域)为白色，同时特征线段上的特征点(Pk+1中特征点)周围像素(以特征点为圆心，半径为20像素区域)为黑色；
4.4.3，计算第一次特征点增补数目N1：
4.4.3.1，若MAXpoint_inline＞[MAXpoint-(npoint-npoint_inline)]，则N1＝MAXpoint- npoint，转4.4.4；
4.4.3.2，若MAXpoint_inline＜[MAXpoint-(npoint-npoint_inline)]，且MAXpoint_inline＞npoint_inline，则N1＝MAXpoint_inline-npoint_inline，转4.4.4；
4.4.4，在第k+1帧单目相机图像中，Mat1中白色像素所对应的区域内，使用FAST算法 (中文名为快速，2008年由Edward Rosten等人在期刊IEEEtransactions onpatternanalysis and machine intelligence第32卷，第1期，第105-119页上发表的文章《Fasterand better：A machine learning approach to comer detection》即《更快更好：用于角点检测的机器学习方法》中提出) 添加N1个特征点，且这些新添加的特征点之间间隔大于20个像素，将新加入的N1个特征点信息添加至特征点集合Pk+1中以更新Pk+1，并更新特征点数npoint＝npoint+N1；
4.4.5，基于第k+1帧单目相机图像制作相对应的掩模图像Mat2，Mat2与第k+1帧单目图像尺寸相同，Mat2所有像素为白，只有特征点(Pk+1中所有特征点)周围像素(以特征点为圆心，半径为20像素区域)为黑色；
4.4.6，计算第二次特征点增补数目N2，若MAXpoint＞npoint，则N2＝MAXpoint-npoint，转4.4.7；
4.4.7，在第k+1帧单目相机图像中，Mat2中白色像素所对应的区域内，使用FAST算法添加N2个特征点，且这些新添加的特征点之间间隔大于20个像素，并将新加入的N2个特征点信息添加至特征点集合Pk+1中以更新Pk+1；并更新特征点数npoint＝npoint+N2；
4.5，视觉追踪模块根据特征点与特征线关系，对第k+1帧单目相机图像中的特征线段和第k帧单目相机图像中的特征线段进行匹配，以实现单目相机图像帧间特征线的追踪，具体步骤如下：
4.5.1，对Lk+1中的特征线段进行筛选，记录Lk+1中特征线与Pk+1中特征点之间的关系(即每条特征线段上有哪些特征点)，将Lk+1中特征线段上特征点数小于2的特征线段从Lk+1中剔除，对Lk+1中的特征线段由长至短重新排序，更新Lk+1；
4.5.2，对重新排序后的Lk+1中的线段进行编号，令线段lc∈Lk+1，c＝1，2，...，C，C表示 Lk+1中线段的总数，lc可表示为lc(sc，ec)，sc为线段lc的起点，ec为线段lc的终点，
Figure GDA0003483993470000071
表示sc在单目图像坐标系x轴坐标，
Figure GDA0003483993470000075
表示sc在单目图像坐标系y轴坐标；
4.5.3，选取Lk+1中的特征线段lc，在第k帧单目相机图像特征线段集合Lk中依次寻找与lc相匹配的特征线段，即属于不同图像帧的同一条特征线段：
4.5.3.1，选取Lk中的特征线段ld，ld∈Lk，d＝1，2，...，D，D表示Lk中线段的总数，若
Figure GDA0003483993470000072
交换ld起点与终点，即令ld(sd，ed)＝(ed，sd)，转4.5.3.2；否则，直接转4.5.3.2；
4.5.3.2，若lc与ld拥有相同的特征点数超过2个，lc与ld线段长度差值小于阈值Tl，Tl大小为30个像素，且lc起点sc到ld起点sd距离
Figure GDA0003483993470000073
小于阈值Tpp，Tpp大小为60个像素，且lc终点 ec到ld终点ed距离
Figure GDA0003483993470000074
小于阈值Tpp，则lc与ld匹配成功；转4.5.4；若上述条件有一项不满足，则lc与ld匹配不成功，转4.5.5；
4.5.4，此时lc与ld匹配成功，表示第k帧单目图像中的特征线段ld在第k+1帧单目图像中追踪成功，记录匹配与追踪信息，包括lc与ld在Lk+1与Lk中的序号，若c＜C，令c＝c+1，转4.5.3；若c＝C，转4.5.6；
4.5.5，此时lc与ld匹配不成功，若d＜D，令d＝d+1，转4.5.3.1；若d＝D且c＜C，令 c＝c+1，转4.5.3；若d＝D且c＝C，转4.5.6；
4.5.6，得到特征线匹配后第k+1帧单目相机图像与第k帧单目相机图像中特征线段的追踪匹配信息，即第k+1帧单目相机图像中特征线段与第k帧单目相机图像中特征线段之间的对应关系；
4.6，视觉追踪模块将第k+1帧单目相机图像中的特征点信息、特征线信息传送至特征重建模块，将第k+1帧单目相机图像与第k帧单目相机图像中的特征点、特征线追踪匹配信息传送至特征重建模块，将第k+1帧单目相机图像中的特征点信息传送至闭环检测与重定位模块。
第五步，IMU预积分模块对从数据获取模块得到的[tk-1，tk]时间区间内的IMU数据，采用预积分算法进行预积分处理，预积分算法是2015年由Shaojie Shen等人在会议IEEE International Conference on Robotics and Automation上发表的文章《Tightly-coupled monocular visual-inertial fusion for autonomous flight ofrotorcraftMAVs》即《用于旋翼飞行器自动驾驶飞行的紧耦合单目视觉-惯性融合技术》中提出。IMU预积分模块将预积分处理后的[tk-1，tk]时间区间内的IMU预积分值发送至非线性优化模块。
第六步，特征重建模块从视觉特征追踪模块获取第k+1帧单目相机图像中的特征点信息、特征线信息，以及第k帧单目相机图像与第k+1帧单目相机图像中的特征点、特征线追踪匹配信息，根据以上信息分别构建第k+1帧特征点管理和特征线管理数据结构实例，对特征进行三维重建，更新特征点管理和特征线管理数据结构，将更新过的特征点管理和特征线管理数据结构发送给非线性优化模块。方法如下：
6.1，特征重建模块根据第k+1帧单目相机图像中的特征点信息和第k帧单目相机图像与第k+1帧单目相机图像中的特征点追踪匹配信息，构建第k+1帧特征点管理数据结构实例，该实例包括第k+1帧单目相机图像中每个特征点的图像坐标系坐标、每个特征点的相机坐标系坐标；
6.2，特征重建模块根据第k+1帧单目相机图像中的特征线信息和第k帧单目相机图像与第k+1帧单目相机图像中的特征线追踪匹配信息，构建第k+1帧特征线管理数据结构实例，该实例包括第k+1帧单目相机图像中每条特征线段起始点的图像坐标系坐标、每条特征线段起始点的相机坐标系坐标、每条特征线段上的特征点图像坐标系坐标、每条特征线段上所有特征点相机坐标系坐标集合；
6.3，若k＜4，特征检测模块将此时的第k+1帧特征点管理数据结构和特征线管理数据结构发送给非线性优化模块，转第七步；
6.4，若k≥4，特征检测模块对点特征和线特征进行三维重建，方法是：
6.4.1，特征重建模块对于第k+1帧单目相机图像中的特征点，筛选出至少在第k帧中出现过的特征点，联立这些特征点在第k+1、k、k-1、k-2、k-3帧中的相机坐标系坐标信息，利用奇异值分解算法(2017年由高翔等人在著作《视觉SLAM十四讲：从理论到实践》中介绍)，对这些特征点在相机坐标系中的深度进行求解，并在第k+1帧单目相机图像特征点管理数据结构实例中添加这些特征点的相机坐标系深度；
6.4.2，特征重建模块对于第k+1帧单目相机图像中的特征线，筛选出至少在第k帧中出现过的特征线lf，lf∈LF，LF表示第k+1帧单目相机图像中所有被筛选出来的特征线的集合，基于lf上特征点信息，进行三维重建，具体步骤如下：
6.4.2.1，特征重建模块将第k+1、k、k-1、k-2、k-3共计五帧图像帧中，所有lf上的特征点三维坐标转换至第k+1帧图像帧相机坐标系中，建立特征点集合
Figure GDA0003483993470000091
储存所有被转换至第k+1帧图像帧相机坐标系中特征点的三维坐标，此时
Figure GDA0003483993470000092
中特征点数量为
Figure GDA0003483993470000093
6.4.2.2，特征重建模块对
Figure GDA0003483993470000094
中的所有特征点进行基于RANSAC(Random SampleConsensus，中文名为随机样本共识，2017年由高翔等人在著作《视觉SLAM十四讲：从理论到实践》中介绍)算法的直线拟合外点剔除，即将不属于同一三维直线的特征点从
Figure GDA0003483993470000095
中剔除，此时
Figure GDA0003483993470000096
中特征点数量为
Figure GDA0003483993470000097
6.4.2.3，若
Figure GDA0003483993470000098
特征重建模块使用最小二乘直线拟合方法对
Figure GDA0003483993470000099
中的特征点进行三维拟合，将拟合直线表示方式转化为普吕克坐标表示方式，在第k+1帧特征线管理数据结构实例中添加特征线段lf的普吕克坐标表示；
6.4.2.4，若
Figure GDA00034839934700000910
特征重建模块将特征线段lf所有相关信息从第k+1帧特征线管理数据结构实例中删除；
6.4.3，特征重建模块将更新过的第k+1帧特征点管理数据结构和特征线管理数据结构发送给非线性优化模块，转第七步。
第七步，闭环检测与重定位模块从视觉特征追踪模块获取第k+1帧单目相机图像中的特征点信息，并基于DBoW2算法(DistributedBagofWords2算法，中文名分布式词袋2算法，2012年由DorianGalvez-Lopez等人在期刊IEEE Transactions on Robotics第28卷，第5期，第1188-1197页上发表的文章《Bags of binary words for fastplace recognitionin image sequences》即《应用于图像序列中快速场景识别的二进制单词词袋》中提出)对第k+1帧单目相机图像中的特征点信息进行转化、存储与检索，并利用VINS-Mono中的闭环检测算法，对第k+1 帧单目相机图像帧与之前的单目图像帧进行闭环检测匹配，若匹配成功，则表明发生了闭环，即机器人运行到了之前运行过的地点，闭环检测与重定位模块将闭环检测结果发送给非线性优化模块，转第八步；若匹配不成功，则表明没有发生闭环，闭环检测与重定位模块不与非线性优化模块进行交互，直接转第八步。
第八步，非线性优化模块从特征重建模块获取第k+1帧特征点管理和特征线管理数据结构，从IMU预积分模块获取[tk-1，tk]时间区间内的IMU预积分数据，从闭环检测与重定位模块获取闭环检测结果，若k＜9，非线性优化模块不对系统状态进行求解，直接转第九步；若 k≥9，非线性优化模块基于特征点数据、特征线数据、IMU预积分数据、系统状态估计历史数据、闭环检测数据构建非线性优化损失函数并进行求解，获取机器人状态。具体步骤如下：
8.1，非线性优化模块构造滑动窗口，窗口内包括{tk-9，tk-8，...，tk-1，tk}共十个时刻内机器人系统相关信息，机器人系统相关信息包括从特征重建模块接收到的第k-8，k-7，...，k+1 共十帧图像帧特征点管理和特征线管理数据结构、从IMU预积分模块中接收到[tk-10，tk-9]，[tk-9，tk-8]，...，[tk-1，tk]共十个时间区间内的IMU预积分信息、从闭环检测与重定位模块中接收到的闭环检测信息、除tk时刻外每个时刻机器人的位姿信息；
8.2，非线性优化模块将十个时刻中所有特征线段的普吕克坐标参数转化为线段所在三维直线的正交法表达参数；
8.3，非线性优化模块根据滑动窗口的信息，构造优化变量χ：
Figure GDA0003483993470000101
其中，xk-9，xk-8，...，xk表示滑动窗口内十个时刻机器人系统状态，λ0，λ1，...，λn表示滑动窗口内十个时刻中所有特征点在三维世界中的深度，
Figure GDA0003483993470000102
表示滑动窗口内十个时刻中所有特征线段的正交法表达参数，ti时刻机器人系统状态xi可以表达为：
Figure GDA0003483993470000103
其中，
Figure GDA0003483993470000104
代表ti时刻机器人在IMU坐标系相对世界坐标系的位移向量，
Figure GDA0003483993470000105
代表ti时刻机器人在IMU坐标系相对世界坐标系的速度向量，
Figure GDA0003483993470000106
代表ti时刻机器人在IMU坐标系相对世界坐标系的四元数，
Figure GDA0003483993470000107
代表ti时刻机器人在IMU坐标系中的IMU加速度计偏置，
Figure GDA0003483993470000108
代表ti时刻机器人在IMU坐标系中的IMU陀螺仪偏置；
8.4，非线性优化模块根据8.3中构造的优化变量构造优化损失函数：
Figure GDA0003483993470000111
其中：
Figure GDA0003483993470000112
表示IMU预积分残差项，
Figure GDA0003483993470000113
表示IMU坐标系中从ti时刻到ti+1时刻IMU预积分值，
Figure GDA0003483993470000114
表示IMU预积分数据集合，
Figure GDA0003483993470000115
表示IMU从ti时刻到ti+1时刻IMU预积分残差，该残差的具体构造方式同VINS-Mono；
Figure GDA0003483993470000116
表示特征点残差项，
Figure GDA0003483993470000117
表示相机坐标系中ti时刻第j个特征点的重投影误差，j≤l，
Figure GDA0003483993470000118
表示滑动窗口内所有特征点的集合，
Figure GDA0003483993470000119
表示ti时刻第j个特征点残差，该残差的具体构造方式同VINS-Mono；
Figure GDA00034839934700001110
表示特征线残差项，
Figure GDA00034839934700001111
表示相机坐标系中ti时刻第m个特征点的重投影误差，m≤n，
Figure GDA00034839934700001112
表示滑动窗口内所有特征线的集合，
Figure GDA00034839934700001113
表示ti时刻第 m个特征点残差，该残差的具体构造方式同PL-VIO；
Figure GDA00034839934700001114
表示滑动窗口外系统状态估计历史数据残差项，该残差项的具体构造方式同VINS-Mono；
Figure GDA00034839934700001115
表示闭环检测重定位残差项，若闭环检测发生在ti时刻与tv时刻之间，即tv时刻单目图像帧和ti时刻单目图像帧被判定为发生闭环检测，tv时刻可为ti时刻之前任一时刻，v<i，
Figure GDA00034839934700001116
表示相机坐标系中ti时刻第j个特征点在tv时刻单目图像帧上的重投影误差，
Figure GDA00034839934700001117
表示世界坐标系中tv时刻机器人的旋转四元数，
Figure GDA00034839934700001118
表示世界坐标系中tv时刻机器人的位移向量，下标w表示该数据为世界坐标系中的数据，
Figure GDA00034839934700001119
表示闭环检测模块中存储的图像信息集合，
Figure GDA00034839934700001120
表示滑动窗口内所有特征点的集合，
Figure GDA00034839934700001121
表示ti时刻闭环检测残差，该残差的具体构造方式同VINS-Mono；
8.5，非线性优化模块采用最小二乘优化方法(2017年由高翔等人在著作《视觉SLAM十四讲：从理论到实践》中介绍)对优化损失函数求解，获得tk时刻机器人位姿估计值、速度估计值、窗口内所有特征点深度估计值、窗口内所有特征线普吕克坐标参数估计值，转第九步。
第九步，令k＝k+1，若还能从视觉-惯性里程计数据集“EuRoC MAV Datasets”数据集中获取双目图像帧中左目相机图像帧和IMU数据，返回第三步，若不能从视觉-惯性里程计数据集“EuRoC MAV Datasets”数据集中获取双目图像帧中左目相机图像帧和IMU数据，转第十步。
第十步，结束。
采用本发明可以达到以下技术效果：
1.本发明在视觉-惯性里程计中同时利用了特征点信息和特征线信息，相比传统基于特征点和特征线的视觉-惯性里程计估计方法，本发明首先使用EDLines提取图像帧中的特征线，然后根据特征点和特征线之间的关系对连续两帧图像中的特征线进行匹配跟踪，在维持一定数量的匹配特征线基础上，大大降低了特征线处理时间；
2.在第六步特征重建过程中，本发明利用特征点和特征线之间的关系，对特征线进行三维拟合和重建，提升了特征线三维重建的精度，提升了视觉-惯性里程计状态估计的精度。
本发明在苏黎世联邦理工学院发布的公开视觉-惯性里程计数据集“EuRoC MAVDatasets” (European Robotics Challenge Micro Aerial Vehicle Datasets，中文名为欧洲机器人挑战赛微型飞行器数据集)上进行了实验验证，并与VINS-Mono、PL-VIO等视觉-惯性里程计方法做了对比实验，实验结果表明本发明不仅提高了机器人状态估计精度，还缩短了状态估计时间。
附图说明
图1为本发明总体流程图；
图2为本发明第一步构建的基于特征点和特征线的视觉-惯性里程计系统逻辑结构图；
图3为本发明与现有特征点和特征线的惯性-视觉里程计估计方法平均状态估计精度误差对比实验结果；
图4为本发明与现有基于特征点和特征线的惯性-视觉里程计方法特征追踪平均时间对比实验结果。
具体实施方式
图1为本发明总体流程图；如图1所示，本发明包括以下步骤：
第一步，构建基于特征点和特征线的视觉-惯性里程计系统。基于特征点和特征线的视觉 -惯性里程计系统如图2所示，由数据获取模块、视觉特征追踪模块、IMU预积分模块、特征重建模块、非线性优化模块、闭环检测与重定位模块组成。
数据获取模块与视觉特征追踪模块、IMU预积分模块相连。数据获取模块从苏黎世联邦理工学院的公开视觉-惯性里程计数据集“EuRoC MAV Datasets”下载数据，获取双目图像帧中左目相机图像帧和IMU数据，将双目图像帧中左目相机图像帧作为单目相机图像帧，将单目相机图像帧发送给视觉特征追踪模块，将IMU数据发送给IMU预积分模块。
视觉特征追踪模块与数据获取模块、特征重建模块、闭环检测与重定位模块相连。视觉特征追踪模块从数据获取模块接收单目相机图像帧，从单目相机图像帧中提取特征点和特征线信息，对连续单目相机图像帧中的特征进行匹配和追踪，将特征点和特征线信息以及特征追踪信息发送给特征重建模块，将特征点信息发送给闭环检测与重定位模块。
IMU预积分模块与数据获取模块、非线性优化模块相连。IMU预积分模块从数据获取模块接收IMU数据，利用IMU预积分算法对连续两帧单目相机图像帧中的IMU数据进行预积分处理，将预积分处理后的IMU预积分数据发送给非线性优化模块。
特征重建模块与视觉特征追踪模块、非线性优化模块相连。特征重建模块从视觉特征追踪模块获取特征点和特征线信息以及特征追踪信息，分别构建特征点管理和特征线管理数据结构，对第一次出现的特征进行三维重建，更新特征点管理和特征线管理数据结构，将更新后的特征点管理和特征线管理数据结构发送给非线性优化模块。
闭环检测与重定位模块与视觉特征追踪模块、非线性优化模块相连。闭环检测与重定位模块从视觉特征追踪模块获取每一帧单目相机图像帧中的特征点信息并进行储存，通过特征点信息匹配，对每一帧单目相机图像帧中的特征点信息进行闭环检测，将闭环检测结果发送给非线性优化模块。
非线性优化模块与特征重建模块、IMU预积分模块、闭环检测与重定位模块相连。非线性优化模块从特征重建模块获取最新的特征点管理数据结构和特征线管理数据结构，从IMU 预积分模块获取最新两帧图像间的IMU预积分数据，从闭环检测与重定位模块获取闭环检测结果，基于特征点管理数据结构和特征线管理数据结构、IMU预积分数据、系统状态估计历史数据、闭环检测结果构建非线性优化损失函数并进行求解，获取机器人状态。
第二步，令变量k＝1，数据获取模块从视觉-惯性里程计数据集“EuRoC MAVDatasets”中获取双目图像帧中左目相机图像帧和IMU数据，将双目图像帧中左目相机图像帧作为单目相机图像帧。令数据获取模块接收到第一帧左目相机图像帧时的时刻为t0时刻；
2.1，t0时刻数据获取模块将第一帧左目相机图像帧发送给视觉特征追踪模块；
2.2，视觉特征追踪模块从数据获取模块得到第一帧左目相机图像帧作为第一帧单目相机图像帧，视觉特征追踪模块利用FAST算法提取第一帧单目相机图像帧中的MAXpoint个特征点，利用EDLines算法提取第一帧单目相机图像帧中MAXline条特征线，根据特征点坐标与特征线坐标关系获取第一帧单目相机图像帧中特征线上特征点信息，并将特征点和特征线信息发送至特征重建模块，将特征点信息发送至闭环检测与重定位模块，MAXpoint设置为200、 MAXline设置为200；
2.3，特征重建模块从视觉特征追踪模块得到第一帧单目相机图像帧中的特征点信息与特征线信息，特征重建模块构建第一帧特征点管理数据结构实例，该实例包括第一帧单目相机图像中每个特征点的图像坐标系坐标、每个特征点的相机坐标系坐标，特征重建模块构建第一帧特征线管理数据结构实例，该实例包括第一帧单目相机图像中每条特征线段起始点的图像坐标系坐标、每条特征线段起始点的相机坐标系坐标、每条特征线段上的特征点图像坐标系坐标、每条特征线段上所有特征点相机坐标系坐标集合；
2.4，闭环检测与重定位模块从视觉特征追踪模块得到第一帧单目相机图像帧中的特征点信息，并基于DBoW2算法对第一帧单目相机图像中的特征点信息进行转化、存储。
第三步，数据获取模块从视觉-惯性里程计数据集“EuRoC MAV Datasets”中获取双目图像帧中左目相机图像帧和IMU数据，将双目图像帧中左目相机图像帧作为单目相机图像帧。数据获取模块在tk时刻从视觉-惯性里程计数据集“EuRoC MAV Datasets”中接收到第k+1帧单目相机图像帧。数据获取模块将第k+1帧单目相机图像帧发送给视觉特征追踪模块，将 [tk-1，tk]时间区间内从视觉-惯性里程计数据集“EuRoC MAV Datasets”中接收到IMU数据发送给IMU预积分模块。
第四步，视觉特征追踪模块对第k+1帧单目相机图像进行特征提取与特征追踪，过程如下：
4.1，视觉特征追踪模块利用KLT稀疏光流算法，在第k+1帧单目相机图像中跟踪第k帧单目相机图像中的特征点，得到第k+1帧单目相机图像中的特征点集合Pk+1，Pk+1中的元素为特征点，每个特征点由其在第k+1帧单目相机图像中的坐标表示；
4.2，视觉特征追踪模块利用EDLines算法提取第k+1帧单目相机图像中的MAXline条特征线段，得到第k+1帧单目相机图像中的特征线段集合Lk+1，Lk+1中的元素为特征线段la， a＝1，2，...，A，A表示Lk+1中特征线段的总数，la表示为la(sa，ea)，sa为特征线段la的起点， ea为特征线段la的终点，
Figure GDA0003483993470000141
表示sa在单目图像坐标系的x轴坐标，
Figure GDA0003483993470000142
表示sa在单目图像坐标系的y轴坐标。
4.3，视觉特征追踪模块对Lk+1中的特征线段进行整合，删除误检测线段，合并属于同一条直线的多条线段，得到整合后的新特征线段集合Lk+1，具体步骤如下：
4.3.1，对Lk+1中的特征线段由长至短进行排序，并对特征线段进行编号；
4.3.2，选取Lk+1中的特征线段la，对la与lb(lb∈la+1，la+2，...，lA)进行整合匹配：
4.3.2.1，若
Figure GDA0003483993470000151
交换lb起点与终点，即令lb(sb，eb)＝(eb，sb)；
4.3.2.2，若lb起点sb到la的距离
Figure GDA0003483993470000152
小于距离阈值Td，Td大小为1个像素，且终点eb到la的距离
Figure GDA0003483993470000153
大于阈值Td，且lb所在直线与la所在直线夹角小于角度阈值Tθ，Tθ大小为5度，将 la与lb合并为la(sa，eb)，转4.3.3；
4.3.2.3，若lb起点sb到la的距离
Figure GDA0003483993470000154
大于Td，且终点eb到la的距离
Figure GDA0003483993470000155
小于Td，且lb所在直线与la所在直线夹角小于Tθ，将la与lb合并为la(sb，ea)，转4.3.3；
4.3.2.4，若lb起点sb到la的距离
Figure GDA0003483993470000156
小于Td，且终点eb到la的距离
Figure GDA0003483993470000157
小于Td，且lb所在直线与la所在直线夹角小于Tθ，将la与lb合并为la(sa，ea)，转4.3.3；
4.3.2.5，若lb起点sb到la的距离
Figure GDA0003483993470000158
大于Td，且终点eb到la的距离
Figure GDA0003483993470000159
大于Td，或lb所在直线与la所在直线夹角大于Tθ，则la与lb无法合并，转4.3.4；
4.3.3，此时la与lb合并成功，将合并后的la存入Lk+1，并将lb从Lk+1中删除，若a＜A，转 4.3.2，令a＝a+1；若a＝A，转4.3.5；
4.3.4，此时la与lb合并不成功，若b＜A，转4.3.2，令b＝b+1；若b＝A，且a＜A，则令a＝a+1，转4.3.2；若b＝A，且a＝A，转4.3.5；
4.3.5，得到整合后的新特征线段集合Lk+1；
4.4，视觉追踪模块根据特征点与特征线关系，对第k+1帧单目相机图像中的特征点进行增补，输出新的特征点集合Pk+1，具体步骤如下：
4.4.1，根据2.2中的设置，单目相机图像帧中特征点数最大值MAXpoint＝200，设置单目图像帧中特征线段上特征点最大值MAXpoint_inline，MAXpoint_inline＝150，并统计第k+1帧单目图像中的特征点数npoint，和第k+1帧单目图像中特征线段上的特征点数npoint_inline；
4.4.2，基于第k+1帧单目图像制作相对应的掩模图像Mat1，Mat1与第k+1帧单目图像尺寸相同，Mat1所有像素为黑，只有特征线段(Lk+1中所有线段)附近像素(距离特征线段10像素以内区域)为白色，同时特征线段上的特征点(Pk+1中特征点)周围像素(以特征点为圆心，半径为20像素区域)为黑色；
4.4.3，计算第一次特征点增补数目N1：
4.4.3.1，若MAXpoint_inline＞[MAXpoint-(npoint-npoint_inline)]，则N1＝MAXpoint- npoint，转4.4.4；
4.4.3.2，若MAXpoint_inline＜[MAXpoint-(npoint-npoint_inline)]，且MAXpoint_inline＞npoint_inline，则N1＝MAXpoint_inline-npoint_inline，转4.4.4；
4.4.4，在第k+1帧单目相机图像中，Mat1中白色像素所对应的区域内，使用FAST算法添加N1个特征点，且这些新添加的特征点之间间隔大于20个像素，将新加入的N1个特征点信息添加至特征点集合Pk+1中以更新Pk+1，并更新特征点数npoint＝npoint+N1；
4.4.5，基于第k+1帧单目相机图像制作相对应的掩模图像Mat2，Mat2与第k+1帧单目图像尺寸相同，Mat2所有像素为白，只有特征点(Pk+1中所有特征点)周围像素(以特征点为圆心，半径为20像素区域)为黑色；
4.4.6，计算第二次特征点增补数目N2，若MAXpoint＞npoint，则N2＝MAXpoint-npoint，转4.4.7；
4.4.7，在第k+1帧单目相机图像中，Mat2中白色像素所对应的区域内，使用FAST算法添加N2个特征点，且这些新添加的特征点之间间隔大于20个像素，并将新加入的N2个特征点信息添加至特征点集合Pk+1中以更新Pk+1；并更新特征点数npoint＝npoint+N2；
4.5，视觉追踪模块根据特征点与特征线关系，对第k+1帧单目相机图像中的特征线段和第k帧单目相机图像中的特征线段进行匹配，以实现单目相机图像帧间特征线的追踪，具体步骤如下：
4.5.1，对Lk+1中的特征线段进行筛选，记录Lk+1中特征线与Pk+1中特征点之间的关系(即每条特征线段上有哪些特征点)，将Lk+1中特征线段上特征点数小于2的特征线段从Lk+1中剔除，对Lk+1中的特征线段由长至短重新排序，更新Lk+1；
4.5.2，对重新排序后的Lk+1中的线段进行编号，令线段lc∈Lk+1，c＝1，2，...，C，C表示 Lk+1中线段的总数，lc可表示为lc(sc，ec)，sc为线段lc的起点，ec为线段lc的终点，
Figure GDA0003483993470000161
表示sc在单目图像坐标系x轴坐标，
Figure GDA0003483993470000162
表示sc在单目图像坐标系y轴坐标；
4.5.3，选取Lk+1中的特征线段lc，在第k帧单目相机图像特征线段集合Lk中依次寻找与lc相匹配的特征线段，即属于不同图像帧的同一条特征线段：
4.5.3.1，选取Lk中的特征线段ld，ld∈Lk，d＝1，2，...，D，D表示Lk中线段的总数，若
Figure GDA0003483993470000171
交换ld起点与终点，即令ld(sd，ed)＝(ed，sd)，转4.5.3.2；否则，直接转4.5.3.2；
4.5.3.2，若lc与ld拥有相同的特征点数超过2个，lc与ld线段长度差值小于阈值Tl，Tl大小为30个像素，且lc起点sc到ld起点sd距离
Figure GDA0003483993470000172
小于阈值Tpp，Tpp大小为60个像素，且lc终点 ec到ld终点ed距离
Figure GDA0003483993470000173
小于阈值Tpp，则lc与ld匹配成功；转4.5.4；若上述条件有一项不满足，则lc与ld匹配不成功，转4.5.5；
4.5.4，此时lc与ld匹配成功，表示第k帧单目图像中的特征线段ld在第k+1帧单目图像中追踪成功，记录匹配与追踪信息，包括lc与ld在Lk+1与Lk中的序号，若c＜C，令c＝c+1，转4.5.3；若c＝C，转4.5.6；
4.5.5，此时lc与ld匹配不成功，若d＜D，令d＝d+1，转4.5.3.1；若d＝D且c＜C，令 c＝c+1，转4.5.3；若d＝D且c＝C，转4.5.6；
4.5.6，得到特征线匹配后第k+1帧单目相机图像与第k帧单目相机图像中特征线段的追踪匹配信息，即第k+1帧单目相机图像中特征线段与第k帧单目相机图像中特征线段之间的对应关系；
4.6，视觉追踪模块将第k+1帧单目相机图像中的特征点信息、特征线信息传送至特征重建模块，将第k+1帧单目相机图像与第k帧单目相机图像中的特征点、特征线追踪匹配信息传送至特征重建模块，将第k+1帧单目相机图像中的特征点信息传送至闭环检测与重定位模块。
第五步，IMU预积分模块对从数据获取模块得到的[tk-1，tk]时间区间内的IMU数据，采用预积分算法进行预积分处理。IMU预积分模块将预积分处理后的[tk-1，tk]时间区间内的 IMU预积分值发送至非线性优化模块。
第六步，特征重建模块从视觉特征追踪模块获取第k+1帧单目相机图像中的特征点信息、特征线信息，以及第k帧单目相机图像与第k+1帧单目相机图像中的特征点、特征线追踪匹配信息，根据以上信息分别构建第k+1帧特征点管理和特征线管理数据结构实例，对特征进行三维重建，更新特征点管理和特征线管理数据结构，将更新过的特征点管理和特征线管理数据结构发送给非线性优化模块。方法如下：
6.1，特征重建模块根据第k+1帧单目相机图像中的特征点信息和第k帧单目相机图像与第k+1帧单目相机图像中的特征点追踪匹配信息，构建第k+1帧特征点管理数据结构实例，该实例包括第k+1帧单目相机图像中每个特征点的图像坐标系坐标、每个特征点的相机坐标系坐标；
6.2，特征重建模块根据第k+1帧单目相机图像中的特征线信息和第k帧单目相机图像与第k+1帧单目相机图像中的特征线追踪匹配信息，构建第k+1帧特征线管理数据结构实例，该实例包括第k+1帧单目相机图像中每条特征线段起始点的图像坐标系坐标、每条特征线段起始点的相机坐标系坐标、每条特征线段上的特征点图像坐标系坐标、每条特征线段上所有特征点相机坐标系坐标集合；
6.3，若k＜4，特征检测模块将此时的第k+1帧特征点管理数据结构和特征线管理数据结构发送给非线性优化模块，转第七步；
6.4，若k≥4，特征检测模块对点特征和线特征进行三维重建，方法是：
6.4.1，特征重建模块对于第k+1帧单目相机图像中的特征点，筛选出至少在第k帧中出现过的特征点，联立这些特征点在第k+1、k、k-1、k-2、k-3帧中的相机坐标系坐标信息，利用奇异值分解算法，对这些特征点在相机坐标系中的深度进行求解，并在第k+1帧单目相机图像特征点管理数据结构实例中添加这些特征点的相机坐标系深度；
6.4.2，特征重建模块对于第k+1帧单目相机图像中的特征线，筛选出至少在第k帧中出现过的特征线lf，lf∈LF，LF表示第k+1帧单目相机图像中所有被筛选出来的特征线的集合，基于lf上特征点信息，进行三维重建，具体步骤如下：
6.4.2.1，特征重建模块将第k+1、k、k-1、k-2、k-3共计五帧图像帧中，所有lf上的特征点三维坐标转换至第k+1帧图像帧相机坐标系中，建立特征点集合
Figure GDA0003483993470000181
储存所有被转换至第k+1帧图像帧相机坐标系中特征点的三维坐标，此时
Figure GDA0003483993470000182
中特征点数量为
Figure GDA0003483993470000183
6.4.2.2，特征重建模块对
Figure GDA0003483993470000184
中的所有特征点进行基于RANSAC算法的直线拟合外点剔除，即将不属于同一三维直线的特征点从
Figure GDA0003483993470000185
中剔除，此时
Figure GDA0003483993470000186
和特征点数量为
Figure GDA0003483993470000187
6.4.2.3，若
Figure GDA0003483993470000188
特征重建模块使用最小二乘直线拟合方法对
Figure GDA0003483993470000189
中的特征点进行三维拟合，将拟合直线表示方式转化为普吕克坐标表示方式，在第k+1帧特征线管理数据结构实例中添加特征线段lf的普吕克坐标表示；
6.4.2.4，若
Figure GDA00034839934700001810
特征重建模块将特征线段lf所有相关信息从第k+1帧特征线管理数据结构实例中删除；
6.4.3，特征重建模块将更新过的第k+1帧特征点管理数据结构和特征线管理数据结构发送给非线性优化模块，转第七步。
第七步，闭环检测与重定位模块从视觉特征追踪模块获取第k+1帧单目相机图像中的特征点信息，并基于DBoW2算法对第k+1帧单目相机图像中的特征点信息进行转化、存储与检索，并利用VINS-Mono中的闭环检测算法，对第k+1帧单目相机图像帧与之前的单目图像帧进行闭环检测匹配，若匹配成功，则表明发生了闭环，即机器人运行到了之前运行过的地点，闭环检测与重定位模块将闭环检测结果发送给非线性优化模块，转第八步；若匹配不成功，则表明没有发生闭环，闭环检测与重定位模块不与非线性优化模块进行交互，直接转第八步。
第八步，非线性优化模块从特征重建模块获取第k+1帧特征点管理和特征线管理数据结构，从IMU预积分模块获取[tk-1，tk]时间区间内的IMU预积分数据，从闭环检测与重定位模块获取闭环检测结果，若k＜9，非线性优化模块不对系统状态进行求解，直接转第九步；若 k≥9，非线性优化模块基于特征点数据、特征线数据、IMU预积分数据、系统状态估计历史数据、闭环检测数据构建非线性优化损失函数并进行求解，获取机器人状态。具体步骤如下：
8.1，非线性优化模块构造滑动窗口，窗口内包括{tk-9，tk-8，...，tk-1，tk}共十个时刻内机器人系统相关信息，机器人系统相关信息包括从特征重建模块接收到的第k-8，k-7，...，k+1 共十帧图像帧特征点管理和特征线管理数据结构、从IMU预积分模块中接收到[tk-10，tk-9]，[tk-9，tk-8]，...，[tk-1，tk]共十个时间区间内的IMU预积分信息、从闭环检测与重定位模块中接收到的闭环检测信息、除tk时刻外每个时刻机器人的位姿信息；
8.2，非线性优化模块将十个时刻中所有特征线段的普吕克坐标参数转化为线段所在三维直线的正交法表达参数；
8.3，非线性优化模块根据滑动窗口的信息，构造优化变量χ：
Figure GDA0003483993470000191
其中，xk-9，xk-8，...，xk表示滑动窗口内十个时刻机器人系统状态，λ0，λ1，...，λn表示滑动窗口内十个时刻中所有特征点在三维世界中的深度，
Figure GDA0003483993470000192
表示滑动窗口内十个时刻中所有特征线段的正交法表达参数，ti时刻机器人系统状态xi可以表达为：
Figure GDA0003483993470000193
其中，
Figure GDA0003483993470000194
代表ti时刻机器人在IMU坐标系相对世界坐标系的位移向量，
Figure GDA0003483993470000195
代表ti时刻机器人在IMU坐标系相对世界坐标系的速度向量，
Figure GDA0003483993470000196
代表ti时刻机器人在IMU坐标系相对世界坐标系的四元数，
Figure GDA0003483993470000197
代表ti时刻机器人在IMU坐标系中的IMU加速度计偏置，
Figure GDA0003483993470000198
代表 ti时刻机器人在IMU坐标系中的IMU陀螺仪偏置；
8.4，非线性优化模块根据8.3中构造的优化变量构造优化损失函数：
Figure GDA0003483993470000201
其中：
Figure GDA0003483993470000202
表示IMU预积分残差项，
Figure GDA0003483993470000203
表示IMU坐标系中从ti时刻到ti+1时刻IMU预积分值，
Figure GDA0003483993470000204
表示IMU预积分数据集合，
Figure GDA0003483993470000205
表示IMU从ti时刻到ti+1时刻IMU预积分残差，该残差的具体构造方式同VINS-Mono；
Figure GDA0003483993470000206
表示特征点残差项，
Figure GDA0003483993470000207
表示相机坐标系中ti时刻第j个特征点的重投影误差，j≤l，
Figure GDA0003483993470000208
表示滑动窗口内所有特征点的集合，
Figure GDA0003483993470000209
表示ti时刻第j个特征点残差，该残差的具体构造方式同VINS-Mono；
Figure GDA00034839934700002010
表示特征线残差项，
Figure GDA00034839934700002011
表示相机坐标系中ti时刻第m个特征点的重投影误差，m≤n，
Figure GDA00034839934700002012
表示滑动窗口内所有特征线的集合，
Figure GDA00034839934700002013
表示ti时刻第 m个特征点残差，该残差的具体构造方式同PL-VIO；
Figure GDA00034839934700002014
表示滑动窗口外系统状态估计历史数据残差项，该残差项的具体构造方式同VINS-Mono；
Figure GDA00034839934700002015
表示闭环检测重定位残差项，若闭环检测发生在ti时刻与tv时刻之间，即tv时刻单目图像帧和ti时刻单目图像帧被判定为发生闭环检测，tv时刻可为ti时刻之前任一时刻，v<i，
Figure GDA00034839934700002016
表示相机坐标系中ti时刻第j个特征点在tv时刻单目图像帧上的重投影误差，
Figure GDA00034839934700002017
表示世界坐标系中tv时刻机器人的旋转四元数，
Figure GDA00034839934700002018
表示世界坐标系中tv时刻机器人的位移向量，下标w表示该数据为世界坐标系中的数据，
Figure GDA00034839934700002019
表示闭环检测模块中存储的图像信息集合，
Figure GDA00034839934700002020
表示滑动窗口内所有特征点的集合，
Figure GDA00034839934700002021
表示ti时刻闭环检测残差，该残差的具体构造方式同VINS-Mono；
8.5，非线性优化模块采用最小二乘优化方法对优化损失函数求解，获得tk时刻机器人位姿估计值、速度估计值、窗口内所有特征点深度估计值、窗口内所有特征线普吕克坐标参数估计值，转第九步。
第九步，令k＝k+1，若还能从视觉-惯性里程计数据集“EuRoC MAV Datasets”数据集中获取双目图像帧中左目相机图像帧和IMU数据，返回第三步，若不能从视觉-惯性里程计数据集“EuRoC MAV Datasets”数据集中获取双目图像帧中左目相机图像帧和IMU数据，转第十步。
第十步，结束。
图3为本发明与现有基于特征点(和特征线)的惯性-视觉里程计方法平均状态估计精度误差对比实验结果；该实验结果为在“EuRoC MAV Datasets”数据集上采用本发明和背景技术所述基于特征点或同时基于特征点和特征线的惯性-视觉里程计方法平均状态估计方法进行测试得到的结果。实验环境为一台配置为i73.2GHzCPU、32GRAM的台式机。该实验的评价指标为状态估计轨迹结果与真值的均方根误差，单位为米。图中左侧第一列为目前效果较好的视觉-惯性里程计方法名，上方第一行为数据集测试数据序列名。图中每行实验数据分别为本发明、其他视觉-惯性里程计在相同的测试数据序列、同样的实验环境下测试的结果。实验结果显示，与其它方法相比，本发明在大部分测试数据序列上都具有更低的平均跟踪误差。图中“本发明w/o loop”表示本发明去除闭环检测与重定位模块，“VINS-Mono_loop”表示 VINS-Mono带闭环检测与重定位模块，黑色粗体为该数据序列中误差最小的结果，蓝色粗体为该数据序列中误差次小的结果，“×”表示无数据。
图4为本发明与现有基于特征点和特征线的惯性-视觉里程计方法特征追踪平均时间对比实验结果。该实验与图3对应的实验的测试数据集和实验环境相同。该实验的评价指标为视觉特征追踪模块中每个部分的平均耗时和视觉特征追踪模块的总耗时，单位为秒。图中左侧第一列为不同方法名。实验结果显示，与传统的使用LSD和LBD跟踪特征线方法PL-VIO相比，本发明在对视觉特征追踪的时候总耗时更少，尤其是从提取直线到匹配直线的过程耗时更少。

Claims (8)

1.一种基于点线特征的视觉-惯性里程计状态估计方法，其特征在于包括以下步骤：
第一步，构建基于特征点和特征线的视觉-惯性里程计系统；基于特征点和特征线的视觉-惯性里程计系统由数据获取模块、视觉特征追踪模块、IMU预积分模块、特征重建模块、非线性优化模块、闭环检测与重定位模块组成；
数据获取模块与视觉特征追踪模块、IMU预积分模块相连；数据获取模块从视觉-惯性里程计数据集下载数据，获取双目图像帧中左目相机图像帧和IMU数据，将双目图像帧中左目相机图像帧作为单目相机图像帧，将单目相机图像帧发送给视觉特征追踪模块，将IMU数据发送给IMU预积分模块；
视觉特征追踪模块与数据获取模块、特征重建模块、闭环检测与重定位模块相连；视觉特征追踪模块从数据获取模块接收单目相机图像帧，从单目相机图像帧中提取特征点和特征线信息，对连续单目相机图像帧中的特征进行匹配和追踪，将特征点和特征线信息以及特征追踪信息发送给特征重建模块，将特征点信息发送给闭环检测与重定位模块；
IMU预积分模块与数据获取模块、非线性优化模块相连；IMU预积分模块从数据获取模块接收IMU数据，利用IMU预积分算法对连续两帧单目相机图像帧中的IMU数据进行预积分处理，将预积分处理后的IMU预积分数据发送给非线性优化模块；
特征重建模块与视觉特征追踪模块、非线性优化模块相连；特征重建模块从视觉特征追踪模块获取特征点和特征线信息以及特征追踪信息，分别构建特征点管理和特征线管理数据结构，对第一次出现的特征进行三维重建，更新特征点管理和特征线管理数据结构，将更新后的特征点管理和特征线管理数据结构发送给非线性优化模块；
闭环检测与重定位模块与视觉特征追踪模块、非线性优化模块相连；闭环检测与重定位模块从视觉特征追踪模块获取每一帧单目相机图像帧中的特征点信息并进行储存，通过特征点信息匹配，对每一帧单目相机图像帧中的特征点信息进行闭环检测，将闭环检测结果发送给非线性优化模块；
非线性优化模块与特征重建模块、IMU预积分模块、闭环检测与重定位模块相连；非线性优化模块从特征重建模块获取最新的特征点管理数据结构和特征线管理数据结构，从IMU预积分模块获取最新两帧图像间的IMU预积分数据，从闭环检测与重定位模块获取闭环检测结果，基于特征点管理数据结构和特征线管理数据结构、IMU预积分数据、系统状态估计历史数据、闭环检测结果构建非线性优化损失函数并进行求解，获取机器人状态；
第二步，令变量k＝1，数据获取模块从视觉-惯性里程计数据集中获取双目图像帧中左目相机图像帧和IMU数据，将双目图像帧中左目相机图像帧作为单目相机图像帧；令数据获取模块接收到第一帧左目相机图像帧时的时刻为t0时刻；
2.1，t0时刻数据获取模块将第一帧左目相机图像帧发送给视觉特征追踪模块；
2.2，视觉特征追踪模块从数据获取模块得到第一帧左目相机图像帧作为第一帧单目相机图像帧，视觉特征追踪模块利用FAST算法提取第一帧单目相机图像帧中的MAXpoint个特征点，利用EDLines算法提取第一帧单目相机图像帧中MAXline条特征线，根据特征点坐标与特征线坐标关系获取第一帧单目相机图像帧中特征线上特征点信息，并将特征点和特征线信息发送至特征重建模块，将特征点信息发送至闭环检测与重定位模块，MAXpoint、MAXline均为正整数；
2.3，特征重建模块从视觉特征追踪模块得到第一帧单目相机图像帧中的特征点信息与特征线信息，特征重建模块构建第一帧特征点管理数据结构实例，该实例包括第一帧单目相机图像中每个特征点的图像坐标系坐标、每个特征点的相机坐标系坐标，特征重建模块构建第一帧特征线管理数据结构实例，该实例包括第一帧单目相机图像中每条特征线段起始点的图像坐标系坐标、每条特征线段起始点的相机坐标系坐标、每条特征线段上的特征点图像坐标系坐标、每条特征线段上所有特征点相机坐标系坐标集合；
2.4，闭环检测与重定位模块从视觉特征追踪模块得到第一帧单目相机图像帧中的特征点信息，采用DBoW2算法对第一帧单目相机图像中的特征点信息进行转化、存储；
第三步，数据获取模块从视觉-惯性里程计数据集中获取双目图像帧中左目相机图像帧和IMU数据，将双目图像帧中左目相机图像帧作为单目相机图像帧；数据获取模块在tk时刻从视觉-惯性里程计数据集中接收到第k+1帧单目相机图像帧；数据获取模块将第k+1帧单目相机图像帧发送给视觉特征追踪模块，将[tk-1,tk]时间区间内从视觉-惯性里程计数据集中接收到IMU数据发送给IMU预积分模块；
第四步，视觉特征追踪模块对第k+1帧单目相机图像进行特征提取与特征追踪，方法是：
4.1，视觉特征追踪模块利用KLT稀疏光流算法，在第k+1帧单目相机图像中跟踪第k帧单目相机图像中的特征点，得到第k+1帧单目相机图像中的特征点集合Pk+1，Pk+1中的元素为特征点，每个特征点由其在第k+1帧单目相机图像中的坐标表示；
4.2，视觉特征追踪模块利用EDLines算法提取第k+1帧单目相机图像中的MAXline条特征线段，得到第k+1帧单目相机图像中的特征线段集合Lk+1，Lk+1中的元素为特征线段la，a＝1,2,…,A，A表示Lk+1中特征线段的总数，la表示为la(sa，ea)，sa为特征线段la的起点，ea为特征线段la的终点，
Figure FDA0003483993460000021
表示sa在单目图像坐标系的x轴坐标，
Figure FDA0003483993460000022
表示sa在单目图像坐标系的y轴坐标；
4.3，视觉特征追踪模块对Lk+1中的特征线段进行整合，删除误检测线段，合并属于同一条直线的多条线段，得到整合后的新特征线段集合Lk+1；
4.4，视觉追踪模块根据特征点与特征线关系，对第k+1帧单目相机图像中的特征点进行增补，得到新的特征点集合Pk+1；
4.5，视觉追踪模块根据特征点与特征线关系，对第k+1帧单目相机图像中的特征线段和第k帧单目相机图像中的特征线段进行匹配，以实现单目相机图像帧间特征线的追踪，方法是：
4.5.1，对Lk+1中的特征线段进行筛选，记录Lk+1中特征线与Pk+1中特征点之间的关系即每条特征线段上有哪些特征点，将Lk+1中特征线段上特征点数小于2的特征线段从Lk+1中剔除，对Lk+1中的特征线段由长至短重新排序，更新Lk+1；
4.5.2，对重新排序后的Lk+1中的线段进行编号，令线段lc∈Lk+1，c＝1,2,…,C，C表示Lk+1中线段的总数，lc可表示为lc(sc，ec)，sc为线段lc的起点，ec为线段lc的终点，
Figure FDA0003483993460000031
表示sc在单目图像坐标系x轴坐标，
Figure FDA0003483993460000032
表示sc在单目图像坐标系y轴坐标；
4.5.3，选取Lk+1中的特征线段lc，在第k帧单目相机图像特征线段集合Lk中依次寻找与lc相匹配的特征线段，即属于不同图像帧的同一条特征线段：
4.5.3.1，选取Lk中的特征线段ld，ld∈Lk，d＝1,2,…,D，D表示Lk中线段的总数，若
Figure FDA0003483993460000033
交换ld起点与终点，即令ld(sd，ed)＝(ed，sd)，转4.5.3.2；否则，直接转4.5.3.2；
4.5.3.2，若lc与ld拥有相同的特征点数超过2个，lc与ld线段长度差值小于阈值Tl，Tl大小为30个像素，且lc起点sc到ld起点sd距离
Figure FDA0003483993460000034
小于阈值Tpp，Tpp大小为60个像素，且lc终点ec到ld终点ed距离
Figure FDA0003483993460000035
小于阈值Tpp，则lc与ld匹配成功；转4.5.4；若上述条件有一项不满足，则lc与ld匹配不成功，转4.5.5；
4.5.4，此时lc与ld匹配成功，表示第k帧单目图像中的特征线段ld在第k+1帧单目图像中追踪成功，记录匹配与追踪信息，包括lc与ld在Lk+1与Lk中的序号，若c<C，令c＝c+1，转4.5.3；若c＝C，转4.5.6；
4.5.5，此时lc与ld匹配不成功，若d<D，令d＝d+1，转4.5.3.1；若d＝D且c<C，令c＝c+1，转4.5.3；若d＝D且c＝C，转4.5.6；
4.5.6，得到特征线匹配后第k+1帧单目相机图像与第k帧单目相机图像中特征线段的追踪匹配信息，即第k+1帧单目相机图像中特征线段与第k帧单目相机图像中特征线段之间的对应关系；
4.6，视觉追踪模块将第k+1帧单目相机图像中的特征点信息、特征线信息传送至特征重建模块，将第k+1帧单目相机图像与第k帧单目相机图像中的特征点、特征线追踪匹配信息传送至特征重建模块，将第k+1帧单目相机图像中的特征点信息传送至闭环检测与重定位模块；
第五步，IMU预积分模块对从数据获取模块得到的[tk-1,tk]时间区间内的IMU数据，采用预积分算法进行预积分处理，将预积分处理后的[tk-1,tk]时间区间内的IMU预积分值发送至非线性优化模块；
第六步，特征重建模块从视觉特征追踪模块获取第k+1帧单目相机图像中的特征点信息、特征线信息，以及第k帧单目相机图像与第k+1帧单目相机图像中的特征点、特征线追踪匹配信息，根据以上信息分别构建第k+1帧特征点管理和特征线管理数据结构实例，对特征进行三维重建，更新特征点管理和特征线管理数据结构，将更新过的特征点管理和特征线管理数据结构发送给非线性优化模块，方法如下：
6.1，特征重建模块根据第k+1帧单目相机图像中的特征点信息和第k帧单目相机图像与第k+1帧单目相机图像中的特征点追踪匹配信息，构建第k+1帧特征点管理数据结构实例，该实例包括第k+1帧单目相机图像中每个特征点的图像坐标系坐标、每个特征点的相机坐标系坐标；
6.2，特征重建模块根据第k+1帧单目相机图像中的特征线信息和第k帧单目相机图像与第k+1帧单目相机图像中的特征线追踪匹配信息，构建第k+1帧特征线管理数据结构实例，该实例包括第k+1帧单目相机图像中每条特征线段起始点的图像坐标系坐标、每条特征线段起始点的相机坐标系坐标、每条特征线段上的特征点图像坐标系坐标、每条特征线段上所有特征点相机坐标系坐标集合；
6.3，若k<4，特征检测模块将此时的第k+1帧特征点管理数据结构和特征线管理数据结构发送给非线性优化模块，转第七步；
6.4，若k≥4，特征检测模块对点特征和线特征进行三维重建，方法是：
6.4.1，特征重建模块对于第k+1帧单目相机图像中的特征点，筛选出至少在第k帧中出现过的特征点，联立这些特征点在第k+1、k、k-1、k-2、k-3帧中的相机坐标系坐标信息，利用奇异值分解算法，对这些特征点在相机坐标系中的深度进行求解，并在第k+1帧单目相机图像特征点管理数据结构实例中添加这些特征点的相机坐标系深度；
6.4.2，特征重建模块对于第k+1帧单目相机图像中的特征线，筛选出至少在第k帧中出现过的特征线lf，lf∈LF，LF表示第k+1帧单目相机图像中所有被筛选出来的特征线的集合，基于lf上特征点信息，进行三维重建；
6.4.3，特征重建模块将更新过的第k+1帧特征点管理数据结构和特征线管理数据结构发送给非线性优化模块，转第七步；
第七步，闭环检测与重定位模块从视觉特征追踪模块获取第k+1帧单目相机图像中的特征点信息，并基于DBoW2算法对第k+1帧单目相机图像中的特征点信息进行转化、存储与检索，并利用VINS-Mono中的闭环检测算法，对第k+1帧单目相机图像帧与之前的单目图像帧进行闭环检测匹配，若匹配成功，闭环检测与重定位模块将闭环检测结果发送给非线性优化模块，转第八步；若匹配不成功，直接转第八步；
第八步，非线性优化模块从特征重建模块获取第k+1帧特征点管理和特征线管理数据结构，从IMU预积分模块获取[tk-1,tk]时间区间内的IMU预积分数据，从闭环检测与重定位模块获取闭环检测结果，若k<9，直接转第九步；若k≥9，非线性优化模块基于特征点数据、特征线数据、IMU预积分数据、系统状态估计历史数据、闭环检测数据构建非线性优化损失函数并进行求解，获取机器人状态，方法是：
8.1，非线性优化模块构造滑动窗口，窗口内包括{tk-9,tk-8,…,tk-1,tk}共十个时刻内机器人系统相关信息，机器人系统相关信息包括从特征重建模块接收到的第k-8,k-7,…,k+1共十帧图像帧特征点管理和特征线管理数据结构、从IMU预积分模块中接收到[tk-10,tk-9],[tk-9,tk-8],…,[tk-1,tk]共十个时间区间内的IMU预积分信息、从闭环检测与重定位模块中接收到的闭环检测信息、除tk时刻外每个时刻机器人的位姿信息；
8.2，非线性优化模块将十个时刻中所有特征线段的普吕克坐标参数转化为线段所在三维直线的正交法表达参数；
8.3，非线性优化模块根据滑动窗口的信息，构造优化变量χ：
Figure FDA0003483993460000051
其中，xk-9,xk-8,…,xk表示滑动窗口内十个时刻机器人系统状态，λ0,λ1,…,λn表示滑动窗口内十个时刻中所有特征点在三维世界中的深度，
Figure FDA0003483993460000052
表示滑动窗口内十个时刻中所有特征线段的正交法表达参数，ti时刻机器人系统状态xi可以表达为：
Figure FDA0003483993460000053
其中，
Figure FDA0003483993460000054
代表ti时刻机器人在IMU坐标系相对世界坐标系的位移向量，
Figure FDA0003483993460000055
代表ti时刻机器人在IMU坐标系相对世界坐标系的速度向量，
Figure FDA0003483993460000056
代表ti时刻机器人在IMU坐标系相对世界坐标系的四元数，
Figure FDA0003483993460000061
代表ti时刻机器人在IMU坐标系中的IMU加速度计偏置，
Figure FDA0003483993460000062
代表ti时刻机器人在IMU坐标系中的IMU陀螺仪偏置；
8.4，非线性优化模块根据8.3中构造的优化变量构造优化损失函数：
Figure FDA0003483993460000063
其中：
Figure FDA0003483993460000064
表示IMU预积分残差项，
Figure FDA0003483993460000065
表示IMU坐标系中从ti时刻到ti+1时刻IMU预积分值，
Figure FDA0003483993460000066
表示IMU预积分数据集合，
Figure FDA0003483993460000067
表示IMU从ti时刻到ti+1时刻IMU预积分残差，该残差的具体构造方式同VINS-Mono；
Figure FDA0003483993460000068
表示特征点残差项，
Figure FDA0003483993460000069
表示相机坐标系中ti时刻第j个特征点的重投影误差，j≤l，
Figure FDA00034839934600000610
表示滑动窗口内所有特征点的集合，
Figure FDA00034839934600000611
表示ti时刻第j个特征点残差，该残差的具体构造方式同VINS-Mono；
Figure FDA00034839934600000612
表示特征线残差项，
Figure FDA00034839934600000613
表示相机坐标系中ti时刻第m个特征点的重投影误差，m≤n，
Figure FDA00034839934600000614
表示滑动窗口内所有特征线的集合，
Figure FDA00034839934600000615
表示ti时刻第m个特征点残差，该残差的具体构造方式同PL-VIO；
Figure FDA00034839934600000616
表示滑动窗口外系统状态估计历史数据残差项，该残差项的具体构造方式同VINS-Mono；
Figure FDA00034839934600000617
表示闭环检测重定位残差项，若闭环检测发生在ti时刻与tv时刻之间，即tv时刻单目图像帧和ti时刻单目图像帧被判定为发生闭环检测，tv时刻可为ti时刻之前任一时刻，v<i，
Figure FDA00034839934600000618
表示相机坐标系中ti时刻第j个特征点在tv时刻单目图像帧上的重投影误差，
Figure FDA00034839934600000619
表示世界坐标系中tv时刻机器人的旋转四元数，
Figure FDA00034839934600000620
表示世界坐标系中tv时刻机器人的位移向量，下标w表示该数据为世界坐标系中的数据，
Figure FDA00034839934600000621
表示闭环检测模块中存储的图像信息集合，
Figure FDA00034839934600000622
表示滑动窗口内所有特征点的集合，
Figure FDA00034839934600000623
表示ti时刻闭环检测残差，该残差的具体构造方式同VINS-Mono；
8.5，非线性优化模块采用最小二乘优化方法对优化损失函数求解，获得tk时刻机器人位姿估计值、速度估计值、窗口内所有特征点深度估计值、窗口内所有特征线普吕克坐标参数估计值，转第九步；
第九步，令k＝k+1，若还能从视觉-惯性里程计数据集中获取双目图像帧中左目相机图像帧和IMU数据，返回第三步，若不能从视觉-惯性里程计数据集中获取双目图像帧中左目相机图像帧和IMU数据，转第十步；
第十步，结束。
2.如权利要求1所述的一种基于点线特征的视觉-惯性里程计状态估计方法，其特征在于所述视觉-惯性里程计数据集指苏黎世联邦理工学院的“EuRoC MAV Datasets”数据集。
3.如权利要求1所述的一种基于点线特征的视觉-惯性里程计状态估计方法，其特征在于2.2步所述MAXpoint为200，所述MAXline为200。
4.如权利要求1所述的一种基于点线特征的视觉-惯性里程计状态估计方法，其特征在于4.3步所述视觉特征追踪模块对Lk+1中的特征线段进行整合，删除误检测线段，合并属于同一条直线的多条线段，得到整合后的新特征线段集合Lk+1的方法是：
4.3.1，对Lk+1中的特征线段由长至短进行排序，并对特征线段进行编号；
4.3.2，选取Lk+1中的特征线段la，对la与lb进行整合匹配，lb∈la+1,la+2,…,lA：
4.3.2.1，若
Figure FDA0003483993460000071
交换lb起点与终点，即令lb(sb，eb)＝(eb，sb)；
4.3.2.2，若lb起点sb到la的距离
Figure FDA0003483993460000072
小于距离阈值Td，且终点eb到la的距离
Figure FDA0003483993460000073
大于阈值Td，且lb所在直线与la所在直线夹角小于角度阈值Tθ，将la与lb合并为la(sa，eb)，转4.3.3；
4.3.2.3，若lb起点sb到la的距离
Figure FDA0003483993460000074
大于Td，且终点eb到la的距离
Figure FDA0003483993460000075
小于Td，且lb所在直线与la所在直线夹角小于Tθ，将la与lb合并为la(sb，ea)，转4.3.3；
4.3.2.4，若lb起点sb到la的距离
Figure FDA0003483993460000076
小于Td，且终点eb到la的距离
Figure FDA0003483993460000077
小于Td，且lb所在直线与la所在直线夹角小于Tθ，将la与lb合并为la(sa，ea)，转4.3.3；
4.3.2.5，若lb起点sb到la的距离
Figure FDA0003483993460000078
大于Td，且终点eb到la的距离
Figure FDA0003483993460000079
大于Td，或lb所在直线与la所在直线夹角大于Tθ，则la与lb无法合并，转4.3.4；
4.3.3，此时la与lb合并成功，将合并后的la存入Lk+1，并将lb从Lk+1中删除，若a<A，转4.3.2，令a＝a+1；若a＝A，转4.3.5；
4.3.4，此时la与lb合并不成功，若b<A，转4.3.2，令b＝b+1；若b＝A，且a<A，则令a＝a+1，转4.3.2；若b＝A，且a＝A，转4.3.5；
4.3.5，得到整合后的新特征线段集合Lk+1。
5.如权利要求4所述的一种基于点线特征的视觉-惯性里程计状态估计方法，其特征在于所述距离阈值Td为1个像素，所述角度阈值Tθ为5度。
6.如权利要求1所述的一种基于点线特征的视觉-惯性里程计状态估计方法，其特征在于4.4步所述视觉追踪模块对第k+1帧单目相机图像中的特征点进行增补，得到新的特征点集合Pk+1的方法是：
4.4.1，设置单目图像帧中特征线段上特征点最大值MAXpoint_inline，并统计第k+1帧单目图像中的特征点数npoint，和第k+1帧单目图像中特征线段上的特征点数npoint_inline；MAXpoint_inline为正整数；
4.4.2，基于第k+1帧单目图像制作相对应的掩模图像Mat1，Mat1与第k+1帧单目图像尺寸相同，Mat1所有像素为黑，只有特征线段即Lk+1中所有线段附近像素为白色，同时特征线段上的特征点即Pk+1中特征点周围像素为黑色；
4.4.3，计算第一次特征点增补数目N1:
4.4.3.1，若MAXpoint_inline>[MAXpoint-(npoint-npoint_inline)]，则N1＝MAXpoint-npoint，转4.4.4；
4.4.3.2，若MAXpoint_inline<[MAXpoint-(npoint-npoint_inline)]，且MAXpoint_inline>npoint_inline，则N1＝MAXpoint_inline-npoint_inline，转4.4.4；
4.4.4，在第k+1帧单目相机图像中，Mat1中白色像素所对应的区域内，使用FAST算法添加N1个特征点，且这些新添加的特征点之间间隔大于20个像素，将新加入的N1个特征点信息添加至特征点集合Pk+1中以更新Pk+1，并更新特征点数npoint＝npoint+N1；
4.4.5，基于第k+1帧单目相机图像制作相对应的掩模图像Mat2，Mat2与第k+1帧单目图像尺寸相同，Mat2所有像素为白，只有Pk+1中所有特征点周围像素为黑色；
4.4.6，计算第二次特征点增补数目N2，若MAXpoint>npoint，则N2＝MAXpoint-npoint，转4.4.7；
4.4.7，在第k+1帧单目相机图像中，Mat2中白色像素所对应的区域内，使用FAST算法添加N2个特征点，且这些新添加的特征点之间间隔大于20个像素，并将新加入的N2个特征点信息添加至特征点集合Pk+1中以更新Pk+1；并更新特征点数npoint＝npoint+N2。
7.如权利要求6所述的一种基于点线特征的视觉-惯性里程计状态估计方法，其特征在于所述MAXpotnt_inline为150,所述Lk+1中所有线段附近像素指距离Lk+110像素以内区域，所述Pk+1中特征点周围像素指以特征点为圆心，半径为20像素区域。
8.如权利要求1所述的一种基于点线特征的视觉-惯性里程计状态估计方法，其特征在于6.4.2步所述特征重建模块对于第k+1帧单目相机图像中的特征线，筛选出至少在第k帧中出现过的特征线lf，基于lf上特征点信息进行三维重建的方法是：
6.4.2.1，特征重建模块将第k+1、k、k-1、k-2、k-3共计五帧图像帧中，所有lf上的特征点三维坐标转换至第k+1帧图像帧相机坐标系中，建立特征点集合
Figure FDA0003483993460000091
储存所有被转换至第k+1帧图像帧相机坐标系中特征点的三维坐标，此时
Figure FDA0003483993460000092
中特征点数量为
Figure FDA0003483993460000093
6.4.2.2，特征重建模块对
Figure FDA0003483993460000094
中的所有特征点进行基于RANSAC算法的直线拟合外点剔除，即将不属于同一三维直线的特征点从
Figure FDA0003483993460000095
中剔除，此时
Figure FDA0003483993460000096
中特征点数量为
Figure FDA0003483993460000097
6.4.2.3，若
Figure FDA0003483993460000098
特征重建模块使用最小二乘直线拟合方法对
Figure FDA0003483993460000099
中的特征点进行三维拟合，将拟合直线表示方式转化为普吕克坐标表示方式，在第k+1帧特征线管理数据结构实例中添加特征线段lf的普吕克坐标表示；
6.4.2.4，若
Figure FDA00034839934600000910
特征重建模块将特征线段lf所有相关信息从第k+1帧特征线管理数据结构实例中删除。
CN202011401030.5A 2020-12-02 2020-12-02 一种基于点线特征的视觉-惯性里程计状态估计方法 Active CN112683305B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011401030.5A CN112683305B (zh) 2020-12-02 2020-12-02 一种基于点线特征的视觉-惯性里程计状态估计方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011401030.5A CN112683305B (zh) 2020-12-02 2020-12-02 一种基于点线特征的视觉-惯性里程计状态估计方法

Publications (2)

Publication Number Publication Date
CN112683305A CN112683305A (zh) 2021-04-20
CN112683305B true CN112683305B (zh) 2022-03-04

Family

ID=75445863

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011401030.5A Active CN112683305B (zh) 2020-12-02 2020-12-02 一种基于点线特征的视觉-惯性里程计状态估计方法

Country Status (1)

Country Link
CN (1) CN112683305B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114485640A (zh) * 2022-01-20 2022-05-13 河北工业职业技术学院 基于点线特征的单目视觉惯性同步定位与建图方法及系统

Family Cites Families (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US9406171B2 (en) * 2014-08-25 2016-08-02 Daqri, Llc Distributed aperture visual inertia navigation
US10371530B2 (en) * 2017-01-04 2019-08-06 Qualcomm Incorporated Systems and methods for using a global positioning system velocity in visual-inertial odometry
CN108827315B (zh) * 2018-08-17 2021-03-30 华南理工大学 基于流形预积分的视觉惯性里程计位姿估计方法及装置
CN109211277B (zh) * 2018-10-31 2021-11-16 北京旷视科技有限公司 视觉惯性里程计的状态确定方法、装置和电子设备
CN111982103B (zh) * 2020-08-14 2021-09-14 北京航空航天大学 一种权值优化的点线综合视觉惯性里程计方法
CN111811506B (zh) * 2020-09-15 2020-12-01 中国人民解放军国防科技大学 视觉/惯性里程计组合导航方法、电子设备及存储介质

Also Published As

Publication number Publication date
CN112683305A (zh) 2021-04-20

Similar Documents

Publication Publication Date Title
CN111311666B (zh) 一种融合边缘特征和深度学习的单目视觉里程计方法
Qin et al. Vins-mono: A robust and versatile monocular visual-inertial state estimator
CN108242079B (zh) 一种基于多特征视觉里程计和图优化模型的vslam方法
Qin et al. Relocalization, global optimization and map merging for monocular visual-inertial SLAM
Civera et al. 1-point RANSAC for EKF-based structure from motion
CN106595659A (zh) 城市复杂环境下多无人机视觉slam的地图融合方法
Zheng et al. Trifo-VIO: Robust and efficient stereo visual inertial odometry using points and lines
CN109166149A (zh) 一种融合双目相机与imu的定位与三维线框结构重建方法与系统
CN108257154B (zh) 基于区域信息和cnn的极化sar图像变化检测方法
CN112634451A (zh) 一种融合多传感器的室外大场景三维建图方法
CN111780754A (zh) 基于稀疏直接法的视觉惯性里程计位姿估计方法
CN110726406A (zh) 一种改进的非线性优化单目惯导slam的方法
CN114001733B (zh) 一种基于地图的一致性高效视觉惯性定位算法
CN108615246A (zh) 提高视觉里程计系统鲁棒性和降低算法计算消耗的方法
Shamwell et al. Vision-aided absolute trajectory estimation using an unsupervised deep network with online error correction
Wen et al. Hybrid semi-dense 3D semantic-topological mapping from stereo visual-inertial odometry SLAM with loop closure detection
CN112802096A (zh) 实时定位和建图的实现装置和方法
Chen et al. A stereo visual-inertial SLAM approach for indoor mobile robots in unknown environments without occlusions
CN111998862A (zh) 一种基于bnn的稠密双目slam方法
CN112101160A (zh) 一种面向自动驾驶场景的双目语义slam方法
Huai et al. Stereo-inertial odometry using nonlinear optimization
CN112683305B (zh) 一种基于点线特征的视觉-惯性里程计状态估计方法
Jin et al. Beyond learning: Back to geometric essence of visual odometry via fusion-based paradigm
Xu et al. Direct visual-inertial odometry with semi-dense mapping
Holliday et al. Scale-robust localization using general object landmarks

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