技术领域
[0001] 本
发明涉及水下航行器航迹跟踪领域，特别是基于地形梯度拟合的粒子滤波水下航迹跟踪方法。
背景技术
[0002] 目前，大多数水下航行器(AUV)水下
导航系统都离不开惯性导航系统(INS)。INS系统具有自主性和隐蔽性，在水下导航中发挥主导作用。但由于INS的误差不可避免地随着时间累积，需要采取其他误差修正手段，主要包括全球
定位系统辅助方法、声学导航方法、多普勒速度测量辅助方法和地球物理属性测量辅助方法等。而这些技术都有各自的局限性。例如，GPS水下不可用；声学导航依赖于母船或水下人造信标，其作用距离也有限。
[0003] 由于近年来多波束声呐和DVL装置的不断普及与快速发展，地形辅助方法发挥越来越重要的作用，成为了当今研究热点。地形属于地球物理属性，是一种稳定、可靠且方便测量的信息源，可为INS系统提供可靠的误差修正。当AUV航行时，利用自身配载的地形探测
传感器实时获取水下地形数据信息，将这些数据信息与已存储的水下参考数字地形图进行匹配，可以估计水下航行器的精确
位置，以此来修正INS累计误差，有效避免了以上GPS以及声学导航存在的不足，很好地满足了水下航行器长时间、自主、安全以及高
精度的导航要求。
[0004] 利用地形的航迹跟踪
算法有多种。其中，粒子滤波是一种可靠的方法，能够解决地形辅助惯性导航中的地形非线性问题，并能够提供较高精度的实时位置标示。然而，粒子滤波算法对测量
数据处理量和粒子数目有着较高的要求，对平台CPU的运算能
力要求较高。在保证实时定位精度的前提下尽量的降低处理器负载，是粒子滤波航迹跟踪算法未来能够广泛应用的前提条件。
发明内容
[0005] 为解决
现有技术中存在的问题，本发明提供了基于地形梯度拟合的粒子滤波水下航迹跟踪方法，相比于其他传统的粒子滤波航迹跟踪算法，具有效率更高，跟踪更可靠的特点，且基于梯度拟合的粒子滤波航迹跟踪算法更大限度地提取地形特征，有利于在给定精度要求下，用最少的粒子数目达到所要求的跟踪效果。
[0006] 基于地形梯度拟合的粒子滤波水下航迹跟踪方法，包括以下步骤：
[0007] S1：以常用的大样本统计分布估计地形梯度的分布，用数学分布近似的表示地形梯度分布；
[0008] S2：根据拟合误差选出最佳分布，并根据分布的性质制定地形数据的挑选
门限，实时地筛选出给定梯度范围的用于匹配的多波束声呐实时地形数据；
[0009] S3：根据筛选出的地形数据建立测量模型，根据惯性导航指示位置变化建立
状态空间模型，通过粒子滤波算法实现水下航行器航迹跟踪。
[0010] 优选地，S1的大样本统计分布估计并表示地形梯度的分布高效提取地形梯度其数学拟合分布为：
[0011] 正态分布：
[0012]
[0013] 其中，x是梯度值，μ用样本均值估计，σ2用样本方差估计；
[0015]
[0016] 其中，x是梯度值，λ为尺度变量，γ为形状变量， 为伽马函数；
[0017] 威布尔分布：
[0018]
[0019] 其中，x是梯度值，λ＞0，为尺度变量；k＞0，为形状变量。
[0020] 优选地，S2包括以下子步骤：
[0021] S21：根据拟合误差实时筛选多波束声呐地形数据，给定拟合误差计算公式，根据拟合误差最小的分布制定梯度挑选门限，对多波束实时测量的地形数据进行挑选，其中误差计算公式为：
[0022]
[0023] 其中，h(t)是梯度值归一化统计直方图，p(t)是概率
密度函数值，M为梯度级，dk越小，分布的拟合程度越高；
[0024] S22：验证了梯度值服从的具体分布之后，利用分布性质，根据算法对匹配点数量和梯度范围的要求，制定挑选准则。
[0025] 优选地，S22包括以下步骤：
[0026] S221：估计分布参数为：
[0027]
[0028]
[0029] 其中，x是梯度值，计算区域为m×n，μ和σ2分别为地形梯度的均值和方差。
[0030] S222：分布的三倍σ准则，认为全部点梯度值均分布在区间(μ-3σ,μ+3σ)内，具体挑点准则为：
[0031] P(|x(i,j)-μ|x＞Δσ)＝PΔ
[0032] S223：准则下挑选梯度值较大的PΔ×100％的匹配点；
[0033] S224：更改梯度值筛选门限Δσ，大致确定匹配点数。
[0034] 优选地，S3的以筛选出来的水下地形数据和惯性导航位置信息，分别作为测量模型和状态更新模型，用粒子滤波算法实现实时航迹跟踪，其模型为：
[0035] Xk＝Xk-1+uk+εk
[0036] zk＝h(Xk-1,δk)+vk
[0037] 其中，Xk为k时刻水下航行器的位置和
姿态，uk为k时刻的惯性导航输出，εk为惯性导航过程噪声。zk为N维向量，表示多波束测深声呐实时测量的地形数据，h(·)表示地形图插值函数，δk表示多波束声呐各个波束的位置补偿量，vk表示测量误差。
[0038] 优选地，S3的粒子滤波算法包括以下步骤：
[0039] S31：根据批处理相关匹配和惯性导航系统得出初始位置，产生初始粒子分布，并设定所有粒子具有相同权值；
[0040] S32：判断
状态方程用惯性导航数据是否更新粒子，若是则进入S33；若否则进入步骤S36；
[0041] S33：对多波束测量数据拟合，用分布参数筛选深度值，挑选出用于匹配的测深数据；
[0042] S34：根据量测方程和梯度拟合结果，得到似然函数p(zk|Xk)，重新计算粒子权值再归一化处理；
[0043] S35：从带权值的粒子集重
采样得到等全值的粒子集；
[0044] S36：输出位置和姿态估计，直到航程结束。
[0045] 本发明基于地形梯度拟合的粒子滤波水下航迹跟踪方法的有益效果如下：
[0046] 本发明根据多波束测深声呐提供的实时水下地形信息，结合惯性导航系统提供的位置标示，利用地形梯度拟合粒子滤波的方法对水下航行器的航迹进行跟踪，相比于其他传统的粒子滤波航迹跟踪算法，具有效率更高，跟踪更可靠的特点，且基于梯度拟合的粒子滤波航迹跟踪算法更大限度地提取地形特征，有利于在给定精度要求下，用最少的粒子数目达到所要求的跟踪效果。
附图说明
[0047] 图1为本发明基于地形梯度拟合的粒子滤波水下航迹跟踪方法的原理
框图。
[0048] 图2为本发明基于地形梯度拟合的粒子滤波水下航迹跟踪方法的两个方向的梯度拟合图。
[0049] 图3为本发明基于地形梯度拟合的粒子滤波水下航迹跟踪方法的水下地形参考地形等值线图及真实航迹图。
[0050] 图4为本发明基于地形梯度拟合的粒子滤波水下航迹跟踪方法的所选航迹的地形起伏度变化图。
[0051] 图5为本发明基于地形梯度拟合的粒子滤波水下航迹跟踪方法的所选航迹三种不同算法的
位置跟踪误差和
角度跟踪误差对比图。
[0052] 图6为本发明基于地形梯度拟合的粒子滤波水下航迹跟踪方法的所选航迹三种不同算法的
稳定性比较图。
[0053] 图7为本发明基于地形梯度拟合的粒子滤波水下航迹跟踪方法的三种不同算法的CPU处理时间对比图。
具体实施方式
[0054] 下面对本发明的具体实施方式进行描述，以便于
本技术领域的技术人员理解本发明，但应该清楚，本发明不限于具体实施方式的范围，对本技术领域的普通技术人员来讲，只要各种变化在所附的
权利要求限定和确定的本发明的精神和范围内，这些变化是显而易见的，一切利用本发明构思的发明创造均在保护之列。
[0055] 基于地形梯度拟合的粒子滤波水下航迹跟踪方法包括以下步骤：S1：选用常用的大样本统计分布，即正态分布、伽马分布和威布尔分布，来估计地形梯度的分布，目的是用数学分布近似的表示地形梯度分布。
[0056] S2：给出拟合误差计算公式，并根据拟合误差选出最佳分布，利用分布的性质估计参数并制定地形数据的挑选门限，以便实时地筛选出给定梯度范围的多波束测深声呐地形数据。
[0057] S3：根据筛选出的地形数据建立测量模型，根据惯性导航指示位置变化建立状态空间模型，最后通过粒子滤波算法实现水下航行器航迹跟踪。
[0058] 惯性导航系统输出基本的位置和角度基准信息，作为粒子滤波的输入并根据数据地图产生预测水深值，同时根据多波束测深声呐和
压力传感器输出的实时测量地形数据做梯度拟合，挑选相应梯度范围的地形数据作为粒子滤波的输入。最后通过粒子滤波输出实时的位置估计，实现惯性导航位置和姿态的修正。每当有的量测值更新时，
滤波器便可进行一次位置和姿态的估计，以实现的是连续实时航迹跟踪，如图1所示。
[0059] S1的大样本分布统计拟合为：
[0060] 高斯分布：
[0061]
[0062] 其中，x是梯度值，μ用样本均值估计，σ2用样本方差估计。
[0063] 伽马分布：
[0064]
[0065] 其中，x是梯度值，λ为尺度变量，γ为形状变量， 为伽马函数。
[0066] 威布尔分布：
[0067]
[0068] 其中，x是梯度值，λ＞0，为尺度变量；k＞0，为形状变量。
[0069] 计算水下地形北向和东向两个方向的梯度，并根据上述分布进行拟合，拟合的效果如图2所示。其中a表示东方向拟合，b表示北向梯度拟合。
[0070] S2给出的拟合误差计算为：
[0071]
[0072] 其中，h(t)是梯度值归一化统计直方图，p(t)是概率密度函数值，M为梯度级。dk越小，分布的拟合程度越高。
[0073] 再估计分布参数为：
[0074]
[0075]
[0076] 其中，x是梯度值，计算区域为m×n，μ和σ2分别为地形梯度的均值和方差。再由分布的三倍σ准则，可以认为全部点梯度值均分布在区间(μ-3σ,μ+3σ)内。
[0077] 最后制定具体挑点准则为：
[0078] P(|x(i,j)-μ|x＞Δσ)＝PΔ
[0079] 在此准则下挑选梯度值较大的PΔ×100％的匹配点。这样，更改梯度值筛选门限Δσ，即可大致确定匹配点数。
[0080] S3的测量模型和状态空间模型为：
[0081] Xk＝Xk-1+uk+εk
[0082] zk＝h(Xk-1,δk)+vk
[0083] 其中，Xk为k时刻水下航行器的指示位置和姿态，uk为k时刻的惯性导航输出，εk为惯性导航单元过程噪声。zk为N维向量，表示多波束测深声呐实时测量的地形数据，h(·)表示地形图插值函数，δk表示多波束声呐各个波束的位置补偿量，vk表示测量误差。
[0084] 由上述建立的方程模型可知，状态转移方程是线性的，由于水下地形的特点，量测模型是非线性的，位置和姿态估计实际上是一个非线性状态估计问题。采用非线性贝叶斯递推估计方法，得到位置和姿态估计的后验概率分布为：
[0085]
[0086] p(Xk|Zk-1)＝∫p(Xk-1|Zk-1)p(Xk|Xk-1)dXk-1
[0087] 其中， 为似然概率。采用
最小均方误差估计准则，则位置和姿态估计和协方差估计为：
[0088]
[0089]
[0090] 具体的粒子滤波航迹跟踪分为以下几个阶段：
[0091] 初始化：根据批处理相关匹配和惯性导航系统得出初始位置，产生初始粒子分布，并设定所有粒子具有相同权值。
[0092] 时间更新：根据状态方程用惯性导航数据更新粒子。
[0093] 实时测量：对多波束测量数据拟合，用分布参数筛选深度值。
[0094] 量测更新：根据量测方程和梯度拟合结果，得到似然函数p(zk|Xk)，重新计算粒子权值 再归一化处理。
[0095] 重采样：从带权值的粒子集重采样得到等全值的粒子集。
[0096] 输出位置和姿态估计。
[0097] 对基于地形梯度拟合的粒子滤波水下航迹跟踪方法在水下航行器实时导航的应用进行了仿真实验分析。以船载多波束测深声呐测量某湖泊水下地形作为数字地形图，其水深在30到60米之间，地图网格
分辨率为1米。假定航行器在恒定深度匀速航行，距离水平面5米，航行速度为2m/s，采样间隔为0.01s，总航行时间为300秒，图3是水下地形参考地形等值线图及真实航迹。
[0098] 采用模拟的惯性导航系统模型，假定惯性导航误差为常数，北向和东向误差均为0.1m/s，采样
频率为100Hz。多波束测深声呐仿真根据设定测量剖面的波束开角、波束数确定各波束对应的横向距离和纵向距离。这里假定每个测量剖面的波束数为127，开角120度。
考虑到地形的起伏程度对航迹跟踪的误差有很大的影响，因此通过对应的波束测量的地形数据做梯度分析，衡量航迹上各个时刻的地形起伏程度。具体计算公式为：
[0099]
[0100] 其中，grad表示地形的起伏程度。其没有固定的评价标准，在此我们用相对起伏度来衡量，即grad>0.5的地形被认为是起伏较大的，grad<0.5的地形被认为是较平坦的。图4表示所选航迹的地形起伏度变化。
[0101] 根据挑选地形点的方法不同，比对三种粒子滤波算法。具体来说分别是：标准粒子滤波(PF)：将一个时刻采集到的有效地形数据全部用来作为测量方程的输出；基于梯度拟合采样的粒子滤波(PFG)：相同时刻采集到的地形深度值按梯度拟合的标准筛选一定数量深度点作为测量方程的输出；基于均匀采样的粒子滤波(PFM)：与PFG相比，均匀采相同数目的地形深度值，作为测量方程的输出。将三种算法经过500次蒙特卡洛仿真实验，采用最小均方误差来衡量位置和姿态估计精度，具体计算为：
[0102]
[0103] 其中，M表示蒙特卡洛仿真次数(M＝500)。
[0104] 根据图5，可知三种算法位置和姿态修正误差趋势基本相同，而所提出的PFG算法无论是在位置修正还是角度修正上都有着最小的平均误差。结合图4的地形起伏度变化曲线，可见地形起伏度和位置姿态估计误差的关系。图4的T1时段地形起伏度较高，相应的位置误差和姿态误差水平都比较低，图5显示所提出的PFG算法优势不明显；T2时段地形起伏度较低，PFG算法的误差水平明显低于PF算法和PFM算法；T3时段地形起伏度较高，并由于T2时段所积累的误差，PFG算法的位置误差和角度误差水平都明显较低。
[0105] 从工程实践的角度上讲，由于实际应用中算法只进行一次，因此整体稳定性更加被看重。而上述误差平均值的统计无法说明算法的稳定性。稳定性的衡量通过统计500次独立重复试验的修正误差，再用箱型图表示误差的水平高低和分散程度。图6所示的象形图对三种不同算法的稳定性进行了比较。箱型图中矩形表示500次试验误差的上下四分位数，衡量数据主体的分散程度。矩形高度越小，表示数据越稳定。矩形上下两端横线表示500次试验误差的上下限，衡量数据的范围和奇异值的出现范围。两端相距越近，数据出现
波动越小。中间横线表示数据的中位数。水平位置越低，表示整体误差越小。
[0106] 图6中a表示位置修正稳定性，b表示姿态修正稳定性。结果表明，基于梯度拟合的粒子滤波算法(PFG)，无论在位置误差修正还是在角度误差修正上，与全部点的粒子滤波(PF)与均匀采样的粒子滤波(PFM)相比，具有更高的整体稳定性，更低的误差水平和更少的奇异值。
[0107] 图7表示三种算法的CPU运行时间对比。可知PFG算法计算量和PFM算法相当，比PF算法计算时间少。然而三种算法处于同一量级，是因为粒子数目从根本上决定着计算时间。而PFG算法的意义并非在于减少了地形处理数据量，而是通过提高了算法的稳定性和可靠性，使得在达到给定精度的条件下降低粒子数目成为可能。
