CN114964266A - 基于多视觉矢量的运动状态协同群组相对姿态确定方法 - Google Patents

Abstract

本发明公开了一种基于多视觉矢量的运动状态协同群组相对姿态确定方法，首先，寻找双目镜头内的多个特征目标，分别得到其相对观测者的位置信息，并利用其坐标构建多个不共线的矢量；面对同类目标检测算法不可识别的情况，利用JPDA算法将同类目标标签化处理加以分辨，确保集群内所有参与观测的节点向量观测一致；最后根据视觉矢量的构建情况选择合适的姿态快速解算算法解算姿态转换矩阵。本发明应用于协同导航领域，充分利用了摄像头捕捉到特征目标构建视觉信息矢量，在协同群组具有共视目标的情况下，利用姿态解算算法解算协同群组相对姿态转换矩阵，提高了协同导航算法相对定姿精度，具有计算简单，鲁棒性强，定姿精度高的优点。

Description

基于多视觉矢量的运动状态协同群组相对姿态确定方法

技术领域

本发明涉及协同导航技术领域，具体是一种基于多视觉矢量的运动状态协同群组相对姿态确定方法。

背景技术

协同导航作为一种新型的作业模式受到国内外的广泛关注，多平台协同对环境或目标进行侦察、搜索、探测、定位是多传感器系统在作业中的典型应用形式，这首先就要求集群内能够有足够精确的相对定姿定位。随着视觉导航系统逐步应用，基于视觉信息的导航方案累积了相当多的成果，同样目标检测技术向着快速轻量发展迅速，为集群内载具的相对定姿定位技术发展提供了有力的支撑。协同导航是稳定性强、对外依赖度低、定位精确、功能丰富的新型集群智能协同技术，通过挖掘集群性能的最大潜力，做到“1+1>2”的效果，从而提升集群内单机的性能；此外，集群拥有卓越的功能性与生存性完成各式各样的任务，具有广阔的应用前景。

协同导航技术主要是通过建立集群内部节点或者外部特征点之间的几何关系，形成三角约束，从而进行节点之间的相对位姿观测。现阶段用于获得集群内相对距离方式相对成熟，主要是使用UWB直接获取两个UWB之间的距离，或者基于双目摄像头，利用对极几何进行目标测距。然而在载具高速运动的情况下，单靠距离约束难以抑制机载低精度IMU的发散。除了平台之间的距离外，还应该对节点之间的相对姿态信息进行观测，形成新的约束。目前公开发表的文献中，对相对姿态的获取往往依靠以下两种方式：一是利用相对距离差分计算角度偏移；二是利用加表与磁力计的输出作为向量，利用Wahba问题解算两机之间姿态，第一类方法在目标载具尺度小，两测量平台之间距离远的情况下误差较大；而第二类方法运用时为保证加速度计测量为当地重力加速度，要求观测载具与目标载具保持静止，这都极大的限制了相对姿态解算的使用，而视觉信息形成的矢量丰富且不因载具运动而产生测量误差，因此寻找一种能够在协同群组运动状态下利用视觉矢量测量载具相对姿态的方法具有十分重要的意义。

发明内容

针对上述现有技术中的不足，本发明提供一种基于多视觉矢量的运动状态协同群组相对姿态确定方法，能够在协同群组运动状态下利用视觉信息解算两平台之间的相对姿态。

为实现上述目的，本发明提供一种基于多视觉矢量的运动状态协同群组相对姿态确定方法，包括以下步骤：

步骤1，获取协同群组内两载具平台共视区域的多个特征目标，并分别解算各特征目标在观测载具镜头坐标系与目标载具镜头坐标系下的探测坐标；

步骤2，基于步骤1解算的探测坐标，得到同一时刻帧下，观测载具镜头坐标系中所有的观测平台视觉矢量，以及目标载具镜头坐标系中所有的目标平台视觉矢量；其中，观测平台视觉矢量由观测载具镜头坐标系中任意两个不同目标的探测坐标相连得到，目标平台视觉矢量由目标载具镜头坐标系中任意两个不同目标的探测坐标相连得到，即观测平台视觉矢量与目标平台视觉矢量一一对应，且一个观测平台视觉矢量与对应的目标平台视觉矢量组成一组矢量对；

步骤3，基于至少两组矢量对，利用姿态解算算法得到观测载具与目标载具之间的相对姿态转换矩阵，即完成相对姿态确定。

与现有技术相比，本发明具有如下有益技术效果：

1、本发明充分利用了双目摄像头提供的视觉信息，在载具运动导致加速度计无法只输出当地重力的情况下，运用姿态解算算法计算不同机体镜头坐标系下的视觉矢量得到姿态转换矩阵，提高了集群内节点相对姿态测量精度；

2、本发明原理简单，鲁棒性强，定姿精度高。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图示出的结构获得其他的附图。

图1为本发明实施例中对姿态确定方法流程图；

图2为本发明实施例中视觉矢量生成步骤示意图；

图3为本发明实施例中态解算算法选择方案流程图。

本发明目的的实现、功能特点及优点将结合实施例，参照附图做进一步说明。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明的一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

在本发明的描述中，“多个”的含义是至少三个，例如三个、四个等，除非另有明确具体的限定。

另外，本发明各个实施例之间的技术方案可以相互结合，但是必须是以本领域普通技术人员能够实现为基础，当技术方案的结合出现相互矛盾或无法实现时应当认为这种技术方案的结合不存在，也不在本发明要求的保护范围之内。

本实施例公开了一种基于多视觉矢量的运动状态协同群组相对姿态确定方法，该方法首先通过机载单目或双目镜头得到特征目标的镜头坐标系坐标，然后最后利用不同的特征目标构建视觉矢量，解算两机载镜头坐标系的相对姿态。

参考图1，本实施例中基于多视觉矢量的运动状态协同群组相对姿态确定方法具体包括如下步骤：

步骤1，首先任意选取协同群组内具有共视区域的两个载具平台，将其中一个载具平台作为观测载具，另一个作为目标载具；在寻找、探测观测载具与目标载具共视区域内的多个特征目标，并分别解算各特征目标在观测载具镜头坐标系与目标载具镜头坐标系下的探测坐标。

本实施例中，特征目标包括但不限于汽车、树木与建筑。目标检测算法能够获取汽车、树木、建筑等特征目标的像素坐标，根据对极几何原理能够使用像素坐标系中的坐标计算出目标在摄像头坐标系中的坐标。因此，分别解算各特征目标在观测载具镜头坐标系与目标载具镜头坐标系下探测坐标的具体实施方式为：

若观测载具与目标载具共视区域中共有M个特征目标，则首先获取目标i在观测载具、目标载具像素坐标中的图像坐标；

再根据对极几何原理能够使用像素坐标系中的坐标，分别计算出特征目标i在摄像头坐标系中的坐标，分别为

、

；

其中，

为j时刻帧第i个特征目标

在观测载具镜头坐标系下的探测坐标，

为j时刻帧第i个特征目标

在目标载具镜头坐标系下的探测坐标，i=1~M。至于如何根据目标检测算法解算探测坐标的过程为本领域的常规技术手段，本实施例中不再对其赘述。

在具体实施过程中，基于机器视觉的目标检测可以基于单目或者双目镜头完成。在利用双目镜头进行检测的同时，能够通过对极几何原理计算出目标在镜头坐标系中的坐标位置，但是目前基于深度学习的目标检测对类内目标难以区分。这是由于在网络的训练阶段，样本分类并不会提供的如此细致。而且在远距离观测时，检测的精度与准确度也难以完全保证。因此，需要寻找一种能够辅助目标检测算法有效去除探测虚警，分辨同构、同形目标的方法。因此，针对步骤1中观测到的同类而无法分辨的特征目标，可以采用JPDA算法将目标标签化以在集群中进行分辨，进而将每一个探测坐标归属到对应的特征目标。具体地：

当共视区域内存在目标检测算法判定为同类的目标时，由于空间位置的不同，集群内观测的不同个体可能对同类的目标难以分辨。联合概率数据互联JPDA是数据关联算法之一，它的基本思想是：对应于观测数据落入跟踪门相交区域的情况，这些观测数据可能来源于多个目标。JPDA的目的在于计算观测数据与每一个目标之间的关联概率，且认为所有的有效回波都可能源于每个特定目标，只是它们源于不同目标的概率不同。JPDA算法的优点在于它不需要任何关于目标和杂波的先验信息，是在杂波环境中对多目标进行跟踪的较好方法之一。

具体到本实施例中，JPDA算法的目的在于通过计算每一个观测坐标

归属于目标i的概率，将观测与实际目标进行对应。对于每一个可能归属于i的观测量

，JPDA算法通过设计的关联门确定检测是否落入检测门，然后根据所得的观测确认矩阵，得到每一个观测对于目标的联合关联概率矩阵。联合关联概率即为最终的每个坐标形成的航迹和目标之间关联的可能性系数。将矩阵引入卡尔曼滤波修正新息，就可以计算对目标i的位置估计，从而得到目标i所有观测值的集合

，此时每一个观测

将有具体归属的目标。经JPDA算法得到的观测集

不仅实现了各目标的分辨，更是有效剔除了检测过程中出现的误判与虚警，示意图即图2所示，其中，实线五角星即为特征目标的实际位置，虚线五角星即为特征目标的探测位置。利用JPDA算法，能够将连续的时间帧对同一目标的观测建立关联，从而通过概率估计判定轨迹对目标的归属，达到分辨同构目标的目的。

步骤2，基于步骤1解算的探测坐标，得到同一时刻帧下，观测载具镜头坐标系中所有的观测平台视觉矢量，以及目标载具镜头坐标系中所有的目标平台视觉矢量；其中，观测平台视觉矢量由观测载具镜头坐标系中任意两个不同目标的探测坐标相连得到，目标平台视觉矢量由目标载具镜头坐标系中任意两个不同目标的探测坐标相连得到，即观测平台视觉矢量与目标平台视觉矢量一一对应，且一个观测平台视觉矢量与对应的目标平台视觉矢量组成一组矢量对。

例如，若在j时刻帧观测载具与目标载具的共视区域共存在M个特征目标，则当前的观测平台视觉矢量为

，目标平台视觉矢量为

，其中，

为j时刻帧第i个特征目标

与第k个特征目标

形成的观测平台视觉矢量，

为j时刻帧第i个特征目标

与第k个特征目标

形成的目标平台视觉矢量，i=1~M、k=1~M且i≠k。

步骤3，基于至少两组矢量对，利用姿态解算算法得到观测载具与目标载具之间的相对姿态转换矩阵，即完成相对姿态确定，其具体实施过程为：

将至少两组矢量对作为姿态解算算法的输入，通过姿态解算算法最小化损失函数

可以得到最优姿态转换矩阵，即可得到观测载具与目标载具之间的最优姿态转换矩阵。

在具体应用中，由于观测节点所处环境的复杂性与自身定位状况的不确定性，视觉矢量构建情况往往不一。总结来讲，矢量精度差、矢量近共线的情况都会对姿态解算的精度产生较大的影响。矢量构建误差主要来自于目标的错误识别与平台的观测误差。在步骤1中使用的JPDA算法通过目标分辨与轨迹生成已经将错误识别引入的误差降低到可以接受的水平。平台观测误差的产生是由于视觉设备获取的深度信息不够准确，这往往是由于观测现场光流不稳定，背景复杂等原因造成。矢量近乎共线则是指由于观测角度不适宜，导致最终得到的矢量近乎平行的情况。根据视觉矢量的构建情况，可以选择鲁棒性强的SVD、FLAE算法针对矢量不够精确的情况，或是选用EQOS2以及QUEST算法对精确构建的矢量进行快速解算。

即在得到视觉矢量后将根据目标探测精度与矢量之间平行程度选择适宜的姿态解算方案，其具体实施过程为：

步骤3.1，针对目标的探测精度，将目标距离与背景复杂程度作为主要影响因素，因此计算目标侦测的置信系数，其过程为：

首先，计算观测载具目标侦测的第一置信系数，为：

式中，

为第一置信度，M表示当前帧所探测出的特征目标个数，S _i 代表第i个特征目标识别框在观测载具图像中占取的面积，S ₁为观测载具图像的图片面积，l ₁为观测载具所使用的摄像头基线长度，d ₁为得到的深度，

为测距误差系数，N ₁为通过计算目标重叠度反应图像帧内目标的复杂度情况的参数，

为测量距离为d ₁的目标的误差参数，体现了双目测距在距离越远时测量精度变差的情况；

其次，计算目标载具目标侦测的第二置信系数，为：

式中，

为第一置信度，M表示当前帧所探测出的特征目标个数，

代表第i个特征目标识别框在目标载具图像中占取的面积，S ₂为目标载具图像的图片面积，l ₂为目标载具所使用的摄像头基线长度，d ₂为得到的深度，N ₂为通过计算目标重叠度反应图像帧内目标的复杂度情况的参数，

为测量距离为d ₂的目标的误差参数，体现了双目测距在距离越远时测量精度变差的情况；

最后，选取第一置信系数与第二置信系数中较低的一个作为步骤3.1中目标侦测的置信系数。

上述置信系数公式反映了同一帧图像中侦测目标多且重叠将对探测结果造成的恶化以及探测距离远大于基线时深度信息获取不精确的状况，

则需要事前根据实验结果进行一定的处理，拟合后得出。

步骤3.2，判断置信系数是否大于第一阈值，若是则进入步骤3.3，否则进入步骤3.4；

步骤3.3，计算任意两个观测平台视觉矢量之间的第一共线度，以及任意两个目标平台视觉矢量之间的第一共线度：

若存在第一共线度低于第二阈值的两个观测平台视觉矢量，则任意舍弃其中一个观测平台视觉矢量，以及舍弃与之对应的目标平台视觉矢量，再重新计算第一共线度与第二共线度，直至不存在第一共线度低于第二阈值的情况；

若存在第二共线度低于第二阈值的两个目标平台视觉矢量，则任意舍弃其中一个目标平台视觉矢量，以及舍弃与之对应的观测平台视觉矢量，再重新计算第一共线度与第二共线度，直至不存在第二共线度低于第二阈值的情况；

若存在一个第一共线度或第二共线度大于第二阈值且低于第三阈值，则选择QUEST算法作为姿态解算算法，否则选择ESOQ2算法作为姿态解算算法；并以剩余的观测平台视觉矢量、目标平台视觉矢量作为姿态解算算法的输入；

步骤3.4，计算任意两个观测平台视觉矢量之间的第一共线度，以及任意两个目标平台视觉矢量之间的第一共线度：

若存在第一共线度低于第二阈值的两个观测平台视觉矢量，则任意舍弃其中一个观测平台视觉矢量，以及舍弃与之对应的目标平台视觉矢量，再重新计算第一共线度与第二共线度，直至不存在第一共线度低于第二阈值的情况；

若存在第二共线度低于第二阈值的两个目标平台视觉矢量，则任意舍弃其中一个目标平台视觉矢量，以及舍弃与之对应的观测平台视觉矢量，再重新计算第一共线度与第二共线度，直至不存在第二共线度低于第二阈值的情况；

若存在一个第一共线度或第二共线度大于第二阈值且低于第三阈值，则选择SVD算法作为姿态解算算法，否则选择FLAE算法作为姿态解算算法；并以剩余的观测平台视觉矢量、目标平台视觉矢量作为姿态解算算法的输入。

向量近似平行的情况较为容易理解，因此在步骤3.3与步骤3.4的第一共线度、第二共线度计算过程中：对于任意两个第一视觉矢量

与

，或任意两个第二视觉矢量

与

，其对应的第一共线度或第二共线度为：

式中，

为第一共线度或第二共线度。当

即可以认为

与

近似共线，同样，考虑到解算稳定性，

时应当弃用

与

的任意一个矢量。在具体应用过程中，目标检测的频率一般能达到40帧每秒，但定姿的观测达到1hz即能满足需求。因此对于可能存在会舍弃矢量对的情况，优先选择特征目标数量在4个以上的时刻帧进行相对姿态确定。

不同的姿态解算算法由于解算原理的不同在快速性、鲁棒性、精确性上表现不一：Quest算法使用了Davenport的在优化时所使用的的四元数算法,是姿态解算领域应用最广的方案。其利用牛顿迭代法解出四元数对应矩阵最大特征值的算法较为稳健，能够在观测比较精确的情况下处理近乎共线的矢量；ESOQ2着重强调了算法的快速性，是目前浮点运算最少、解算最为快速的姿态解算算法，在观测良好的情况下使用可以提高解算的速度。FLAE算法通过展开姿态矩阵，以列向量形式计算损失函数，最终施密特正交化计算矩阵的伪逆求解最优四元数，算法同样具有较强的鲁棒性，而近乎共线的矢量将在求解伪逆矩阵时引入较大的误差；而SVD算法通过奇异值分解求得最优姿态旋转矩阵的算法虽然计算量较大，但是鲁棒性出色且处理近乎共线的向量时依旧能够保持一定的精度。综上所述，参考图3，当目标侦测的置信系数

时，认为目标检测精度良好，因此QUEST算法与ESOQ2算法可供选择；反之，则选择SVD算法与FLAE算法。下一步，当

时，则选择适宜处理共线的算法，

时，则可以根据观测精度选择ESOQ2算法或者FLAE算法。综上，结合不同姿态解算算法的表现，得到图1中的算法选择方案。

以上所述仅为本发明的优选实施例，并非因此限制本发明的专利范围，凡是在本发明的发明构思下，利用本发明说明书及附图内容所作的等效结构变换，或直接/间接运用在其他相关的技术领域均包括在本发明的专利保护范围内。

Claims (8)

1.一种基于多视觉矢量的运动状态协同群组相对姿态确定方法，其特征在于，包括以下步骤：

步骤1，获取协同群组内两载具平台共视区域的多个特征目标，并分别解算各特征目标在观测载具镜头坐标系与目标载具镜头坐标系下的探测坐标；

步骤2，基于步骤1解算的探测坐标，得到同一时刻帧下，观测载具镜头坐标系中所有的观测平台视觉矢量，以及目标载具镜头坐标系中所有的目标平台视觉矢量；其中，观测平台视觉矢量由观测载具镜头坐标系中任意两个不同目标的探测坐标相连得到，目标平台视觉矢量由目标载具镜头坐标系中任意两个不同目标的探测坐标相连得到，即观测平台视觉矢量与目标平台视觉矢量一一对应，且一个观测平台视觉矢量与对应的目标平台视觉矢量组成一组矢量对；

步骤3，基于至少两组矢量对，利用姿态解算算法得到观测载具与目标载具之间的相对姿态转换矩阵，即完成相对姿态确定。

2.根据权利要求1所述的基于多视觉矢量的运动状态协同群组相对姿态确定方法，其特征在于，步骤1中，所述特征目标包括但不限于汽车、树木与建筑。

3.根据权利要求2所述的基于多视觉矢量的运动状态协同群组相对姿态确定方法，其特征在于，步骤1中，所述分别解算各特征目标在观测载具镜头坐标系与目标载具镜头坐标系下的探测坐标，具体为：

若两载具平台共视区域中共有M个特征目标，首先获取目标i在观测载具、目标载具像素坐标中的图像坐标；

再根据对极几何原理能够使用像素坐标系中的坐标，分别计算出特征目标i在摄像头坐标系中的坐标，分别为

、

；

其中，

为j时刻帧第i个特征目标

在观测载具镜头坐标系下的探测坐标，

为j时刻帧第i个特征目标

在目标载具镜头坐标系下的探测坐标，i=1~M。

4.根据权利要求1或2或3所述的基于多视觉矢量的运动状态协同群组相对姿态确定方法，其特征在于，在步骤1中，对于观测到的同类而无法分辨的目标，采用JPDA算法将目标标签化以在集群中进行分辨，进而将每一个探测坐标归属到对应的特征目标。

5.根据权利要求1或2或3所述的基于多视觉矢量的运动状态协同群组相对姿态确定方法，其特征在于，步骤3中，所述基于至少两组矢量对，利用姿态解算算法得到观测载具与目标载具之间的相对姿态转换矩阵，具体为：

根据目标观测的精确性以及观测平台视觉矢量

、目标平台视觉矢量

的共线性选择对应的姿态解算算法，计算最小化损失函数的矩阵，即可得到观测载具与目标载具之间的最优姿态转换矩阵；

其中，

为j时刻帧第i个特征目标

与第k个特征目标

形成的观测平台视觉矢量，

为j时刻帧第i个特征目标

与第k个特征目标

形成的目标平台视觉矢量，i=1~M、k=1~M且i≠k。

6.根据权利要求5所述的基于多视觉矢量的运动状态协同群组相对姿态确定方法，其特征在于，所述选择对应的姿态解算算法，具体为：

步骤3.1，计算目标侦测的置信系数；

步骤3.2，判断置信系数是否大于第一阈值，若是则进入步骤3.3，否则进入步骤3.4；

步骤3.3，计算任意两个观测平台视觉矢量之间的第一共线度，以及任意两个目标平台视觉矢量之间的第一共线度：

若存在第一共线度低于第二阈值的两个观测平台视觉矢量，则任意舍弃其中一个观测平台视觉矢量，以及舍弃与之对应的目标平台视觉矢量，再重新计算第一共线度与第二共线度，直至不存在第一共线度低于第二阈值的情况；

若存在第二共线度低于第二阈值的两个目标平台视觉矢量，则任意舍弃其中一个目标平台视觉矢量，以及舍弃与之对应的观测平台视觉矢量，再重新计算第一共线度与第二共线度，直至不存在第二共线度低于第二阈值的情况；

若存在一个第一共线度或第二共线度大于第二阈值且低于第三阈值，则选择QUEST算法作为姿态解算算法，否则选择ESOQ2算法作为姿态解算算法；并以剩余的观测平台视觉矢量、目标平台视觉矢量作为姿态解算算法的输入；

步骤3.4，计算任意两个观测平台视觉矢量之间的第一共线度，以及任意两个目标平台视觉矢量之间的第一共线度：

若存在第一共线度低于第二阈值的两个观测平台视觉矢量，则任意舍弃其中一个观测平台视觉矢量，以及舍弃与之对应的目标平台视觉矢量，再重新计算第一共线度与第二共线度，直至不存在第一共线度低于第二阈值的情况；

若存在第二共线度低于第二阈值的两个目标平台视觉矢量，则任意舍弃其中一个目标平台视觉矢量，以及舍弃与之对应的观测平台视觉矢量，再重新计算第一共线度与第二共线度，直至不存在第二共线度低于第二阈值的情况；

若存在一个第一共线度或第二共线度大于第二阈值且低于第三阈值，则选择SVD算法作为姿态解算算法，否则选择FLAE算法作为姿态解算算法；并以剩余的观测平台视觉矢量、目标平台视觉矢量作为姿态解算算法的输入。

7.根据权利要求6所述的基于多视觉矢量的运动状态协同群组相对姿态确定方法，其特征在于，步骤3.1中，所述计算目标侦测的置信系数，具体为：

计算观测载具目标侦测的第一置信系数，为：

式中，

为第一置信度，M表示当前帧所探测出的特征目标个数，

代表第i个特征目标识别框在观测载具图像中占取的面积，S ₁为观测载具图像的图片面积，l ₁为观测载具所使用的摄像头基线长度，d ₁为得到的深度，

为测距误差系数，N ₁为通过计算目标重叠度反应图像帧内目标的复杂度情况的参数，

为测量距离为d ₁的目标的误差参数；

计算目标载具目标侦测的第二置信系数，为：

式中，

为第一置信度，M表示当前帧所探测出的特征目标个数，

代表第i个特征目标识别框在目标载具图像中占取的面积，S ₂为目标载具图像的图片面积，l ₂为目标载具所使用的摄像头基线长度，d ₂为得到的深度，N ₂为通过计算目标重叠度反应图像帧内目标的复杂度情况的参数，

为测量距离为d ₂的目标的误差参数；

选取第一置信系数与第二置信系数中较低的一个作为步骤3.1中目标侦测的置信系数。

8.根据权利要求6所述的基于多视觉矢量的运动状态协同群组相对姿态确定方法，其特征在于，步骤3.3与步骤3.4中，所述第一共线度与第二共线度的计算过程为：

对于任意两个第一视觉矢量

与

，或任意两个第二视觉矢量

与

，其对应的第一共线度或第二共线度为：

式中，

为第一共线度或第二共线度。

Images