CN114693754B - 一种基于单目视觉惯导融合的无人机自主定位方法与系统 - Google Patents

一种基于单目视觉惯导融合的无人机自主定位方法与系统 Download PDF

Info

Publication number
CN114693754B
CN114693754B CN202210596099.0A CN202210596099A CN114693754B CN 114693754 B CN114693754 B CN 114693754B CN 202210596099 A CN202210596099 A CN 202210596099A CN 114693754 B CN114693754 B CN 114693754B
Authority
CN
China
Prior art keywords
pose
unmanned aerial
aerial vehicle
inertial navigation
frame
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
CN202210596099.0A
Other languages
English (en)
Other versions
CN114693754A (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.)
Hunan Xinxin Xiangrong Intelligent Technology Co ltd
Original Assignee
Hunan University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Hunan University filed Critical Hunan University
Priority to CN202210596099.0A priority Critical patent/CN114693754B/zh
Publication of CN114693754A publication Critical patent/CN114693754A/zh
Application granted granted Critical
Publication of CN114693754B publication Critical patent/CN114693754B/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/30—Determination of transform parameters for the alignment of images, i.e. image registration
    • G06T7/33—Determination of transform parameters for the alignment of images, i.e. image registration using feature-based methods
    • G06T7/337—Determination of transform parameters for the alignment of images, i.e. image registration using feature-based methods involving reference images or patches
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/22—Matching criteria, e.g. proximity measures
    • 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
    • G06T7/74—Determining position or orientation of objects or cameras using feature-based methods involving reference images or patches
    • 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/10028—Range image; Depth image; 3D point clouds
    • 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/30244—Camera pose
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Landscapes

  • Engineering & Computer Science (AREA)
  • Theoretical Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • General Physics & Mathematics (AREA)
  • Data Mining & Analysis (AREA)
  • Artificial Intelligence (AREA)
  • Bioinformatics & Cheminformatics (AREA)
  • Bioinformatics & Computational Biology (AREA)
  • Evolutionary Biology (AREA)
  • Evolutionary Computation (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • General Engineering & Computer Science (AREA)
  • Navigation (AREA)

Abstract

本发明公开了一种基于单目视觉惯导融合的无人机自主定位方法与系统，本发明方法包括针对无人机航拍得到的连续图像：将相邻图像帧进行帧‑帧配准，再经对极几何测姿、三角测量获得特征点的深度，基于特征点的深度、帧‑帧配准后的图像帧中的二维坐标进行位姿解算得到相对视觉位姿；同时对连续图像提取关键帧，与基准图进行帧‑图匹配以获取特征点的绝对位置坐标，再与三角测量得到的深度进行位姿解算得到绝对视觉位姿；根据相对视觉位姿、绝对视觉位姿以及基于惯性测量单元IMU的检测数据得到的惯导位姿联合优化得到无人机的绝对位姿。本发明能够在不依靠GPS的情况下，实现无人机的高精度、长时间自主实时定位。

Description

一种基于单目视觉惯导融合的无人机自主定位方法与系统
技术领域
本发明涉及无人机定位技术，具体涉及一种基于单目视觉惯导融合的无人机自主定位方法与系统。
背景技术
随着无人飞行技术的日益发展，无人机已广泛应用于各个领域。无论是军用或民用无人机，无人机自身的定位与导航技术尤为重要。近些年，大量研究人员对无人机的定位与导航技术做出贡献，但随着无人机的应用场景愈加复杂与苛刻，无人机自身的定位与导航仍然是一项具有挑战性的技术。尤其是随着现代战争对武器载体隐蔽性的要求越来越高，无人机自主导航设备的需求量也越来越大；武器系统作战性能不断改善，对无人机自主导航设备的要求也不断提高。并且军事应用中的战略型号导航往往不能使用GNSS。需要一个不依赖于GNSS的自主导航方法来提供高精度长航时的导航信息，来满足军民用领域的迫切需求。
GPS是依靠地球人造卫星为基础的导航定位系统，能够提供近地点空间位置的位姿、速度和时间，是目前应用最为广泛的导航定位系统。GPS轻巧简便，造价低，且能实时定位。但是其缺点是GPS接收机需要与GPS卫星进行导航信息的交流，若是无人机进入卫星信号缺失或信号干扰的区域，会导致定位不准确或难以定位。
惯性导航系统（INS）是一种不需要与外界进行信息收发的自主导航系统，被广泛用于无人机、船舶和车辆。INS利用陀螺仪和加速度计测量的角加速度和速度加速度对时间进行积分，获得载体在导航坐标系下的位姿和速度。其缺点是定位过程需要对时间进行积分，而陀螺仪和加速度计会有初始漂移，误差会根据时间而累积，导致无法进行长时间的精确定位与导航。所以，单靠惯性导航系统并不能保证飞机的长期高精度导航，需要借助其他传感器信息进行校正。INS和GPS的组合导航方法是目前应用最为广泛的方法，使用GPS提供的位姿和速度信息对惯导系统进行误差修正，能够实现无人机的长时间不间断定位。并且，对于丢失卫星信号或信号干扰的情况，能够实现短时间内的高精度定位，但是仍然难以实现长时间的高精度自主定位。随着视觉图像技术的发展，基于视觉的定位导航技术也得到完善。视觉匹配定位技术使用相机拍摄实时图与基准图匹配，并进行PNP (Perspective-N-Points Problem)位姿估计的方法实现全局定位或者使用光流法获得相对位置估计。视觉匹配定位系统是全自主的定位系统，拥有全天时全天候工作的特点，且无累积误差，抗干扰能力强。但是，在实际的应用场景中，会因相机的性能、载体移动迅速、光照变化剧烈、特征不突出等情况导致定位精度并不高，所以往往无法单独进行工作。
发明内容
考虑到综合视觉和惯性定位的特点，两种定位系统性能上具有明显的互补性，可将视觉导航辅助传统惯性导航系统（INS），作为一种可靠外部信息抑制导航误差发散，得到一个高稳定性、高精度的自主组合定位系统。并且相机和惯性测量单元IMU都有轻巧、简便的特点，非常适用于无人机。基于上述发现，本发明要解决的技术问题是，提供一种基于单目视觉惯导融合的无人机自主定位方法与系统，本发明旨在实现一种单目视觉惯导组合定位方法，通过视觉匹配定位技术和惯性里程计定位技术互补，能够在不依靠GPS的情况下，实现无人机的高精度、长时间自主实时定位。
为了解决上述技术问题，本发明采用的技术方案为：
一种基于单目视觉惯导融合的无人机自主定位方法，包括：
S1、针对无人机航拍得到的连续图像：将连续图像中的相邻图像帧进行帧-帧配准，再依次经过对极几何测姿、三角测量获得图像帧中特征点的深度，基于特征点的深度、特征点帧-帧配准后的图像帧中的二维坐标进行位姿解算得到相对视觉位姿；同时对所述连续图像提取关键帧，分别将关键帧与预设的基准图进行帧-图匹配以获取关键帧上特征点的绝对位置坐标，根据关键帧上特征点的绝对位置坐标，以及特征点经过三角测量得到的深度进行位姿解算得到绝对视觉位姿；根据惯性测量单元IMU的检测数据获取惯导位姿；
S2、根据相对视觉位姿、绝对视觉位姿以及惯导位姿联合优化，得到无人机的绝对位姿。
可选地，步骤S1中将关键帧与预设的基准图进行帧-图匹配时，计算关键帧与预设的基准图之间的相似性值作为匹配度，以获取关键帧上特征点在匹配度最高的基准图上对应位置像素点的绝对位置坐标作为该特征点的绝对位置坐标，且计算关键帧与预设的基准图之间的相似性值作为匹配度的函数表达式为：
Figure 492781DEST_PATH_IMAGE001
，
上式中，NCC为相似性值，t i 表示关键帧中像素点i的灰度值，w i 表示基准图的候选窗口中像素点i的灰度值，
Figure 203248DEST_PATH_IMAGE002
表示关键帧所有像素的灰度平均值，
Figure 806268DEST_PATH_IMAGE003
表示基准图的候选窗口中所有像素的灰度平均值。
可选地，所述匹配度的函数表达式中，
Figure 449739DEST_PATH_IMAGE004
的计算函数表达式为：
Figure 378380DEST_PATH_IMAGE005
，
上式中，F -1表示逆快速傅里叶变换函数，F(t)表示对关键帧t的快速傅里叶变换，F * (t)表示对基准图的候选窗口w复共轭的快速傅里叶变换以实现灰度特征的反转。
可选地，所述匹配度的函数表达式中，
Figure 310564DEST_PATH_IMAGE006
和
Figure 389379DEST_PATH_IMAGE007
的计算函数表达式为：
Figure 719647DEST_PATH_IMAGE008
，
Figure 491294DEST_PATH_IMAGE009
，
上式中，(u 1,u 2)为基准图的候选窗口w的横坐标区间，(v 1,v 2)为基准图的候选窗口w的纵坐标区间，S表示基准图中对应区域和的积分图，S 2表示基准图中对应区域平方和的积分图。
可选地，所述匹配度的函数表达式中，
Figure 35408DEST_PATH_IMAGE010
和
Figure 121175DEST_PATH_IMAGE011
的计算函数表达式为：
Figure 473659DEST_PATH_IMAGE012
，
Figure 9683DEST_PATH_IMAGE013
，
上式中，(u 1,u 2)为基准图的候选窗口w的横坐标区间，(v 1,v 2)为基准图的候选窗口w的纵坐标区间，S表示关键帧中对应区域和的积分图，S 2表示关键帧中对应区域平方和的积分图。
可选地，步骤S1中将关键帧与预设的基准图进行帧-图匹配之前，还包括对关键帧进行校正，以使关键帧与基准图之间只存在平移；所述对关键帧进行校正包括方向校正和尺度校正，所述方向校正是指在已知无人机的飞行方向和基准图的拍摄方向的前提下，将连续图像的朝向转到基准图的方向，使得连续图像和基准图两者之间的朝向一致；所述尺度校正是指根据已知的飞行高度h、相机分辨率w以及相机视场角r，根据f = (h * tan(r/2))/w求得连续图像的地面分辨率f，然后根据基准图的地面分辨率和连续图像的地面分辨率f的比值对连续图像进行缩放，使得连续图像和基准图两者之间的分辨率一致。
可选地，所述进行位姿解算是指首先采用PNP算法进行位姿解算，然后采用局部捆集调整算法BA优化PNP算法解算得到的位姿。
可选地，步骤S2中根据相对视觉位姿、绝对视觉位姿以及惯导位姿联合优化具体是指采用基于滑动窗口的紧耦合单目VIO算法实现对相对视觉位姿、绝对视觉位姿以及惯导位姿联合优化，得到无人机的绝对位姿。
此外，本发明还提供一种基于单目视觉惯导融合的无人机自主定位系统，包括相互连接的微处理器和存储器，该微处理器被编程或配置以执行所述基于单目视觉惯导融合的无人机自主定位方法的步骤。
此外，本发明还提供一种计算机可读存储介质，所述计算机可读存储介质中存储有计算机程序，该计算机程序用于被微处理器编程或配置以执行所述基于单目视觉惯导融合的无人机自主定位方法的步骤。
和现有技术相比，本发明主要具有下述优点：视觉匹配定位技术可获得无人机的绝对位姿，但是会因为图像纹理结构不够或无人机移动速度太快而导致定位精度不够，但其误差不随时间而发散；而惯性里程计可获得较为精确的无人机相对位姿，受图像质量和无人机速度影响较小，但是在长时间定位过程中，误差会发散增大。针对上述单一定位技术的特点，本发明实现了一种基于单目视觉惯导融合的无人机自主定位方法，通过视觉匹配定位技术和惯性里程计定位技术互补，能在不依靠GPS的情况下，实现无人机自主实时绝对定位。而且，本发明定位精度受传统视觉定位和惯导定位的缺点影响较小，例如图像纹理结构、无人机飞行速度和长时间飞行等问题，能够实现无人机的长时间、高精度自主绝对定位，为未来空中军事战场任务执行提供保障。
附图说明
为了更清楚地说明本发明实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其它的附图。
图1为本发明实施例方法的基本原理示意图。
图2为本发明实施例中P3P问题示意图。
具体实施方式
下面将结合本发明实施例对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅是本发明的一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其它实施例，都属于本发明保护的范围。
如图1所示，本实施例基于单目视觉惯导融合的无人机自主定位方法包括：
S1、针对无人机航拍得到的连续图像：将连续图像中的相邻图像帧进行帧-帧配准，再依次经过对极几何测姿、三角测量获得图像帧中特征点的深度，基于特征点的深度、特征点帧-帧配准后的图像帧中的二维坐标进行位姿解算得到相对视觉位姿；同时对所述连续图像提取关键帧，分别将关键帧与预设的基准图进行帧-图匹配以获取关键帧上特征点的绝对位置坐标，根据关键帧上特征点的绝对位置坐标，以及特征点经过三角测量得到的深度进行位姿解算得到绝对视觉位姿；根据惯性测量单元IMU的检测数据获取惯导位姿；
S2、根据相对视觉位姿、绝对视觉位姿以及惯导位姿联合优化，得到无人机的绝对位姿。
本实施例中，步骤S1中将连续图像中的相邻图像帧进行帧-帧配准为采用SIFT算法实现，SIFT算法是经典的特征匹配算法，提出了对图像的旋转和尺度变换具有鲁棒性的特征检测器和特征描述符，并且具有区分性好、多量性和高速性等特点，对于相邻序列帧间匹配具有良好的效果，具体算法这里不再赘述。
对极几何测姿用于根据特征点在相邻图像帧中的投影点的位置关系，估算出相机相对姿态，包括旋转矩阵R和位移向量t。需要说明的是，对极几何测姿为现有方法，例如可参考现有技术：Tong, Q. ,P. Li , and S. Shen . "VINS-Mono: A Robust andVersatile Monocular Visual-Inertial State Estimator." IEEE Transactions onRobotics PP.99(2017):1-17.
三角测量用于根据相机相对姿态用相机的运动估计特征点的空间位置（深度），该深度包括相对深度和绝对深度的信息。需要说明的是，三角测量为现有方法，例如可参考现有技术Tong, Q. ,P. Li , and S. Shen . "VINS-Mono: A Robust and VersatileMonocular Visual-Inertial State Estimator." IEEE Transactions on RoboticsPP.99(2017):1-17.
本实施例中，步骤S1中将关键帧与预设的基准图进行帧-图匹配时，计算关键帧与预设的基准图之间的相似性值作为匹配度，以获取关键帧上特征点在匹配度最高的基准图上对应位置像素点的绝对位置坐标作为该特征点的绝对位置坐标，且计算关键帧与预设的基准图之间的相似性值作为匹配度的函数表达式为：
Figure 447617DEST_PATH_IMAGE014
，
上式中，NCC为相似性值，t i 表示关键帧中像素点i的灰度值，w i 表示基准图的候选窗口中像素点i的灰度值，
Figure 196131DEST_PATH_IMAGE002
表示关键帧所有像素的灰度平均值，
Figure 668700DEST_PATH_IMAGE003
表示基准图的候选窗口中所有像素的灰度平均值。
本实施例中，匹配度的函数表达式中，
Figure 250991DEST_PATH_IMAGE004
的计算函数表达式为：
Figure 5583DEST_PATH_IMAGE005
，
上式中，F -1表示逆快速傅里叶变换函数，F(t)表示对关键帧t的快速傅里叶变换，F * (t)表示对基准图的候选窗口w复共轭的快速傅里叶变换以实现灰度特征的反转。通过上述方式，相对传统基于像素的计算方法而言，利用快速傅里叶变换计算相似性值NCC的效率更高。
本实施例中，所述匹配度的函数表达式中，
Figure 229891DEST_PATH_IMAGE006
和
Figure 760229DEST_PATH_IMAGE007
的计算函数表达式为：
Figure 44580DEST_PATH_IMAGE008
，
Figure 785003DEST_PATH_IMAGE009
，
上式中，(u 1,u 2)为基准图的候选窗口w的横坐标区间，(v 1,v 2)为基准图的候选窗口w的纵坐标区间，S表示基准图中对应区域和的积分图，S 2表示基准图中对应区域平方和的积分图。通过上述方式，可以实现线性时间的复杂度计算，而且时间消耗近似常量跟候选窗口w的半径大小无关，有利于提高计算相似性值NCC的效率。
本实施例中，所述匹配度的函数表达式中，
Figure 140898DEST_PATH_IMAGE010
和
Figure 322481DEST_PATH_IMAGE011
的计算函数表达式为：
Figure 980995DEST_PATH_IMAGE012
，
Figure 880818DEST_PATH_IMAGE013
，
上式中，(u 1,u 2)为基准图的候选窗口w的横坐标区间，(v 1,v 2)为基准图的候选窗口w的纵坐标区间，S表示关键帧中对应区域和的积分图，S 2表示关键帧中对应区域平方和的积分图。通过上述方式，可以实现线性时间的复杂度计算，而且时间消耗近似常量跟候选窗口w的半径大小无关，有利于提高计算相似性值NCC的效率。
本实施例中，步骤S1中将关键帧与预设的基准图进行帧-图匹配之前，还包括对关键帧进行校正，以使关键帧与基准图之间只存在平移；所述对关键帧进行校正包括方向校正和尺度校正，所述方向校正是指在已知无人机的飞行方向和基准图的拍摄方向的前提下，将连续图像的朝向转到基准图的方向，使得连续图像和基准图两者之间的朝向一致；所述尺度校正是指根据已知的飞行高度h、相机分辨率w以及相机视场角r，根据f = (h * tan(r/2))/w求得连续图像的地面分辨率f，然后根据基准图的地面分辨率和连续图像的地面分辨率f的比值对连续图像进行缩放，使得连续图像和基准图两者之间的分辨率一致。
本实施例中，所述进行位姿解算是指首先采用PNP算法进行位姿解算，然后采用局部捆集调整算法BA（Bundle Adjustment）优化PNP算法解算得到的位姿。采用PNP算法进行位姿解算时，输入为特征点的像素坐标(x,y)，输出为相机（无人机）的位姿，位姿可表示为：
Figure 774825DEST_PATH_IMAGE015
，
其中，
Figure 76493DEST_PATH_IMAGE016
表示三维位置，
Figure 529078DEST_PATH_IMAGE017
表示姿态四元数，其中上标w表示世界坐标系（world），下标c k 代表第k帧图像时的相机(camera)坐标系。
PNP(Perspective-n-Point，角度-N-点)算法是一种求解 3D 到 2D 点对运动的方法，其中的N可根据需要取值，例如本实施例中P3P只需要估计3对匹配的特征点。而这三对匹配的特征点需要给出三维世界/坐标和相应的二维坐标，也就是说需要知道三个匹配的特征点之间的几何关系。假设这3个匹配的特征点的3D分别是为A,B,C，2D匹配的特征点分别是a，b，c，它们之间按顺序一一对应，如图2所示。使用3对匹配的特征点进行估计仅仅完成了前期的估计求解工作，P3P还需要额外的1对匹配的特征点去验证前面所获取的所有可能的解。从验证结果中寻找出最正确的结果。假设用于验证的匹配的特征点对是D-d，相机的光心是O。当得到三维点在相机坐标系下对应的位置，就可以得到3D-3D的对应点。需要说明的是，采用PNP算法进行位姿解算为现有公知方法，例如可参见现有方法Tong, Q. ,P. Li , and S. Shen . "VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator." IEEE Transactions on Robotics PP.99(2017):1-17.
局部捆集调整算法BA是一个尽量消除重投影误差的问题，可以用它对PnP进行优化。需要说明的是，采用局部捆集调整算法BA优化PNP算法解算得到的位姿为现有方法，例如可参见现有方法Tong, Q. , P. Li , and S. Shen . "VINS-Mono: A Robust andVersatile Monocular Visual-Inertial State Estimator." IEEE Transactions onRobotics。
步骤S1中，根据惯性测量单元IMU的检测数据获取惯导位姿可根据需要采用可实现的方式。例如作为一种可选的实施方式，本实施例中为摆脱无人机的IMU定位模块在位姿迭代优化过程中的重复积分，使用IMU预积分计算得到无人机帧间内的位置和姿态变化：
Figure 119459DEST_PATH_IMAGE018
，
上式中，
Figure 489260DEST_PATH_IMAGE019
，
Figure 973331DEST_PATH_IMAGE020
，
Figure 770386DEST_PATH_IMAGE021
分别为相邻图像帧间的时间区间[tk,tk+1]内IMU坐标系下的位置、速度、姿态的预积分增量，
Figure 707118DEST_PATH_IMAGE022
为第k个图像帧bk的坐标系下时刻t对应的姿态，
Figure 880610DEST_PATH_IMAGE023
为时刻t对应的加速度，
Figure 360133DEST_PATH_IMAGE024
为t时刻的加速度计偏差；
Figure 328089DEST_PATH_IMAGE025
为t时刻的陀螺仪偏差，
Figure 752117DEST_PATH_IMAGE026
为t时刻的角加速度，且函数Ω的函数表达式为：
Figure 463722DEST_PATH_IMAGE027
，
上式中，ω为函数Ω的角加速度自变量，ω x 、ω y 、ω z 分别为角加速度自变量ω的x、y、z三轴分量。
本实施例中，步骤S2中根据相对视觉位姿、绝对视觉位姿以及惯导位姿联合优化具体是指采用基于滑动窗口的紧耦合单目VIO算法实现对相对视觉位姿、绝对视觉位姿以及惯导位姿联合优化，得到无人机的绝对位姿。采用基于滑动窗口的紧耦合单目VIO算法实现对相对视觉位姿、绝对视觉位姿以及惯导位姿联合优化为现有方法，可参见现有技术Tong, Q. , P. Li , and S. Shen . "VINS-Mono: A Robust and Versatile MonocularVisual-Inertial State Estimator." IEEE Transactions on Robotics PP.99(2017):1-17.该方法中，输入和输出向量X的函数表达式为：
Figure 158270DEST_PATH_IMAGE028
，
上式中，x1～x n 为滑动窗口第1～n帧图像的采集时间内惯性测量单元IMU采集的状态量，n是滑动窗口内的关键帧数量；
Figure 297128DEST_PATH_IMAGE029
表示本体坐标系到相机坐标系的转换矩阵，λ1～λm是第1～m个特征点在基于时序的图像中第一次通过三角测量观测到的特征点的相对逆深度，相对逆深度是指三角测量观测到的相对定位深度的倒数，其中：
Figure 349397DEST_PATH_IMAGE030
，
Figure 599113DEST_PATH_IMAGE031
，
上式中，x k 是滑动窗口第k帧关键帧的采集时间内惯性测量单元IMU采集的状态量，其包含的元素分别为世界坐标系下的位置
Figure 912283DEST_PATH_IMAGE032
、速度
Figure 222041DEST_PATH_IMAGE033
、姿态
Figure 761607DEST_PATH_IMAGE034
、加速度计偏差
Figure 815014DEST_PATH_IMAGE035
和陀螺仪偏差
Figure 982690DEST_PATH_IMAGE036
，n是滑窗内的关键帧数量。
Figure 463350DEST_PATH_IMAGE037
分别表示三维位置和姿态四元数的本体坐标系到相机坐标系的转换矩阵，其上标b表示世界坐标系，下标c表示相机坐标系。
作为一种可选的实施方式，本实施例中在基于滑动窗口的紧耦合单目VIO算法的基础上，将输入和输出向量X进行了扩展，即加入了基于时序的图像中第一次通过三角测量观测到的特征点的绝对逆深度，绝对逆深度是指三角测量观测到的绝对定位深度的倒数，此外还增加了世界坐标系到本体坐标系的转换矩阵
Figure 349266DEST_PATH_IMAGE038
，因此扩展后的输入和输出向量X的函数表达式为：
Figure 206364DEST_PATH_IMAGE039
，
上式中，x1～x n 为滑动窗口第1～n帧图像的采集时间内惯性测量单元IMU采集的状态量，n是滑动窗口内的关键帧数量；
Figure 103913DEST_PATH_IMAGE029
表示本体坐标系到相机坐标系的转换矩阵，
Figure 755474DEST_PATH_IMAGE038
表示世界坐标系到本体坐标系的转换矩阵，λ1～λm是第1～m个特征点在基于时序的图像中第一次通过三角测量观测到的特征点的相对逆深度，m为滑动窗口内的特征点数量，f 1～f j 是第1～j个特征点在基于时序的图像中第一次通过三角测量观测到的特征点的绝对逆深度，其中j表示特征点的总数量，且有：
Figure 957300DEST_PATH_IMAGE040
，
上式中，
Figure 352509DEST_PATH_IMAGE041
分别表示三维位置和姿态四元数的世界坐标系到本体坐标系的转换矩阵，其上标w表示世界坐标系，下标b表示本体坐标系（无人机的本体坐标系）。针对扩展后的输入和输出向量X，基于滑动窗口的紧耦合单目VIO算法可采用相同方法进行求解，最终可获得最优的输入和输出向量X，从而可获得得到无人机的绝对位姿。
此外，本实施例还提供一种基于单目视觉惯导融合的无人机自主定位系统，包括相互连接的微处理器和存储器，该微处理器被编程或配置以执行前述基于单目视觉惯导融合的无人机自主定位方法的步骤。
此外，本实施例还提供一种计算机可读存储介质，所述计算机可读存储介质中存储有计算机程序，该计算机程序用于被微处理器编程或配置以执行前述基于单目视觉惯导融合的无人机自主定位方法的步骤。
本领域内的技术人员应明白，本申请的实施例可提供为方法、系统、或计算机程序产品。因此，本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可读存储介质（包括但不限于磁盘存储器、CD-ROM、光学存储器等）上实施的计算机程序产品的形式。本申请是参照根据本申请实施例的方法、设备（系统）、和计算机程序产品的流程图和／或方框图来描述的。应理解可由计算机程序指令实现流程图和／或方框图中的每一流程和／或方框、以及流程图和／或方框图中的流程和／或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和／或方框图一个方框或多个方框中指定的功能的装置。这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和／或方框图一个方框或多个方框中指定的功能。这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和／或方框图一个方框或多个方框中指定的功能的步骤。
以上所述仅是本发明的优选实施方式，本发明的保护范围并不仅局限于上述实施例，凡属于本发明思路下的技术方案均属于本发明的保护范围。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理前提下的若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (10)

1.一种基于单目视觉惯导融合的无人机自主定位方法，其特征在于，包括：
S1、针对无人机航拍得到的连续图像：将连续图像中的相邻图像帧进行帧-帧配准，再依次经过对极几何测姿、三角测量获得图像帧中特征点的深度，基于特征点的深度、特征点帧-帧配准后的图像帧中的二维坐标进行位姿解算得到相对视觉位姿；同时对所述连续图像提取关键帧，分别将关键帧与预设的基准图进行帧-图匹配以获取关键帧上特征点的绝对位置坐标，根据关键帧上特征点的绝对位置坐标，以及特征点经过三角测量得到的深度进行位姿解算得到绝对视觉位姿；根据惯性测量单元IMU的检测数据获取惯导位姿；
S2、根据相对视觉位姿、绝对视觉位姿以及惯导位姿联合优化，得到无人机的绝对位姿。
2.根据权利要求1所述的基于单目视觉惯导融合的无人机自主定位方法，其特征在于，步骤S1中将关键帧与预设的基准图进行帧-图匹配时，计算关键帧与预设的基准图之间的相似性值作为匹配度，以获取关键帧上特征点在匹配度最高的基准图上对应位置像素点的绝对位置坐标作为该特征点的绝对位置坐标，且计算关键帧与预设的基准图之间的相似性值作为匹配度的函数表达式为：
Figure 33352DEST_PATH_IMAGE001
，
上式中，NCC为相似性值，t i 表示关键帧中像素点i的灰度值，w i 表示基准图的候选窗口中像素点i的灰度值，
Figure 950493DEST_PATH_IMAGE002
表示关键帧所有像素的灰度平均值，
Figure 402334DEST_PATH_IMAGE003
表示基准图的候选窗口中所有像素的灰度平均值。
3.根据权利要求2所述的基于单目视觉惯导融合的无人机自主定位方法，其特征在于，所述匹配度的函数表达式中，
Figure 797543DEST_PATH_IMAGE004
的计算函数表达式为：
Figure 516975DEST_PATH_IMAGE005
，
上式中，F -1表示逆快速傅里叶变换函数，F(t)表示对关键帧t的快速傅里叶变换，F * (t)表示对基准图的候选窗口w复共轭的快速傅里叶变换以实现灰度特征的反转。
4.根据权利要求2所述的基于单目视觉惯导融合的无人机自主定位方法，其特征在于，所述匹配度的函数表达式中，
Figure 339437DEST_PATH_IMAGE006
和
Figure 403208DEST_PATH_IMAGE007
的计算函数表达式为：
Figure 602108DEST_PATH_IMAGE008
，
Figure 677512DEST_PATH_IMAGE009
，
上式中，(u 1,u 2)为基准图的候选窗口w的横坐标区间，(v 1,v 2)为基准图的候选窗口w的纵坐标区间，S表示基准图中对应区域和的积分图，S 2表示基准图中对应区域平方和的积分图。
5.根据权利要求2所述的基于单目视觉惯导融合的无人机自主定位方法，其特征在于，所述匹配度的函数表达式中，
Figure 670876DEST_PATH_IMAGE010
和
Figure 97309DEST_PATH_IMAGE011
的计算函数表达式为：
Figure 834321DEST_PATH_IMAGE012
，
Figure 888864DEST_PATH_IMAGE013
，
上式中，(u 1,u 2)为关键帧的候选窗口w的横坐标区间，(v 1,v 2)为关键帧的候选窗口w的纵坐标区间，S表示关键帧中对应区域和的积分图，S 2表示关键帧中对应区域平方和的积分图。
6.根据权利要求1所述的基于单目视觉惯导融合的无人机自主定位方法，其特征在于，步骤S1中将关键帧与预设的基准图进行帧-图匹配之前，还包括对关键帧进行校正，以使关键帧与基准图之间只存在平移；所述对关键帧进行校正包括方向校正和尺度校正，所述方向校正是指在已知无人机的飞行方向和基准图的拍摄方向的前提下，将连续图像的朝向转到基准图的方向，使得连续图像和基准图两者之间的朝向一致；所述尺度校正是指根据已知的飞行高度h、相机分辨率w以及相机视场角r，根据f = (h * tan(r/2))/w求得连续图像的地面分辨率f，然后根据基准图的地面分辨率和连续图像的地面分辨率f的比值对连续图像进行缩放，使得连续图像和基准图两者之间的分辨率一致。
7.根据权利要求1所述的基于单目视觉惯导融合的无人机自主定位方法，其特征在于，所述进行位姿解算是指首先采用PNP算法进行位姿解算，然后采用局部捆集调整算法BA优化PNP算法解算得到的位姿。
8.根据权利要求7所述的基于单目视觉惯导融合的无人机自主定位方法，其特征在于，步骤S2中根据相对视觉位姿、绝对视觉位姿以及惯导位姿联合优化具体是指采用基于滑动窗口的紧耦合单目VIO算法实现对相对视觉位姿、绝对视觉位姿以及惯导位姿联合优化，得到无人机的绝对位姿。
9.一种基于单目视觉惯导融合的无人机自主定位系统，包括相互连接的微处理器和存储器，其特征在于，该微处理器被编程或配置以执行权利要求1～8中任意一项所述基于单目视觉惯导融合的无人机自主定位方法的步骤。
10.一种计算机可读存储介质，所述计算机可读存储介质中存储有计算机程序，其特征在于，该计算机程序用于被微处理器编程或配置以执行权利要求1～8中任意一项所述基于单目视觉惯导融合的无人机自主定位方法的步骤。
CN202210596099.0A 2022-05-30 2022-05-30 一种基于单目视觉惯导融合的无人机自主定位方法与系统 Active CN114693754B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210596099.0A CN114693754B (zh) 2022-05-30 2022-05-30 一种基于单目视觉惯导融合的无人机自主定位方法与系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210596099.0A CN114693754B (zh) 2022-05-30 2022-05-30 一种基于单目视觉惯导融合的无人机自主定位方法与系统

Publications (2)

Publication Number Publication Date
CN114693754A CN114693754A (zh) 2022-07-01
CN114693754B true CN114693754B (zh) 2022-08-19

Family

ID=82145351

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210596099.0A Active CN114693754B (zh) 2022-05-30 2022-05-30 一种基于单目视觉惯导融合的无人机自主定位方法与系统

Country Status (1)

Country Link
CN (1) CN114693754B (zh)

Families Citing this family (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115128655B (zh) * 2022-08-31 2022-12-02 智道网联科技(北京)有限公司 自动驾驶车辆的定位方法和装置、电子设备和存储介质
CN115618749B (zh) * 2022-12-05 2023-04-07 四川腾盾科技有限公司 一种大型无人机实时定位的误差补偿方法
CN117475358B (zh) * 2023-12-27 2024-04-23 广东南方电信规划咨询设计院有限公司 一种基于无人机视觉的碰撞预测方法及装置
CN117570969B (zh) * 2024-01-16 2024-09-10 锐驰激光(深圳)有限公司 割草机视觉定位方法、装置、设备及存储介质

Family Cites Families (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20170336220A1 (en) * 2016-05-20 2017-11-23 Daqri, Llc Multi-Sensor Position and Orientation Determination System and Device
CN110345944A (zh) * 2019-05-27 2019-10-18 浙江工业大学 融合视觉特征和imu信息的机器人定位方法
CN111595342B (zh) * 2020-04-02 2022-03-18 清华大学 一种可大规模部署的室内定位方法及系统

Also Published As

Publication number Publication date
CN114693754A (zh) 2022-07-01

Similar Documents

Publication Publication Date Title
CN109993113B (zh) 一种基于rgb-d和imu信息融合的位姿估计方法
CN114693754B (zh) 一种基于单目视觉惯导融合的无人机自主定位方法与系统
CN108731670B (zh) 基于量测模型优化的惯性/视觉里程计组合导航定位方法
Kim et al. Ground vehicle navigation in harsh urban conditions by integrating inertial navigation system, global positioning system, odometer and vision data
US8213706B2 (en) Method and system for real-time visual odometry
CN111024066A (zh) 一种无人机视觉-惯性融合室内定位方法
US8305430B2 (en) System and method for multi-camera visual odometry
CN110726406A (zh) 一种改进的非线性优化单目惯导slam的方法
CN109211241A (zh) 基于视觉slam的无人机自主定位方法
CN111880207B (zh) 一种基于小波神经网络的视觉惯性卫星紧耦合定位方法
CN106780699A (zh) 一种基于sins/gps和里程计辅助的视觉slam方法
CN108917753B (zh) 基于从运动恢复结构的飞行器位置确定方法
CN113376669B (zh) 一种基于点线特征的单目vio-gnss融合定位算法
CN114001733B (zh) 一种基于地图的一致性高效视觉惯性定位算法
CN114964276B (zh) 一种融合惯导的动态视觉slam方法
CN113551665A (zh) 一种用于运动载体的高动态运动状态感知系统及感知方法
CN114234967A (zh) 一种基于多传感器融合的六足机器人定位方法
Khoshelham et al. Vehicle positioning in the absence of GNSS signals: Potential of visual-inertial odometry
Xian et al. Fusing stereo camera and low-cost inertial measurement unit for autonomous navigation in a tightly-coupled approach
CN114690229A (zh) 一种融合gps的移动机器人视觉惯性导航方法
Beauvisage et al. Robust multispectral visual-inertial navigation with visual odometry failure recovery
CN113345032B (zh) 一种基于广角相机大畸变图的初始化建图方法及系统
Veth et al. Two-dimensional stochastic projections for tight integration of optical and inertial sensors for navigation
CN115344033B (zh) 一种基于单目相机/imu/dvl紧耦合的无人船导航与定位方法
Mirisola et al. Trajectory recovery and 3d mapping from rotation-compensated imagery for an airship

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
TR01 Transfer of patent right
TR01 Transfer of patent right

Effective date of registration: 20230714

Address after: 410001 No. 002, Floor 5, Building B, No. 10, Zone 2, CSCEC Smart Industrial Park, No. 50, Jinjiang Road, Yuelu Street, Yuelu District, Changsha, Hunan Province

Patentee after: Hunan Xinxin Xiangrong Intelligent Technology Co.,Ltd.

Address before: Yuelu District City, Hunan province 410082 Changsha Lushan Road No. 1

Patentee before: HUNAN University