CN105180938A - 一种基于粒子滤波的重力采样矢量匹配定位方法 - Google Patents

Abstract

本发明提出一种基于粒子滤波的重力采样矢量匹配定位方法，矢量匹配算法考虑匹配点之间的位置相关性，解决了传统匹配算法匹配结果可信度不高的问题。本发明利用重力采样点所组成的矢量进行匹配，对单点匹配结果的二次估计，利用基于贝叶斯估计的粒子滤波实现。在单点匹配之后，计算匹配结果相邻点之间的欧氏距离，判断其是否满足无误差惯导系统两相邻采样点之间的欧氏距离所确定的范围。若满足，则该点匹配结果可信；若不满足，则根据限定条件重新进行单点匹配。

Description

一种基于粒子滤波的重力采样矢量匹配定位方法

技术领域

本发明涉及一种重力采样矢量匹配定位方法，属于重力辅助惯导系统匹配方法领域。

背景技术

水下运载体长时间隐蔽航行最常用的无源导航方式是惯导系统，但惯导误差会随着时间累积。为保证导航的隐蔽性和自主性，利用地球物理特征的无源导航来校正惯导误差，因此开展了地形匹配导航技术、重力辅助导航技术、地磁辅助导航技术等研究。对于水下导航，地形数据的测量比较困难，地球的磁场不是很稳定，重力场稳定而且重力数据可以利用卫星数据反演，因此应用重力导航具有很大的优势。重力匹配算法是重力辅助惯导技术的核心技术之一，它是利用重力仪实时测量的重力信息与预存重力图信息以一定的算法进行比较，从而估计惯导的位置。

现阶段借鉴较为成熟的地形匹配，重力匹配算法按照采样方式主要分为两大类，序列匹配和单点匹配。序列匹配是一种验后估计或批处理方法，每一次匹配都要在采样到足够的点数后进行，所以实时性比较差，主要以ICCP算法和相关分析法为代表。ICCP算法最初由图像配准算法发展而来的，是一种寻找全局意义下的最优对准方法，精度叫较高。相关分析方法是由地形匹配中的地形轮廓匹配TERCOM算法发展而来的，它的匹配精度不受惯导位置误差的影响，但实时性差并且难以从理论上进行提高。单点匹配算法主要为由美国桑迪亚实验室提出的桑迪亚(SITAN)辅助导航方法，利用扩展卡尔曼滤波技术实现，该算法精度高实时性好，但由于重力特征的非线性，在重力特征明显的区域，扩展卡尔曼滤波线性化带来的误差较大，严重时导致滤波发散，匹配失去意义。这些匹配算法各个采样点之间相互独立，没有考虑惯导相邻点之间的位置相关性，匹配结果可信度低。

发明内容

本发明的目的是为了克服已有技术存在的不足，提出一种基于粒子滤波的重力采样矢量匹配定位方法，矢量匹配算法考虑匹配点之间的位置相关性，解决了传统匹配算法匹配结果可信度不高的问题。

本发明的原理：利用重力采样点所组成的矢量进行匹配，对单点匹配结果的二次估计，利用基于贝叶斯估计的粒子滤波实现。在单点匹配之后，计算匹配结果相邻点之间的欧氏距离，判断其是否满足无误差惯导系统两相邻采样点之间的欧氏距离所确定的范围。若满足，则该点匹配结果可信；若不满足，则根据限定条件重新进行单点匹配。

本发明的目的是通过下述技术方案实现的。

一种基于粒子滤波的重力采样矢量匹配定位方法，包括如下步骤：

步骤一、根据水下载体的运动规律，将运载体的经纬度信息作为状态变量，重力仪实时测量重力异常值为观测量，建立重力辅助惯导单点匹配模型：

△x_k,k+1＝△x_k-1,k+△U_k+e_k(1)

y_k＝h_k(x_k)+v_k(2)

式中,△x_k,k+1表示k时刻到k+1时刻输出的潜艇的经纬度差值，△U_k为惯导系统给出k时刻的偏移增量，e_k惯导系统的误差，y_k表示k时刻的重力仪测量的重力异常信息，h_k(x_k)表示在x_k处在重力基准图上读取的重力异常值，v_k表示重力异常测量误差和重力基准图误差；

步骤二、计算矢量匹配基准条件：设水下运载体为匀速航行，P_k表示运载体航行过程中惯导系统输出的点k的位置信息，则等间隔采样点集{P_k-4,P_k-3,P_k-2,P_k-1,P_k}的矢量信息应满足，其中k≥4

$\mathrm{ndist}\left(\stackrel{\→}{u_0{u}_1}\right)-\mathrm{\σ}\≤\mathrm{dist}\left(\stackrel{\→}{P_k{P}_{k-n}}\right)\≤\mathrm{ndist}\left(\stackrel{\→}{u_0{u}_1}\right)+\mathrm{\σ}$ 中n＝1,2,3,4(3)

其中(m_k,n_k)表示k点惯导系统输出的经纬度信息，表示无误差惯导系统两相邻采样点之间的欧氏距离，σ表示测量误差的上限；

步骤三、序贯重要性采样：假设从后验概率密度p(x_k|Y_k)中抽出N个独立同分布的随机样本则p(x_k|Y_k)被表示成这些随机样本求和形式；引入一个已知、容易采样的重要性概率密度函数q(x_k|Y_k),从中生成采样粒子，实现求和逼近后验概率密度函数p(x_k|Y_k),则后验概率密度表示为从重要性密度函数采样的随机样本点与每个样本所对应的权值乘积和的形式，粒子权值定义为重要性密度函数逼近后验概率函数的程度；从重要性概率密度采样中生成采样粒子，并随着测量值的依次到来递推求得相应的权值，进而得到状态估计；

步骤四：重采样：根据步骤三得到的粒子权值，计算有效粒子数来衡量粒子权值的退化程度；

步骤五、对惯性导航系统的状态进行估计：根据现有的观测其中量估计出重力辅助惯导定位系统的位置信息；

步骤六、判断单点匹配结果是否可信：单点匹配结果应满足惯导相邻点之间位相关性，则单点匹配结果也应满足公式(3)；若满足，则将匹配结果视为可信点，若不满足，则在点匹配过程中加入的限定的条件得到新的重要性函数，根据由式(3)所确定的可信范围重新计算状态变量的转移概率密度函数，得到进行步骤五，得到新的匹配结果

步骤七、计算矢量匹配结果权值：若匹配结果均不满足式(3)，则得到四个局部最优解，利用加权最小二乘估计得到最终估计值

步骤八：按照步骤一中的△U_k更新p(x_k|x_k-1)，再根据步骤四更新的粒子权值，进行k+1时刻的惯导位置估计，再进行步骤六、七，得到矢量匹配结果。

进一步地，所述的估计包含预测和更新两个阶段，预测过程利用建立的系统模型预测状态的先验概率密度，更新过程则利用最新的测量值对先验概率密度进行修正，得到后验概率密度。

本发明的有益效果：

与已有的单点匹配算法相比，矢量匹配定位方法，考虑了惯导相邻点的位置相关性，对区域的重力特征变化具有较强的鲁棒性，并且减小了匹配误差，匹配结果更可信，实时性好评。本发明利用基于贝叶斯估计的粒子滤波方法实现，在解决重力特征非线性问题时，避免了扩展卡尔曼滤波线性化带来的误差，克服了传统点匹配算法桑迪亚算法在重力特征变化明显的区域内易发散的缺点，适应各种重力特征的变化，拓宽了传统算法的适用范围。

具体实施方式

下面对本发明进行详细说明。

选取经纬度跨度各一度的匹配区域，等值线越密集，颜色变化程度越大，该区域重力特征越明显。所用重力异常数据由卫星测高获得的分辨率为30′×30′的数据插值而来，插值后重力图分辨率为1′×1′。重力仪实测数据由真实值加随机噪声构成。仿真条件：水下潜器航行速度5节；惯导三个方向陀螺零偏为0.02°/h，随机漂移均为0.01°/h；三个方向加速度零偏均为1×10^-4g,随机漂移均为5×10^-5g；初始姿态角误差均为5′；初始位置误差均为1′；初始速度误差均为0.1m/s；重力仪测量噪声为方差为2mgal的高斯白噪声，仿真时间共10小时。

步骤一：根据水下载体的运动规律，将运载体的经纬度信息作为状态变量，重力仪实时测量重力异常值为观测量，建立重力辅助惯导单点匹配模型：

△x_k,k+1＝△x_k-1,k+△U_k+e_k(19)

y_k＝h_k(x_k)+v_k(20)

式中,△x_k,k+1表示k时刻到k+1时刻输出的潜艇的经纬度差值，△U_k为惯导系统给出k时刻的偏移增量，e_k惯导系统的误差，Y_k表示k时刻的重力仪测量的重力异常信息，h_k(x_k)表示在x_k处在重力基准图上读取的重力异常值，v_k表示重力异常测量误差和重力基准图误差。

步骤二：计算矢量匹配基准条件

假设水下运载体为匀速航行，P_k表示运载体航行过程中惯导系统输出的点k的位置信息，则等间隔采样点集{P_k-4,P_k-3,P_k-2,P_k-1,P_k}(k≥4)中的矢量信息(k≥4)应满足

$\mathrm{dist}\left(\stackrel{\→}{P_k{P}_{k-n}}\right)\≤\mathrm{ndist}\left(\stackrel{\→}{u_0{u}_1}\right)+\mathrm{\σ}(n=\mathrm{1,2,3,4})---\left(21\right)$

其中(m_k,n_k)表示k点惯导系统输出的经纬度信息，表示无误差惯导系统两相邻采样点之间的欧氏距离，σ表示设定误差。

步骤三：序贯重要性采样

贝叶斯估计将状态估计视为一个概率推理过程，即将目标状态估计问题转换为利用贝叶斯公式求解后验概率密度p(x_k|Y_k)，进而积分获得惯导系统的最优估计。贝叶斯估计需要积分运算，对于重力特征的非线性系统，很难得到后验概率的封闭解析式，则利用粒子滤波将积分问题转化为有限样本点求和问题。粒子滤波就是寻找一组在状态空间中传播的随机样本对后验概率密度函数p(x_k|Y_k)进行近似，将服从p(x_k|Y_k)分布的随机样本称为粒子。

在实际计算中，通常无法直接从后验分布中采样，则引入一个已知、容易采样的重要性概率密度函数q(x_k|Y_k),从中生成采样粒子，实现求和逼近后验概率密度函数p(x_k|Y_k),本发明选取状态变量的转移概率密度函数p(x_k|x_k-1)作为重要性概率密度函数，则后验概率密度可以表示为

$p\left(x_k\right|Y_k)=\stackrel{N}{\mathrm{\Σ}}{w}_k^{\left(i\right)}\mathrm{\δ}(x_k-x_k^{\left(i\right)})\left(22\right)$

其中δ(i)为狄拉克函数，为每个粒子相应的权值

$w_k^{\left(i\right)}\∞\frac{p\left(x_k^{\left(i\right)}\right|Y_k)}{p\left(x_k^{\left(i\right)}\right|x_{k-1})}---\left(23\right)$

但是在基于重要性采样的粒子滤波中，估计后验概率概率密度需要利用所有的观测数据，每次更新观测数据来到都需要重新计算整个状态的重要性权值，计算量大，影响算法的实时性。

为解决这一问题，将统计学中的序贯分析方法应用到蒙特卡洛方法中，称为序贯重要性采样。即从重要性概率密度采样中生成采样粒子，并随着测量值的依次到来递推求得相应的权值，进而得到状态估计。重要性概率密度函数可以分解为

p(x_0:k|y_1:k)＝q(x_0:k-1|y_1:k-1)q(x_k|x_0:k-1y_1:k)(24)

粒子权值的递归形式可以表示为

$\begin{array}{c}{w}_k^{\left(i\right)}\∞\frac{p\left(x_{0:k}^{\left(i\right)}\right|Y_k)}{q\left(x_{0:k}^{\left(i\right)}\right|Y_k)}\\ =\frac{p\left(y_k\right|x_k^{\left(i\right)})p\left(x_k^{\left(i\right)}\right|x_{k-1}^{\left(i\right)})p\left(x_{0:k}^{\left(i\right)}\right|Y_{k-1})}{q\left(x_k^{\left(i\right)}\right|x_{0:k}^{\left(i\right)},Y_k)q\left(x_{0:k-1}^{\left(i\right)}\right|Y_{k-1})}\\ =w_{k-1}^{\left(i\right)}\frac{p\left(y_k\right|x_k^{\left(i\right)})p\left(x_k^{\left(i\right)}\right|x_{k-1}^{\left(i\right)})}{q\left(x_k^{\left(i\right)}\right|x_{0:k}^{\left(i\right)},Y_k)}\end{array}---\left(25\right)$

代入选取的重要性密度函数p(x_k|x_k-1)得粒子的权值为

$w_k^{\left(i\right)}=w_{k-1}^{\left(i\right)}p\left(y_k\right|x_k^{\left(i\right)})$

对粒子的权值进行归一化处理，即

${\tilde{w}}_k^{\left(i\right)}=\frac{w_k^{\left(i\right)}}{\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{w}_k^{\left(i\right)}}---\left(26\right)$

步骤四：重采样

为了得到正确的状态估计，希望粒子权值的方差尽可能趋近于零。但在实际计算中，经过数次迭代，只有少数粒子的权值较大，其余粒子的权值可忽略不计。权值的方差随着时间增大，状态空间的有效粒子数较少。随着无效采样粒子数目的增多，使得大量的计算浪费在对估计后验概率密度概率分布几乎不起作用的粒子更新上，使得估计性能下降。采样有效粒子数来衡量粒子权值的退化程度，即

进行序贯重要性采样时，若小于设定的阈值，则应当采用重采样的方法克服粒子的权值退化，重采样方法舍弃权值较小的粒子，代之以权值较大的粒子。重采样过程满足条件下，将粒子集合更新为本发明中当采用基于重要性采样的残差重采样更新粒子。残差重采样具有效率高、实现方便的特点。设其中为取整操作。残余重采样采用新的权值选择剩余下的个粒子，残差重采样的主要过程为：

(1)计算剩余粒子的权值累计量

(2)生成个在[0,1]区间内均匀分布的随机数

(3)对于每个μ^l,寻找归一化权值累计量大于或者等于μ^l的最小标号m,即λ_m-1<μ^l<λ_m。当μ^l落在区间[λ_m-1,λ_m]时，被复制一次。

这样，每个粒子经重采样后的个数为步骤(3)中所选中的若干粒子数目与Nⁱ。

步骤五：对惯性导航系统的状态x_k进行估计

贝叶斯估计利用概率分布的形式估计非线性系统的状态x_k。根据现有的观测量Y_k(Y_k表示y₁,y₂,…,y_k)，要估计出重力辅助惯导定位系统的位置信息x_k。依据贝叶斯公式有：

$p\left(x_k\right|Y_k)=\frac{p\left(y_k\right|x_k)p\left(x_k\right|Y_{k-1})}{\&Integral;p\left(y_k\right|x_k)p\left(x_k\right|Y_{k-1})d{x}_k}---\left(28\right)$

由式(19)可知，分母为一个与x_k无关的归一化常量，它可以保证p(x_k|Y_k)的积分为1。由式(20)可知测量误差v_k的概率密度函数p(y_k|x_k)，即为先验概率密度函数：

p(y_k|x_k)＝∫δ(y_k-h_k(x_k)-v_k)p(v_k)dv_k(29)

其中δ(i)为狄拉克函数。

由于观测值y_k只与当前状态x_k有关，则p(x_k|x_k-1,Y_k-1)＝p(x_k|x_k-1)，根据全概率公式可知：

p(x_k,x_k-1|Y_k-1)＝p(x_k|x_k-1,Y_k-1)p(x_k-1|Y_k-1)(30)

化简并进行积分可得CK方程：

p(x_k|Y_k-1)＝∫p(x_k|x_k-1)p(x_k-1|Y_k-1)dx_k-1(31)

其中，p(x_k-1|Y_k-1)假设已知，p(x_k|x_k-1)为状态的Markov过程。

即根据最小均方差(MMSE)准则，将条件均值作为系统状态x_k的最优估计值，即

则上式(28)(29)(31)组成了贝叶斯估计系统状态x_k的基本公式。式(29)为概率密度函数，式(28)、(29)分别为位置更新和测量更新公式。由上述可知，贝叶斯估计需要进行积分运算，非线性系统多采用近似算法解决积分问题，从而得到状态的最优估计。基于贝叶斯估计的粒子滤波利用所求状态空间中的大量样本点来近似逼近待估变量的后验概率分布，从而将积分问题转换为有限样本点的求和问题。

粒子滤波就是寻找一组在状态空间中传播的随机样本对后验概率密度函数p(x_k|Y_k)进行近似，以样本均值代替条件均值E[x_k|Y_k]。将服从p(x_k|Y_k)分布的随机样本称为粒子。

设为从后验概率密度函数p(x_k|Y_k)中获取的采样粒子，则对于要估计的惯导位置x_k，条件均值E[x_k|Y_k]可以用求和方式逼近，即

$E\left[x_k\right|Y_k]=\&Integral;x_{k}p\left(x_k\right|Y_k)d{x}_k=\frac{1}{N}\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}{x}_k^{\left(i\right)}---\left(31\right)$

此时进行步骤(三)中的序贯重要性采样，生成粒子，当采样粒子数目很大时，式(13)便可逼近真实的后验概率密度。因此要估计的惯导位置x_k的条件均值E[x_k|Y_k]可表示为：

$E\left[x_k\right|Y_k]=\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}{\tilde{x}}_k^{\left(i\right)}{\tilde{w}}_k^{\left(i\right)}---\left(32\right)$

此时在进行步骤(四)重采样，要估计的惯导位置x_k的条件均值E[x_k|Y_k]可表示为：

$E\left[x_k\right|Y_k]=\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}{\hat{x}}_k^{\left(i\right)}{\hat{w}}_k^{\left(i\right)}$

输出此时的惯导位置x_k的估计值P′_k。

步骤六：判断单点匹配结果是否可信

单点匹配结果也应满足惯导相邻点之间位相关性，则单点匹配结果{P′_k-4,P′_k-3,P′_k-2,P′_k-1,P′_k}(k≥4)中的(k≥4)也应满足式(21)。以为例，若满足，则将P′_k的匹配结果视为可信点，若不满足，则在点匹配过程中加入的限定的条件得到新的重要性函数，根据由式(21)所确定的可信范围重新计算状态变量的转移概率密度函数，得到进行步骤(五)，得到新的匹配结果

步骤七：计算矢量匹配结果权值

若(k≥4)都不满足式(21)，则得到四个局部最优解利用加权最小二乘估计得到最终估计值 加权最小二乘估计观测模型为

$V=\mathrm{\&beta;}\tilde{P}+n---\left(33\right)$

其中V表示采样点{P_k-4,P_k-3,P_k-2,P_k-1}惯导系统输出的速度{V_k-4,V_k-3,V_k-2,V_k-1}，β为系数矩阵[β_k-4,β_k-3.β_k-2,β_k-1]，n为观测噪声。取σ_i＝|v_i-w_ip_i|，求加权偏差平方和s最小的w_k，其中

$s=\underset{k=k-4}{\overset{k-1}{\mathrm{\&Sigma;}}}{w}_k{\mathrm{\&sigma;}}^2=\underset{k=k-4}{\overset{k-1}{\mathrm{\&Sigma;}}}{w}_k{(v_k-w_k{p}_k)}^2---\left(34\right)$

求得[w_k-4,w_k-3,w_k-2,w_k-1]，然后归一化权值，求得P_k的最终估计值

步骤八：按照步骤(一)中的△U_k更新p(x_k|x_k-1)，再根据步骤(四)更新粒子权值，进行k+1时刻的惯导位置估计，再进行步骤(六)(七)，得到矢量匹配结果。

为了说明本发明的效果，采用桑迪亚算法在相同的实验设置下进行试验，采用本发明后匹配轨迹在匹配区域内对惯导轨迹起到了良好的校正作用，经纬度误差明显减小，经过多次试验，本发明的经纬度误差平均值与传统桑迪亚算法比较的结果如表1所示。

表1：误差结果

表2误差结果

其中若单点粒子滤波结果无须进行矢量匹配则视为可信。如表2所示，在重力异常变化率大的区域，单点粒子滤波结果可信，在重力异常变化率相对小的区域，就要进行矢量匹配。从仿真结果可以看出，在重力异常变化率较小的区域，单点粒子滤波的平均圆概率误差增量平均值比在重力异常变化率大的区域大，改进后的矢量匹配算法的平均圆概率误差比单点粒子滤波平均减小，并且在匹配区域增量平稳，说明改进后的矢量匹配算法对重力特征变化具有较强的鲁棒性。

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进，或者对其中部分技术特征进行等同替换，这些改进和替换也应视为本发明的保护范围。

Claims (2)

1.一种基于粒子滤波的重力采样矢量匹配定位方法，其特征在于，包括如下步骤：

步骤一、根据水下载体的运动规律，将运载体的经纬度信息作为状态变量，重力仪实时测量重力异常值为观测量，建立重力辅助惯导单点匹配模型：

△x_k,k+1＝△x_k-1,k+△U_k+e_k(1)

y_k＝h_k(x_k)+v_k(2)

式中,△x_k,k+1表示k时刻到k+1时刻输出的潜艇的经纬度差值，△U_k为惯导系统给出k时刻的偏移增量，e_k惯导系统的误差，y_k表示k时刻的重力仪测量的重力异常信息，h_k(x_k)表示在x_k处在重力基准图上读取的重力异常值，v_k表示重力异常测量误差和重力基准图误差；

步骤二、计算矢量匹配基准条件：设水下运载体为匀速航行，P_k表示运载体航行过程中惯导系统输出的点k的位置信息，则等间隔采样点集{P_k-4,P_k-3,P_k-2,P_k-1,P_k}的矢量信息应满足，其中k≥4

$\mathrm{ndist}\left(\stackrel{\&RightArrow;}{u_0{u}_1}\right)-\mathrm{\&sigma;}\&le;\mathrm{dist}\left(\stackrel{\&RightArrow;}{P_k{P}_{k-n}}\right)\&le;\mathrm{ndist}\left(\stackrel{\&RightArrow;}{u_0{u}_1}\right)+\mathrm{\&sigma;}$ 中n＝1,2,3,4(3)

其中(m_k,n_k)表示k点惯导系统输出的经纬度信息，表示无误差惯导系统两相邻采样点之间的欧氏距离，σ表示测量误差的上限；

步骤三、序贯重要性采样：假设从后验概率密度p(x_k|Y_k)中抽出N个独立同分布的随机样本i＝1,…,N,则p(x_k|Y_k)被表示成这些随机样本求和形式；引入一个已知、容易采样的重要性概率密度函数q(x_k|Y_k),从中生成采样粒子，实现求和逼近后验概率密度函数p(x_k|Y_k),则后验概率密度表示为从重要性密度函数采样的随机样本点与每个样本所对应的权值乘积和的形式，粒子权值定义为重要性密度函数逼近后验概率函数的程度；从重要性概率密度采样中生成采样粒子，并随着测量值的依次到来递推求得相应的权值，进而得到状态估计；

步骤四：重采样：根据步骤三得到的粒子权值，计算有效粒子数来衡量粒子权值的退化程度；

步骤五、对惯性导航系统的状态进行估计：根据现有的观测其中量估计出重力辅助惯导定位系统的位置信息；

步骤六、判断单点匹配结果是否可信：单点匹配结果应满足惯导相邻点之间位相关性，则单点匹配结果也应满足公式(3)；若满足，则将匹配结果视为可信点，若不满足，则在点匹配过程中加入的限定的条件得到新的重要性函数，根据由式(3)所确定的可信范围重新计算状态变量的转移概率密度函数，得到进行步骤五，得到新的匹配结果

步骤七、计算矢量匹配结果权值：若匹配结果均不满足式(3)，则得到四个局部最优解，利用加权最小二乘估计得到最终估计值

步骤八：按照步骤一中的△U_k更新p(x_k|x_k-1)，再根据步骤四更新的粒子权值，进行k+1时刻的惯导位置估计，再进行步骤六、七，得到矢量匹配结果。

2.如权利要求1所述的一种基于粒子滤波的重力采样矢量匹配定位方法，其特征在于，进一步地，所述的估计包含预测和更新两个阶段，预测过程利用建立的系统模型预测状态的先验概率密度，更新过程则利用最新的测量值对先验概率密度进行修正，得到后验概率密度。