基于纯方位的多无人机协同目标跟踪算法

辛沙欧，陈可，宋震林，桂欣颖，戚国庆

（南京理工大学自动化学院，江苏南京 210094）

近年来，无人机技术发展迅猛，广泛应用于各领域之中。随着电子对抗技术在现代战争中的地位越来越高，通过获取目标的方位、多普勒变化率、到达时间等信息确定目标位置的无源定位研究逐渐吸引了更多的关注[1]。由于成本和重量的限制，无源纯方位传感器被广泛应用于被动跟踪中的无源定位。例如，分布式控制及定位中的纯方位跟踪[2]、包围控制中的方位或相对视距观测[3]、无人机编队飞行的视觉导航与跟踪[4]。纯方位跟踪系统是对目标进行连续观测，积累测角信息，从而获得目标状态的精确估计，包括目标的位置和速度，从而控制观测器进行跟踪[5]。多站无源组网定位技术作为更加有效的无源定位方法，己经受到国内外科研人员的高度重视[6]。

针对纯方位目标跟踪问题，文献[7-9]讨论了纯方位下的目标估计策略，其中文献[9]在文献[7]中的估计器基础上加入卡尔曼滤波，得到目标状态的无偏估计，进一步改善了目标估计的精度，为运动目标被动跟踪提供了更多的可能。

随着任务和应用的日益复杂，多无人机协同跟踪问题越来越受到重视。常见的是研究一组移动智能体对未知目标的定位和绕航问题，目的是使智能体以期望的半径围绕目标旋转，并在目标周围形成均匀的队形[10-13]。Liya Dou 等[11]研究了一组移动智能体基于纯方位测量对静止或者移动目标的定位和包围问题，在文献[7]的估计器基础上，设计了分布式控制率，保证智能体群分别在目标或其邻域周围全局收敛到一个精确的均匀圆形，估计误差和控制误差可以收敛到零的邻域。针对未知目标，J.Shao 以及R.Li 在不同维度研究了定位与环航问题[14-15]。

现有的纯方位量测下多无人机协同目标定位与跟踪的估计误差和控制误差只能收敛到零的邻域，而不能收敛到零，尤其是对于快速移动的目标。因此，针对快速移动的目标，如何提高目标估计精度和减小控制误差还有待进一步研究。

该文研究了多无人机在二维环境下围绕一个位置未知的运动目标的控制器设计，以及仅利用方位测量信息的精确目标状态估计器的设计问题。针对文献[7]中的估计器方法，提出了结合卡尔曼滤波的方式，以获得更加精确的目标状态。结果表明，相较于没有滤波的作用，对于运动目标的估计误差收敛到零，而不是零的邻域。针对文献[11]中的控制器设计问题，通过引入虚拟交叉算法，加入目标估计速度项对原有的控制器进行改进，可以保证控制误差收敛到零。最后，仿真结果验证了文中所提出的估计器以及控制律的优越性。

1 预备知识与问题描述

1.1 图 论

多无人机系统中N个无人机之间的通信情况可以采用无向图G={V,E} 进行描述，其中V={1,2,∙∙∙,N}为无向图节点的集合，E⊆V×V为无向图的边集。A=[aij]∈RN×N是加权邻接矩阵，当(i,j)∈E，aij=aji=1，(i,j)∉E时，aij=0。假设该图没有自循环，则aii=0 。节点i的所有邻域集合用Ni={j∈V:(i,j)∈E}表示。定义图G的拉普拉斯矩阵为L=[lij]∈RN×N,当i≠j时，lij=-aij，。在图G中由节点i到节点j的路径是以节点i为起点，以节点j为终点的一系列不同边的组合。如果图G的每一对不同的节点都有一条路径，则图G是连通的。每个索引i都是对N求余得到的，例如，索引N+1 与1 等价[16]。

1.2 问题描述

首先，假设无人机可以在固定的高度飞行，因此只关注无人机在二维笛卡尔坐标系下的目标估计与定位。无人机与目标在二维平面下的相对位置如图1 所示。目标及其估计位置用“+”表示，无人机的位置用“●”表示。

图1 无人机与目标相对位置图形化视图

考虑一个在时间t内具有未知轨迹pT(t)=[xT(t),yT(t)]T∈R2的运动目标，和N个具有已知轨迹pi(t)=[xi(t),yi(t)]T∈R2的无人机，其中i∈V。假设每个无人机的运动服从单积分模型：

其中，ui(t)表示无人机i的控制输入。

无人机i对目标的估计位置可以表示为。无人机i对目标的观测角为θi(t)∈[0,2π)，与目标相对距离用表示，无人机i与对目标的估计位置的距离为，无人机i对目标的估计误差为。d为无人机与目标期望间距，即期望环航旋转半径。用φi(t)∈R2表示无人机i指向目标的单位方向向量，则：

(t)∈R2是φi(t)绕pi(t)顺时针旋转π/2 得到的垂直于φi(t)的单位向量。为了方便起见，假设逆时针旋转方向为正。因此，无人机i到无人机j的分离角(i＜j)是通过从无人机i到目标的射线逆时针旋转直到遇见无人机j到目标的射线：

为了设计一个分布式定位方案，利用纯方位测量值来估计目标的未知位置pT(t)，然后设计基于局部测量的分布式控制律，从而实现以下目标：

①估计误差收敛到0，即：

②无人机i与目标之间的距离收敛到指定的相同环航半径，即：

③一组无人机按照等角均匀分布在以目标为中心的指定半径的圆周上，即：

该文主要工作是基于以下假设进行实现的：

假设1：存在正常数u0使得对于t＞0 时，且保证任一无人机i的速度满足。

假设2：通信拓扑G是无向连通的。

假设3：所选择的pi(0)、pT(0)以及(0)能够使得ρi(0)、是有限的。

2 算法设计

已知多无人机跟踪位置未知的移动目标是一个估计和控制的综合问题，因此这一部分，尝试同时为每个无人机构造估计器和控制器[17-19]。

为了定位未知目标，首先为每个无人机提出如下协作的估计器：

其中，k是正常数，φi(t)φi(t)T是向量φi(t) 的投影矩阵。目标与无人机之间的几何位置关系如图2所示。

图2 无人机与目标之间的几何位置关系图

然后，基于无人机对目标的估计位置以及邻居节点的角度测量信息为每个无人机设计如下控制器：

其 中，kd、α是设计参数，分离角误差。

例如，当N=5 时，期望无人机能够在以目标为中心的具有指定半径的圆周上形成如图3 所示的等角编队。

图3 5个无人机期望队形示意图

控制输入ui(t) 由两部分组成。径向项-d)φi(t) 控制无人机移动到指定的环航跟踪半径的圆周上，切向项则保证各个无人机之间按照指定的角度分布在以目标为中心的圆周上。另外，因为切向项依赖于各个无人机的期望距离，因此当目标期望距离较小时，可以避免过高的角速度。

引理3.1[9]:在基于虚拟交叉算法[14]的式（7）估计器下，卡尔曼滤波得到的估计误差，在t→∞时是渐近无偏的。其中。

引理3.2[11]:假设目标是静止状态，在式（7）估计器和式（8）控制器下，一组无人机将收敛于以目标为圆心，半径为d的圆周上，即当t→∞时，ρi(t)→d。

引理3.3[11]:假设目标是静止状态，在式（7）估计器和式（8）控制器下，如果无向拓扑G是树结构，则一组无人机将按照指定分离角度分布在以目标为中心的具有指定半径的圆周上，对于i∈V，当t→∞时，满足=2π/N。

3 稳定性分析

根据引理5[11]，对于一个运动的目标，在式（7）估计器和式（8）控制器下，无人机i与目标的相对距离的控制误差收敛到一个半径有限的区域，但不是收敛到零。因此，提出下列控制器：

推论1：对于运动目标，式（9）控制器下无人机i的环航跟踪半径控制误差-d一致收敛到零。

证明：定义图1 所示的笛卡尔坐标系为C1，定义以目标pT(t) 为原点的参考坐标系为C2。则无人机在C2 坐标系下的相对于目标的运动速度可以描述为：

因此，原来的环航编队控制问题可以转化为C2坐标系下一组无人机环航跟踪静止目标，根据引理3.2，一组无人机将收敛于以目标为圆心，半径为d的圆周上，即当t→∞时ρi(t)→d。证毕。

推论2：对于运动目标，在式（9）控制器下，如果无向拓扑G是树结构，则一组无人机将按照指定分离角度分布在以目标为中心的圆周上，对于i∈V，当t→∞时，满足=2π/N。

证明：基于推论1 的证明，在C2 坐标系下一组无人机环航跟踪静止目标的问题，可以根据引理3.3，使得无人机之间的分离角按照2π/n分布在指定半径的圆周上。

4 仿真验证

4.1 动力学模型选择

为了能够得到目标更精确的估计位置，参考文献[9]在式（7）估计器引入卡尔曼滤波。首先，选择目标动力学模型相关参数为：

其中，采样时间Δt=0.1 s,x、、分别是估计目标的位置、速度以及加速度。假设模型噪声w(k)和外界干扰v(k)是均值，且为0，方差分别为的高斯白噪声，δ(k,l)是狄利克雷函数，q=0.2,R=0.2。

4.2 低速目标仿真对比

低速目标以文献[11]中的目标轨迹vT(t)=[0.2,0.1(cos(0.3t)+1)]T为例，在该文引入卡尔曼滤波的式（7）估计器和式（9）控制器下，选择参数k=5,kd=1,α=0.5 时，环航半径d=10，无人机初始位置任意，轨迹图如图4(a)所示，估计误差、收敛半径以及分离角情况如图4(b)、(c)、(d)所示。

通过图4 可以看出，添加滤波以及补偿项的控制策略，能够有效地使一组无人机在以目标为中心且具有指定半径的圆周上形成等角编队。将该次仿真结果与相同参数条件下的文献[11]中的估计控制策略进行比较，以无人机1 的估计误差、收敛半径以及分离角的收敛情况为例,结果如图5 所示。

图4 低速目标在滤波控制策略下的仿真结果图

由图5可以看出加入滤波后的估计策略收敛速度更快且能够全局渐近收敛到指定位置，控制误差更小。

图5 低速目标两种方法结果对比图

4.3 快速目标仿真对比

快速目标以文献[7]中的目标轨迹vT(t)=[1,cos(0.1t)+1)]T为例，在相同动力学模型下，引入滤波方法。在式（7）估计器与式（9）控制器的作用下，选择参数k=5,kd=1,α=0.5 时，环航半径d=8，无人机初始位置任意，轨迹图如图6(a)所示，估计误差、收敛半径以及分离角情况如图6(b)、(c)、(d)所示。

图6 快速目标在滤波控制策略下的仿真结果图

通过图6 可知，添加滤波以及补偿项的控制策略针对高速目标也有良好的跟踪性能。通过两组仿真结果对比可知，在目标速度加快且估计策略相同的情况下，引入滤波的方式所得到的估计误差显著减小，半径收敛误差减小，分离角也收敛到2π/5，如图7所示。

图7 快速目标两种方法结果对比图

5 结论

该文主要研究了纯方位量测下多无人机协同目标跟踪问题，讨论了一种具有无偏估计误差的卡尔曼滤波器在多无人机协同跟踪中的应用，基于虚拟交叉定位算法对现有控制律加入估计目标速度进行改进，保证多无人机在目标周围形成以目标为中心具有指定半径的等角圆形编队。与现有的纯方位量测下多无人机协同跟踪方法进行了比较，仿真结果表明，不管是低速还是快速目标，所提出的算法能显著提高目标状态估计的精度，并减小了控制误差。