CN112233177B - 一种无人机位姿估计方法及系统 - Google Patents

一种无人机位姿估计方法及系统 Download PDF

Info

Publication number
CN112233177B
CN112233177B CN202011079450.6A CN202011079450A CN112233177B CN 112233177 B CN112233177 B CN 112233177B CN 202011079450 A CN202011079450 A CN 202011079450A CN 112233177 B CN112233177 B CN 112233177B
Authority
CN
China
Prior art keywords
image
frame
matching
points
eye image
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
CN202011079450.6A
Other languages
English (en)
Other versions
CN112233177A (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.)
China Academy of Safety Science and Technology CASST
Original Assignee
China Academy of Safety Science and Technology CASST
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 China Academy of Safety Science and Technology CASST filed Critical China Academy of Safety Science and Technology CASST
Priority to CN202011079450.6A priority Critical patent/CN112233177B/zh
Publication of CN112233177A publication Critical patent/CN112233177A/zh
Application granted granted Critical
Publication of CN112233177B publication Critical patent/CN112233177B/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
    • 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
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/22—Matching criteria, e.g. proximity measures

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)
  • Image Analysis (AREA)

Abstract

本发明公开了一种无人机位姿估计方法，所述方法包括：前端检测：对搭载在无人机上的摄像机拍摄的双目立体图像提取每帧图像的特征点，对相邻帧图像进行特征匹配和特征跟踪，得到无人机的初始位姿估计结果，并获取关键帧；后端检测：对所述关键帧进行选择，并基于选取的关键帧优化无人机的位姿和姿态图形；回环检测：对选取出的关键帧，基于词袋模型计算图像之间的相似度，在相似度超过阈值时，计算相似变换并优化无人机的姿态图形；其中，所述前端检测、所述后端检测和所述回环检测并行进行。本发明还公开了一种无人机位姿估计系统。本发明的无人机位姿估计方法，可实现复杂环境中无人机高实时性、高精度的定位。

Description

一种无人机位姿估计方法及系统
技术领域
本发明涉及无人机技术领域，具体而言，涉及一种无人机位姿估计方法及系统。
背景技术
相关技术中，无人机在进行识别和定位时，大多通过无人机本体实现，其精度和可靠性都较低。利用单一模式传感器进行识别和定位，又无法全面、精准的构建定位地图，也无法获取无人机的准确位置信息。而利用人工标记点进行识别定位的方法，需要进行特征点不具和标定，过程较为繁琐。
发明内容
本发明针对复杂工业场景中无人机的高实时性、高精度定位问题，提供一种无人机位姿估计方法及系统。
本发明提供了一种无人机位姿估计方法，所述方法包括：
前端检测：对搭载在无人机上的摄像机拍摄的双目立体图像提取每帧图像的特征点，对相邻帧图像进行特征匹配和特征跟踪，得到无人机的初始位姿估计结果，并获取关键帧；
后端检测：对所述关键帧进行选择，并基于选取的关键帧优化无人机的位姿和姿态图形；
回环检测：对选取出的关键帧，基于词袋模型计算图像之间的相似度，在相似度超过阈值时，计算相似变换并优化无人机的姿态图形；
其中，所述前端检测、所述后端检测和所述回环检测并行进行。
作为本发明进一步的改进，所述对搭载在无人机上的摄像机拍摄的双目立体图像提取每帧图像的特征点，对相邻帧图像进行特征匹配和特征跟踪，得到无人机的初始位姿估计结果，并获取关键帧，包括：
搭载在对无人机上的摄像机拍摄的双目立体图像进行校正；
对校正后的左目图像和右目图像分别提取出每帧图像的Shi-Tomasi角点；
对相邻帧图像提取出的角点进行循环匹配，初步筛选出每帧图像的匹配点；
对初步筛选出的匹配点进行过滤，确定每帧图像的特征点；
基于IMU不确定性模型对每帧图像的特征点进行特征跟踪，确定每帧图像的跟踪特征点；
基于每帧图像的跟踪特征点，对无人机进行初始位姿估计，得到初始位姿估计结果，并获取关键帧。
作为本发明进一步的改进，所述对相邻帧图像提取出的角点进行循环匹配，初步筛选出每帧图像的匹配点，包括：
确定匹配顺序，其中，所述匹配顺序为第k-1帧左目图像
Figure GDA0003098468200000026
第k-1帧右目图像
Figure GDA0003098468200000023
第k帧右目图像
Figure GDA0003098468200000024
和第k帧左目图像
Figure GDA0003098468200000025
按照所述匹配顺序，依次从每幅图像中筛选出匹配点；
在所述匹配顺序中，如果下一幅图像中的角点与上一幅图像中的角点相同，则保留该角点，如果下一幅图像中的角点与上一幅图像中的角点不相同，则剔除该角点，依次类推，得到每帧图像的匹配点。
作为本发明进一步的改进，所述对初步筛选出的匹配点进行过滤，确定每帧图像的特征点，包括：
对每帧图像，确定左目图像和右目图像中对应匹配点的归一化相关系数；
将左目图像和右目图像分别分成多个区域，对每个区域中的匹配点构造一个类进行存储；
在每个类中，对区域中的匹配点进行特征属性排序，并将特征属性排序第一的匹配点提取为表示该区域的特征点。
作为本发明进一步的改进，所述归一化相关系数的计算公式如下：
Figure GDA0003098468200000021
其中，IR(x,y)、IT(x,y)分别表示左目图像和右目图像的像素灰度值，s表示视差，D表示匹配点所在的图像匹配块，
Figure GDA0003098468200000022
分别表示左目图像和右目图像在图像匹配块中像素灰度值的平均值。
作为本发明进一步的改进，所述基于IMU不确定性模型对每帧图像的特征点进行特征跟踪，确定每帧图像的跟踪特征点，包括：
对左目图像和右目图像，通过极线约束方程lk+1＝K-TΔt^ΔRK-1分别确定第k帧图像Ik和第k+1帧图像Ik+1的平面相交形成的极线lk+1，其中，ΔR、Δt分别表示第k帧图像Ik和第k+1帧图像Ik+1之间的旋转变化和平移变化，K表示IMU连续采样时的增益；
基于摄像机的内部参数，通过IMU预积分计算第k帧图像Ik和第k+1帧图像Ik+1之间的旋转变化和平移变化ΔR、Δt；
基于摄像机的外部参数，预测极线lk+1的投影矩阵π(Pk+1)l和包含IMU预积分误差项的投影矩阵π(Pk+1)t；
将IMU预积分的不确定性模型集成到极线搜索中，确定不确定系数σn：
Figure GDA0003098468200000031
将极线lk+1作为原始的特征点搜索区域，并在垂直方向上分别向上和向下延伸±σnεe个像素，形成最终的特征点搜索区域，其中，εe表示原始的特征点搜索区域中的单位像素。
作为本发明进一步的改进，所述投影矩阵π(Pk+1)l和所述包含IMU预积分误差项的投影矩阵π(Pk+1)t计算方法如下：
Figure GDA0003098468200000032
Figure GDA0003098468200000033
Figure GDA0003098468200000034
Figure GDA0003098468200000035
Figure GDA0003098468200000036
其中，P表示界点，pk、pk+1分别表示点P在第k帧图像和第k+1帧图像的像素平面上的投影点，
Figure GDA0003098468200000037
δvk,k+1，δpk,k+1为IMU惯导估计的三个方向角度的变化量，K为IMU连续采样时的增益，Rk为k时刻相邻两个节点姿态旋转变化矩阵，Rk+1为k+1时刻相邻两个节点姿态旋转变化矩阵，tk为k时刻，Δtk,k+1为k+1时刻，g为重力加速度，vk为所述无人机移动的速度，
Figure GDA0003098468200000038
为基于i+1坐标的旋转变化率，
Figure GDA0003098468200000041
为坐标系变化的雅克比矩阵，
Figure GDA0003098468200000042
为离散高斯白噪声，Δt为时间变化率，
Figure GDA0003098468200000043
为a节点估计值，
Figure GDA0003098468200000044
为k时刻b节点相对于a节点的变化估计，
Figure GDA0003098468200000045
为基于i坐标的旋转变化率。
作为本发明进一步的改进，所述基于每帧图像的跟踪特征点，对无人机进行初始位姿估计，得到初始位姿估计结果，并获取关键帧，包括：
基于每帧图像的跟踪特征点，进行三角剖分，确定各个跟踪特征点的位置关系；
根据各个跟踪特征点的3D坐标，通过EPnP方法对无人机进行初始位姿估计，得到初始位姿估计结果；
在每帧图像估计完成时，确定是否将当前帧定为关键帧，获取所有关键帧。
作为本发明进一步的改进，对所述关键帧进行选择，并基于选取的关键帧优化无人机的位姿和姿态图形，包括：
对每帧关键帧，确定最优状态量
Figure GDA0003098468200000046
其中，χk表示关键帧k的优化变量，
Figure GDA0003098468200000047
分别表示无人机坐标系在关键帧k处相对于世界坐标系的位移、速度和旋转，
Figure GDA0003098468200000048
分别表示陀螺仪和加速度计在关键帧k处的随机巡视；
对关键帧k中的VSLAM累积残差项、立体视觉重投影残差项和IMU预积分残差项进行优化，并形成约束条件：
Figure GDA0003098468200000049
其中，ES表示VSLAM累积残差项，EIMU表示IMU预积分残差项，EStereo表示立体视觉重投影残差项；
对约束条件求解最小残差项，得到优化的位姿估计结果。
本发明还提供了一种无人机位姿估计系统，所述系统包括：
前端检测模块，对搭载在无人机上的摄像机拍摄的双目立体图像提取每帧图像的特征点，对相邻帧图像进行特征匹配和特征跟踪，得到无人机的初始位姿估计结果，并获取关键帧；
后端检测模块，对所述关键帧进行选择，并基于选取的关键帧优化无人机的位姿和姿态图形；
回环检测模块，对选取出的关键帧，基于词袋模型计算图像之间的相似度，在相似度超过阈值时，计算相似变换并优化无人机的姿态图形；
其中，所述前端检测模块、所述后端检测模块和所述回环检测模块并行处理。
作为本发明进一步的改进，所述前端检测模块被配置为：
搭载在对无人机上的摄像机拍摄的双目立体图像进行校正；
对校正后的左目图像和右目图像分别提取出每帧图像的Shi-Tomasi角点；
对相邻帧图像提取出的角点进行循环匹配，初步筛选出每帧图像的匹配点；
对初步筛选出的匹配点进行过滤，确定每帧图像的特征点；
基于IMU不确定性模型对每帧图像的特征点进行特征跟踪，确定每帧图像的跟踪特征点；
基于每帧图像的跟踪特征点，对无人机进行初始位姿估计，得到初始位姿估计结果，并获取关键帧。
作为本发明进一步的改进，所述对相邻帧图像提取出的角点进行循环匹配，初步筛选出每帧图像的匹配点，包括：
确定匹配顺序，其中，所述匹配顺序为第k-1帧左目图像
Figure GDA0003098468200000051
第k-1帧右目图像
Figure GDA0003098468200000054
第k帧右目图像
Figure GDA0003098468200000053
和第k帧左目图像
Figure GDA0003098468200000052
按照所述匹配顺序，依次从每幅图像中筛选出匹配点；
在所述匹配顺序中，如果下一幅图像中的角点与上一幅图像中的角点相同，则保留该角点，如果下一幅图像中的角点与上一幅图像中的角点不相同，则剔除该角点，依次类推，得到每帧图像的匹配点。
作为本发明进一步的改进，所述对初步筛选出的匹配点进行过滤，确定每帧图像的特征点，包括：
对每帧图像，确定左目图像和右目图像中对应匹配点的归一化相关系数；
将左目图像和右目图像分别分成多个区域，对每个区域中的匹配点构造一个类进行存储；
在每个类中，对区域中的匹配点进行特征属性排序，并将特征属性排序第一的匹配点提取为表示该区域的特征点。
作为本发明进一步的改进，所述归一化相关系数的计算公式如下：
Figure GDA0003098468200000061
其中，IR(x,y)、IT(x,y)分别表示左目图像和右目图像的像素灰度值，s表示视差，D表示匹配点所在的图像匹配块，
Figure GDA0003098468200000062
分别表示左目图像和右目图像在图像匹配块中像素灰度值的平均值。
作为本发明进一步的改进，所述基于IMU不确定性模型对每帧图像的特征点进行特征跟踪，确定每帧图像的跟踪特征点，包括：
对左目图像和右目图像，通过极线约束方程lk+1＝K-TΔt^ΔRK-1分别确定第k帧图像Ik和第k+1帧图像Ik+1的平面相交形成的极线lk+1，其中，ΔR、Δt分别表示第k帧图像Ik和第k+1帧图像Ik+1之间的旋转变化和平移变化，K表示IMU连续采样时的增益；
基于摄像机的内部参数，通过IMU预积分计算第k帧图像Ik和第k+1帧图像Ik+1之间的旋转变化和平移变化ΔR、Δt；
基于摄像机的外部参数，预测极线lk+1的投影矩阵π(Pk+1)l和包含IMU预积分误差项的投影矩阵π(Pk+1)t；
将IMU预积分的不确定性模型集成到极线搜索中，确定不确定系数σn：
Figure GDA0003098468200000063
将极线lk+1作为原始的特征点搜索区域，并在垂直方向上分别向上和向下延伸±σnεe个像素，形成最终的特征点搜索区域，其中，εe表示原始的特征点搜索区域中的单位像素。
作为本发明进一步的改进，所述投影矩阵π(Pk+1)l和所述包含IMU预积分误差项的投影矩阵π(Pk+1)t计算方法如下：
Figure GDA0003098468200000064
Figure GDA0003098468200000065
Figure GDA0003098468200000066
Figure GDA0003098468200000071
Figure GDA0003098468200000072
其中，P表示界点，pk、pk+1分别表示点P在第k帧图像和第k+1帧图像的像素平面上的投影点，
Figure GDA0003098468200000073
δvk,k+1，δpk,k+1为IMU惯导估计的三个方向角度的变化量，K为IMU连续采样时的增益，Rk为k时刻相邻两个节点姿态旋转变化矩阵，Rk+1为k+1时刻相邻两个节点姿态旋转变化矩阵，tk为k时刻，Δtk,k+1为k+1时刻，g为重力加速度，vk为所述无人机移动的速度，
Figure GDA0003098468200000074
为基于i+1坐标的旋转变化率，
Figure GDA0003098468200000075
为坐标系变化的雅克比矩阵，
Figure GDA0003098468200000076
为离散高斯白噪声，Δt为时间变化率，
Figure GDA0003098468200000077
为a节点估计值，
Figure GDA0003098468200000078
为k时刻b节点相对于a节点的变化估计，
Figure GDA0003098468200000079
为基于i坐标的旋转变化率。
作为本发明进一步的改进，所述基于每帧图像的跟踪特征点，对无人机进行初始位姿估计，得到初始位姿估计结果，并获取关键帧，包括：
基于每帧图像的跟踪特征点，进行三角剖分，确定各个跟踪特征点的位置关系；
根据各个跟踪特征点的3D坐标，通过EPnP方法对无人机进行初始位姿估计，得到初始位姿估计结果；
在每帧图像估计完成时，确定是否将当前帧定为关键帧，获取所有关键帧。
作为本发明进一步的改进，所述后端检测模块被配置为：对每帧关键帧，确定最优状态量
Figure GDA00030984682000000710
其中，χk表示关键帧k的优化变量，
Figure GDA00030984682000000711
Figure GDA00030984682000000712
分别表示无人机坐标系在关键帧k处相对于世界坐标系的位移、速度和旋转，
Figure GDA00030984682000000713
分别表示陀螺仪和加速度计在关键帧k处的随机巡视；
对关键帧k中的VSLAM累积残差项、立体视觉重投影残差项和IMU预积分残差项进行优化，并形成约束条件：
Figure GDA00030984682000000714
其中，ES表示VSLAM累积残差项，EIMU表示IMU预积分残差项，EStereo表示立体视觉重投影残差项；
对约束条件求解最小残差项，得到优化的位姿估计结果。
本发明的有益效果为：
本发明所述的方法可实现非结构化复杂环境中无人机高实时性、高精度的定位。前端采用循环匹配和特征筛选来优化特征点匹配，使特征匹配点更加准确、稳定；在图像的特征跟踪，采用IMU不确定性模型对像素可能发生变化的区域进行约束和预测，以此来确定匹配区域，缩短特征匹配搜索时间。后端融合先验残差(VSLAM累积残差)、IMU预积分残差和立体视觉残差，作为约束条件，减少系统累积误差，提高精度，得到全局一致的地图。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍。显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
图1为本发明一示例性实施例所述的一种无人机位姿估计方法的流程示意图；
图2为本发明一示例性实施例所述的循环匹配原理示意图；
图3为本发明一示例性实施例所述的极线搜索原理示意图；
图4为本发明一示例性实施例所述的特征搜索区域原理示意图；
图5为本发明一示例性实施例所述的在前端检测线程上优化的特征跟踪和三角深度测量结果；
图6为本发明一示例性实施例所述的无人机运动轨迹估计的结果示意图；
图7为本发明一示例性实施例所述的绝对位置误差(APE)分析的结果示意图；
图8为本发明一示例性实施例所述的绝对位置误差(APE)的直方图；
图9为本发明一示例性实施例所述的无人机位置估计误差；
图10为本发明一示例性实施例所述的无人机姿态估计误差。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
需要说明，若本发明实施例中有涉及方向性指示(诸如上、下、左、右、前、后……)，则该方向性指示仅用于解释在某一特定姿态(如附图所示)下各部件之间的相对位置关系、运动情况等，如果该特定姿态发生改变时，则该方向性指示也相应地随之改变。
另外，在本发明的描述中，所用术语仅用于说明目的，并非旨在限制本发明的范围。术语“包括”和/或“包含”用于指定所述元件、步骤、操作和/或组件的存在，但并不排除存在或添加一个或多个其他元件、步骤、操作和/或组件的情况。术语“第一”、“第二”等可能用于描述各种元件，不代表顺序，且不对这些元件起限定作用。此外，在本发明的描述中，除非另有说明，“多个”的含义是两个及两个以上。这些术语仅用于区分一个元素和另一个元素。结合以下附图，这些和/或其他方面变得显而易见，并且，本领域普通技术人员更容易理解关于本发明所述实施例的说明。附图仅出于说明的目的用来描绘本发明所述实施例。本领域技术人员将很容易地从以下说明中认识到，在不背离本发明所述原理的情况下，可以采用本发明所示结构和方法的替代实施例。
本发明实施例所述的一种无人机位姿估计方法，如图1所示，所述方法包括：
前端检测：对搭载在无人机上的摄像机拍摄的双目立体图像提取每帧图像的特征点，对相邻帧图像进行特征匹配和特征跟踪，得到无人机的初始位姿估计结果，并获取关键帧；
后端检测：对所述关键帧进行选择，并基于选取的关键帧优化无人机的位姿和姿态图形；
回环检测：对选取出的关键帧，基于词袋模型计算图像之间的相似度，在相似度超过阈值时，计算相似变换并优化无人机的姿态图形；
其中，所述前端检测、所述后端检测和所述回环检测并行进行。
本发明所述的方法，分为三个部分(前端检测、后端检测和回环检测)，每个部分独立设置一个线程，保证所述前端检测、所述后端检测和所述回环检测的并行操作。
在一种可选的实施方式中，所述对搭载在无人机上的摄像机拍摄的双目立体图像提取每帧图像的特征点，对相邻帧图像进行特征匹配和特征跟踪，得到无人机的初始位姿估计结果，并获取关键帧，包括：
S1，搭载在对无人机上的摄像机拍摄的双目立体图像进行校正；
S2，对校正后的左目图像和右目图像分别提取出每帧图像的Shi-Tomasi角点；
S3，对相邻帧图像提取出的角点进行循环匹配，初步筛选出每帧图像的匹配点；
S4，对初步筛选出的匹配点进行过滤，确定每帧图像的特征点；
S5，基于IMU不确定性模型对每帧图像的特征点进行特征跟踪，确定每帧图像的跟踪特征点；
S6，基于每帧图像的跟踪特征点，对无人机进行初始位姿估计，得到初始位姿估计结果，并获取关键帧。
本发明在前端检测输入双目立体图像和IMU(惯性测量单元)姿态测量信息。对双目立体图像进行校正，通过标定的摄像机内部参数中的畸变信息去除摄像机成像失真，使左目图像和右目图像的光心位于同一水平线上，便于循环匹配和特征跟踪。本发明对左目图像和右目图像提取Shi-Tomasi角点，以适合于图像的特征跟踪。
在一种可选的实施方式中，所述对相邻帧图像提取出的角点进行循环匹配，初步筛选出每帧图像的匹配点，包括：
S31，确定匹配顺序，其中，所述匹配顺序为第k-1帧左目图像
Figure GDA0003098468200000101
第k-1帧右目图像
Figure GDA0003098468200000104
第k帧右目图像
Figure GDA0003098468200000103
和第k帧左目图像
Figure GDA0003098468200000102
S32，按照所述匹配顺序，依次从每幅图像中筛选出匹配点；
S33，在所述匹配顺序中，如果下一幅图像中的角点与上一幅图像中的角点相同，则保留该角点，如果下一幅图像中的角点与上一幅图像中的角点不相同，则剔除该角点，依次类推，得到每帧图像的匹配点。
本发明所述的方法在循环匹配时，如图2所示，假设第k-1帧、第k帧两个相邻帧的左目图像和右目图像分别为
Figure GDA0003098468200000111
和
Figure GDA0003098468200000112
在第k-1帧处获取新的角点时，按照
Figure GDA0003098468200000113
的顺序在每幅图像中找到最佳的匹配点，如果循环匹配的角点与上一副中相同，则认为该角点是稳定的高质量点，否则认为低质量点，并将其从队列中剔除。所述的方法通过各个角点在前后两帧以及左右目图像中的对应关系，去除错误的匹配点，提高匹配的精度。
在一种可选的实施方式中，所述对初步筛选出的匹配点进行过滤，确定每帧图像的特征点，包括：
S41，对每帧图像，确定左目图像和右目图像中对应匹配点的归一化相关系数；
其中，所述归一化相关系数的计算公式如下：
Figure GDA0003098468200000114
其中，IR(x,y)、IT(x,y)分别表示左目图像和右目图像的像素灰度值，s表示视差，D表示匹配点所在的图像匹配块，
Figure GDA0003098468200000115
分别表示左目图像和右目图像在图像匹配块中像素灰度值的平均值；
S42，将左目图像和右目图像分别分成多个区域(例如划分成像素大小为50*50的区域)，对每个区域中的匹配点构造一个类进行存储；
S43，在每个类中，对区域中的匹配点进行特征属性排序，并将特征属性排序第一的匹配点提取为表示该区域(例如该50*50区域)的特征点。
其中，例如可以采用比较函数进行特征属性排序。所述比较函数例如为：
Figure GDA0003098468200000116
其中，Lifetime表示特征点生存时间，ρ1和ρ2表示区域中的两个匹配点即两个经过归一化的像素点，life()为计算过程中定义的函数，NCC表示计算过程中定义的代号，不具有特殊含义。
采用比较函数select对特征属性进行排序是对归一化数据进行数据选择的一个过程，可以理解为数据前处理，以使处理后的数据能适于后续的特征跟踪等。
本发明为了增强特征的稳定性，对匹配点进行过滤，计算匹配对(左目图像和右目图像中对应匹配点)的归一化相关系数，并使用组合的特征属性进行筛选。其中，特征属性包括：(1)特征标识符、(2)特征点生存时间、(3)特征组归一化相关系数、(4)初始描述符。所述的方法采用归一化相关系数对左右两幅图像进行相似度的衡量，左目图像和右目图像同一水平线上相关性最高的是最优匹配。在通过归一化相关系数选取最优匹配的基础上，进一步增加特征属性来确定各个小像素区域内的最优特征点。在得到最优匹配后，即可获取左目图像和右目图像之间的差值(视差)。利用相似三角形即可反推以左目图像作为参考系的深度。
立体特征点的精确匹配是影响三角化深度估计精度的重要条件。在实际场景中，由于纹理重复、纹理信息量低，导致立体特征点不能精确匹配。本发明所述的方法通过循环匹配、利用归一化相关系数和特征属性对特征进行过滤，可以获取最佳匹配点。
在一种可选的实施方式中，所述基于IMU不确定性模型对每帧图像的特征点进行特征跟踪，确定每帧图像的跟踪特征点，包括：
S51，对左目图像和右目图像，通过极线约束方程lk+1＝K-TΔt^ΔRK-1分别确定第k帧图像Ik和第k+1帧图像Ik+1的平面相交形成的极线lk+1，其中，ΔR、Δt分别表示第k帧图像Ik和第k+1帧图像Ik+1之间的旋转变化和平移变化，K表示IMU连续采样时的增益，可以在实际采样中进行调整；
S52，基于摄像机的内部参数，通过IMU预积分计算第k帧图像Ik和第k+1帧图像Ik+1之间的旋转变化和平移变化ΔR、Δt；
S53，基于摄像机的外部参数，预测极线lk+1的投影矩阵π(Pk+1)l和包含IMU预积分误差项的投影矩阵π(Pk+1)t；
其中，所述投影矩阵π(Pk+1)l和所述包含IMU预积分误差项的投影矩阵π(Pk+1)t计算方法如下：
Figure GDA0003098468200000121
Figure GDA0003098468200000122
Figure GDA0003098468200000131
Figure GDA0003098468200000132
Figure GDA0003098468200000133
其中，P表示界点，pk、pk+1分别表示点P在第k帧图像和第k+1帧图像的像素平面上的投影点，
Figure GDA0003098468200000134
δvk,k+1，δpk,k+1为IMU惯导估计的三个方向角度的变化量，K为IMU连续采样时的增益，Rk为k时刻相邻两个节点姿态旋转变化矩阵，Rk+1为k+1时刻相邻两个节点姿态旋转变化矩阵，tk为k时刻，Δtk,k+1为k+1时刻，g为重力加速度，vk为所述无人机移动的速度，
Figure GDA0003098468200000135
为基于i+1坐标的旋转变化率，
Figure GDA0003098468200000136
为坐标系变化的雅克比矩阵，
Figure GDA0003098468200000137
为离散高斯白噪声，Δt为时间变化率，
Figure GDA0003098468200000138
为a节点估计值，
Figure GDA0003098468200000139
为k时刻b节点相对于a节点的变化估计，
Figure GDA00030984682000001310
为基于i坐标的旋转变化率；
S54，将IMU预积分的不确定性模型集成到极线搜索中，确定不确定系数σn：
Figure GDA00030984682000001311
S55，将极线lk+1作为原始的特征点搜索区域，并在垂直方向上分别向上和向下延伸±σnεe个像素，形成最终的特征点搜索区域，其中，εe表示原始的特征点搜索区域中的单位像素。
本发明所述的方法在特征跟踪时，如图3所示，Ik和Ik+1是两个相邻帧k和k+1的图像，P是界点，pk和pk+1是点P在像素平面上的投影点，Ik和Ik+1还是两个极平面，两个极平面相交形成的极线lk+1可以从极线约束中获知。将IMU预积分的不确定性模型集成到极线搜索中，特征搜索区域由不确定系数σn和极线lk+1来共同确定，如图4所示，特征区域的定义是以极线lk+1作为原始的搜索区域，在垂直方向上分别向上和向下延伸±σnεe个像素，形成最终的特征点搜索区域，将区域内的特征匹配从全局匹配改为定向搜索。
本发明基于IMU不确定性模型(IMU预积分模型)进行特征跟踪，采用IMU辅助极线搜索法，在待搜索图像中建立基于IMU不确定性模型的特征点搜索区域，可以大大降低全局特征搜索带来的运算冗余。
在一种可选的实施方式中，所述基于每帧图像的跟踪特征点，对无人机进行初始位姿估计，得到初始位姿估计结果，并获取关键帧，包括：
基于每帧图像的跟踪特征点，进行三角剖分，确定各个跟踪特征点的位置关系；
根据各个跟踪特征点的3D坐标，通过EPnP方法对无人机进行初始位姿估计，得到初始位姿估计结果；
在每帧图像估计完成时，确定是否将当前帧定为关键帧，获取所有关键帧。
其中，三角剖分是将图像进行分割的方法，由于平面总能找到三个特征点进行拓扑划分，并且三角连通性好，因此可以进行拓扑三角平面的分割。根据每帧图像的跟踪特征点，将散点集合剖分成不均匀的三角形网格，利用两个相邻三角形构成的凸四边形的对角线进行图像处理，确定各个跟踪特征点的位置关系。
其中，EPnP方法通过各个跟踪特征点的3D坐标，各个3D点对应的、投影在图像上的2D参考点坐标以及摄像机的内部参数，求解PnP问题可以得到无人机的位姿。EPnP方法将世界坐标系中的3D坐标表示为一组虚拟的控制点的加权和。对于一般情形，EPnP方法要求控制点的数目为4，且这4个控制点不能共面，因此将任意3D点的坐标用4个不共面的3D点坐标的权重表示。通常选取世界坐标下的四个控制点坐标为CW＝[0,0,0,1]T,[1,0,0,1]T,[0,1,0,1]T,[0,0,1,1]T；通过n个3D点在相机平面的投影关系，以及与这四个控制点的权重关系，构建一个12*12方阵，求得其零空间特征向量，可以得到虚拟控制点的相机平面坐标，然后使用POSIT算法即可求出相机位姿，得到无人机的位姿。
在一种可选的实施方式中，对所述关键帧进行选择，并基于选取的关键帧优化无人机的位姿和姿态图形，包括：
对每帧关键帧，确定最优状态量
Figure GDA0003098468200000141
其中，χk表示关键帧k的优化变量，
Figure GDA0003098468200000142
分别表示无人机坐标系在关键帧k处相对于世界坐标系的位移、速度和旋转，
Figure GDA0003098468200000143
分别表示陀螺仪和加速度计在关键帧k处的随机巡视；
对关键帧k中的VSLAM累积残差项、立体视觉重投影残差项和IMU预积分残差项进行优化，并形成约束条件：
Figure GDA0003098468200000151
其中，ES表示VSLAM累积残差项，EIMU表示IMU预积分残差项，EStereo表示立体视觉重投影残差项；
对约束条件求解最小残差项，得到优化的位姿估计结果。
本发明采用立体视觉残差和IMU残差相结合的方法实现大规模场景位姿优化。对其中所用到的紧密耦合立体视觉惯性非线性SLAM状态估计问题，本文用图形优化的方法进行表示，并用例如Levenberg-MarQuardt方法(非线性回归中回归参数最小二乘估计的一种估计方法)进行求解，得到最小残差项位姿估计结果。
本发明还通过回环检测来消除累积误差。当摄像机经过同一位置时，及时向全局轨迹提供修正数据，对局部位姿估计偏差较大的数据进行修正，获得全局一致的估计结果。本发明采用图像间匹配检测的方法进行回环，使用词袋方法检测图像之间的相似度和回环检测，并在回环检测成功后计算Sim3(相似变换)，对闭环进行校正。将一幅图像作为一个文本对象，图像中的不同特征构成图像的不同词汇，可以使用图像特征在图像中出现的频率，并使用一个一维向量来描述图像。计算不同图像的一维向量之间的相似度可获取图像之间的相似度，当相似度在阈值范围内时，可以理解为回环检测成功。
本发明所述的方法选取欧洲机器人挑战赛(EURC)公共数据集中的MH_03_Medium进行了测试。其中，数据集场景包括Vicon房间和工厂。无人机使用Vicon或激光雷达收集真实的环境数据，误差在毫米范围内。测试平台是一台配备英特尔酷睿i7处理器和16GB内存的PC计算机。图5为前端检测得到的特征跟踪和三角深度测量结果。图6为MH_03_Medium数据集中无人机运动轨迹估计的结果。同时，为了检验本发明所述的方法的位姿估计误差，采用绝对位置误差(APE)方法进行评定，包括均值、中位数、均方根误差(RMSE)、标准差(STD)等，结果如图7-10所示，结果表明，本发明所述的方法均方根误差为0.285，所述的方法具有较高的轨迹估计精度。
本发明实施例所述的一种无人机位姿估计系统，所述系统包括：
前端检测模块，对搭载在无人机上的摄像机拍摄的双目立体图像提取每帧图像的特征点，对相邻帧图像进行特征匹配和特征跟踪，得到无人机的初始位姿估计结果，并获取关键帧；
后端检测模块，对所述关键帧进行选择，并基于选取的关键帧优化无人机的位姿和姿态图形；
回环检测模块，对选取出的关键帧，基于词袋模型计算图像之间的相似度，在相似度超过阈值时，计算相似变换并优化无人机的姿态图形；
其中，所述前端检测模块、所述后端检测模块和所述回环检测模块各设置一个线程，三个模块并行处理。
在一种可选的实施方式中，所述前端检测模块被配置为：
搭载在对无人机上的摄像机拍摄的双目立体图像进行校正；
对校正后的左目图像和右目图像分别提取出每帧图像的Shi-Tomasi角点；
对相邻帧图像提取出的角点进行循环匹配，初步筛选出每帧图像的匹配点；
对初步筛选出的匹配点进行过滤，确定每帧图像的特征点；
基于IMU不确定性模型对每帧图像的特征点进行特征跟踪，确定每帧图像的跟踪特征点；
基于每帧图像的跟踪特征点，对无人机进行初始位姿估计，得到初始位姿估计结果，并获取关键帧。
本发明在前端检测输入双目立体图像和IMU(惯性测量单元)姿态测量信息。对双目立体图像进行校正，通过标定的摄像机内部参数中的畸变信息去除摄像机成像失真，使左目图像和右目图像的光心位于同一水平线上，便于循环匹配和特征跟踪。本发明对左目图像和右目图像提取Shi-Tomasi角点，以适合于图像的特征跟踪。
在一种可选的实施方式中，所述对相邻帧图像提取出的角点进行循环匹配，初步筛选出每帧图像的匹配点，包括：
确定匹配顺序，其中，所述匹配顺序为第k-1帧左目图像
Figure GDA0003098468200000161
第k-1帧右目图像
Figure GDA0003098468200000171
第k帧右目图像
Figure GDA0003098468200000172
和第k帧左目图像
Figure GDA0003098468200000173
按照所述匹配顺序，依次从每幅图像中筛选出匹配点；
在所述匹配顺序中，如果下一幅图像中的角点与上一幅图像中的角点相同，则保留该角点，如果下一幅图像中的角点与上一幅图像中的角点不相同，则剔除该角点，依次类推，得到每帧图像的匹配点。
本发明所述的系统在循环匹配时，如图2所示，假设第k-1帧、第k帧两个相邻帧的左目图像和右目图像分别为
Figure GDA0003098468200000174
和
Figure GDA0003098468200000175
在第k-1帧处获取新的角点时，按照
Figure GDA0003098468200000176
的顺序在每幅图像中找到最佳的匹配点，如果循环匹配的角点与上一副中相同，则认为该角点是稳定的高质量点，否则认为低质量点，并将其从队列中剔除。所述的系统通过各个角点在前后两帧以及左右目图像中的对应关系，去除错误的匹配点，提高匹配的精度。
在一种可选的实施方式中，所述对初步筛选出的匹配点进行过滤，确定每帧图像的特征点，包括：
对每帧图像，确定左目图像和右目图像中对应匹配点的归一化相关系数；
其中，所述归一化相关系数的计算公式如下：
Figure GDA0003098468200000177
其中，IR(x,y)、IT(x,y)分别表示左目图像和右目图像的像素灰度值，s表示视差，D表示匹配点所在的图像匹配块，
Figure GDA0003098468200000178
分别表示左目图像和右目图像在图像匹配块中像素灰度值的平均值；
将左目图像和右目图像分别分成多个区域(例如划分成像素大小为50*50的区域)，对每个区域中的匹配点构造一个类进行存储；
在每个类中，对区域中的匹配点进行特征属性排序，并将特征属性排序第一的匹配点提取为表示该区域(例如该50*50区域)的特征点。
其中，例如可以采用比较函数进行特征属性排序。所述比较函数例如为：
Figure GDA0003098468200000179
其中，Lifetime表示特征点生存时间，ρ1和ρ2表示区域中的两个匹配点即两个经过归一化的像素点，life()为计算过程中定义的函数，NCC表示计算过程中定义的代号，不具有特殊含义。
采用比较函数select对特征属性进行排序是对归一化数据进行数据选择的一个过程，可以理解为数据前处理，以使处理后的数据能适于后续的特征跟踪等。
本发明为了增强特征的稳定性，对匹配点进行过滤，计算匹配对(左目图像和右目图像中对应匹配点)的归一化相关系数，并使用组合的特征属性进行筛选。其中，特征属性包括：(1)特征标识符、(2)特征点生存时间、(3)特征组归一化相关系数、(4)初始描述符。所述的方法采用归一化相关系数对左右两幅图像进行相似度的衡量，左目图像和右目图像同一水平线上相关性最高的是最优匹配。在通过归一化相关系数选取最优匹配的基础上，进一步增加特征属性来确定各个小像素区域内的最优特征点。在得到最优匹配后，即可获取左目图像和右目图像之间的差值(视差)。利用相似三角形即可反推以左目图像作为参考系的深度。
立体特征点的精确匹配是影响三角化深度估计精度的重要条件。在实际场景中，由于纹理重复、纹理信息量低，导致立体特征点不能精确匹配。本发明所述的方法通过循环匹配、利用归一化相关系数和特征属性对特征进行过滤，可以获取最佳匹配点。
在一种可选的实施方式中，所述基于IMU不确定性模型对每帧图像的特征点进行特征跟踪，确定每帧图像的跟踪特征点，包括：
对左目图像和右目图像，通过极线约束方程lk+1＝K-TΔt^ΔRK-1分别确定第k帧图像Ik和第k+1帧图像Ik+1的平面相交形成的极线lk+1，其中，ΔR、Δt分别表示第k帧图像Ik和第k+1帧图像Ik+1之间的旋转变化和平移变化，K表示IMU连续采样时的增益，可以在实际采样中进行调整；
基于摄像机的内部参数，通过IMU预积分计算第k帧图像Ik和第k+1帧图像Ik+1之间的旋转变化和平移变化ΔR、Δt；
基于摄像机的外部参数，预测极线lk+1的投影矩阵π(Pk+1)l和包含IMU预积分误差项的投影矩阵π(Pk+1)t；
其中，所述投影矩阵π(Pk+1)l和所述包含IMU预积分误差项的投影矩阵π(Pk+1)t计算方法如下：
Figure GDA0003098468200000191
Figure GDA0003098468200000192
Figure GDA0003098468200000193
Figure GDA0003098468200000194
Figure GDA0003098468200000195
其中，P表示界点，pk、pk+1分别表示点P在第k帧图像和第k+1帧图像的像素平面上的投影点，
Figure GDA0003098468200000196
δvk,k+1，δpk,k+1为IMU惯导估计的三个方向角度的变化量，K为IMU连续采样时的增益，Rk为k时刻相邻两个节点姿态旋转变化矩阵，Rk+1为k+1时刻相邻两个节点姿态旋转变化矩阵，tk为k时刻，Δtk,k+1为k+1时刻，g为重力加速度，vk为所述无人机移动的速度，
Figure GDA0003098468200000197
为基于i+1坐标的旋转变化率，
Figure GDA0003098468200000198
为坐标系变化的雅克比矩阵，
Figure GDA0003098468200000199
为离散高斯白噪声，Δt为时间变化率，
Figure GDA00030984682000001910
为a节点估计值，
Figure GDA00030984682000001911
为k时刻b节点相对于a节点的变化估计，
Figure GDA00030984682000001912
为基于i坐标的旋转变化率；
将IMU预积分的不确定性模型集成到极线搜索中，确定不确定系数σn：
Figure GDA00030984682000001913
将极线lk+1作为原始的特征点搜索区域，并在垂直方向上分别向上和向下延伸±σnεe个像素，形成最终的特征点搜索区域，其中，εe表示原始的特征点搜索区域中的单位像素。
本发明所述的系统在特征跟踪时，如图3所示，Ik和Ik+1是两个相邻帧k和k+1的图像，P是界点，pk和pk+1是点P在像素平面上的投影点，Ik和Ik+1还是两个极平面，两个极平面相交形成的极线lk+1可以从极线约束中获知。将IMU预积分的不确定性模型集成到极线搜索中，特征搜索区域由不确定系数σn和极线lk+1来共同确定，如图4所示，特征区域的定义是以极线lk+1作为原始的搜索区域，在垂直方向上分别向上和向下延伸±σnεe个像素，形成最终的特征点搜索区域，将区域内的特征匹配从全局匹配改为定向搜索。
本发明基于IMU不确定性模型(IMU预积分模型)进行特征跟踪，采用IMU辅助极线搜索法，在待搜索图像中建立基于IMU不确定性模型的特征点搜索区域，可以大大降低全局特征搜索带来的运算冗余。
在一种可选的实施方式中，所述基于每帧图像的跟踪特征点，对无人机进行初始位姿估计，得到初始位姿估计结果，并获取关键帧，包括：
基于每帧图像的跟踪特征点，进行三角剖分，确定各个跟踪特征点的位置关系；
根据各个跟踪特征点的3D坐标，通过EPnP方法对无人机进行初始位姿估计，得到初始位姿估计结果；
在每帧图像估计完成时，确定是否将当前帧定为关键帧，获取所有关键帧。
其中，三角剖分是将图像进行分割的方法，由于平面总能找到三个特征点进行拓扑划分，并且三角连通性好，因此可以进行拓扑三角平面的分割。根据每帧图像的跟踪特征点，将散点集合剖分成不均匀的三角形网格，利用两个相邻三角形构成的凸四边形的对角线进行图像处理，确定各个跟踪特征点的位置关系。
其中，EPnP方法通过各个跟踪特征点的3D坐标，各个3D点对应的、投影在图像上的2D参考点坐标以及摄像机的内部参数，求解PnP问题可以得到无人机的位姿。EPnP方法将世界坐标系中的3D坐标表示为一组虚拟的控制点的加权和。对于一般情形，EPnP方法要求控制点的数目为4，且这4个控制点不能共面，因此将任意3D点的坐标用4个不共面的3D点坐标的权重表示。通常选取世界坐标下的四个控制点坐标为CW＝[0,0,0,1]T,[1,0,0,1]T,[0,1,0,1]T,[0,0,1,1]T；通过n个3D点在相机平面的投影关系，以及与这四个控制点的权重关系，构建一个12*12方阵，求得其零空间特征向量，可以得到虚拟控制点的相机平面坐标，然后使用POSIT算法即可求出相机位姿，得到无人机的位姿。
在一种可选的实施方式中，所述后端检测模块被配置为：
对每帧关键帧，确定最优状态量
Figure GDA0003098468200000211
其中，χk表示关键帧k的优化变量，
Figure GDA0003098468200000212
分别表示无人机坐标系在关键帧k处相对于世界坐标系的位移、速度和旋转，
Figure GDA0003098468200000213
分别表示陀螺仪和加速度计在关键帧k处的随机巡视；
对关键帧k中的VSLAM累积残差项、立体视觉重投影残差项和IMU预积分残差项进行优化，并形成约束条件：
Figure GDA0003098468200000214
其中，ES表示VSLAM累积残差项，EIMU表示IMU预积分残差项，EStereo表示立体视觉重投影残差项；
对约束条件求解最小残差项，得到优化的位姿估计结果。
本发明采用立体视觉残差和IMU残差相结合的方法实现大规模场景位姿优化。对其中所用到的紧密耦合立体视觉惯性非线性SLAM状态估计问题，本文用图形优化的方法进行表示，并用例如Levenberg-MarQuardt方法(非线性回归中回归参数最小二乘估计的一种估计方法)进行求解，得到最小残差项位姿估计结果。
本发明还通过回环检测来消除累积误差。当摄像机经过同一位置时，及时向全局轨迹提供修正数据，对局部位姿估计偏差较大的数据进行修正，获得全局一致的估计结果。本发明采用图像间匹配检测的方法进行回环，使用词袋方法检测图像之间的相似度和闭环，并在闭环检测成功后计算Sim3(相似变换)，对闭环进行校正。将一幅图像作为一个文本对象，图像中的不同特征构成图像的不同词汇，可以使用图像特征在图像中出现的频率，并使用一个一维向量来描述图像。计算不同图像的一维向量之间的相似度可获取图像之间的相似度，当相似度在阈值范围内时，可以理解为回环检测成功。
本发明所述的系统选取欧洲机器人挑战赛(EURC)公共数据集中的MH_03_Medium进行了测试。其中，数据集场景包括Vicon房间和工厂。无人机使用Vicon或激光雷达收集真实的环境数据，误差在毫米范围内。测试平台是一台配备英特尔酷睿i7处理器和16GB内存的PC计算机。图5为前端检测得到的特征跟踪和三角深度测量结果。图6为MH_03_Medium数据集中无人机运动轨迹估计的结果。同时，为了检验本发明所述的系统的位姿估计误差，采用绝对位置误差(APE)方法进行评定，包括均值、中位数、均方根误差(RMSE)、标准差(STD)等，结果如图7-10所示，结果表明，本发明所述的系统均方根误差为0.285，所述的系统具有较高的轨迹估计精度。
在此处所提供的说明书中，说明了大量具体细节。然而，能够理解，本发明的实施例可以在没有这些具体细节的情况下实践。在一些实例中，并未详细示出公知的方法、结构和技术，以便不模糊对本说明书的理解。
此外，本领域普通技术人员能够理解，尽管在此所述的一些实施例包括其它实施例中所包括的某些特征而不是其它特征，但是不同实施例的特征的组合意味着处于本发明的范围之内并且形成不同的实施例。例如，在权利要求书中，所要求保护的实施例的任意之一都可以以任意的组合方式来使用。
本领域技术人员应理解，尽管已经参考示例性实施例描述了本发明，但是在不脱离本发明的范围的情况下，可进行各种改变并可用等同物替换其元件。另外，在不脱离本发明的实质范围的情况下，可进行许多修改以使特定情况或材料适应本发明的教导。因此，本发明不限于所公开的特定实施例，而是本发明将包括落入所附权利要求范围内的所有实施例。

Claims (14)

1.一种无人机位姿估计方法，其特征在于，所述方法包括：
前端检测：对搭载在无人机上的摄像机拍摄的双目立体图像提取每帧图像的特征点，对相邻帧图像进行特征匹配和特征跟踪，得到无人机的初始位姿估计结果，并获取关键帧，包括：
搭载在对无人机上的摄像机拍摄的双目立体图像进行校正；
对校正后的左目图像和右目图像分别提取出每帧图像的Shi-Tomasi角点；
对相邻帧图像提取出的角点进行循环匹配，初步筛选出每帧图像的匹配点；
对初步筛选出的匹配点，计算左目图像和右目图像中对应匹配点的归一化相关系数，并使用组合的特征属性进行筛选，确定每帧图像的特征点；
基于IMU不确定性模型对每帧图像的特征点进行特征跟踪，确定每帧图像的跟踪特征点；
基于每帧图像的跟踪特征点，对无人机进行初始位姿估计，得到初始位姿估计结果，并获取关键帧；
后端检测：对所述关键帧进行选择，并基于选取的关键帧优化无人机的位姿和姿态图形；
回环检测：对选取出的关键帧，基于词袋模型计算图像之间的相似度，在相似度超过阈值时，计算相似变换并优化无人机的姿态图形；
其中，所述前端检测、所述后端检测和所述回环检测并行进行；
其中，所述基于IMU不确定性模型对每帧图像的特征点进行特征跟踪，确定每帧图像的跟踪特征点，包括：
对左目图像和右目图像，通过极线约束方程lk+1＝K-TΔt^ΔRK-1分别确定第k帧图像Ik和第k+1帧图像Ik+1的平面相交形成的极线lk+1，其中，ΔR、Δt分别表示第k帧图像Ik和第k+1帧图像Ik+1之间的旋转变化和平移变化，K表示IMU连续采样时的增益；
基于摄像机的内部参数，通过IMU预积分计算第k帧图像Ik和第k+1帧图像Ik+1之间的旋转变化和平移变化ΔR、Δt；
基于摄像机的外部参数，预测极线lk+1的投影矩阵π(Pk+1)l和包含IMU预积分误差项的投影矩阵π(Pk+1)t；
将IMU预积分的不确定性模型集成到极线搜索中，确定不确定系数σn：
Figure FDA0003098468190000021
将极线lk+1作为原始的特征点搜索区域，并在垂直方向上分别向上和向下延伸±σnεe个像素，形成最终的特征点搜索区域，其中，εe表示原始的特征点搜索区域中的单位像素。
2.如权利要求1所述的方法，其中，所述对相邻帧图像提取出的角点进行循环匹配，初步筛选出每帧图像的匹配点，包括：
确定匹配顺序，其中，所述匹配顺序为第k-1帧左目图像
Figure FDA0003098468190000022
第k-1帧右目图像
Figure FDA0003098468190000023
第k帧右目图像
Figure FDA0003098468190000024
和第k帧左目图像
Figure FDA0003098468190000025
按照所述匹配顺序，依次从每幅图像中筛选出匹配点；
在所述匹配顺序中，如果下一幅图像中的角点与上一幅图像中的角点相同，则保留该角点，如果下一幅图像中的角点与上一幅图像中的角点不相同，则剔除该角点，依次类推，得到每帧图像的匹配点。
3.如权利要求1所述的方法，其中，所述对初步筛选出的匹配点进行过滤，确定每帧图像的特征点，包括：
对每帧图像，确定左目图像和右目图像中对应匹配点的归一化相关系数；
将左目图像和右目图像分别分成多个区域，对每个区域中的匹配点构造一个类进行存储；
在每个类中，对区域中的匹配点进行特征属性排序，并将特征属性排序第一的匹配点提取为表示该区域的特征点。
4.如权利要求3所述的方法，其中，所述归一化相关系数的计算公式如下：
Figure FDA0003098468190000026
其中，IR(x,y)、IT(x,y)分别表示左目图像和右目图像的像素灰度值，s表示视差，D表示匹配点所在的图像匹配块，
Figure FDA0003098468190000027
分别表示左目图像和右目图像在图像匹配块中像素灰度值的平均值。
5.如权利要求1所述的方法，其中，所述投影矩阵π(Pk+1)l和所述包含IMU预积分误差项的投影矩阵π(Pk+1)t计算方法如下：
Figure FDA0003098468190000031
Figure FDA0003098468190000032
Figure FDA0003098468190000033
Figure FDA0003098468190000034
Figure FDA0003098468190000035
其中，P表示界点，pk、pk+1分别表示点P在第k帧图像和第k+1帧图像的像素平面上的投影点，
Figure FDA0003098468190000036
δvk,k+1，δpk,k+1为IMU惯导估计的三个方向角度的变化量，K为IMU连续采样时的增益，Rk为k时刻相邻两个节点姿态旋转变化矩阵，Rk+1为k+1时刻相邻两个节点姿态旋转变化矩阵，tk为k时刻，Δtk,k+1为k+1时刻，g为重力加速度，vk为所述无人机移动的速度，
Figure FDA0003098468190000037
为基于i+1坐标的旋转变化率，
Figure FDA0003098468190000038
为坐标系变化的雅克比矩阵，
Figure FDA0003098468190000039
为离散高斯白噪声，Δt为时间变化率，
Figure FDA00030984681900000310
为a节点估计值，
Figure FDA00030984681900000311
为k时刻b节点相对于a节点的变化估计，
Figure FDA00030984681900000312
为基于i坐标的旋转变化率。
6.如权利要求1所述的方法，其中，所述基于每帧图像的跟踪特征点，对无人机进行初始位姿估计，得到初始位姿估计结果，并获取关键帧，包括：
基于每帧图像的跟踪特征点，进行三角剖分，确定各个跟踪特征点的位置关系；
根据各个跟踪特征点的3D坐标，通过EPnP方法对无人机进行初始位姿估计，得到初始位姿估计结果；
在每帧图像估计完成时，确定是否将当前帧定为关键帧，获取所有关键帧。
7.如权利要求1所述的方法，其中，对所述关键帧进行选择，并基于选取的关键帧优化无人机的位姿和姿态图形，包括：
对每帧关键帧，确定最优状态量
Figure FDA0003098468190000041
其中，χk表示关键帧k的优化变量，
Figure FDA0003098468190000042
分别表示无人机坐标系在关键帧k处相对于世界坐标系的位移、速度和旋转，
Figure FDA0003098468190000043
分别表示陀螺仪和加速度计在关键帧k处的随机巡视；
对关键帧k中的VSLAM累积残差项、立体视觉重投影残差项和IMU预积分残差项进行优化，并形成约束条件：
Figure FDA0003098468190000044
其中，ES表示VSLAM累积残差项，EIMU表示IMU预积分残差项，EStereo表示立体视觉重投影残差项；
对约束条件求解最小残差项，得到优化的位姿估计结果。
8.一种无人机位姿估计系统，其特征在于，所述系统包括：
前端检测模块，对搭载在无人机上的摄像机拍摄的双目立体图像提取每帧图像的特征点，对相邻帧图像进行特征匹配和特征跟踪，得到无人机的初始位姿估计结果，并获取关键帧，包括：
搭载在对无人机上的摄像机拍摄的双目立体图像进行校正；
对校正后的左目图像和右目图像分别提取出每帧图像的Shi-Tomasi角点；
对相邻帧图像提取出的角点进行循环匹配，初步筛选出每帧图像的匹配点；
对初步筛选出的匹配点，计算左目图像和右目图像中对应匹配点的归一化相关系数，并使用组合的特征属性进行筛选，确定每帧图像的特征点；
基于IMU不确定性模型对每帧图像的特征点进行特征跟踪，确定每帧图像的跟踪特征点；
基于每帧图像的跟踪特征点，对无人机进行初始位姿估计，得到初始位姿估计结果，并获取关键帧；
后端检测模块，对所述关键帧进行选择，并基于选取的关键帧优化无人机的位姿和姿态图形；
回环检测模块，对选取出的关键帧，基于词袋模型计算图像之间的相似度，在相似度超过阈值时，计算相似变换并优化无人机的姿态图形；
其中，所述前端检测模块、所述后端检测模块和所述回环检测模块并行处理；
其中，所述基于IMU不确定性模型对每帧图像的特征点进行特征跟踪，确定每帧图像的跟踪特征点，包括：
对左目图像和右目图像，通过极线约束方程lk+1＝K-TΔt^ΔRK-1分别确定第k帧图像Ik和第k+1帧图像Ik+1的平面相交形成的极线lk+1，其中，ΔR、Δt分别表示第k帧图像Ik和第k+1帧图像Ik+1之间的旋转变化和平移变化，K表示IMU连续采样时的增益；
基于摄像机的内部参数，通过IMU预积分计算第k帧图像Ik和第k+1帧图像Ik+1之间的旋转变化和平移变化ΔR、Δt；
基于摄像机的外部参数，预测极线lk+1的投影矩阵π(Pk+1)l和包含IMU预积分误差项的投影矩阵π(Pk+1)t；
将IMU预积分的不确定性模型集成到极线搜索中，确定不确定系数σn：
Figure FDA0003098468190000051
将极线lk+1作为原始的特征点搜索区域，并在垂直方向上分别向上和向下延伸±σnεe个像素，形成最终的特征点搜索区域，其中，εe表示原始的特征点搜索区域中的单位像素。
9.如权利要求8所述的系统，其中，所述对相邻帧图像提取出的角点进行循环匹配，初步筛选出每帧图像的匹配点，包括：
确定匹配顺序，其中，所述匹配顺序为第k-1帧左目图像
Figure FDA0003098468190000052
第k-1帧右目图像
Figure FDA0003098468190000053
第k帧右目图像
Figure FDA0003098468190000054
和第k帧左目图像
Figure FDA0003098468190000055
按照所述匹配顺序，依次从每幅图像中筛选出匹配点；
在所述匹配顺序中，如果下一幅图像中的角点与上一幅图像中的角点相同，则保留该角点，如果下一幅图像中的角点与上一幅图像中的角点不相同，则剔除该角点，依次类推，得到每帧图像的匹配点。
10.如权利要求8所述的系统，其中，所述对初步筛选出的匹配点进行过滤，确定每帧图像的特征点，包括：
对每帧图像，确定左目图像和右目图像中对应匹配点的归一化相关系数；
将左目图像和右目图像分别分成多个区域，对每个区域中的匹配点构造一个类进行存储；
在每个类中，对区域中的匹配点进行特征属性排序，并将特征属性排序第一的匹配点提取为表示该区域的特征点。
11.如权利要求10所述的系统，其中，所述归一化相关系数的计算公式如下：
Figure FDA0003098468190000061
其中，IR(x,y)、IT(x,y)分别表示左目图像和右目图像的像素灰度值，s表示视差，D表示匹配点所在的图像匹配块，
Figure FDA0003098468190000062
分别表示左目图像和右目图像在图像匹配块中像素灰度值的平均值。
12.如权利要求8所述的系统，其中，所述投影矩阵π(Pk+1)l和所述包含IMU预积分误差项的投影矩阵π(Pk+1)t计算方法如下：
Figure FDA0003098468190000063
Figure FDA0003098468190000064
Figure FDA0003098468190000065
Figure FDA0003098468190000066
Figure FDA0003098468190000067
其中，P表示界点，pk、pk+1分别表示点P在第k帧图像和第k+1帧图像的像素平面上的投影点，
Figure FDA0003098468190000068
δvk,k+1，δpk,k+1为IMU惯导估计的三个方向角度的变化量，K为IMU连续采样时的增益，Rk为k时刻相邻两个节点姿态旋转变化矩阵，Rk+1为k+1时刻相邻两个节点姿态旋转变化矩阵，tk为k时刻，Δtk,k+1为k+1时刻，g为重力加速度，vk为所述无人机移动的速度，
Figure FDA0003098468190000069
为基于i+1坐标的旋转变化率，
Figure FDA00030984681900000610
为坐标系变化的雅克比矩阵，
Figure FDA00030984681900000611
为离散高斯白噪声，Δt为时间变化率，
Figure FDA00030984681900000612
为a节点估计值，
Figure FDA00030984681900000613
为k时刻b节点相对于a节点的变化估计，
Figure FDA00030984681900000614
为基于i坐标的旋转变化率。
13.如权利要求8所述的系统，其中，所述基于每帧图像的跟踪特征点，对无人机进行初始位姿估计，得到初始位姿估计结果，并获取关键帧，包括：
基于每帧图像的跟踪特征点，进行三角剖分，确定各个跟踪特征点的位置关系；
根据各个跟踪特征点的3D坐标，通过EPnP方法对无人机进行初始位姿估计，得到初始位姿估计结果；
在每帧图像估计完成时，确定是否将当前帧定为关键帧，获取所有关键帧。
14.如权利要求8所述的系统，其中，所述后端检测模块被配置为：对每帧关键帧，确定最优状态量
Figure FDA0003098468190000071
其中，χk表示关键帧k的优化变量，
Figure FDA0003098468190000072
分别表示无人机坐标系在关键帧k处相对于世界坐标系的位移、速度和旋转，
Figure FDA0003098468190000073
分别表示陀螺仪和加速度计在关键帧k处的随机巡视；
对关键帧k中的VSLAM累积残差项、立体视觉重投影残差项和IMU预积分残差项进行优化，并形成约束条件：
Figure FDA0003098468190000074
其中，ES表示VSLAM累积残差项，EIMU表示IMU预积分残差项，EStereo表示立体视觉重投影残差项；
对约束条件求解最小残差项，得到优化的位姿估计结果。
CN202011079450.6A 2020-10-10 2020-10-10 一种无人机位姿估计方法及系统 Active CN112233177B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011079450.6A CN112233177B (zh) 2020-10-10 2020-10-10 一种无人机位姿估计方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011079450.6A CN112233177B (zh) 2020-10-10 2020-10-10 一种无人机位姿估计方法及系统

Publications (2)

Publication Number Publication Date
CN112233177A CN112233177A (zh) 2021-01-15
CN112233177B true CN112233177B (zh) 2021-07-30

Family

ID=74111843

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011079450.6A Active CN112233177B (zh) 2020-10-10 2020-10-10 一种无人机位姿估计方法及系统

Country Status (1)

Country Link
CN (1) CN112233177B (zh)

Families Citing this family (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113076883B (zh) * 2021-04-08 2022-05-06 西南石油大学 一种基于图像特征识别的井喷气体流速测量方法
CN113420590B (zh) * 2021-05-13 2022-12-06 北京航空航天大学 弱纹理环境下的机器人定位方法、装置、设备及介质
CN113379911A (zh) * 2021-06-30 2021-09-10 深圳市银星智能科技股份有限公司 Slam方法、slam系统及智能机器人
CN113674340A (zh) * 2021-07-05 2021-11-19 北京物资学院 一种基于路标点的双目视觉导航方法与装置
CN114170296B (zh) * 2021-11-10 2022-10-18 埃洛克航空科技（北京）有限公司 基于多模式综合决策的旋转平均估计方法以及装置
CN114821372A (zh) * 2022-05-12 2022-07-29 中山大学 基于单目视觉的无人机编队中个体相对位姿测量方法
CN115035157B (zh) * 2022-05-31 2024-07-12 广东天太机器人有限公司 一种基于视觉跟踪的agv运动控制方法、装置及介质
CN115272494B (zh) * 2022-09-29 2022-12-30 腾讯科技（深圳）有限公司 相机与惯性测量单元的标定方法、装置和计算机设备
CN115406447B (zh) * 2022-10-31 2023-03-24 南京理工大学 拒止环境下基于视觉惯性的四旋翼无人机自主定位方法
CN116612184B (zh) * 2023-04-11 2023-12-05 西南交通大学 一种基于监控场景的无人机相机位姿精确估算方法
CN117237230B (zh) * 2023-11-09 2024-03-22 武汉中观自动化科技有限公司 激光线及标志点识别方法及系统

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106846417A (zh) * 2017-02-06 2017-06-13 东华大学 基于视觉里程计的单目红外视频三维重建方法
CN108489482A (zh) * 2018-02-13 2018-09-04 视辰信息科技(上海)有限公司 视觉惯性里程计的实现方法及系统
CN109465832A (zh) * 2018-12-18 2019-03-15 哈尔滨工业大学（深圳） 高精度视觉和imu紧融合定位方法与系统

Family Cites Families (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108665540A (zh) * 2018-03-16 2018-10-16 浙江工业大学 基于双目视觉特征和imu信息的机器人定位与地图构建系统
CN109084732B (zh) * 2018-06-29 2021-01-12 北京旷视科技有限公司 定位与导航方法、装置及处理设备
CN110125928B (zh) * 2019-03-27 2021-04-06 浙江工业大学 一种基于前后帧进行特征匹配的双目惯导slam系统
CN111024078B (zh) * 2019-11-05 2021-03-16 广东工业大学 基于gpu加速的无人机视觉slam方法
CN111121767B (zh) * 2019-12-18 2023-06-30 南京理工大学 一种融合gps的机器人视觉惯导组合定位方法
CN111275763B (zh) * 2020-01-20 2023-10-13 深圳市普渡科技有限公司 闭环检测系统、多传感器融合slam系统及机器人
CN111508026A (zh) * 2020-04-17 2020-08-07 国网四川省电力公司电力科学研究院 一种视觉与imu融合的室内巡检机器人定位与地图构建方法

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106846417A (zh) * 2017-02-06 2017-06-13 东华大学 基于视觉里程计的单目红外视频三维重建方法
CN108489482A (zh) * 2018-02-13 2018-09-04 视辰信息科技(上海)有限公司 视觉惯性里程计的实现方法及系统
CN109465832A (zh) * 2018-12-18 2019-03-15 哈尔滨工业大学（深圳） 高精度视觉和imu紧融合定位方法与系统

Also Published As

Publication number Publication date
CN112233177A (zh) 2021-01-15

Similar Documents

Publication Publication Date Title
CN112233177B (zh) 一种无人机位姿估计方法及系统
CN109974693B (zh) 无人机定位方法、装置、计算机设备及存储介质
JP7326720B2 (ja) 移動体位置推定システムおよび移動体位置推定方法
CN104704384B (zh) 具体用于装置的基于视觉的定位的图像处理方法
CN111210477B (zh) 一种运动目标的定位方法及系统
CN110533722A (zh) 一种基于视觉词典的机器人快速重定位方法及系统
CN111462200A (zh) 一种跨视频行人定位追踪方法、系统及设备
CN111862201B (zh) 一种基于深度学习的空间非合作目标相对位姿估计方法
Muñoz-Bañón et al. Targetless camera-LiDAR calibration in unstructured environments
CN108682027A (zh) 基于点、线特征融合的vSLAM实现方法及系统
CN112634451A (zh) 一种融合多传感器的室外大场景三维建图方法
CN108648194B (zh) 基于cad模型三维目标识别分割和位姿测量方法及装置
CN109472828B (zh) 一种定位方法、装置、电子设备及计算机可读存储介质
CN110490900A (zh) 动态环境下的双目视觉定位方法及系统
Ding et al. Vehicle pose and shape estimation through multiple monocular vision
CN112734852A (zh) 一种机器人建图方法、装置及计算设备
CN108446710A (zh) 室内平面图快速重建方法及重建系统
CN109214254B (zh) 一种确定机器人位移的方法及装置
US10977810B2 (en) Camera motion estimation
CN116449384A (zh) 基于固态激光雷达的雷达惯性紧耦合定位建图方法
CN106846367B (zh) 一种基于运动约束光流法的复杂动态场景的运动物体检测方法
Zhang LILO: A novel LiDAR–IMU SLAM system with loop optimization
CN115861352A (zh) 单目视觉、imu和激光雷达的数据融合和边缘提取方法
CN113920254B (zh) 一种基于单目rgb的室内三维重建方法及其系统
Tsaregorodtsev et al. Extrinsic camera calibration with semantic segmentation

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