CN108319130A - 基于李雅普诺夫制导向量和粒子滤波的机动目标跟踪方法 - Google Patents

Abstract

本发明提出的基于李雅普诺夫制导向量和粒子滤波的机动目标跟踪方法，属于无人机目标跟踪领域。当无人机确定待跟踪的机动目标后，该方法首先对机动目标建立动力学模型；采用粒子滤波方法及光学相机的量测信息，估计机动目标的位置和速度：在每个时刻，分别对每个粒子进行重要性采样，更新每个粒子的状态估计和估计方差矩阵以及重要性权值；若粒子有效性小于有效性阈值，则对粒子进行重采样；若大于等于阈值，则输出对机动目标的位置和速度估计；然后，利用李雅普诺夫制导向量法进行无人机跟踪路径规划，得到无人机该时刻进行机动目标跟踪的控制量。本发明可以有效估计具有未知输入地面机动目标的状态信息，实现无人机对机动目标的盘旋跟踪。

Description

基于李雅普诺夫制导向量和粒子滤波的机动目标跟踪方法

技术领域

本发明涉及一种基于李雅普诺夫制导向量和粒子滤波的机动目标跟踪方法，属于无人机目标跟踪领域。

背景技术

随着无人机技术在军用、民用领域的快速发展，应用无人机跟踪地面机动目标已经成为一种新趋势，比如边界巡防、救灾防护、运输护航等。

一般情况下，无人机通过自身搭载的光学相机捕捉目标，相机的焦距需要随着目标与无人机的距离变化而调整，但是，在运动过程中调节相机焦距是很困难的。自然而然，我们希望无人机在跟踪过程中保持与被跟踪目标之间的相对距离不变。因此，控制无人机按照圆形轨道环绕机动目标作盘旋运动是一种很好的解决方案，跟踪示意图如图1所示，图中坐标系为惯性坐标系，坐标原点为工作站，X轴指向正东，Y轴指向正北，Z轴与X轴和Y轴构成右手坐标系，垂直地面指向正上方，无人机的期望轨道是以被跟踪目标为圆心，以r_d为半径的圆，示意图中曲线A表示机动目标的运动轨迹，视轴即为相机的视线，无人机本体坐标的原点在无人机的质心，X轴与无人机纵轴平行，指向机头方向，Y轴与无人机的横轴平行，指向机身左侧，Z轴与X、Y轴构成右手坐标系，垂直X-Y平面向上。

目前的技术只能对非机动目标进行跟踪，即目标的运动状态不会发生改变，总是保持静止或者匀速直线运动。针对这类非机动目标的跟踪多采用卡尔曼滤波或者扩展卡尔曼滤波方法对目标运动状态进行估计。当目标存在未知控制输入，即目标的前进速度和前进方向会发生改变时，对目标运动状态的估计将会变得很困难。而实际跟踪问题中的目标大多为机动目标，针对存在未知控制输入的机动目标的研究还比较少。粒子滤波方法可以对系统中的不确定性进行估计，因此采用粒子滤波方法估计机动目标的未知控制输入是一种很好的方法。目前，粒子滤波方法多被用于解决目标跟踪过程中量测信息丢失的问题，通过构建粒子滤波器，对量测信息的有效性进行估计。

当被跟踪机动目标的实际状态信息已知时，采用李雅普诺夫制导向量法进行路径规划已经被广泛研究了，该方法可以通过机动目标与无人机的位置产生制导指令，从而引导无人机按照预定的半径盘旋跟踪机动目标。当被跟踪目标的位置、速度等状态信息未知时，如何进行路径规划，从而产生期望指令还没有被研究。

无人机系统功能模块构成示意图如图2，从图中可以看出，制导模块和平台控制模块都依赖于运动估计模块输出的位置、速度信息。运动估计模块可以结合相机提供的量测信息，对机动目标的运动状态进行有效估计，制导模块基于运动估计器的输出和无人机自带导航模块的输出信息产生制导指令，飞行控制模块根据制导模块产生的制导指令和无人机自带导航模块的输出信息产生无人机控制指令，控制无人机实现对存在未知控制输入机动目标的盘旋跟踪。光学相机通过两轴稳定平台固定在无人机的底部，该两轴平台可以隔离无人机的扰动，保持相机的视轴始终指向被跟踪机动目标，从而避免目标丢失的问题，两轴平台的控制技术已经非常成熟，在此不作过多说明。

发明内容

本发明的目的是为克服被跟踪机动目标存在未知控制输入，导致状态信息难以估计以及难以跟踪的问题，提出一种基于李雅普诺夫制导向量和粒子滤波的机动目标跟踪方法。本发明方法，采用粒子滤波方法可以有效估计机动目标的运动状态，基于李雅普诺夫制导向量的路径规划方法可以进行实时路径规划，可实现对机动目标进行有效跟踪。

本发明提出的一种基于李雅普诺夫制导向量和粒子滤波的机动目标跟踪方法，其特征在于，该方法包括以下步骤：

(1)当无人机确定待跟踪的机动目标后，对机动目标建立动力学模型，表达式如下：

其中,为待跟踪的机动目标在k时刻的状态向量，上标t表示机动目标，下标k表示第k个采样时刻，为机动目标k时刻在惯性坐标系中的三维坐标位置，为机动目标k时刻在惯性坐标系下X方向和Y方向的速度；F_k为k时刻的机动目标系统矩阵，B_k为k时刻的机动目标输入矩阵，Π_k为k时刻的机动目标噪声矩阵，表达式分别如下：

其中，τ为采样时间间隔，是k时刻的过程噪声，代表机动目标在惯性坐标系下X方向和Y方向的加速度不确定性和Z方向的速度不确定性， 分别为机动目标在惯性坐标系下X方向速度不确定性的方差、Y方向速度不确定性的方差和Z方向速度不确定性的方差；是机动目标k时刻分别在X方向和Y方向的控制输入，γ_k是k时刻的三状态的马尔科夫链，分别对应k时刻机动目标的三种控制输入：直行、左转、右转；

令s＝{1,2,3}代表机动目标三种不同的控制输入：1代表直行，2代表左转，3代表右转，则机动目标k时刻到k+1时刻三种控制输入的概率转移矩阵为：

(2)采用粒子滤波方法并结合光学相机提供的量测信息，对机动目标的位置和速度进行估计；

定义：Γ_k＝{γ₀,...,γ_k}表示机动目标前k时刻的三状态的马尔科夫链集合，Z_k＝{z₀,...,z_k}表示前k时刻无人机搭载相机对机动目标的量测集合，U_k＝{u^t(γ₀),...,u^t(γ_k)}表示前k时刻机动目标的控制输入集合，其中0时刻为无人机开始跟踪机动目标的时刻；具体步骤如下：

(2-1)利用粒子滤波方法对粒子进行初始化；

k＝0时刻，从p(γ₀|γ_-1)随机生成N个粒子其中γ_-1＝1，并使Σ_0|0＝Σ₀，为第i个粒子-1时刻的重要性权值，N为粒子总数，和Σ_0|0分别为从0时刻到0时刻的机动目标状态估计和估计方差矩阵，x₀和Σ₀分别为粒子滤波方法设定的机动目标初始状态和初始方差矩阵Σ₀＝α*diag(5)，α＞0，；

(2-2)对每个粒子进行重要性采样；具体步骤如下：

(2-2-1)令当前时刻为k，k＞0，更新对每个粒子的状态预测和预测方差矩阵，表达式分别如下：

式中，和分别为粒子i从k-1时刻到k时刻的状态预测和预测方差矩阵；

针对机动目标状态向量的最小方差滤波方法表示为：

式中，表示粒子i从k时刻到k时刻的状态预测；

采用全概率公式对进行分解：

式中，是高斯概率密度；采用N个粒子去近似p(Γ_k-1|Z_k)，表示第i个粒子前k时刻的三状态的马尔科夫链集合：

式中，表示第i个粒子k-1时刻的重要性权值；

(2-2-2)对于每个粒子i，i＝1,...,N，更新k-1时刻的重要性权值，表达式如下：

其中，z_k为无人机中光学相机k时刻提供的量测信息：z_k＝[b_k c_k]^T，b_k和c_k为像素点在成像平面内k时刻的二维坐标位置，z_k＝h(X_k)+w_k，其中是k时刻的独立高斯噪声，R是量测噪声的协方差矩阵，且R＝diag(σ_b,σ_c)，σ_b和σ_c是量测噪声的标准差，h(X_k)是机动目标在成像平面内k时刻的真实二维坐标，其定义为：

式中，和X_k分别是无人机与机动目标的相对位置在相机坐标系与惯性坐标系中的表示，是无人机在k时刻的状态向量，C_ci是惯性坐标系到相机坐标系的旋转矩阵；

量测方程的雅可比矩阵表示为：

式中，和分别为向量的第一个、第二个和第三个元素；

(2-3)对粒子进行重采样；具体步骤如下：

(2-3-1)计算粒子有效性并进行判定；

粒子有效性的计算表达式如下：

如果则进行重采样，其中N_T是预设的粒子有效性阈值，按照概率分布从中生成N个新的粒子进入步骤(2-3-2)；如果则进入步骤(2-4)；

(2-3-2)用新产生的粒子代替旧的粒子，使得 进入步骤(2-4)；

(2-4)结合光学相机提供的量测信息，输出对机动目标的位置估计和速度估计；具体步骤如下：

(2-4-1)对于每一个执行如下过程：

式中，表示第i个粒子k时刻的卡尔曼滤波增益，和分别表示第i个粒子从k时刻到k时刻的状态预测和预测方差矩阵；

(2-4-2)量测更新；

其中，是对的估计，即Σ_k|k是从k时刻到k时刻对的估计方差矩阵；即为基于粒子滤波方法k时刻生成的对机动目标位置估计的输出结果，为基于粒子滤波方法k时刻生成的对机动目标惯性坐标系下X方向和Y方向的速度估计的输出结果；

(3)利用李雅普诺夫制导向量法进行无人机跟踪路径规划，得到无人机进行机动目标跟踪的控制量；具体步骤如下：

(3-1)利用李雅普诺夫制导向量法进行无人机跟踪路径规划；

假设待跟踪的机动目标为静止，则无人机k时刻的期望速度为：

式中，为无人机k时刻在惯性坐标系下X方向和Y方向期望的相对速度，表示无人机k时刻在惯性坐标系下X方向和Y方向的位置，u^d和r^d分别表示预定的盘旋速度和盘旋轨迹半径；当r_k＞r^d时，无人机向机动目标靠近直到期望的圆形轨道；当r_k＜r^d时，无人机远离机动目标直到期望的圆形轨道；当r_k＝r^d时，无人机在期望的圆形轨道上绕机动目标作盘旋运动；

将转换到极坐标为：

直角坐标系与极坐标系的关系为：

x_k＝r_kcosθ_k

y_k＝r_k sinθ_k

设计李雅普诺夫函数V为：

将V对时间求导：

当且仅当r_k＝0时，则无人机运动轨迹渐近收敛到期望的圆形轨道；

当机动目标移动时，无人机k时刻的期望速度和前进方向表达式分别如下：

式中，和分别表示无人机k时刻的期望速度和前进方向；

(3-2)计算无人机进行机动目标跟踪的控制量；

基于无人机期望的前进速度前进方向和无人机的当前状态采用比例控制方法控制无人机跟踪控制指令，实现对存在未知控制输入的地面机动目标的盘旋跟踪：

式中，和为无人机k时刻的控制量，分别控制无人机的前进速度和前进方向，和分别表示无人机k时刻的速度和前进方向，K₁和K₂为控制增益，和分别是和对时间的微分；

(4)令k＝k+1，对于i＝1,...,N，重新生成N个粒子重新返回步骤(2-2-1)，进行下一时刻无人机对于地面机动目标的盘旋跟踪。

本发明的特点及有益效果在于：

(1)本发明通过结合粒子滤波方法和光学相机提供的二维坐标信息可以估计出机动目标存在的未知控制输入，进而估计出机动目标的三维坐标信息以及水平速度信息，实现对存在未知控制输入机动目标的运动估计，该方法具有很强的推广性，更加适合工程应用；

(2)本发明在采用粒子滤波方法估计机动目标状态信息的基础上，采用李雅普诺夫制导向量法对无人机前进路径进行规划，产生制导速度，该制导方法可以保证无人机运动轨迹渐近收敛到期望的圆形虚拟轨道上，该方法设计简单，稳定性高，李雅普诺夫稳定性理论保证了其收敛性；

(3)本发明基于粒子滤波方法输出的估计状态和李雅普诺夫制导向量法产生的期望路径，采用结构最简单的比例控制方法对无人机进行控制，实现无人机路径跟踪控制，达到按照预定半径和速度进行盘旋跟踪的目的，该方法设计简单，仅有两个需要调试的控制变量。

附图说明

图1为无人机按照圆形轨道跟踪机动目标的示意图。

图2为无人机系统功能模块构成示意图。

图3为本发明方法的整体流程图。

图4为本发明实施例中无人机、机动目标的运行轨迹与粒子滤波方法对机动目标位置估计的示意图。

图5为本发明实施例中基于粒子滤波方法对机动目标位置的估计误差示意图。

图6为本发明实施例中无人机与机动目标间的实际距离示意图。

图7为本发明实施例中无人机位置在机动目标坐标系中的示意图。

图8为本发明实施例中1500次重复实验中粒子滤波方法对机动目标位置估计误差的平均值示意图。

具体实施方式

本发明提出的基于李雅普诺夫制导向量和粒子滤波的机动目标跟踪方法，下面结合附图和具体实施例进一步详细说明如下。

本发明提出的基于李雅普诺夫制导向量和粒子滤波的机动目标跟踪方法，首先针对机动目标的未知控制输入，进行动力学建模，采用三种可能输入的情况简化问题描述，建立被跟踪目标的动力学运动方程，需要注意的是，这种方法可以被推广到三种以上可能输入的情况。采用粒子滤波方法结合光学相机提供的量测信息估计出目标的输入，进而估计出目标的位置、速度信息；其次，基于粒子滤波方法的输出，采用李雅普诺夫制导向量法进行路径规划，产生制导指令，利用比例控制实现路径跟踪，实现按照预定的半径和速度进行盘旋跟踪的目的。

本发明提出的基于李雅普诺夫制导向量和粒子滤波的机动目标跟踪方法，整体流程如图3所示，包括以下步骤：

(1)当无人机确定待跟踪的机动目标后，针对机动目标的未知控制输入，对机动目标建立动力学模型，表达式如下：

其中,为待跟踪的机动目标在k时刻的状态向量，上标t表示机动目标，下标k表示第k个采样时刻，为机动目标k时刻在惯性坐标系中的三维坐标位置，为机动目标k时刻在惯性坐标系下X方向和Y方向的速度，惯性坐标系的坐标原点为工作站，X轴指向正东，Y轴指向正北，Z轴与X轴和Y轴构成右手坐标系，垂直地面指向正上方。F_k为k时刻的机动目标系统矩阵，B_k为k时刻的机动目标输入矩阵，Π_k为k时刻的机动目标噪声矩阵，表达式分别如下：

其中，τ为采样时间间隔，该时间间隔与无人机的计算能力和选用的相机型号有关，本实施例中τ＝0.02秒，是k时刻的过程噪声，代表机动目标在惯性坐标系下X方向和Y方向的加速度不确定性和Z方向的速度不确定性， 分别为机动目标在惯性坐标系下X方向速度不确定性的方差、Y方向速度不确定性的方差和Z轴方向速度不确定性的方差。是机动目标k时刻分别在X方向和Y方向的控制输入，γ_k是k时刻的三状态的马尔科夫链，分别对应k时刻机动目标的三种控制输入：直行、左转、右转。

令s＝{1,2,3}代表机动目标三种不同的控制输入：1代表直行，2代表左转，3代表右转，则机动目标k时刻到k+1时刻三种控制输入的概率转移矩阵为：

特别的，当机动目标的当前状态为直行时，则在下一时刻该机动目标保持直行的概率是0.9，左拐和右拐的概率均为0.05。

(2)采用粒子滤波方法并结合光学相机提供的量测信息，对机动目标的位置和速度进行估计；

定义：Γ_k＝{γ₀,...,γ_k}表示机动目标前k时刻的三状态的马尔科夫链集合，Z_k＝{z₀,...,z_k}表示前k时刻无人机搭载相机对机动目标的量测集合，U_k＝{u^t(γ₀),...,u^t(γ_k)}表示前k时刻机动目标的控制输入集合，其中0时刻为无人机开始跟踪机动目标的时刻，具体步骤如下：

(2-1)利用粒子滤波方法对粒子进行初始化；

k＝0时刻，从p(γ₀|γ_-1)随机生成N个粒子其中γ_-1＝1，并使Σ_0|0＝Σ₀，为第i个粒子-1时刻的重要性权值，其中和Σ_0|0分别为从0时刻到0时刻的机动目标状态估计和估计方差矩阵，x₀和Σ₀分别为粒子滤波方法设定的机动目标初始状态和初始方差矩阵，N为粒子总数，可以依据无人机的计算能力和期望达到的精度进行选取，x₀可以依据机动目标的初始位置进行选取，Σ₀＝α*diag(5)，α＞0，α的值一般根据期望达到的跟踪经度进行选取，α通常略大于期望达到的跟踪经度，本实施例中N＝200，x₀＝[0 100 0 0 0]^T，α＝10；

(2-2)对每个粒子进行重要性采样；具体步骤如下：

(2-2-1)令当前时刻为k，k＞0，更新对每个粒子的状态估计和估计方差矩阵，表达式分别如下：

式中，和分别为粒子i从k-1时刻到k时刻的状态预测和预测方差矩阵，上标i表示第i个粒子。

针对机动目标状态向量的最小方差滤波方法可以表示为：

式中，表示粒子i从k时刻到k时刻的状态预测，通过上式可以发现是非高斯的，并且不可能通过分析的方法获得，因此采用全概率公式对其进行分解：

式中，是高斯概率密度，可以通过卡尔曼滤波方法迭代计算得到；因为Γ_k-1是三状态序列，p(Γ_k-1|Z_k)难以计算，因此本发明采用N个粒子去近似p(Γ_k-1|Z_k)，表示第i个粒子前k时刻的三状态的马尔科夫链集合：

式中表示第i个粒子k-1时刻的重要性权值，上标i表示第i个粒子，下标k表示k时刻；

(2-2-2)对于每个粒子i，i＝1,...,N，更新k-1时刻的重要性权值，表达式如下：

其中，z_k为无人机中光学相机k时刻提供的量测信息：z_k＝[b_k c_k]^T，b_k和c_k为像素点在成像平面内k时刻的二维坐标位置，该成像平面的二维坐标原点为成像平面的中心，X轴平行于成像面纵轴向下，Y轴平行于成像面横轴向右，z_k＝h(X_k)+w_k，其中是k时刻的独立高斯噪声，R是量测噪声的协方差矩阵，且R＝diag(σ_b,σ_c)，σ_b和σ_c是量测噪声的标准差，一般情况下σ_b＝σ_c，h(X_k)是机动目标在成像平面内k时刻的真实二维坐标，其定义为：

式中，和X_k分别是无人机与机动目标的相对位置在相机坐标系与惯性坐标系中的表示，是无人机在k时刻的状态向量，该状态向量由无人机自带的导航模块提供，C_ci是惯性坐标系到相机坐标系的旋转矩阵，相机坐标系的坐标原点为相机中心，X轴与相机视轴平行指向镜头方向，Y轴与相机机体的横轴平行指向右方，Z轴与X轴和Y轴构成右手坐标系。

量测方程的雅可比矩阵可以表示为：

式中和分别为向量的第一个、第二个和第三个元素。

(2-3)对粒子进行重采样；具体步骤如下：

(2-3-1)计算粒子有效性并进行判定；

粒子有效性的计算表达式如下：

如果则进行重采样，其中N_T是预设的粒子有效性阈值，本实施例中N_T＝2N/3，按照概率分布从中生成N个新的粒子进入步骤(2-3-2)；如果则进入步骤(2-4)；

(2-3-2)用新产生的粒子代替旧的粒子，使得 i＝1,...,N，进入步骤(2-4)；

(2-4)结合光学相机提供的量测信息，输出对机动目标的位置估计和速度估计；具体步骤如下：

(2-4-1)对于每一个执行如下过程：

式中，表示第i个粒子k时刻的卡尔曼滤波增益，上标i表示第i个粒子，下标k表示k时刻，和分别表示第i个粒子从k时刻到k时刻的状态预测和预测方差矩阵；

(2-4-2)量测更新；

其中是对的估计，即Σ_k|k是从k时刻到k时刻对的估计方差矩阵。即为基于粒子滤波方法k时刻生成的对机动目标位置估计的输出结果。为基于粒子滤波方法k时刻生成的对机动目标惯性坐标系下X方向和Y方向的速度估计的输出结果；

(3)利用李雅普诺夫制导向量法进行无人机跟踪路径规划，得到无人机进行机动目标跟踪的控制量；具体步骤如下：

李雅普诺夫制导向量法可以保证无人机运动轨迹渐近收敛到期望的圆形虚拟轨道上，因此可以采用李雅普诺夫制导向量对无人机进行路径规划，产生期望的前进速度和前进方向，李雅普诺夫制导向量的输入为无人机的当前位置和基于粒子滤波方法的对机动目标的状态估计。

(3-1)利用李雅普诺夫制导向量法进行无人机跟踪路径规划；

假设待跟踪的机动目标为静止，则无人机k时刻的期望速度为：

式中，为无人机k时刻在惯性坐标系中X方向和Y方向期望的相对速度，表示无人机k时刻在惯性坐标系下X方向和Y方向的位置，上标a表示无人机，为步骤(2)输出的k时刻对机动目标位置的估计，u^d和r^d分别表示预定的盘旋速度和盘旋轨迹半径，且均为常值。当r_k＞r^d时，无人机向机动目标靠近直到期望的圆形轨道；当r_k＜r^d时，无人机远离机动目标直到期望的圆形轨道；当r_k＝r^d时，无人机在期望的圆形轨道上绕机动目标作盘旋运动。

将转换到极坐标为：

直角坐标系与极坐标系的关系为：

x_k＝r_k cosθ_k

y_k＝r_k sinθ_k

设计李雅普诺夫函数V为：

将V对时间求导：

当且仅当r_k＝0时，所以无人机运动轨迹会渐近收敛到期望的圆形轨道。

当机动目标移动时，无人机k时刻的期望速度和前进方向表达式分别如下：

式中和分别表示无人机k时刻的期望速度和前进方向，下标k表示第k个采样时刻,表示步骤(2)输出的k时刻生成的对机动目标惯性坐标系水平面内X方向和Y方向的速度估计。

(3-2)计算无人机进行机动目标跟踪的控制量；

基于无人机期望的前进速度前进方向和无人机的当前状态和采用比例控制方法控制无人机跟踪控制指令，实现对存在未知控制输入的地面机动目标的盘旋跟踪：

式中，和为无人机k时刻的控制量，分别控制无人机的前进速度和前进方向，下标k表示第k个采样时刻，上标a表示无人机，和分别表示无人机k时刻的速度和前进方向，K₁和K₂为控制增益，两者均为正常数，和分别是和对时间的微分。

将和输入到无人机系统，即可实现无人机对k时刻存在未知控制输入机动地面目标的盘旋跟踪。

(4)令k＝k+1，对于i＝1,...,N，重新生成N个粒子重新返回步骤(2-2-1)，进行下一时刻无人机对于地面机动目标的盘旋跟踪。

本发明方法所采用的无人机以及所搭载的相机均为常规型号设备；本发明方法本领域技术人员通过编程即可实现。

仿真实验

(1)仿真设置

被跟踪的机动目标的起始位置为(0,100,0)米，初始速度为8米/秒，初始前进方向为π/4弧度,三种可能的控制输入为：u^t(1)＝[0,0]^T，u^t(2)＝[-1,1]^T，u^t(3)＝[1,-1]^T。无人机的起始位置为(-300,100,50)米，初始速度为10米/秒，初始航向为-π/2弧度。过程噪声标准差为量测噪声标准差为σ_b＝σ_c＝0.003。基于粒子滤波方法生成的粒子数为N＝200，采样间隔为τ＝0.02秒，初始状态为x₀＝[0 100 0 0 0]^T，初始方差矩阵为Σ₀＝10*diag(5)。

(2)仿真结果

图4显示的无人机、机动目标的运动轨迹和基于粒子滤波方法对机动目标的运动估计，图中点划线表示无人机的运动轨迹，小菱形表示无人机的起始位置，右下角是机动目标的运动轨迹和基于粒子滤波方法对机动目标的运动估计的局部放大图，图中实线表示机动目标的运动轨迹,虚线表示基于粒子滤波方法对机动目标位置的估计，图4中小正方形表示机动目标的起始位置，图5显示的是基于粒子滤波方法的估计误差，从图5中可以看出本次实验中，无人机进入期望轨道后，基于粒子滤波方法的最大估计误差小于3米。

图6为无人机与机动目标间的实际距离，从图中可以看出，一旦无人机进入期望的轨道，就会一直在期望的圆形虚拟轨道上运行下去，图7为无人机位置在机动目标坐标中的表示，图中小正方形表示机动目标的位置，小菱形表示无人机的起始位置。

为了验证本发明中方法的重复性和稳定性，采用Monte Carlo方法将实验重复1500次，图8表示的是1500次重复实验中基于粒子滤波方法对机动目标状态估计误差的平均值，从图8中可以发现，无人机进入轨道后，粒子滤波方法的平均估计误差小于1.5米。

本发明方法可以估计存在未知控制输入的目标的运动状态，很好的对机动目标进行盘旋跟踪，相比于跟踪静态目标或者匀速直线运动目标的方法，本发明方法更具实用价值。

本发明说明书中未作详细描述的内容属于本领域专业技术人员公知的现有技术。

Claims (1)

1.一种基于李雅普诺夫制导向量和粒子滤波的机动目标跟踪方法，其特征在于，该方法包括以下步骤：

(1)当无人机确定待跟踪的机动目标后，对机动目标建立动力学模型，表达式如下：

其中,为待跟踪的机动目标在k时刻的状态向量，上标t表示机动目标，下标k表示第k个采样时刻，为机动目标k时刻在惯性坐标系中的三维坐标位置，为机动目标k时刻在惯性坐标系下X方向和Y方向的速度；F_k为k时刻的机动目标系统矩阵，B_k为k时刻的机动目标输入矩阵，Π_k为k时刻的机动目标噪声矩阵，表达式分别如下：

其中，τ为采样时间间隔，是k时刻的过程噪声，代表机动目标在惯性坐标系下X方向和Y方向的加速度不确定性和Z方向的速度不确定性， 分别为机动目标在惯性坐标系下X方向速度不确定性的方差、Y方向速度不确定性的方差和Z方向速度不确定性的方差；是机动目标k时刻分别在X方向和Y方向的控制输入，γ_k是k时刻的三状态的马尔科夫链，分别对应k时刻机动目标的三种控制输入：直行、左转、右转；

令s＝{1,2,3}代表机动目标三种不同的控制输入：1代表直行，2代表左转，3代表右转，则机动目标k时刻到k+1时刻三种控制输入的概率转移矩阵为：

(2)采用粒子滤波方法并结合光学相机提供的量测信息，对机动目标的位置和速度进行估计；

定义：Γ_k＝{γ₀,...,γ_k}表示机动目标前k时刻的三状态的马尔科夫链集合，Z_k＝{z₀,...,z_k}表示前k时刻无人机搭载相机对机动目标的量测集合，U_k＝{u^t(γ₀),...,u^t(γ_k)}表示前k时刻机动目标的控制输入集合，其中0时刻为无人机开始跟踪机动目标的时刻；具体步骤如下：

(2-1)利用粒子滤波方法对粒子进行初始化；

k＝0时刻，从p(γ₀|γ_-1)随机生成N个粒子其中γ_-1＝1，并使Σ_0|0＝Σ₀，为第i个粒子-1时刻的重要性权值，N为粒子总数，和Σ_0|0分别为从0时刻到0时刻的机动目标状态估计和估计方差矩阵，x₀和Σ₀分别为粒子滤波方法设定的机动目标初始状态和初始方差矩阵Σ₀＝α*diag(5)，α＞0，；

(2-2)对每个粒子进行重要性采样；具体步骤如下：

(2-2-1)令当前时刻为k，k＞0，更新对每个粒子的状态预测和预测方差矩阵，表达式分别如下：

式中，和分别为粒子i从k-1时刻到k时刻的状态预测和预测方差矩阵；

针对机动目标状态向量的最小方差滤波方法表示为：

式中，表示粒子i从k时刻到k时刻的状态预测；

采用全概率公式对进行分解：

式中，是高斯概率密度；采用N个粒子去近似p(Γ_k-1|Z_k)，表示第i个粒子前k时刻的三状态的马尔科夫链集合：

式中，表示第i个粒子k-1时刻的重要性权值；

(2-2-2)对于每个粒子i，i＝1,...,N，更新k-1时刻的重要性权值，表达式如下：

其中，z_k为无人机中光学相机k时刻提供的量测信息：z_k＝[b_k c_k]^T，b_k和c_k为像素点在成像平面内k时刻的二维坐标位置，z_k＝h(X_k)+w_k，其中是k时刻的独立高斯噪声，R是量测噪声的协方差矩阵，且R＝diag(σ_b,σ_c)，σ_b和σ_c是量测噪声的标准差，h(X_k)是机动目标在成像平面内k时刻的真实二维坐标，其定义为：

式中， 和X_k分别是无人机与机动目标的相对位置在相机坐标系与惯性坐标系中的表示，是无人机在k时刻的状态向量，C_ci是惯性坐标系到相机坐标系的旋转矩阵；

量测方程的雅可比矩阵表示为：

式中，和分别为向量的第一个、第二个和第三个元素；

(2-3)对粒子进行重采样；具体步骤如下：

(2-3-1)计算粒子有效性并进行判定；

粒子有效性的计算表达式如下：

如果则进行重采样，其中N_T是预设的粒子有效性阈值，按照概率分布从中生成N个新的粒子进入步骤(2-3-2)；如果则进入步骤(2-4)；

(2-3-2)用新产生的粒子代替旧的粒子，使得 进入步骤(2-4)；

(2-4)结合光学相机提供的量测信息，输出对机动目标的位置估计和速度估计；具体步骤如下：

(2-4-1)对于每一个执行如下过程：

式中，表示第i个粒子k时刻的卡尔曼滤波增益，和分别表示第i个粒子从k时刻到k时刻的状态预测和预测方差矩阵；

(2-4-2)量测更新；

其中，是对的估计，即Σ_k|k是从k时刻到k时刻对的估计方差矩阵；即为基于粒子滤波方法k时刻生成的对机动目标位置估计的输出结果，为基于粒子滤波方法k时刻生成的对机动目标惯性坐标系下X方向和Y方向的速度估计的输出结果；

(3)利用李雅普诺夫制导向量法进行无人机跟踪路径规划，得到无人机进行机动目标跟踪的控制量；具体步骤如下：

(3-1)利用李雅普诺夫制导向量法进行无人机跟踪路径规划；

假设待跟踪的机动目标为静止，则无人机k时刻的期望速度为：

式中，为无人机k时刻在惯性坐标系下X方向和Y方向期望的相对速度， 表示无人机k时刻在惯性坐标系下X方向和Y方向的位置，u^d和r^d分别表示预定的盘旋速度和盘旋轨迹半径；当r_k＞r^d时，无人机向机动目标靠近直到期望的圆形轨道；当r_k＜r^d时，无人机远离机动目标直到期望的圆形轨道；当r_k＝r^d时，无人机在期望的圆形轨道上绕机动目标作盘旋运动；

将转换到极坐标为：

直角坐标系与极坐标系的关系为：

x_k＝r_kcosθ_k

y_k＝r_ksinθ_k

设计李雅普诺夫函数V为：

将V对时间求导：

当且仅当r_k＝0时，则无人机运动轨迹渐近收敛到期望的圆形轨道；

当机动目标移动时，无人机k时刻的期望速度和前进方向表达式分别如下：

式中，和分别表示无人机k时刻的期望速度和前进方向；

(3-2)计算无人机进行机动目标跟踪的控制量；

基于无人机期望的前进速度前进方向和无人机的当前状态采用比例控制方法控制无人机跟踪控制指令，实现对存在未知控制输入的地面机动目标的盘旋跟踪：

式中，和为无人机k时刻的控制量，分别控制无人机的前进速度和前进方向，和分别表示无人机k时刻的速度和前进方向，K₁和K₂为控制增益，和分别是和对时间的微分；

(4)令k＝k+1，对于i＝1,...,N，重新生成N个粒子重新返回步骤(2-2-1)，进行下一时刻无人机对于地面机动目标的盘旋跟踪。