CN109307511A - 一种人工鱼群算法优化的容积粒子滤波导航方法 - Google Patents

Abstract

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

Description

一种人工鱼群算法优化的容积粒子滤波导航方法

技术领域

本发明涉及的是一种粒子滤波方法，特别是涉及一种人工鱼群算法优化的容积粒子滤波导航方法，属于非线性滤波导航技术领域。

背景技术

非线性非高斯系统状态估计问题广泛存在于运载体(移动机器人、水下航行器等)导航领域，备受许多学者关注。

粒子滤波(particle filter，PF)是一种处理非线性非高斯系统的有效方法，该方法利用大量的随机样本描述概率分布，并在测量的基础上调节各粒子权值的大小和样本位置来近似实际后验概率分布，因此该方法在任何非线性非高斯系统中都可以进行估计。由于粒子需要从重要性密度函数(或称为建议分布)中抽取，因此重要性密度函数的选择就影响着粒子滤波性能的好坏。而传统的粒子滤波方法采用未含当前最新测量数据的状态转移先验分布作为重要性密度函数，引入了较大的权重方差，无法很好地逼近后验概率，尤其当量测数据出现在转移概率分布的尾部或似然函数同转移概率分布相比过于集中时(如呈尖峰型)，粒子滤波可能失败。

为了很好地解决粒子滤波出现的上述问题，文献《Cubature粒子滤波》(系统工程与电子技术,2011,33(11):2554-2557)提出容积粒子滤波，将容积卡尔曼滤波(CubatureKalman Filter，即CKF)引入到粒子滤波框架中，在先验分布更新阶段融入了最新的量测数据，采用CKF设计粒子滤波的重要性密度函数，使重要性密度函数更加接近系统状态后验概率密度，提高了滤波估计精度。

上述容积粒子滤波方法在精度和稳定性方面仍有提高的余地。

发明内容

本发明的目的在于提供一种精度更高、稳定性更强的运载体自主导航定位方法。该方法从先验分布中选取粒子，采用容积卡尔曼滤波设计重要性密度函数，产生新的建议分布，从中重新选择粒子，利用人工鱼群算法中觅食行为和聚群行为的交替，使得粒子不断向高似然域移动，从而改善粒子分布，提高定位精度。

为实现上述目的，本发明包括下列步骤：

(1)初始化k＝0。对于i＝1,2,…,N，利用式(22)～(23)从先验分布p(X₀)中选取粒子

(2)对于k＝1,2,…，采用容积卡尔曼滤波设计重要性密度函数，产生新的建议分布，并从中重新选择粒子。具体过程为式(24)～(37)。

计算容积点：

时间更新：

通过状态方程传播容积点：

估计k时刻的状态预测值：

估计k时刻的状态误差协方差预测值：

量测更新

计算第i个粒子的容积点：

通过量测方程传播容积点：

估计k时刻的第i个粒子的量测预测值：

估计第i个粒子的自相关协方差阵：

估计第i个粒子的互相关协方差阵：

估计第i个粒子的卡尔曼增益矩阵：

k时刻第i个粒子的状态估计值：

k时刻第i个粒子的状态误差协方差估计值：

重新产生粒子：

其中，n为状态维数。

(3)取得量测值z_k。

其中，z_k为最新量测值，为预测量测值，σ_v为观测噪声的方差。

(4)对于i＝1,2,…,N，利用式(39)计算重要性权值。

(5)利用人工鱼群算法更新粒子的位置，使得粒子不断地向真实状态靠近。定义目标函数Y：

初始化人工鱼群视野Visual、移动步长step、拥挤度因子δ、最大重复次数、更新阈值和最大迭代次数num等参数。

I.觅食行为。粒子通过比较目标函数不断更新自己向更真实的状态靠近，描述为式(41)：

式(41)中，为第i个粒子k时刻的状态预测值。为对第m∈{1,2,…,n}次迭代后的值。rand为随机数，step为移动步长。分别为k时刻人工鱼i、j当前所在位置的食物浓度。

II.聚群行为。设在范围Visual内的其他预测量测值集合为A_v，|A_v|为集合A_v的势，则其中心为聚群行为描述为式(42)：

式(42)中，为k时刻第i个粒子在中心处的状态预测值，δ为拥挤度因子，为k时刻人工鱼i在中心处的食物浓度。

通过限制迭代次数或者以目标函数前后偏差小于某阈值(误差的控制阈值)为条件来终止人工鱼群算法。

(6)权值更新并归一化。根据更新权值。根据对取出的N₁个粒子进行权值归一化。

(7)输出结果。输出状态估计值和方差估计值

(8)判断是否结束。若是，则退出本算法。若否，则返回步骤(1)。

本发明的有益效果在于：本发明提供了一种人工鱼群算法优化的容积粒子滤波方法。该方法将人工鱼群算法与容积粒子滤波相融合，将人工鱼群算法中的觅食行为和聚群行为引入到容积粒子滤波中，采用容积卡尔曼滤波设计重要性密度函数，更新每个粒子，并利用人工鱼群算法中觅食行为和聚群行为的交替，使得粒子不断向高似然域移动，改善了粒子分布，提高了导航定位精度。

附图说明

图1为人工鱼群算法优化的容积粒子滤波方法设计框图；

图2为全局坐标系与自主水下航行器船体坐标系；

图3为GPS真实轨迹与三种导航方法估计轨迹对比曲线；

图4为东向误差曲线比较；

图5为北向误差曲线比较。

具体实施方式

下面给出本发明的优选实施方式，并结合附图和自主水下航行器(AutonomousUnmanned Vehicle，即AUV)海试例证加以说明。

如附图1所示，本发明所提供的一种人工鱼群算法优化的容积粒子滤波方法(Improved Cubature Particle Filter，简称ICPF)是通过如下步骤实现的：

(1)选取AUV运动模型

如附图2所示，以AUV初始位置和初始艏向角建立全局坐标系L；V是AUV船体坐标系；E为北东坐标系，North方向为地磁北向。x、y为AUV在L中的位置；ψ为AUV 在L中的艏向角，显然其中z_ψ为采用运动传感器OCTANS测得的AUV艏向角。

本发明选取式(43)一个简单的四自由度、常速动力学模型x_k＝f(x_k-1)+w_k-1对AUV的运动过程进行建模：

式中，[x,y,z,ψ]表示AUV在L中的位置和艏向；[u,v,w,r]表示AUV在V中相应的线速度和角速度；k表示任意采样时刻；T为航位推算传感器的采样时间间隔；w_k-1为随机系统噪声。

(2)选取AUV测量模型

AUV配置了深度计、运动传感器OCTANS和多普勒计程仪DVL。深度计即压力传感器，通过测量水柱压力提供AUV的深度数据；AUV通过OCTANS实时测量其艏向角，即 AUV艏艉向与地磁北之间的夹角；DVL可以测量海流速度、对底跟踪速度等，AUV在海试中使用DVL进行对底跟踪速度的测量。它们提供状态向量中深度、艏向和对底速度的直接测量值，因而观测模型为线性的。本发明选取式(44)的测量模型z_k＝h(x_k)+v_k对AUV 的传感器测量进行建模：

z_k＝Hx_k+v_k (44)

式中，z_k是观测向量，v_k是观测噪声，测量矩阵H为：

(3)通过AUV运动模型和测量模型构建动态系统

根据AUV运动模型和测量模型构建动态系统：

式中，z_k是观测向量；随机系统噪声 w_k～N(0,Q_k)，随机观测噪声v_k～N(0,R_k)，系统初始状态为x₀，x₀与w_k，v_k统计独立；非线性函数f(x_k-1)和h(x_k)为关于状态的一阶连续偏导数：

(4)滤波器参数初始化：

自主水下航行器初始时刻的状态x₀＝[0]_8×1，初始时刻的状态误差协方差P₀，系统噪声 Q，观测噪声R，粒子数N。初始化k＝0。对于i＝1,2,…,N，利用式(46)～(47)从先验分布 p(X₀)中选取粒子

(5)对于k＝1,2,…，采用容积卡尔曼滤波设计重要性采样密度函数。重要性采样：对于i＝1,2,…,N，采用容积卡尔曼滤波更新每个粒子。具体过程为式(48)～(61)。

计算容积点：

时间更新：

通过状态方程传播容积点：

估计k时刻的状态预测值：

估计k时刻的状态误差协方差预测值：

量测更新

计算第i个粒子的容积点：：

通过量测方程传播容积点：

估计k时刻第i个粒子的量测预测值：

估计第i个粒子的自相关协方差阵：

估计第i个粒子的互相关协方差阵：

估计第i个粒子的卡尔曼增益矩阵：

k时刻第i个粒子的状态估计值：

k时刻第i个粒子的状态误差协方差估计值：

重新产生粒子：

其中，n为状态维数。

(6)取得量测值z_k。

其中，z_k为最新量测值，为预测量测值，σ_v为观测噪声的方差。

(7)对于i＝1,2,…,N，利用式(63)计算重要性权值。

(8)利用人工鱼群算法更新粒子的位置，使得粒子不断地向真实状态靠近。定义目标函数Y：

初始化人工鱼群视野Visual、移动步长step、拥挤度因子δ、最大重复次数、更新阈值和最大迭代次数num等参数。

I.觅食行为。粒子通过比较目标函数不断更新自己向更真实的状态靠近，描述为式(65)：

式(65)中，为第i个粒子k时刻的状态预测值。为对第m∈{1,2,…,n}次迭代后的值。rand为随机数，step为移动步长。分别为k时刻人工鱼i、j当前所在位置的食物浓度。

II.聚群行为。设在范围Visual内的其他预测量测值集合为A_v，|A_v|为集合A_v的势，则其中心为聚群行为描述为式(66)：

式(66)中，为k时刻第i个粒子在中心处的状态预测值，δ为拥挤度因子，为k时刻人工鱼i在中心处的食物浓度。

通过限制迭代次数或者以目标函数前后偏差小于某阈值(误差的控制阈值)为条件来终止人工鱼群算法。

(9)权值更新并归一化。根据更新权值。根据对取出的N₁个粒子进行权值归一化。

(10)输出结果。输出状态估计值和方差估计值

(11)判断是否结束。若是，则退出本算法。若否，则返回步骤(1)。

以下描述本发明的实施例。

(1)试验条件

西班牙Girona大学的开源数据集进行自主导航定位方法的验证。试验采用Ictineu AUV 在西班牙一个废弃的港口进行，AUV航行时间约50分钟，航程约600m，航行过程中配备多普勒流速仪获得速度测量值，MTi获得姿态及加速度测量值，AUV速度约为0.2m/s。

(2)试验结果及分析

附图3为采用EPF、CPF与本发明所提出的人工鱼群优化的容积粒子滤波进行自主水下航行器自主导航定位运行轨迹与GPS轨迹的对比曲线。点划虚线为EPF估计轨迹，虚线为 CPF估计轨迹，实线为人工鱼群优化的容积粒子滤波估计航迹，点线为GPS轨迹。由图3可知，人工鱼群优化的容积粒子滤波估计轨迹与GPS轨迹最接近，导航精度最高，CPF导航精度稍差，EPF导航精度最差。

附图4、附图5分别对应三种滤波方法北向和东向的估计误差，点划虚线是EPF的误差，虚线为CPF的误差，实线为人工鱼群优化的容积粒子滤波(ICPF)的误差。横轴t代表时间(共3342s)。由附图4可知，本发明所提出的方法北向误差最小，导航精度最高。由附图5可知，本发明所提出的方法东向误差最小，导航精度最高。

由前述海试数据集的验证结果可见，人工鱼群优化的容积粒子滤波方法的导航定位估计精度相比CPF和EPF有了显著提高。人工鱼群优化的容积粒子滤波AUV自主导航方法，其导航定位精度的提高使得AUV无需周期性地上浮至水面进行GPS校正，这对于AUV执行长航时水下隐蔽监测与作业具有重要的实际应用意义。

Claims (2)

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)取得量测值z_k：

其中，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)。

2.根据权利要求1所述的人工鱼群算法优化的容积粒子滤波方法，其特征在于：采用容积卡尔曼滤波改进建议分布函数，重新产生N个粒子后，采用人工鱼群算法的觅食行为和聚群行为更新N个粒子的位置，使得粒子不断地向真实状态靠近。