一种多无人机协同跟踪定位方法与流程

1.本发明涉及无人机控制与导航技术领域，特别是一种多无人机协同跟踪定位方法。


背景技术：

2.随着科技的发展，无人机在军事和民用领域得到广泛的应用，由于单架无人机存在传感器角度限制，不能多方位观测到目标以及续航能力较弱等缺点，多无人机协同工作从而达到拓宽观测范围，安全可靠的目的。
3.在无人机协同工作时，其网络拓扑结构时常发生变化，通常将这种时变的拓扑结构称为切换拓扑结构。通常来说，基于切换拓扑结构的估计问题能够很好地应对拓扑结构变化的要求，最终使得位置估计结果更加精确。对这样的实际情况，离散系统往往不能够完全反应拓扑结构的切换情况，所以研究连续时间系统的滤波方法意义重大。近年来，具有固定拓扑结构的分布式一致性卡尔曼滤波算法引起了广泛的关注。在这些滤波算法中，传感器节点接收来自其邻域的量测信息并使用协方差矩阵的逆进行状态估计。传感器网络中，对于一些节点来说目标状态可能不是完全可观察到的，因此该节点的目标状态估计不够准确。


技术实现要素：

4.本发明的目的在于提供一种实时性好、准确度高的多无人机协同跟踪定位方法。
5.实现本发明目的的技术解决方案为：一种多无人机协同跟踪定位方法，包括以下步骤：
6.步骤1、根据无人机通讯网络拓扑结构图，基于图论的方法建立有向连通图，获得无人机传感器节点与相邻节点的连通信息；
7.步骤2、建立线性连续时间系统模型；
8.步骤3、利用ikcf滤波算法，对目标未知信息进行估计；
9.步骤4、采用序贯快速协方差交叉融合算法，对各节点的估计结果进行数据融合，得出确定的目标位置。
10.进一步地，步骤2所述建立线性连续时间系统模型，具体如下：
[0011][0012]
其中x∈r
n
,过程噪声w～n(0,q)表示方差为q的高斯白噪声；为目标状态的导数，u为输入状态，a为系统矩阵，b为输入矩阵，f为噪声矩阵；
[0013]
假设式(1)中的状态量被n个连通图为g1的传感器节点观测，系统中节点的观测模型表示为：
[0014]
[0015]
其中ω
ij
～n(0,n
c
)表示方差为的点间通信噪声；z
i
是传感器i对目标的直接观测结果：
[0016]
z
i
＝h
i
x v
i
ꢀꢀꢀ
(3)
[0017]
h
i
为量测矩阵，量测噪声v
i
～(0,r
i
)是方差为r
i
的高斯白噪声；
[0018]
z
i
是传感器附带邻居节点状态及加权系数的观测结果；a
ij
表示传感器节点i与节点j是否通讯，若通讯则a
ij
＝1，否则a
ij
＝0；和p
j
是状态估计量和估计量的方差矩阵；如果传感器i能够直接获得目标的量测信息，那么g
i0
＝1，否则g
i0
＝0。
[0019]
进一步地，步骤3所述利用ikcf滤波算法，对目标未知信息进行估计，具体如下：
[0020]
根据式(3)～(6)进行ikcf滤波
[0021][0022][0023][0024][0025]
其中下标i、j表示第i、j个传感器节点；其中下标i、j表示第i、j个传感器节点；和分别为直接和间接卡尔曼增益；和p
j
是状态估计量和估计量的方差矩阵。
[0026]
进一步地，步骤4所述采用序贯快速协方差交叉融合算法，对各节点的估计结果进行数据融合，得出确定的目标位置，具体如下：
[0027]
根据式(7)～(10)进行序贯快速协方差交叉融合，将目标的运动信息融合分为多步完成，各节点在每收到一帧数据后即进行一次融合：
[0028][0029][0030][0031][0032]
其中k是当前时刻本融合节点包含自身局部估计值已经融合过的局部估计值个数，是当前节点融合k个局部估计值后得到的融合值，p
f,k
是当前节点融合k个局部估计
值后得到的融合值的方差矩阵，是当前节点融合k个局部估计值后新收到的待融合局部估计值，p
new
是当前节点融合k个局部估计值后新收到的待融合局部估计值的方差矩阵，ε
f
是当前节点融合k个局部估计值后得到的融合值的中间融合系数，ω
f
是当前节点融合k个局部估计值后得到的融合值的归一化融合系数，ε
new
是当前节点融合k个局部估计值后新收到的待融合局部估计值的中间融合系数，ω
new
是当前节点融合k个局部估计值后新收到的待融合局部估计值的归一化融合系数，是k 1个节点局部目标状态估计值的融合值，p
f,k 1
是k 1个节点局部目标状态估计值的融合方差矩阵。
[0033]
本发明与现有技术相比，其显著优点为：
[0034]
(1)ikcf算法跟踪结果误差均方差更小，估计结果更加精确，用来解决已知拓扑通信结构的传感器网络状态估计问题，为后续切换系统的目标估计问题提供了技术支持；
[0035]
(2)在信息加权卡尔曼一致性滤波算法(ikcf)中，传感器节点i测量模型的构建使用节点的局部测量信息和邻居节点的目标运动状态的估计信息，从而使得不能观测到目标的节点可通过邻居节点的观测值进行状态更新；
[0036]
(3)采用序贯快速协方差交叉算法(sfci)可以减少各个融合节点的计算量，保证各个节点融合结果一致，为系统后续的功能实现提供统一的数据输入。
附图说明
[0037]
图1为无人机协同工作示意图。
[0038]
图2为网络拓扑有向连通图。
[0039]
图3为各节点跟踪状态误差图，其中(a)～(d)分别为x1～x4的跟踪状态误差图。
[0040]
图4为数据融合结果图，其中(a)～(d)分别为x1～x4的数据融合结果图。
具体实施方式
[0041]
本发明公开了一种多无人机协同跟踪定位方法，所述方法首先设定一种多无人机通讯拓扑结构。无人机协同飞行过程中跟踪目标，使用信息加权卡尔曼一致性滤波算法(information-weightedkalmanconsensusfilter,ikcf)对目标未知信息进行估计，并采用序贯快速协方差交叉融合算法(sequentialfastcovarianceintersection,sfci)对各节点的估计结果进行数据融合，得出确定的目标位置。该方法充分利用了相邻无人机节点的估计信息，进一步提高了系统的实时性，使目标位置估计更加准确。
[0042]
结合图1，本发明多无人机协同跟踪定位方法，包括以下步骤：
[0043]
步骤1、根据无人机通讯网络拓扑结构图，基于图论的方法建立有向连通图，获得无人机传感器节点与相邻节点的连通信息；
[0044]
步骤2、建立线性连续时间系统模型；
[0045]
步骤3、利用ikcf滤波算法，对目标未知信息进行估计；
[0046]
步骤4、采用序贯快速协方差交叉融合算法，对各节点的估计结果进行数据融合，得出确定的目标位置。
[0047]
进一步地，步骤2所述建立线性连续时间系统模型，具体如下：
[0048]
[0049]
其中x∈r
n
,过程噪声w～n(0,q)表示方差为q的高斯白噪声；为目标状态的导数，u为输入状态，a为系统矩阵，b为输入矩阵，f为噪声矩阵；
[0050]
假设式(1)中的状态量被n个连通图为g1的传感器节点观测，系统中节点的观测模型表示为：
[0051][0052]
其中ω
ij
～n(0,n
c
)表示方差为的点间通信噪声；z
i
是传感器i对目标的直接观测结果：
[0053]
z
i
＝h
i
x v
i
ꢀꢀꢀ
(3)
[0054]
h
i
为量测矩阵，量测噪声v
i
～(0,r
i
)是方差为r
i
的高斯白噪声；
[0055]
z
i
是传感器附带邻居节点状态及加权系数的观测结果；a
ij
表示传感器节点i与节点j是否通讯，若通讯则a
ij
＝1，否则a
ij
＝0；和p
j
是状态估计量和估计量的方差矩阵；如果传感器i能够直接获得目标的量测信息，那么g
i0
＝1，否则g
i0
＝0。
[0056]
进一步地，步骤3所述利用ikcf滤波算法，对目标未知信息进行估计，具体如下：
[0057]
根据式(3)～(6)进行ikcf滤波
[0058][0059][0060][0061][0062]
其中下标i、j表示第i、j个传感器节点；其中下标i、j表示第i、j个传感器节点；和分别为直接和间接卡尔曼增益；和p
j
是状态估计量和估计量的方差矩阵。
[0063]
进一步地，步骤4所述采用序贯快速协方差交叉融合算法，对各节点的估计结果进行数据融合，得出确定的目标位置，具体如下：
[0064]
根据式(7)～(10)进行序贯快速协方差交叉融合，将目标的运动信息融合分为多步完成，各节点在每收到一帧数据后即进行一次融合：
[0065][0066][0067][0068][0069]
其中k是当前时刻本融合节点包含自身局部估计值已经融合过的局部估计值个数，是当前节点融合k个局部估计值后得到的融合值，p
f,k
是当前节点融合k个局部估计值后得到的融合值的方差矩阵，是当前节点融合k个局部估计值后新收到的待融合局部估计值，p
new
是当前节点融合k个局部估计值后新收到的待融合局部估计值的方差矩阵，ε
f
是当前节点融合k个局部估计值后得到的融合值的中间融合系数，ω
f
是当前节点融合k个局部估计值后得到的融合值的归一化融合系数，ε
new
是当前节点融合k个局部估计值后新收到的待融合局部估计值的中间融合系数，ω
new
是当前节点融合k个局部估计值后新收到的待融合局部估计值的归一化融合系数，是k 1个节点局部目标状态估计值的融合值，p
f,k 1
是k 1个节点局部目标状态估计值的融合方差矩阵。
[0070]
下面结合实施例对本发明技术方案进行详细说明，但是本发明的保护范围不局限于所述实施例。
[0071]
实施例
[0072]
本实施例针对如图2所示的无人机拓扑结构有向连通图，对目标target0进行搜索。其中节点1能够直接观测到目标，新的量测值在循环网络中传输状态估计信息，用于节点状态更新。
[0073]
首先建立连通网络g＝{v,ε,a}，使用ikcf算法进行滤波估计，最后使用sfci算法进行数据融合，得到唯一确定的目标状态信息。具体步骤如下：
[0074]
步骤1、基于获得的网络拓扑结构图，根据图论的方法建立有向连通图，获得无人机传感器节点与相邻节点的连通信息如下：
[0075][0076]
步骤2、建立线性连续时间系统模型,此处假设系统输入为0，目标运动状态方程为:
[0077]
[0078]
状态x＝[x
1 x
2 x
3 x4]
t
包含两个位置信息和两个速度信息，w是方差为q＝[2 2 1 1]
t
的高斯白噪声。
[0079]
建立各节点观测模型，观测矩阵h
i
＝i4，量测噪声w
ij
是方差为n
c
＝0.5*[1 1 1 1]
t
的高斯白噪声。
[0080]
步骤3、根据式(3)-(6)进行ikcf滤波。
[0081]
步骤4、根据式(7)-(10)进行序贯快速协方差交叉融合。
[0082]
本实施例基于matlab仿真平台。由图3(a)～(d)可以看出各无人机传感器节点的跟踪误差逐渐趋于0，各节点对目标的估计误差均方差如下表所示：
[0083]
source(ikcf)msenode10.002535node20.002611node30.002591node40.002469node50.002693datafusion0.002425
[0084]
经过sfci数据融合结果如图4(a)～(d)所示，误差基本处于0附近。从以上结果可以看出，本发明采用基于ikcf滤波的目标跟踪定位方法实现了固定拓扑结构下多无人机对观测目标的状态一致估计，即整个无人机网络渐近达到对目标的一致性跟踪，并能够保证各节点融合结果一致，为系统后续的跟踪侦察实现提供统一的数据输入。