CN113920194B - 基于视觉惯性融合的四旋翼飞行器定位方法 - Google Patents
基于视觉惯性融合的四旋翼飞行器定位方法 Download PDFInfo
- Publication number
- CN113920194B CN113920194B CN202111171135.0A CN202111171135A CN113920194B CN 113920194 B CN113920194 B CN 113920194B CN 202111171135 A CN202111171135 A CN 202111171135A CN 113920194 B CN113920194 B CN 113920194B
- Authority
- CN
- China
- Prior art keywords
- feature
- feature point
- point pair
- points
- characteristic
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 102
- 230000000007 visual effect Effects 0.000 title claims abstract description 24
- 230000004927 fusion Effects 0.000 title claims abstract description 14
- 230000003068 static effect Effects 0.000 claims abstract description 39
- 238000001514 detection method Methods 0.000 claims abstract description 25
- 230000002457 bidirectional effect Effects 0.000 claims description 33
- 239000011159 matrix material Substances 0.000 claims description 30
- 230000003287 optical effect Effects 0.000 claims description 28
- 238000012216 screening Methods 0.000 claims description 11
- 238000005259 measurement Methods 0.000 claims description 7
- 238000004364 calculation method Methods 0.000 claims description 6
- 238000005457 optimization Methods 0.000 claims description 4
- 238000005516 engineering process Methods 0.000 description 7
- 238000011160 research Methods 0.000 description 3
- 230000007547 defect Effects 0.000 description 2
- 238000002996 descriptor matching method Methods 0.000 description 2
- 238000013461 design Methods 0.000 description 2
- 238000011161 development Methods 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 238000000605 extraction Methods 0.000 description 2
- 238000013507 mapping Methods 0.000 description 2
- 238000013473 artificial intelligence Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 238000013527 convolutional neural network Methods 0.000 description 1
- 230000010485 coping Effects 0.000 description 1
- 238000002790 cross-validation Methods 0.000 description 1
- 230000010354 integration Effects 0.000 description 1
- 230000003993 interaction Effects 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 230000007774 longterm Effects 0.000 description 1
- 238000007500 overflow downdraw method Methods 0.000 description 1
- 230000001360 synchronised effect Effects 0.000 description 1
- 238000012549 training Methods 0.000 description 1
- 238000013519 translation Methods 0.000 description 1
- 238000012795 verification Methods 0.000 description 1
Images
Classifications
-
- 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
- G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
-
- 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
- G01C21/1656—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 with passive imaging devices, e.g. cameras
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F18/00—Pattern recognition
- G06F18/20—Analysing
- G06F18/25—Fusion techniques
- G06F18/253—Fusion techniques of extracted features
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06N—COMPUTING ARRANGEMENTS BASED ON SPECIFIC COMPUTATIONAL MODELS
- G06N3/00—Computing arrangements based on biological models
- G06N3/02—Neural networks
- G06N3/04—Architecture, e.g. interconnection topology
- G06N3/045—Combinations of networks
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06N—COMPUTING ARRANGEMENTS BASED ON SPECIFIC COMPUTATIONAL MODELS
- G06N3/00—Computing arrangements based on biological models
- G06N3/02—Neural networks
- G06N3/08—Learning methods
- G06N3/088—Non-supervised learning, e.g. competitive learning
-
- 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
- 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/30108—Industrial image inspection
- G06T2207/30164—Workpiece; Machine component
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Theoretical Computer Science (AREA)
- General Physics & Mathematics (AREA)
- Remote Sensing (AREA)
- Radar, Positioning & Navigation (AREA)
- Data Mining & Analysis (AREA)
- Life Sciences & Earth Sciences (AREA)
- Artificial Intelligence (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Evolutionary Computation (AREA)
- General Engineering & Computer Science (AREA)
- Computing Systems (AREA)
- Biomedical Technology (AREA)
- General Health & Medical Sciences (AREA)
- Mathematical Physics (AREA)
- Software Systems (AREA)
- Computational Linguistics (AREA)
- Health & Medical Sciences (AREA)
- Molecular Biology (AREA)
- Biophysics (AREA)
- Evolutionary Biology (AREA)
- Bioinformatics & Computational Biology (AREA)
- Bioinformatics & Cheminformatics (AREA)
- Automation & Control Theory (AREA)
- Multimedia (AREA)
- Image Analysis (AREA)
Abstract
本发明公开了一种基于视觉惯性融合的四旋翼飞行器定位方法,在四旋翼飞行器运动过程中,采用摄像头持续拍摄周围环境图像,采用惯性测量装置测量惯性数据,对于摄像头拍摄的图像序列提取特征点及对应的描述子,进行特征跟踪得到特征点对,然后对图像序列进行目标检测,将特征点对划分为静态特征点对集合和动态特征点对集合,利用静态特征点的运动状态对动态特征点进行运动一致性检测,筛选出可靠特征点对加入静态特征点对集合,根据静态特征点对获取基于视觉的位姿参数,然后根据惯性测量装置测量的惯性数据获取位姿参数,将两个位姿参数融合,得到最终的位姿参数。本发明可以实现在高动态场景下的自主实时稳定定位。
Description
技术领域
本发明属于四旋翼飞行器定位技术领域,更为具体地讲,涉及一种基于视觉惯性融合的四旋翼飞行器定位方法。
背景技术
近年来,随着SLAM(Simutanious Localization and Mapping,同步定位与地图构建)技术的飞速发展,SLAM开始被应用到各个领域,如自动驾驶、无人机、VR/AR等。同时,随着人工智能技术的快速发展,上层应用开始承担越来越复杂的任务,这使得对底层定位技术的精度和实时性要求越来越高。同时由于面对的场景越来越复杂,且现有的实时定位技术存在着或多或少的缺陷,而移动机器人各项基本功能的实现都离不开SLAM技术的支持,这导致越来越多的企业和高校开始研究泛用性更好、定位精度更高的SLAM定位技术。在过去的十几年中,不少院校和研究机构都在SLAM领域取得了很大的进展,设计并提出了很多性能优越的SLAM***。
视觉SLAM由于其由于其轻量化、高精度、低延时的特性,逐步成为在无GPS导航应用场景下最受关注的方法之一。视觉SLAM又可以分为仅使用视觉信息的纯视觉SLAM和基于视觉惯性融合的VI-SLAM,VI-SLAM相较于纯视觉SLAM在大幅度旋转、运动模糊以及大尺度场景下具有更好的精度和鲁棒性,所以VI-SLAM逐步成为视觉SLAM领域热门研究方向。然而目前,VI-SLAM还有一些复杂的问题没有得到有效解决,比如在动态环境中避免对动态特征地跟踪,以及使机器人感知更高层次的语义信息并完成更高级别的智能交互。
目前,主流的纯视觉SLAM与VI-SLAM***能够在静态场景中正常执行定位任务,但是在高度动态的场景中由于缺乏对动态特征的应对方法,通常会无法正常对自身定位。这是因为视觉的SLAM方案,有一个强假设——利用的地图点、线特征或图像特征都是静止的,一旦传感器的感知范围内出现了大量的运动物体,定位就可能发生漂移甚至丢失。除了高动态环境对VI-SLAM的定位具有较大影响,VI-SLAM还面临着另外两个方面的挑战。一方面,VI-SLAM在应对逆光、弱光、运动模糊等环境时,其视觉前端的特征跟踪往往会出现帧间特征跟踪误差较大或跟踪失败的情况,从而导致后端的状态估计器求解过程无法收敛。所以,在具有挑战性场景下如何保持特征跟踪的跟踪质量也是VI-SLAM需要解决的问题。另一方面,目前主流VI-SLAM***所采用的视觉前端都是基于单一特征跟踪方式——KLT光流法或描述子匹配法。而KLT光流法存在着累积跟踪误差较大的问题,描述子匹配法则由于其特征提取过程的低重复性和匹配过程的不稳定性,存在着跟踪长度较短的问题。但是长期稳定且精确的特征关联是前端构建视觉约束的基础,而准确的视觉约束可以有效地提高VI-SLAM的鲁棒性和精度。由于上述的这些问题,目前VI-SLAM还无法在高动态且复杂多变的大范围场景中实现稳定定位,这对VI-SLAM的应用造成了阻碍。
发明内容
本发明的目的在于克服现有技术的不足,提供一种基于视觉惯性融合的四旋翼飞行器定位方法,实现在高动态场景下的自主实时稳定定位。
为了实现上述发明目的,本发明于视觉惯性融合的四旋翼飞行器定位方法包括以下步骤:
S1:在四旋翼飞行器运动过程中,采用摄像头持续拍摄周围环境图像得到图像序列,同时采用惯性测量装置测量惯性数据;
S2:对于摄像头拍摄的图像序列,采用SuperPoint网络提取得到当前帧图像It的特征点集合P,同时获取每个特征点的描述子;
S3:对于摄像头拍摄的图像序列,对特征点集合P中的特征点采用双向光流法进行特征跟踪,得到当前帧图像It与上一帧图像It-1匹配的特征点对集合φL;
S4:在采用双向光流法跟踪进行特征跟踪的同时,对特征点集合P中的特征点采用双向描述子法进行特征跟踪,得到当前帧图像It与上一帧图像It-1匹配的特征点对集合φD;
S5:采用如下方法筛选可靠特征点对,得到可靠特征点对集合Qvalid:
S5.1:对于特征点集合P中的每个特征点p(i),i=1,2,…,K,K表示特征点集合P中特征点数量,判断其在特征点对集合φL和特征点对集合φD中是否存在对应的特征点对,如果均不存在,则不作任何操作,如果只在一个特征点对集合中存在对应特征点对,则将该特征点对加入可靠特征点对集合Qvalid;如果在两个特征点对集合中均存在对应特征点对,则分别记双向光流法得到的特征点对为(p(i),pL,t-1(i))、双向描述子法得到的特征点对为(p(i),pD,t-1(i)),然后计算特征点pL,t-1(i)和pD,t-1(i)之间的距离,如果距离小于预设阈值,则说明该特征点可靠,将双向描述子法得到的特征点对(p(i),pD,t-1(i))加入可靠特征点对集合Qvalid,否则说明该特征点待定,将双向光流法得到的特征点对(p(i),pL,t-1(i))、双向描述子法得到的特征点对(p(i),pD,t-1(i))均加入待定特征点对集合Qcand;
S5.2:根据可靠特征点对集合Qvalid中的特征点对计算得到基础矩阵F;
S5.3:对于待定特征点对集合Qcand中每组特征点对(p(i),pL,t-1(i))和(p(i),pD,t-1(i)),分别计算这两个特征点对的Sampson误差S(p(i),pL,t-1(i))、S(p(i),pD,t-1(i));
S5.4:对于待定特征点对集合Qcand中每组特征点对(p(i),pL,t-1(i))和(p(i),pD,t-1(i)),如果S(p(i),pL,t-1(i))<S(p(i),pD,t-1(i))且S(p(i),pL,t-1(i))<λ,λ表示预设阈值,则判定特征点对(p(i),pL,t-1(i))可靠,将其加入可靠特征点对集合Qvalid,如果S(p(i),pD,t-1(i))<S(p(i),pL,t-1(i))且S(p(i),pD,t-1(i))<λ,λ表示预设阈值,则判定特征点对(p(i),pD,t-1(i))可靠,将其加入可靠特征点对集合Qvalid,其他情况不作任何操作;
S6:对于摄像头拍摄的图像序列,采用目标检测算法进行目标检测,将检测得到的目标像素点作为当前帧图像It中的动态特征点,其余像素点作为当前帧图像It中的静态特征点;
然后对于步骤S5得到的各个特征点对,判断其中当前帧图像It特征点是否为静态特征点,如果是,则将该特征点对加入静态特征点对集合ψS,否则将该特征点对加入动态特征对集合ψD;
S7:利用静态特征点的运动状态对动态特征点进行运动一致性检测,具体方法为:
S7.1:静态特征点对集合ψS中随机选取n个特征点对;
S7.2:根据步骤S7.1选取的n个特征点对计算计算得到基础矩阵F′;
S7.3:对于静态特征点对集合ψS中每个特征点对,根据基础矩阵F′计算得到该特征点对的Sampson误差;
S7.4:统计Sampson误差大于预设阈值的特征点对数量,如果大于预设数量阈值,则说明当前基础矩阵可靠,进入步骤S7.5,否则返回S7.1;
S7.5:令迭代次数t=1;
S7.6:将Sampson误差大于预设阈值的特征点对加入可靠特征点对集合D;
S7.8:判断是否达到预设的迭代结束条件,如果未达到,进入步骤S7.9,如果达到,进入步骤S7.11;
S7.9:对于静态特征点对集合ψS中每个特征点对,根据基础矩阵F′计算得到该特征点对的Sampson误差;
S7.10:令t=t+1,返回步骤S7.6;
S7.11:对于动态特征对集合ψD中的每个特征点对,根据基础矩阵计算得到该特征点对的Sampson误差,如果将Sampson误差小于预设阈值,则将该特征点对加入静态特征点对集合ψS,否则不作任何操作;
S8:根据静态特征点对集合ψS中的特征点对,采用BA优化算法获取四旋翼飞行器基于视觉的位姿参数;
S9:根据惯性测量装置测量的惯性数据获取四旋翼飞行器当前的位姿参数;
S10:将基于视觉的位姿参数和基于惯性传感器的位姿参数进行融合,得到最终的位姿参数。
本发明基于视觉惯性融合的四旋翼飞行器定位方法,在四旋翼飞行器运动过程中,采用摄像头持续拍摄周围环境图像,采用惯性测量装置测量惯性数据,对于摄像头拍摄的图像序列提取特征点及对应的描述子,进行特征跟踪得到特征点对,然后对图像序列进行目标检测,将特征点对划分为静态特征点对集合和动态特征点对集合,利用静态特征点的运动状态对动态特征点进行运动一致性检测,筛选出可靠特征点对加入静态特征点对集合,根据静态特征点对获取基于视觉的位姿参数,然后根据惯性测量装置测量的惯性数据获取位姿参数,将两个位姿参数融合,得到最终的位姿参数。
本发明具有以下有益效果:
1)本发明在进行特征跟踪时采用两种方法进行跟踪,然后交叉验证得到跟踪结果,融合了两种特征跟踪方法的优势,解决了单一方法所存在的跟踪长度短以及累积跟踪误差大的问题,提高了对于高动态场景的适应性;
2)本发明采用目标检测将特征跟踪得到的特征点对划分为静态特征点对和动态特征点对,对动态特征点对进行运动一致性检测从而筛选出可靠的动态特征点,有效解决了动态特征对位姿估计的影响,进一步提高了在高动态场景下的位姿估计准确度。
附图说明
图1是本发明基于视觉惯性融合的四旋翼飞行器定位方法的具体实施方式流程图;
图2是本实施例中SuperPoint网络的结构图;
图3是本实施例中双向光流法的流程图;
图4是本实施例中双向描述子法的流程图;
图5是本发明中筛选可靠特征点对的流程图;
图6是本发明中运动一致性检测的流程图。
具体实施方式
下面结合附图对本发明的具体实施方式进行描述,以便本领域的技术人员更好地理解本发明。需要特别提醒注意的是,在以下的描述中,当已知功能和设计的详细描述也许会淡化本发明的主要内容时,这些描述在这里将被忽略。
实施例
图1是本发明基于视觉惯性融合的四旋翼飞行器定位方法的具体实施方式流程图。如图1所示,本发明基于视觉惯性融合的四旋翼飞行器定位方法的具体步骤包括:
S101:获取四旋翼飞行器数据:
在四旋翼飞行器运动过程中,采用摄像头持续拍摄周围环境图像得到图像序列,同时采用惯性测量装置(Inertial Measurement Uni,IMU)测量惯性数据。
S102:提取图像特征点:
对于摄像头拍摄的图像序列,采用SuperPoint网络提取得到当前帧图像It的特征点集合P,同时获取每个特征点的描述子。图2是本实施例中SuperPoint网络的结构图。如图2所示,SuperPoint网络是一种完全卷积的神经网络,基于自监督训练的特征点检测和描述子提取方法进行特征跟踪,从而提取出有效特征点以及描述子。
S103:采用双向光流法进行特征跟踪:
对于摄像头拍摄的图像序列,对特征点集合P中的特征点采用双向光流法进行特征跟踪,得到当前帧图像It与上一帧图像It-1匹配的特征点对集合φL。
图3是本实施例中双向光流法的流程图。如图3所示,本实施例中双向光流法的具体步骤包括:
S301:采用前向光流法进行特征跟踪:
采用光流法从上一帧图像It-1到当前帧图像It进行特征跟踪,在当前帧图像It中搜索与上一帧图像It-1中各个特征点匹配的特征点,得到匹配的特征点对集合记特征点对为分别表示特征点对集合中第m个特征点对中当前帧图像It、上一帧图像It-1中的特征点, 表示特征点对集合中特征点对数量。
S302:采用后向光流法进行特征跟踪:
S303:特征跟踪结果筛选:
理论上说,特征点和特征点是上一帧图像It-1中的同一个点,其位置应该重合,但是因为实际情况不完全满足光度一致性假设,存在估计误差,因此这两个点之间会存在一定的偏移误差,因此可以通过该偏移误差来对跟踪结果的可靠性进行验证,以筛选出可靠的特征点对,具体方法为:
计算特征点和特征点之间的偏移距离,如果该偏移距离小于预设阈值(通常设置为0.5个像素),则说明当前帧图像It中的特征点为可靠特征点,否则说明特征点为不可靠特征点。然后在特征点对集合筛选出可靠特征点对应的特征点对,作为双向光流法得到的特征点对集合φL。
S104:采用双向描述子法进行特征跟踪:
在采用双向光流法跟踪进行特征跟踪的同时,对特征点集合P中的特征点采用双向描述子法进行特征跟踪,得到当前帧图像It与上一帧图像It-1匹配的特征点对集合φD。
双向描述子法与双向光流法类似,双向描述子匹配法采用的是前向和反向匹配。图4是本实施例中双向描述子法的流程图。如图4所示,本实施例中双向光流法的具体步骤包括:
S401:采用前向描述子法进行特征跟踪:
采用描述子法从上一帧图像It-1到当前帧图像It进行特征跟踪,在当前帧图像It中搜索与上一帧图像It-1中各个特征点匹配的特征点,匹配方法如下:
计算待匹配特征点p0的描述子与各个备选特征点的描述子之间的欧式距离,显然欧式距离越小,两个特征点越相似。然后将备选特征点按照欧式距离从小到大进行排列,选择前2个备选特征点p1、p2,然后采用如下计算公式计算欧式距离比值r:
如果欧式距离比值r小于预设阈值(本实施例中为0.7),则将备选特征点p1作为待匹配特征点p0的匹配特征点,否则待匹配特征点p0匹配不成功。
S402:采用后向描述子法进行特征跟踪:
S403:特征跟踪结果筛选:
计算特征点和特征点之间的偏移距离,如果该偏移距离小于预设阈值,则说明当前帧图像It中的特征点为可靠特征点,否则说明特征点为不可靠特征点。然后在特征点对集合筛选出可靠特征点对应的特征点对,作为双向描述子法得到的特征点对集合φD。
S105:筛选可靠特征点对:
为了获得更可靠的特征跟踪结果,以适应高动态场景的需求,本发明需要从跟踪结果中剔除动态点,同时对步骤S103和步骤S104所得到的特征跟踪结果进行交叉验证,从而尽可能地剔除跟踪结果中的误跟踪部分以及跟踪精度较差的部分,筛选出可靠特征点对,得到可靠特征点对集合Qvalid。图5是本发明中筛选可靠特征点对的流程图。如图5所示,本发明中筛选可靠特征点对的具体步骤如下:
S501:特征点对初步分类:
对步骤S102得到的特征点对集合φL和特征点对集合φD中的特征点对进行初步分类,具体方法为:
对于特征点集合P中的每个p(i),i=1,2,…,K,K表示特征点集合P中特征点数量,判断其在特征点对集合φL和特征点对集合φD中是否存在对应的特征点对,如果均不存在,则不作任何操作,如果只在一个特征点对集合中存在对应特征点对,则将该特征点对加入可靠特征点对集合Qvalid;如果在两个特征点对集合中均存在对应特征点对,则分别记双向光流法得到的特征点对为(p(i),pL,t-1(i))、双向描述子法得到的特征点对为(p(i),pD,t-1(i)),然后计算特征点pL,t-1(i)和pD,t-1(i)之间的距离,如果距离小于预设阈值,则说明该特征点可靠,将双向描述子法得到的特征点对(p(i),pD,t-1(i))加入可靠特征点对集合Qvalid,否则说明该特征点待定,将双向光流法得到的特征点对(p(i),pL,t-1(i))、双向描述子法得到的特征点对(p(i),pD,t-1(i))均加入待定特征点对集合Qcand。
S502:计算基础矩阵:
根据可靠特征点对集合Qvalid中的特征点对计算得到基础矩阵F。基础矩阵的计算是SLAM技术领域的常用技术,具体计算过程在此不再赘述。
S503:计算Sampson误差:
对于待定特征点对集合Qcand中每组特征点对(p(i),pL,t-1(i))和(p(i),pD,t-1(i)),分别计算这两个特征点对的Sampson误差S(p(i),pL,t-1(i))、S(p(i),pD,t-1(i))。Sampson误差是复杂性介于代数误差和几何误差之间,但非常近似于几何误差的一种误差。Sampson误差是一种常用误差,其具体计算过程在此不再赘述。
S504:待定特征点对可靠性判定:
对于待定特征点对集合Qcand中每组特征点对(p(i),pL,t-1(i))和(p(i),pD,t-1(i)),如果S(p(i),pL,t-1(i))<S(p(i),pD,t-1(i))且S(p(i),pL,t-1(i))<λ,λ表示预设阈值,则判定特征点对(p(i),pL,t-1(i))可靠,将其加入可靠特征点对集合Qvalid,如果S(p(i),pD,t-1(i))<S(p(i),pL,t-1(i))且S(p(i),pD,t-1(i))<λ,λ表示预设阈值,则判定特征点对(p(i),pD,t-1(i))可靠,将其加入可靠特征点对集合Qvalid,其他情况不作任何操作。
S106:动态目标检测:
对于摄像头拍摄的图像序列,采用目标检测算法进行目标检测,将检测得到的目标像素点作为当前帧图像It中的动态特征点,其余像素点作为当前帧图像It中的静态特征点。然后对于步骤S105得到的各个特征点对,判断其中当前帧图像It特征点是否为静态特征点,如果是,则将该特征点对加入静态特征点对集合ψS,否则将该特征点对加入动态特征点对集合ψD。
本实施例中目标检测算法采用Nanodet目标检测算法。通过目标检测,可以提供特征点的先验动静态信息,从而保证基于视觉的位姿估计的实行,也能在较低的算力开销下提供较为准确的先验信息。
S107:运动一致性检测:
在步骤S106中对静态特征点和动态特征点进行了一个初步划分,但是动态特征点中仍然可能存在可靠的特征点,需要利用静态特征点的运动状态对动态特征点进行运动一致性检测,以便更准确地对动态特征点进行划分。图6是本发明中运动一致性检测的流程图。如图6所示,本发明中运动一致性检测的具体步骤包括:
S601:选取特征点对样本:
从静态特征点对集合ψS中随机选取n个特征点对。本实施例中采用“八点法”计算基础矩阵,因此n=8。
S602:计算基础矩阵:
根据步骤S601选取的n个特征点对计算计算得到基础矩阵F′。
S603:计算Sampson误差:
对于静态特征点对集合ψS中每个特征点对,根据基础矩阵F′计算得到该特征点对的Sampson误差。
S604:统计Sampson误差小于预设阈值的特征点对数量,如果大于预设数量阈值,则说明当前基础矩阵可靠,进入步骤S605,否则返回S601。
S605:令迭代次数t=1。
S606:更新可靠特征点对集合:
将Sampson误差小于预设阈值的特征点对加入可靠特征点对集合D。
S607:重新计算基础矩阵:
S608:判断是否达到预设的迭代结束条件,如果未达到,进入步骤S609,如果达到,进入步骤S611。
迭代结束条件可以根据需要进行设置,一般有两种:一为可靠特征点不再增加,一为迭代次数达到预设阈值。
S609:重新计算Sampson误差:
S610:令t=t+1,返回步骤S606。
S611:运动一致性检测:
S108:确定基于视觉的位姿参数:
根据静态特征点对集合ψS中的特征点对,采用BA优化算法获取四旋翼飞行器基于视觉的位姿参数,包括位置、速度、旋转矩阵和平移矩阵。BA优化算法的具体原理和方法可以参考文献“Mur-Artal R,Montiel J M M,Tardos J D.ORB-SLAM:a versatile andaccurate monocular SLAM system[J].IEEE transactions on robotics,2015,31(5):1147-1163.”。
S109:确定基于惯性传感器的位姿参数:
根据惯性测量装置测量的惯性数据获取四旋翼飞行器当前的位姿参数。本实施例中采用IMU积分方法来根据惯性数据获取基于惯性传感器的位姿参数,其具体原理和方法可以参考文献“Qin T,Li P,Shen S.Vins-mono:A robust and versatile monocularvisual-inertial state estimator[J].IEEE Transactions on Robotics,2018,34(4):1004-1020.”。
S110:位姿参数融合:
将基于视觉的位姿参数和基于惯性传感器的位姿参数进行融合,得到最终的位姿参数。
位姿参数融合的具体方法可以根据实际需要设置,本实施例中采用文献“Qin T,Li P,Shen S.Vins-mono:A robust and versatile monocular visual-inertial stateestimator[J].IEEE Transactions on Robotics,2018,34(4):1004-1020.”中的位姿参数融合方法,具体过程在此不再赘述。
尽管上面对本发明说明性的具体实施方式进行了描述,以便于本技术领域的技术人员理解本发明,但应该清楚,本发明不限于具体实施方式的范围,对本技术领域的普通技术人员来讲,只要各种变化在所附的权利要求限定和确定的本发明的精神和范围内,这些变化是显而易见的,一切利用本发明构思的发明创造均在保护之列。
Claims (3)
1.一种基于视觉惯性融合的四旋翼飞行器定位方法,其特征在于,包括以下步骤:
S1:在四旋翼飞行器运动过程中,采用摄像头持续拍摄周围环境图像得到图像序列,同时采用惯性测量装置测量惯性数据;
S2:对于摄像头拍摄的图像序列,采用SuperPoint网络提取得到当前帧图像It的特征点集合P,同时获取每个特征点的描述子;
S3:对于摄像头拍摄的图像序列,对特征点集合P中的特征点采用双向光流法进行特征跟踪,得到当前帧图像It与上一帧图像It-1匹配的特征点对集合φL;
S4:在采用双向光流法跟踪进行特征跟踪的同时,对特征点集合P中的特征点采用双向描述子法进行特征跟踪,得到当前帧图像It与上一帧图像It-1匹配的特征点对集合φD;
S5:采用如下方法筛选可靠特征点对,得到可靠特征点对集合Qvalid:
S5.1:对于特征点集合P中的每个特征点p(i),i=1,2,…,K,K表示特征点集合P中特征点数量,判断其在特征点对集合φL和特征点对集合φD中是否存在对应的特征点对,如果均不存在,则不作任何操作,如果只在一个特征点对集合中存在对应特征点对,则将该特征点对加入可靠特征点对集合Qvalid;如果在两个特征点对集合中均存在对应特征点对,则分别记双向光流法得到的特征点对为(p(i),pL,t-1(i))、双向描述子法得到的特征点对为(p(i),pD,t-1(i)),然后计算特征点pL,t-1(i)和pD,t-1(i)之间的距离,如果距离小于预设阈值,则说明该特征点可靠,将双向描述子法得到的特征点对(p(i),pD,t-1(i))加入可靠特征点对集合Qvalid,否则说明该特征点待定,将双向光流法得到的特征点对(p(i),pL,t-1(i))、双向描述子法得到的特征点对(p(i),pD,t-1(i))均加入待定特征点对集合Qcand;
S5.2:根据可靠特征点对集合Qvalid中的特征点对计算得到基础矩阵F;
S5.3:对于待定特征点对集合Qcand中每组特征点对(p(i),pL,t-1(i))和(p(i),pD,t-1(i)),分别计算这两个特征点对的Sampson误差S(p(i),pL,t-1(i))、S(p(i),pD,t-1(i));
S5.4:对于待定特征点对集合Qcand中每组特征点对(p(i),pL,t-1(i))和(p(i),pD,t-1(i)),如果S(p(i),pL,t-1(i))<S(p(i),pD,t-1(i))且S(p(i),pL,t-1(i))<λ,λ表示预设阈值,则判定特征点对(p(i),pL,t-1(i))可靠,将其加入可靠特征点对集合Qvalid,如果S(p(i),pD,t-1(i))<S(p(i),pL,t-1(i))且S(p(i),pD,t-1(i))<λ,λ表示预设阈值,则判定特征点对(p(i),pD,t-1(i))可靠,将其加入可靠特征点对集合Qvalid,其他情况不作任何操作;
S6:对于摄像头拍摄的图像序列,采用目标检测算法进行目标检测,将检测得到的目标像素点作为当前帧图像It中的动态特征点,其余像素点作为当前帧图像It中的静态特征点;
然后对于步骤S5得到的各个特征点对,判断其中当前帧图像It特征点是否为静态特征点,如果是,则将该特征点对加入静态特征点对集合ψS,否则将该特征点对加入动态特征对集合ψD;
S7:利用静态特征点的运动状态对动态特征点进行运动一致性检测,具体方法为:
S7.1:静态特征点对集合ψS中随机选取n个特征点对;
S7.2:根据步骤S7.1选取的n个特征点对计算得到基础矩阵F′;
S7.3:对于静态特征点对集合ψS中每个特征点对,根据基础矩阵F′计算得到该特征点对的Sampson误差;
S7.4:统计Sampson误差大于预设阈值的特征点对数量,如果大于预设数量阈值,则说明当前基础矩阵可靠,进入步骤S7.5,否则返回S7.1;
S7.5:令迭代次数t=1;
S7.6:将Sampson误差大于预设阈值的特征点对加入可靠特征点对集合D;
S7.8:判断是否达到预设的迭代结束条件,如果未达到,进入步骤S7.9,如果达到,进入步骤S7.11;
S7.9:对于静态特征点对集合ψS中每个特征点对,根据基础矩阵F′计算得到该特征点对的Sampson误差;
S7.10:令t=t+1,返回步骤S7.6;
S7.11:对于动态特征对集合ψD中的每个特征点对,根据基础矩阵计算得到该特征点对的Sampson误差,如果将Sampson误差小于预设阈值,则将该特征点对加入静态特征点对集合ψS,否则不作任何操作;
S8:根据静态特征点对集合ψS中的特征点对,采用BA优化算法获取四旋翼飞行器基于视觉的位姿参数;
S9:根据惯性测量装置测量的惯性数据获取四旋翼飞行器当前的位姿参数;
S10:将基于视觉的位姿参数和基于惯性传感器的位姿参数进行融合,得到最终的位姿参数。
2.根据权利要求1所述的旋翼飞行器定位方法,其特征在于,所述步骤S3中双向光流法的具体步骤包括:
S3.1:采用光流法从上一帧图像It-1到当前帧图像It进行特征跟踪,在当前帧图像It中搜索与上一帧图像It-1中各个特征点匹配的特征点,得到匹配的特征点对集合记特征点对为分别表示特征点对集合中第m个特征点对中当前帧图像It、上一帧图像It-1中的特征点,表示特征点对集合中特征点对数量;
3.根据权利要求1所述的旋翼飞行器定位方法,其特征在于,所述步骤S4中双向描述子法的具体步骤包括:
S4.1:采用描述子法从上一帧图像It-1到当前帧图像It进行特征跟踪,在当前帧图像It中搜索与上一帧图像It-1中各个特征点匹配的特征点,匹配方法如下:
计算待匹配特征点p0的描述子与各个备选特征点的描述子之间的欧式距离,然后将备选特征点按照欧式距离从小到大进行排列,选择前2个备选特征点p1、p2,然后采用如下计算公式计算欧式距离比值r:
如果欧式距离比值r小于预设阈值,则将备选特征点p1作为待匹配特征点p0的匹配特征点,否则待匹配特征点p0匹配不成功;
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111171135.0A CN113920194B (zh) | 2021-10-08 | 2021-10-08 | 基于视觉惯性融合的四旋翼飞行器定位方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111171135.0A CN113920194B (zh) | 2021-10-08 | 2021-10-08 | 基于视觉惯性融合的四旋翼飞行器定位方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN113920194A CN113920194A (zh) | 2022-01-11 |
CN113920194B true CN113920194B (zh) | 2023-04-21 |
Family
ID=79238131
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202111171135.0A Active CN113920194B (zh) | 2021-10-08 | 2021-10-08 | 基于视觉惯性融合的四旋翼飞行器定位方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN113920194B (zh) |
Citations (11)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101695136A (zh) * | 2009-10-22 | 2010-04-14 | 北京交通大学 | 一种自动式视频颜色协调处理方法和处理*** |
CN104378582A (zh) * | 2013-08-16 | 2015-02-25 | 北京博思廷科技有限公司 | 一种基于ptz摄像机巡航的智能视频分析***及方法 |
CN105931275A (zh) * | 2016-05-23 | 2016-09-07 | 北京暴风魔镜科技有限公司 | 基于移动端单目和imu融合的稳定运动跟踪方法和装置 |
CN110490900A (zh) * | 2019-07-12 | 2019-11-22 | 中国科学技术大学 | 动态环境下的双目视觉定位方法及*** |
CN110555901A (zh) * | 2019-09-05 | 2019-12-10 | 亮风台(上海)信息科技有限公司 | 动静态场景的定位和建图方法、装置、设备和存储介质 |
CN111024066A (zh) * | 2019-12-10 | 2020-04-17 | 中国航空无线电电子研究所 | 一种无人机视觉-惯性融合室内定位方法 |
CN111046856A (zh) * | 2020-03-13 | 2020-04-21 | 南京晓庄学院 | 基于动静态特征提取并行位姿跟踪与地图创建的方法 |
CN111724439A (zh) * | 2019-11-29 | 2020-09-29 | 中国科学院上海微***与信息技术研究所 | 一种动态场景下的视觉定位方法及装置 |
CN112446882A (zh) * | 2020-10-28 | 2021-03-05 | 北京工业大学 | 一种动态场景下基于深度学习的鲁棒视觉slam方法 |
CN112907633A (zh) * | 2021-03-17 | 2021-06-04 | 中国科学院空天信息创新研究院 | 动态特征点识别方法及其应用 |
CN113066129A (zh) * | 2021-04-12 | 2021-07-02 | 北京理工大学 | 基于动态环境下的目标检测的视觉定位与建图*** |
Family Cites Families (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US7925049B2 (en) * | 2006-08-15 | 2011-04-12 | Sri International | Stereo-based visual odometry method and system |
US20190080166A1 (en) * | 2017-09-13 | 2019-03-14 | TuSimple | Data acquistion and input of neural network method for deep odometry assisted by static scene optical flow |
-
2021
- 2021-10-08 CN CN202111171135.0A patent/CN113920194B/zh active Active
Patent Citations (11)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101695136A (zh) * | 2009-10-22 | 2010-04-14 | 北京交通大学 | 一种自动式视频颜色协调处理方法和处理*** |
CN104378582A (zh) * | 2013-08-16 | 2015-02-25 | 北京博思廷科技有限公司 | 一种基于ptz摄像机巡航的智能视频分析***及方法 |
CN105931275A (zh) * | 2016-05-23 | 2016-09-07 | 北京暴风魔镜科技有限公司 | 基于移动端单目和imu融合的稳定运动跟踪方法和装置 |
CN110490900A (zh) * | 2019-07-12 | 2019-11-22 | 中国科学技术大学 | 动态环境下的双目视觉定位方法及*** |
CN110555901A (zh) * | 2019-09-05 | 2019-12-10 | 亮风台(上海)信息科技有限公司 | 动静态场景的定位和建图方法、装置、设备和存储介质 |
CN111724439A (zh) * | 2019-11-29 | 2020-09-29 | 中国科学院上海微***与信息技术研究所 | 一种动态场景下的视觉定位方法及装置 |
CN111024066A (zh) * | 2019-12-10 | 2020-04-17 | 中国航空无线电电子研究所 | 一种无人机视觉-惯性融合室内定位方法 |
CN111046856A (zh) * | 2020-03-13 | 2020-04-21 | 南京晓庄学院 | 基于动静态特征提取并行位姿跟踪与地图创建的方法 |
CN112446882A (zh) * | 2020-10-28 | 2021-03-05 | 北京工业大学 | 一种动态场景下基于深度学习的鲁棒视觉slam方法 |
CN112907633A (zh) * | 2021-03-17 | 2021-06-04 | 中国科学院空天信息创新研究院 | 动态特征点识别方法及其应用 |
CN113066129A (zh) * | 2021-04-12 | 2021-07-02 | 北京理工大学 | 基于动态环境下的目标检测的视觉定位与建图*** |
Non-Patent Citations (5)
Title |
---|
Meister, S.等.UnFlow: Unsupervised Learning of Optical Flow With a Bidirectional Census Loss.《Thirty-Second AAAI Conference on Artificial Intelligence》.2018,7251-7259. * |
Syed Tafseer Haider Shah等.Traditional and modern strategies for optical flow: an investigation.《SN Applied Sciences》.2021,1-14. * |
张格.可持续在线机器视觉同步定位与建图关键技术研究.《中国博士学位论文全文数据库 信息科技辑》.2021,I138-35. * |
李瑞等.基于MATLAB和Gazebo的四旋翼飞行器联合仿真教学平台.《实验技术与管理》.2021,第38卷190-194+199. * |
黄平等.基于环形特征匹配的双目视觉里程计.《光学学报》.2021,第41卷166-176. * |
Also Published As
Publication number | Publication date |
---|---|
CN113920194A (zh) | 2022-01-11 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110009681B (zh) | 一种基于imu辅助的单目视觉里程计位姿处理方法 | |
JP7326720B2 (ja) | 移動***置推定システムおよび移動***置推定方法 | |
CN107341814B (zh) | 基于稀疏直接法的四旋翼无人机单目视觉测程方法 | |
CN108955718B (zh) | 一种视觉里程计及其定位方法、机器人以及存储介质 | |
Kneip et al. | Robust real-time visual odometry with a single camera and an IMU | |
CN113108771B (zh) | 一种基于闭环直接稀疏视觉里程计的移动位姿估计方法 | |
CN106595659A (zh) | 城市复杂环境下多无人机视觉slam的地图融合方法 | |
CN116205947B (zh) | 基于相机运动状态的双目-惯性融合的位姿估计方法、电子设备及存储介质 | |
CN108519102B (zh) | 一种基于二次投影的双目视觉里程计算方法 | |
CN110570453A (zh) | 一种基于双目视觉的闭环式跟踪特征的视觉里程计方法 | |
CN114485640A (zh) | 基于点线特征的单目视觉惯性同步定位与建图方法及*** | |
Zhou et al. | Fast, accurate thin-structure obstacle detection for autonomous mobile robots | |
CN114964276A (zh) | 一种融合惯导的动态视觉slam方法 | |
Xian et al. | Fusing stereo camera and low-cost inertial measurement unit for autonomous navigation in a tightly-coupled approach | |
CN116989772B (zh) | 一种空地多模态多智能体协同定位与建图方法 | |
Zhu et al. | PairCon-SLAM: Distributed, online, and real-time RGBD-SLAM in large scenarios | |
CN112731503B (zh) | 一种基于前端紧耦合的位姿估计方法及*** | |
Li et al. | RD-VIO: Robust visual-inertial odometry for mobile augmented reality in dynamic environments | |
Bai et al. | A novel feedback mechanism-based stereo visual-inertial slam | |
CN112945233A (zh) | 一种全局无漂移的自主机器人同时定位与地图构建方法 | |
Lu et al. | Vision-based localization methods under GPS-denied conditions | |
CN113920194B (zh) | 基于视觉惯性融合的四旋翼飞行器定位方法 | |
CN115861352A (zh) | 单目视觉、imu和激光雷达的数据融合和边缘提取方法 | |
CN116151320A (zh) | 一种抗动态目标干扰的视觉里程计方法及视觉里程计装置 | |
CN115482282A (zh) | 自动驾驶场景下具有多目标追踪能力的动态slam方法 |
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 |