一种无人机位姿估计方法及系统与流程
本发明涉及无人机技术领域，具体而言，涉及一种无人机位姿估计方法及系统。
背景技术：
相关技术中，无人机在进行识别和定位时，大多通过无人机本体实现，其精度和可靠性都较低。利用单一模式传感器进行识别和定位，又无法全面、精准的构建定位地图，也无法获取无人机的准确位置信息。而利用人工标记点进行识别定位的方法，需要进行特征点不具和标定，过程较为繁琐。
技术实现要素：
本发明针对复杂工业场景中无人机的高实时性、高精度定位问题，提供一种无人机位姿估计方法及系统。
本发明提供了一种无人机位姿估计方法，所述方法包括：
前端检测：对搭载在无人机上的摄像机拍摄的双目立体图像提取每帧图像的特征点，对相邻帧图像进行特征匹配和特征跟踪，得到无人机的初始位姿估计结果，并获取关键帧；
后端检测：对所述关键帧进行选择，并基于选取的关键帧优化无人机的位姿和姿态图形；
回环检测：对选取出的关键帧，基于词袋模型计算图像之间的相似度，在相似度超过阈值时，计算相似变换并优化无人机的姿态图形；
其中，所述前端检测、所述后端检测和所述回环检测并行进行。
作为本发明进一步的改进，所述对搭载在无人机上的摄像机拍摄的双目立体图像提取每帧图像的特征点，对相邻帧图像进行特征匹配和特征跟踪，得到无人机的初始位姿估计结果，并获取关键帧，包括：
搭载在对无人机上的摄像机拍摄的双目立体图像进行校正；
对校正后的左目图像和右目图像分别提取出每帧图像的shio-tomasi角点；
对相邻帧图像提取出的角点进行循环匹配，初步筛选出每帧图像的匹配点；
对初步筛选出的匹配点进行过滤，确定每帧图像的特征点；
基于imu不确定性模型对每帧图像的特征点进行特征跟踪，确定每帧图像的跟踪特征点；
基于每帧图像的跟踪特征点，对无人机进行初始位姿估计，得到初始位姿估计结果，并获取关键帧。
作为本发明进一步的改进，所述对相邻帧图像提取出的角点进行循环匹配，初步筛选出每帧图像的匹配点，包括：
确定匹配顺序，其中，所述匹配顺序为第k-1帧左目图像
按照所述匹配顺序，依次从每幅图像中筛选出匹配点；
在所述匹配顺序中，如果下一幅图像中的角点与上一幅图像中的角点相同，则保留该角点，如果下一幅图像中的角点与上一幅图像中的角点不相同，则剔除该角点，依次类推，得到每帧图像的匹配点。
作为本发明进一步的改进，所述对初步筛选出的匹配点进行过滤，确定每帧图像的特征点，包括：
对每帧图像，确定左目图像和右目图像中对应匹配点的归一化相关系数；
将左目图像和右目图像分别分成多个区域，对每个区域中的匹配点构造一个类进行存储；
在每个类中，对区域中的匹配点进行特征属性排序，并将特征属性排序第一的匹配点提取为表示该区域的特征点。
作为本发明进一步的改进，所述归一化相关系数的计算公式如下：
其中，ir(x,y)、it(x,y)分别表示左目图像和右目图像的像素灰度值，s表示视差，d表示匹配点所在的图像匹配块，
作为本发明进一步的改进，所述基于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：
将极线lk+1作为原始的特征点搜索区域，并在垂直方向上分别向上和向下延伸±σnεe个像素，形成最终的特征点搜索区域，其中，εe表示原始的特征点搜索区域中的单位像素。
作为本发明进一步的改进，所述投影矩阵π(pk+1)l和所述包含imu预积分误差项的投影矩阵π(pk+1)t计算方法如下：
其中，p表示界点，pk、pk+1分别表示点p在第k帧图像和第k+1帧图像的像素平面上的投影点，
作为本发明进一步的改进，所述基于每帧图像的跟踪特征点，对无人机进行初始位姿估计，得到初始位姿估计结果，并获取关键帧，包括：
基于每帧图像的跟踪特征点，进行三角剖分，确定各个跟踪特征点的位置关系；
根据各个跟踪特征点的3d坐标，通过epnp方法对无人机进行初始位姿估计，得到初始位姿估计结果；
在每帧图像估计完成时，确定是否将当前帧定为关键帧，获取所有关键帧。
作为本发明进一步的改进，对所述关键帧进行选择，并基于选取的关键帧优化无人机的位姿和姿态图形，包括：
对每帧关键帧，确定最优状态量
对关键帧k中的vslam累积残差项、立体视觉重投影残差项和imu预积分残差项进行优化，并形成约束条件：
其中，es表示vslam累积残差项，eimu表示imu预积分残差项，estereo表示立体视觉重投影残差项；
对约束条件求解最小残差项，得到优化的位姿估计结果。
本发明还提供了一种无人机位姿估计系统，所述系统包括：
前端检测模块，对搭载在无人机上的摄像机拍摄的双目立体图像提取每帧图像的特征点，对相邻帧图像进行特征匹配和特征跟踪，得到无人机的初始位姿估计结果，并获取关键帧；
后端检测模块，对所述关键帧进行选择，并基于选取的关键帧优化无人机的位姿和姿态图形；
回环检测模块，对选取出的关键帧，基于词袋模型计算图像之间的相似度，在相似度超过阈值时，计算相似变换并优化无人机的姿态图形；
其中，所述前端检测模块、所述后端检测模块和所述回环检测模块并行处理。
作为本发明进一步的改进，所述前端检测模块被配置为：
搭载在对无人机上的摄像机拍摄的双目立体图像进行校正；
对校正后的左目图像和右目图像分别提取出每帧图像的shio-tomasi角点；
对相邻帧图像提取出的角点进行循环匹配，初步筛选出每帧图像的匹配点；
对初步筛选出的匹配点进行过滤，确定每帧图像的特征点；
基于imu不确定性模型对每帧图像的特征点进行特征跟踪，确定每帧图像的跟踪特征点；
基于每帧图像的跟踪特征点，对无人机进行初始位姿估计，得到初始位姿估计结果，并获取关键帧。
作为本发明进一步的改进，所述对相邻帧图像提取出的角点进行循环匹配，初步筛选出每帧图像的匹配点，包括：
确定匹配顺序，其中，所述匹配顺序为第k-1帧左目图像
按照所述匹配顺序，依次从每幅图像中筛选出匹配点；
在所述匹配顺序中，如果下一幅图像中的角点与上一幅图像中的角点相同，则保留该角点，如果下一幅图像中的角点与上一幅图像中的角点不相同，则剔除该角点，依次类推，得到每帧图像的匹配点。
作为本发明进一步的改进，所述对初步筛选出的匹配点进行过滤，确定每帧图像的特征点，包括：
对每帧图像，确定左目图像和右目图像中对应匹配点的归一化相关系数；
将左目图像和右目图像分别分成多个区域，对每个区域中的匹配点构造一个类进行存储；
在每个类中，对区域中的匹配点进行特征属性排序，并将特征属性排序第一的匹配点提取为表示该区域的特征点。
作为本发明进一步的改进，所述归一化相关系数的计算公式如下：
其中，ir(x,y)、it(x,y)分别表示左目图像和右目图像的像素灰度值，s表示视差，d表示匹配点所在的图像匹配块，
作为本发明进一步的改进，所述基于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：
将极线lk+1作为原始的特征点搜索区域，并在垂直方向上分别向上和向下延伸±σnεe个像素，形成最终的特征点搜索区域，其中，εe表示原始的特征点搜索区域中的单位像素。
作为本发明进一步的改进，所述投影矩阵π(pk+1)l和所述包含imu预积分误差项的投影矩阵π(pk+1)t计算方法如下：
其中，p表示界点，pk、pk+1分别表示点p在第k帧图像和第k+1帧图像的像素平面上的投影点，
作为本发明进一步的改进，所述基于每帧图像的跟踪特征点，对无人机进行初始位姿估计，得到初始位姿估计结果，并获取关键帧，包括：
基于每帧图像的跟踪特征点，进行三角剖分，确定各个跟踪特征点的位置关系；
根据各个跟踪特征点的3d坐标，通过epnp方法对无人机进行初始位姿估计，得到初始位姿估计结果；
在每帧图像估计完成时，确定是否将当前帧定为关键帧，获取所有关键帧。
作为本发明进一步的改进，所述后端检测模块被配置为：对每帧关键帧，确定最优状态量
对关键帧k中的vslam累积残差项、立体视觉重投影残差项和imu预积分残差项进行优化，并形成约束条件：
其中，es表示vslam累积残差项，eimu表示imu预积分残差项，estereo表示立体视觉重投影残差项；
对约束条件求解最小残差项，得到优化的位姿估计结果。
本发明的有益效果为：
本发明所述的方法可实现非结构化复杂环境中无人机高实时性、高精度的定位。前端采用循环匹配和特征筛选来优化特征点匹配，使特征匹配点更加准确、稳定；在图像的特征跟踪，采用imu不确定性模型对像素可能发生变化的区域进行约束和预测，以此来确定匹配区域，缩短特征匹配搜索时间。后端融合先验残差(vslam累积残差)、imu预积分残差和立体视觉残差，作为约束条件，减少系统累积误差，提高精度，得到全局一致的地图。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍。显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
图1为本发明一示例性实施例所述的一种无人机位姿估计方法的流程示意图；
图2为本发明一示例性实施例所述的循环匹配原理示意图；
图3为本发明一示例性实施例所述的极线搜索原理示意图；
图4为本发明一示例性实施例所述的特征搜索区域原理示意图；
图5为本发明一示例性实施例所述的在前端检测线程上优化的特征跟踪和三角深度测量结果；
图6为本发明一示例性实施例所述的无人机运动轨迹估计的结果示意图；
图7为本发明一示例性实施例所述的绝对位置误差(ape)分析的结果示意图；
图8为本发明一示例性实施例所述的绝对位置误差(ape)的直方图；
图9为本发明一示例性实施例所述的无人机位置估计误差；
图10为本发明一示例性实施例所述的无人机姿态估计误差。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
需要说明，若本发明实施例中有涉及方向性指示(诸如上、下、左、右、前、后……)，则该方向性指示仅用于解释在某一特定姿态(如附图所示)下各部件之间的相对位置关系、运动情况等，如果该特定姿态发生改变时，则该方向性指示也相应地随之改变。
另外，在本发明的描述中，所用术语仅用于说明目的，并非旨在限制本发明的范围。术语“包括”和/或“包含”用于指定所述元件、步骤、操作和/或组件的存在，但并不排除存在或添加一个或多个其他元件、步骤、操作和/或组件的情况。术语“第一”、“第二”等可能用于描述各种元件，不代表顺序，且不对这些元件起限定作用。此外，在本发明的描述中，除非另有说明，“多个”的含义是两个及两个以上。这些术语仅用于区分一个元素和另一个元素。结合以下附图，这些和/或其他方面变得显而易见，并且，本领域普通技术人员更容易理解关于本发明所述实施例的说明。附图仅出于说明的目的用来描绘本发明所述实施例。本领域技术人员将很容易地从以下说明中认识到，在不背离本发明所述原理的情况下，可以采用本发明所示结构和方法的替代实施例。
本发明实施例所述的一种无人机位姿估计方法，如图1所示，所述方法包括：
前端检测：对搭载在无人机上的摄像机拍摄的双目立体图像提取每帧图像的特征点，对相邻帧图像进行特征匹配和特征跟踪，得到无人机的初始位姿估计结果，并获取关键帧；
后端检测：对所述关键帧进行选择，并基于选取的关键帧优化无人机的位姿和姿态图形；
回环检测：对选取出的关键帧，基于词袋模型计算图像之间的相似度，在相似度超过阈值时，计算相似变换并优化无人机的姿态图形；
其中，所述前端检测、所述后端检测和所述回环检测并行进行。
本发明所述的方法，分为三个部分(前端检测、后端检测和回环检测)，每个部分独立设置一个线程，保证所述前端检测、所述后端检测和所述回环检测的并行操作。
在一种可选的实施方式中，所述对搭载在无人机上的摄像机拍摄的双目立体图像提取每帧图像的特征点，对相邻帧图像进行特征匹配和特征跟踪，得到无人机的初始位姿估计结果，并获取关键帧，包括：
s1，搭载在对无人机上的摄像机拍摄的双目立体图像进行校正；
s2，对校正后的左目图像和右目图像分别提取出每帧图像的shio-tomasi角点；
s3，对相邻帧图像提取出的角点进行循环匹配，初步筛选出每帧图像的匹配点；
s4，对初步筛选出的匹配点进行过滤，确定每帧图像的特征点；
s5，基于imu不确定性模型对每帧图像的特征点进行特征跟踪，确定每帧图像的跟踪特征点；
s6，基于每帧图像的跟踪特征点，对无人机进行初始位姿估计，得到初始位姿估计结果，并获取关键帧。
本发明在前端检测输入双目立体图像和imu(惯性测量单元)姿态测量信息。对双目立体图像进行校正，通过标定的摄像机内部参数中的畸变信息去除摄像机成像失真，使左目图像和右目图像的光心位于同一水平线上，便于循环匹配和特征跟踪。本发明对左目图像和右目图像提取shio-tomasi角点，以适合于图像的特征跟踪。
在一种可选的实施方式中，所述对相邻帧图像提取出的角点进行循环匹配，初步筛选出每帧图像的匹配点，包括：
s31，确定匹配顺序，其中，所述匹配顺序为第k-1帧左目图像
s32，按照所述匹配顺序，依次从每幅图像中筛选出匹配点；
s33，在所述匹配顺序中，如果下一幅图像中的角点与上一幅图像中的角点相同，则保留该角点，如果下一幅图像中的角点与上一幅图像中的角点不相同，则剔除该角点，依次类推，得到每帧图像的匹配点。
本发明所述的方法在循环匹配时，如图2所示，假设第k-1帧、第k帧两个相邻帧的左目图像和右目图像分别为
在一种可选的实施方式中，所述对初步筛选出的匹配点进行过滤，确定每帧图像的特征点，包括：
s41，对每帧图像，确定左目图像和右目图像中对应匹配点的归一化相关系数；
其中，所述归一化相关系数的计算公式如下：
其中，ir(x,y)、it(x,y)分别表示左目图像和右目图像的像素灰度值，s表示视差，d表示匹配点所在的图像匹配块，
s42，将左目图像和右目图像分别分成多个区域(例如划分成像素大小为50*50的区域)，对每个区域中的匹配点构造一个类进行存储；
s43，在每个类中，对区域中的匹配点进行特征属性排序，并将特征属性排序第一的匹配点提取为表示该区域(例如该50*50区域)的特征点。
其中，例如可以采用比较函数进行特征属性排序。所述比较函数例如为：
其中，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计算方法如下：
其中，p表示界点，pk、pk+1分别表示点p在第k帧图像和第k+1帧图像的像素平面上的投影点，
s54，将imu预积分的不确定性模型集成到极线搜索中，确定不确定系数σn：
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算法即可求出相机位姿，得到无人机的位姿。
在一种可选的实施方式中，对所述关键帧进行选择，并基于选取的关键帧优化无人机的位姿和姿态图形，包括：
对每帧关键帧，确定最优状态量
对关键帧k中的vslam累积残差项、立体视觉重投影残差项和imu预积分残差项进行优化，并形成约束条件：
其中，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，所述的方法具有较高的轨迹估计精度。
本发明实施例所述的一种无人机位姿估计系统，所述系统包括：
前端检测模块，对搭载在无人机上的摄像机拍摄的双目立体图像提取每帧图像的特征点，对相邻帧图像进行特征匹配和特征跟踪，得到无人机的初始位姿估计结果，并获取关键帧；
后端检测模块，对所述关键帧进行选择，并基于选取的关键帧优化无人机的位姿和姿态图形；
回环检测模块，对选取出的关键帧，基于词袋模型计算图像之间的相似度，在相似度超过阈值时，计算相似变换并优化无人机的姿态图形；
其中，所述前端检测模块、所述后端检测模块和所述回环检测模块各设置一个线程，三个模块并行处理。
在一种可选的实施方式中，所述前端检测模块被配置为：
搭载在对无人机上的摄像机拍摄的双目立体图像进行校正；
对校正后的左目图像和右目图像分别提取出每帧图像的shio-tomasi角点；
对相邻帧图像提取出的角点进行循环匹配，初步筛选出每帧图像的匹配点；
对初步筛选出的匹配点进行过滤，确定每帧图像的特征点；
基于imu不确定性模型对每帧图像的特征点进行特征跟踪，确定每帧图像的跟踪特征点；
基于每帧图像的跟踪特征点，对无人机进行初始位姿估计，得到初始位姿估计结果，并获取关键帧。
本发明在前端检测输入双目立体图像和imu(惯性测量单元)姿态测量信息。对双目立体图像进行校正，通过标定的摄像机内部参数中的畸变信息去除摄像机成像失真，使左目图像和右目图像的光心位于同一水平线上，便于循环匹配和特征跟踪。本发明对左目图像和右目图像提取shio-tomasi角点，以适合于图像的特征跟踪。
在一种可选的实施方式中，所述对相邻帧图像提取出的角点进行循环匹配，初步筛选出每帧图像的匹配点，包括：
确定匹配顺序，其中，所述匹配顺序为第k-1帧左目图像
按照所述匹配顺序，依次从每幅图像中筛选出匹配点；
在所述匹配顺序中，如果下一幅图像中的角点与上一幅图像中的角点相同，则保留该角点，如果下一幅图像中的角点与上一幅图像中的角点不相同，则剔除该角点，依次类推，得到每帧图像的匹配点。
本发明所述的系统在循环匹配时，如图2所示，假设第k-1帧、第k帧两个相邻帧的左目图像和右目图像分别为
在一种可选的实施方式中，所述对初步筛选出的匹配点进行过滤，确定每帧图像的特征点，包括：
对每帧图像，确定左目图像和右目图像中对应匹配点的归一化相关系数；
其中，所述归一化相关系数的计算公式如下：
其中，ir(x,y)、it(x,y)分别表示左目图像和右目图像的像素灰度值，s表示视差，d表示匹配点所在的图像匹配块，
将左目图像和右目图像分别分成多个区域(例如划分成像素大小为50*50的区域)，对每个区域中的匹配点构造一个类进行存储；
在每个类中，对区域中的匹配点进行特征属性排序，并将特征属性排序第一的匹配点提取为表示该区域(例如该50*50区域)的特征点。
其中，例如可以采用比较函数进行特征属性排序。所述比较函数例如为：
其中，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计算方法如下：
其中，p表示界点，pk、pk+1分别表示点p在第k帧图像和第k+1帧图像的像素平面上的投影点，
将imu预积分的不确定性模型集成到极线搜索中，确定不确定系数σn：
将极线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算法即可求出相机位姿，得到无人机的位姿。
在一种可选的实施方式中，所述后端检测模块被配置为：
对每帧关键帧，确定最优状态量
对关键帧k中的vslam累积残差项、立体视觉重投影残差项和imu预积分残差项进行优化，并形成约束条件：
其中，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，所述的系统具有较高的轨迹估计精度。
在此处所提供的说明书中，说明了大量具体细节。然而，能够理解，本发明的实施例可以在没有这些具体细节的情况下实践。在一些实例中，并未详细示出公知的方法、结构和技术，以便不模糊对本说明书的理解。
此外，本领域普通技术人员能够理解，尽管在此所述的一些实施例包括其它实施例中所包括的某些特征而不是其它特征，但是不同实施例的特征的组合意味着处于本发明的范围之内并且形成不同的实施例。例如，在权利要求书中，所要求保护的实施例的任意之一都可以以任意的组合方式来使用。
本领域技术人员应理解，尽管已经参考示例性实施例描述了本发明，但是在不脱离本发明的范围的情况下，可进行各种改变并可用等同物替换其元件。另外，在不脱离本发明的实质范围的情况下，可进行许多修改以使特定情况或材料适应本发明的教导。因此，本发明不限于所公开的特定实施例，而是本发明将包括落入所附权利要求范围内的所有实施例。
- 还没有人留言评论。精彩留言会获得点赞！