CN113932811A - 一种新的地形匹配导航系统、方法 - Google Patents

Abstract

本发明公开了一种新的地形匹配导航系统、方法，包括初始定位模块、滤波器初始化模块、初始对准滤波过程模块、滤波器状态监测模块一、跟踪导航滤波过程模块、滤波器状态监测模块二和定位输出模块。在地形匹配定位似然函数存在多个伪波峰的情况下，本发明所述地形匹配导航系统、方法可获得较传统方法更加稳定的定位信息和更加高效的滤波收敛速度。

Description

一种新的地形匹配导航系统、方法

技术领域

本发明涉及一种地形匹配导航系统，尤其涉及一种新的地形匹配导航系统、方法。

背景技术

地形匹配导航采用环境特征匹配与跟踪方式进行定位解算，其本质是一种组合导航系统，由主导航系统(一般为惯性导航)和地形匹配定位系统通过信息融合模块组合而成，而地形匹配定位信息仅仅作为主导航系统的辅助信息。由于主导航系统往往存在随时间无限累积的误差，在地形匹配导航系统启动时刻，主导航系统往往会输出较大的定位误差，导致地形匹配导航起始时刻的匹配定位误差和融合定位误差均较大，由此引起较大的地形匹配导航系统滤波器初始化误差。在滤波器迭代过程中，初始化误差随迭代过程向前传递导致滤波过程及其不稳定。

发明内容

本发明的目的是提供一种新的地形匹配导航系统、方法，可获得较传统方法更加稳定的定位信息和更加高效的滤波收敛速度。

本发明为实现上述发明目的采用如下技术方案：

本发明提供了一种新的地形匹配导航系统，包括初始定位模块、滤波器初始化模块、初始对准滤波过程模块、滤波器状态监测模块一、跟踪导航滤波过程模块、滤波器状态监测模块二和定位输出模块；

所述初始定位模块获取系统在导航起点的初始定位信息，将该初始定位信息输入到所述滤波器初始化模块；

所述滤波器初始化模块获取到初始定位信息后执行滤波器初始化过程，并获得初始化信息，将该初始化信息输入到所述初始对准滤波过程模块；

所述初始对准滤波过程模块中的滤波器通过获取所述滤波器初始化模块输入的初始化信息完成滤波器启动，并实时地输出地形匹配系统的初始对准信息，将该信息输入到所述滤波器状态监测模块一；

所述滤波器状态监测模块一判断所述初始对准滤波过程模块中的初始对准信息是否收敛，如果初始对准信息收敛，则系统完成初始对准，进入所述跟踪导航滤波过程模块，如果初始对准信息未收敛，则继续执行所述初始对准滤波过程模块；

所述跟踪导航滤波过程模块的滤波器输出地形匹配导航信息至所述滤波器状态监测模块二，如果所述滤波器状态监测模块二判断所述跟踪导航滤波过程模块输出的地形匹配导航信息未发散，则所述跟踪导航滤波过程的滤波器输出的地形匹配导航信息为有效的地形匹配导航信息，否则，则系统重新执行所述初始定位模块。

进一步地，所述初始定位信息包括定位点、定位误差及定位概率分布函数。

本发明提供了一种新的地形匹配导航系统、方法，采用上述一种新的地形匹配导航系统，所述方法包括：

进行地形匹配导航系统的初始匹配定位模块；

在完成初始匹配定位后进行地形匹配导航系统滤波器初始化模块；

在完成地形匹配导航系统滤波器初始化模块之后，开始运行系统初始对准滤波过程模块；

在运行系统初始对准滤波过程模块的过程中，根据系统初始对准滤波过程模块的输出信息实时监测输出信息的收敛性；

在监测出系统初始对准滤波过程模块的输出信息收敛后，执行地形匹配导航跟踪导航滤波模块；

在执行地形匹配导航跟踪导航滤波模块的过程中，根据地形匹配导航跟踪导航滤波模块的输出信息实时监测输出信息的收敛性，如果输出信息始终保持收敛，则系统输出定位信息，如果监测到输出信息发散，则进行重新初始匹配定位。

进一步地，进行地形匹配导航系统的初始匹配定位模块的方法包括：

在地形匹配导航规划点的起点wp_i，i＝1，基于主导航系统给出的导航定位点

i＝1和导航误差

i＝1计算主导航系统的误差椭圆及矩形搜索区间；

以矩形搜索区间为匹配搜索区间，获得地形匹配导航规划点的起点wp_i，i＝1的地形匹配定位点

i＝1和地形匹配定位点的定位误差

i＝1：

其中，k_i表示地形匹配定位点的索引号，k_i＝1，2…K_i，K_i表示地形匹配导航规划点wp_i获得的地形匹配定位点总数。

进一步地，在完成初始匹配定位模块后进行地形匹配导航系统滤波器初始化模块的方法包括：

在地形匹配导航规划点的起点wp_i；i＝1，根据主导航系统给出的主导航定位点

i＝1、主导航定位点的定位误差

i＝1、地形匹配定位点

i＝1和地形匹配定位点的定位误差

i＝1，计算水下航行器路径定位点

i＝1和水下航行器路径定位点的定位误差

i＝1，计算公式如下：

式中，n_i∈{1，2，3…N_i}表示地形匹配导航规划点wp_i点的水下航行器路径定位点的索引，N_i表示有效路径的总数，其中N₀＝1，n₀＝1；

在地形匹配导航规划点的起点wp_i，i＝1，计算水下航行器路径定位点的估计代价

N_i＝n_i-1×k_i表示wp_i点获得的有效路径总数，公式如下：

将

按照由小到大的顺序排序，得到序列

保存

对应在

中的索引号；

根据

的索引号对地形匹配导航规划点wp_i获得的水下航行器路径定位点

水下航行器路径定位点的定位误差

水下航行器路径定位点的定位偏差

水下航行器路径定位点的定位误差的方差下确界

进行排序，得到序列

进一步地，在完成地形匹配导航系统初始化模块之后，开始执行系统初始对准滤波过程模块的方法包括：

在i＞1的时，水下航行器经过水下航行器路径定位点

到达wp_i+1点的过程中，主导航系统给出的航行距离D_i，i+1，主导航系统给出的主导航误差增值De_i，i+1，则水下航行器经过wp_i点的N_i条路径定位点序列

到达wp_i+1点后，主导航系统的定位点为：

主导航定位误差为：

根据wp_i+1点获得的地形匹配定位点

地形匹配定位误差

计算水下航行器路径定位点序列

和水下航行器路径定位点的定位误差

其中n_i+1∈{1，2，3…N_i+1}表示wp_i+1点的水下航行器路径定位点索引，k_i+1∈{1，2，3…K_i+1}为wp_i+1点的地形匹配定位点索引号，公式如下：

在获得水下航行器路径定位点

和水下航行器路径定位点的定位误差

后，计算④中的N_i+1条水下航行器路径定位点的估计代价

公式如下：

式中，k∈{1，2，3…K_i+1}为wp_i+1点的地形匹配定位点索引号，n_i∈{1，2，3…N_i}为wp_i点的水下航行器路径定位点索引号；

将

按照由小到大的顺序排序，得到序列

保存

对应在

中的索引号；

根据

的索引号对地形匹配导航规划点wp_i+1获取的水下航行器路径定位点

水下航行器路径定位点的定位误差

水下航行器路径定位点的定位偏

水下航行器路径定位点的定位误差的方差下确界

进行排序，得到序列

根据水下航行器路径定位点的定位偏

和水下航行器路径定位点的定位误差

对水下航行器路径定位点的有效性进行筛选；

获得筛选后的水下航行器路径定位点、水下航行器路径定位点的定位误差分别为：Ddte(m)、Ddt(m)，并将此序列重新赋值给

从而获得新的水下航行器路径定位点、水下航行器路径定位点的定位误差；

根据新的水下航行器路径定位点

和水下航行器路径定位点的定位误差

及水下航行器路径定位点的估计代价

选出水下航行器路径定位点的估计代价中的极小值

并提取其在序列

中对应的索引号

根据索引号

选择水下航行器路径定位点

中与索引号

对应的最优水下航行器路径定位点

随后执行水下航行器路径定位过程的收敛性判断。

进一步地，进行系统初始对准滤波过程模块的输出信息的状态检测和收敛性判断的方法包括：

地形匹配导航系统状态检测模块一根据获得的水下航行器路径定位点

和水下航行器路径定位点的定位误差

及水下航行器路径定位点的估计代价

计算水下航行器路径定位点的融合定位点

α是为了确保

成立的归一化矩阵，I为单位矩阵；

计算最优水下航行器路径定位点

与

之间的偏差

水下航行器路径定位收敛判断关系式：D_Ddtlimlnli+1min＜γ·Ddtei+1ni+1，1＜γ＜3，是否成立，如果成立，则表明水下航行器路径定位满足收敛要求，地形匹配导航系统初始对准滤波过程模块的输出信息收敛，完成系统初始对准过程模块的运行，i+1时刻的最优水下航行器路径定位点

即为地形匹配导航输出，同时地形匹配导航系统进入地形匹配导航跟踪导航滤波模块。

本发明的有益效果如下：

在地形匹配定位似然函数存在多个伪波峰的情况下，本发明所述地形匹配导航系统、方法可获得较传统方法更加稳定的定位信息和更加高效的滤波收敛速度；

本发明所述新的地形匹配导航系统包含系统状态监测模块一和系统状态监测模块二，该模块通过监测和反馈系统滤波器运行状态，可实现地形匹配导航系统运行状态的自主感知和运行状态发散情况下的自主重新初始对准；

新的地形匹配导航系统可自主感知地形匹配导航系统滤波器发散运行状态，并在地形匹配导航系统滤波器发散状态下自主控制地形匹配导航系统完成系统重新初始定位，新的地形匹配导航系统运行架构可使地形匹配导航系统具有更高的运行可靠性；

新的地形匹配导航系统可自主感知地形匹配导航系统滤波器收敛运行状态，并在地形匹配导航系统滤波器收敛状态下自主完成导航模式的切换，将系统导航模式由初始对准模式切换至跟踪导航模式，并输出有效的地形匹配导航信息。

附图说明

图1为根据本发明实施例提供的一种新的地形匹配导航系统的框架图；

图2为根据本发明实施例提供的一种新的地形匹配导航方法中初始定位误差椭圆及初始地形匹配搜索区间图；

图3为根据本发明实施例提供的一种新的地形匹配导航方法中地形匹配定位点及其置信区间图；

图4为根据本发明实施例提供的一种新的地形匹配导航方法中初始地形匹配定位及其到达下一wp_i的航迹图；

图5为根据本发明实施例提供的一种新的地形匹配导航方法中wp₁点的有效航迹和wp₂点的地形匹配定位点图；

图6为根据本发明实施例提供的一种新的地形匹配导航方法中地形匹配导航初始对准过程中数据结果和流程图。

具体实施方式

下面结合具体实施例对本发明作进一步描述。以下实施例仅用于更加清楚地说明本发明的技术方案，而不能以此来限制本发明的保护范围。

如图1所示，本发明公开的一种新的地形匹配导航系统，①包括地形匹配导航系统初始定位模块1、地形匹配导航系统滤波初始化模块2、地形匹配导航系统初始对准滤波模块3、地形匹配导航系统滤波器状态监测模块一4、地形匹配导航系统收敛性判断5、地形匹配导航系统跟踪导航滤波6、地形匹配导航系统滤波器状态监测模块二7、地形匹配导航系统滤波发散判定8、地形匹配导航系统输出9。其中地形匹配导航系统初始定位1、地形匹配导航系统滤波初始化2、地形匹配导航系统初始对准滤波3、地形匹配导航系统滤波器状态监测一4构成了地形匹配导航系统的初始对准模块10，该模块也是本算法运行框架与其他地形匹配导航系统不同之处。

②在新的地形匹配导航算法运行框架中的地形匹配导航系统初始对准滤波过程模块3和地形匹配导航跟踪滤波过程模块6后面均加入了地形匹配导航滤波器状态检测模块一4和地形匹配导航滤波器状态检测模块二7；在地形匹配导航滤波器状态检测模块一4和地形匹配导航滤波器状态检测模块二7后又分别加入了系统滤波器状态收敛检测模块5和滤波器发散检测模块8，这也是本算法运行框架与其他算法运行框架不同之处。

③地形匹配导航系统运行框架中的各个模块及其内部算法的执行流程概述如下：

a)地形匹配导航系统初始定位模块1获取系统在导航起点的定位点、定位误差及定位概率分布信息，将该信息输入到地形匹配导航系统滤波器初始化模块2；

b)地形匹配导航系统滤波器初始化模块2获取到初始定位信息后执行滤波器初始化过程，并获得地形匹配导航系统初始对准过程滤波器的初始化信息，包括初始定位点、定位误差、定位概率分布等滤波器初始化信息，该信息直接输入到地形匹配导航系统初始对准滤波过程模块3。

c)地形匹配导航系统初始对准滤波过程模块3执行地形匹配导航系统的初始对准过程，在该过程中模块3通过获取地形匹配导航系统滤波器初始化模块2的初始化信息完成初始对准滤波器启动，并实时输出地形匹配导航的定位信息、定位误差信息、实时测量地形、地形测量误差等信息，这些信息被输入到地形匹配导航滤波器状态监测模块一4。

d)地形匹配导航滤波器状态监测模块一4判断地形匹配导航系统初始对准滤波过程模块3输出的导航信息是否收敛，如果模块3输出的导航信息是收敛的，则系统完成初始对准，进入跟踪导航阶段。如果尚未收敛，则继续执行地形匹配导航系统初始对准滤波过程模块3。

e)在地形匹配导航滤波器状态监测模块一4判断地形匹配导航系统初始对准滤波过程模块3输出的导航信息是收敛的情况下，系统将运行地形匹配导航跟踪导航滤波模块6，进入跟踪导航阶段。跟踪导航阶段同样是执行连续的定位和定位误差估计，跟踪导航阶段的滤波器初始值即在图1地形匹配导航滤波器状态监测模块一判断初始对准过程滤波收敛后，初始对准滤波器输出的定位信息作为地形匹配导航跟踪导航滤波模块6的初始化信息。

f)地形匹配导航跟踪导航滤波模块6的输出信息同样的被输入到地形匹配导航滤波器状态监测模块二7中，如果地形匹配导航滤波器状态监测模块二判断地形匹配导航跟踪导航滤波模块6的输出信息是稳定和收敛的，也即是跟踪导航滤波器状态没有发散，则地形匹配导航跟踪导航滤波模块6输出的定位信息即是有效的地形匹配导航定位信息。否则，跟踪导航滤波器发散，则系统进入重新初始对准过程，此时地形匹配导航跟踪导航滤波模块6输出定位点、定位误差等信息，并执行地形匹配导航系统初始定位模块1。

新的地形匹配导航方法的执行过程为：

①地形匹配导航系统的初始匹配定位模块执行过程。初始匹配定位模块也即是图1中模块1，模块1执行后将获取地形匹配导航系统初始定位信息。假设地形匹配导航规划点wp_i，其中i表示地形匹配导航规划点索引号，主导航系统在地形匹配导航规划点wp_i输出导航定位点D_i和导航误差De_i。当i＝1时表示第一个地形匹配导航规划点，也就是地形匹配导航的起点。因此，主导航定位点位于wp_i，i＝1时，将触发地形匹配导航系统的初始匹配定位模块执行。初始匹配定位的执行过程如下：

a)基于主导航系统给出的导航定位点D₁11和导航误差De₁计算主导航系统的误差椭圆12及矩形搜索区间13。

b)以矩形搜索区间13为匹配搜索区间，计算该区间内的地形匹配定位概率分布，并获得地形匹配定位点15、16和地形匹配定位置信区间17、18。

c)地形匹配定位可能存在多个地形匹配定位点和地形匹配定位置信区间。

表示地形匹配定位点，

表示地形匹配定位误差，其中i表示地形匹配导航规划点的索引号，

k_i∈{1，2…K_i}表示地形匹配定位获得的定位点索引号，K₁表示wp₁点获得的地形匹配定位点总数(即i＝1，地形匹配到起点)。

②在完成初始匹配定位模块执行过程①后进行进入地形匹配导航系统滤波器初始化模块执行过程，其滤波器初始化模块的执行过程的流程如下：

a)在地形匹配导航起点wp₁(即i＝1时)，根据主导航系统给出的主导航定位点D₁，主导航误差De₁，地形匹配定位点

地形匹配定位误差

计算水下航行器路径定位点

i＝1和水下航行器路径定位点的定位误差

i＝1。

式中，n_i∈{1，2，3…N_i}表示地形匹配导航规划点wp_i点的水下航行器路径定位点索引，N_i表示水下航行器路径定位点的总数，其中N₀＝1，n_i＝1。

b)然后，计算i＝1时水下航行器路径定位点估计的代价函数值

k_i＝1，2…K₁，K₁表示在地形匹配导航规划点wp₁获得的地形匹配定位点总数，k_i表示地形匹配定位点的索引号，N₁＝n_i-1×k_i表示在地形匹配导航规划点wp₁获得的水下航行器路径定位点总数。

c)然后，将

按照由小到大的顺序排序，得到序列

n_i∈{1，2，3…N_i}，

保存

对应在

中的索引号。根据

的索引号对地形匹配导航规划点wp_i获得的水下航行器路径定位点

水下航行器路径定位点定位误差

水下航行器路径定位点定位偏差

水下航行器路径定位点定位误差的方差下确界

进行排序，得到序列

n_i∈{1，2，3…N_i}。

③在完成地形匹配导航系统初始化模块的执行过程②之后，进入地形匹配导航系统初始对准滤波模块的执行过程，该过程的具体流程如下。

a)水下航行器根据wp_i点获得的水下航行器路径定位点继续向前航行到达下一个地形匹配导航规划点wp_i+1。如图4所示，11和21分别表示在wp_i点获得的水下航行器路径定位点对应的航迹，水下航行器可通过19和22到达wp_i+1。

获得水下航行器由wp_i点的N_i个水下航行器路径定位点到达wp_i+1点的主导航距离：D_i，i+1，主导航误差为De_i，i+1，则水下航行器经过地形匹配导航路径规划点wp_i的水下航行器路径定位点由wp_i到达地形匹配导航规划点wp_i+1后的主导航定位点为：

主导航误差为：

根据地形匹配导航系统在wp_i+1点获得的地形匹配定位点

地形匹配定位误差

计算水下航行器路径定位点

和水下航行器路径定位点的定位误差

其中n_i+1∈{1，2，3…N_i+1}表示wp_i+1点的水下航行器路径定位点索引，k_i+1∈{1，2，3…K_i+1}为wp_i+1点的地形匹配定位点索引号。如图5所示，22表示wp₂的地形匹配定位搜索区间，23、24、25为wp₂点的地形匹配定位点，26、27、28分别表示其定位置信区间，20和23表示由wp₁点的水下航行器路径定位点到达wp₂点的主导航点，19和22为wp₁的水下航行器路径定位点对应的主导航航迹，则wp₂点的有效航迹数为N₂＝2×3＝6。

在获得水下航行器路径定位点

后，计算④中的N_i+1个水下航行器路径定位点的估计代价值

式中，k∈{1，2，3…K_i+1}为wp_i+1点的地形匹配定位点索引号，n_i∈{1，2，3…N_i}为wp_i点的有效航迹的索引号。

b)将

按照由小到大的顺序排序，得到序列

n_i+1∈{1，2，3…N_i+1}，

保存

对应在

中的索引号。

其次，根据

的索引号对地形匹配导航规划点wp_i+1的水下航行器路径定位点

水下航行器路径定位点的定位误差

水下航行器路径定位点的定位偏差

水下航行器路径定位误差的方差下确界

进行排序，得到序列

至此，在地形匹配导航路径规划点wp_i，i≥2获得经过筛选的

个水下航行器路径定位点、同时获得筛选后的水下航行器路径定位点的定位误差、水下航行器路径定位点的估计代价分别为：

c)根据b)中的序列

选出其中的极小值

并提取其在序列

中对应的索引号

根据索引号

选择水下航行器路径定位点

中与索引号

对应的最优水下航行器路径定位点

d)在每一个地形匹配导航规划点wp_i进行定位算法的收敛性判断。具体如下：首先，地形匹配导航系统状态检测模块一根据获得的水下航行器路径定位点

和水下航行器路径定位点的定位误差

及水下航行器路径定位点的估计代价

计算水下航行器路径定位点的融合定位点

α是为了确保

成立的归一化矩阵，I为单位矩阵。然后，计算最优水下航行器路径定位点

之间的偏差

其次，水下航行器路径定位收敛判断关系式：

1＜γ＜3，是否成立，如果成立，则表明水下航行器路径定位满足收敛要求，地形匹配导航系统初始对准滤波过程模块的输出信息收敛，完成系统初始对准过程模块的运行，i+1时刻的最优水下航行器路径定位点

即为地形匹配导航输出，同时地形匹配导航系统进入地形匹配导航跟踪导航滤波模块。如果不满足要求，水下航行器继续航行，到达下一个地形匹配导航规划点wp_i+2，i≥2，在wp_i+2，i≥2点重复步骤③，直到水下航行器路径定位收敛判断关系式满足为止。随着i值(29)的增加，形成如图6所示的数据流程图。

④在完成系统初始对准滤波过程模块执行过程③后，地形匹配导航系统初始对准滤波过程模块的输出信息收敛，地形匹配导航系统进入地形匹配导航跟踪导航滤波模块的执行阶段。地形匹配导航跟踪导航滤波模块在运行过程中向地形匹配导航滤波器状态检测模块二输入地形匹配导航信息、测量地形和地形测量误差信息等，地形匹配导航滤波器状态检测模块二根据这些信息判断地形匹配导航跟踪导航滤波模块的输出信息是否发散，以此判断该模块的运行状态是否稳定，若地形匹配导航跟踪导航滤波模块的输出信息发散，则地形匹配导航跟踪导航滤波模块的运行状态不稳定，则地形匹配导航系统需要重新回到地形匹配导航初始定位模块，并重新完成上述程序。若地形匹配导航跟踪导航滤波模块的输出信息没有发散，则地形匹配导航跟踪导航滤波模块输出有效的导航定位信息。

以上对本发明的较佳实施进行了具体说明，当然，本发明还可以采用与上述实施方式不同的形式，熟悉本领域的技术人员在不违背本发明精神的前提下所作的等同的变换或相应的改动，都应该属于本发明的保护范围内。

Claims (7)

1.一种新的地形匹配导航系统，其特征在于，包括初始定位模块、滤波器初始化模块、初始对准滤波过程模块、滤波器状态监测模块一、跟踪导航滤波过程模块、滤波器状态监测模块二和定位输出模块；

所述初始定位模块获取系统在导航起点的初始定位信息，将该初始定位信息输入到所述滤波器初始化模块；

所述滤波器初始化模块获取到初始定位信息后执行滤波器初始化过程，并获得初始化信息，将该初始化信息输入到所述初始对准滤波过程模块；

所述初始对准滤波过程模块中的滤波器通过获取所述滤波器初始化模块输入的初始化信息完成滤波器启动，并实时地输出地形匹配系统的初始对准信息，将该信息输入到所述滤波器状态监测模块一；

所述滤波器状态监测模块一判断所述初始对准滤波过程模块中的初始对准信息是否收敛，如果初始对准信息收敛，则系统完成初始对准，进入所述跟踪导航滤波过程模块，如果初始对准信息未收敛，则继续执行所述初始对准滤波过程模块；

所述跟踪导航滤波过程模块的滤波器输出地形匹配导航信息至所述滤波器状态监测模块二，如果所述滤波器状态监测模块二判断所述跟踪导航滤波过程模块输出的地形匹配导航信息未发散，则所述跟踪导航滤波过程的滤波器输出的地形匹配导航信息为有效的地形匹配导航信息，否则，则系统重新执行所述初始定位模块。

2.根据权利要求1所述的一种新的地形匹配导航系统，其特征在于，所述初始定位信息包括定位点、定位误差及定位概率分布函数。

3.一种新的地形匹配导航方法，其特征在于，采用如权利要求1或2所述的一种新的地形匹配导航系统，所述方法包括：

进行地形匹配导航系统的初始匹配定位模块；

在完成初始匹配定位后进行地形匹配导航系统滤波器初始化模块；

在完成地形匹配导航系统滤波器初始化模块之后，开始运行系统初始对准滤波过程模块；

在运行系统初始对准滤波过程模块的过程中，根据系统初始对准滤波过程模块的输出信息实时监测输出信息的收敛性；

在监测出系统初始对准滤波过程模块的输出信息收敛后，执行地形匹配导航跟踪导航滤波模块；

在执行地形匹配导航跟踪导航滤波模块的过程中，根据地形匹配导航跟踪导航滤波模块的输出信息实时监测输出信息的是否发散，如果输出信息没有发散，则系统输出定位信息，如果监测到输出信息发散，则进行回到初始匹配定位模块，进行重新进行初始对准。

4.根据权利要求3所述的一种新的地形匹配导航方法，其特征在于，进行地形匹配导航系统的初始匹配定位模块的方法包括：

在地形匹配导航规划点的起点wp_i，i＝1，基于主导航系统给出的导航定位点

和导航误差

计算主导航系统的误差椭圆及矩形搜索区间；

以矩形搜索区间为匹配搜索区间，获得地形匹配导航规划点的起点wp_i，i＝1的地形匹配定位点

和地形匹配定位点的定位误差

其中，k_i表示地形匹配定位点的索引号，k_i＝1，2...K_i，K_i表示地形匹配导航规划点wp_i获得的地形匹配定位点总数。

5.根据权利要求4所述的一种新的地形匹配导航方法，其特征在于，在完成初始匹配定位模块后进行地形匹配导航系统滤波器初始化模块的方法包括：

在地形匹配导航规划点的起点wp_i；i＝1，根据主导航系统给出的主导航定位点

主导航定位点的定位误差

地形匹配定位点

和地形匹配定位点的定位误差

计算水下航行器路径定位点

和水下航行器路径定位点的定位误差

计算公式如下：

式中，n_i∈{1，2，3...N_i}表示地形匹配导航规划点wp_i点的水下航行器路径定位点的索引，N_i表示有效路径的总数，其中N₀＝1，n₀＝1；

在地形匹配导航规划点的起点wp_i，i＝1，计算水下航行器路径定位点的估计代价

N_i＝n_i-1×k_i表示wp_i点获得的有效路径总数，公式如下：

将

按照由小到大的顺序排序，得到序列

保存

对应在

中的索引号；

根据

的索引号对地形匹配导航规划点wp_i获得的水下航行器路径定位点

水下航行器路径定位点的定位误差

水下航行器路径定位点的定位偏差

水下航行器路径定位点的定位误差的方差下确界

进行排序，得到序列

6.根据权利要求5所述的一种新的地形匹配导航方法，其特征在于，在完成地形匹配导航系统初始化模块之后，开始执行系统初始对准滤波过程模块的方法包括：

在i＞1的时，水下航行器经过水下航行器路径定位点

到达wp_i+1点的过程中，主导航系统给出的航行距离D_i，i+1，主导航系统给出的主导航误差增值De_i，i+1，则水下航行器经过wp_i点的N_i条路径定位点序列

到达wp_i+1点后，主导航系统的定位点为：

主导航定位误差为：

根据wp_i+1点获得的地形匹配定位点

地形匹配定位误差

计算水下航行器路径定位点序列

和水下航行器路径定位点的定位误差

其中n_i+1∈{1，2，3...N_i+1}表示wp_i+1点的水下航行器路径定位点索引，k_i+1∈{1，2，3...K_i+1}为wp_i+1点的地形匹配定位点索引号，公式如下：

在获得水下航行器路径定位点

和水下航行器路径定位点的定位误差

后，计算④中的N_i+1条水下航行器路径定位点的估计代价

公式如下：

式中，k∈{1，2，3...K_i+1}为wp_i+1点的地形匹配定位点索引号，n_i∈{1，2，3...N_i}为wp_i点的水下航行器路径定位点索引号；

将

按照由小到大的顺序排序，得到序列

保存

对应在

中的索引号；

根据

的索引号对地形匹配导航规划点wp_i+1获取的水下航行器路径定位点

水下航行器路径定位点的定位误差

水下航行器路径定位点的定位偏

水下航行器路径定位点的定位误差的方差下确界

进行排序，得到序列

根据水下航行器路径定位点的定位偏

和水下航行器路径定位点的定位误差

对水下航行器路径定位点的有效性进行筛选；

获得筛选后的水下航行器路径定位点、水下航行器路径定位点的定位误差分别为：Ddte(m)、Ddt(m)，并将此序列重新赋值给

从而获得新的水下航行器路径定位点、水下航行器路径定位点的定位误差；

根据新的水下航行器路径定位点

和水下航行器路径定位点的定位误差

及水下航行器路径定位点的估计代价

选出水下航行器路径定位点的估计代价中的极小值

并提取其在序列

中对应的索引号

根据索引号

选择水下航行器路径定位点

中与索引号

对应的最优水下航行器路径定位点

随后执行水下航行器路径定位过程的收敛性判断。

7.根据权利要求6所述的一种新的地形匹配导航系统初始化方法，其特征在于，进行系统初始对准滤波过程模块的输出信息的状态检测和收敛性判断的方法包括：

地形匹配导航系统状态检测模块一根据获得的水下航行器路径定位点

和水下航行器路径定位点的定位误差

及水下航行器路径定位点的估计代价

计算水下航行器路径定位点的融合定位点

α是为了确保

成立的归一化矩阵，I为单位矩阵；

计算最优水下航行器路径定位点

与

之间的偏差

水下航行器路径定位收敛判断关系式：

是否成立，如果成立，则表明水下航行器路径定位满足收敛要求，地形匹配导航系统初始对准滤波过程模块的输出信息收敛，完成系统初始对准过程模块的运行，i+1时刻的最优水下航行器路径定位点

即为地形匹配导航输出，同时地形匹配导航系统进入地形匹配导航跟踪导航滤波模块。

Images