CN104390646B - 水下潜器地形辅助惯性导航系统的位置匹配方法 - Google Patents

Abstract

本发明公开一种水下潜器地形辅助惯性导航系统的位置匹配方法，包括：1)在进入海图覆盖区域后，利用深度计和多波束扫描测深仪，测量潜器在起始时间段Δt₁内航行区域的水深值；2)计算时间段Δt₁的水深累积直方图H₁、地形粗糙度K_r1和下次水深测量的时间段Δt₂；3)确定潜器在Δt₁/2时刻的可能位置集L₁；4)测量潜器在时间段Δt_n(n＝2，3...，N)内航行区域的水深值，计算时间段Δt_n的水深累积直方图H_n、地形粗糙度K_rn和下次水深测量的时间段Δt_n+1等步骤。木发明是一种自主式匹配方法具有精度高、鲁棒性好的特点，可应用于地形、地磁或者重力辅助惯性导航定位。

Description

水下潜器地形辅助惯性导航系统的位置匹配方法

技术领域

本发明涉及一种水下潜器地形匹配方法，主要针对水下潜器地形辅助惯性导航系统的位置匹配方法。

背景技术

水下潜器经过长时间航行后，纯惯性导航系统输出的位置已经积累较大误差。考虑到安全性、隐蔽性和长时间水下作业的实际情况，采用地球物理场辅助惯性导航的无源导航技术，主要包括惯性/地形、惯性/重力和惯性/地磁组合导航。其基本原理都是通过水下潜器装备的传感器测量航行轨迹上的地球物理场数据，再与已有的数据库进行匹配，确定水下潜器的最佳位置。

地形匹配方法最早应用在航空领域，作为战机和巡航导弹的一种辅助导航方法，其中最著名的有TERCOM和SITAN。由于水下环境的特殊性，Besl和Mckay等人提出了ICP图像对准方法。近年来，很多学者将ICP算法与其他方法进行组合以改进地形匹配导航方法。

为保证惯性导航系统长时间、高精度的工作，地形匹配算法应具有实时性和可靠性。实时性方面，ICP算法的旋转、平移计算量较大，并且实际应用中要求水下潜器初始误差较小。可靠性方面，目前大多数地形导航确定搜索区域的方法是采用惯导系统输出的水下潜器位置为中心，搜索半径根据实际情况确定。这种方法在惯性导航初始误差不大的情况下是可取的，在初始误差较大时，确定的搜索区域可能是不正确的或范围很大。

发明内容

发明目的：本发明的目的在于克服现有技术的不足，提出一种水下潜器地形辅助惯性导航系统的位置匹配方法，可有效地提高地形辅助惯性导航系统快速、准确定位，从而消除和克服在惯导系统初始误差大的情况下地形匹配算法失效的缺陷。

技术方案：本发明所述的水下潜器地形辅助惯性导航系统的位置匹配方法包括以下步骤：

1)在进入海图覆盖区域后，利用水下潜器所装备的深度计和多波束扫描测深仪，测量水下潜器在起始时间段Δt₁内航行区域的水深值；

2)计算时间段Δt₁的水深累积直方图H₁和地形粗糙度K_r1，并由K_r1计算下次水深测量的时间段Δt₂；

3)将时间段Δt₁内扫描区域的大小作为整个海图搜索的网格单元，其中，扫描区域的大小具体为：

S＝(kd)² (1)

式中，k为与水下潜器航迹垂直的平面内一次给出的多波束测深点个数，d为k个多波束测深点的中心点间距，S为时间段内扫描区域的大小，也称为网格单元；

采用滑动窗的方式搜索，搜索步进值为海图的分辨率，计算第1次测量时第m个滑动窗的水深累积直方图H_1m，其中，m＝1,2,…,M，M由搜索区域和网格单元大小决定，并计算H₁与H_1m之间的距离，其中，水深累积直方图之间的距离定义为相同水深间隔范围内累积概率值的均方误差，距离满足的匹配准则为：

式中，n表示测量次数，num表示水深累积直方图的区间数，H_ni表示测量时间段Δt_n内第i个水深间隔范围的累积概率值，H_nmi表示时间段Δt_n内第m个滑动窗的第i个水深间隔范围的累积概率值，K_rn表示时间段Δt_n的地形粗糙度，ε表示调节因子，调节ε，使得(ε/K_rn)在12～25范围内，H_min为第n次测量的水深累积直方图H_n与第n次测量时第m个滑动窗的水深累积直方图H_nm之间的最小距离；

当n＝1时，将计算H₁与H_1m之间的距离中满足式(2)的第m个滑动窗的中心位置作为水下潜器在Δt₁/2时刻的可能位置集L₁；

4)测量水下潜器在时间段Δt_n内航行区域的水深值，其中，n＝2,3…,N，计算时间段Δt_n的水深累积直方图H_n和地形粗糙度K_rn，并由K_rn计算下次水深测量的时间段Δt_n+1；

5)计算惯导系统在时间段Δt_n-1内航行的相对距离D_n-1,n，计算更新的可能位置集L′_n-1中各位置点之间的最大距离D_n-1，当n＝2时，L′₁＝L₁，计算L′_n-1集中各位置点之间的最大距离D_n-1，取D_n-1和2D_n-1,n中较大值为R，确定时间段Δt_n内水深测量的搜索区域为：纬度范围为[Latimin-R,Latimax+R]，经度范围为[longimin-R,longimax+R]，其中，Latimax和Latimin分别是L′_n-1集中纬度最大值和最小值，longimax和longimin分别是L′_n-1集中经度最大值和最小值，如果搜索区域的某边界超出海图的边界，以海图边界为最大值或者最小值；

采用滑动窗的方式搜索，计算第n次测量时第m个滑动窗的水深累积直方图H_nm，并计算H_nm与第n次测量的水深累积直方图H_n之间的距离，将其中距离满足匹配准则式(2)的第m个滑动窗的中心位置作为水下潜器在Δt_n/2时刻的可能位置集L_n；

6)计算L′_n-1和L_n中各位置点之间的距离并与D_n-1,n比较，将满足与D_n-1,n的绝对差值小于三倍海图分辨率条件的L′_n-1集作为水下潜器在Δt_n-1/2时刻的参考位置集P_n-1，同时将满足上述条件的L_n集作为水下潜器在Δt_n/2时刻的更新的可能位置集L′_n；

7)依据地形粗糙度K_rn决定实测总次数N，并判断实测次数是否达到N，若未达到，将n加1，并重复步骤4)～步骤6)；若实测次数达到N，则计算参考位置集P₁至P_n-1的价值函数为：

式中，D(P_n-1,P_n)表示参考位置集P_n-1与P_n各位置点之间的距离，K为价值函数；依据地形粗糙度K_rn确定实测总次数N，具体确定原则为：K_rn>1时，取N≧3，K_rn<1时，取N≧4；根据式(3)计算参考位置集P₁至P_n-1的价值函数K，取价值函数为最小值时的参考位置连线为最终获取的水下潜器航迹，由匹配的各时间段中点位置，结合水下潜器短时内匀速直航的特点，推算水下潜器当前的实际位置，最后通过位置重置修正惯导系统的位置参数。

进一步地，所述步骤1)中水下潜器在测量起始时间段内航行的区域应尽量保持正方形，此外，在后续每次测量时间段内航行的区域也同样保持正方形。

进一步地，所述步骤2)中计算时间段Δt₁的水深累积直方图H₁具体过程为：根据整个海图的水深量程，将水深值平均分成10组到20组不重叠区间，确定水深间隔范围，统计步骤1)测量的水深值在每个水深间隔范围内的水深值个数，累积各水深间隔范围的水深值概率分布情况，得到时间段Δt₁的水深累积直方图H₁；

地形粗糙度定义为单位面积的水深值方差，K_r1表示时间段Δt₁的地形粗糙度，依据K_r1确定下次水深测量的时间段Δt₂，其中，Δt₂取大于1/K_r1的最小整数。

本发明与现有技术相比，其有益效果是：(1)针对实时性要求，目前国内刘承香采用的图形对准ICCP算法计算量较大，Ingemar Nygren、陈小龙等人采用的极大似然估计需要高精度插值处理，孙枫等人利用惯性导航系统短时高精度的特点，采用基于误差平方和价值函数最优的等值线匹配算法，以及朱庄生等人提出基于三角形几何约束模型都需获取高精度等值线。利用本发明，可直接利用实测水深数据得到的水深累积直方图、地形粗糙度和惯导系统短时相对距离信息，无需插值处理、等值线获取，经过几次的水深测量即可快速、准确得到水下潜器航迹。(2)针对可靠性要求，目前大多数地形导航确定搜索区域的方法是采用惯导系统输出的水下潜器位置为中心，搜索半径根据实际情况确定。这种方法在惯性导航初始误差不大的情况下是可取的，在初始误差较大时，确定的搜索区域可能是不正确的或范围很大。利用本发明，不依赖惯导初始位置误差，第一次可能位置集的确定通过搜索整个海图，以后通过惯导系统短时相对距离信息和更新的可能位置集中各位置点之间的最大距离缩小搜索的区域，从而消除和克服在惯导系统初始误差大的情况下地形匹配算法失效的缺陷。

附图说明

图1为本发明所应用的水下潜器地形辅助惯性导航系统的基本原理框图；

图2为本发明水下潜器地形辅助惯性导航系统的位置匹配方法的算法流程图；

图3为本发明具体实施方式中多波束扫描区域示意图；

图4为本发明具体实施方式中不同地形区域的位置匹配仿真图。

具体实施方式

下面对本发明技术方案进行详细说明，但是本发明的保护范围不局限于所述实施例。

如图1所示，传统的地形辅助惯性导航系统一般包括A、B和C三个模块。A模块的主要目的是利用水下潜器装备的深度计和多波束扫描测深仪，测量水下潜器航行区域的水深值；B模块的主要目的是利用水下潜器装备的惯导系统的输出位置信息，计算相邻测量时刻之间的距离，并结合已有的海图获得海图搜索区域；C模块的目的是利用匹配算法，最终获得水下潜器的位置估计并修正惯导系统的位置参数。

本发明主要是研究C模块中匹配算法，以提高地形辅助惯性导航系统的实时性和可靠性。

如图2所示，本发明将按照以下步骤实现：

1)在进入海图覆盖区域后，利用水下潜器所装备的深度计和多波束扫描测深仪，测量水下潜器在起始时间段Δt₁内航行区域的水深值。

如图3所示，采用深度计和多波束扫描测深仪，测量水下潜器在起始时间段Δt₁内航行区域的水深值。实际中由于多波束测深的有效波束宽带，多波束脚印是有一定面积的，并且中央波束脚印间距小于边缘波束间距。若考虑将等角和等面积发射模式应用于新型多波束系统，使得中央波束脚印面积同边缘波束相近，测点间距基本一致。所以图3中为仿真简化，将多波束脚印视为一个质点，设多波束脚印间距一致。多波束的2个重要参数：多波束间距和多波束个数。多波束间距通常情况下最大为一个地图分辨率，保证一个地图分辨率网格至少存在一个多波束脚印。多波束个数过多过少均不宜，需要实际仿真中调整，水下潜器在测量时间段内航行的区域应尽量保持正方形。

具体仿真环境设置：

海图经度范围：130°～130.027°，纬度范围：20°～20.054°，分辨率约为25m×25m；水深量程范围：-7000m～0m；多波束扫描测深仪的多波束间距约为25m，多波束个数为20×20，此外，多波束间距可小于25m，多波束个数也可增加，可根据仿真情况调整。

2)计算时间段Δt₁的水深累积直方图H₁和地形粗糙度K_r1，并由K_r1计算下次水深测量的时间段Δt₂。计算过程如下：

根据整个海图的水深量程，将水深值平均分成14组，水深间隔为500m，统计步骤1)测量的水深值在每个水深间隔范围内的水深值个数，累积各水深间隔范围的水深值概率分布情况，得到时间段Δt₁的水深累积直方图H₁。依据K_r1确定下次水深测量的时间段Δt₂。其中，Δt₂取大于1/K_r1的最小整数。

3)将时间段Δt₁内扫描区域的大小作为整个海图搜索的网格单元，其中，扫描区域的大小根据式(1)计算，具体为：S＝(20×25)²m²；

将时间段Δt₁内扫描区域的大小S作为整个海图搜索的网格单元，采用滑动窗的方式搜索，搜索步进值为海图的分辨率，计算第1次测量时第m个滑动窗的水深累积直方图H_1m，计算方法同步骤2)，其中，m＝1,2,…,M，M由搜索区域和网格单元大小决定，计算H₁与H_1m之间的距离，将其中距离满足匹配准则式(2)的第m个滑动窗的中心位置作为水下潜器在Δt₁/2时刻的可能位置集L₁。

4)测量水下潜器在时间段Δt_n内航行区域的水深值，其中，n＝2,3…,N，计算时间段Δt_n的水深累积直方图H_n和地形粗糙度K_rn，并由K_rn计算下次水深测量的时间段Δt_n+1。

5)计算惯导系统在时间段Δt_n-1内航行的相对距离D_n-1,n，计算更新的可能位置集L′_n-1中各位置点之间的最大距离D_n-1，当n＝2时，L′₁＝L₁，计算L′_n-1集中各位置点之间的最大距离D_n-1，取D_n-1和2D_n-1,n中较大值为R，确定时间段Δt_n内水深测量的搜索区域为：纬度范围为[Latimin-R,Latimax+R]，经度范围为[longimin-R,longimax+R]，其中，Latimax和Latimin分别是L′_n-1集中纬度最大值和最小值，longimax和longimin分别是L′_n-1集中经度最大值和最小值，如果搜索区域的某边界超出海图的边界，以海图边界为最大值或者最小值；

采用滑动窗的方式搜索，计算第n次测量时第m个滑动窗的水深累积直方图H_nm，并计算H_nm与第n次测量的水深累积直方图H_n之间的距离，将其中距离满足匹配准则式(2)的第m个滑动窗的中心位置作为水下潜器在Δt_n/2时刻的可能位置集L_n。

6)计算L′_n-1和L_n中各位置点之间的距离并与D_n-1,n比较，将满足与D_n-1,n的绝对差值小于三倍海图分辨率条件的L′_n-1集作为水下潜器在Δt_n-1/2时刻的参考位置集P_n-1，同时将满足上述条件的L_n集作为水下潜器在Δt_n/2时刻的更新的可能位置集L′_n。

7)依据地形粗糙度K_rn决定实测总次数N，并判断实测次数是否达到N，若未达到，将n加1，并重复步骤4)～步骤6)；若实测次数达到N，根据式(3)计算参考位置集P₁至P_n-1的价值函数K，取价值函数最小值时的参考位置连线为最终获取的水下潜器航迹。由匹配的各时间段中点位置，结合水下潜器短时内匀速直航的特点，推算水下潜器当前的实际位置，最后通过位置重置修正惯导系统的位置参数。

如图4所示，仿真条件为水下潜器匀速直航，航行速度8节，速度误差0.1m/s，惯导系统陀螺常值漂移0.05°/h，加速度计常值偏置0.1mg。如图4(a)所示，初始航向北偏西15°，初始航向误差0.3°，初始位置误差1000米，K_r1、K_r2和K_r3均大于1，K_r平均值为5.5，地形起伏明显，实际调试中，N＝4，由P₁、P₂和P₃确定初始航迹；如图4(b)所示，初始航向北偏西30°，初始航向误差0.3°，初始位置误差500米，K_r1、K_r2和K_r3均小于1，K_r4>1，地形起伏相对平坦，实际调试中，N＝5，由P₁、P₂、P₃和P₄确定初始航迹。黑色直线代表真实航迹，黑色曲线代表惯导输出航迹，黑色方框为每次的搜索区域，粗黑色线段代表位置匹配的航迹。由匹配的各时间段中点位置，结合水下潜器短时内匀速直航的特点，推算水下潜器当前的实际位置，最后通过位置重置修正惯导系统的位置参数，修正结果与仿真中真实位置绝对误差小于一个地图分辨率。

以上所述仅是水下潜器地形辅助惯性导航系统的位置匹配方法，应当指出，水下潜器在两个相邻时间段内做严格的直线运动，不相邻的时间段可做任意运动。航迹跟踪也可采用类似位置匹配的方法，这些使用也应视为本发明的保护范围。

以上所述仅是水下潜器地形辅助惯性导航系统的位置匹配方法，应当指出，该方法用于惯性/重力和惯性/地磁无源组合导航也应视为本发明的保护范围。

如上所述，尽管参照特定的优选实施例已经表示和表述了本发明，但其不得解释为对本发明自身的限制。在不脱离所附权利要求定义的本发明的精神和范围前提下，可对其在形式上和细节上作出各种变化。

本发明说明书中未作详细描述的内容属于本领域专业技术人员公知的现有技术。

Claims (3)

1.水下潜器地形辅助惯性导航系统的位置匹配方法，其特征在于包括以下步骤：

1)在进入海图覆盖区域后，利用水下潜器所装备的深度计和多波束扫描测深仪，测量水下潜器在起始时间段Δt₁内航行区域的水深值；

2)计算时间段Δt₁的水深累积直方图H₁和地形粗糙度K_r1，并由K_r1计算下次水深测量的时间段Δt₂；

3)将时间段Δt₁内扫描区域的大小作为整个海图搜索的网格单元，其中，扫描区域的大小具体为：

S＝(kd)² (1)

式中，k为与水下潜器航迹垂直的平面内一次给出的多波束测深点个数，d为k个多波束测深点的中心点间距，S为时间段内扫描区域的大小，也称为网格单元；

采用滑动窗的方式搜索，搜索步进值为海图的分辨率，计算第1次测量时第m个滑动窗的水深累积直方图H_1m，其中，m＝1,2,…,M，M由搜索区域和网格单元大小决定，并计算H₁与H_1m之间的距离，其中，水深累积直方图之间的距离定义为相同水深间隔范围内累积概率值的均方误差，距离满足的匹配准则为：

$\frac{1}{num}\underset{i=1}{\overset{num}{\&Sigma;}}{(H_{ni}-H_{nmi})}^2<(\mathrm{\&epsiv;}/K_{rn})H_{\min }---\left(2\right)$

式中，n表示测量次数，num表示水深累积直方图的区间数，H_ni表示测量时间段Δt_n内第i个水深间隔范围的累积概率值，H_nmi表示时间段Δt_n内第m个滑动窗的第i个水深间隔范围的累积概率值，K_rn表示时间段Δt_n的地形粗糙度，ε表示调节因子，调节ε，使得(ε/K_rn)在12～25范围内，H_min为第n次测量的水深累积直方图H_n与第n次测量时第m个滑动窗的水深累积直方图H_nm之间的最小距离；

当n＝1时，将计算H₁与H_1m之间的距离中满足式(2)的第m个滑动窗的中心位置作为水下潜器在Δt₁/2时刻的可能位置集L₁；

4)测量水下潜器在时间段Δt_n内航行区域的水深值，其中，n＝2,3…,N，计算时间段Δt_n的水深累积直方图H_n和地形粗糙度K_rn，并由K_rn计算下次水深测量的时间段Δt_n+1；

5)计算惯导系统在时间段Δt_n-1内航行的相对距离D_n-1,n，计算更新的可能位置集L′_n-1中各位置点之间的最大距离D_n-1，当n＝2时，L′₁＝L₁，计算L′_n-1集中各位置点之间的最大距离D_n-1，取D_n-1和2D_n-1,n中较大值为R，确定时间段Δt_n内水深测量的搜索区域为：纬度范围为[Latimin-R,Latimax+R]，经度范围为[longimin-R,longimax+R]，其中，Latimax和Latimin分别是L′_n-1集中纬度最大值和最小值，longimax和longimin分别是L′_n-1集中经度最大值和最小值，如果搜索区域的某边界超出海图的边界，以海图边界为最大值或者最小值；

采用滑动窗的方式搜索，计算第n次测量时第m个滑动窗的水深累积直方图H_nm，并计算H_nm与第n次测量的水深累积直方图H_n之间的距离，将其中距离满足匹配准则式(2)的第m个滑动窗的中心位置作为水下潜器在Δt_n/2时刻的可能位置集L_n；

6)计算L′_n-1和L_n中各位置点之间的距离并与D_n-1,n比较，将满足与D_n-1,n的绝对差值小于三倍海图分辨率条件的L′_n-1集作为水下潜器在Δt_n-1/2时刻的参考位置集P_n-1，同时将满足上述条件的L_n集作为水下潜器在Δt_n/2时刻的更新的可能位置集L′_n；

7)依据地形粗糙度K_rn决定实测总次数N，并判断实测次数是否达到N，若未达到，将n加1，并重复步骤4)～步骤6)；若实测次数达到N，则计算参考位置集P₁至P_n-1的价值函数为：

$K=\frac{1}{N-2}\underset{n=2}{\overset{N-1}{\&Sigma;}}{\&lsqb;D(P_{n-1},P_n)-D_{n-1,n}\&rsqb;}^2---\left(3\right)$

式中，D(P_n-1,P_n)表示参考位置集P_n-1与P_n各位置点之间的距离，K为价值函数；依据地形粗糙度K_rn确定实测总次数N，具体确定原则为：K_rn>1时，取N≧3，K_rn<1时，取N≧4；根据式(3)计算参考位置集P₁至P_n-1的价值函数K，取价值函数为最小值时的参考位置连线为最终获取的水下潜器航迹，由匹配的各时间段中点位置，结合水下潜器短时内匀速直航的特点，推算水下潜器当前的实际位置，最后通过位置重置修正惯导系统的位置参数。

2.根据权利要求1所述的水下潜器地形辅助惯性导航系统的位置匹配方法，其特征在于：所述步骤1)中水下潜器在测量起始时间段内航行的区域应尽量保持正方形，此外，在后续每次测量时间段内航行的区域也同样保持正方形。

3.根据权利要求1所述的水下潜器地形辅助惯性导航系统的位置匹配方法，其特征在于：所述步骤2)中计算时间段Δt₁的水深累积直方图H₁具体过程为：根据整个海图的水深量程，将水深值平均分成10组到20组不重叠区间，确定水深间隔范围，统计步骤1)测量的水深值在每个水深间隔范围内的水深值个数，累积各水深间隔范围的水深值概率分布情况，得到时间段Δt₁的水深累积直方图H₁；

地形粗糙度定义为单位面积的水深值方差，K_r1表示时间段Δt₁的地形粗糙度，依据K_r1确定下次水深测量的时间段Δt₂，其中，Δt₂取大于1/K_r1的最小整数。