一种基于蜂群算法改进CKF的单点重力匹配方法

摘要

本发明涉及一种基于蜂群算法改进CKF的单点重力匹配方法，获取状态量初值X

说明书

技术领域

本发明涉及一种基于蜂群算法改进CKF的单点重力匹配方法。

背景技术

以惯导器件（陀螺和加速度计）为主的惯性导航系统（INS）已经成为当前 导航领域广泛应用的导航系统，被广泛应用于航海、航天和航空领域，由于陀螺 漂移和加速度计零偏等因素的影响，惯性导航系统也存在一定程度的误差，且惯 性导航系统的位置误差随着时间不断积累，难以满足长期和高精度导航的要求。 重力辅助导航可以有效地弥补惯性导航的缺点，它不受时间的限制，重力仪可以 提供实时的重力数据，通过与惯性导航系统结合，可以分析重力基准图上的重力 强度数据。目前，导航中采用的CKF（容积卡尔曼滤波）方法存在易受环境干 扰和误差影响而导致的估计不精确等缺点。

发明内容

本发明目的在于提供一种基于蜂群算法改进CKF的单点重力匹配方法，能 够有效提高CKF的稳定性和精度，减小环境和偏差的影响。

实现本发明目的技术方案：

一种基于蜂群算法改进CKF的单点重力匹配方法，其特征在于，包括以下 步骤：

步骤1：采集重力仪测量重力信号F，获取载体位置信息经度λ和纬度 速度信息V_x和V_y、载体姿态信息和

步骤2：以载体位置误差信息和δλ、速度误差信息δV_x和δV_y、载体姿 态信息和陀螺仪和加速度计的漂移信息作为状态量，得到状态向量X； 以系统的外部噪声作为系统扰动向量W，建立系统状态方程；

步骤3：根据步骤1采集的信息，获取状态量初值X₀和协方差阵初值P₀； 采用CKF方法进行时间更新，得到状态一步预测值和一步预测协方差阵

步骤4：根据惯性导航系统输出的载体经度λ_k和纬度在重力基准图中 得到对应的重力强度F_k；根据重力强度F_k和多普勒输出的载体速度V_k，采用人 工蜂群算法在惯性导航系统指示位置进行迭代搜索，得到经度和纬度的最优估计 值λ_k′和

步骤5：根据惯性导航系统实时输出的经度λ_k和纬度在重力基准图获 得相关重力强度由重力仪测量得到重力强度的观测值两 者做差得到系统观测向量

步骤6：根据步骤4中得到的经度和纬度的最优估计值λ_k′和采用CKF 方法进行量测更新，得到量测预测值新息方差P_xz,k、协方差矩阵P_zz,k、滤 波增益K_k、状态估计协方阵P_k，状态向量的最优估计值

步骤7：重复步骤4至步骤6，进行下一步状态向量和状态协方差阵的估计。

步骤2中，状态向量X表示如下：

其中，为系统姿态角；ε_di(i＝x,y,z)为陀螺仪常值漂移误差， 且满足ε_ri(i＝x,y,z)为陀螺仪一阶马尔可夫漂移误差，满足 式中T代表陀螺仪一阶马尔可夫过程的时间常数， w_ri(i＝x,y,z)为陀螺仪的一阶噪声，为加速度计零阶漂移，满足一阶 马尔可夫过程,且式中T_a代表加速度计一阶马尔可夫 过程的时间常数，w_i(i＝x,y)为加速度计一阶噪声，[……]^T代表向量的转置；

系统的扰动向量W表示如下：

W＝[w_gx>gy>gz>rx>ry>rz>ax>ay]^T

其中，w_gi,i＝(x,y,z)为白噪声，满足w_gi～N(0,σ²),i＝x,y,z；

系统的状态方程表示如下：

$\stackrel{·}{X}=\mathrm{FX}+\mathrm{GW}$

其中，F为系统矩阵，G为系统控制矩阵，将上式离散化得到：

${\stackrel{·}{X}}_{k+1}={\Phi }_{k+1,k}{X}_k+{\Gamma }_{k+1,k}{W}_k$

其中，

${\stackrel{·}{\Phi }}_{k+1,k}=F\left(t\right){\Phi }_{k+1,k}$

${\Gamma }_{k+1,k}=\underset{k}{\overset{k+1}{\int }}{\Phi }_{k+1.k}G\left(\tau \right)\mathrm{d\tau }$

步骤3中，状态量初值X₀和协方差阵初值P₀表示为：

$P_0=E\left[(x_0-{\hat{x}}_0){(x_0-{\hat{x}}_0)}^T\right]({\hat{x}}_0=E\left(x_0\right))$

采用CKF方法进行时间更新时，考虑具有加性高斯噪声的非线性系统状态 方程和量测方程具有以下形式

x_k＝f(x_k-1,μ_k-1)+ω_k-1

z_k＝h(x_k,μ_k)+ν_k

其中，x_k∈Rⁿ是系统在k时刻的状态，是系统的输入，z_k∈R^m是系 统的量测值，ω_k-1和ν_k是不相关零均值高斯白噪声，其协方差阵分别为Q_k-1和R_k；

对于任意函数f(x)的加权高斯计分为

$I_N\left(f\right)={\int }_{R^n}f\left(x\right)N(x;\mu ,P)\mathrm{dx}\approx \frac{1}{2n}\underset{i=1}{\overset{2n}{\Sigma }}f(\mu +\sqrt{P}{\xi }_i)$

具有2n个元素的容积点集{ξ_i}为

$\sqrt{n}\{\left(\begin{array}{c}1\\ 0\\ .\\ .\\ .\\ 0\end{array}\right)...\left(\begin{array}{c}0\\ 0\\ .\\ .\\ .\\ 1\end{array}\right)\left(\begin{array}{c}-1\\ 0\\ .\\ .\\ .\\ 0\end{array}\right)...\left(\begin{array}{c}0\\ 0\\ .\\ .\\ .\\ -1\end{array}\right)\}$

其中，容积点ξ_i是n维列向量，n为状态维数；为协方差P的Cholesky 分解，且满足利用容积法则，在贝叶斯估计框架下，假设第k时 刻已知后验密度函数$p\left(x_{k-1}\right)=N(x_{k-1};{\hat{x}}_{k-1},P_{k-1});$

采用CKF方法进行时间更新如下：

（1）计算容积点

$X_{j,k-1}=\sqrt{P_{k-1}}{\xi }_k+{\hat{x}}_{k-1},j=\mathrm{1,2},...2n$

（2）容积点的传播

$X_{j,k}^{*}=f(X_{j,k-1},{\mu }_{k-1}),j=\mathrm{1,2},...2n$

（3）状态预测值和协方差阵

${\bar{X}}_k=\frac{1}{2n}\underset{j=1}{\overset{2n}{\Sigma }}{X}_{j,k}^{*}$

${\bar{P}}_k=\frac{1}{2n}\underset{j=1}{\overset{2n}{\Sigma }}{X}_{j,k}^{*}{X_{j,k}^{*}}^T-{\bar{X}}_k{{\bar{X}}_k}^T+Q_{k-1}$

其中Q_k-1为系统输入的协方差阵。

步骤4中，具体包括如下步骤：

步骤4.1：参数初始化；随机生成n个关于给定经度λ_k、纬度速度V 的附近的初始解X_i，位置为当前给定的并按下式初始化解群；

X_i＝X_i0+rand*R

其中，X_i(i＝1,2,…n)是二维向量，X_i0为由所获得的初始解，rand为 [0,1]之间的任意数，R为邻域半径；

步骤4.2：按照下式，在当前解的范围内搜索新的位置解，计算新解的适应 度；当新解的适应度更优时，则位置解更新到新的位置，否则保持原解不变，并 且未更新次数s加1；

$V_i^j=X_i^j+{\phi }_i^j(X_i^j-X_k^j)$

其中，k＝1,2,…n，j＝1,2，且i≠j；为[-1,1]之间任意数；

计算当前的适应度函数值fitness：

fitness＝1/(1+f_i)

其中，

其中，m₁,m₂是因子，且满足m₁+m₂＝1，λ_k-1|k-1和分别表示k-1时 刻滤波后的经度和纬度的最优估计值，F为当前重力场强度测量值，F_k为根据 惯性导航系统输出的当前经度λ_k和纬度在重力基准图中得到重力场强度，V_k为多普勒输出的载体速度；

步骤4.3：按下式计算每个引领蜂被选中的概率：

$P_i=\mathrm{fithess}/\underset{i=1}{\overset{n}{\Sigma }}\mathrm{fitness}$

P_i代表特定解的适应度函数值占适应度函数值总和的比例；

步骤4.4：跟随蜂按轮盘赌的方式选取一个引领蜂，即步骤4.2中更新后的 解进行跟随，按步骤4.1中的方式更新位置信息，如果位置更优，则更新到新解， 否则未更新次数s加1；

步骤4.5：当在第i个解附近未更新次数大于给定值s_max时，仍没有找到比 当前的解更适合的解，则引领蜂放弃当前的位置解，变成侦查蜂，按步骤4.1 的方法初始化新的解群；

步骤4.6：若迭代次数达到最大值，则算法迭代结束，输出此时的最优解， 否则返回步骤4.2继续循环。

步骤6中，采用CKF方法进行量测更新如下：

（1）计算容积点

$X_{j,k}=\sqrt{{\bar{P}}_k}{\xi }_i+{\bar{X}}_k$

（2）容积点传播

Z_j,k＝h(X_j,k,μ_k),j＝1,2,…2n.

（3）估计k时刻的量测预测值

${\bar{Z}}_k=\frac{1}{2n}\underset{j=1}{\overset{2n}{\Sigma }}{Z}_{j,k}$

（4）估计新息方差

$P_{\mathrm{zz},k}=\frac{1}{2n}\underset{j=1}{\overset{2n}{\Sigma }}{Z}_{j,k}{Z_{j,k}}^T-{\bar{Z}}_k{{\bar{Z}}_k}^T+R_k$

（5）估计协方差矩阵

$P_{\mathrm{xz},k}=\frac{1}{2n}\underset{j=1}{\overset{2n}{\Sigma }}{X}_{j,k}{Z}_{j,k}^T-{\bar{X}}_k{{\bar{Z}}_k}^T$

（6）估计滤波增益

K_k＝P_xz,kP_zz,k^-1

（7）k时刻状态估计值

${\hat{X}}_k={\bar{X}}_k+K_x(Z_k-{\bar{Z}}_k)$

（8）k时刻状态估计协方差值

$P_k={\bar{P}}_k-K_x{P}_{\mathrm{zz},k}{K_k}^T$

本发明具有的有益效果：

本发明针对系统的非线性和噪声干扰导致数学模型难以准确建立的问题，采 用单点重力匹配方式，避免了模型近似带来的误差，使匹配滤波结果的精确度提 高。

本发明针对单点重力匹配方式，采用CKF方法，基于高斯假设的贝叶斯框 架，将非线性滤波归结为非线性函数与高斯概率密度乘积的积分问题，实现简单， 估计精度高，无需像EKF算法那样计算雅克比矩阵，也无需像UKF算法那样 预先假设参数。

本发明针对滤波模型不准确而带来的滤波性能下降的问题，采用人工蜂群算 法对CKF加以改进。人工蜂群算法的流程简单，鲁棒性强，具有群体内多角色 分工明确，良好的协同工作机制，且容易与其它算法结合的优势。本发明采用人 工蜂群算法对CKF一步预测信息进行二次匹配，利用匹配后的状态量用于CKF 的量测更新过程，提高了CKF的稳定性和精度，减小了环境和偏差的影响，从 而提高匹配定位的精度。

附图说明

图1是本发明的方法流程图；

图2是本发明提供的基于蜂群算法改进CKF的单点重力匹配方法的原理框 图。

具体实施方式

如图1所示，本发明基于蜂群算法改进CKF的单点重力匹配方法包括以下 几个步骤：

步骤1：采集重力仪测量重力信号F，获取由惯性导航系统输出的载体位置 信息λ和速度信息V_x和V_y、载体姿态信息和

步骤2：以载体位置误差信息和δλ、速度误差信息δV_x和δV_y、载体姿 态信息和陀螺仪和加速度计的漂移信息作为状态量，得到状态向量X； 以系统的外部噪声作为系统扰动向量W，进而建立系统状态方程。

具体为，以载体位置误差信息和δλ、速度误差信息δV_x和δV_y、载体姿 态信息和陀螺仪和加速度计的漂移信息作为状态量，定义得到系统 状态向量

其中，为系统姿态角；ε_di(i＝x,y,z)为陀螺仪常值漂移误差，且满 足ε_ri(i＝x,y,z)为陀螺仪一阶马尔可夫漂移误差，满足式中T代表陀螺仪一阶马尔可夫过程的时间常数，w_ri(i＝x,y,z)为陀螺仪的一阶 噪声，为加速度计零阶漂移，满足一阶马尔可夫过程,且 式中T_a代表加速度计一阶马尔可夫过程的时间常数， w_i(i＝x,y)为加速度计一阶噪声，[……]^T代表向量的转置。系统的扰动向量可 以表示为如下：

W＝[w_gx>gy>gz>rx>ry>rz>ax>ay]^T

其中，w_gi,i＝(x,y,z)为白噪声，满足w_gi～N(0,σ²),i＝x,y,z。系统的状态方程 可以表示为

$\stackrel{·}{X}=\mathrm{FX}+\mathrm{GW}$

其中，F为系统矩阵，G为系统控制矩阵，将上式离散化得到：

${\stackrel{·}{X}}_{k+1}={\Phi }_{k+1,k}{X}_k+{\Gamma }_{k+1,k}{W}_k$

其中，

${\stackrel{·}{\Phi }}_{k+1,k}=F\left(t\right){\Phi }_{k+1,k}$

${\Gamma }_{k+1,k}=\underset{k}{\overset{k+1}{\int }}{\Phi }_{k+1.k}G\left(\tau \right)\mathrm{d\tau }$

步骤3：根据步骤1采集的信息，获取状态量初值X₀和协方差阵初值P₀； 采用CKF方法进行时间更新，得到状态一步预测值和一步预测协方差阵

状态量初值X₀和协方差阵初值P₀表示为：

其中，为系统姿态角；ε_di0(i＝x,y,z)为陀螺仪常值漂移误差，且满 足ε_ri0(i＝x,y,z)为陀螺仪一阶马尔可夫漂移误差，满足 上式中T代表陀螺仪一阶马尔可夫过程的时间常数， w_ri(i＝x,y,z)为陀螺仪的一阶噪声；为加速计零阶漂移，满足一阶 马尔可夫过程,且上式中T_a代表加速度计一阶马尔可夫 过程的时间常数，w_i(i＝x,y)为加速度计一阶噪声，[……]^T代表向量的转置。 协方差阵初值，即

$P_0=E\left[(x_0-{\hat{x}}_0){(x_0-{\hat{x}}_0)}^T\right]({\hat{x}}_0=E\left(x_0\right))$

具体为，获取的状态量初值和协方差阵初值分别为

$P_0=E\left[(x_0-{\hat{x}}_0){(x_0-{\hat{x}}_0)}^T\right]({\hat{x}}_0=E\left(x_0\right))$

进行CKF时间更新。考虑具有加性高斯噪声的非线性系统状态方程和量测方程 具有以下形式

x_k＝f(x_k-1,μ_k-1)+ω_k-1

z_k＝h(x_k,μ_k)+ν_k

其中，x_k∈Rⁿ是系统在k时刻的状态，是系统的输入，z_k∈R^m是系统的 量测值，ω_k-1和ν_k是不相关零均值高斯白噪声，其协方差阵分别为Q_k-1和R_k。

非线性高斯滤波的核心问题使求解多变量非线性函数与高斯密度函数乘积 的积分，通过三阶容积积分法则，利用2n个容积点求和来代替高斯积分加权问 题，对于任意函数f(x)的加权高斯计分为

$I_N\left(f\right)={\int }_{R^n}f\left(x\right)N(x;\mu ,P)\mathrm{dx}\approx \frac{1}{2n}\underset{i=1}{\overset{2n}{\Sigma }}f(\mu +\sqrt{P}{\xi }_i)$

具有2n个元素的容积点集{ξ_i}为

$\sqrt{n}\{\left(\begin{array}{c}1\\ 0\\ .\\ .\\ .\\ 0\end{array}\right)...\left(\begin{array}{c}0\\ 0\\ .\\ .\\ .\\ 1\end{array}\right)\left(\begin{array}{c}-1\\ 0\\ .\\ .\\ .\\ 0\end{array}\right)...\left(\begin{array}{c}0\\ 0\\ .\\ .\\ .\\ -1\end{array}\right)\}$

其中，容积点ξ_i是n维列向量，n为状态维数；为协方差P的Cholesky分 解，且满足利用容积法则，在贝叶斯估计框架下，假设第k时刻 已知后验密度函数$p\left(x_{k-1}\right)=N(x_{k-1};{\hat{x}}_{k-1},P_{k-1}).$

CKF时间更新如下：

（1）计算容积点

$X_{j,k-1}=\sqrt{P_{k-1}}{\xi }_k+{\hat{x}}_{k-1},j=\mathrm{1,2},...2n$

（2）容积点的传播

$X_{j,k}^{*}=f(X_{j,k-1},{\mu }_{k-1}),j=\mathrm{1,2},...2n$

（3）状态预测值和协方差阵

${\bar{X}}_k=\frac{1}{2n}\underset{j=1}{\overset{2n}{\Sigma }}{X}_{j,k}^{*}$

${\bar{P}}_k=\frac{1}{2n}\underset{j=1}{\overset{2n}{\Sigma }}{X}_{j,k}^{*}{X_{j,k}^{*}}^T-{\bar{X}}_k{{\bar{X}}_k}^T+Q_{k-1}$

其中，Q_k-1为系统输入的协方差阵。

步骤4：根据惯性导航系统输出的经度λ_k和纬度在重力基准图中得到重 力场强度F_k。多普勒输出载体速度为V_k。采用人工蜂群算法在惯性导航系统指 示位置进行迭代搜索，不断更新经度和纬度信息，并定义代价函数f_i

其中，m₁,m₂是因子，且满足m₁+m₂＝1，λ_k-1|k-1和分别表示k-1时刻经 过滤波后的经度和纬度的最优估计值，F为由重力仪输出的重力场强度测量值， 表示二范数的平方。V_k为多普勒输出的载体速度。由于代价函数f_i>0，进 一步定义适应度函数fitness

fitness＝1/(1+f_i)

在给定初始值及迭代次数后，可以得到了经度和纬度的最优估计值λ_k′和

具体为，根据惯性导航系统输出的当前经度λ_k和纬度在重力基准图中得 到重力场强度F_k，多普勒输出载体速度为V_k。采用人工蜂群算法进行迭代操作， 具体步骤如下：

步骤4.1：参数初始化：随机生成n个关于给定经度λ_k、纬度速度V 的附近的初始解X_i，初始位置为当前给定的并按下式初始化 解群；

X_i＝X_i0+rand*R

其中，X_i（i＝1,2,…n）是二维向量，X_i0为由所获得的初始解。rand为 [0,1]之间的任意数，R为邻域半径。

步骤4.2：按照下式，在当前解的范围内搜索新的位置解，计算新解的适应 度。当新解的适应度更优时，则位置解更新到新的位置，否则保持原解不变，并 且未更新次数s加1；

$V_i^j=X_i^j+{\phi }_i^j(X_i^j-X_k^j)$

其中，k＝1,2,…n，j＝1,2，且i≠j；为[-1,1]之间任意数。

计算当前的代价函数值f_i：

其中，m₁,m₂是因子，且满足m₁+m₂＝1，λ_k-1|k-1和分别表示k-1时刻 滤波后的经度和纬度的最优估计值，F为当前重力场强度测量值，F_k为根据惯 性导航系统输出的当前经度λ_k和纬度由重力基准图得到的重力场强度。由于 代价函数f_i>0，进一步定义适应度函数fitness

fitness＝1/(1+f_i)

计算当前的适应度函数值fitness。

步骤4.3：按下式计算每个引领蜂被选中的概率：

$P_i=\mathrm{fitness}/\underset{i=1}{\overset{n}{\Sigma }}\mathrm{fitness}$

其中，P_i代表特定解的适应度函数值占适应度函数值总和的比例。

步骤4.4：跟随蜂按轮盘赌的方式选取一个引领蜂即步骤4.2中更新后的解 进行跟随，按步骤4.1中的方式更新位置信息，如果位置更优，则更新到新解， 否则未更新次数s加1；

步骤4.5：当在第i个解附近未更新次数大于给定值s_max时，仍没有找到比 当前的解更适合的解，则引领蜂放弃当前的位置解，变成侦查蜂，按步骤4.2 中的方法搜索新解；

步骤4.6：若迭代次数达到最大值，则算法迭代结束，输出此时的最优解， 否则返回步骤4.2继续循环。

步骤5：由惯性导航系统实时输出的经度信息λ_k和纬度信息根据重力基 准图获得相关重力强度值，由重力仪测量得到重力强度的观测值 两者做差得到系统观测向量

具体为，根据惯性导航系统实时输出的经度信息λ_k和纬度信息根据重力 基准图获得相关重力强度值由重力仪测量得到重力强度的观测值 两者做差得到系统观测向量

其中，δg_m为基准图误，δg_gg为重力仪测量误差；

步骤6：对步骤4得到的最优估计的经度λ_k′和纬度惯性导航系统输出 的载体姿态信息和采用CKF进行量测更新，得到量测预测值新 息方差P_xz,k、协方差矩阵P_zz,k、滤波增益K_k、状态估计均方差值P_k，状态向量 的最优估计值

具体为，应用步骤4得到的最优估计的经度λ_k′和纬度以及多普勒输出载 体速度V_k对CKF进行量测更新，具体如下：

（1）计算容积点

$X_{j,k}=\sqrt{{\bar{P}}_k}{\xi }_i+{\bar{X}}_k$

（2）容积点传播

Z_j,k＝h(X_j,k,μ_k),j＝1,2,…2n.

（3）估计k时刻的量测预测值

${\bar{Z}}_k=\frac{1}{2n}\underset{j=1}{\overset{2n}{\Sigma }}{Z}_{j,k}$

（4）估计新息方差

$P_{\mathrm{zz},k}=\frac{1}{2n}\underset{j=1}{\overset{2n}{\Sigma }}{Z}_{j,k}{Z_{j,k}}^T-{\bar{Z}}_k{{\bar{Z}}_k}^T+R_k$

（5）估计协方差矩阵

$P_{\mathrm{xz},k}=\frac{1}{2n}\underset{j=1}{\overset{2n}{\Sigma }}{X}_{j,k}{Z}_{j,k}^T-{\bar{X}}_k{{\bar{Z}}_k}^T$

（6）估计滤波增益

K_k＝P_xz,kP_zz,k^-1

（7）k时刻状态估计值

${\hat{X}}_k={\bar{X}}_k+K_x(Z_k-{\bar{Z}}_k)$

（8）k时刻状态估计协方差值

$P_k={\bar{P}}_k-K_x{P}_{\mathrm{zz},k}{K_k}^T$

步骤7：采用由CKF滤波得到的新的初值和方差、经度和纬度的最优估计 值以及速度为V_k重复步骤4至步骤6，进行下一步滤波及人工蜂群算法的处理。 从以上实施例不难看出，对于单点重力匹配，相对于现有的匹配方法，本发明提 供一种基于人工蜂群算法改进CKF的单点重力匹配方法，解决了CKF滤波过 程中易受环境干扰和误差影响而导致的估计不精确的问题。蜂群算法具有群体内 多角色分工明确，良好的协同工作机制，且容易与其它算法结合的优势。采用人 工蜂群算法对CKF一步预测信息进行二次匹配，利用匹配后的状态量用于CKF 滤波的量测更新过程，提高了CKF滤波的稳定性和精度，减小了环境和偏差的 影响，从而提高匹配定位精度。