CN112577496B - 一种基于自适应选权的多源融合定位方法 - Google Patents

Abstract

本发明提供一种基于自适应选权的多源融合定位方法，方法步骤为：根据传感器初始数据获取初始节点；由初始节点生成初始位置变量节点和初始误差变量节点；惯性导航因子对位置变量节点进行拓展，误差因子则对误差变量节点进行拓展，并且由误差变量节点对惯性导航因子进行纠正；按照不同权重将卫星导航因子和视觉里程计因子的输出结果与位置变量节点中的定位结果进行数据融合。本发明将自适应选权的因子图模型应用于多源融合导航定位，用较为简便的方式实现了对传感器因子自适应选权的过程，能够通过动态调整权重实现卫星定位信息的故障排除和重新接入，并且在一定程度上减小定位误差。

Description

一种基于自适应选权的多源融合定位方法

技术领域

本发明涉及一种基于自适应选权的多源融合定位方法，属于多源融合导航定位领域。

背景技术

随着人工智能技术的发展，无人驾驶汽车成为未来汽车发展的趋势。对于无人车来说，解决定位问题是实现无人驾驶的基础。定位系统的精度直接影响了无人车的行驶安全和工作效率，依赖单一传感器完成导航定位功能的系统已经无法满足对定位精度的需求，此时便需要对多个传感器数据进行信息融合，多源融合技术由此诞生。多源融合技术可以利用各个传感器互补的特性，提高定位结果的精度，还能使定位系统能够适应各种环境，增强了系统的鲁棒性。

如今，卫星导航系统在种类繁多的导航定位系统中起着重要作用，当需要提供定位的绝对位置时，大部分车辆都将其作为首选目标。但卫星导航系统也存在固有的缺陷，系统需要在接收机收到卫星信号后对其进行解算才能得到定位结果，定位过程中系统对可见卫星的数量和卫星信号的强度都有一定的要求。且卫星信号难以穿透钢筋混凝土结构的建筑，导致卫星信号在城市峡谷、隧道、立交桥等半封闭环境中的信号衰减非常严重，有时还会出现接收不到信号的情况。当车辆在这些场景下行驶时，定位精度会受到极大的负面影响。因此，就需要对卫星定位信息的权重根据当前卫星性能进行调整，完成卫星定位信息的自主断开和接入，以减小定位误差。

在导航定位领域中传统的多源融合方法以卡尔曼滤波系列算法为主，其中包括为了减小线性化误差发展出的扩展卡尔曼滤波算法，和为了实现即插即用功能发展出的联邦卡尔曼滤波算法。本文使用的因子图算法也是多源融合算法中的一种，相比于卡尔曼滤波算法，因子图算法的线性化误差更小，并且结构变化更方便，易于实现即插即用功能。

发明内容

本发明针对传统权重选择方法结构复杂且计算量大的问题，提出了一种因子图模型下基于自适应选权的多源融合定位方法。该方法计算了因子图的位置变量节点中的定位结果与卫星导航因子的定位结果之间的位置差值，并利用位置差值构建权函数来完成自适应调整融合权重的作用。这种自适应选权方法能够完成卫星定位信息的故障排除和自主接入，还可以降低异常量的影响，在一定程度上提高精度。

本发明的目的是这样实现的：依靠惯性导航因子推动位置变量节点拓展，误差变量节点对惯性导航因子进行纠正，添加视觉里程计因子和卫星导航因子通过直接与位置变量节点进行数据融合的方式纠正定位结果，步骤如下：

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

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

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

本发明还包括这样一些特征：

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。

与现有技术相比，本发明的有益效果是：在自适应选权问题中，目前应用于因子图的自适应选权方法主要有抗差估计方法和可观测度分析方法，现有方法的主要问题是需要存储大量数据并且计算量大。本发明提出一种利用因子图的位置变量节点中的定位结果与卫星导航因子的定位结果之间的位置差值构建权函数完成自适应选权的方法，该方法实现起来更为简单，能够减轻系统存储数据的负担并且减小计算量。

附图说明

图1是基于自适应选权的多源融合定位方法的模型结构图，图中x_i和c_i分别表示位置变量和误差变量这两类变量节点，f_GNSS、f_VO、f_IMU和f_bias分别表示卫星导航因子、视觉里程计因子、惯性导航因子和误差因子这四类因子节点。

图2是基于自适应选权的多源融合定位方法的权重选择部分流程图。

图3是实验结果中的轨迹对比图。

图4是实验结果中的绝对轨迹误差对比图。

具体实施方式

下面结合附图与具体实施方式对本发明作进一步详细描述。

本发明是一种因子图下基于自适应选权的多源融合定位方法，其特征是以位置差值作为判断标准，构建权重选择函数。所述方法如下：

(1)建立因子图数据融合模型：此方法中采用的因子图是基于贝叶斯网络的概率图模型，因子图通过构建一定时间区间内系统的图模型，在当前导航测量和系统解算的导航状态量之间建立起联系，是基于后验估计理论的数据融合方式。

(2)变量节点及因子节点拓展：因子图内包含有变量节点与因子节点这两类节点，一般来说将待估计的变量表示为“变量节点”，这些变量之间的约束则被表示为“因子节点”，变量节点和变量节点之间用因子节点连接。本方法中，多源融合定位系统的初始量测信息由多种不同传感器负责，不同种类的传感器作为因子节点并作用于变量节点，通过变量节点和因子节点不断拓展,建立基于因子图的多源融合系统。

(3)本方法中，因子节点作用于变量节点时，两者的定位结果按照一定权重进行融合，该权重能够按照当前状态进行调整，具备自适应选权功能。

步骤(1)具体为：假设函数因式分解后的结果可以由函数来表示，其中u₁,u₂,…,u_n代表系统中各个变量，g(u₁,u₂,…,u_n)代表系统整体函数，f_j(u_j)则表示因式分解后与各个变量相关的子函数。

则对应该函数的因子图G＝(F,U,ε)可以分成三个部分，变量节点u_j∈U表示待估计的变量，U为变量节点的集合，因子节点f_i∈F表示一个局部函数，F为因子节点的集合，只有当u_j是f_i中的变量时，才会有一条边e_ij∈ε将因子节点和变量节点连接起来，ε代表模型中连接着因子节点和变量节点的边的集合。

如果用x表示位置、姿态等导航状态量，用c表示惯性传感器的标定参数，则t_i时刻的导航状态量和误差变量就分别可以用x_i和c_i来表示。

定义集合X_k为截止到t_k时刻的所有导航状态量，集合C_k为所有误差变量，它们可以表示为:

到t_k时刻为止所有的变量集合可以表示为:

U_k＝{X_k,C_k} (5)

概率密度函数可以表示为:

p(U_k∣Z_k) (6)

式中，Z_k表示截止至t_k时刻的所有量测信息。则系统的最大后验概率可以表示为：

通过因式分解，因子图模型中的每一个因子节点都可以表示最大后验概率p(U_k∣Z_k)中的独立一项，因此可以表示为：

式中，代表了t_i时刻的变量节点。对于高斯系统来说，在因子图中的每一个因子节点f_i都与误差方程/>有关，则有：

其中d(.)表示代价函数，指的是平方马氏距离，Σ是量测噪声协方差矩阵。因此，对MAP的估计可以等效为求解非线性最小二乘方/>的最小值。对于系统的量测模型来说，其因子表达式为：

其中，h_i(.)代表传感器相对于系统状态量的量测模型，z_i表示实际量测值。估计最大后验概率其实就是对当前构建的图模型进行最优化。

作为本发明的一种改进，所述步骤(2)具体如下。该方法主要依靠惯性导航因子推动位置变量节点拓展，误差变量节点对惯性导航因子进行纠正，还添加了视觉里程计因子和卫星导航因子通过直接与位置变量节点进行数据融合的方式纠正定位结果。其建立步骤如下：

步骤2.1：基于卫星定位提供的初始位置，确定因子图的初始节点f_prior；其中包含初始位置以及初始误差；

步骤2.2：在初始节点f_prior基础上生成位置变量节点x₁和误差变量节点c₁，位置变量节点中包含位置信息，误差变量节点则用于表示惯性导航系统在工作过程中累积的误差，用于在惯性导航因子节点的运算过程中进行纠正；通过惯性导航因子和误差因子进行拓展，误差变量节点对惯性导航因子进行纠正；

步骤2.3：生成惯性导航因子，一个惯性导航因子的作用实际上是惯性导航系统工作一次，在位置变量节点x₁的基础上进行推算，得到位置变量节点x₂，误差变量节点c₁参与计算过程；当从其他传感器获得新的量测信息时，引入卫星导航因子和视觉里程计因子与位置变量节点进行数据融合。其中卫星导航因子在数据融合前还需要经过一个权重计算的过程，完成自适应选权功能。

步骤2.4、生成视觉里程计因子，即视觉里程计工作一次，由视觉里程计输出的定位结果作用于位置变量节点x₂。与视觉里程计因子输出的定位结果融合前，位置变量节点x₂中保留的是由上一个惯性导航因子推算得到的定位结果，两个定位结果各自以0.5的权重进行加权融合，得出的最终结果对位置变量节点x₂进行更新；

步骤2.5、生成误差因子，由误差变量节点c₁产生误差变量节点c₂，生成惯性导航因子，在位置变量节点x₂的基础上进行推算，得到位置变量节点x₃，误差变量节点c₂参与计算过程；

步骤2.6、生成卫星导航因子，即卫星导航系统工作一次，由卫星导航系统输出的定位结果作用于位置变量节点x₃。与卫星导航因子输出的定位结果融合前，位置变量节点x₃中保留的是由上一个惯性导航因子推算得到的定位结果，两个定位结果按照自适应选权的结果进行加权融合，得出的最终结果对位置变量节点x₃进行更新。

以上步骤中，惯性导航因子构成了模型的骨架，推动位置变量节点不断随时间拓展，并且每个惯性导航因子都有对应的误差变量节点对其进行纠正。而视觉里程计因子和卫星导航因子则是以一定步长接入因子图模型，该步长由两个传感器与惯性导航系统工作频率的比值决定。在每个连接了因子节点f_GNSS或f_VO的位置变量节点之间，存在多个仅由惯性导航因子得到的位置变量节点。

作为本发明的一种改进，所述步骤(3)具体如下，在因子图拓展的步骤中，是通过以下步骤自适应选取权重：

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

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

其中，和/>为卫星导航因子输出的位置信息在东和北两个方向的坐标，/>和为当前位置变量节点输出的位置信息在东和北两个方向的坐标，ΔL_i为两者之间的位置差值；

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

n＝2+t (12)

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

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

其中，ΔL_i为t_i时刻卫星导航因子提供的位置信息与位置变量节点提供的位置信息之差的绝对值。n则为位置差值的阈值，表示为一个定值与随时间累积的值之和。权函数λ_i针对ΔL_i的取值不同调整卫星导航因子的权重,在能正常接收到卫星信号时，由于权重一直在调整，ΔL_i不会超过n。但当卫星信号丢失，卫星导航因子保留下最后状态，此时载具已经行驶出一段距离，由惯性导航系统和视觉里程计组成的组合导航系统依然在更新位置信息。当经过卫星工作的一个周期，再次进行融合时，卫星导航因子输出的定位结果依旧保持之前的状态，就会因为差值过大而被赋予0的权值，此时卫星定位信号断开。

在卫星定位信号断开后，惯性导航系统和视觉里程计继续工作，直到无人车离开卫星失效区域重新获得卫星信号。由于大多数场景都是卫星短时失效环境，此时定位并没有偏离过大，当卫星信号重新接入时，其定位仍然在真实轨迹附近，只要满足ΔL_i≤n，定位误差依旧可以依靠卫星纠正回来。

为评价本发明有效性，利用KITTI数据集进行实验验证。

实验使用真实路况下采集的KITTI数据集进行实验，选取其中的两段路径，在总计70秒的时间里设置三个时间段的卫星信号丢失，分别是第10秒到第15秒的5秒间隔，第25秒到第35秒的10秒间隔和第45秒到第65秒的20秒间隔，中间各留了10秒的时间观察能够正常接收到卫星信号时误差的恢复情况。本方法与传统方法比较的误差统计结果，如表1所示：

表1实验结果误差对比

实验结果	本文方法	传统因子图
			平均值/m	3.94	4.49
最大值/m	14.84	17.04
			RMSE/m	4.59	5.02

由表1可见，本发明在精度上优于传统的因子图方法。而实验还验证了依靠位置差值进行权重调整方法的可行性，由图3和图4展示的结果证明本方法的确能够在卫星信号失效时自主排除卫星信号，并且能够在卫星信号恢复时自主接入，迅速减小误差，实现了自适应调整权重的功能，有着一定的应用价值。

综上，本发明提供了一种因子图模型下基于自适应选权的多源融合定位方法。方法步骤为：根据传感器初始数据获取初始节点；由初始节点生成初始位置变量节点和初始误差变量节点；惯性导航因子对位置变量节点进行拓展，误差因子则对误差变量节点进行拓展，并且由误差变量节点对惯性导航因子进行纠正；按照不同权重将卫星导航因子和视觉里程计因子的输出结果与位置变量节点中的定位结果进行数据融合。本发明将自适应选权的因子图模型应用于多源融合导航定位，用较为简便的方式实现了对传感器因子自适应选权的过程，能够通过动态调整权重实现卫星定位信息的故障排除和重新接入，并且在一定程度上减小定位误差。

Claims (1)

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

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

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

步骤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。