一种基于粒子滤波的重力采样矢量匹配定位方法
【技术领域】
[0001] 本发明涉及一种重力采样矢量匹配定位方法，属于重力辅助惯导系统匹配方法领 域。
【背景技术】
[0002] 水下运载体长时间隐蔽航行最常用的无源导航方式是惯导系统，但惯导误差会随 着时间累积。为保证导航的隐蔽性和自主性，利用地球物理特征的无源导航来校正惯导误 差，因此开展了地形匹配导航技术、重力辅助导航技术、地磁辅助导航技术等研究。对于水 下导航，地形数据的测量比较困难，地球的磁场不是很稳定，重力场稳定而且重力数据可以 利用卫星数据反演，因此应用重力导航具有很大的优势。重力匹配算法是重力辅助惯导技 术的核心技术之一，它是利用重力仪实时测量的重力信息与预存重力图信息以一定的算法 进行比较，从而估计惯导的位置。
[0003] 现阶段借鉴较为成熟的地形匹配，重力匹配算法按照采样方式主要分为两大类， 序列匹配和单点匹配。序列匹配是一种验后估计或批处理方法，每一次匹配都要在采样到 足够的点数后进行，所以实时性比较差，主要以ICCP算法和相关分析法为代表。ICCP算法 最初由图像配准算法发展而来的，是一种寻找全局意义下的最优对准方法，精度叫较高。相 关分析方法是由地形匹配中的地形轮廓匹配TERCOM算法发展而来的，它的匹配精度不受 惯导位置误差的影响，但实时性差并且难以从理论上进行提高。单点匹配算法主要为由美 国桑迪亚实验室提出的桑迪亚（SITAN)辅助导航方法，利用扩展卡尔曼滤波技术实现，该 算法精度高实时性好，但由于重力特征的非线性，在重力特征明显的区域，扩展卡尔曼滤波 线性化带来的误差较大，严重时导致滤波发散，匹配失去意义。这些匹配算法各个采样点之 间相互独立，没有考虑惯导相邻点之间的位置相关性，匹配结果可信度低。
【发明内容】
[0004] 本发明的目的是为了克服已有技术存在的不足，提出一种基于粒子滤波的重力采 样矢量匹配定位方法，矢量匹配算法考虑匹配点之间的位置相关性，解决了传统匹配算法 匹配结果可信度不高的问题。
[0005] 本发明的原理：利用重力采样点所组成的矢量进行匹配，对单点匹配结果的二次 估计，利用基于贝叶斯估计的粒子滤波实现。在单点匹配之后，计算匹配结果相邻点之间的 欧氏距离，判断其是否满足无误差惯导系统两相邻采样点之间的欧氏距离所确定的范围。 若满足，则该点匹配结果可信；若不满足，则根据限定条件重新进行单点匹配。
[0006] 本发明的目的是通过下述技术方案实现的。
[0007] -种基于粒子滤波的重力采样矢量匹配定位方法，包括如下步骤：
[0008] 步骤一、根据水下载体的运动规律，将运载体的经炜度信息作为状态变量，重力仪 实时测量重力异常值为观测量，建立重力辅助惯导单点匹配模型：
[0009] Δ Xkik+1 = A xk lik+ Δ Uk+ek (I)
[0010] yk= h k(xk)+vk (2)
[0011] 式中，Λ xkik+1表示k时刻到k+1时刻输出的潜艇的经炜度差值，Λ Uk为惯导系统 给出k时刻的偏移增量，ek惯导系统的误差，y k表示k时刻的重力仪测量的重力异常信息， hk(Xk)表示在Xk处在重力基准图上读取的重力异常值，V k表示重力异常测量误差和重力基 准图误差；
[0012] 步骤二、计算矢量匹配基准条件：设水下运载体为匀速航行，Pk表示运载体航行过 程中惯导系统输出的点k的位置信息，则等间隔采样点集{Pk 4, Pk 3, Pk 2, Pk i，PJ的矢量信 息
应满足，其中k^4
度信息表示无误差惯导系统两相邻采样点之间的欧氏距离，σ表示测量误差的 上限；
[0015] 步骤三、序贯重要性采样：假设从后验概率密度p(Xk|Yk)中抽出N个独立同分布的 随机样本C/ = 1,…，見则ρ (xk I Yk)被表示成这些随机样本求和形式；引入一个已知、容 易采样的重要性概率密度函数q (xk I Yk)，从中生成采样粒子，实现求和逼近后验概率密度 函数p (xk I Yk)，则后验概率密度表示为从重要性密度函数采样的随机样本点与每个样本所 对应的权值乘积和的形式，粒子权值定义为重要性密度函数逼近后验概率函数的程度；从 重要性概率密度采样中生成采样粒子，并随着测量值的依次到来递推求得相应的权值，进 而得到状态估计；
[0016] 步骤四：重采样：根据步骤三得到的粒子权值，计算有效粒子数次y来衡量粒子权 值的退化程度；
[0017] 步骤五、对惯性导航系统的状态进行估计：根据现有的观测其中量估计出重力辅 助惯导定位系统的位置信息；
[0018] 步骤六、判断单点匹配结果是否可信：单点匹配结果应满足惯导相邻点之间位相 关性，则单点匹配结果也应满足公式（3);若满足，则将匹配结果视为可信点，若不满足，则 在点匹配过程中加入泣的限定的条件得到新的重要性函数，根据由式（3)所 确定的可信范围重新计算状态变量的转移概率密度函数，得到;进行步骤五，得 到新的匹配结果|? ;
[0019] 步骤七、计算矢量匹配结果权值：若匹配结果均不满足式（3)，则得到四个局部最 优解，利用加权最小二乘估计得到最终估计值為. .，
[0020] 步骤八：按照步骤一中的Λ Uk更新p (X k I Xk i)，再根据步骤四更新的粒子权值，进 行k+Ι时刻的惯导位置估计，再进行步骤六、七，得到矢量匹配结果。
[0021] 进一步地，所述的估计包含预测和更新两个阶段，预测过程利用建立的系统模型 预测状态的先验概率密度，更新过程则利用最新的测量值对先验概率密度进行修正，得到 后验概率密度。
[0022] 本发明的有益效果：
[0023] 与已有的单点匹配算法相比，矢量匹配定位方法，考虑了惯导相邻点的位置相关 性，对区域的重力特征变化具有较强的鲁棒性，并且减小了匹配误差，匹配结果更可信，实 时性好评。本发明利用基于贝叶斯估计的粒子滤波方法实现，在解决重力特征非线性问题 时，避免了扩展卡尔曼滤波线性化带来的误差，克服了传统点匹配算法桑迪亚算法在重力 特征变化明显的区域内易发散的缺点，适应各种重力特征的变化，拓宽了传统算法的适用 范围。
【具体实施方式】
[0024] 下面对本发明进行详细说明。
[0025] 选取经炜度跨度各一度的匹配区域，等值线越密集，颜色变化程度越大，该区域重 力特征越明显。所用重力异常数据由卫星测高获得的分辨率为30' X30'的数据插值而 来，插值后重力图分辨率为P XP。重力仪实测数据由真实值加随机噪声构成。仿真条 件：水下潜器航行速度5节；惯导三个方向陀螺零偏为0.02° /h，随机漂移均为0.0Γ /h; 三个方向加速度零偏均为1X10 4g，随机漂移均为5X10 5g;初始姿态角误差均为Y ;初 始位置误差均为P ;初始速度误差均为〇. lm/s ;重力仪测量噪声为方差为2mgal的高斯白 噪声，仿真时间共10小时。
[0026] 步骤一：根据水下载体的运动规律，将运载体的经炜度信息作为状态变量，重力仪 实时测量重力异常值为观测量，建立重力辅助惯导单点匹配模型：
[0027] Δ Xkik+1 = A xk lik+ Δ Uk+ek (19)
[0028] yk=hk (xk)+vk (20)
[0029] 式中，Λ xkik+1表示k时刻到k+1时刻输出的潜艇的经炜度差值，Λ Uk为惯导系统 给出k时刻的偏移增量，ek惯导系统的误差，Y k表示k时刻的重力仪测量的重力异常信息， hk(Xk)表示在Xk处在重力基准图上读取的重力异常值，V k表示重力异常测量误差和重力基 准图误差。
[0030] 步骤二：计算矢量匹配基准条件
[0031] 假设水下运载体为匀速航行，Pk表示运载体航行过程中惯导系统输出的 点k的位置信息，则等间隔采样点集{Pk 4, Pk 3, Pk 2, Pk i，Pk} (k彡4)中的矢量信息
度信息，表示无误差惯导系统两相邻采样点之间的欧氏距离，σ表示设定误差。
[0034] 步骤三：序贯重要性采样
[0035] 贝叶斯估计将状态估计视为一个概率推理过程，即将目标状态估计问题转换为利 用贝叶斯公式求解后验概率密度P (xk I Yk)，进而积分获得惯导系统的最优估计。贝叶斯估 计需要积分运算，对于重力特征的非线性系统，很难得到后验概率的封闭解析式，则利用粒 子滤波将积分问题转化为有限样本点求和问题。粒子滤波就是寻找一组在状态空间中传 播的随机样本对后验概率密度函数P (Xk IYk)进行近似，将服从P (Xk IYk)分布的随机样本称 为粒子。
[0036] 在实际计算中，通常无法直接从后验分布中采样，则引入一个已知、容易采样 的重要性概率密度函数q(xk|Yk)，从中生成采样粒子，实现求和逼近后验概率密度函数 P (xk I Yk)，本发明选取状态变量的转移概率密度函数P (xk I Xkl)作为重要性概率密度函数， 则后验概率密度可以表示为
[0038] 其中δ (i)为狄拉克函数，^为每个粒子相应的权值
[0040] 但是在基于重要性采样的粒子滤波中，估计后验概率概率密度需要利用所有的观 测数据，每次更新观测数据来到都需要重新计算整个状态的重要性权值，计算量大，影响算 法的实时性。
[0041] 为解决这一问题，将统计学中的序贯分析方法应用到蒙特卡洛方法中，称为序贯 重要性采样。即从重要性概率密度采样中生成采样粒子，并随着测量值的依次到来递推求 得相应的权值，进而得到状态估计。重要性概率密度函数可以分解为
[0042] p(x〇:k|y1:k) = q(x〇：k ilyi：k i)q(xk|x〇：k iyi：k) (24)
[0043] 粒子权值》f的递归形式可以表示为
[0045] 代入选取的重要性密度函数p (xk k J得粒子的权值为
[0047] 对粒子的权值进行归一化处理，即
[0049] 步骤四：重采样
[0050] 为了得到正确的状态估计，希望粒子权值的方差尽可能趋近于零。但在实际计算 中，经过数次迭代，只有少数粒子的权值较大，其余粒子的权值可忽略不计。权值的方差随 着时间增大，状态空间的有效粒子数较少。随着无效采样粒子数目的增多，使得大量的计算 浪费在对估计后验概率密度概率分布几乎不起作用的粒子更新上，使得估计性能下降。采 样有效粒子数来衡量粒子权值的退化程度，即
[0052]