[发明专利]基于粒子滤波的变尺度单方向重力采样矢量匹配定位方法

摘要

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

主权项

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、利用得到第一个时刻采样粒子的经纬度信息，利用重力仪测量的y₁与各个粒子在重力基准图上读取的重力异常值的差值计算各个粒子的权值各个粒子的经纬度信息乘以相应的权值得到当前时刻水下运载体的匹配点x′₁，按照对第一个时刻匹配点的计算方法计算第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时刻的匹配点的经纬度信息，表示理想惯导系统两匹配点p_k和p_k‑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的经纬度相乘并将所有乘积相加得到当前匹配点的最终结果。