CN109901108A - 一种基于后验线性化置信传播的编队无人机协同定位方法 - Google Patents

Abstract

本发明公开了一种基于后验线性化置信传播的编队无人机协同定位方法，属于定位导航技术领域。该方法包括以下步骤：（1）初始化所有无人机的初始位置值及协方差阵；（2）根据t‑1时刻的无人机位置值采用后验线性化方法计算得到t时刻线性化量测模型参数；（3）在后验线性化量测模型上采用置信传播算法进行M次迭代计算；（4）经过M次迭代计算后，根据无人机边缘后验位置概率分布求得无人机的后验位置估计值，为了得到收敛解回到步骤（2）进行J次迭代计算。本发明可显著提高编队中僚机定位精度，且适用于卫星拒止等复杂飞行环境，适合工程实际应用。

Description

一种基于后验线性化置信传播的编队无人机协同定位方法

技术领域

本发明公开了一种基于后验线性化置信传播的编队无人机协同定位方法，属于群体无人机编队协同定位导航技术领域。

背景技术

无人机集群编队技术在战场侦察、通信中继、3D重建等领域具备极大的潜在应用价值。由于无人机集群飞行机间间距小、动态高，高精度且高可靠的定位与导航信息是无人机集群高效执行其他任务的重要基础保障。无人机编队协同导航定位使得编队中无人机在各自导航信息的基础上，通过相互观测构成的约束关系，综合其他运动体的导航信息进行数据融合，以实现“1+1>2”的导航系统性能提升。

现有的无人机群体协同导航系统多采用惯性/卫星组合导航系统实现，然而在卫星信号缺失飞行环境下，以卫星导航为核心的无人机协同导航系统将无法满足应用需求。此外，由于无人机在编队飞行过程的量测模型多为非线性，对量测模型的线性化方法将直接影响到导航精度，而目前主流的线性化方法多采用粒子逼近，导致计算量庞大且对通信带宽需求大，无法满足编队过程中对导航定位信息的精度及实时性需求。

利用后验线性化方法对量测模型进行处理，在此基础上利用置信传播算法进行编队无人机之间的协同定位，极大降低了导航定位解算过程的计算量及对通信带宽的需求，将具有突出的应用价值。

发明内容

为了克服编队无人机在卫星信号缺失情况下的协同定位问题及多无人机进行协同定位结算时计算量大、对通信带宽需求高的问题，本发明提出了一种基于后验线性化置信传播的编队无人机协同定位方法，首先采用置信传播算法作为无人机协同定位框架，然后采用统计线性回归方法设计了量测模型后验线性化模型，进而将该模型应用于无人机置信传播协同定位解算，满足了编队无人机的高精度定位需求。

本发明为解决其技术问题采用如下技术方案：

一种基于后验线性化置信传播的编队无人机协同定位算法，包括以下步骤：

(1)初始化所有无人机的初始位置值及协方差阵其中：分别为第i架无人机和第j架无人机0时刻在地心地固坐标系下的三维地理位置，分别为待估计位置量的协方差矩阵；

(2)根据t-1时刻的无人机位置信息协方差阵采用后验线性化方法计算得到t时刻线性化量测模型参数其中均为线性化系数矩阵，为常数项，Ω_i,j为服从零均值高斯分布的噪声协方差矩阵；

(3)根据参数计算相邻无人机之间的协同信息μ_i,j，从而在后验线性化量测模型上进行M次置信传播算法迭代计算；

(4)经过M次迭代计算后，根据无人机边缘后验位置概率分布求得无人机的后验位置估计值，回到步骤(2)进行J次迭代计算得到收敛解，即获得无人机在t时刻的后验位置最小均方误差估值

步骤(2)中所述量测模型的具体形式为：

其中，为无人机i和无人机j之间的相对观测量，表示和之间的函数关系，为第i架无人机在地心地固坐标系下的三维地理位置，η_i,j为零均值、协方差阵为R_i,j的高斯噪声，(i,j)∈K_i，K_i为第i架无人机测距范围内的相邻无人机集合；

将量测模型的非线性部分线性化为带有加性高斯噪声的仿射函数：

其中，为相邻无人机(i,j)之间的仿射函数，均为线性化系数矩阵，为常数项，为服从零均值高斯分布的噪声，其协方差矩阵为Ω_i,j。

步骤(2)中所述线性化量测模型参数的计算步骤如下：

①根据无人机位置均值及分布p(·)的协方差P选取m个sigma-points χ₁,χ₂,…χ_m及权重ω₁,ω₂,…ω_m；

②对χ₁,χ₂,…χ_m进行变换Z_j＝h(χ_j)，j＝1,2,…,m；

③计算：

其中：为Z_j的加权平均值，Ψ为χ_j,Z_j的加权协方差阵，Φ为Z_j的方差阵，ω_j为第j个权重因子，χ_j为第j个sigma-points，Z_j为第j个χ_j进行变换后得到的值；

④求得参数

A_i,j＝Ψ^TP^-1,

Ω_i,j＝Φ-A_i,jP(A_i,j)^T

其中，P-1为协方差P的逆矩阵。

步骤(3)中所述相邻无人机之间的协同信息μ_i,j为：

其中l_i,j为编队无人机i,j之间的相对观测信息的似然函数，为无人机i在t时刻的导航位置量的先验概率密度函数，且服从均值为协方差阵为P_i ^t的高斯分布 为t时刻无人机p流入到无人机i的协同信息，为零均值高斯白噪声，N表示高斯函数。。

步骤(3)中所述进行M次置信传播算法迭代计算的具体过程如下：

①令其中为无人机i在t-1时刻的位置期望值，P_i ^t-1为无人机i在t-1时刻的位置协方差阵，进行|n(i)\{j}|次卡尔曼滤波更新：

②采用后验线性化量测模型计算得到无人机i流入无人机j的信息μ_i,j，回到步骤①进行M次迭代计算。

步骤(4)中所述无人机边缘后验位置概率分布为：

其中为无人机i在t时刻的导航位置量的先验概率密度函数，且服从均值为协方差阵为P_i ^t的高斯分布 为t时刻无人机p流入到无人机i的协同信息。

本发明的有益效果如下：

本发明基于后验线性化置信传播的编队无人机协同定位方法，利用后验线性化方法对编队无人机协同定位量测模型进行线性化处理，在此基础上利用置信传播算法进行编队无人机之间的协同定位，极大降低了导航定位解算过程的计算量及对通信带宽的需求，且在整个过程编队中僚机完全不依赖卫星导航信号即可获得高精度定位信息。该方法满足了编队无人机对高精度、高可靠定位信息的需求，实验结果表明，本发明使得僚机在X、Y、Z(地心地固坐标系)三个方向的定位精度相对于无协同情形提高了8.9、13.9、17.3倍。

附图说明

图1为编队无人机初始位置分布图。

图2为本发明协同定位算法的流程图。

图3为本发明的执行过程中的因子图。

图4(a)采用本发明得到的编队中长机和僚机在x轴方向上的定位误差曲线。

图4(b)采用本发明得到的编队中长机和僚机在y轴方向上的定位误差曲线。

图4(c)采用本发明得到的编队中长机和僚机在z轴方向上的定位误差曲线。

图5为僚机采用无协同定位方法进行定位得到的在X、Y、Z三个方向(地心地固坐标系)上的位置误差曲线。

图6为僚机采用置信传播(BP)协同定位方法进行定位得到的在X、Y、Z三个方向(地心地固坐标系)上的位置误差曲线。

图7为僚机采用本发明方法进行定位得到的在X、Y、Z三个方向(地心地固坐标系)上的位置误差曲线。

具体实施方式

下面结合附图对发明的技术方案进行详细说明。

图1为本发明方法中编队无人机在仿真环境中的初始位置分布图，包括三架长机和十架僚机，僚机跟随长机飞行。按照图2所示的算法执行流程图对每架飞进行相应的定位解算。

在编队无人机组中，在长机上搭载高精度导航传感器(如差分GPS、高精度IMU等)以获取高精度的实时位置信息。僚机则可通过机间测距传感器(UWB)获得与长机/其他僚机机间测距信息并进行信息交换，推断得到自身位置信息。该推断问题可推广到贝叶斯框架中，采用概率图模型求解如图3所示。在此基础上，编队中无人机基于消息传递进行协同定位便可获得与长机精度相当的位置信息，整个编队过程中，僚机完全不依赖卫星导航信息。

图3中为第i架无人机在t-1时刻的位置信息；为第i架无人机根据0～t-1时刻位置信息经过自身预测得到的t时刻位置信息；表示t时刻相邻无人机i和j之间的似然函数；为t时刻无人机i流入到无人机j的协同信息，包括无人机i的自身预测信息和相对量测信息。

该方法主要包括以下步骤：

(1)初始化所有无人机的初始位置值及协方差阵其中：分别为第i架无人机和第j架无人机0时刻在地心地固坐标系(ECEF)下的三维地理位置，分别为待估计位置量的协方差矩阵。

(2)根据t-1时刻的无人机位置信息协方差阵采用后验线性化方法计算得到t时刻线性化量测模型参数量测模型的具体形式为：

其中，为无人机i和无人机j之间的相对观测量，表示和之间的函数关系，分别为第i,j架无人机在地心地固坐标系(ECEF)下的三维地理位置，η_i,j为零均值、协方差阵为R_i,j的高斯噪声，(i,j)∈K_i，K_i为第i架无人机测距范围内的相邻无人机集合。将量测模型的非线性部分线性化为带有加性高斯噪声的仿射函数：

其中，均为线性化系数矩阵，为常数项，为服从零均值高斯分布的噪声，其协方差矩阵为Ω_i,j。

参数的计算步骤如下：

①根据无人机位置均值及分布p(·)的协方差P选取m个sigma-points χ₁,χ₂,…χ_m及权重ω₁,ω₂,…ω_m；

②对χ₁,χ₂,…χ_m进行变换Z_j＝h(χ_j)，j＝1,2,…,m；

③计算：

其中：为Z_j的加权平均值，Ψ为χ_j,Z_j的加权协方差阵，Φ为Z_j的方差阵，ω_j为第j个权重因子，χ_j为第j个sigma-points，Z_j为第j个χ_j进行变换后得到的值；

④求得参数

A_i,j＝Ψ^TP^-1,

Ω_i,j＝Φ-A_i,jP(A_i,j)^T

其中，P^-1为协方差P的逆矩阵。

(3)根据参数计算相邻无人机之间的协同信息μ_i,j，从而在后验线性化量测模型上进行M次置信传播算法迭代计算；协同信息μ_i,j为：

其中l_i,j为编队无人机i,j之间的相对观测信息的似然函数，为无人机i在t时刻的导航位置量的先验概率密度函数，且服从均值为协方差阵为P_i ^t的高斯分布 为t时刻无人机p流入到无人机i的协同信息，为无人机i和无人机j之间的相对观测量，为零均值高斯白噪声，为无人机i的期望位置，P_i ^t为无人机i，N表示高斯函数。进行M次置信传播算法迭代计算的具体过程如下：

①令其中为无人机i在t-1时刻的位置期望值，P_i ^t-1为无人机i在t-1时刻的位置协方差阵，进行|n(i)\{j}|次卡尔曼滤波更新：

②采用后验线性化量测模型计算得到无人机i流入无人机j的信息μ_i,j。为了获得更加精确的后验线性化量测模型，回到步骤①进行M次迭代计算；

(4)经过M次迭代计算后，根据无人机边缘后验位置概率分布求得无人机的后验位置估计值，为了得到收敛解回到步骤(2)进行J次迭代计算，即可获得无人机在t时刻的后验位置最小均方误差估值

为了验证本发明提出的方法的性能，在仿真环境下进行了无人机编队协同导航仿真。编队系统由3架长机和10架僚机组成，长机进行惯导/差分GPS组合导航解算，可获得高精度导航位置信息。僚机搭载低成本惯性/GPS传感器，在飞行过程中采用本文算法解算得到的位置信息和惯导进行组合导航。所有无人机均搭载UWB设备，测距范围设定为75m。编队飞行时长为1小时，所有僚机跟随长机飞行，其起始位置分布如图1所示。图4对比了采用本发明方法进行协同定位得到的编队中长机和僚机的定位误差；图5、图6、图7分别对比了3种方法的编队无人机协同定位误差，对照组分别为：

①无协同：无人机之间无协同信息交换；②BP协同：基于置信传播的协同定位算法；③本发明方法：采用本发明提出的基于后验线性化置信传播的协同定位方法。

由图4(a)、(b)、(c)可知，僚机在整个编队飞行过程完全不依赖卫星导航信号的情况下，仅采用后验线性化置信传播算法进行协同定位，其导航定位误差精度完全可以与长机定位误差精度相媲美。从图5-7可以看出，采用本发明方法或BP算法进行协同定位时，僚机的定位误差精度要远远高于无协同的情形。相对于BP算法，本发明方法解算得到的僚机定位误差精度更接近于编队过程中长机的高精度定位解，且在X、Y、Z三个方向上的定位精度相较于BP算法分别提升了2.9、1.05、2.04倍。此外，本发明和BP算法在Matlab仿真过程中的运行时间分别为4133秒、23184秒，算法效率提升了5.6倍。可见本发明提出的方法实现了运算量和精度的平衡，适用于编队无人机协同定位算法在工程中的实际运用。

Claims (6)

1.一种基于后验线性化置信传播的编队无人机协同定位方法，其特征在于，包括以下步骤：

(1)初始化所有无人机的初始位置值及协方差阵其中：分别为第i架无人机和第j架无人机0时刻在地心地固坐标系下的三维地理位置，分别为待估计位置量的协方差矩阵；

(2)根据t-1时刻的无人机位置信息协方差阵采用后验线性化方法计算得到t时刻线性化量测模型参数其中均为线性化系数矩阵，为常数项，Ω_i,j为服从零均值高斯分布的噪声协方差矩阵；

(3)根据参数计算相邻无人机之间的协同信息μ_i,j，从而在后验线性化量测模型上进行M次置信传播算法迭代计算；

(4)经过M次迭代计算后，根据无人机边缘后验位置概率分布求得无人机的后验位置估计值，回到步骤(2)进行J次迭代计算得到收敛解，即获得无人机在t时刻的后验位置最小均方误差估值

2.根据权利要求1所述的一种基于后验线性化置信传播的编队无人机协同定位方法，其特征在于，步骤(2)中所述量测模型的具体形式为：

其中，为无人机i和无人机j之间的相对观测量，表示和之间的函数关系，为第i架无人机在地心地固坐标系下的三维地理位置，η_i,j为零均值、协方差阵为R_i,j的高斯噪声，i,j∈K_i，K_i为第i架无人机测距范围内的相邻无人机集合；

将量测模型的非线性部分线性化为带有加性高斯噪声的仿射函数：

其中，为相邻无人机(i,j)之间的仿射函数，均为线性化系数矩阵，为常数项，为服从零均值高斯分布的噪声，其协方差矩阵为Ω_i,j。

3.根据权利要求1所述的一种基于后验线性化置信传播的编队无人机协同定位方法，其特征在于，步骤(2)中所述线性化量测模型参数的计算步骤如下：

①根据无人机位置均值及分布p(·)的协方差P选取m个sigma-points χ₁,χ₂,…χ_m及权重ω₁,ω₂,…ω_m；

②对χ₁,χ₂,…χ_m进行变换Z_j＝h(χ_j)，j＝1,2,…,m；

③计算：

其中：为Z_j的加权平均值，Ψ为χ_j,Z_j的加权协方差阵，Φ为Z_j的方差阵，ω_j为第j个权重因子，χ_j为第j个sigma-points，Z_j为第j个χ_j进行变换后得到的值；

④求得参数

A_i,j＝Ψ^TP^-1,

Ω_i,j＝Φ-A_i,jP(A_i,j)^T

其中，P^-1为协方差P的逆矩阵。

4.根据权利要求1所述的一种基于后验线性化置信传播的编队无人机协同定位方法，其特征在于，步骤(3)中所述相邻无人机之间的协同信息μ_i,j为：

其中l_i,j为编队无人机i,j之间的相对观测信息的似然函数，为无人机i在t时刻的导航位置量的先验概率密度函数，且服从均值为协方差阵为P_i ^t的高斯分布 为t时刻无人机p流入到无人机i的协同信息，为零均值高斯白噪声，N表示高斯函数。

5.根据权利要求1所述的一种基于后验线性化置信传播的编队无人机协同定位方法，其特征在于，步骤(3)中所述进行M次置信传播算法迭代计算的具体过程如下：

①令其中为无人机i在t-1时刻的位置期望值，

P_i ^t-1为无人机i在t-1时刻的位置协方差阵，进行|n(i)\{j}|次卡尔曼滤波更新：

②采用后验线性化量测模型计算得到无人机i流入无人机j的信息μ_i,j，回到步骤①进行M次迭代计算。

6.根据权利要求1所述的一种基于后验线性化置信传播的编队无人机协同定位方法，其特征在于，步骤(4)中所述无人机边缘后验位置概率分布为：

其中为无人机i在t时刻的导航位置量的先验概率密度函数，且服从均值为协方差阵为P_i ^t的高斯分布 为t时刻无人机p流入到无人机i的协同信息。