[发明专利]一种人工鱼群算法优化的容积粒子滤波导航方法

摘要

本发明提供了一种人工鱼群算法优化的容积粒子滤波方法。该方法包括如下步骤：（1）初始化参数，并从先验分布中选取粒子。（2）采用容积卡尔曼滤波设计重要性密度函数，产生改进的建议分布函数，并从中选择新的粒子。（3）取得量测值。（4）计算粒子的重要性权值。（5）利用人工鱼群算法更新粒子的位置，使得粒子不断地向真实状态靠近。（6）更新粒子权值并归一化。（7）输出状态估计值和方差估计值。（8）判断是否结束。若是，则退出；若否，则返回第一步。该方法采用容积卡尔曼滤波设计重要性密度函数，并利用人工鱼群算法中觅食行为和聚群行为的交替，使粒子不断向高似然域移动，从而改善了粒子分布，提高了定位精度。

主权项

1.一种人工鱼群算法优化的容积粒子滤波方法，其特征在于，包括如下步骤：(1)初始化k＝0。对于i＝1,2,…,N，利用式(1)～(2)从先验分布p(X₀)中选取粒子(2)对于k＝1,2,…，采用容积卡尔曼滤波设计重要性密度函数，产生改进的建议分布函数，并从建议分布函数中重新选择N个粒子，具体过程为式(3)～(16)：计算容积点：时间更新：通过状态方程传播容积点：估计k时刻的状态预测值：估计k时刻的状态误差协方差预测值：量测更新：计算第i个粒子的容积点：通过量测方程传播容积点：估计k时刻第i个粒子的量测预测值：估计第i个粒子的自相关协方差阵：估计第i个粒子的互相关协方差阵：估计第i个粒子的卡尔曼增益矩阵：k时刻第i个粒子的状态估计值：k时刻第i个粒子的状态误差协方差估计值：重新产生粒子：其中，n为状态维数；(3)取得量测值zk：其中，z_k为最新量测值，为预测量测值，σ_v为观测噪声的方差；(4)对于i＝1,2,…,N，利用式(18)计算重要性权值：(5)利用人工鱼群算法更新粒子的位置，使得粒子不断地向真实状态靠近，定义目标函数Y：初始化人工鱼群视野Visual、移动步长step、拥挤度因子δ、最大重复次数、更新阈值和最大迭代次数num等参数；I.觅食行为，粒子通过比较目标函数不断更新自己向更真实的状态靠近，描述为式(20)：式(20)中，为第i个粒子k时刻的状态预测值。为对第m∈{1,2,…,n}次迭代后的值，rand为随机数，step为移动步长，分别为k时刻人工鱼i、j当前所在位置的食物浓度；II.聚群行为，设在范围Visual内的其他预测量测值集合为A_v，|A_v|为集合A_v的势，则其中心为聚群行为描述为式(21)：式(21)中，为k时刻第i个粒子在中心处的状态预测值，δ为拥挤度因子，为k时刻人工鱼i在中心处的食物浓度；通过限制迭代次数或者以目标函数前后偏差小于某阈值(误差的控制阈值)为条件来终止人工鱼群算法；(6)权值更新并归一化；(7)输出结果。输出状态估计值和方差估计值；(8)判断是否结束。若是，则退出本算法；若否，则返回步骤(1)。