CN110274588B - 基于无人机集群信息的双层嵌套因子图多源融合导航方法 - Google Patents

Abstract

本发明公开了基于无人机集群信息的双层嵌套因子图多源融合导航方法。首先，集群中每一架无人机个体的各种机载传感器感知这架无人机的状态量测信息，通过内层因子图多源导航信息融合框架获得无人机自身的自主导航信息。然后，基于合作目标的视觉量测方法获取集群无人机间的相对位姿信息，解算协同无人机的绝对定位信息。最后，将集群中的无人机自主导航系统量测信息、飞行控制参数信息以及协同导航量测信息作为外层因子图的因子节点，通过双层嵌套因子图的框架将众多不同构型、不同更新率下的无人机协同导航信息进行融合应用，提高了无人机集群协同定位的整体性能。

Description

基于无人机集群信息的双层嵌套因子图多源融合导航方法

技术领域

本发明属于组合导航技术领域，特别涉及了一种无人机集群的多源融合导航方法。

背景技术

近些年，无人机凭借其巨大的应用市场和潜在的扩展领域受到世界各国的高度重视，无人机技术在军事领域和民用领域均获得了广泛应用。随着无人机技术的快速发展和无人机数量的日益增多，单架无人机执行任务能力有限，集群无人机概念和技术需求也逐步提上日程。相比于单一无人机平台，集群无人机具备功能分布化、体系生存率高、大范围态势感知、成本低、效率高等优势，必将在军事作战、救灾抢险、精密农业、线路巡检、测绘测量、安全监视等军民领域获得广泛应用。

无人机之间的协同导航是无人机集群的重要技术之一，与单架无人机的导航方式有所区别，集群无人机的定位不仅依靠自生的自主导航系统的绝对位置信息，同时也依靠集群内其他协同无人机的相位位置信息。一般来说，无人机之间相对测距和测角精度较高，可以作为基准来校正集群中无人机的自主导航系统绝对位置信息。

在无人机导航系统中，由于各传感器所采用的导航原理不同，各类传感器之间存在极强的互补性。目前在无人机多源组合导航系统中常用的导航传感器包括：惯性导航系统、卫星导航系统、视觉导航系统、大气数据系统、地形匹配导航系统、激光雷达系统等等。在实际应用中，由于不同传感器的更新频率不同，存在时间不同步的问题，同时在组合过程中，各传感器在不同环境和条件下的使用局限，可用性会发生变化，会影响到滤波结构，比如，卫星导航信号难以穿透地面和建筑物等密度较大的物质，在都市、室内、地下环境中信号的衰减现象非常严重，导致卫星导航接收机难以正常工作。通常，采用固定的滤波结构和方法均很难满足这种复杂多变的应用需求。

概率图模型是一种以图模型表示变量概率依存关系的理论。因子图是一种双向概率图模型，图中包含两种类型的节点：一种是变量节点，代表全局多元函数中的变量；一种是因子节点，代表因式分解中的局部函数。每个局部函数只与全局多元函数中的部分变量相关，当且仅当变量是局部函数的自变元时，因子图中与之相应的变量节点与因子节点之间存在一条连接边。因子图作为一种分析问题的图形工具，或许有助于解决无人机传感器量测信息的非等间隔、不同步、动态变化等问题。

发明内容

为了解决上述背景技术提到的技术问题，本发明提出了基于无人机集群信息的双层嵌套因子图多源融合导航方法。

为了实现上述技术目的，本发明的技术方案为：

基于无人机集群信息的双层嵌套因子图多源融合导航方法，包括以下步骤：

(1)在无人机集群飞行任务中，根据无人机集群的编队结构、任务需求以及所处环境设计载体的运动动作和运动轨迹，并确定集群中各无人机机载导航传感器的类型；

(2)对于集群中的每一架无人机个体，将其状态变量定义为因子图的变量节点，机载各导航传感器测量值定义为因子节点，构建基于内层因子图的多源导航信息融合框架，在此框架下，完成多源导航信息的有效融合；

(3)通过机载视觉相机量测协同无人机的相对位姿信息并基于相对位姿信息求解协同无人机的绝对位姿信息；

(4)定义集群中无人机状态变量为外层因子图的变量节点，定义无人机自主导航系统量测信息、飞行控制参数信息以及协同导航量测信息为外层因子图的因子节点，构建基于外层因子图的多源导航信息融合框架，从而形成双层嵌套因子图的多源导航信息融合框架，在此框架下，完成无人机集群间多源导航定位信息融合。

进一步地，步骤(2)的具体过程如下：

(2a)每一架无人机均通过惯性测量单元中的陀螺仪和加速度计获取载体的角速度信息和比力信息，同时，通过其他各类机载高精度传感器获取载体各类量测信息；

(2b)定义无人机个体的导航系统状态变量为内层因子图的变量节点，定义惯性测量单元和其他各类机载传感器获取的载体量测信息为内层因子图的因子节点，构建基于内层因子图的多源导航信息融合框架，构建导航系统的状态方程和量测方程；

(2c)在基于内层因子图的多源导航信息融合框架下，选取多源导航信息融合的约束规则，建立惯性测量单元及其他各类机载高精度导航传感器的因子节点表达式，选择因子节点的代价函数，并在代价函数值取最小时对状态变量求偏导数，令偏导数为0得到状态变量的估计，经过实时滤波估计和修正，完成多源导航信息的有效融合。

进一步地，导航系统状态变量X如下：

上式中，φ_E，φ_N，φ_U为平台误差角，δV_E，δV_N，δV_U东北天方向的速度误差，δL，δλ，δh纬度、经度、高度位置误差，ε_bx，ε_by，ε_bz为陀螺仪随机常数，ε_rx，ε_ry，ε_rz为陀螺仪一阶马尔柯夫过程，

加速度计一阶马尔柯夫过程；

所述内层因子图的变量节点f_IMU如下：

f_IMU＝L(X_k+1-F(X_k,Z_IMU))

所述内层因子图的因子节点f(X_k)如下：

f(X_k)＝L(Z_k-H)

上式中，L(·)表示代价函数，H是量测函数，X_k为k时刻的系统状态变量，Z_k为k时刻实际量测值，F是系统的传递函数矩阵，Z_IMU为惯性测量单元的量测值，Z_IMU＝{f_b,ω_b}，其中f_b、ω_b分别为惯性测量单元得到的比力和角速度；

所述因子节点的代价函数L(Z_k-H)如下：

上式中，

为X_k的估计值，W为正定加权矩阵。

进一步地，步骤(3)的具体过程如下：

(3a)通过集群内无人机的机载视觉相机获取协同无人机的合作特征点视觉图像，提取图像中协同无人机的合作特征点，建立视觉相机的针孔模型；

(3b)计算图像中合作特征点的位置坐标，并确定视觉相机的内部参数矩阵和外部参数矩阵，构建系统的相对位姿量测方程，解算出协同无人机的相对量测信息；

(3c)通过协同无人机的自身定位信息和基于视觉相机量测的协同无人机上合作特征点的相对位姿信息进行推算，转换空间坐标系，获取协同无人机的绝对位姿信息。

进一步地，合作特征点映射在焦距平面的位置坐标q：

q＝K[R|t]Q

其中，

[R|t]代表R和t组成的分块矩阵，Q为合作特征点在地理坐标系中的点坐标，K为视觉相机的内部参数矩阵，

为视觉相机中心在地理坐标系中的坐标，R为相机坐标系相对于地理坐标系的旋转方向，

和R统称为相机的外部参数矩阵。

进一步地，在步骤(4)中，在双层嵌套因子图的多源导航信息融合框架下，建立无人机集群间的多源导航信息融合滤波方程，选择因子节点的代价函数，并在代价函数值取最小时对状态变量求偏导数，令偏导数为0得到状态变量的估计，经过实时滤波估计和修正，完成基于无人机集群信息的双层嵌套因子图多源融合导航。

进一步地，在步骤(4)中：

自主导航系统量测信息的因子节点f_SELF如下：

f_SELF＝L(X_m+1-F(X_m,Z_SELF))

协同导航量测信息的因子节点f(X_m)如下：

f(X_m)＝L(Z_m-H)

上式中，L(·)表示代价函数，H是量测函数，X_m为m时刻的系统状态变量，Z_m为m时刻实际量测值，F是系统的传递函数矩阵，Z_SELF为自身导航系统量测信息、飞行控制参数信息以及协同导航定位信息；

因子节点的代价函数L(Z_m-H)如下：

上式中，

为X_m的估计值，W为正定加权矩阵。

采用上述技术方案带来的有益效果：

本发明采用双层嵌套因子图架构对集群无人机协同导航方法进行配置。内层因子图将状态量定义为变量节点，各传感器测量值定义为因子节点，构建基于因子图的多源导航信息融合框架，能够有效地利用传感器信息，做到即插即用。同时，针对无人机集群协同导航架构实时变化的特点，将集群内无人机单体的自身导航系统量测信息、飞行控制参数信息以及协同导航量测信息抽象为广义传感器引入外层因子图结构中，作为抽象量测因子节点，构建外层因子图的全局导航融合方法，能够解决集群构型变化带来的无人机协同导航信息难以同步的问题，做到无人机集群实时构型变化下的有效信息融合。

附图说明

图1为本发明的整体流程图；

图2为本发明双层嵌套因子图多源融合结构图。

具体实施方式

以下将结合附图，对本发明的技术方案进行详细说明。

如图1所示，以无人机1为例，首先基于任务需求，获得机载各导航系统得到传感器量测信息，进行因子图信息融合，接着通过视觉相机得到无人机1与其他集群无人机的相对量测信息，构建外层因子图构多源信息融合框架，通过双层嵌套因子图多源信息滤波融合优化得到导航信息。下文将对本发明的整个过程进行详细说明。

1、在无人机集群飞行环境下，根据实际情况、任务需求以及所处环境设计无人机集群的载体数量、集群构型、运动动作、运动轨迹等。

此处以无人机集群为例，对集群构型和航迹的设计进行具体的说明。根据任务目标，要设计符合无人机集群飞行需求、载体所处的复杂环境以及任务特点的飞行轨迹。对于不同任务的需求，例如：军事攻击、抢险救援、线路巡检、快递运送、飞行表演等，其工作环境、飞行动作、任务需求也有很大的不同，飞行构型和飞行航迹也不一样。对于无人机集群而言，典型的飞行构型可以分为如下几种：

①三角形；②菱形；③S形；④8字形；⑤梯形；⑥圆形；⑦直线形等的组合。

要完成不同的飞行任务，无人机集群可以在上述构型中选择一种或多种进行组合。根据上述要求，设计一个符合实际情况的构型和各自的航迹，使其能满足任务要求和实际需要。

2、由于集群中无人机的任务需求不同，集群无人机中各无人机所需要的运动动作与运动轨迹各有不同，需要以此确定可用的传感器类型。

可用于因子图框架的导航传感器类型众多，惯性导航传感器(包括加速度计、陀螺仪)、卫星导航传感器、视觉相机、气压计、激光雷达、磁力计、星敏感器、倾角罗盘、距离/伪距测距仪、红外传感器、偏振光传感器、光流传感器等。根据任务需求及实际情况，对集群无人机的导航系统方案进行设计。

3、双层嵌套因子图多源信息融合框架的建立

因子图是一种概率图模型G＝(F,X,E)，包含两种类型的节点：一种是因子节点f_i∈F，代表因式分解中的局部函数；一种是变量节点x_j∈X，代表全局多元函数中的变量。当因子图架构中的状态变量节点x_j和与之对应的因子节点f_i相关时，它们之间存在一条连接边e_ij∈E。

概率模型是因子图的一个主要应用方向。例如，设X，Y，Z为可以组成马尔科夫链的随机变量，它们的联合概率密度：

p_XYZ(x,y,z)＝p_X(x)p_Y|X(y|x)p_Z|Y(z|y)

其中，p_Y|X(y|x)和p_Z|Y(z|y)代表条件概率密度函数。

本发明中，无人机集群采用双层嵌套式的因子图架构作为多源信息融合方案，内层因子图架构将每一个无人机的状态量定义为变量节点，选择每一架无人机的捷联惯导系统、卫星导航系统、大气数据系统、视觉相机导航系统、磁航向系统、地形轮廓匹配系统等通用导航传感器量测值定义为因子节点，由于在多源组合导航系统中，各传感器更新观测量的速率不一致，因此采用更新速率最高的捷联惯导作为主导航源，将多源融合导航系统架构用因子图构造表示出来，如图2下方所示。图中，圆圈代表状态变量节点，黑色方块代表因子节点，X代表系统的导航状态，f代表各传感器量测信息，f_Prior表示先前的量测信息，f_IMU表示来自IMU的量测信息，与k时刻和k+1时刻的导航状态相关，f_GNSS、f_ADS、f_SAR、f_BA也分别是其他导航系统的量测信息。在因子图框架下，说明系统的状态及量测更新过程，建立滤波方程，经过实时滤波估计和修正，从而完成多源传感器信息的有效融合。

集群中的无人机之间采用视觉传感器量测相对位姿信息，通过提取协同无人机上确定的合作特征点确定协同无人机在图像中的坐标。建立视觉传感器的针孔模型，合作特征点映射在焦距平面的位置坐标为：

q＝K[R|t]Q

其中

[R|t]代表是R和t组成的分块矩阵。Q为合作特征点在地理坐标系中的点坐标，q为合作特征点在焦距平面的点坐标，参数K为视觉相机的内部参数矩阵(内参)，与图像坐标系中x和y轴方向上焦距长度表示的像素个数及主点p在图像坐标系中像素纬度下的坐标有关，参数

为视觉相机中心在地理坐标系中的坐标，参数R为相机坐标系相对于地理坐标系的旋转方向，两者均代表视觉相机的方向和位置信息，统称为相机的外部参数矩阵(外参)。利用集群无人机间的相对位姿量测信息和自身导航信息来推算协同导航信息。

外层因子图架构将集群中无人机个体看作因子图中的一个广义的传感器节点，依靠无人机自身导航定位系统的量测信息和飞行控制参数信息，以及协同导航量测信息看作主导航传感器信息，基于协同无人机相对位姿信息推算的自身定位信息看作辅助导航传感器量测信息，将其均定位为因子节点，将无人机集群协同融合导航系统用外层嵌套因子图构造表示出来，如图2上方所示。图中，圆圈代表状态变量节点，黑色方块代表因子节点，X代表系统的导航状态，外部无人机协同信息包括外部无人机的飞行控制参数信息和协同导航量测信息。这些信息与k时刻和k+1时刻的导航状态相关，无人机信息1到无人机信息n分别是其他协同无人机推算获得的量测信息。与内层因子图架构类似，结合因子图多源信息融合系统的状态及量测更新过程，建立滤波方程，经过实时滤波估计和修正，从而完成多源传感器信息的有效融合。

实施例仅为说明本发明的技术思想，不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明保护范围之内。

Claims (3)

1.基于无人机集群信息的双层嵌套因子图多源融合导航方法，其特征在于，包括以下步骤：

(1)在无人机集群飞行任务中，根据无人机集群的编队结构、任务需求以及所处环境设计载体的运动动作和运动轨迹，并确定集群中各无人机机载导航传感器的类型；

(2)对于集群中的每一架无人机个体，将其状态变量定义为因子图的变量节点，机载各导航传感器测量值定义为因子节点，构建基于内层因子图的多源导航信息融合框架，在此框架下，完成多源导航信息的有效融合；该步骤的具体过程如下：

(2a)每一架无人机均通过惯性测量单元中的陀螺仪和加速度计获取载体的角速度信息和比力信息，同时，通过其他各类机载高精度传感器获取载体各类量测信息；

(2b)定义无人机个体的导航系统状态变量为内层因子图的变量节点，定义惯性测量单元和其他各类机载传感器获取的载体量测信息为内层因子图的因子节点，构建基于内层因子图的多源导航信息融合框架，构建导航系统的状态方程和量测方程；

(2c)在基于内层因子图的多源导航信息融合框架下，选取多源导航信息融合的约束规则，建立惯性测量单元及其他各类机载高精度导航传感器的因子节点表达式，选择因子节点的代价函数，并在代价函数值取最小时对状态变量求偏导数，令偏导数为0得到状态变量的估计，经过实时滤波估计和修正，完成多源导航信息的有效融合；

所述导航系统状态变量X如下：

上式中，

为平台误差角，δV_e，δV_n，δV_d为东北天方向的速度误差，δL，δλ，δh为纬度、经度、高度位置误差，ε_bx，ε_by，ε_bz为陀螺仪随机常数，ε_rx，ε_ry，ε_rz为陀螺仪一阶马尔柯夫过程，

为加速度计一阶马尔柯夫过程；

所述内层因子图的变量节点f_IMU如下：

f_IMU＝L(X_k+1-F(X_k,Z_IMU))

所述内层因子图的因子节点f(X_k)如下：

f(X_k)＝L(Z_k-H)

上式中，L(·)表示代价函数，H是量测函数，X_k为k时刻的系统状态变量，Z_k为k时刻实际量测值，F是系统的传递函数矩阵，Z_IMU为惯性测量单元的量测值，Z_IMU＝{f_b,ω_b}，其中f_b、ω_b分别为惯性测量单元得到的比力和角速度；

所述因子节点的代价函数L(Z_k-H)如下：

上式中，

为X_k的估计值，W为正定加权矩阵；

(3)通过机载视觉相机量测协同无人机的相对位姿信息并基于相对位姿信息求解协同无人机的绝对位姿信息；

(4)定义集群中无人机状态变量为外层因子图的变量节点，定义无人机自主导航系统量测信息、飞行控制参数信息以及协同导航量测信息为外层因子图的因子节点，构建基于外层因子图的多源导航信息融合框架，从而形成双层嵌套因子图的多源导航信息融合框架，在此框架下，完成无人机集群间多源导航定位信息融合；

在双层嵌套因子图的多源导航信息融合框架下，建立无人机集群间的多源导航信息融合滤波方程，选择因子节点的代价函数，并在代价函数值取最小时对状态变量求偏导数，令偏导数为0得到状态变量的估计，经过实时滤波估计和修正，完成基于无人机集群信息的双层嵌套因子图多源融合导航；

自主导航系统量测信息的因子节点f_SELF如下：

f_SELF＝L(X_m+1-F(X_m,Z_SELF))

协同导航量测信息的因子节点f(X_m)如下：

f(X_m)＝L(Z_m-H)

上式中，L(·)表示代价函数，H是量测函数，X_m为m时刻的系统状态变量，Z_m为m时刻实际量测值，F是系统的传递函数矩阵，Z_SELF为自身导航系统量测信息、飞行控制参数信息以及协同导航定位信息；

因子节点的代价函数L(Z_m-H)如下：

上式中，

为X_m的估计值，W为正定加权矩阵。

2.根据权利要求1所述基于无人机集群信息的双层嵌套因子图多源融合导航方法，其特征在于，步骤(3)的具体过程如下：

(3a)通过集群内无人机的机载视觉相机获取协同无人机的合作特征点视觉图像，提取图像中协同无人机的合作特征点，建立视觉相机的针孔模型；

(3b)计算图像中合作特征点的位置坐标，并确定视觉相机的内部参数矩阵和外部参数矩阵，构建系统的相对位姿量测方程，解算出协同无人机的相对量测信息；

(3c)通过协同无人机的自身定位信息和基于视觉相机量测的协同无人机上合作特征点的相对位姿信息进行推算，转换空间坐标系，获取协同无人机的绝对位姿信息。

3.根据权利要求2所述基于无人机集群信息的双层嵌套因子图多源融合导航方法，其特征在于，合作特征点映射在焦距平面的位置坐标q：

q＝K[R|t]Q

其中，

[R|t]代表R和t组成的分块矩阵，Q为合作特征点在地理坐标系中的点坐标，K为视觉相机的内部参数矩阵，

为视觉相机中心在地理坐标系中的坐标，R为相机坐标系相对于地理坐标系的旋转方向，

和R统称为相机的外部参数矩阵。

Images