[发明专利]一种基于自适应选权的多源融合定位方法

权利要求书

1.一种基于自适应选权的多源融合定位方法，其特征在于：依靠惯性导航因子推动位置变量节点拓展，误差变量节点对惯性导航因子进行纠正，添加视觉里程计因子和卫星导航因子通过直接与位置变量节点进行数据融合的方式纠正定位结果，步骤如下：

步骤1：基于卫星定位提供的初始位置，确定因子图的初始节点f_prior；

步骤2：在初始节点f_prior基础上生成位置变量节点x₁和误差变量节点c₁，通过惯性导航因子和误差因子进行拓展，误差变量节点对惯性导航因子进行纠正；

步骤3：当获得新的量测信息时，结合卫星导航因子和视觉里程计因子与位置变量节点进行数据融合，实现定位；其中卫星导航因子在数据融合前要经过一个权重计算的过程，完成自适应选权功能。

2.根据权利要求1所述的一种基于自适应选权的多源融合定位方法，其特征在于：步骤3的自适应选权具体步骤如下：

步骤3.1：检测卫星定位信号是否正常接入，若卫星定位信号正常接入，则以卫星定位信号提供的当前定位结果作为卫星导航因子输出的定位结果；若卫星信号定位信号未接入，卫星定位系统保留下卫星信号失效前最终的定位结果，并且卫星导航因子将始终以这个定位结果与位置变量节点进行数据融合；

步骤3.2：在与位置变量节点进行数据融合前，比较卫星导航因子输出的位置信息与位置变量节点提供的位置信息之间的差值，其公式如下：

其中，和为卫星导航因子输出的位置信息在东和北两个方向的坐标，和为当前位置变量节点输出的位置信息在东和北两个方向的坐标，ΔL_i为t_i时刻卫星导航因子提供的位置信息与位置变量节点提供的位置信息之差的绝对值；

步骤3.3：在卫星导航因子中加入一个变量t，t对相同的位置信息进行计数，相当于累计卫星定位信号失效的时间；加入一个变量n代表位置差值的阈值，参与自适应选择权重的过程，其中：

n＝2+t (2)

步骤3.4：最终的融合权重由以下公式决定；

最终，卫星导航因子输出的位置信息的融合权重为λ_i，而与之进行数据融合的位置变量节点x_i输出的位置信息的融合权重为1-λ_i。