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

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

【技术实现步骤摘要】
基于李雅普诺夫制导向量和粒子滤波的机动目标跟踪方法
本专利技术涉及一种基于李雅普诺夫制导向量和粒子滤波的机动目标跟踪方法，属于无人机目标跟踪领域。
技术介绍
随着无人机技术在军用、民用领域的快速发展，应用无人机跟踪地面机动目标已经成为一种新趋势，比如边界巡防、救灾防护、运输护航等。一般情况下，无人机通过自身搭载的光学相机捕捉目标，相机的焦距需要随着目标与无人机的距离变化而调整，但是，在运动过程中调节相机焦距是很困难的。自然而然，我们希望无人机在跟踪过程中保持与被跟踪目标之间的相对距离不变。因此，控制无人机按照圆形轨道环绕机动目标作盘旋运动是一种很好的解决方案，跟踪示意图如图1所示，图中坐标系为惯性坐标系，坐标原点为工作站，X轴指向正东，Y轴指向正北，Z轴与X轴和Y轴构成右手坐标系，垂直地面指向正上方，无人机的期望轨道是以被跟踪目标为圆心，以rd为半径的圆，示意图中曲线A表示机动目标的运动轨迹，视轴即为相机的视线，无人机本体坐标的原点在无人机的质心，X轴与无人机纵轴平行，指向机头方向，Y轴与无人机的横轴平行，指向机身左侧，Z轴与X、Y轴构成右手坐标系，垂直X-Y平面向上。目前的技术只能对非机动目标进行跟踪，即目标的运动状态不会发生改变，总是保持静止或者匀速直线运动。针对这类非机动目标的跟踪多采用卡尔曼滤波或者扩展卡尔曼滤波方法对目标运动状态进行估计。当目标存在未知控制输入，即目标的前进速度和前进方向会发生改变时，对目标运动状态的估计将会变得很困难。而实际跟踪问题中的目标大多为机动目标，针对存在未知控制输入的机动目标的研究还比较少。粒子滤波方法可以对系统中的不确定性进行估计，因此采用粒子滤波方法估计机动目标的未知控制输入是一种很好的方法。目前，粒子滤波方法多被用于解决目标跟踪过程中量测信息丢失的问题，通过构建粒子滤波器，对量测信息的有效性进行估计。当被跟踪机动目标的实际状态信息已知时，采用李雅普诺夫制导向量法进行路径规划已经被广泛研究了，该方法可以通过机动目标与无人机的位置产生制导指令，从而引导无人机按照预定的半径盘旋跟踪机动目标。当被跟踪目标的位置、速度等状态信息未知时，如何进行路径规划，从而产生期望指令还没有被研究。无人机系统功能模块构成示意图如图2，从图中可以看出，制导模块和平台控制模块都依赖于运动估计模块输出的位置、速度信息。运动估计模块可以结合相机提供的量测信息，对机动目标的运动状态进行有效估计，制导模块基于运动估计器的输出和无人机自带导航模块的输出信息产生制导指令，飞行控制模块根据制导模块产生的制导指令和无人机自带导航模块的输出信息产生无人机控制指令，控制无人机实现对存在未知控制输入机动目标的盘旋跟踪。光学相机通过两轴稳定平台固定在无人机的底部，该两轴平台可以隔离无人机的扰动，保持相机的视轴始终指向被跟踪机动目标，从而避免目标丢失的问题，两轴平台的控制技术已经非常成熟，在此不作过多说明。
技术实现思路
本专利技术的目的是为克服被跟踪机动目标存在未知控制输入，导致状态信息难以估计以及难以跟踪的问题，提出一种基于李雅普诺夫制导向量和粒子滤波的机动目标跟踪方法。本专利技术方法，采用粒子滤波方法可以有效估计机动目标的运动状态，基于李雅普诺夫制导向量的路径规划方法可以进行实时路径规划，可实现对机动目标进行有效跟踪。本专利技术提出的一种基于李雅普诺夫制导向量和粒子滤波的机动目标跟踪方法，其特征在于，该方法包括以下步骤：(1)当无人机确定待跟踪的机动目标后，对机动目标建立动力学模型，表达式如下：其中,为待跟踪的机动目标在k时刻的状态向量，上标t表示机动目标，下标k表示第k个采样时刻，为机动目标k时刻在惯性坐标系中的三维坐标位置，为机动目标k时刻在惯性坐标系下X方向和Y方向的速度；Fk为k时刻的机动目标系统矩阵，Bk为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(γ0|γ-1)随机生成N个粒子其中γ-1＝1，并使Σ0|0＝Σ0，为第i个粒子-1时刻的重要性权值，N为粒子总数，和Σ0|0分别为从0时刻到0时刻的机动目标状态估计和估计方差矩阵，x0和Σ0分别为粒子滤波方法设定的机动目标初始状态和初始方差矩阵Σ0＝α*diag(5)，α＞0，；(2-2)对每个粒子进行重要性采样；具体步骤如下：(2-2-1)令当前时刻为k，k＞0，更新对每个粒子的状态预测和预测方差矩阵，表达式分别如下：式中，和分别为粒子i从k-1时刻到k时刻的状态预测和预测方差矩阵；针对机动目标状态向量的最小方差滤波方法表示为：式中，表示粒子i从k时刻到k时刻的状态预测；采用全概率公式对进行分解：式中，是高斯概率密度；采用N个粒子去近似p(Γk-1|Zk)，表示第i个粒子前k时刻的三状态的马尔科夫链集合：式中，表示第i个粒子k-1时刻的重要性权值；(2-2-2)对于每个粒子i，i＝1,...,N，更新k-1时刻的重要性权值，表达式如下：其中，zk为无人机中光学相机k时刻提供的量测信息：zk＝[bkck]T，bk和ck为像素点在成像平面内k时刻的二维坐标位置，zk＝h(Xk)+wk，其中是k时刻的独立高斯噪声，R是量测噪声的协方差矩阵，且R＝diag(σb,σc)，σb和σc是量测噪声的标准差，h(Xk)是机动目标在成像平面内k时刻的真实二维坐标，其定义为：式中，和Xk分别是无人机与机动目标的相对位置在相机坐标系与惯性坐标系中的表示，是无人机在k时刻的状态向量，Cci是惯性坐标系到相机坐标系的旋转矩阵；量测方程的雅可比矩阵表示为：式中，和分别为向量的第一个、第二个和第三个元素；(2-3)对粒子进行重采样；具体步骤如下：(2-3-1)计算粒子有效性并进行判定；粒子有效性的计算表达式如下：如果则进行重采样，其中NT是预设的粒子有效性阈值，按照概率分布从中生成N个新的粒子进入步骤(2-3-2)；如果则进入步骤(2-4)；(2-3-2)用新产生的粒子代替旧的粒子，使得进入步骤(2-4)；(2-4)结合光学相机提供的量测信息，输出对机动目标的位置估计和速度估计；具体步骤如下：(2-4-1)对于每一个执行如本文档来自技高网...

【技术保护点】
1.一种基于李雅普诺夫制导向量和粒子滤波的机动目标跟踪方法，其特征在于，该方法包括以下步骤：(1)当无人机确定待跟踪的机动目标后，对机动目标建立动力学模型，表达式如下：

【技术特征摘要】
1.一种基于李雅普诺夫制导向量和粒子滤波的机动目标跟踪方法，其特征在于，该方法包括以下步骤：(1)当无人机确定待跟踪的机动目标后，对机动目标建立动力学模型，表达式如下：其中,为待跟踪的机动目标在k时刻的状态向量，上标t表示机动目标，下标k表示第k个采样时刻，为机动目标k时刻在惯性坐标系中的三维坐标位置，为机动目标k时刻在惯性坐标系下X方向和Y方向的速度；Fk为k时刻的机动目标系统矩阵，Bk为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(γ0|γ-1)随机生成N个粒子其中γ-1＝1，并使Σ0|0＝Σ0，为第i个粒子-1时刻的重要性权值，N为粒子总数，和Σ0|0分别为从0时刻到0时刻的机动目标状态估计和估计方差矩阵，x0和Σ0分别为粒子滤波方法设定的机动目标初始状态和初始方差矩阵Σ0＝α*diag(5)，α＞0，；(2-2)对每个粒子进行重要性采样；具体步骤如下：(2-2-1)令当前时刻为k，k＞0，更新对每个粒子的状态预测和预测方差矩阵，表达式分别如下：式中，和分别为粒子i从k-1时刻到k时刻的状态预测和预测方差矩阵；针对机动目标状态向量的最小方差滤波方法表示为：式中，表示粒子i从k时刻到k时刻的状态预测；采用全概率公式对进行分解：式中，是高斯概率密度；采用N个粒子去近似p(Γk-1|Zk)，表示第i个粒子前k时刻的三状态的马尔科夫链集合：式中，表示第i个粒子k-1时刻的重要性权值；(2-2-2)对于每个粒子i，i＝1,...,N，更新k-1时刻的重要性权值，表达式如下：其中，zk为无人机中光学相机k时刻提供的量测信息：zk＝[bkck]T，bk和ck为像素点在成像平面内k时刻的二维坐标位置，zk＝h(Xk)+wk，其中是k时刻的独立高斯噪声，R是量测噪声的协方...

【专利技术属性】
技术研发人员：游科友，董斐，张家绮，宋士吉，
申请(专利权)人：清华大学，
类型：发明
国别省市：北京,11