CN109903336A - 基于局部特征的飞行器姿态跨视场估计方法及装置 - Google Patents

Abstract

本发明公开了一种基于局部特征的飞行器姿态跨视场估计方法及装置，对输入的RGB图像进行Tag识别，检测到Tag后，识别其位置，进行飞行器中心定位，基于深度信息，得到飞行器中心位置坐标。接着计算最大帧得分选择最佳输入图像，然后对输入的RGB图像提取特征点，并且结合深度图将特征点转化为三维点。使用光束平差法求得相机姿态后，再对相机姿态进行转换。最后，初始相机与飞行器之间的转换矩阵，将相机姿态转化为飞行器姿态，并将飞行器姿态转化为六自由度姿态输出。本发明满足了飞行器姿态估计的实时性需求，实时性好而且精度高。

Description

基于局部特征的飞行器姿态跨视场估计方法及装置

技术领域

本发明涉及计算机视觉技术领域，尤其涉及一种基于局部特征的飞行器姿态跨视场估计方法及装置。

背景技术

在计算机视觉的工作中，飞行器姿态估计可被视作一项物体姿态估计的任务。物体姿态识别任务在计算机视觉中已经得到了广泛的研究。传统的物体姿态测量方法主要可分为模板匹配与特征匹配两种。模板匹配的方法通常应用于弱纹理场景。这种方法首先需要对三维物体进行建模，然后通过将真实场景与三维模型做匹配来寻找最佳姿态。比如经典的ICP算法与RANSAC算法就是通过最小化真实场景与模型之间的对应点的距离来求解当前姿态。特征匹配的方法，通常使用对象描述子进行识别，描述子又有全局和局部描述子两种。全局描述子是对物体整体状态的描述，通过描述子的匹配就可以得到当前姿态。而局部描述子则是基于点的匹配，计算得到当前物体相对于初始状态的位姿。也有一部分研究者认为在多数的计算机视觉应用场景中，由于基于特征或者模板的识别方法都有可能失效，对象的轮廓才是最可靠的信息。

在风洞场景中，对自由飞的飞行器进行实时姿态估计需要克服很多难题。首先，飞行器在自由飞状态下很容易飞离单个相机的视场范围，因而需要多台相机协作，覆盖更大的视场。其次，在高动态的飞行状态下，模板和轮廓匹配的方法很难达到实时性需求，而且精度不高。

发明内容

本发明的目的是提供一种基于局部特征的飞行器姿态跨视场估计方法及装置，来解决跨视场问题并实时估计飞行器的姿态，该方法仅需获取多个相机的RGB-D输入，通过飞行器与初始相机的相对位置关系，就可以将相机的相对姿态转化为飞行器的相对姿态并且输出。

为了实现上述目的，本发明技术方案如下：

一种基于局部特征的飞行器姿态跨视场估计方法，所述飞行器上设置有对称的标签，在初始时飞行器位于初始相机的视场范围内，所述基于局部特征的飞行器姿态跨视场估计方法，包括：

对场景内的所有深度相机进行标定，得到所有深度相机的内参、及各个深度相机相对于初始相机的转换矩阵；

采用初始相机的视频图像作为输入，进行标签识别，根据标签在图像坐标系中的位置确定飞行器与初始相机的相对位置关系；

以所有深度相机的视频图像作为输入，进行跨视场相机选取，将选取的深度相机的视频图像作为当前输入帧；

对当前输入帧进行特征点提取，计算当前相机姿态；

将当前相机姿态转换为初始相机姿态；

根据飞行器与初始相机的相对位置关系，将相机姿态转化为飞行器姿态。

进一步地，所述以所有深度相机的视频图像作为输入，进行跨视场相机选取，包括：

对于所有深度相机集合N中的深度相机n，相应的有RGB图像和深度图前溯M帧，计算每帧时，各个深度相机的帧得分，帧得分表达为：

其中，

p[(x,y)是深度图在(x,y)位置满足深度阈值H的深度得分，表示深度图在(x,y)位置的深度值，h和w分别是当前帧的高和宽；

将每帧最大帧得分对应的深度相机，放入一个集合

根据最大帧得分从深度相机集合N中进行选择最佳输入相机n_i：

其中ψ(n)是一个比例计算函数，代表着深度相机n在集合中所占的比例。

进一步地，所述将当前相机姿态转换为初始相机姿态，包括：

所述将当前相机姿态转换为初始相机姿态，根据如下公式进行转换：

{R,t}＝T_ctc{R′,t′}

其中，T_ctc为切换前深度相机到切换后深度相机的变换矩阵，{R,t}为初始相机姿态，{R′,t′}为当前相机姿态。

进一步地，所述根据飞行器与初始相机的相对位置关系，将相机姿态转化为飞行器姿态，转换公式如下：

T_p＝T_c×T-t_c

其中，T为飞行器与初始相机的相对位置关系，T_c为前一步骤计算的到的相机姿态，T_p为飞行器姿态，t_c为飞行器中心位置。

本发明还提出了一种基于局部特征的飞行器姿态跨视场估计装置，所述飞行器上设置有对称的标签，在初始时飞行器位于初始相机的视场范围内，所述基于局部特征的飞行器姿态跨视场估计装置，包括：

标定模块，用于对场景内的所有深度相机进行标定，得到所有深度相机的内参、及各个深度相机相对于初始相机的转换矩阵；

标签识别模块，用于采用初始相机的视频图像作为输入，进行标签识别，根据标签在图像坐标系中的位置确定飞行器与初始相机的相对位置关系；

相机选取模块，用于以所有深度相机的视频图像作为输入，进行跨视场相机选取，将选取的深度相机的视频图像作为当前输入帧；

特征提取模块，用于对当前输入帧进行特征点提取，计算当前相机姿态；

姿态转换模块，用于将当前相机姿态转换为初始相机姿态，根据飞行器与初始相机的相对位置关系，将相机姿态转化为飞行器姿态。

进一步地，所述相机选取模块以所有深度相机的视频图像作为输入，进行跨视场相机选取，执行如下操作：

对于所有深度相机集合N中的深度相机n，相应的有RGB图像和深度图前溯M帧，计算每帧时，各个深度相机的帧得分，帧得分表达为：

其中，

p(x,y)是深度图在(x,y)位置满足深度阈值H的深度得分，表示深度图在(x,y)位置的深度值，h和w分别是当前帧的高和宽；

将每帧最大帧得分对应的深度相机，放入一个集合

根据最大帧得分从深度相机集合N中进行选择最佳输入相机n_i：

其中ψ(n)是一个比例计算函数，代表着深度相机n在集合中所占的比例。

进一步地，所述姿态转换模块将当前相机姿态转换为初始相机姿态，执行如下操作：

所述将当前相机姿态转换为初始相机姿态，根据如下公式进行转换：

{R,t}＝T_ctc{R′,t′}

其中，T_ctc为切换前深度相机到切换后深度相机的变换矩阵，{R,t}为初始相机姿态，{R′,t′}为当前相机姿态。

进一步地，所述姿态转换模块根据飞行器与初始相机的相对位置关系，将相机姿态转化为飞行器姿态，转换公式如下：

T_p＝T_c×T-t_c

其中，T为飞行器与初始相机的相对位置关系，T_c为前一步骤计算的到的相机姿态，T_p为飞行器姿态，t_c为飞行器中心位置。

本发明提出了一种基于局部特征的飞行器姿态跨视场估计方法及装置，对输入的RGB图像进行Tag识别，检测到Tag后，识别其位置，进行飞行器中心定位，基于深度信息，得到飞行器中心位置坐标。接着计算最大帧得分选择最佳输入图像，然后对输入的RGB图像提取特征点，并且结合深度图将特征点转化为三维点。使用光束平差法求得相机姿态后，再对相机姿态进行转换。最后，初始相机与飞行器之间的转换矩阵，将相机姿态转化为飞行器姿态，并将飞行器姿态转化为六自由度姿态输出。本发明满足了飞行器姿态估计的实时性需求，而且精度高。

附图说明

图1为本发明基于局部特征的飞行器姿态跨视场估计方法流程图；

图2本发明实施例的应用场景示意图；

图3为本发明标签示意图。

具体实施方式

下面结合附图和实施例对本发明技术方案做进一步详细说明，以下实施例不构成对本发明的限定。

如图1所示，本发明一种基于局部特征的飞行器姿态跨视场估计方法，所述飞行器上设置有对称的标签，在初始时飞行器位于初始相机的视场范围内，所述基于局部特征的飞行器姿态跨视场估计方法，包括：

步骤S1、对场景内的所有深度相机进行标定，得到所有深度相机的内参、及各个深度相机相对于初始相机的转换矩阵。

在本实施例的应用场景中，如图2所示，设置多台从下往上拍摄的深度相机，飞行器悬置于深度相机上空，飞行器的左右翼对称处，各放置一个大小已知且尺寸相同的标签Tag，如图3所示。选定任意一个相机作为初始相机，并让飞行器一开始完全处于初始相机的视场范围内。

通过对场景内的所有深度相机进行标定，可以得到所有深度相机的内参、及各个深度相机相对于初始相机的转换矩阵，关于标定的具体方法，在本领域已经为比较成熟的技术，这里不再赘述。

步骤S2、采用初始相机的视频图像作为输入，进行标签识别，根据标签在图像坐标系中的位置确定飞行器与初始相机的相对位置关系。

本实施例使用AprilTag提出的tag检测器来识别tag，并不限于具体的标签识别方法，本领域其他的标签识别方法同样适用。将初始相机的RGB图像作为输入，首先将RGB图像转化为灰度图，并且采用自适应阈值的方法，将灰度图像阈值化为黑白图像。然后根据黑白分量的特性对边缘进行分割，从中找到可能形成tag边界的边缘。最后，通过搜索少量角点来计算近似分区，然后迭代所有可能的角点组合以找到所有拟合四边形。经过上述步骤，就能得到tag在图像坐标系中的位置，两个tag的中心就代表着飞行器的中心位置。

具体地，使用公式(1)将飞行器中心在图像坐标系的坐标(u，v)以及对应的深度值d转化为三维坐标，就能得到飞行器与初始相机的相对位置关系。

其中(f_x,f_y)是初始相机的焦距，(u₀,v₀)是初始相机的主点坐标，都可以通过相机标定获得。(X_c,Y_c,Z_c)是飞行器中心的三维坐标，从而确定了飞行器与初始相机的相对位置关系。其中主点坐标是相机主视线与透视面的交点，是本领域的公知常识，这里不再赘述。

步骤S3、以所有深度相机的视频图像作为输入，进行跨视场相机选取，将选取的深度相机的视频图像作为当前输入帧。

本步骤在时间i，基于所有深度相机的RGB-D数据，选择最佳的输入相机n_i。

对于所有深度相机集合N中的深度相机n，相应的有RGB图像和深度图前溯M帧，计算每帧时，各个深度相机的帧得分，帧得分可以表达为

其中，

p(x,y)是深度图在(x,y)位置满足深度阈值H的深度得分。表示深度图在(x,y)位置的深度值。h和w分别是当前帧的高和宽。将每帧最大帧得分对应的深度相机，放入集合最佳输入相机n_i是根据最大帧得分从深度相机集合N中进行选择的。

其中ψ(n)是一个比例计算函数，代表着深度相机n在集合中所占的比例。得到最佳输入深度相机后，将该深度相机获得到的图像作为当前输入帧。

步骤S4、对当前输入帧进行特征点提取，计算当前相机姿态。

本实施例对当前输入帧提取ORB描述子，使用公式(1)将特征点的二维坐标转化为三维坐标。

在得到特征点之后，使用光束法平差，最小化三维点Xⁱ与其对应二维点之间的重投影误差来估计相机相对于前一帧的瞬时姿态{R′,t′}。

其中ρ是Huber能量函数，

Σ是与特征点的尺度相关的协方差矩阵。投影函数φ的定义如下：

其中(f_x,f_y)是焦距，(u₀,v₀)是相机的主点。(X,Y,Z)为空间点的三维坐标。

步骤S5、将当前相机姿态转换为初始相机姿态。

如果步骤S3选取到的深度相机与上一帧的深度相机编号不同，说明相机发生切换，需要使用步骤S1标定得到的相机之间的转换矩阵，将相机姿态进行如下转换，将当前相机姿态转换为初始相机姿态。

{R,t}＝T_ctc{R′,t′} (8)

其中，T_ctc为切换前相机到切换后相机的变换矩阵。

从而将当前相机姿态{R′,t′}转换为初始相机姿态{R,t}；如果相机未发生切换，则不需要进行转换。

步骤S6、根据飞行器与初始相机的相对位置关系，将相机姿态转化为飞行器姿态。

使用飞行器与初始相机的相对位置关系，将相机姿态转化为飞行器姿态。使用光束平差法得到相机姿态后，使用步骤S2得到的飞行器中心位置(X_c,Y_c,Z_c)(记为t_c)作为平移矩阵，3乘3的单位矩阵E作为旋转矩阵，组合得到飞行器与初始相机的相对位置关系T，将步骤S5求得的相机姿态{R,t}记为T_c那么相机姿态与飞行器姿态T_p的转化方法为：

T_p＝T_c×T-t_c (9)

其中，

得到飞行器姿态后，将姿态转化为六自由度进行输出。

在一个实施例中，还提供了一种基于局部特征的飞行器姿态跨视场估计装置，所述飞行器上设置有对称的标签，在初始时飞行器位于初始相机的视场范围内，所述基于局部特征的飞行器姿态跨视场估计装置，包括：

标定模块，用于对场景内的所有深度相机进行标定，得到所有深度相机的内参、及各个深度相机相对于初始相机的转换矩阵；

标签识别模块，用于采用初始相机的视频图像作为输入，进行标签识别，根据标签在图像坐标系中的位置确定飞行器与初始相机的相对位置关系；

相机选取模块，用于以所有深度相机的视频图像作为输入，进行跨视场相机选取，将选取的深度相机的视频图像作为当前输入帧；

特征提取模块，用于对当前输入帧进行特征点提取，计算当前相机姿态；

姿态转换模块，用于将当前相机姿态转换为初始相机姿态，根据飞行器与初始相机的相对位置关系，将相机姿态转化为飞行器姿态。

进一步地，所述相机选取模块以所有深度相机的视频图像作为输入，进行跨视场相机选取，执行如下操作：

对于所有深度相机集合N中的深度相机n，相应的有RGB图像和深度图前溯M帧，计算每帧时，各个深度相机的帧得分，帧得分表达为：

其中，

p(x,y)是深度图在(x,y)位置满足深度阈值H的深度得分，表示深度图在(x,y)位置的深度值，h和w分别是当前帧的高和宽；

将每帧最大帧得分对应的深度相机，放入一个集合

根据最大帧得分从深度相机集合N中进行选择最佳输入相机n_i：

其中ψ(n)是一个比例计算函数，代表着深度相机n在集合中所占的比例。

进一步地，所述姿态转换模块将当前相机姿态转换为初始相机姿态，执行如下操作：

所述将当前相机姿态转换为初始相机姿态，根据如下公式进行转换：

{R,t}＝T_ctc{R′,t′}

其中，T_ctc为切换前深度相机到切换后深度相机的变换矩阵，{R,t}为初始相机姿态，{R′,t′}为当前相机姿态。

进一步地，所述姿态转换模块根据飞行器与初始相机的相对位置关系，将相机姿态转化为飞行器姿态，转换公式如下：

T_p＝T_c×T-t_c

其中，T为飞行器与初始相机的相对位置关系，T_c为前一步骤计算的到的相机姿态，T_p为飞行器姿态，t_c为飞行器中心位置。

关于基于局部特征的飞行器姿态跨视场估计装置的具体限定可以参见上文中对于基于局部特征的飞行器姿态跨视场估计方法的限定，在此不再赘述。上述基于局部特征的飞行器姿态跨视场估计装置中的各个模块可全部或部分通过软件、硬件及其组合来实现。上述各模块可以硬件形式内嵌于或独立于计算机设备中的处理器中，也可以以软件形式存储于计算机设备中的存储器中，以便于处理器调用执行以上各个模块对应的操作。

以上实施例仅用以说明本发明的技术方案而非对其进行限制，在不背离本发明精神及其实质的情况下，熟悉本领域的技术人员当可根据本发明作出各种相应的改变和变形，但这些相应的改变和变形都应属于本发明所附的权利要求的保护范围。

Claims (8)

1.一种基于局部特征的飞行器姿态跨视场估计方法，所述飞行器上设置有对称的标签，在初始时飞行器位于初始相机的视场范围内，其特征在于，所述基于局部特征的飞行器姿态跨视场估计方法，包括：

对场景内的所有深度相机进行标定，得到所有深度相机的内参、及各个深度相机相对于初始相机的转换矩阵；

采用初始相机的视频图像作为输入，进行标签识别，根据标签在图像坐标系中的位置确定飞行器与初始相机的相对位置关系；

以所有深度相机的视频图像作为输入，进行跨视场相机选取，将选取的深度相机的视频图像作为当前输入帧；

对当前输入帧进行特征点提取，计算当前相机姿态；

将当前相机姿态转换为初始相机姿态；

根据飞行器与初始相机的相对位置关系，将相机姿态转化为飞行器姿态。

2.如权利要求1所述的基于局部特征的飞行器姿态跨视场估计方法，其特征在于，所述以所有深度相机的视频图像作为输入，进行跨视场相机选取，包括：

对于所有深度相机集合N中的深度相机n，相应的有RGB图像f_i ⁿ和深度图前溯M帧，计算每帧时，各个深度相机的帧得分，帧得分表达为：

其中，

p(x,y)是深度图在(x,y)位置满足深度阈值H的深度得分，表示深度图在(x,y)位置的深度值，h和w分别是当前帧的高和宽；

将每帧最大帧得分对应的深度相机，放入一个集合

根据最大帧得分从深度相机集合N中进行选择最佳输入相机n_i：

其中ψ(n)是一个比例计算函数，代表着深度相机n在集合中所占的比例。

3.如权利要求1所述的基于局部特征的飞行器姿态跨视场估计方法，其特征在于，所述将当前相机姿态转换为初始相机姿态，包括：

所述将当前相机姿态转换为初始相机姿态，根据如下公式进行转换：

{R,t)＝T_ctc{R′,t′}

其中，T_ctc为切换前深度相机到切换后深度相机的变换矩阵，{R,t}为初始相机姿态，{R′,t′}为当前相机姿态。

4.如权利要求1所述的基于局部特征的飞行器姿态跨视场估计方法，其特征在于，所述根据飞行器与初始相机的相对位置关系，将相机姿态转化为飞行器姿态，转换公式如下：

T_p＝T_c×T-t_c

其中，T为飞行器与初始相机的相对位置关系，T_c为前一步骤计算的到的相机姿态，T_p为飞行器姿态，t_c为飞行器中心位置。

5.一种基于局部特征的飞行器姿态跨视场估计装置，所述飞行器上设置有对称的标签，在初始时飞行器位于初始相机的视场范围内，其特征在于，所述基于局部特征的飞行器姿态跨视场估计装置，包括：

标定模块，用于对场景内的所有深度相机进行标定，得到所有深度相机的内参、及各个深度相机相对于初始相机的转换矩阵；

标签识别模块，用于采用初始相机的视频图像作为输入，进行标签识别，根据标签在图像坐标系中的位置确定飞行器与初始相机的相对位置关系；

相机选取模块，用于以所有深度相机的视频图像作为输入，进行跨视场相机选取，将选取的深度相机的视频图像作为当前输入帧；

特征提取模块，用于对当前输入帧进行特征点提取，计算当前相机姿态；

姿态转换模块，用于将当前相机姿态转换为初始相机姿态，根据飞行器与初始相机的相对位置关系，将相机姿态转化为飞行器姿态。

6.如权利要求5所述的基于局部特征的飞行器姿态跨视场估计装置，其特征在于，所述相机选取模块以所有深度相机的视频图像作为输入，进行跨视场相机选取，执行如下操作：

对于所有深度相机集合N中的深度相机n，相应的有RGB图像f_i ⁿ和深度图前溯M帧，计算每帧时，各个深度相机的帧得分，帧得分表达为：

其中，

p(x,y)是深度图在(x,y)位置满足深度阈值H的深度得分，表示深度图在(x,y)位置的深度值，h和w分别是当前帧的高和宽；

将每帧最大帧得分对应的深度相机，放入一个集合

根据最大帧得分从深度相机集合N中进行选择最佳输入相机n_i：

其中ψ(n)是一个比例计算函数，代表着深度相机n在集合中所占的比例。

7.如权利要求5所述的基于局部特征的飞行器姿态跨视场估计装置，其特征在于，所述姿态转换模块将当前相机姿态转换为初始相机姿态，执行如下操作：

所述将当前相机姿态转换为初始相机姿态，根据如下公式进行转换：

{R,t)＝T_ctc{R′,t′}

其中，T_ctc为切换前深度相机到切换后深度相机的变换矩阵，{R,t}为初始相机姿态，{R′,t′}为当前相机姿态。

8.如权利要求5所述的基于局部特征的飞行器姿态跨视场估计装置，其特征在于，所述姿态转换模块根据飞行器与初始相机的相对位置关系，将相机姿态转化为飞行器姿态，转换公式如下：

T_p＝T_c×T-t_c

其中，T为飞行器与初始相机的相对位置关系，T_c为前一步骤计算的到的相机姿态，T_p为飞行器姿态，t_c为飞行器中心位置。