[发明专利]基于地形梯度拟合的粒子滤波水下航迹跟踪方法

权利要求书

1.基于地形梯度拟合的粒子滤波水下航迹跟踪方法，其特征在于，包括以下步骤：

S1：以常用的大样本统计分布估计地形梯度的分布，用数学分布近似的表示地形梯度分布；

S2：根据拟合误差选出最佳分布，并根据分布的性质制定地形数据的挑选门限，实时地筛选出给定梯度范围的用于匹配的多波束声呐实时地形数据；

S3：根据筛选出的地形数据建立测量模型，根据惯性导航指示位置变化建立状态空间模型，通过粒子滤波算法实现水下航行器航迹跟踪。

2.根据权利要求1所述的基于地形梯度拟合的粒子滤波水下航迹跟踪方法，其特征在于，所述S1的大样本统计分布估计并表示地形梯度的分布高效提取地形梯度特征，其数学拟合分布为：

正态分布：

其中，x是梯度值，μ用样本均值估计，σ²用样本方差估计；

伽马分布：

其中，x是梯度值，λ为尺度变量，γ为形状变量，为伽马函数；

威布尔分布：

其中，x是梯度值，λ＞0，为尺度变量；k＞0，为形状变量。

3.根据权利要求1所述的基于地形梯度拟合的粒子滤波水下航迹跟踪方法，其特征在于，所述S2包括以下子步骤：

S21：根据拟合误差实时筛选多波束声呐地形数据，给定拟合误差计算公式，根据拟合误差最小的分布制定梯度挑选门限，对多波束实时测量的地形数据进行挑选，其中误差计算公式为：

其中，h(t)是梯度值归一化统计直方图，p(t)是概率密度函数值，M为梯度级，dk越小，分布的拟合程度越高；

S22:验证了梯度值服从的具体分布之后，利用分布性质，根据算法对匹配点数量和梯度范围的要求，制定挑选准则。

4.根据权利要求3所述的基于地形梯度拟合的粒子滤波水下航迹跟踪方法，其特征在于，所述S22包括以下步骤：

S221：估计分布参数为：

其中，x是梯度值计算区域为m×n，μ为地形梯度的均值，σ²为地形梯度的方差；

S222：分布的三倍σ准则，认为全部点梯度值均分布在区间(μ-3σ,μ+3σ)内，具体挑点准则为：

P(|x(i,j)-μ|x＞Δσ)＝P_Δ

S223：准则下挑选梯度值较大的P_Δ×100％的匹配点；

S224：更改梯度值筛选门限Δσ，大致确定匹配点数。

5.根据权利要求1所述的基于地形梯度拟合的粒子滤波水下航迹跟踪方法，其特征在于，所述S3的以筛选出来的水下地形数据和惯性导航位置信息，分别作为测量模型和状态更新模型，用粒子滤波算法实现实时航迹跟踪，其模型为：

X_k＝X_k-1+u_k+ε_k

z_k＝h(X_k-1,δ_k)+v_k

其中，X_k为k时刻水下航行器的位置和姿态，u_k为k时刻的惯性导航输出，ε_k为惯性导航过程噪声。z_k为N维向量，表示多波束测深声呐实时测量的地形数据，h(·)表示地形图插值函数，δ_k表示多波束声呐各个波束的位置补偿量，v_k表示测量误差。

6.根据权利要求1所述的基于地形梯度拟合的粒子滤波水下航迹跟踪方法，其特征在于，所述S3的粒子滤波算法包括以下步骤：

S31：根据批处理相关匹配和惯性导航系统得出初始位置，产生初始粒子分布，并设定所有粒子具有相同权值；

S32：判断状态方程用惯性导航数据是否更新粒子，若是则进入S33；若否则进入步骤S36；

S33：对多波束测量数据拟合，用分布参数筛选深度值，挑选出用于匹配的测深数据；

S34：根据量测方程和梯度拟合结果，得到似然函数p(z_k|X_k)，重新计算粒子权值再归一化处理；

S35：从带权值的粒子集重采样得到等全值的粒子集；

S36：输出位置和姿态估计，直到航程结束。