一种基于贝叶斯估计的粒子滤波重力辅助惯导匹配方法
【技术领域】
[0001] 本发明涉及一种重力辅助惯导匹配方法，属于重力辅助惯导系统匹配技术领域。
【背景技术】
[0002] 水下运载体长时间隐蔽航行最常用的无源导航方式是惯导系统，但惯导误差会随 着时间累积。为保证导航的隐蔽性和自主性，利用地球物理特征的无源导航来校正惯导误 差，因此开展了地形匹配导航技术、重力辅助导航技术、地磁辅助导航技术等研究。对于水 下导航，地形数据的测量比较困难，地球的磁场不是很稳定，重力场稳定而且重力数据可以 利用卫星数据反演，因此应用重力导航具有很大的优势。重力匹配算法是重力辅助惯导技 术的核心技术之一，它是利用重力仪实时测量的重力信息与预存重力图信息以一定的算法 进行比较，从而估计惯导的位置。
[0003] 现阶段借鉴较为成熟的地形匹配，重力匹配算法按照采样方式主要分为两大类， 序列匹配和单点匹配。序列匹配是一种验后估计或批处理方法，每一次匹配都要在采样到 足够的点数后进行，所以实时性比较差，主要以ICCP算法和相关分析法为代表。ICCP算法 最初由图像配准算法发展而来的，是一种寻找全局意义下的最优对准方法，精度叫较高。相 关分析方法是由地形匹配中的地形轮廓匹配TERCOM算法发展而来的，它的匹配精度不受 惯导位置误差的影响，但实时性差并且难以从理论上进行提高。单点匹配算法主要为由美 国桑迪亚实验室提出的桑迪亚（SITAN)辅助导航方法，利用扩展卡尔曼滤波技术实现，该 算法精度高实时性好，但由于重力特征的非线性，在重力特征明显的区域，扩展卡尔曼滤波 线性化带来的误差较大，严重时导致滤波发散，匹配失去意义。
【发明内容】
[0004] 本发明的目的是为了克服已有技术存在的不足，提出一种基于贝叶斯估计的粒子 滤波重力辅助惯导匹配方法，在解决非线性问题时，粒子滤波避免了线性化带来的误差，解 决了传统点匹配算法桑迪亚算法在重力异常变化大的匹配区域易发散的缺点。
[0005] 本发明的目的是通过下述技术方案实现的。
[0006] -种基于贝叶斯估计的粒子滤波重力辅助惯导匹配方法，包括如下步骤：
[0007] 步骤一、根据水下载体的运动规律，将运载体的经炜度信息作为状态变量，重力仪 实时测量重力异常值为观测量，建立重力辅助惯导单点匹配模型：
[0008]
[0009]
[0010] 式中，AXk, k+1表示k时刻到k+Ι时刻输出的潜艇的经炜度差值，AUk为惯导系统 给出k时刻的偏移增量，e k惯导系统的误差，y k表示k时刻的重力仪测量的重力异常信息， hk(Xk)表示在Xk处在重力基准图上读取的重力异常值，V k表示重力异常测量误差和重力基 准图误差；
[0011] 步骤二、序贯重要性采样：假设从后验概率密度P(X1JYk)中抽出N个独立同分布的 随机样本= i，…，N，则p(Xk|Yk)被表示成这些随机样本求和形式，引入一个已知、容 易采样的重要性概率密度函数q (xk I Yk)，从中生成采样粒子，实现求和逼近后验概率密度函 数P (xk I Yk)，则后验概率密度表示为从重要性密度函数采样的随机样本点与每个样本所对 应的权值乘积和的形式，粒子权值定义为重要性密度函数逼近后验概率函数的程度，从重 要性概率密度采样中生成采样粒子，并随着测量值的依次到来递推求得相应的权值，进而 得到状态估计；
[0012] 步骤三：重采样：根据步骤二得到的粒子权值，计算有效粒子数来衡量粒子权值 的退化程度；
[0013] 步骤四、对惯性导航系统的状态Xk进行估计：根据现有的观测其中量Yk，其中\表 示yp y2，…，yk，估计出重力辅助惯导定位系统的位置信息xk。
[0014] 步骤五：按照步骤一中的λ Uk更新P (X k I xk i)，再根据步骤三更新粒子权值，进行 k+Ι时刻的惯导位置估计。
[0015] 进一步地，贝叶斯估计包含预测和更新两个阶段，预测过程利用建立的系统模型 预测状态的先验概率密度，更新过程则利用最新的测量值对先验概率密度进行修正，得到 后验概率密度。
[0016] 本发明的有益效果：
[0017] 与已有的单点匹配方法相比，本发明利用基于贝叶斯估计的粒子滤波方法实现， 在解决重力特征非线性问题时，避免了扩展卡尔曼滤波线性化带来的误差，克服了传统点 匹配算法桑迪亚算法在重力特征变化明显的区域内易发散的缺点，适应各种重力特征的变 化，拓宽了传统算法的适用范围，并且匹配精度明显提高，实时性好。
【具体实施方式】
[0018] 选取经炜度跨度各一度的匹配区域，在此区域内相邻采样点之间重力异常变化 率平均值为57. 413mgal，重力特征明显。所用重力异常数据由卫星测高获得的分辨率为 30' X30'的数据插值而来，插值后重力图分辨率为I' Xl'。重力仪实测数据由真实值 加随机噪声构成。仿真条件：水下潜器航行速度5节；惯导三个方向陀螺零偏为0. 02° /h， 随机漂移均为0. 0Γ /h ;三个方向加速度零偏均为I X 10 4g，随机漂移均为5 X 10 5g ;初始 姿态角误差均为5初始位置误差均为1初始速度误差均为0. lm/s ;重力仪测量噪声 为方差为2mgal的高斯白噪声，仿真时间共10小时。
[0019]
[0020] 步骤一：根据水下载体的运动规律，将运载体的经炜度信息作为状态变量，重力仪 实时测量重力异常值为观测量，建立重力辅助惯导单点匹配模型：
[0021]
[0022]
[0023] 式中，Λ Xk, k+1表示k时刻到k+Ι时刻输出的潜艇的经炜度差值，Δ U k为惯导系统 给出k时刻的偏移增量，ek惯导系统的误差，Y k表示k时刻的重力仪测量的重力异常信息， hk(Xk)表示在Xk处在重力基准图上读取的重力异常值，V k表示重力异常测量误差和重力基 准图误差。
[0024] 步骤二：序贯重要性采样
[0025] 贝叶斯估计将状态估计视为一个概率推理过程，即将目标状态估计问题转换为利 用贝叶斯公式求解后验概率密度p (xk I Yk)，进而积分获得惯导系统的最优估计。贝叶斯估 计需要积分运算，对于重力特征的非线性系统，很难得到后验概率的封闭解析式，则利用 粒子滤波将积分问题转化为有限样本点求和问题。粒子滤波就是寻找一组在状态空间中传 播的随机样本对后验概率密度函数P (Xk IYk)进行近似，将服从P (Xk IYk)分布的随机样本称 为粒子。
[0026] 在实际计算中，通常无法直接从后验分布中采样，则引入一个已知、容易采样 的重要性概率密度函数q(x k| Yk)，从中生成采样粒子，实现求和逼近后验概率密度函数 P (xk I Yk)，本发明选取状态变量的转移概率密度函数P (xk I Xk i)作为重要性概率密度函数， 则后验概率密度可以表示为
[0027]
(m)
[0028] 其中δ (·)为狄拉克函数，#为每个粒子相应的权值
[0029]
[0030] 但是在基于重要性采样的粒子滤波中，估计后验概率概率密度需要利用所有的观 测数据，每次更新观测数据来到都需要重新计算整个状态的重要性权值，计算量大，影响算 法的实时性。
[0031] 为解决这一问题，将统计学中的序贯分析方法应用到蒙特卡洛方法中，称为序贯 重要性采样。即从重要性概率密度采样中生成采样粒子，并随着测量值的依次到来递推求 得相应的权值，进而得到状态估计。重要性概率密度函数可以分解为
[0032]
[0033] 粒子权值 < 的递归形式可以表示为
[0034] (2i>
[0035] 代入选取的重要性密度函数p (xk I xk J得粒子的权值为
[0036]
[0037] 对粒子的权值进行归一化处理，即