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

摘要

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

主权项

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＝{γ0,...,γk}表示机动目标前k时刻的三状态的马尔科夫链集合，Zk＝{z0,...,zk}表示前k时刻无人机搭载相机对机动目标的量测集合，Uk＝{ut(γ0),...,ut(γ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时，无人机在期望的圆形轨道上绕机动目标作盘旋运动；将转换到极坐标为：直角坐标系与极坐标系的关系为：xk＝rkcosθkyk＝rksinθ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)，进行下一时刻无人机对于地面机动目标的盘旋跟踪。