CN110348560A - 一种基于改进萤火虫算法优化粒子滤波的轨迹预测的方法 - Google Patents

Abstract

本发明涉及一种基于改进萤火虫算法优化粒子滤波的轨迹预测的方法，在原始萤火虫算法的基础上，提出萤火虫算法新的位置更新策略和变步长策略，然后结合粒子滤波算法的运行机制用改进的萤火虫算法优化粒子滤波算法，然后将其应用在空域飞行目标轨迹预测中。在位置更新策略中采用全局优化思想，改善了粒子贫化现象，避免了计算的复杂度；在步长策略中改变传统算法中定步长的思想，利用非线性方程设计了步长动态调整方案，平衡了全局搜索和局部开发的能力。

Description

一种基于改进萤火虫算法优化粒子滤波的轨迹预测的方法

技术领域

本发明涉及一种基于改进萤火虫算法优化粒子滤波的轨迹预测的方法，其属于轨迹预测领域。

背景技术

随着空中交通密度的迅猛增长，以明确航路飞行的空中交通运行模式弊端愈加明显，已不能迎合未来空管***的运行需求。不管在军用还是民用空域交通管制***中都把飞行安全作为首要目标，如何实现对于空域目标的实时监控和预测，如何实现航空器与航空器之间、航空器与空中其它物体之间的实时准确告警，一直是空域管理探索的热点问题。

由于定位环境的复杂，往往会出现目标丢失的情况，会出现定位***轨迹不能连续跟踪和落点不能判断的情形。航迹预测目的是准确快速地判断飞行器在下一个时间点最可能出现的位置，完成轨迹修复和落点预测，更加高效地对飞行目标进行观察。随着航迹处理技术发展产生的一些成熟的算法都已成功应用于目标航迹预测：如卡尔曼滤波算法、粒子滤波算法、混沌神经网络以及神经网络结合遗传算法等，这些原始方法在轨迹预测中占据很大的优势，有着相对较高的精度。但在环境恶劣和设备故障等一些恶性情况下，飞行器的实时监视会受到破坏，容易造成飞行器监测数据的丢失，进而导致电子地图上显示不完整的目标轨迹，也就是航迹的缺失。在交通管理***中采取有效的措施，实时、准确、可靠的对空中飞行器轨迹预测提供精准的基于位置的服务是实现空中交通管理自动化、智能化的一个必要条件。因此，对飞行器实时航迹进行分析、预警和预测研究，保障飞行器实时监控正常稳定运行具有非常深远的意义。

现有的轨迹预测技术有扩展卡尔曼滤波技术和粒子滤波技术，但是扩展卡尔曼滤波轨迹预测存在一些缺点，扩展卡尔曼滤波技术在线性处理中忽略了二阶和二阶以上的高阶项，当初值选择不合适的时候,该算法可能不收敛，也就是算法的鲁棒性不强。而粒子滤波轨迹预测也存在一些缺点，粒子滤波算法的重采样会导致粒子的退化和样本贫化的现象，并且粒子滤波需要大量的粒子进行状态的估计。

发明内容

本发明所要解决的技术问题是提供了一种基于改进萤火虫算法优化粒子滤波的轨迹预测的方法。

为解决上述技术问题，本发明采用如下技术方案：

其包括如下步骤：

步骤1:初始化：采样N个粒子作为初始粒子样本，其中表示序号为i的粒子在初始时刻的状态；

步骤2:计算粒子i与j之间的吸引度β_i,j和全局最优值G_best,吸引度β_i,j的计算公式如下：

其中，r_i,j是粒子i和j之间的笛卡尔距离，计算式为其中，x_i,k表示粒子i在d维空间中的第k个分量， x_j,k表示粒子j在d维空间中的第k个分量；d为3维空间；

β₀是r_i,j＝0时的吸引度；

γ是光强吸收系数；

全局最优值G_best的计算公式如下：

G_best＝max{x_t,x_t+1}；

其中，x_t表示t时刻的位置坐标，x_t+1表示t+1时刻的位置坐标；

步骤3：根据吸引度β_i,j和变步长策略更新粒子位置，更新粒子位置的公式如下：

其中，分别表示粒子在t+1和t时刻所处空间位置；

表示拥有全局最优值的粒子j与粒子i位置之间的距离，其计算式为其中，x_i表示i粒子x方向上位置， y_i表示i粒子y方向上的位置，z_i表示i粒子z方向上的位置，x_best表示i粒子x方向最优位置，y_best表示i粒子y方向最优位置，z_best表示i粒子z方向最优位置；

η(t_N)表示步长因子，其非线性方程为η(t_N)＝0.4/(1+exp(0.015*(t_N-Maxgeneration)/3)),t_N是当前迭代次数， Maxgeneration是最大迭代次数；

步骤4：对粒子权重进行更新，更新的计算式如下：

其中，表示t时刻粒子i的权重，表示t-1时刻粒子i的权重，表示粒子i状态测量模型，表示i粒子在t-1时刻向t时刻状态转移模型，分别与观测方程和状态方程的概率形式对应，是由已知的参考分布q(x_0:t|y_1:t)按照序列重要性采样改写得到的；

步骤5:对粒子权重进行归一化处理，归一化处理的计算式如下：

其中，为i粒子在t时刻的权值。

步骤6:判断是否满足最大迭代次数，若否则返回步骤2；

步骤7:计算t时刻的状态估计值：

其中，x_t表示N个粒子t时刻的位置输出，表示i粒子t时刻的位置，表示i粒子t时刻的权重。

进一步的，步骤3中变步长策略为在初始阶段，采用大步长，随着搜索的进行，步长随迭代次数的增加呈非线性递减。

进一步的，本方法应用于在空域目标轨迹预测。

本发明的有益效果如下：

为更加高效的对空域飞行目标进行观察，针对在目标定位***中粒子滤波出现的定位***目标丢失，轨迹不能连续跟踪的问题，本发明是用改进萤火虫算法优化传统的粒子滤波算法，通过结合粒子滤波算法的运行机制对萤火虫算法位置更新策略和变步长策略进行了改进。在位置更新策略中采用全局优化思想，改善了粒子贫化现象，避免了计算的复杂度；在步长策略中改变传统算法中定步长的思想，利用非线性方程设计了步长动态调整方案，平衡了全局搜索和局部开发的能力。

本发明运用改进的萤火虫算法和粒子滤波机制结合，使粒子不断向高似然区移动，提高粒子有效性。

具体实施方式

下面结合具体实施方式对本发明的技术方案和有益效果做进一步说明。本实施例涉及一种基于改进萤火虫算法优化粒子滤波的轨迹预测的方法，在原始萤火虫算法的基础上，提出萤火虫算法新的位置更新策略和变步长策略，然后结合粒子滤波算法的运行机制用改进的萤火虫算法优化粒子滤波算法，然后将其应用在空域飞行目标轨迹预测中。在位置更新策略中采用全局优化思想，改善了粒子贫化现象，避免了计算的复杂度；在步长策略中改变传统算法中定步长的思想，利用非线性方程设计了步长动态调整方案，平衡了全局搜索和局部开发的能力。运用改进的萤火虫算法和粒子滤波机制结合，使粒子不断向高似然区移动，提高粒子有效性。本实施例所涉及的方法具有较高的快速性和较好的估计精度。

现有的轨迹预测技术有扩展卡尔曼滤波技术和粒子滤波技术，接下来对两种技术进行描述。

其一、扩展卡尔曼滤波轨迹预测

经典Kalman滤波器假设动力学***和观测***都是线性的，然而，实际工程应用多为非线性***，此时则必须考虑是使用扩展卡尔曼滤波等非线性处理办法进行状态估计，扩展卡尔曼滤波与卡尔曼滤波最大的不同是将非线性***线性化。虽然形式上与卡尔曼相似，但是由于采用线性逼近，实际上已不是最优估计，而是一种限制复杂性的滤波器。

在非线性情况下，通泰勒级数相似，可以利用状态方程和观测方程的偏导数在当前的估计值处线性化来计算状态估计值。假设***状态向量x∈Rⁿ，观测变量 z∈R^m，则状态方程和观测方程可以表示为：

z(t)＝h[x(t)]+V(t) (2)

其中，x(t)是n维状态变量矩阵；W(t)是过程噪声矩阵，假定其实零均值高斯白噪声，且

E{W(t)W^T(t)}＝Q(t)δ(t-τ)；δ(t-τ)是单位脉冲响应，即z(t)是m维观测矩阵；V(t)是观测噪声，假定其为零均值高斯白噪声，且与W(t)不相关，即E{V(t)V^T(t)}＝R(t)δ(t-τ)。

为了进行计算机数值模拟计算，即将式(1)离散化。由于

式(1)对x^T进行求导运算，则

将其带入式(3)，于是

记

并在距x(t)不远的处进行泰勒级数展开：

再记：

x(t+Δt)＝x_k (9)

x(t)＝x_k-1 (10)

W(t)Δt＝W_k-1 (11)

则式(5)可表示为

式(12)中

同样，对式(2)的观测方程进行离散化，将h(x)在x附近处进行泰勒展开，得：

取则

式中

记且将式(17)中的U_k-1看作外加项，式(14)的后两项看作测量***误差，则***状态方程和测量方程可表示为：

z_k＝H_kx_k+S_k+V_k (17)

利用扩展卡尔曼进行目标估计预测时，预测方程主要包括***状态方程和观测方程。

1)状态方程建立

取x、y、z、v_x、v_y、v_z作为扩展卡尔曼滤波的状态变量，即

X＝(x₁,x₂,x₃,x₄,x₅,x₆)^T＝(x y z v_x v_y v_z)^T (18)

则式(17)所示的方程组可改写为：

由于非线性方程式(19)只是对目标运动的近似描述，总存在一些误差。为了补偿这个误差，引入一个噪声向量W作为状态扰动量，则状态方程可表示为：

其中W为零均值高斯白噪声，且服从N(0,Q)的正态分布。

2)观测方程建立

在进行目标定位预测时，由于关注的是目标位置，即目标(x,y,z)，因此可定义***观测向量

则可得观测方程

Z＝h(X)+V＝[x y z]^T+V (22)

其中，V是***观测噪声，设置其为零均值的高斯白噪声，服从方差为R的正态分布，即V～(0,R)。

将(20)(22)所示的状态方程和观测方程分别加入时间信息

Z(t)＝h(X)+V(t) (24)

其中，W(t)和V(t)分别为过程噪声和观测噪声，且都为零均值高斯白噪声。直接对式(23)进行扩展卡尔曼递推滤波计算，其中Δt为采样间隔，状态转移矩阵取式(7)的前两项，做近似计算，则状态转移矩阵为：

由式(18)可得

其中，A₄₂＝-x₄c(b₂-b₄),A₄₅＝-x₄x₅cb₃,A₄₂＝-x₄c(b₂-b₄)；

又因为：

h＝[x y z]^T (27)

由(15)(24)可知观测矩阵为：

在扩展卡尔曼轨迹预测中，除状态转移矩阵和观测矩阵外，初始值X₀、误差协方差矩阵初始值P₀、过程噪声协方差Q和观测噪声协方差R都必须设置。

[1]初始值

在目标估计预测中，利用已获取的时差数据进行定位，并以定位段末点为轨迹预测的初始值，即：

X＝[x₀ y₀ z₀ v_x0 v_y0 v_z0]^T (29)

其中x₀、y₀、z₀、v_x0、v_y0、v_z0分别是定位段末点的位置和速度。

[2]误差协方差矩阵初始值P₀

其中分别为三维方向上位置和速度的误差方差。

[3]过程噪声协方差Q

其中分别为三维方向上目标位置和速度的过程噪声方差。

[4]观测噪声协方差R

其中,分别为x、y、z方向的观测噪声方差。

在各参数矩阵设置完成后，代入扩展卡尔曼滤波算法，则

预测方程：

预测方差：

观测量预测方程：

滤波方程：

增益矩阵：

滤波方差：

P_k＝(I-K_kH_k)P_k/k-1 (38)

其二、粒子滤波轨迹预测：

粒子滤波采用的信号处理模型是用状态方程和观测方程来表示的，即

***方程

x_t＝f_t(x_t-1,w_t) (39)

观测方程

y_t＝h_t(x_t,v_t) (40)

粒子滤波实质是递推贝叶斯滤波的一种实现形式，在每个时刻t，利用所获得的实时信息D_t求得状态x_t的后验概率密度函数p(x_t|D_t),t∈N，从而得到t时刻的状态估计：

因此，后验概率密度函数p(x_t|D_t)，t∈N的计算就比不可少。他可以通过预测和更新2个步骤完成。

1)预测。利用***方程观测状态的先验概率密度。假设已经得到t-1时刻的后验概率密度p(x_t-1|D_t-1)，t∈N，则t时刻状态的先验概率密度为

p(x_t|D_t-1)＝∫p(x_t|x_t-1)p(x_t-1|D_t-1)dx_t-1 (42)

2)更新。利用贝叶斯规则，用t时刻的量测值y_t来修正状态的先验概率密度，从而得到该时刻的后验概率密度，即：

(42)(43)构成贝叶斯滤波的最优解，但是其解析解只对有限的模型成立。对于很多情况，其解析解并不存在，即使存在，求其统计量仍面临着高维积分问题，不便于计算，这时提出用逼近算法来解决积分问题，常用的是MonteCarlo方法。利用该方法如果能从p(x_0:t|y_1:t)抽取N个样本，状态的PDF可以用经验分布逼近为

无人机的俯仰角是水平面和无人机机身之间的夹角，偏航角是实际航线和计划航线之间的夹角，而横滚角则是无人机的实际航向和机身之间的夹角。在无人机飞行中，如果知道起始航迹点信息跟该时刻的姿态角(俯仰角、横滚角、偏航角) 以及速度，便可以得到下一个时刻的航迹坐标。因此对无人机的航迹预测便可以转变为对姿态角的预测，即通过粒子滤波来实现对这些姿态角的预测，而间接实现对无人机飞行航迹的预测。取俯仰角作为状态参数x(t)，升降舵偏度指令作为输入参数u(t)，观测向量y(t)取无人机真实飞行时采集的俯仰角值。通过粒子滤波，便可由t时刻的x(t)、u(t)、y(t)得到t+1时刻俯仰角的预测值x(t+1)。

首先在t时刻，根据俯仰角的先验条件概率产生一组随机样本，这组随机样本称为粒子。通过***方程，由t时刻采集到的随机样本便可以得到t+1时刻的预测样本，然后根据观测向量和预测样本。使用贝叶斯法得到后验概率。再根据权值更新公式，计算t时刻样本的权值w_t，这样便得到了t时刻俯仰角的预测值以此类推，便可以完成整个粒子滤波过程。

本实施例所涉及方法的核心内容如下：

萤火虫种群利用短促有节奏的光作为个体间信息的传递介质,萤火虫发出光亮作为信号***的主要标识，其目的是通过发光特性，吸引其他的萤火虫个体,个体持续向位置较优的个体移动(也就是亮度大的个体)。FA算法通过对此生物特性的简化与模拟具有参数少、易于实现、寻优能力强等优点，理想化简化准则如下：1)假设萤火虫种群中的个体不分性别且相互吸引，个体将会向比它自身更亮的萤火虫方向移动，从而完成位置迭代。2)个体间的吸引力只与亮度和距离有关，个体间吸引力正比于萤火虫发出的亮度,光亮强的个体会吸引周围发光弱的个体，但随着距离的增大，个体间吸引力逐渐减小。如果没有人比特定的萤火虫更亮的个体，它会随机移动；3)萤火虫的亮度由待优化的目标函数决定。可见，亮度决定萤火虫个体所在位置的好坏以及移动方向，吸引度决定其移动的距离。

完成目标优化就是依据亮度和吸引度的不断更新实现。萤火虫算法优化原理用数学公式描述如下：

1)萤火虫i与j相对亮度

L₀萤火虫原始荧光亮度；γ是光强吸收系数，荧光会随着距离的增加和传播媒介的吸收逐渐减弱；r_i,j是萤火虫i和j之间的笛卡尔距离,即为:

2)萤火虫i与j的吸引度

其中，β₀是r_i,j＝0时的吸引度；γ是光强吸收系数；r_i,j是萤火虫i和j之间的空间距离。

3)萤火虫i被亮度更大的萤火虫j吸引向其移动而更新自己的位置，位置更新公式如下：

其中是在t时刻萤火虫j和i所处的空间位置；η∈[0,1]是步长因子；rand是[0,1]区间上服从均匀分布的随机因子。

传统的粒子滤波重采样思想是通过大量繁殖高权重粒子，淘汰小权值粒子集以避免粒子匮乏的现象,但通过多次迭代寻优,出现了权值退化、粒子多样性匮乏等问题，直接导致了滤波精度的下降。FA虽然具有很多的优点，但它也存在缺点，个体间相互吸引的行为必须发生在感知范围内并且依靠个体所提供的信息，它依赖的是在自己搜索范围内亮度比自身更亮的个体，对优秀个体的依赖程度太高，若感知范围内没有自身更亮的个体则萤火虫做随机运动。单考虑两个体间距离和吸引度对其的影响，忽略个体自身亮度的影响，会导致算法在迭代初期搜索能力很弱。随着迭代次数的增加，个体与最优值之间的距离越来越小，在个体向最优值趋近的过程中，极有可能出现萤火虫移动的距离大于个体与最优值间距的情形，使个体在更新自己的位置时跳过了最优值。如果这种情况发生多次(即在最优值附近出现震荡)，严重影响算法的收敛精度和速度。为此将对萤火虫算法进行相应的改进，介绍一种基于改进萤火虫算法优化粒子滤波的轨迹预测新方法。

(1)萤火虫算法改进：位置更新策略

根据公式(48)可以发现，每只萤火虫都会向决策与半径内亮度比自身强的个体移动。此时萤火虫的寻优与周围其他个体的亮度有关，但是如果每一次寻优都需要粒子i(i＝1,2,…N,i≠j)和其他的个体j(j＝1,2,…N,j≠i)进行比较运算，这会导致运算的复杂度非常高。在本研究中,将粒子滤波中的粒子比作萤火虫个体, 在位置更新中引入全局最优值思想。在迭代过程中将每次迭代前一时刻与下一时刻的目标函数值进行对比,从而得到当前滤波时刻所有粒子所经历的全局最优值, 利用全局最优值的粒子i与粒子j进行信息交互。在改进的算法中，使用笛卡尔距离计算拥有全局最优值的粒子j与粒子i位置之间的距离：

重新定义萤火虫算法的位置更新公式:

其中是萤火虫在t+1和t时刻所处的空间位置；是i和j个体间最优距离；G_best是全局最优值；η∈[0,1]是步长因子；rand是[0,1]区间上服从均匀分布的随机因子。

公式(50)利用全局优化的思想指导粒子整个移动寻优过程。从理论角度分析，粒子的经历时间段内全局最优值只有一个，因此粒子滤波中的粒子i只需与当前时刻的最优值进行对比,避免了计算的复杂度，可以显著提高萤火虫优化部分的全局寻优能力。

(2)萤火虫算法改进：动态调整步长机制

根据FA算法可以知道,个体间吸引度会随着距离的增加而减弱。每个粒子都有相同的步长设置，可能会陷入局部最优，导致早熟收敛。从萤火虫i与j之间的吸引度公式中可以知道，粒子i与j之间间隔距离很大时，也就是r_i,j→∞时，两者之间的吸引度为0，则位置更新公式就会变成：

从公式(51)可以知道，个体在t时刻的位置更新与其他亮度更高的个体无关。因此可以得到如下结论：FA算法运行初期，种群的分散会导致个体间距过大，即 r_i,j→∞。如果此时步长η取值过小，那么所在位置不好的个体就不能被位置佳的个体吸引，只能按照上式进行位置的更新，以较小的步长进行局部寻优。

当个体间距离很小时，光强吸收系数γ→0,此时的β_i,j≈β₀，即为常数1，那么此时的位置更新公式也为(51)。

FA算法运行后期，个体间距离较小γ→0算法即将收敛的时候，如果步长η取值过大，那么位于劣势位置的个体会更新自身位置，而导致跳过最优位置，多次发生这种情况时，会使算法发生振荡现象，进而降低收敛速度。

在标准的FA中，萤火虫的运动是基于固定的步长。这显然是不合适的，可能会影响全局搜索和局部搜索之间的平衡。因此，在不同的阶段，需要动态调整适当的步长值。在本研究中，我们在搜寻阶段采用变步长策略。在初始阶段，我们采用大步长，随着搜索的进行，步长随迭代次数的增加呈非线性递减。本文利用非线性方程，设计了步长动态调整方案。步长η的动态调整如下式：

η(t_N)＝0.4/(1+exp(0.015*(t_N-Maxgeneration)/3)),t_N是当前迭代次数，Maxgeneration是最大迭代次数；

该线性方程步长在早期是大的，然后随着迭代的增加而减少。这有助于算法平衡全局搜索和局部开发的能力。

在本研究中,将粒子滤波中的粒子比作萤火虫个体,在位置更新中引入全局最优值思想和变步长策略，位置更新完成之后，对粒子的权重进行更新，得到t时刻状态估计的值，以此类推，便可以完成整个粒子滤波过程。

粒子滤波算法，扩展卡尔曼滤波算法，人工物理优化的粒子滤波算法，蝴蝶算法优化的粒子滤波算法等都可以实现目标轨迹的预测，但是改进萤火虫算法优化粒子滤波算法更适合应用于空域目标轨迹的预测，因为它能够有效的解决粒子贫化问题和计算复杂问题。

进一步的，本发明中相应术语缩写解释如下：

粒子滤波：Particle Filter(PF)

萤火虫算法：Firefly Algorithm(FA)

改进萤火虫优化粒子滤波：Firefly Algorithm-Particle Filter(FA-PF)。

最后应说明的是：以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明实施例技术方案的精神和范围。

Claims (3)

1.一种基于改进萤火虫算法优化粒子滤波的轨迹预测的方法，其特征在于，其包括如下步骤：

步骤1:初始化：采样N个粒子作为初始粒子样本，其中表示序号为i的粒子在初始时刻的状态；

步骤2:计算粒子i与j之间的吸引度β_i,j和全局最优值G_best,吸引度β_i,j的计算公式如下：

其中，r_i,j是粒子i和j之间的笛卡尔距离，计算式为其中，x_i,k表示粒子i在d维空间中的第k个分量，x_j,k表示粒子j在d维空间中的第k个分量；d表示3维空间；

β₀是r_i,j＝0时的吸引度；

γ是光强吸收系数；

全局最优值G_best的计算公式如下：

G_best＝max{x_t,x_t+1}；

其中，x_t表示t时刻的位置坐标，x_t+1表示t+1时刻的位置坐标；

步骤3：根据吸引度β_i,j和变步长策略更新粒子位置，更新粒子位置的公式如下：

其中，分别表示粒子在t+1和t时刻所处的空间位置；

表示拥有全局最优值的粒子j与粒子i位置之间的距离，其计算式为其中，x_i表示i粒子x方向上位置，y_i表示i粒子y方向上的位置，z_i表示i粒子z方向上的位置，x_best表示i粒子x方向最优位置，y_best表示i粒子y方向最优位置，z_best表示i粒子z方向最优位置；

η(t_N)表示步长因子，其非线性方程为η(t_N)＝0.4/(1+exp(0.015*(t_N-Maxgeneration)/3)),其中，t_N是当前迭代次数，Maxgeneration是最大迭代次数；

步骤4：对粒子权重进行更新，更新的计算式如下：

其中，表示t时刻粒子i的权重，表示t-1时刻粒子i的权重，表示粒子i状态测量模型，表示i粒子在t-1时刻向t时刻状态转移模型，分别与观测方程和状态方程的概率形式对应，是由已知的参考分布q(x_0:t|y_1:t)按照序列重要性采样改写得到的；

步骤5:对粒子权重进行归一化处理，归一化处理的计算式如下：

其中，为i粒子在t时刻的权值；

步骤6:判断是否满足最大迭代次数，若否则返回步骤2；

步骤7:计算t时刻的状态估计值：

其中，x_t表示N个粒子t时刻的位置输出，表示i粒子t时刻的位置，表示i粒子t时刻的权重。

2.根据权利要求1所述的基于改进萤火虫算法优化粒子滤波的轨迹预测的方法，其特征在于，步骤3中变步长策略为随着搜索的进行，步长随迭代次数的增加呈非线性递减。

3.根据权利要求1所述的基于改进萤火虫算法优化粒子滤波的轨迹预测的方法，其特征在于，其应用于在空域目标轨迹预测。