[发明专利]一种匹配导航系统概率函数自主分裂与融合滤波方法

说明书

本发明公开了一种匹配导航系统概率函数自主分裂与融合滤波方法，包括以下步骤：S01，主导航系统定位信息输入到地形匹配导航系统中，地形匹配导航系统内提前录入初始输入信息；S02，当主导航系统定位到地形匹配导航规划点的起点，地形匹配导航系统启动滤波器初始化程序，完成概率分布函数分裂步骤，S03，地形匹配导航系统启动实时地形探测程序，并将数据包输入到地形匹配导航系统的滤波模块中，最后进行定位概率分布函数的融合；S04，地形匹配导航程序运行结束，输出导航定位信息。本发明提供的一种匹配导航系统概率函数自主分裂与融合滤波方法，能够在地形匹配导航系统的初始运行阶段，通过定位概率分布函数的自主分裂降低伪波峰的干扰。

技术领域

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

背景技术

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

发明内容

本发明要解决的技术问题是，克服现有技术的缺陷，提供一种匹配导航系统概率函数自主分裂与融合滤波方法，能够在地形匹配导航系统的初始运行阶段，通过定位概率分布函数的自主分裂降低伪波峰的干扰。

为解决上述技术问题，本发明采用的技术方案为：

一种匹配导航系统概率函数自主分裂与融合滤波方法，包括以下步骤：

S01，地形匹配导航系统提前录入地形匹配导航规划点序列Wp_i,i＝1,2,…I，其中I代表地形匹配导航规划点总数，主导航系统将定位信息输入到地形匹配导航系统中，地形匹配导航系统根据索引号从地形匹配导航规划点序列中获取当前需要处理的地形匹配导航规划点，地形匹配导航系统根据定位信息判断载体是否到达地形匹配导航规划点；

S02，当主导航系统定位到载体到达地形匹配导航规划点的起点时，此时地形匹配导航规划点序列索引号i为1，地形测量模块获取实时地形测量信息，实时地形测量信息、定位信息和地形匹配导航规划点构成的数据包输入到地形匹配定位模块中进行处理，处理后输入到概率分布函数分裂模块进行概率分布函数分裂，最后，地形匹配导航规划点的索引号i增加1，即i＝i+1；

S03，当主导航系统定位到载体到达地形匹配导航规划点的非起点时，此时地形匹配导航规划点序列索引号i大于1，实时地形测量模块获取实时地形测量信息，实时地形测量信息、定位信息和地形匹配导航规划点构成的数据包输入到滤波模块中进行处理，处理后输入到概率分布函数融合模块进行概率分布函数融合，最后，地形匹配导航规划点的索引号i增加1，即i＝i+1；

S04，重复S03，并判断i＞I是否成立，若成立则地形匹配导航程序运行结束，概率分布函数融合模块输出导航定位信息，如不成立则继续执行S03，直到其成立为止。

S01中，定位信息包括导航t时间节点的主导航定位点D_t和主导航定位误差De_t，地形匹配导航规划点序列Wp_i的索引号i初始化等于1，当Wp_i与主导航定位点D_t差值的绝对值小于误差∈时，则判断载体到达第i个地形匹配导航规划点。

S02中，地形匹配定位模块处理后获得匹配定位信息，匹配定位信息包括地形匹配定位似然函数、地形匹配定位点和地形匹配定位误差，将匹配定位信息输入到概率分布函数分裂模块进行概率分布函数分裂流程。