基于粒子滤波的变尺度单方向重力采样矢量匹配定位方法技术

本发明专利技术公开了基于粒子滤波的变尺度单方向重力采样矢量匹配定位方法，包括以下步骤：一、建立重力辅助惯导单点匹配模型，二、利用重力辅助惯导单点匹配模型得到基于粒子滤波的匹配点，三、判断重力辅助惯导单点匹配点是否可信，四、计算当前匹配点的最终结果；本发明专利技术考虑了载体的机动航行，克服了原有的重力矢量匹配算法在载体的非直线或者非匀速航行情况下失效的缺点，减小了匹配误差。

【技术实现步骤摘要】
基于粒子滤波的变尺度单方向重力采样矢量匹配定位方法
本专利技术属于重力辅助惯导系统匹配方法领域，具体涉及一种基于粒子滤波的变尺度单方向重力采样矢量匹配定位方法。
技术介绍
水下运载体长时间隐蔽航行最常用的无源导航方式是惯导系统，但惯导误差会随着时间累积。为保证导航的隐蔽性和自主性，利用地球物理特征的无源导航来校正惯导误差，因此开展了地形匹配导航技术、重力辅助导航技术、地磁辅助导航技术等研究。对于水下导航，地形数据的测量比较困难，地球的磁场不是很稳定，重力场稳定而且重力数据可以利用卫星数据反演，因此应用重力导航具有很大的优势。重力匹配算法是重力辅助惯导技术的核心技术之一，它是利用重力仪实时测量的重力信息与预存重力图信息以一定的算法进行比较，从而估计惯导的位置。现阶段借鉴较为成熟的地形匹配，重力匹配算法按照采样方式主要分为两大类，序列匹配和单点匹配。序列匹配是一种验后估计或批处理方法，每一次匹配都要在采样到足够的点数后进行，所以实时性比较差，主要以ICCP算法和相关分析法为代表。ICCP算法最初由图像配准算法发展而来的，是一种寻找全局意义下的最优对准方法，精度叫较高。相关分析方法是由地形匹配中的地形轮廓匹配TERCOM算法发展而来的，它的匹配精度不受惯导位置误差的影响，但实时性差并且难以从理论上进行提高。单点匹配算法主要为由美国桑迪亚实验室提出的桑迪亚(SITAN)辅助导航方法，利用扩展卡尔曼滤波技术实现，该算法精度高实时性好，但由于重力特征的非线性，在重力特征明显的区域，扩展卡尔曼滤波线性化带来的误差较大，严重时导致滤波发散，匹配失去意义。这些匹配算法各个采样点之间相互独立，基于单点滤波的重力采样匹配算法考虑了各个采样点之间的位置相关性，匹配结果相比传统的匹配算法精度和可信度都有所提高。但是，定尺度定方向的重力矢量采样方法对运载体的机动适应性差，并不适用于实际航行。
技术实现思路
有鉴于此，本专利技术提供了一种基于粒子滤波的变尺度单方向重力采样矢量匹配定位方法，能够解决传统匹配算法匹配结果可信度不高和定尺度定方向重力矢量匹配算法机动适应性不强的问题。实现本专利技术的技术方案如下：一种基于粒子滤波的变尺度单方向重力采样矢量匹配定位方法，具体步骤包括：步骤一、将水下运载体的经纬度差值作为状态变量，将重力仪实时测量的水下载体当前位置的重力异常值作为观测量，建立重力辅助惯导单点匹配模型：Δxk,k+1＝Δxk-1,k+ΔUk+ek(1)yk+1＝hk+1(xk+1)+vk+1(2)式中，Δxk,k+1表示k时刻到k+1时刻水下运载体的经纬度差值，k≥1，当k＝0时，Δx0,1＝ΔU0+e0，ΔUk为惯导系统给出的k时刻的位置偏移增量，ek为惯导系统k时刻的高斯白噪声误差，yk+1表示k+1时刻的重力仪测量的重力异常值，xk+1为惯导系统输出的k+1时刻水下运载体的经纬度信息，hk+1(xk+1)表示在xk+1处在重力基准图上读取的重力异常值，vk+1表示k+1时刻的测量误差，设定为高斯白噪声；步骤二、利用重力辅助惯导单点匹配模型得到基于粒子滤波的匹配点；具体包括：S201、由惯导初始位置的先验概率生成j个初始采样粒子其中，l＝1,2,3……j，采样粒子的运动规律与水下运载体的运动规律一致，则采样粒子的状态方程为Δqk,k+1＝Δqk-1,k+ΔUk+ek(3)式中，Δqk,k+1表示k时刻到k+1时刻采样粒子的经纬度差值，k≥1，当k＝0时，Δq0,1＝ΔU0+e0；S202、利用得到第一个时刻采样粒子的经纬度信息，利用重力仪测量的y1与各个粒子在重力基准图上读取的重力异常值的差值计算各个粒子的权值各个粒子的经纬度信息乘以相应的权值得到当前时刻水下运载体的匹配点x1′，按照对第一个时刻匹配点的计算方法计算第2,3……个时刻的匹配点；步骤三、当k＜3时，认定匹配点x′k可信；当k≥3时，对x′k-n中的所有匹配点计算其重力异常变化率Gk-n，n＝1,2,......k-1；从x′k-n中选出h个匹配点分别与当前匹配点x′k组成h个匹配点对，其中2≤h≤k-1，确保所选的匹配点中至少有一个匹配点的重力异常变化率至少为前后相邻两个匹配点的两倍；若所有匹配点x′k-n的重力异常变化率均小于前后相邻匹配点的两倍，则选择所有匹配点x′k-n分别与当前匹配点x′k组成k-1个匹配点对；所述匹配点对的欧氏距离应满足惯导匹配点之间的距离相关性，即满足式其中，(m′k,n′k)表示水下运载体k时刻的匹配点的经纬度，表示理想惯导系统两匹配点pk和pk-n之间的欧氏距离，ξ表示设定误差；步骤四、若步骤三中所述匹配点对均满足式(5)，则将当前匹配点xk′视为可信点；若步骤三中所述匹配点对不全满足式(5)，则根据理想惯导系统两匹配点pk和pk-n之间的欧氏距离更新Δqk-1,k并根据S202中的计算方法得到新的匹配点x″k,k-n；新的匹配点x″k,k-n与不满足式(5)匹配点组成新的匹配点对，新的匹配点对与原先满足式(5)的匹配点对按照最小二乘法计算每个匹配点对的权值，将每个匹配点对的权值与每个匹配点对中的x″k,k-n或x′k的经纬度相乘并将所有乘积相加得到当前匹配点的最终结果；若步骤三中所述匹配点对全不满足式(5)，则根据理想惯导系统两匹配点pk和pk-n之间的欧氏距离更新Δqk-1,k并根据S202中的计算方法得到新的匹配点x″k,k-n；新的匹配点x″k,k-n与所选的x′k-n组成新的匹配点对，将新的匹配点对按照最小二乘法计算每个匹配点对的权值，将每个匹配点对的权值与每个匹配点对中的x″k,k-n的经纬度相乘并将所有乘积相加得到当前匹配点的最终结果。进一步地，步骤四中所述按照最小二乘法计算每个匹配点对的权值为：当存在i个匹配点对，每个匹配点对的长度为lm,m＝1,2…i,Δl＝l1+l2+...+lm，则每个匹配点对的权值wm为有益效果：与已有的定尺度定方向的重力矢量匹配算法相比，本专利技术考虑了载体的机动航行，克服了原有的重力矢量匹配算法在载体的非直线或者非匀速航行情况下失效的缺点，减小了匹配误差。本专利技术利用基于贝叶斯估计的粒子滤波方法实现，在解决重力特征非线性问题时，避免了扩展卡尔曼滤波线性化带来的误差，克服了传统点匹配算法桑迪亚算法在重力特征变化明显的区域内易发散的缺点，适应各种重力特征的变化，拓宽了传统算法的适用范围。附图说明图1为本专利技术的流程示意图；图2为本专利技术的具体实例中匹配轨迹示意图；图3为本专利技术的具体实例纬度误差比较示意图；图4为本专利技术的具体实例经度误差比较示意图。具体实施方式下面结合附图并举实施例，对本专利技术进行详细描述。如图1所示，本专利技术提供了一种基于粒子滤波的变尺度单方向重力采样矢量匹配定位方法，具体步骤包括：步骤一、选取合适的匹配区域，所用重力异常数据由卫星测高获得的分辨率为30′×30′的数据插值而来，插值后重力图分辨率为1′×1′。重力仪实测数据由真实值加随机噪声构成。仿真条件：水下潜器航行速度5节；惯导三个方向陀螺零偏为0.02°/h，随机漂移均为0.01°/h；三个方向加速度零偏均为1×10-4g,随机漂移均为5×10-5g；初始姿态角误差均为5′；初始位置误差均为1′；初始速度误差均为0.1m/s；重本文档来自技高网...

【技术保护点】
基于粒子滤波的变尺度单方向重力采样矢量匹配定位方法，其特征在于，包括以下步骤：步骤一、将水下运载体的经纬度差值作为状态变量，将重力仪实时测量的水下载体当前位置的重力异常值作为观测量，建立重力辅助惯导单点匹配模型：Δxk,k+1＝Δxk‑1,k+ΔUk+ek     (1)yk+1＝hk+1(xk+1)+vk+1          (2)式中，Δxk,k+1表示k时刻到k+1时刻水下运载体的经纬度差值，k≥1，当k＝0时，Δx0,1＝ΔU0+e0ΔUk为惯导系统给出的k时刻的位置偏移增量，ek为惯导系统k时刻的高斯白噪声误差，yk+1表示k+1时刻的重力仪测量的重力异常值，xk+1为惯导系统输出的k+1时刻水下运载体的经纬度信息，hk+1(xk+1)表示在xk+1处在重力基准图上读取的重力异常值，vk+1表示k+1时刻的测量误差，设定为高斯白噪声；步骤二、利用重力辅助惯导单点匹配模型得到基于粒子滤波的匹配点；具体包括：S201、由惯导初始位置的先验概率生成j个初始采样粒子其中，采样粒子的运动规律与水下运载体的运动规律一致，则采样粒子的状态方程为Δqk,k+1＝Δqk‑1,k+ΔUk+ek     (3)式中，Δqk,k+1表示k时刻到k+1时刻采样粒子的经纬度差值，k≥1，当k＝0时，Δq0,1＝ΔU0+e0；S202、利用得到第一个时刻采样粒子的经纬度信息，利用重力仪测量的y1与各个粒子在重力基准图上读取的重力异常值的差值计算各个粒子的权值各个粒子的经纬度信息乘以相应的权值得到当前时刻水下运载体的匹配点x′1，按照对第一个时刻匹配点的计算方法计算第2,3……个时刻的匹配点；步骤三、当k＜3时，认定匹配点x′k可信；当k≥3时，对x′k‑n中的所有匹配点计算其重力异常变化率Gk‑n，n＝1,2,......k‑1；从x′k‑n中选出h个匹配点分别与当前匹配点x′k组成h个匹配点对，其中2≤h≤k‑1，确保所选的匹配点中至少有一个匹配点的重力异常变化率至少为前后相邻两个匹配点的两倍；若所有匹配点x′k‑n的重力异常变化率均小于前后相邻匹配点的两倍，则选择所有匹配点x′k‑n分别与当前匹配点x′k组成k‑1个匹配点对；所述匹配点对的欧氏距离应满足惯导匹配点之间的距离相关性，即满足式其中，(m′k,n′k)表示水下运载体k时刻的匹配点的经纬度信息，表示理想惯导系统两匹配点pk和pk‑n之间的欧氏距离，ξ表示设定误差；步骤四、若步骤三中所述匹配点对均满足式(5)，则将当前匹配点x′k视为可信点；若步骤三中所述匹配点对不全满足式(5)，则根据预设的理想惯导系统两匹配点pk和pk‑n之间的欧氏距离更新Δqk‑1,k并根据S202中的计算方法得到新的匹配点x″k,k‑n；新的匹配点x″k,k‑n与不满足式(5)匹配点组成新的匹配点对，新的匹配点对与原先满足式(5)的匹配点对按照最小二乘法计算每个匹配点对的权值，将每个匹配点对的权值与每个匹配点对中的x″k,k‑n或x′k的经纬度相乘并将所有乘积相加得到当前匹配点的最终结果；若步骤三中所述匹配点对全不满足式(5)，则根据预设的理想惯导系统两匹配点pk和pk‑n之间的欧氏距离更新Δqk‑1,k并根据S202中的计算方法得到新的匹配点x″k,k‑n；新的匹配点x″k,k‑n与所选的x′k‑n组成新的匹配点对，将新的匹配点对按照最小二乘法计算每个匹配点对的权值，将每个匹配点对的权值与每个匹配点对中的x″k,k‑n的经纬度相乘并将所有乘积相加得到当前匹配点的最终结果。...

【技术特征摘要】
1.基于粒子滤波的变尺度单方向重力采样矢量匹配定位方法，其特征在于，包括以下步骤：步骤一、将水下运载体的经纬度差值作为状态变量，将重力仪实时测量的水下载体当前位置的重力异常值作为观测量，建立重力辅助惯导单点匹配模型：Δxk,k+1＝Δxk-1,k+ΔUk+ek(1)yk+1＝hk+1(xk+1)+vk+1(2)式中，Δxk,k+1表示k时刻到k+1时刻水下运载体的经纬度差值，k≥1，当k＝0时，Δx0,1＝ΔU0+e0，ΔUk为惯导系统给出的k时刻的位置偏移增量，ek为惯导系统k时刻的高斯白噪声误差，yk+1表示k+1时刻的重力仪测量的重力异常值，xk+1为惯导系统输出的k+1时刻水下运载体的经纬度信息，hk+1(xk+1)表示在xk+1处在重力基准图上读取的重力异常值，vk+1表示k+1时刻的测量误差，设定为高斯白噪声；步骤二、利用重力辅助惯导单点匹配模型得到基于粒子滤波的匹配点；具体包括：S201、由惯导初始位置的先验概率生成j个初始采样粒子其中，l＝1,2,3……j，采样粒子的运动规律与水下运载体的运动规律一致，则采样粒子的状态方程为Δqk,k+1＝Δqk-1,k+ΔUk+ek(3)式中，Δqk,k+1表示k时刻到k+1时刻采样粒子的经纬度差值，k≥1，当k＝0时，Δq0,1＝ΔU0+e0；S202、利用得到第一个时刻采样粒子的经纬度信息，利用重力仪测量的y1与各个粒子在重力基准图上读取的重力异常值的差值计算各个粒子的权值各个粒子的经纬度信息乘以相应的权值得到当前时刻水下运载体的匹配点x′1，按照对第一个时刻匹配点的计算方法计算第2,3……个时刻的匹配点；步骤三、当k<3时，认定匹配点x′k可信；当k≥3时，对x′k-n中的所有匹配点计算其重力异常变化率Gk-n，n＝1,2,......k-1；从x′k-n中选出h个匹配点分别与当前匹配点x′k组成h个匹配...

【专利技术属性】
技术研发人员：王博，于力，肖烜，邓志红，
申请(专利权)人：北京理工大学，
类型：发明
国别省市：北京;11