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

权利要求书

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为正定加权矩阵。