CN115797400A - 一种多无人系统协同长期目标跟踪方法 - Google Patents

Abstract

本发明公开了一种多无人系统协同长期目标跟踪方法，包括：任取第j个无人系统，其相机采集图像帧作为当前帧，剪切目标区域图像块，确定目标三维位置，确定自定位信息；将上述信息发送给队友并且接收队友的上述信息；目标协同定位模块融合自身及队友的上述信息，确定目标位置信息并预测目标信息，将目标位置信息和目标预测信息以及队友的目标区域图像块传递给目标跟踪模块进行更新；相机采集新的图像帧作为当前帧，根据目标位置信息和目标预测信息得到多个目标候选区域，进一步确定目标三维位置；重复以上步骤直到跟踪任务完成。本发明的方法提高了目标跟踪的准确率和成功率。

Description

一种多无人系统协同长期目标跟踪方法

技术领域

本发明属于无人系统技术领域，尤其涉及一种基于多无人系统协同长期目标跟踪方法。

背景技术

目标跟踪作为计算机视觉领域的一个核心的并且极具挑战性的研究课题，多年来科研人员对其进行了深入研究，但是在复杂环境下多无人系统跟踪运动目标时仍然存在着较大的挑战。目前，主流的视觉跟踪算法步骤主要包括图像输入、目标外观建模(目标描述、统计模型)、运动估计、目标定位、目标外观模型更新等。近年来，基于深度学习的目标跟踪算法也取得了较好的性能。现有的方法主要解决了基于图像序列的目标跟踪问题。

然而，在多无人系统实际应用中，目标跟踪问题变得更加复杂，是一个分布式协同跟踪问题，其跟踪效果受到传感器类型、目标的外观、外界环境、多传感器信息融合框架的影响，因而现有的多无人系统目标跟踪的准确率和成功率偏低。

发明内容

为解决上述现有技术中存在的技术问题，本发明提供了一种多无人系统协同长期目标跟踪方法，所述多无人系统协同长期目标跟踪方法包括如下步骤：

S1：构建多无人系统，所述多无人系统中每个无人系统的编号为i＝1,2,3,…,n，每个无人系统上安装有自定位模块、通信模块、目标跟踪模块、目标协同定位模块、以及用于采集图像数据的相机ci，其中所述目标跟踪模块由跟踪线程和检测线程构成，第i个无人系统的自定位信息为相机ci的相机坐标系与世界坐标系之间的旋转和平移变换关系

S2：任取第j个无人系统，其自定位信息为

第j个无人系统的相机cj采集图像帧作为当前帧，剪切目标区域图像块I_cj，利用目标跟踪模块确定目标三维位置p_wj；

S3：将第j个无人系统跟踪的目标区域图像块I_cj、目标三维位置p_wj以及第j个无人系统的自定位信息

通过第j个无人系统的通信模块发送给其他队友，并且第j个无人系统的通信模块接收来自其他队友通过执行与所述S2相同的步骤所得的目标区域图像块、目标三维位置和队友自定位信息I_ci,p_wi,

第j个无人系统的通信模块接收到的信息传输到目标协同定位模块；

S4：第j个无人系统的目标协同定位模块融合第j个无人系统跟踪的目标区域图像块、目标三维位置和第j个无人系统自定位信息I_cj,p_wj,

及其他队友跟踪的目标区域图像块、目标三维位置和队友自定位信息I_ci,p_wi,

确定目标位置信息和速度信息并预测目标下一帧可能出现的图像坐标，并将目标位置信息和目标下一帧可能出现的图像坐标的目标预测信息传递给目标跟踪模块，以及将其他队友的目标区域图像块I_ci,i＝1,2…,n,i≠j传递给第j个无人系统的目标跟踪模块；

S5：第j个无人系统利用自身得到的目标区域图像块I_cj和其他队友的目标区域图像块I_ci,i＝1,2…,n,i≠j更新其目标跟踪模块；

S6：第j个无人系统的相机cj采集新的图像帧作为当前帧，根据所述步骤S4提供的目标位置信息和目标下一帧可能出现的图像坐标预测信息，得到目标候选区域，利用目标跟踪模块进一步确定目标三维位置；

S7：重复所述步骤S3-S6，直到跟踪任务完成。

进一步地，在上述多无人系统协同长期目标跟踪方法中，所述多无人系统中所有无人系统都执行所述步骤S1-S7，实现多无人系统协同长期目标跟踪。

进一步地，在上述多无人系统协同长期目标跟踪方法中，第i个无人系统的自定位信息按如下方式确认：自定位模块确定每个无人系统自身的位姿信息，得到第i个无人系统的自定位模块坐标系与世界坐标系之间旋转和平移变换关系

通过离线标定自定位模块和相机ci的相机坐标系之间的外参关系，得到相机ci的相机坐标系与世界坐标系之间的旋转和平移变换关系

进一步地，在上述多无人系统协同长期目标跟踪方法中，所述自定位模块是组合导航定位系统或视觉惯性导航定位系统，所有无人系统定义的世界坐标系一致，且系统时间统一。

进一步地，在上述多无人系统协同长期目标跟踪方法中，所述目标跟踪模块的跟踪线程的跟踪子是能够输出目标的像素坐标的目标跟踪算法，所述目标跟踪模块的检测线程的检测子是能够在整帧图像中确定目标是否存在的目标检测算法或深度卷积神经网络算法。

进一步地，在上述多无人系统协同长期目标跟踪方法中，所述步骤S2的具体实现过程包括：

S21、相机cj采集当前帧图像，跟踪线程的跟踪子输出目标图像的像素坐标，检测线程的检测子在整帧图像中确定目标是否存在，如不存在目标，相机cj采集下一帧图像；如存在目标，则确定目标区域，剪切目标区域图像块I_cj；

S22、初始化目标跟踪模块的跟踪线程和检测线程，具有离线目标数据集时，通过离线数据集训练检测线程的检测子，通过检测子确定当前帧图像中的目标图像区域，初始化跟踪线程的跟踪子；无离线目标数据集时，直接手动确定当前帧图像中的目标图像区域，然后用目标区域的视觉特征初始化跟踪子和检测子；确定目标图像区域后，再进一步确定目标三维位置p_wj。

进一步地，在上述多无人系统协同长期目标跟踪方法中，所述步骤S22中确定目标三维位置p_wj的具体实现过程包括：

(1)根据相机cj的内参数和扭曲参数k_cj,d_cj，按照下式将目标图像的像素齐次坐标转换到相机坐标：

其中，p_j＝[u v 1]^T为目标图像的像素齐次坐标，p_cj＝[x y z]^T为目标图像的相机坐标；

(2)在相机cj为单目相机的情况下，对于第j个无人系统，目标处于三维空间的射线l_j上，所述射线l_j以如下世界坐标系下的射线方程表示：

其中k_j为系数，v＝[x/z y/z 1]^T为相机坐标系下所述射线方程的方向向量，所述射线过原点，且通过P(k_cj,d_cj)逆变换可以得到：

所述目标三维位置p_wj由目标所在的射线l_j的单位方向向量归一化表示，即

其中norm表示对向量进行归一化；

(3)在相机cj为双目相机或者为集成有测距传感器的单目相机的情况下，直接求解目标图像的相机坐标p_cj＝[x y z]^T得出世界坐标系下的所述目标三维位置：

进一步地，在上述多无人系统协同长期目标跟踪方法中，所述步骤S5的具体实现过程包括：

S51：跟踪线程首先初始化，然后采集图像帧，通过目标跟踪算法获得跟踪结果，对结果进行状态自检，跟踪结果置信度高则继续跟踪，并且将结果和图像帧传输到检测线程；结果置信度低，则重新进行初始化；

S52：检测线程首先初始化，然后对跟踪线程传输的图像帧和跟踪结果进行处理，进一步判断跟踪状态好坏，如果跟踪状态好，则进一步学习更新检测模型；如果跟踪状态差，则从图像帧中重新检测目标，并将其传输给跟踪线程，对跟踪线程重新初始化

本发明的多无人系统协同长期目标跟踪方法具有以下优点和有益效果：通过将目标跟踪与目标检测深度集成融合，提高了目标跟踪的准确率和成功率，同时，将不同无人系统的跟踪结果深度融合，可以更进一步的实现鲁棒和快速的长时间目标跟踪。

附图说明

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

图1为本发明的多无人系统协同长期目标跟踪方法的实施示意图；

图2为本发明的多无人系统协同长期目标跟踪方法中更新目标跟踪模块的实施示意图。

具体实施方式

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

目标跟踪主要是利用目标在时间和空间上的相关性，估计连续图像帧中目标的位置信息，跟踪算法的实时性好，计算资源消耗少，但是经常会失败，并且不能重新搜寻目标。目标检测主要是指在图像中确定待检测的目标是否存在，算法鲁棒性好，但需要较多的计算资源和处理时间。

本发明实施例提供的多无人系统协同长期目标跟踪方法将目标跟踪与目标检测深度集成融合，能够提高跟踪算法的总体性能，提高目标跟踪的准确率和成功率，同时，将不同无人系统的跟踪结果深度融合，可以更进一步的实现鲁棒和快速的长时间目标跟踪。

本发明实施例提供的多无人系统协同长期目标跟踪方法具体包括如下步骤：

S1：构建多无人系统，多无人系统中每个无人系统的编号为i＝1,2,3,…,n，如图1所示，每个无人系统上安装有自定位模块、通信模块、目标跟踪模块、目标协同定位模块、以及用于采集图像数据的相机ci，其中目标跟踪模块由跟踪线程和检测线程构成，在所述多无人系统中，第i个无人系统的自定位模块坐标系与世界坐标系之间旋转和平移变换关系为

通过离线标定自定位模块和相机ci的相机坐标系之间外参关系，确定第i个无人系统的自定位信息，所述自定位信息为相机cu的相机坐标系与世界坐标系之间的旋转和平移变换关系

具体地，对于多无人系统，所有无人系统的世界坐标系是统一的，通过自定位模块，可以确定每个无人系统自身的位姿信息，最终得到第i个无人系统的自定位模块坐标系与世界坐标系之间旋转和平移变换关系

通过离线标定自定位模块和相机ci的相机坐标系之间的外参关系，最终可得到相机ci的相机坐标系与世界坐标系之间的旋转和平移变换关系

即第i个无人系统的自定位信息。

S2：任取第j个无人系统，其自定位信息为

第j个无人系统的相机cj采集图像帧作为当前帧，剪切目标区域图像块I_cj，利用目标跟踪模块确定目标三维位置p_wj。

更详细地，上述步骤S2的具体实现过程包括：

S21、相机cj采集当前帧图像，跟踪线程的跟踪子输出目标图像的像素坐标，检测线程的检测子在整帧图像中确定目标是否存在，如不存在目标，相机cj采集下一帧图像；如存在目标，则确定目标区域，剪切目标区域图像块I_cj；

S22、初始化目标跟踪模块的跟踪线程和检测线程，具有离线目标数据集时，可以通过离线数据集训练检测线程的检测子，通过检测子确定当前帧图像中的目标＝图像区域，初始化跟踪线程的跟踪子；无离线目标数据集时，直接手动确定当前帧图像中的目标图像区域，然后用目标区域的视觉特征初始化跟踪子和检测子。确定目标图像区域后，再进一步确定目标三维位置p_wj。

在上步骤S22中，确定目标三维位置p_wj的具体实现过程包括：

(1)根据相机的内参数和扭曲参数k_cj,d_cj，将目标图像的像素齐次坐标转换到相机坐标：

其中，p_j＝[u v 1]^T为目标图像的像素齐次坐标，p_cj＝[x y z]^T为目标图像的相机坐标；

(2)在相机cj为单目相机的情况下，对于第j个无人系统，目标处于三维空间的射线l_j上，该射线l_j以如下世界坐标系下的射线方程表示：

其中k_j为系数，

为第j个无人系统的相机cj的相机坐标系与世界坐标系旋转和平移变换关系，v＝[x/z y/z 1]^T为相机坐标系下射线方程的方向向量，射线过原点，且通过P(k_cj,d_cj)逆变换可以得到：

目标三维位置p_wj由目标所在的射线l_j的单位方向向量归一化表示，即

其中norm表示对向量进行归一化。

(3)在相机cj为双目相机或者为集成有测距传感器的单目相机的情况下，直接求解目标图像的相机坐标p_cj＝[x y z]^T，得出世界坐标系下的目标三维位置p_wj：

其中，

为第j个无人系统的相机cj的相机坐标系与世界坐标系旋转和平移变换关系。采用卡尔曼滤波可以融合不同无人系统估算的目标三维位置，可以得到更加精确的结果。

在以上(2)中，获得是目标射线方向，需要采用基于射线相交的协同定位算法估计目标三维位置，得到更加精确的结果，具体而言：

(2.1)第j个无人系统目标所在的射线为l_j:k_jp_wj+t_cj,k_j＞0，由于l_j包含未知的参数k_j，可能存在多条射线。对于任意两条不平行的射线，可以找到一个离两条射线最近的三维点p_w。每条射线距离p_w最近的点定义为q_j，q_j与p_w之间的最小化距离d_j为：

d_j＝||k_jp_wj+t_cj—p_w||²

在

时q_j与p_w之间的距离取最小值，此时射线上离p_w最近点q_j为：

(p_w-t_cj)_||表示(p_w-t_cj)的投影到矢量p_wj方向上，q_j与p_w之间的平方距离

为：

I为单位矩阵，(p_w-t_cj)_⊥表示(p_w-t_cj)相对于p_wj方向的垂直分量。采用最小二乘方法，通过累计所有无人系统的

的和，可以求解p_w，得到如下方程组：

(2.2)建立最小抽样集的势为k＝2的模型和作为测量单元的三维点p_w的样本集M＝{p_wi},i＝1,2,…,n，并从集合M中随机抽取包含m个样本作为子集S，其中m大于k，构建上述(2.1)中的方程组，求解目标位置p_w。

(2.3)集合M的余集集合为SC＝M-S，目标位置p_w与余集中射线l_j:k_jp_wj+t_cj,k_j＞0的距离表示误差，误差值小于某一设定阈值t的样本集以及S构成内点集S*。

(2.4)重新随机抽取新的子集S，重复以上过程。

(2.5)在完成N次抽样后，内点集S*最大的势对应的目标位置p_wj确定为最终的目标三维位置p_wj。

S3：将第j个无人系统跟踪的目标区域图像块I_cj、目标三维位置p_wj以及第j个无人系统自定位信息

通过第j个无人系统的通信模块发送给其他队友，并且第j个无人系统的通信模块接收来自其他队友通过执行与上述S2相同的步骤所得的目标区域图像块、目标三维位置和队友自定位信息I_ci,p_wi,

通信模块接收到的信息最终传输到目标协同定位模块。

S4：第j个无人系统的目标协同定位模块融合第j个无人系统跟踪的目标区域图像块、目标三维位置和第j个无人系统自定位信息I_cj,p_wj,

及其他队友跟踪的目标区域图像块、目标三维位置和队友自定位信息I_ci,p_wi,

确定目标位置信息和目标速度信息并预测目标下一帧可能出现的图像坐标，并将目标位置信息和目标下一帧可能出现的图像坐标预测信息传递给目标跟踪模块，以及将其他队友的目标区域图像块I_ci,i＝1,2…,n,i≠j传递给第j个无人系统的目标跟踪模块。

更详细地，上述步骤S4的具体实现过程包括：

S41、确定目标位置信息。对于单目相机，多个无人系统的目标三维位置p_wi,i＝1,2…,n实质上是表示由相机ci光心出发出的射线，多个无人系统得到的射线相交，交点即为目标的三维位置，由此确定目标位置信息；对于双目相机或者集成有测距传感器的单目相机，将多个无人系统的的目标三维位置p_wi,i＝1,2…,n采用Kalman滤波实现多传感器信息融合，由此确定目标位置信息；

S42.确定目标速度信息。基于已知记录的目标位置与时间信息，通过最小二乘算法，估算出目标速度信息；

S43.预测目标下一帧可能出现的图像坐标。目标当前时刻位置和速度已知，假设短时间内目标相对无人平台为匀速运动，则可得到目标下一时刻的三维位置，在相机内参已知情况下，通过射影变换即可预测目标下一帧可能出现的图像坐标。

S5：第j个无人系统利用自身得到的目标区域图像块I_cj和其他队友的目标区域图像块I_ci,i＝1,2…,n,i≠j更新其目标跟踪模块。

更详细地，如图2所示，上述步骤S5的具体实现过程包括：

S51：跟踪线程首先初始化，然后采集图像帧，通过目标跟踪算法获得跟踪结果，对结果进行状态自检，跟踪结果置信度高则继续跟踪，并且将结果和图像帧传输到检测线程；结果置信度低，则重新进行初始化；

S52：检测线程首先初始化，然后对跟踪线程传输的图像帧和跟踪结果进行处理，进一步判断跟踪状态好坏，如果跟踪状态好，则进一步学习更新检测模型；如果跟踪状态差，则从图像帧中重新检测目标，并将其传输给跟踪线程，对跟踪线程重新初始化。

S6：第j个无人系统的相机cj采集新的图像帧作为当前帧，根据上述步骤S4提供的目标位置信息和目标下一帧可能出现的图像坐标预测信息，得到目标候选区域，利用目标跟踪模块进一步确定目标三维位置。

S7：重复上述步骤S3-S6，直到跟踪任务完成。

在本发明实施例提供的多无人系统协同长期目标跟踪方法中，多无人系统中所有无人系统都执行所述步骤S1-S7，由此最终实现多无人系统协同长期目标跟踪。

在本发明实施例提供的多无人系统协同长期目标跟踪方法中，自定位模块可以是组合导航定位系统、视觉惯性导航定位系统等，而且，所有无人系统定义的世界坐标系必须一致，且系统时间必须统一。

在本发明实施例提供的多无人系统协同长期目标跟踪方法中，目标跟踪模块的跟踪线程的跟踪子可以是任意的目标跟踪算法，只要能够输出目标的像素坐标；目标跟踪模块的检测线程的检测子可以是传统的目标检测算法或深度卷积神经网络算法，能够在整帧图像中确定目标是否存在即可。

综上所述，本发明的多无人系统协同长期目标跟踪方法将目标跟踪与目标检测深度集成融合，能够提高目标跟踪的准确率和成功率，同时，将不同无人系统的跟踪结果深度融合，可以更进一步的实现鲁棒和快速的长时间目标跟踪。

需要说明的是，在本文中，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。此外，本文中给定的一些特定参数仅作为示范，在不同的实施方式中该值可以相应地改变为合适的值。

最后应说明的是：以上实施例仅用于说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。

Claims (8)

1.一种多无人系统协同长期目标跟踪方法，其特征在于，包括步骤：

S1：构建多无人系统，所述多无人系统中每个无人系统的编号为i＝1，2，3，…，n，每个无人系统上安装有自定位模块、通信模块、目标跟踪模块、目标协同定位模块、以及用于采集图像数据的相机ci，其中所述目标跟踪模块由跟踪线程和检测线程构成，第i个无人系统的自定位信息为相机ci的相机坐标系与世界坐标系之间的旋转和平移变换关系

S2：任取第j个无人系统，其自定位信息为

第j个无人系统的相机cj采集图像帧作为当前帧，剪切目标区域图像块I_cj，利用目标跟踪模块确定目标三维位置p_wj；

S3：将第j个无人系统跟踪的目标区域图像块I_cj、目标三维位置p_wj以及第j个无人系统的自定位信息

t_cj通过第j个无人系统的通信模块发送给其他队友，并且第j个无人系统的通信模块接收来自其他队友通过执行与所述S2相同的步骤所得的目标区域图像块、目标三维位置和队友自定位信息

第j个无人系统的通信模块接收到的信息传输到目标协同定位模块；

S4：第j个无人系统的目标协同定位模块融合第j个无人系统跟踪的目标区域图像块、目标三维位置和第j个无人系统自定位信息

及其他队友跟踪的目标区域图像块、目标三维位置和队友自定位信息

确定目标位置信息和速度信息并预测目标下一帧可能出现的图像坐标，并将目标位置信息和目标下一帧可能出现的图像坐标的目标预测信息传递给目标跟踪模块，以及将其他队友的目标区域图像块I_ci，i＝1，2…，n，i≠j传递给第j个无人系统的目标跟踪模块；

S5：第j个无人系统利用自身得到的目标区域图像块I_cj和其他队友的目标区域图像块I_ci，i＝1，2…，n，i≠j更新其目标跟踪模块；

S6：第j个无人系统的相机cj采集新的图像帧作为当前帧，根据所述步骤S4提供的目标位置信息和目标下一帧可能出现的图像坐标预测信息，得到目标候选区域，利用目标跟踪模块进一步确定目标三维位置；

S7：重复所述步骤S3-S6，直到跟踪任务完成。

2.根据权利要求1所述的多无人系统协同长期目标跟踪方法，其特征在于，所述多无人系统中所有无人系统都执行所述步骤S1-S7，实现多无人系统协同长期目标跟踪。

3.根据权利要求1所述的多无人系统协同长期目标跟踪方法，其特征在于，第i个无人系统的自定位信息按如下方式确认：自定位模块确定每个无人系统自身的位姿信息，得到第i个无人系统的自定位模块坐标系与世界坐标系之间旋转和平移变换关系

，t_i，通过离线标定自定位模块和相机ci的相机坐标系之间的外参关系，得到相机ci的相机坐标系与世界坐标系之间的旋转和平移变换关系

，t_ci。

4.根据权利要求1所述的多无人系统协同长期目标跟踪方法，其特征在于，所述自定位模块是组合导航定位系统或视觉惯性导航定位系统，所有无人系统定义的世界坐标系一致，且系统时间统一。

5.根据权利要求1所述的多无人系统协同长期目标跟踪方法，其特征在于，所述目标跟踪模块的跟踪线程的跟踪子是能够输出目标的像素坐标的目标跟踪算法，所述目标跟踪模块的检测线程的检测子是能够在整帧图像中确定目标是否存在的目标检测算法或深度卷积神经网络算法。

6.根据权利要求1至5中任一项所述的多无人系统协同长期目标跟踪方法，其特征在于，所述步骤S2的具体实现过程包括：

S21、相机cj采集当前帧图像，跟踪线程的跟踪子输出目标图像的像素坐标，检测线程的检测子在整帧图像中确定目标是否存在，如不存在目标，相机cj采集下一帧图像；如存在目标，则确定目标区域，剪切目标区域图像块I_cj；

S22、初始化目标跟踪模块的跟踪线程和检测线程，具有离线目标数据集时，通过离线数据集训练检测线程的检测子，通过检测子确定当前帧图像中的目标图像区域，初始化跟踪线程的跟踪子；无离线目标数据集时，直接手动确定当前帧图像中的目标图像区域，然后用目标区域的视觉特征初始化跟踪子和检测子；确定目标图像区域后，再进一步确定目标三维位置p_wj。

7.根据权利要求6所述的多无人系统协同长期目标跟踪方法，其特征在于，所述步骤S22中确定目标三维位置p_wj的具体实现过程包括：

(1)根据相机cj的内参数和扭曲参数k_cj，d_cj，按照下式将目标图像的像素齐次坐标转换到相机坐标：

其中，p_j＝[u v 1]^T为目标图像的像素齐次坐标，p_cj＝[x y z]^T为目标图像的相机坐标；

(2)在相机cj为单目相机的情况下，对于第j个无人系统，目标处于三维空间的射线l_j上，所述射线l_j以如下世界坐标系下的射线方程表示：

其中k_j为系数，v＝[x/z y/z 1]^T为相机坐标系下所述射线方程的方向向量，所述射线过原点，且通过P(k_cj，d_cj)逆变换得到：

所述目标三维位置p_wj由目标所在的射线l_j的单位方向向量归一化表示，即

其中norm表示对向量进行归一化；

(3)在相机cj为双目相机或者为集成有测距传感器的单目相机的情况下，直接求解目标图像的相机坐标p_cj＝[x y z]^T得出世界坐标系下的所述目标三维位置：

8.根据权利要求1至5中任一项所述的多无人系统协同长期目标跟踪方法，其特征在于，所述步骤S5的具体实现过程包括：

S51：跟踪线程首先初始化，然后采集图像帧，通过目标跟踪算法获得跟踪结果，对结果进行状态自检，跟踪结果置信度高则继续跟踪，并且将结果和图像帧传输到检测线程；结果置信度低，则重新进行初始化；

S52：检测线程首先初始化，然后对跟踪线程传输的图像帧和跟踪结果进行处理，进一步判断跟踪状态好坏，如果跟踪状态好，则进一步学习更新检测模型；如果跟踪状态差，则从图像帧中重新检测目标，并将其传输给跟踪线程，对跟踪线程重新初始化。

Images