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

本发明专利技术涉及一种基于蜂群算法改进CKF的单点重力匹配方法，获取状态量初值X0和协方差阵初值P0；采用CKF方法进行时间更新，得到状态一步预测值和一步预测协方差阵根据惯性导航系统输出的载体经度λk和纬度在重力基准图中得到对应的重力强度Fk；根据重力强度Fk和多普勒输出的载体速度Vk，采用人工蜂群算法在惯性导航系统指示位置进行迭代搜索，得到经度和纬度的最优估计值λk′和根据经度和纬度的最优估计值λk′和采用CKF方法进行量测更新，状态估计协方差值Pk，状态向量的最优估计值

【技术实现步骤摘要】
一种基于蜂群算法改进CKF的单点重力匹配方法
本专利技术涉及一种基于蜂群算法改进CKF的单点重力匹配方法。
技术介绍
以惯导器件(陀螺和加速度计)为主的惯性导航系统(INS)已经成为当前导航领域广泛应用的导航系统，被广泛应用于航海、航天和航空领域，由于陀螺漂移和加速度计零偏等因素的影响，惯性导航系统也存在一定程度的误差，且惯性导航系统的位置误差随着时间不断积累，难以满足长期和高精度导航的要求。重力辅助导航可以有效地弥补惯性导航的缺点，它不受时间的限制，重力仪可以提供实时的重力数据，通过与惯性导航系统结合，可以分析重力基准图上的重力强度数据。目前，导航中采用的CKF(容积卡尔曼滤波)方法存在易受环境干扰和误差影响而导致的估计不精确等缺点。
技术实现思路
本专利技术目的在于提供一种基于蜂群算法改进CKF的单点重力匹配方法，能够有效提高CKF的稳定性和精度，减小环境和偏差的影响。实现本专利技术目的技术方案：一种基于蜂群算法改进CKF的单点重力匹配方法，其特征在于，包括以下步骤：步骤1：采集重力仪测量重力信号F，获取载体位置信息经度λ和纬度速度信息Vx和Vy、载体姿态信息和步骤2：以载体位置误差信息和δλ、速度误差信息δVx和δVy、载体姿态信息和陀螺仪和加速度计的漂移信息作为状态量，得到状态向量X；以系统的外部噪声作为系统扰动向量W，建立系统状态方程；步骤3：根据步骤1采集的信息，获取状态量初值X0和协方差阵初值P0；采用CKF方法进行时间更新，得到状态一步预测值和一步预测协方差阵步骤4：根据惯性导航系统输出的载体经度λk和纬度在重力基准图中得到对应的重力强度Fk；根据重力强度Fk和多普勒输出的载体速度Vk，采用人工蜂群算法在惯性导航系统指示位置进行迭代搜索，得到经度和纬度的最优估计值λk′和步骤5：根据惯性导航系统实时输出的经度λk和纬度在重力基准图获得相关重力强度由重力仪测量得到重力强度的观测值两者做差得到系统观测向量步骤6：根据步骤4中得到的经度和纬度的最优估计值λk′和采用CKF方法进行量测更新，得到量测预测值新息方差Pzz,Pxz,k、协方差矩阵Pxz,Pzz,k、滤波增益Kk、状态估计协方阵Pk，状态向量的最优估计值步骤7：重复步骤4至步骤6，进行下一步状态向量和状态协方差阵的估计。步骤2中，状态向量X表示如下：其中，为系统姿态角；εdi(i＝x,y,z)为陀螺仪常值漂移误差，且满足εri(i＝x,y,z)为陀螺仪一阶马尔可夫漂移误差，满足式中T代表陀螺仪一阶马尔可夫过程的时间常数，wri(i＝x,y,z)为陀螺仪的一阶噪声，为加速度计零阶漂移，满足一阶马尔可夫过程,且式中Ta代表加速度计一阶马尔可夫过程的时间常数，wi(i＝x,y)为加速度计一阶噪声，[……]T代表向量的转置；系统的扰动向量W表示如下：W＝[wgxwgywgzwrxwrywrzwaxway]T其中，wgi,i＝(x,y,z)为白噪声，满足wgi～N(0,σ2),i＝x,y,z；系统的状态方程表示如下：其中，F为系统矩阵，G为系统控制矩阵，将上式离散化得到：其中，步骤3中，状态量初值X0和协方差阵初值P0表示为：采用CKF方法进行时间更新时，考虑具有加性高斯噪声的非线性系统状态方程和量测方程具有以下形式xk＝f(xk-1,μk-1)+ωk-1zk＝h(xk,μk)+vk其中，xk∈Rn是系统在k时刻的状态，是系统的输入，zk∈Rm是系统的量测值，ωk-1和vk是不相关零均值高斯白噪声，其协方差阵分别为Qk-1和Rk；对于任意函数f(x)的加权高斯计分为具有2n个元素的容积点集{ξi}为其中，容积点ξi是n维列向量，n为状态维数；为协方差P的Cholesky分解，且满足利用容积法则，在贝叶斯估计框架下，假设第k时刻已知后验密度函数采用CKF方法进行时间更新如下：(1)计算容积点(2)容积点的传播(3)状态预测值和协方差阵其中Qk-1为系统输入的协方差阵。步骤4中，具体包括如下步骤：步骤4.1：参数初始化；随机生成n个关于给定经度λk、纬度速度V的附近的初始解Xi，位置为当前给定的并按下式初始化解群；Xi＝Xi0+rand*R其中，Xi(i＝1,2,…n)是二维向量，Xi0为由所获得的初始解，rand为[0,1]之间的任意数，R为邻域半径；步骤4.2：按照下式，在当前解的范围内搜索新的位置解，计算新解的适应度；当新解的适应度更优时，则位置解更新到新的位置，否则保持原解不变，并且未更新次数s加1；其中，k＝1,2,…n，j＝1,2，且i≠j；为[-1,1]之间任意数；计算当前的适应度函数值fitness：fitness＝1/(1+fi)其中，其中，m1,m2是因子，且满足m1+m2＝1，λk-1|k-1和分别表示k-1时刻滤波后的经度和纬度的最优估计值，F为当前重力场强度测量值，Fk为根据惯性导航系统输出的当前经度λk和纬度在重力基准图中得到重力场强度，Vk为多普勒输出的载体速度；步骤4.3：按下式计算每个引领蜂被选中的概率：Pi代表特定解的适应度函数值占适应度函数值总和的比例；步骤4.4：跟随蜂按轮盘赌的方式选取一个引领蜂，即步骤4.2中更新后的解进行跟随，按步骤4.1中的方式更新位置信息，如果位置更优，则更新到新解，否则未更新次数s加1；步骤4.5：当在第i个解附近未更新次数大于给定值smax时，仍没有找到比当前的解更适合的解，则引领蜂放弃当前的位置解，变成侦查蜂，按步骤4.1的方法初始化新的解群；步骤4.6：若迭代次数达到最大值，则算法迭代结束，输出此时的最优解，否则返回步骤4.2继续循环。步骤6中，采用CKF方法进行量测更新如下：(1)计算容积点(2)容积点传播Zj,k＝h(Xj,k,μk),j＝1,2,…2n.(3)估计k时刻的量测预测值(4)估计新息方差(5)估计协方差矩阵(6)估计滤波增益Kk＝Pxz,kPzz,k-1(7)k时刻状态估计值(8)k时刻状态估计协方差值本专利技术具有的有益效果：本专利技术针对系统的非线性和噪声干扰导致数学模型难以准确建立的问题，采用单点重力匹配方式，避免了模型近似带来的误差，使匹配滤波结果的精确度提高。本专利技术针对单点重力匹配方式，采用CKF方法，基于高斯假设的贝叶斯框架，将非线性滤波归结为非线性函数与高斯概率密度乘积的积分问题，实现简单，估计精度高，无需像EKF算法那样计算雅克比矩阵，也无需像UKF算法那样预先假设参数。本专利技术针对滤波模型不准确而带来的滤波性能下降的问题，采用人工蜂群算法对CKF加以改进。人工蜂群算法的流程简单，鲁棒性强，具有群体内多角色分工明确，良好的协同工作机制，且容易与其它算法结合的优势。本专利技术采用人工蜂群算法对CKF一步预测信息进行二次匹配，利用匹配后的状态量用于CKF的量测更新过程，提高了CKF的稳定性和精度，减小了环境和偏差的影响，从而提高匹配定位的精度。附图说明图1是本专利技术的方法流程图；图2是本专利技术提供的基于蜂群算法改进CKF的单点重力匹配方法的原理框图。具体实施方式如图1所示，本专利技术基于蜂群算法改进CKF的单点重力匹配方法包括以下几个步骤：步骤1：采集重力仪测量重力信号F，获取由惯性导航系统输出的载体位置信息λ和速度信息Vx和Vy、载体姿态信息和步骤2：以载体位置误差本文档来自技高网...

【技术保护点】
一种基于蜂群算法改进CKF的单点重力匹配方法，其特征在于：包括以下步骤： 步骤1：采集重力仪测量重力信号F，获取载体位置信息经度λ和纬度速度信息Vx和Vy、载体姿态信息和步骤2：以载体位置误差信息和δλ、速度误差信息δVx和δVy、载体姿态信息和陀螺仪和加速度计的漂移信息作为状态量，得到状态向量X；以系统的外部噪声作为系统扰动向量W，建立系统状态方程；步骤3：根据步骤1采集的信息，获取状态量初值X0和协方差阵初值P0；采用CKF方法进行时间更新，得到状态一步预测值和一步预测协方差阵步骤4：根据惯性导航系统输出的载体经度λk和纬度在重力基准图中得到对应的重力强度Fk；根据重力强度Fk和多普勒输出的载体速度Vk，采用人工蜂群算法在惯性导航系统指示位置进行迭代搜索，得到经度和纬度的最优估计值λk′和步骤5：根据惯性导航系统实时输出的经度λk和纬度在重力基准图获得相关重力强度由重力仪测量得到重力强度的观测值两者做差得到系统观测向量步骤6：根据步骤4中得到的经度和纬度的最优估计值λk′和，采用CKF方法进行量测更新，得到量测预测值新息方差Pxz,k、协方差矩阵Pzz,k、滤波增益Kk、状态估计协方差阵Pk，状态向量的最优估计值步骤7：重复步骤4至步骤6，进行下一步状态向量和状态协方差阵的估计。...

【技术特征摘要】
1.一种基于蜂群算法改进CKF的单点重力匹配方法，其特征在于：包括以下步骤：步骤1：采集重力仪测量重力信号F，获取载体位置信息经度λ和纬度速度信息Vx和Vy、载体姿态信息和步骤2：以载体位置误差信息和δλ、速度误差信息δVx和δVy、载体姿态信息和陀螺仪和加速度计的漂移信息作为状态量，得到状态向量X；以系统的外部噪声作为系统扰动向量W，建立系统状态方程；步骤3：根据步骤1采集的信息，获取状态量初值X0和协方差阵初值P0；采用CKF方法进行时间更新，得到状态一步预测值和一步预测协方差阵步骤4：根据惯性导航系统输出的载体经度λk和纬度在重力基准图中得到对应的重力强度Fk；根据重力强度Fk和多普勒输出的载体速度Vk，采用人工蜂群算法在惯性导航系统指示位置进行迭代搜索，得到经度和纬度的最优估计值λ′k和步骤5：根据惯性导航系统实时输出的经度λk和纬度在重力基准图获得相关重力强度由重力仪测量得到重力强度的观测值两者做差得到系统观测向量步骤6：根据步骤4中得到的经度和纬度的最优估计值λ′k和采用CKF方法进行量测更新，得到量测预测值新息方差Pzz,k、协方差矩阵Pxz,k、滤波增益Kk、状态估计协方差阵Pk，状态向量的最优估计值步骤7：重复步骤4至步骤6，进行下一步状态向量和状态协方差阵的估计。2.根据权利要求1所述的基于蜂群算法改进CKF的单点重力匹配方法，其特征在于：步骤2中，状态向量X表示如下：其中，为系统姿态角；εdi(i＝x,y,z)为陀螺仪常值漂移误差，且满足为陀螺仪一阶马尔可夫漂移误差，满足式中T代表陀螺仪一阶马尔可夫过程的时间常数，wri(i＝x,y,z)为陀螺仪的一阶噪声，为加速度计零阶漂移，满足一阶马尔可夫过程,且式中Ta代表加速度计一阶马尔可夫过程的时间常数，wai(i＝x，y)为加速度计一阶噪声，[……]T代表向量的转置；系统的扰动向量W表示如下：W＝[wgxwgywgzwrxwrywrzwaxway]T其中，wgi,i＝(x,y,z)为白噪声，满足wgi～N(0,σ2),i＝x,y,z；系统的状态方程表示如下：其中，F为系统矩阵，G为系统控制矩阵，将上式离散化得到：其中，。3.根据权利要求2所述的基于蜂群算法改进CKF的单点重力匹配方法，其特征在于：步骤3中，状态量初值X0和协方差阵初值P0表示为：采用CKF方法进行时间更新时，考虑具有加性高斯噪声的非线性系统状态方程和量测方程具有以下形式xk＝f(xk-1,μk-1)+ωk-1zk＝h(xk,μk)+νk其中，xk∈Rn是系统在k时刻的状态，是系统的输入，zk∈Rm是系统的量测值，ωk-1和νk是不相关...

【专利技术属性】
技术研发人员：周广涛，赵博，张鹏，姜鑫，孙艳涛，郝勤顺，杨若雨，林萌萌，孙妍忞，张思，
申请(专利权)人：哈尔滨工程大学，
类型：发明
国别省市：黑龙江;23