CN113920194A - 基于视觉惯性融合的四旋翼飞行器定位方法 - Google Patents

基于视觉惯性融合的四旋翼飞行器定位方法 Download PDF

Info

Publication number
CN113920194A
CN113920194A CN202111171135.0A CN202111171135A CN113920194A CN 113920194 A CN113920194 A CN 113920194A CN 202111171135 A CN202111171135 A CN 202111171135A CN 113920194 A CN113920194 A CN 113920194A
Authority
CN
China
Prior art keywords
characteristic point
feature
characteristic
pairs
point pair
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
CN202111171135.0A
Other languages
English (en)
Other versions
CN113920194B (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.)
University of Electronic Science and Technology of China
Original Assignee
University of Electronic Science and Technology of China
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 University of Electronic Science and Technology of China filed Critical University of Electronic Science and Technology of China
Priority to CN202111171135.0A priority Critical patent/CN113920194B/zh
Publication of CN113920194A publication Critical patent/CN113920194A/zh
Application granted granted Critical
Publication of CN113920194B publication Critical patent/CN113920194B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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.7：根据可靠特征点对集合D中的特征点对重新计算得到基础矩阵
Figure BDA0003293221880000042
S7.8：判断是否达到预设的迭代结束条件，如果未达到，进入步骤S7.9，如果达到，进入步骤S7.11；
S7.9：对于静态特征点对集合ψS中每个特征点对，根据基础矩阵F′计算得到该特征点对的Sampson误差；
S7.10：令t＝t+1，返回步骤S7.6；
S7.11：对于动态特征对集合ψD中的每个特征点对，根据基础矩阵
Figure BDA0003293221880000041
计算得到该特征点对的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中各个特征点匹配的特征点，得到匹配的特征点对集合
Figure BDA0003293221880000061
记特征点对为
Figure BDA0003293221880000062
分别表示特征点对集合
Figure BDA0003293221880000063
中第m个特征点对中当前帧图像It、上一帧图像It-1中的特征点，
Figure BDA0003293221880000064
Figure BDA0003293221880000065
表示特征点对集合
Figure BDA0003293221880000066
中特征点对数量。
S302：采用后向光流法进行特征跟踪：
采用光流法从当前帧图像It到上一帧图像It-1进行特征跟踪，求得上一帧图像It-1中与当前帧图像It中各个特征点
Figure BDA0003293221880000067
匹配的特征点
Figure BDA0003293221880000068
得到匹配的特征点对集合
Figure BDA0003293221880000069
记特征点对为
Figure BDA00032932218800000610
S303：特征跟踪结果筛选：
理论上说，特征点
Figure BDA00032932218800000611
和特征点
Figure BDA00032932218800000612
是上一帧图像It-1中的同一个点，其位置应该重合，但是因为实际情况不完全满足光度一致性假设，存在估计误差，因此这两个点之间会存在一定的偏移误差，因此可以通过该偏移误差来对跟踪结果的可靠性进行验证，以筛选出可靠的特征点对，具体方法为：
计算特征点
Figure BDA00032932218800000613
和特征点
Figure BDA00032932218800000614
之间的偏移距离，如果该偏移距离小于预设阈值(通常设置为0.5个像素)，则说明当前帧图像It中的特征点
Figure BDA00032932218800000615
为可靠特征点，否则说明特征点
Figure BDA00032932218800000616
为不可靠特征点。然后在特征点对集合
Figure BDA00032932218800000617
筛选出可靠特征点对应的特征点对，作为双向光流法得到的特征点对集合φL。
S104：采用双向描述子法进行特征跟踪：
在采用双向光流法跟踪进行特征跟踪的同时，对特征点集合P中的特征点采用双向描述子法进行特征跟踪，得到当前帧图像It与上一帧图像It-1匹配的特征点对集合φD。
双向描述子法与双向光流法类似，双向描述子匹配法采用的是前向和反向匹配。图4是本实施例中双向描述子法的流程图。如图4所示，本实施例中双向光流法的具体步骤包括：
S401：采用前向描述子法进行特征跟踪：
采用描述子法从上一帧图像It-1到当前帧图像It进行特征跟踪，在当前帧图像It中搜索与上一帧图像It-1中各个特征点匹配的特征点，匹配方法如下：
计算待匹配特征点p0的描述子与各个备选特征点的描述子之间的欧式距离，显然欧式距离越小，两个特征点越相似。然后将备选特征点按照欧式距离从小到大进行排列，选择前2个备选特征点p1、p2，然后采用如下计算公式计算欧式距离比值r：
Figure BDA0003293221880000071
其中，
Figure BDA0003293221880000072
分别表示待匹配特征点p0和备选特征点p1、p2的欧式距离。
如果欧式距离比值r小于预设阈值(本实施例中为0.7)，则将备选特征点p1作为待匹配特征点p0的匹配特征点，否则待匹配特征点p0匹配不成功。
记得到的匹配特征点对集合为
Figure BDA0003293221880000073
特征点对为
Figure BDA0003293221880000074
Figure BDA0003293221880000075
分别表示特征点对集合
Figure BDA0003293221880000076
中第m′个特征点对中当前帧图像It、上一帧图像It-1中的特征点，
Figure BDA0003293221880000077
Figure BDA0003293221880000078
表示特征点对集合
Figure BDA0003293221880000079
中特征点对数量。
S402：采用后向描述子法进行特征跟踪：
采用描述子法从当前帧图像It到上一帧图像It-1进行特征跟踪，求得上一帧图像It-1中与当前帧图像It中各个特征点
Figure BDA00032932218800000710
匹配的特征点
Figure BDA00032932218800000711
得到匹配的特征点对集合
Figure BDA00032932218800000712
记特征点对为
Figure BDA00032932218800000713
S403：特征跟踪结果筛选：
与双向光流法类似，通过特征点
Figure BDA0003293221880000081
和特征点
Figure BDA0003293221880000082
之间的偏移距离来对跟踪结果的可靠性进行验证，以筛选出可靠的特征点对，具体方法为：
计算特征点
Figure BDA0003293221880000083
和特征点
Figure BDA0003293221880000084
之间的偏移距离，如果该偏移距离小于预设阈值，则说明当前帧图像It中的特征点
Figure BDA0003293221880000085
为可靠特征点，否则说明特征点
Figure BDA0003293221880000086
为不可靠特征点。然后在特征点对集合
Figure BDA0003293221880000087
筛选出可靠特征点对应的特征点对，作为双向描述子法得到的特征点对集合φ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：重新计算基础矩阵：
根据可靠特征点对集合D中的特征点对重新计算得到基础矩阵
Figure BDA0003293221880000101
S608：判断是否达到预设的迭代结束条件，如果未达到，进入步骤S609，如果达到，进入步骤S611。
迭代结束条件可以根据需要进行设置，一般有两种：一为可靠特征点不再增加，一为迭代次数达到预设阈值。
S609：重新计算Sampson误差：
对于静态特征点对集合ψS中每个特征点对，根据基础矩阵
Figure BDA0003293221880000102
计算得到该特征点对的Sampson误差。
S610：令t＝t+1，返回步骤S606。
S611：运动一致性检测：
对于动态特征对集合ψD中的每个特征点对，根据基础矩阵
Figure BDA0003293221880000103
计算得到该特征点对的Sampson误差，如果将Sampson误差小于预设阈值，则将该特征点对加入静态特征点对集合ψS，否则不作任何操作。
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.7：根据可靠特征点对集合D中的特征点对重新计算得到基础矩阵
Figure FDA0003293221870000021
S7.8：判断是否达到预设的迭代结束条件，如果未达到，进入步骤S7.9，如果达到，进入步骤S7.11；
S7.9：对于静态特征点对集合ψS中每个特征点对，根据基础矩阵F′计算得到该特征点对的Sampson误差；
S7.10：令t＝t+1，返回步骤S7.6；
S7.11：对于动态特征对集合ψD中的每个特征点对，根据基础矩阵
Figure FDA0003293221870000022
计算得到该特征点对的Sampson误差，如果将Sampson误差小于预设阈值，则将该特征点对加入静态特征点对集合ψS，否则不作任何操作；
S8：根据静态特征点对集合ψS中的特征点对，采用BA优化算法获取四旋翼飞行器基于视觉的位姿参数；
S9：根据惯性测量装置测量的惯性数据获取四旋翼飞行器当前的位姿参数；
S10：将基于视觉的位姿参数和基于惯性传感器的位姿参数进行融合，得到最终的位姿参数。
2.根据权利要求1所述的旋翼飞行器定位方法，其特征在于，所述步骤S3中双向光流法的具体步骤包括：
S3.1：采用光流法从上一帧图像It-1到当前帧图像It进行特征跟踪，在当前帧图像It中搜索与上一帧图像It-1中各个特征点匹配的特征点，得到匹配的特征点对集合
Figure FDA0003293221870000031
记特征点对为
Figure FDA0003293221870000032
Figure FDA0003293221870000033
分别表示特征点对集合
Figure FDA0003293221870000034
中第m个特征点对中当前帧图像It、上一帧图像It-1中的特征点，
Figure FDA0003293221870000035
Figure FDA0003293221870000036
表示特征点对集合
Figure FDA0003293221870000037
中特征点对数量；
S3.2：采用光流法从当前帧图像It到上一帧图像It-1进行特征跟踪，求得上一帧图像It-1中与当前帧图像It中各个特征点
Figure FDA0003293221870000038
匹配的特征点
Figure FDA0003293221870000039
得到匹配的特征点对集合
Figure FDA00032932218700000310
记特征点对为
Figure FDA00032932218700000311
S3.3：计算特征点
Figure FDA00032932218700000312
和特征点
Figure FDA00032932218700000313
之间的偏移距离，如果该偏移距离小于预设阈值(通常设置为0.5个像素)，则说明当前帧图像It中的特征点
Figure FDA00032932218700000314
为可靠特征点，否则说明特征点
Figure FDA00032932218700000315
为不可靠特征点；然后在特征点对集合
Figure FDA00032932218700000316
筛选出可靠特征点对应的特征点对，作为双向光流法得到的特征点对集合φL。
3.根据权利要求1所述的旋翼飞行器定位方法，其特征在于，所述步骤S4中双向描述子法的具体步骤包括：
S4.1：采用描述子法从上一帧图像It-1到当前帧图像It进行特征跟踪，在当前帧图像It中搜索与上一帧图像It-1中各个特征点匹配的特征点，匹配方法如下：
计算待匹配特征点p0的描述子与各个备选特征点的描述子之间的欧式距离，然后将备选特征点按照欧式距离从小到大进行排列，选择前2个备选特征点p1、p2，然后采用如下计算公式计算欧式距离比值r：
Figure FDA00032932218700000317
如果欧式距离比值r小于预设阈值，则将备选特征点p1作为待匹配特征点p0的匹配特征点，否则待匹配特征点p0匹配不成功；
记得到的匹配特征点对集合为
Figure FDA0003293221870000041
特征点对为
Figure FDA0003293221870000042
Figure FDA0003293221870000043
Figure FDA0003293221870000044
分别表示特征点对集合
Figure FDA0003293221870000045
中第m′个特征点对中当前帧图像It、上一帧图像It-1中的特征点，
Figure FDA0003293221870000046
Figure FDA0003293221870000047
表示特征点对集合
Figure FDA0003293221870000048
中特征点对数量；
S4.2：采用描述子法从当前帧图像It到上一帧图像It-1进行特征跟踪，求得上一帧图像It-1中与当前帧图像It中各个特征点
Figure FDA0003293221870000049
匹配的特征点
Figure FDA00032932218700000410
得到匹配的特征点对集合
Figure FDA00032932218700000411
记特征点对为
Figure FDA00032932218700000412
S4.3：计算特征点
Figure FDA00032932218700000413
和特征点
Figure FDA00032932218700000414
之间的偏移距离，如果该偏移距离小于预设阈值，则说明当前帧图像It中的特征点
Figure FDA00032932218700000415
为可靠特征点，否则说明特征点
Figure FDA00032932218700000416
为不可靠特征点；然后在特征点对集合
Figure FDA00032932218700000417
筛选出可靠特征点对应的特征点对，作为双向描述子法得到的特征点对集合φD。
CN202111171135.0A 2021-10-08 2021-10-08 基于视觉惯性融合的四旋翼飞行器定位方法 Active CN113920194B (zh)

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 true CN113920194A (zh) 2022-01-11
CN113920194B 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 (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20080144925A1 (en) * 2006-08-15 2008-06-19 Zhiwei Zhu Stereo-Based Visual Odometry Method and System
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融合的稳定运动跟踪方法和装置
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
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 北京理工大学 基于动态环境下的目标检测的视觉定位与建图系统

Patent Citations (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20080144925A1 (en) * 2006-08-15 2008-06-19 Zhiwei Zhu Stereo-Based Visual Odometry Method and System
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融合的稳定运动跟踪方法和装置
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
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)

* Cited by examiner, † Cited by third party
Title
MEISTER, S.等: "UnFlow: Unsupervised Learning of Optical Flow With a Bidirectional Census Loss" *
SYED TAFSEER HAIDER SHAH等: "Traditional and modern strategies for optical flow: an investigation" *
张格: "可持续在线机器视觉同步定位与建图关键技术研究" *
李瑞等: "基于MATLAB和Gazebo的四旋翼飞行器联合仿真教学平台" *
黄平等: "基于环形特征匹配的双目视觉里程计" *

Also Published As

Publication number Publication date
CN113920194B (zh) 2023-04-21

Similar Documents

Publication Publication Date Title
CN110009681B (zh) 一种基于imu辅助的单目视觉里程计位姿处理方法
CN108986037B (zh) 基于半直接法的单目视觉里程计定位方法及定位系统
CN107341814B (zh) 基于稀疏直接法的四旋翼无人机单目视觉测程方法
CN110490900B (zh) 动态环境下的双目视觉定位方法及系统
CN102298070B (zh) 估算无人机,尤其是能够在自动驾驶下执行悬停飞行的无人机的水平速度的方法
CN110084832B (zh) 相机位姿的纠正方法、装置、系统、设备和存储介质
CN107478220B (zh) 无人机室内导航方法、装置、无人机及存储介质
CN111024066A (zh) 一种无人机视觉-惯性融合室内定位方法
CN110595466B (zh) 轻量级的基于深度学习的惯性辅助视觉里程计实现方法
CN112598757A (zh) 一种多传感器时间空间标定方法及装置
CN113108771A (zh) 一种基于闭环直接稀疏视觉里程计的移动位姿估计方法
CN110310304B (zh) 单目视觉建图和定位方法、装置、存储介质及移动设备
CN112556719B (zh) 一种基于cnn-ekf的视觉惯性里程计实现方法
CN103886107A (zh) 基于天花板图像信息的机器人定位与地图构建系统
CN112204946A (zh) 数据处理方法、装置、可移动平台及计算机可读存储介质
CN114693754B (zh) 一种基于单目视觉惯导融合的无人机自主定位方法与系统
CN108900775B (zh) 一种水下机器人实时电子稳像方法
CN113551665A (zh) 一种用于运动载体的高动态运动状态感知系统及感知方法
CN114964276A (zh) 一种融合惯导的动态视觉slam方法
CN113012224A (zh) 定位初始化方法和相关装置、设备、存储介质
Zhu et al. PairCon-SLAM: Distributed, online, and real-time RGBD-SLAM in large scenarios
CN112432653B (zh) 基于点线特征的单目视觉惯性里程计方法
CN113888607A (zh) 基于事件相机的目标检测与跟踪方法、系统和存储介质
CN112991400A (zh) 一种无人艇的多传感器辅助定位方法
CN112731503A (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