0 引言

多传感器融合技术已经广泛应用于目标跟踪、监控、导航、通信以及信号和图像处理等领域。信息融合主要包括集中式融合、分布式融合以及混合式融合。集中式融合是融合测量数据获得全局最佳状态估计。许多学者在集中式融合(Centralised Fusion,CF)^[1-4]问题上已经做了大量的工作，而且融合算法大部分都是以高斯逼近滤波器作为基本的滤波。由于高斯假设固有的缺陷，这些算法不适用于非线性动态系统。粒子滤波(Particle Filtering,PF)适用于非线性、非高斯系统，而且提供更加精确的状态估计结果，已经取得了巨大的成就^[5-6]。然而，粒子滤波算法较难选取合适的重要性采样密度，从而导致滤波精度不高，质量低以及严重的粒子退化的问题。为了克服粒子滤波的不足，Wan^[7]提出了Unscented粒子滤波，虽然可以克服粒子滤波的缺点，滤波精度高，适应于非线性、非高斯的系统，但是其精度与粒子数成正比，粒子数越多其精度越高，而实时性会随着粒子数的增多而下降^[7-8]。Torma等^[9]提出了基于似然分布的自适应调整的粒子滤波算法，该算法将重要性密度函数选取为先验密度，而且忽略了最新量测信息在系统中的影响。虽然在一定程度上提高了滤波的稳定性，但是当满足非归一化的似然度函数值超过预先设定的阈值的条件时，才产生新的粒子，而且需要考虑到最新的量测信息。薛丽等^[10]提出一种新的权值自适应调整Unscented粒子滤波算法，在考虑最新量测影响的基础上，利用无迹变换(Unscented Transformation,UT)变换获得重要性密度函数，并且对粒子权值进行了自适应的调整，提高了滤波精度。但是当似然分布位于转移先验分布尾部或者观测模型具有很高精度时，很多样本由于归一化权重很小而成为无效样本，过低的采样率很可能导致粒子滤波失败。

综上所述，相对于标准粒子滤波，针对不同具体的应用背景其算法的滤波精度获得一定程度的改善。但以上方法的共同缺陷在于算法构建原理局限在单传感器量测系统。熊伟等^[11]提出的多传感器顺序粒子滤波算法能够明显提高多传感器系统状态估计精度，并且随着传感器数增多，改善的效果越好。但是由于多传感器信息仅仅能够影响粒子的权重，对状态值没有影响，从而导致低质量粒子的出现^[12]。这些问题源于粒子滤波及其特有的以粒子滤波为基础的融合算法。

为了减轻粒子贫化问题，Higuchi^[13]提出将变异和交叉用于修改PF。在重采样之前，粒子要进行二进制编码，遗传因子要应用在所有的编码粒子中，编码步骤提高了粒子的复杂性；而且在修改PF中的每一个粒子可能被遗传因子改变，减小了大权重粒子的权重。Kwok等^[14]提出了进化粒子滤波，仅仅包括交叉算子，但是缺点在于依赖重采样粒子，对提高粒子的多样性很差。Heris等^[15]提出通过求解遗传算法(Genetic Algorithm,GA)多目标优化问题减轻样本贫化。但是在每一次迭代中，要求求解一次多目标优化问题，计算量过大。因此，本文建立一种二阶智能粒子滤波(Two-Stage Intelligent Particle Filtering,TSIPF)的信息融合算法来解决此问题。

本文将多传感器的信息融合过程分为两个阶段，将多传感器数据送入与之对应的粒子滤波计算模块中，达到以优化粒子分布为目的对建议分布密度(Proposal distribution Density,PD)的更新。而后，在智能粒子滤波模块中设计的交叉算子和变异算子将小权重粒子修正为大权重粒子，扩展了粒子的状态搜索空间，近似于真实的后验分布，从而有效地提高了的粒子质量，解决粒子贫化问题，最终实现更精确的估计。

1 二阶智能粒子滤波的多传感器融合 1.1 集中式融合的标准粒子滤波

考虑以下的多传感器非线性离散系统^[16]：

$\left\{ \begin{array}{*{35}{l}} {{x}_{t}}={{f}_{t}}({{x}_{t-1}})+{{v}_{t-1}} \\ z_{t}^{(m)}=h_{t}^{(m)}({{x}_{t}})+w_{t}^{(m)} \\ \end{array} \right.$

(1)

其中：m=1，2，…，M；t和m是时间指数和传感器指数；x_t和z_t^(m)为状态变量和量测变量。过程噪声v_t－1和量测噪声w_t^(m)是零均值并且相互独立。 f(·)为状态转移方程，h(·)为观测方程。我们的目的是根据所有的量测值z_1:t:={z_τ}_τ=1^t递归估计出x_t，其中z_τ:={z_τ^(m)}_m=1^M。

在粒子滤波中，后验概率密度$p({{x}_{t}}\left| {{z}_{1:t}} \right.)=\sum\limits_{i=1}^{N}{\omega _{t}^{i}\delta ({{x}_{t}}-x_{t}^{i})}$,x_tⁱ是带有权重ω_tⁱ的第i个粒子。在多传感器测量的独立假设的基础上，似然函数p(z_tx_t)能够被分解为$\prod\limits_{m=1}^{M}{p(z_{t}^{(m)}\left| {{x}_{t}} \right.)}$。在每一个滤波周期中，根据式(2)计算权重ω_tⁱ：

$\omega _{t}^{i}\propto \frac{p(x_{t}^{i}\left| x_{t-1}^{i} \right.)\prod\limits_{m=1}^{M}{p(z_{t}^{(m)}\left| x_{t}^{i} \right.)}}{q(x_{t}^{i}\left| x_{t-1}^{i} \right.,{{z}_{t}})}$

(2)

其中：q(x_tⁱx_t－1ⁱ,z_t)是PD，用于产生粒子x_tⁱ并且评价q(x_tⁱx_t－1ⁱ,z_t)。由于很难获得最优采样分布，本文将先验概率密度函数p(x_tⁱx_t－1ⁱ)作为PD，因此，权重计算可以表示为简化形式$\omega _{t}^{i}\propto \prod\limits_{m=1}^{M}{p(z_{t}^{(m)}\left| x_{t}^{i} \right.)}$。

1.2 二阶集中式粒子滤波

提高滤波估计精度的基本方式之一就是有效地使用多传感器数据。然而，在集中式融合的标准粒子滤波算法中，多传感器数据仅仅在似然模型中融合，因而忽视了重要性采样过程。为此提出了基于粒子滤波框架下的二阶数据融合方法，如图 1所示。在第一阶段的数据处理层中，多传感器数据发送给粒子滤波计算模块，在状态空间中以优化粒子分布为目的相应地更新PD。在此状态中有M个粒子滤波模块，其中第n个模块标记为PF_n。第二阶段的数据融合层中包括标记PF_M+1的智能粒子滤波模块，此模块中的多传感器数据用于构造完整的似然函数$\prod\limits_{m=1}^{M}{p(z_{t}^{(m)}\left| {{x}_{t}} \right.)}$)，智能粒子滤波模块中设计的交叉算子和变异算子将小权重粒子修正为大权重粒子，达到降低粒子退化程度，保持粒子的多样性的目的，进而得到状态估计_t。

从图 1可以看出，每一个滤波周期需要M个基本的粒子滤波模块和一个智能粒子滤波模块。第n个模块中使用的PD表示为q_n(x_t|x_t－1ⁱ,z_t)，n∈{1,2,…,M+1}。采用文献中算法确定q_n(x_t|x_t－1ⁱ,z_t)，可以得到式(3)~(4)的更新系统方程：

${{x}_{t}}=f({{x}_{t-1}})+{{c}_{n}}({{z}_{t}})+{{v}_{t-1}}$

(3)

${{q}_{n}}({{x}_{t}}\left| x_{t-1}^{i},{{z}_{t}} \right.):={{p}_{v}}({{x}_{t}}-f(x_{t-1}^{i})-{{c}_{n}}({{z}_{t}}))$

(4)

因此，c_n(z_t)为修正项，初始设置为零向量并且随着n增加而更新。根据式(4)，在“PF_1”中，使c₁(z_t)=0意味着q₁(x_t|x_t－1ⁱ,z_t)=p(x_t|x_t－1ⁱ)，使用第一个传感器数据来定义本地似然函数p(z_t⁽¹⁾|x_t)并且通过运行“PF_1”获得估计${{\hat{x}}_{t,1}}$。根据式(4)，在“PF_2”使${{c}_{2}}({{z}_{t}})={{\hat{x}}_{t,1}}-f({{\hat{x}}_{t-1}})$并且定义q₂(x_t|x_t－1ⁱ,z_t)，使用第二个传感器数据来定义本地似然函数p(z_t⁽²⁾|x_t)并且获得估计${{\hat{x}}_{t,2}}$，而后被用于获得q₃(x_t|x_t－1ⁱ,z_t)。整个过程持续到所有传感器数据用完为止，得出的估计${{\hat{x}}_{t,n}}$，n∈{1,2,…,M}只是用于优化PD的中间结果。

1.3 智能粒子滤波

为了解决粒子贫化问题，将遗传算法中的交叉和变异应用于粒子滤波中。同时为了适应PF的特点，在交叉和变异两个算子中进行修正。交叉算子主要用于大权重粒子信息修正小权重粒子信息，但是，大权重粒子不变。在遗传算法中，根据一个给定的变异概率，对粒子进行修正也要使用变异。通过这两个算子，小权重粒子将被修正为大权重粒子，提高了粒子的多样性。

假设得到的粒子表示为$\{x_{t}^{i},\tilde{\omega }_{t}^{i}\}(i=1,2,\cdots ,N)$，$\tilde{\omega }_{t}^{i}$表示为归一化的粒子权重。为了从其他大权重粒子中分离出小权重粒子，构造一个选择策略，记为：

$x_{t}^{i}\in \left\{ \begin{matrix} {{C}_{L}}, \\ {{C}_{H}}, \\ \end{matrix} \right.\begin{matrix} \tilde{\omega }_{t}^{i}\le {{W}_{T}} \\ \tilde{\omega }_{t}^{i}>{{W}_{T}} \\ \end{matrix}$

(5)

C_L和C_H分别为小权重粒子和大权重粒子的集合。W_T是阈值。式(5)所示，比W_T小的权重粒子定义为小权重粒子并且存储在C_L中，比W_T大的权重粒子定义为大权重粒子并且存储在C_H中，有效抽样尺度N_eff决定W_T，按照以下步骤。

1) 计算N_eff并取整：

${{N}_{eff}}=\left[ {1}/{\sum\limits_{i=1}^{N}{{{({{{\tilde{\omega }}}_{t}})}^{2}}}}\; \right]$

(6)

其中[·]表示取整符号。

2) 以降序排列粒子权重，并且存储在$\tilde{W}$中：

$\tilde{W}=\left[ \tilde{\omega }_{t}^{1},\tilde{\omega }_{t}^{2},\cdots ,\tilde{\omega }_{t}^{N} \right]$

(7)

3) 设定第N_eff个粒子权重为阈值W_T：

${{W}_{T}}=\tilde{W}({{N}_{eff}})$

(8)

粒子分离之后，进行交叉。产生多个交叉算子。考虑到简单性以及该状态x_tⁱ是浮点数编码，选择算术交叉来修改小权重粒子。假设x_tL^l为来自C_L的粒子，x_tH^j为来自C_H的粒子，修正后的小权重粒子记为x_tS^l。算术因子公式表示为：

$x_{tS}^{l}=\alpha x_{tL}^{l}+(1-\alpha )x_{tH}^{j}$

(9)

其中：l=1,2,…,N_L； j=1,2,…,N_H；N_L和N_H分别表示在C_L和C_H中的粒子数。每一个x_tS^l和x_tH^j是随机从C_H中选出的。α∈[0,1>]是参数，决定了x_tL^l被转移给x_tS^l多少信息。α越大，从x_tL^l被转移给x_tS^l信息越多。特别是，当α=1时，x_tL^l=x_tS^l意味着不需要交叉。

为了进一步提高粒子的多样性，设计了变异策略。根据概率可能发生在修改的小权重粒子x_tS^l。

$x_{tM}^{l}=\left\{ \begin{array}{*{35}{l}} 2x_{tH}^{J}-x_{tS}^{l}, \\ x_{tS}^{l}, \\ \end{array} \right.\begin{matrix} {{r}_{l}}\le {{p}_{M}} \\ {{r}_{l}}>{{p}_{M}} \\ \end{matrix}$

(10)

其中：r_l是x_tS^l的随机变量，它服从[0,1]上均匀分布；p_M表示变异率，是预先设定的，当p_M=0时，不需要变异。在实际应用中，不同的系统参数α和p_M也不同。

1.4 二阶智能粒子滤波的多传感器信息算法

一个周期的算法如下：

第一阶段

1) 初始化。

n=1并且c_n(z_t)=0；

FOR n=1∶M

2) 更新。

由式(4)定义的PD为q_n(x_t|x_t－1ⁱ,z_t)产生粒子x_tⁱ；使用ω_tⁱ∝p(x_tⁱ|x_t－1ⁱ)p(z_t⁽ⁿ⁾|x_tⁱ)/q_n(x_tⁱ|x_t－1ⁱ,z_t)更新粒子权值，并且归一化$\omega _{t}^{i}={\omega _{t}^{i}}/{\sum\limits_{i=1}^{N}{\omega _{t}^{i}}}\;$。给出估计${{\hat{x}}_{t,n}}=\sum\limits_{i=1}^{N}{\omega _{t}^{i}x_{t}^{i}}{{c}_{n+1}}({{z}_{t}})={{\hat{x}}_{t,n}}-f({{\hat{x}}_{t-1}})$，而后n:=n+1。

END FOR

第二阶段 执行以下步骤，t=1,2,…

1) 已知c_M+1(z_t)，定义PD为q(x_t|x_t－1ⁱ,z_t)=q_M+1(x_t|x_t－1ⁱ,z_t)，从分布p(x_t|x_t－1ⁱ)中采样得到粒子x_tⁱ。

2)根据式(2)计算所有粒子的归一化权值并且表示为$\{x_{t}^{i},\tilde{\omega }_{t}^{i}\}$。

3)通过最小均方误差(Minimum Mean Squared Error,MMSE)估计隐藏状态${{\hat{x}}_{t}}$，表示为：

${{\hat{x}}_{t}}=\sum\limits_{i=1}^{N}{x_{t}^{i}\tilde{\omega }_{t}^{i}}$

(11)

4) 将粒子分为小权重和大权重，小权重粒子x_tL^l(l=1,2,…,N_L)存储在C_L中，x_tH^j(j=1,2,…,N_H)存储在C_H中。

5) 按照式(9)、(10)执行交叉因子和变异因子，获得x_tM^l。

6) 估计并归一化x_tM^l和x_tH^j的权值。

7) 执行重采样步骤。

8) 给出状态量的估计值${{\hat{x}}_{t,n}}=\sum\limits_{i=1}^{N}{\tilde{\omega }_{t}^{{{i}^{*}}}x_{t}^{i}}$。返回第1)步，按新观测量递归计算下一时刻的状态估计值。

2 仿真结果与实验分析 2.1 仿真结果

本文按照式(12)、(13)的状态空间模型^[18]进行仿真：

${{x}_{t+1}}=1+\sin (0.04\pi t)+0.5{{x}_{t}}+{{v}_{t}}$

(12)

$\left\{ \begin{array}{*{35}{l}} z_{t}^{(1)}={}^{x{{(t)}^{2}}}\!\!\diagup\!\!{}_{20}\;+{}^{{{x}_{t}}}\!\!\diagup\!\!{}_{3}\;+w_{t}^{(1)} \\ z_{t}^{(2)}=\left\{ \begin{array}{*{35}{l}} 0.2x_{t}^{2}+w_{t}^{(2)}, \\ 0.5{{x}_{t}}-2+w_{t}^{(2)}, \\ \end{array}\begin{matrix} t\le 30 \\ t>30 \\ \end{matrix} \right. \\ \end{array} \right.$

(13)

v_t为根据gamma(3,2)分布建模的过程噪声；w_t⁽¹⁾～N(0,1E－1)和w_t⁽²⁾～N(0,1E－3)是量测噪声。包括三个滤波：无迹卡尔曼滤波(Unscented Kalman Filter_Centralised Fusion,UKF_CF)，粒子滤波(Simple unscented Particle Filter_Centralised Fusion,SPF_CF)和二阶智能粒子滤波(TSIPF)，二阶智能粒子滤波中α=0.1，p_M=0.5。估计出状态序列x_t(t=1,2,…,60)。二阶自适应权值粒子滤波分为两个阶段：TSIPF_case1为数据处理层；TSIPF_case2为数据融合层。使用均方根误差(Root Mean Squared Error,RMSE)测量出估计精度。每一个算法运行100次。表 1总结了各种算法的总体性能。两个TSIPF的估计精度比其他滤波方法明显高，但是以时间作为代价来实现的，而且TSIPF_case2的精度比TSIPF_case1的精度稍高，根据数据处理层提供的优化粒子的状态估计，通过交叉和变异因子使小权重粒子将被修正为大权重粒子，增加了有用粒子的权值，进而提高了估计的精度，多传感器应用到重要性采样过程也提高了最终的估计精度。TSIPF的性能依靠数据处理层中传感器数据的使用顺序。一般来讲，由粗到细的策略有助于提高估计精度，因此优先使用来自低精度传感器的数据。

2.2 实船实验

将UKF_CF、SPF_CF以及TSIPF应用于GPS/SINS/LOG的船舶组合导航系统中，见图 2所示。

为了验证本文算法的性能，以大连海事大学“育鲲”轮实验数据为例，按照图 2所示的组合导航系统进行实验的设计。组合导航的实验设计分为两部分：一是原始数据的采集；二是根据本文提出的算法对原始数据进行仿真实验研究并分析结果。

图 3为船舶右旋回测试的位置结果。

由图 3可以看出，UKF_CF和SPF_CF在整个阶段的位置误差都较大，而TSIPF算法在初始阶段时位置误差较大，但随着船舶航行，融合算法的曲线基本稳定。同时，可以看出当GPS数据存在野值跳变时，TSIPF算法能够有效地抑制野值的影响从而减小定位误差。图 4为三种滤波的位置误差曲线。图 4(a)为三种算法的纬度误差，其中UKF_CF的误差在-20 m~100 m以内；SPF_CF的误差在-20 m~90 m以内；TSIPF的误差在-10 m~70 m以内。图 4(b)为三种算法的经度误差，UKF_CF的误差在-70 m~50 m以内，SPF_CF的误差在-70 m~50 m以内，TSIPF的误差在-60 m~40 m以内。由误差曲线也能明显看出：在初始阶段和转向时UKF_CF和SPF_CF的定位误差均较大；但是TSIPF相比UKF_CF和SPF_CF整体上系统定位误差较小，基本稳定。

图 5为航向和航向误差曲线，航向对于船舶的控制是一个很重要的参数，因此航向的估计对整个组合导航控制系统都有影响。图 5(a)为三种算法的航向角，其中TSIPF最接近参考航向值；由图 5(b)中可看出，UKF_CF和SPF_CF的航向角误差范围是±0.5°，TSIPF的航向角误差范围是±0.3°。因此，本文方法能够较好地估计出船舶的航向。

图 6为三种算法的速度曲线。 由图 6(a)、(b)可以看出，UKF_CF和SPF_CF总体上速度曲线波动较大，TSIPF在前期加速运动期间和船舶转向时速度有一定的波动，但是总体上速度较平稳。

3 结语

本文提出了以智能粒子滤波为框架的二阶多传感器数据融合算法。通过使用多传感器数据来更新建议分布密度，多传感器数据能够体现在重要性采样的过程中，在数据融合层构造完整的似然函数；设计了基于遗传因子的智能粒子滤波，将小权重粒子修改为大权重粒子，解决了粒子贫化问题。同时重采样和马尔可夫链蒙特卡罗过程，保留了权值较大的粒子，又避免了粒子耗尽问题，进一步保持粒子的多样性，提高了滤波精度，进而得到最终的估计值。最后，根据实船实验的数据进行了验证，将提出的基于二阶智能粒子滤波算法应用于GPS/SINS/LOG组合导航系统进行仿真计算，并且同无迹卡尔曼滤波、粒子滤波进行了比较分析，二阶智能粒子滤波算法能够得到精确的位置、速度和航向信息，而且也能有效改善滤波性能，提高组合导航系统的解算精度，能够满足船舶高精度导航定位的要求。但是该算法会增大计算量，需要下一步进行深入研究，对算法进行改进，使其性能更加完善。