CN112346104B - 一种无人机信息融合定位方法 - Google Patents

Abstract

本发明公开了一种无人机信息融合定位方法，用于解决现有无人机定位在非晴朗气候条件下，红外线波的侦测能力会大幅衰減，存在检测概率低、虚警率高的问题，利用SINS/GPS组合导航***解算出无人机在地面坐标系中的初步位置坐标；在多机交叉定位中解算出无人机位置坐标，结合SINS/GPS组合导航***输出的无人机位置坐标分别作差计算，再经过Kalman滤波，估算出SINS/GPS组合导航***的误差并对其进行校正输出经过校正后目标无人机的位置坐标，可以使多个无人机在松组合导航***和多机无源交叉测向定位下进行信息融合，从而在复杂气候环境中获得更好的定位效果。

Description

一种无人机信息融合定位方法

技术领域

本发明涉及无人机定位技术领域，具体为一种无人机信息融合定位方法。

背景技术

无人机作为未来战场上重要的侦察工具和攻击武器受到越来越多的关注。无人机在执行任务时，其机载光电设备对无人机的定位精度有较高要求。目前对于无人机编队的精准定位已有一些解决方案；

专利CN2020100538363提出一种基于激光雷达与定位向量匹配多无人机协同定位方法，但激光雷达作为有源器件受天候和大气的影响，若在大雨、下雪、浓雾等非晴朗气候条件下，红外线波的侦测能力会大幅衰減，此时存在检测概率低、虚警率高等问题。而基于捷联式惯性导航***(SINS)和全球定位***(GPS)的组合导航***可对无人机观测数据进行初步定位，再结合无源测向交叉定位技术进行信息融合，最后解算出精准无人机位置信息，这种多源信息融合的定位方法能够使无人机编队在恶劣天气下工作，定位盲区更小，更加适合于对无人机进行定位。

发明内容

本发明的目的就在于为了解决现有无人机定位在非晴朗气候条件下，红外线波的侦测能力会大幅衰減，存在检测概率低、虚警率高的问题，而提出一种无人机信息融合定位方法，本发明可以使多个无人机在松组合导航***和多机无源交叉测向定位下进行信息融合，从而在复杂气候环境中获得更好的定位效果。

本发明的目的可以通过以下技术方案实现：一种无人机信息融合定位方法，该方法包括以下步骤：

步骤一：通过测向交叉定位法和UAV视觉传感器***测得各无人机的相对位置信息；无人机为无人机；

步骤二：通过SINS/GPS组合导航***解算出无人机在地面坐标系中的初步位置坐标；具体为：在其中一时刻，通过SINS/GPS松组合导航***实时地得到目标无人机的初步位置信息及姿态角信息(θ₂γ₂ψ)；

步骤三：在多机交叉定位中解算出无人机位置坐标，结合SINS/GPS组合导航***输出的无人机位置坐标分别作差计算，把得到的差值作为量测值，再经过Kalman滤波，估算出SINS/GPS组合导航***的误差并对其进行校正输出经过校正后目标无人机的位置坐标；其中，多机交叉定位的具体表现为：当有多架无人机时，各无人机的定位除组合导航自定位外，通过其余无人机实现，即将其余无人机作为测向基站，分别对某架目标无人机进行测向得到一组俯仰角和方位角；

当多架无人机数量等于二时，则通过双机三维定位算法进行分析，具体分析过程为：以A(x₁,y₁,z₁)、B(x₂,y₂,z₂)两无人机为测向基站，分别对无人机目标C(x,y,z)进行测向，分别得到一组俯仰角和方位角，即为(α₁,β₁)和(α₂,β₂)，两者相互组合得到四组测量子集(α₁,β₁,α₂)、(α₁,β₁,β₂)、(α₂,β₂,α₁)和(α₂,β₂,β₁)，其中α_i(i＝1,2)为方位角，设定X轴为正北方向，β_i(i＝1,2)为俯仰角，由方位角α₁、α₂和俯仰角β₁、β₂的三角关系可得：

将上述方程表示为矩阵形式：

HX＝Z

其中

X＝(x,y,z)^T

由此可得目标无人机坐标为：

X＝H^-1Z

选取上述方程(1)中3组进行组合解出目标无人机的位置。

优选的，所述相对位置信息包括相对方位角和俯仰角；初步位置坐标包括经度L、纬度B和高度H。

优选的，当多架无人机数量大于二时时，对于每个无人机均可得到N组方位角和俯仰角的观测值，通过两两交叉定位进行定位解算，选择其中一个无人机为测向主站，坐标为A(x₀,y₀,z₀)；其它无人机作为辅站，坐标分别为A(x_i,y_i,z_i)(i＝1,2,...,N-1)，然后通过主站与辅站两两组合对目标进行定位，由此得到N-1个目标估计位置，分别为C(x_i,y_i,z_i)(i＝1,2,...,N-1)，通过聚类法该组数据进行处理，从而得到目标位置坐标的估计值，具体步骤为：

S1：设每个样本自成一类，即建立N-1类：标号(0)表示聚类开始运算前的状态，样本即一个目标位置坐标；

S2：计算各类间的距离，得到一个N-1维的方阵D⁽⁰⁾，初始时即为各样本间的距离；

S3：设前一步聚类运算中已求得距离矩阵D⁽ⁿ⁾，n为逐次聚类的次数，则求D⁽ⁿ⁾中的最小元素，设其为和/>两类间的距离，则将/>与/>合并为一类/>由此建立新的分类：/>

S4：计算合并后新类别间的距离，即计算与其他未发生合并的/>间的距离；通过重心法作为距离计算准则，得D⁽ⁿ⁺¹⁾；重复计算及合并，直到距离矩阵中的最小元素超过定位精度上限或所有元素都属于同一类则输出包含元素最多的类的重心作为最终输出结果，即目标位置坐标的估计值。

优选的，所述Kalman滤波的具体过程为：设随机线性离散***的状态方程和量测方程分别是：

式中：T为迭代周期，Φ_k,k-1是k-1到k时刻的一步转移矩阵；Γ_k-1是***噪声矩阵；H_k是量测矩阵；V_k是量测噪声序列；W_k-1为k-1时刻的***噪声矩阵；

设定过程噪声和观测噪声有如下统计特性：

式中Q_k为***噪声序列的方差矩阵；R_k为量测噪声方差矩阵；

线性离散***的基本卡尔曼滤波(KF)方程；状态一步预测方程：

状态估计方程：

最优滤波增益方程：

最后得到预测均方误差方程：

上述是离散KF的基本方程，给出初始值和P₀，再根据k时刻的量测值Z_k，递推出k时刻的状态估计值/>

与现有技术相比，本发明的有益效果是：基于测向交叉定位法利用UAV视觉传感器***测得各无人机的相对位置信息，利用SINS/GPS组合导航***解算出无人机在地面坐标系中的初步位置坐标；在多机交叉定位中解算出无人机位置坐标，结合SINS/GPS组合导航***输出的无人机位置坐标分别作差计算，把得到的差值作为量测值，再经过Kalman滤波，估算出SINS/GPS组合导航***的误差并对其进行校正输出经过校正后目标无人机的位置坐标，可以使多个无人机在松组合导航***和多机无源交叉测向定位下进行信息融合，从而在复杂气候环境中获得更好的定位效果。

附图说明

为了便于本领域技术人员理解，下面结合附图对本发明作进一步的说明。

图1为本发明的无人机定位示意图；

图2为本发明的无人机定位流程图；

图3为本发明的双机交叉定位示意图。

具体实施方式

下面将结合实施例对本发明的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其它实施例，都属于本发明保护的范围。

请参阅图1-3所示，一种无人机信息融合定位方法，无人机定位算法就是通过视觉传感器***测得各无人机之间的相对方位角α、俯仰角β，结合SINS/GPS组合导航***给出的目标无人机位置(经度λ、纬度及高度h)和姿态角(俯仰角θ、横滚角γ及航向角ψ)，利用齐次坐标转换法解算得到无人机在大地坐标系下的经度L、纬度B和高度H的过程；

本方法包括三部分，第一部分是基于测向交叉定位法利用UAV视觉传感器***测得各无人机的相对位置信息(相对方位角、俯仰角)；第二部分是利用SINS/GPS组合导航***解算出无人机在地面坐标系中的初步位置坐标(经度L、纬度B和高度H)；第三部分是在多机交叉定位中解算出无人机位置坐标，结合SINS/GPS组合导航***输出的无人机位置坐标分别作差计算，把得到的差值作为量测值，再经过Kalman滤波，估算出SINS/GPS组合导航***的误差并对其进行校正输出经过校正后目标无人机的位置坐标；该方法的示意图和流程图如图1、图2所示；

实施例1

由于在本定位***中主要关注的最终输出结果为无人机的位置坐标因而可对其余误差来源做简化处理，即略过无人机姿态角(θ,γ,ψ)的测量误差，同时对减振器振动角(Δθ_BA,Δγ_BA,Δψ_BA)、测向法测得无人机的相对位置信息(α,β)以及其余无人机测得的自身位置坐标、目标无人机相对方位角和俯仰角作如下近似假设：

考虑到航迹位置信息可得到(α,β,R)的准确值，可假设无人机对目标的定位过程中，参数X(X＝α,β,R)的参数误差ΔX都服从于均值为准确值，方差是σ_ΔX的正态分布；类似地，可假设参数X(X＝Δθ_BA,Δγ_BA,Δψ_BA)的参数误差ΔX都服从于均值为0，方差是σ_ΔX的正态分布，基站方位角误差、俯仰角误差，测向基站的自身定位误差都是零均值的，并且标准差分别为σ_φ、σ_ε、σ_S，并且可以认为测向基站自身定位的误差与基站所测的方位角误差、俯仰角误差之间都是相互独立的；具体流程如下：

(1)在某一时刻，通过SINS/GPS松组合导航***实时地得到目标无人机的初步位置信息及姿态角信息(θ,γ,ψ)；松组合方式就是分别获得SINS与GPS的量测信息，再分别进行做差运算，把得到的差值作为组合导航***的量测值，再经过Kalman滤波，估计出SINS的误差，然后对SINS进行校正；

(2)通过多机交叉定位测得的目标无人机位置坐标对上述初步位置坐标进行校正，得到校正后的目标无人机位置坐标；多机交叉定位具体实现为当有多架无人机时，各无人机的定位除组合导航自定位外，还可通过其余无人机实现，即将其余无人机作为测向基站，分别对某架目标无人机进行测向得到一组俯仰角和方位角；下面就双机三维定位算法进行分析；

如图3所示，A(x₁,y₁,z₁)、B(x₂,y₂,z₂)两无人机为测向基站，分别对无人机目标C(x,y,z)进行测向，分别得到一组俯仰角和方位角为(α₁,β₁)和(α₂,β₂)它们相互组合可以得到四组测量子集(α₁,β₁,α₂)、(α₁,β₁,β₂)、(α₂,β₂,α₁)和(α₂,β₂,β₁)，其中α_i(i＝1,2)为方位角(假设X轴为正北方向)，β_i(i＝1,2)为俯仰角；由方位角α₁、α₂和俯仰角β₁、β₂的三角关系可得：

可将上述方程表示为矩阵形式：

HX＝Z

其中

X＝(x,y,z)^T

由此可得目标无人机坐标为：

X＝H^-1Z

易知，由上述4个方程任选3组就可以进行组合解出目标无人机的位置；当推广到N(N＞2)个无人机的情形时，对于每个目标均可得到N组方位角和俯仰角的观测值，在定位解算时我们采用两两交叉定位；选择其中一个无人机为测向主站，坐标为A(x₀,y₀,z₀)；其它无人机作为辅站，坐标分别为A(x_i,y_i,z_i)(i＝1,2,...,N-1)，然后通过主站与辅站两两组合对目标进行定位，由此可以得到N-1个目标估计位置，分别为C(x_i,y_i,z_i)(i＝1,2,...,N-1)；采用聚类法对这组数据进行处理，从而得到目标位置坐标的估计值，具体流程为：设每个样本(即一个目标位置坐标)自成一类，即建立N-1类：(标号(0)表示聚类开始运算前的状态)，计算各类间的距离(初始时即为各样本间的距离)，得到一个N-1维的方阵D⁽⁰⁾；然后，假设前一步聚类运算中已求得距离矩阵D⁽ⁿ⁾(n为逐次聚类的次数)，则求D⁽ⁿ⁾中的最小元素，设其为/>和/>两类间的距离，则将/>与/>合并为一类/>由此建立新的分类：/>最后，计算合并后新类别间的距离，即计算/>与其他未发生合并的/>间的距离，此处我们采用重心法作为距离计算准则，得D⁽ⁿ⁺¹⁾；重复计算及合并，直到距离矩阵D⁽ⁿ⁾中的最小元素超过定位精度上限或所有元素都属于同一类则输出包含元素最多的类的重心作为最终输出结果，即目标位置坐标的估计值；

(3)将SINS/GPS松组合定位和无源测向多机交叉定位结果进行作差计算，经过Kalman滤波，估算出松组合导航***定位差值并校正；具体Kalman滤波在SINS/GPS组合导航***和无源交叉定位过程为：

设随机线性离散***的状态方程和量测方程分别是：

式中：T为迭代周期；Φ_k,k-1是k-1到k时刻的一步转移矩阵；Γ_k-1是***噪声矩阵；H_k是量测矩阵；V_k是量测噪声序列；W_k-1为k-1时刻的***噪声矩阵；假设***的过程噪声和观测噪声有如下统计特性：

式中Q_k为***噪声序列的方差矩阵；R_k为量测噪声方差矩阵；下面给出线性离散***的基本卡尔曼滤波(KF)方程；KF的基本思想是采用信号与噪声的状态空间模型，通过前一时刻的估计值和现在时刻的观测值来更新状态变量，进而计算出现在时刻的估计值；状态一步预测方程：

状态估计方程：

最优滤波增益方程：

最后得到预测均方误差方程：

上述是离散KF的基本方程，只要给出初始值和P₀，再根据k时刻的量测值Z_k，就可以递推出k时刻的状态估计值/>

本发明在使用时，基于测向交叉定位法利用UAV视觉传感器***测得各无人机的相对位置信息，利用SINS/GPS组合导航***解算出无人机在地面坐标系中的初步位置坐标；在多机交叉定位中解算出无人机位置坐标，结合SINS/GPS组合导航***输出的无人机位置坐标分别作差计算，把得到的差值作为量测值，再经过Kalman滤波，估算出SINS/GPS组合导航***的误差并对其进行校正输出经过校正后目标无人机的位置坐标，可以使多个无人机在松组合导航***和多机无源交叉测向定位下进行信息融合，从而在复杂气候环境中获得更好的定位效果。

以上公开的本发明优选实施例只是用于帮助阐述本发明。优选实施例并没有详尽叙述所有的细节，也不限制该发明仅为的具体实施方式。显然，根据本说明书的内容，可作很多的修改和变化。本说明书选取并具体描述这些实施例，是为了更好地解释本发明的原理和实际应用，从而使所属技术领域技术人员能很好地理解和利用本发明。本发明仅受权利要求书及其全部范围和等效物的限制。

Claims (4)

1.一种无人机信息融合定位方法，其特征在于，该方法包括以下步骤：

步骤一：通过测向交叉定位法和UAV视觉传感器***测得各无人机的相对位置信息；

步骤二：通过SINS/GPS组合导航***解算出无人机在地面坐标系中的初步位置坐标；具体为：在其中一时刻，通过SINS/GPS松组合导航***实时地得到目标无人机的初步位置信息及姿态角信息(θ₂γ₂ψ)；

步骤三：在多机交叉定位中解算出无人机位置坐标，结合SINS/GPS组合导航***输出的无人机位置坐标分别作差计算，把得到的差值作为量测值，再经过Kalman滤波，估算出SINS/GPS组合导航***的误差并对其进行校正输出经过校正后目标无人机的位置坐标；其中，多机交叉定位的具体表现为：当有多架无人机时，各无人机的定位除组合导航自定位外，通过其余无人机实现，即将其余无人机作为测向基站，分别对某架目标无人机进行测向得到一组俯仰角和方位角；

当多架无人机数量等于二时，则通过双机三维定位算法进行分析，具体分析过程为：以A(x₁,y₁,z₁)、B(x₂,y₂,z₂)两无人机为测向基站，分别对无人机目标C(x,y,z)进行测向，分别得到一组俯仰角和方位角，即为(α₁,β₁)和(α₂,β₂)，两者相互组合得到四组测量子集(α₁,β₁,α₂)、(α₁,β₁,β₂)、(α₂,β₂,α₁)和(α₂,β₂,β₁)，其中α_i(i＝1,2)为方位角，设定X轴为正北方向，β_i(i＝1,2)为俯仰角，由方位角α₁、α₂和俯仰角β₁、β₂的三角关系可得：

将上述方程表示为矩阵形式：

HX＝Z

其中

X＝(x,y,z)^T

由此可得目标无人机坐标为：

X＝H^-1Z

选取上述方程(1)中3组进行组合解出目标无人机的位置。

2.根据权利要求1所述的一种无人机信息融合定位方法，其特征在于，所述相对位置信息包括相对方位角和俯仰角；初步位置坐标包括经度L、纬度B和高度H。

3.根据权利要求1所述的一种无人机信息融合定位方法，其特征在于，当多架无人机数量大于二时时，对于每个无人机均可得到N组方位角和俯仰角的观测值，通过两两交叉定位进行定位解算，选择其中一个无人机为测向主站，坐标为A(x₀,y₀,z₀)；其它无人机作为辅站，坐标分别为A(x_i,y_i,z_i)(i＝1,2,...,N-1)，然后通过主站与辅站两两组合对目标进行定位，由此得到N-1个目标估计位置，分别为C(x_i,y_i,z_i)(i＝1,2,...,N-1)，通过聚类法该组数据进行处理，从而得到目标位置坐标的估计值，具体步骤为：

S1：设每个样本自成一类，即建立N-1类：标号(0)表示聚类开始运算前的状态，样本即一个目标位置坐标；

S2：计算各类间的距离，得到一个N-1维的方阵D⁽⁰⁾，初始时即为各样本间的距离；

S3：设前一步聚类运算中已求得距离矩阵D⁽ⁿ⁾，n为逐次聚类的次数，则求D⁽ⁿ⁾中的最小元素，设其为和/>两类间的距离，则将/>与/>合并为一类/>由此建立新的分类：/>

S4：计算合并后新类别间的距离，即计算与其他未发生合并的/>…间的距离；通过重心法作为距离计算准则，得D⁽ⁿ⁺¹⁾；重复计算及合并，直到距离矩阵中的最小元素超过定位精度上限或所有元素都属于同一类则输出包含元素最多的类的重心作为最终输出结果，即目标位置坐标的估计值。

4.根据权利要求1所述的一种无人机信息融合定位方法，其特征在于，所述Kalman滤波的具体过程为：设随机线性离散***的状态方程和量测方程分别是：

式中：T为迭代周期，Φ_k,k-1是k-1到k时刻的一步转移矩阵；Γ_k-1是***噪声矩阵；H_k是量测矩阵；V_k是量测噪声序列；W_k-1为k-1时刻的***噪声矩阵；

设定过程噪声和观测噪声有如下统计特性：

式中Q_k为***噪声序列的方差矩阵；R_k为量测噪声方差矩阵；

线性离散***的基本卡尔曼滤波(KF)方程；状态一步预测方程：

状态估计方程：

最优滤波增益方程：

最后得到预测均方误差方程：

上述是离散KF的基本方程，给出初始值和P₀，再根据k时刻的量测值Z_k，递推出k时刻的状态估计值/>