一种匹配导航系统概率函数自主分裂与融合滤波方法

1.本发明涉及一种匹配导航系统概率函数自主分裂与融合滤波方法，属于地形匹配导航系统技术领域。


背景技术：

2.深海空间是电磁波屏蔽空间，因此无法接受卫星定位信号，无法进行长期的精确导航定位。水下地形匹配导航是一种基于环境信息特征匹配跟踪的定位导航方法，该导航方式利用水下机器人感知环境特征并与先验环境地图进行匹配识别从而获得定位估计，从而实现水下机器人的完全自主定位与导航。但对于执行深海或者长航程探测任务的水下机器人，往往因为长时间的主导航系统误差累计而在地形匹配导航初始时刻造成很大的初始定位偏差。地形匹配导航系统滤波器的定位概率分布函数存在伪波峰，伪波峰之间形成信息的相互耦合和干扰，导致系统运行过程中存在滤波不稳定和滤波发散的风险，并对地形匹配导航系统的可靠性有直接影响。


技术实现要素：

3.本发明要解决的技术问题是，克服现有技术的缺陷，提供一种匹配导航系统概率函数自主分裂与融合滤波方法，能够在地形匹配导航系统的初始运行阶段，通过定位概率分布函数的自主分裂降低伪波峰的干扰。
4.为解决上述技术问题，本发明采用的技术方案为：
5.一种匹配导航系统概率函数自主分裂与融合滤波方法，包括以下步骤：
6.s01，地形匹配导航系统提前录入地形匹配导航规划点序列wpi,i＝1,2,
…
i，其中i代表地形匹配导航规划点总数，主导航系统将定位信息输入到地形匹配导航系统中，地形匹配导航系统根据索引号从地形匹配导航规划点序列中获取当前需要处理的地形匹配导航规划点，地形匹配导航系统根据定位信息判断载体是否到达地形匹配导航规划点；
7.s02，当主导航系统定位到载体到达地形匹配导航规划点的起点时，此时地形匹配导航规划点序列索引号i为1，地形测量模块获取实时地形测量信息，实时地形测量信息、定位信息和地形匹配导航规划点构成的数据包输入到地形匹配定位模块中进行处理，处理后输入到概率分布函数分裂模块进行概率分布函数分裂，最后，地形匹配导航规划点的索引号i增加1，即i＝i+1；
8.s03，当主导航系统定位到载体到达地形匹配导航规划点的非起点时，此时地形匹配导航规划点序列索引号i大于1，实时地形测量模块获取实时地形测量信息，实时地形测量信息、定位信息和地形匹配导航规划点构成的数据包输入到滤波模块中进行处理，处理后输入到概率分布函数融合模块进行概率分布函数融合，最后，地形匹配导航规划点的索引号i增加1，即i＝i+1；
9.s04，重复s03，并判断i＞i是否成立，若成立则地形匹配导航程序运行结束，概率分布函数融合模块输出导航定位信息，如不成立则继续执行s03，直到其成立为止。
10.s01中，定位信息包括导航t时间节点的主导航定位点d
t
和主导航定位误差de
t
，地形匹配导航规划点序列wpi的索引号i初始化等于1，当wpi与主导航定位点d
t
差值的绝对值小于误差∈时，则判断载体到达第i个地形匹配导航规划点。
11.s02中，地形匹配定位模块处理后获得匹配定位信息，匹配定位信息包括地形匹配定位似然函数、地形匹配定位点和地形匹配定位误差，将匹配定位信息输入到概率分布函数分裂模块进行概率分布函数分裂流程。
12.概率分布函数分裂模块中概率函数分裂流程具体包括以下步骤：s021，设置一个阈值分别表示一个二维序列的统计特征阈值，其中表示序列的方差统计、ε
μ
表示序列的均值、εr表示序列的相关系数；
13.s022，从地形匹配定位模块获得地形匹配定位似然函数l，计算地形匹配定位似然函数l的所有峰值点其中n0表示峰值点的总数，表示峰值点的坐标值；表示峰值点的定位误差的协方差矩阵，通过地形的fisher信息量求解；
14.s023，利用高斯核对似然函数l进行拟合，公式如下：
[0015][0016]
其中，l1表示第1个地形匹配定位点的地形匹配定位似然函数的高斯核拟合函数；n表示高斯核的个数；表示高斯核函数的系数；表示均值为方差为的高斯核函数；
[0017]
在此基础上，计算拟合残差δl1：
[0018]
δl1＝l-l1ꢀꢀ
(2)
[0019]
然后判断残差δl1的统计参数是否满足公式(3)，
[0020][0021]
如果满足则完成概率分布函数分裂步骤；如果不满足，则继续转s024；
[0022]
s024，在获得δl1之后，同s021，计算δl1的所有峰值点的所有峰值点其中n1表示峰值点的总数，表示峰值点的坐标值，表示峰值点的定位误差的方差，通过地形的fisher信息量求解；
[0023]
s025，再次利用公式(1)，计算δl1的高斯拟合l2：
[0024][0025]
在此基础上，计算拟合残差：
[0026]
δl2＝δl
1-l2ꢀꢀ
(5)
[0027]
在此基础上，将公式(3)中δl1替换为δl2，判断残差的统计参数是否满足公式(3)，如果满足则完成分裂步骤；如果不满足，重复s024和s025过程，计算上次拟合残差的所有峰值点和高斯拟合，得到新的拟合残差，直到新的拟合残差的统计参数满足条件为止。
[0028]
滤波模块包括地形匹配定位模块、观测校正模块和定位预测模块，地形匹配定位
模块通过实时地形测量信息估计地形匹配定位概率分布，定位预测模块基于t-1时刻的定位概率分布对当前时刻的定位概率分布进行预测得到定位预测概率分布，然后观测校正模块对地形匹配定位概率分布和定位预测概率分布进行融合估计，得到观测校正后的地形匹配定位概率分布，然后将观测校正后的地形匹配定位概率分布输入到概率分布函数融合模块中进行概率分布函数融合。
[0029]
概率分布函数融合具体包括以下步骤：
[0030]
s031，首先计算地形匹配导航系统在第i个导航规划点wpi,i＞1的定位点和定位误差pi估计结果，计算公式如下：
[0031][0032]
s032，输入定位序列，该定位序列表示成其中：表示权重值，表示定位点，表示定位点的方差，k表示定位点的索引号；
[0033]
k表示当前时刻的导航定位点总数；
[0034]
s033，对定位序列进行融合判定，判定方法如下：首先，计算序列中任意两个定位点与的融合误差
[0035][0036]
然后，计算与的距离，并根据该距离是否在融合误差所确定的距离阈值范围内为判别依据对两个定位信息进行是否合并的判别，判别方法如下：
[0037][0038]
其中，γ表示放大系数。
[0039]
γ取值为1＜γ＜3。
[0040]
本发明的有益效果：本发明提供的一种匹配导航系统概率函数自主分裂与融合滤波方法，在地形匹配导航系统的初始运行阶段，通过定位概率分布函数的自主分裂降低伪波峰的干扰，该方法可提高滤波器的稳定性和收敛速度，确保系统从初始对准过程的不稳定性状态快速过渡到稳定跟踪状态。
附图说明
[0041]
图1是本发明一种匹配导航系统概率函数自主分裂与融合滤波方法的流程示意图。
具体实施方式
[0042]
下面结合附图对本发明作进一步描述，以下实施例仅用于更加清楚地说明本发明的技术方案，而不能以此来限制本发明的保护范围。
[0043]
本发明公开一种匹配导航系统概率函数自主分裂与融合滤波方法，包括以下步骤：
[0044]
步骤一，地形匹配导航系统提前录入地形匹配导航规划点序列wpi,i＝1,2,
…
i(见图1中流程3)，其中i代表地形匹配导航规划点总数，主导航系统将定位信息输入到地形匹配导航系统中，地形匹配导航系统根据索引号从地形匹配导航规划点序列中获取当前需要处理的地形匹配导航规划点，地形匹配导航系统根据定位信息判断载体是否到达地形匹配导航规划点。其中，定位信息包括导航t时间节点的主导航定位点d
t
和主导航定位误差de
t
(见图1中流程1)，地形匹配导航规划点序列wpi的索引号i初始化等于1(见图1中流程2)，当wpi与主导航定位点d
t
差值的绝对值小于误差∈(见图1中流程4)时，则判断载体到达第i个地形匹配导航规划点。
[0045]
步骤二，当主导航系统定位到载体到达地形匹配导航规划点的起点时，此时地形匹配导航规划点序列索引号i为1，地形测量模块获取实时地形测量信息(见图1中流程5)，实时地形测量信息、定位信息和地形匹配导航规划点构成的数据包(见图1中流程6)输入到地形匹配定位模块中进行处理(见图1中流程7)，地形匹配定位模块处理后获得匹配定位信息，匹配定位信息包括地形匹配定位似然函数、地形匹配定位点和地形匹配定位误差，将匹配定位信息输入到概率分布函数分裂模块进行概率分布函数分裂流程(见图1中流程8)，最后，地形匹配导航规划点的索引号i增加1，即i＝i+1。
[0046]
概率分布函数分裂模块中概率函数分裂流程具体包括以下步骤：
[0047]
步骤a，设置一个阈值分别表示一个二维序列的统计特征阈值，其中表示序列的方差统计、ε
μ
表示序列的均值、εr表示序列的相关系数；
[0048]
步骤b，从地形匹配定位模块获得地形匹配定位似然函数l，计算地形匹配定位似然函数l的所有峰值点其中n0表示峰值点的总数，表示峰值点的坐标值；表示峰值点的定位误差的协方差矩阵，通过地形的fisher信息量求解；
[0049]
步骤c，利用高斯核对似然函数l进行拟合，公式如下：
[0050]
其中，l1表示第1个地形匹配定位点的地形匹配定位似然函数的高斯核拟合函数；n表示高斯核的个数；表示高斯核函数的系数；表示均值为方差为的高斯核函数；
[0051]
在此基础上，计算拟合残差δl1：
[0052]
δl1＝l-l1ꢀꢀ
(2)
[0053]
然后判断残差δl1的统计参数是否满足公式(3)，如果满足则完成概率分布函数分裂步骤；如果不满足，则继续转步骤d；
[0054]
步骤d，在获得δl1之后，同s021，计算δl1的所有峰值点其中n1表示峰值点的总数，表示峰值点的总数，表示峰值点的坐标值，表示峰值点的定位误差的方差，通过地形的fisher信息量求解；
[0055]
步骤e，再次利用公式(1)，计算δl1的高斯拟合l2：
[0056][0057]
在此基础上，计算拟合残差：
[0058]
δl2＝δl
1-l2ꢀꢀ
(5)
[0059]
在此基础上，将公式(3)中δl1替换为δl2，判断残差的统计参数是否满足公式(3)，如果满足则完成分裂步骤；如果不满足，重复步骤d和步骤e过程，计算上次拟合残差的所有峰值点和高斯拟合，得到新的拟合残差，直到新的拟合残差的统计参数满足条件为止。
[0060]
步骤三，当主导航系统定位到载体到达地形匹配导航规划点的非起点时，此时地形匹配导航规划点序列索引号i大于1，实时地形测量模块获取实时地形测量信息，实时地形测量信息、定位信息和地形匹配导航规划点构成的数据包输入到滤波模块中进行处理(见图1中流程9)。滤波模块包括地形匹配定位模块、观测校正模块和定位预测模块，地形匹配定位模块通过实时地形测量信息估计地形匹配定位概率分布，定位预测模块基于t-1时刻的定位概率分布对当前时刻的定位概率分布进行预测得到定位预测概率分布，然后观测校正模块对地形匹配定位概率分布和定位预测概率分布进行融合估计，得到观测校正后的地形匹配定位概率分布，由于当前概率分布函数是经过概率分布函数分裂后的结果，其中包含多个子概率分布。然后将观测校正后的地形匹配定位概率分布输入到概率分布函数融合模块中，利用融合规则对子概率分布函数进行融合。
[0061]
处理后输入到概率分布函数融合模块进行概率分布函数融合(见图1中流程10)，最后，地形匹配导航规划点的索引号i增加1，即i＝i+1。概率分布函数融合具体包括以下步骤：
[0062]
步骤f，首先计算地形匹配导航系统在第i个导航规划点wpi,i＞1的定位点和定位误差pi估计结果，计算公式如下：
[0063][0064]
步骤g，输入定位序列，该定位序列表示成其中：表示权重值，表示定位点，表示定位点的方差，k表示定位点的索引号；k表示当前时刻的导航定位点总数；
[0065]
步骤h，对定位序列进行融合判定，判定方法如下：首先，计算序列中任意两个定位点与的融合误差
[0066]
然后，计算与的距离，并根据该距离是否在融合误差所确定的距离阈值范围内为判别依据对两个定位信息进行是否合并的判别，判别方法如下：
[0067][0068]
其中，γ表示放大系数，γ取值为1＜γ＜3。
[0069]
步骤四，重复步骤三，并判断i＞i是否成立，若成立则地形匹配导航程序运行结束，概率分布函数融合模块输出导航定位信息(见图1中流程11)，如不成立则继续执行步骤三，直到其成立为止。
[0070]
以上仅是本发明的优选实施方式，应当指出：对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。