一种无人机编队协同导航方法与流程

本发明属于导航技术领域，特别涉及了一种无人机导航方法。

背景技术：

无人机系统是一种无人驾驶，是能够自主控制或者遥控的空中飞行机器人系统，可执行对地、对海、对空的作战任务，具有可回收，可重复使用，高效等特点。随着任务需求的多样性和复杂性，单架无人机执行任务的能力有限，生存能力受到极大的挑战。而多架无人机之间的协同可以实现信息的互享和行动协调，实现单架无人机的任务扩展，提高任务执行的成功率。导航系统为无人机提供其飞行控制系统所必须的导航信息，是其完成各种复杂飞行任务的必要保障。

现阶段惯导/gps组合导航系统广泛应用为无人机提供导航服务。无人机编队中受限于成本、载荷等因素，不可能让编队中所有的无人机都搭载高精度的导航传感器，因此主从式的导航方式成为现阶段研究无人机编队协同导航的主流，长机搭载高精度的导航传感器通过滤波融合得到长机高精度的导航信息，在编队导航数据链及其他测距传感器的辅助下，通过编队协同方式完成对编队中其他无人机节点的高精度定位。然而现有的多无人机协同导航方法的精度并不理想。

技术实现要素：

为了解决上述背景技术提出的技术问题，本发明提出了一种无人机编队协同导航方法。

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

一种无人机编队协同导航方法，无人机编队包含4架无人机节点：无人机节点1～4，包括以下步骤：

(1)建立机体坐标系和导航坐标系；

(2)在无人机节点1上搭载惯性导航系统和gps接收机，周期读取k时刻无人机节点1的机载传感器信息，包括速度信息、位置信息，陀螺仪信息和加速度计信息；

(3)预测k时刻无人机节点i的姿态四元数、速度和位置信息，其中i＝1,2,3,4；

(4)利用卡尔曼滤波器，对k时刻的无人机节点1的姿态四元数、速度、位置信息进行校正，得到无人机节点1的高精度导航信息；

(5)在无人机节点i上搭载惯性导航系统、视觉导航设备和光流传感器，周期性读取k时刻无人机节点i的机载传感器信息，包括相对于无人机节点1的距离信息、俯仰角和方位角，以及通过光流解算出的速度信息、陀螺仪信息和加速度计信息，其中i＝2,3,4；

(6)根据相对于无人机节点1的距离信息、俯仰角和方位角解算出无人机节点2和无人机节点3的绝对位置信息；

(7)利用卡尔曼滤波器，根据无人机节点2和无人机节点3的位置信息和速度信息对k时刻的姿态四元数、速度、位置信息进行校正，得到无人机节点2和无人机节点3的高精度导航信息；

(8)根据无人机节点1～3的位置信息推导出无人机节点4的绝对位置信息；

(9)利用卡尔曼滤波器，将无人机节点4的位置信息和速度信息作为量测信息，对k时刻的姿态四元数、速度、位置信息进行校正，得到无人机节点4的高精度导航信息。

进一步地，在步骤(1)中，机体坐标系的x、y、z轴分别为无人机的右向、机头方向、天向，导航坐标系的x、y、z轴分别为东向、北向、天向。

进一步地，在步骤(3)中，采用下式预测姿态四元数：

其中，qi(k)＝[qi0(k)qi1(k)qi2(k)qi3(k)]^t为k时刻的姿态四元数，上标t表示矩阵的转置；qi(k-1)＝[qi0(k-1)qi1(k-1)qi2(k-1)qi3(k-1)]^t为k-1时刻的姿态四元数；δt为离散采样周期；通过下式计算：

其中通过下式计算：

其中，为k时刻陀螺仪读取的无人机节点i机体系相对于导航系的角速度在机体系x、y、z轴上的分量，为k时刻陀螺仪的零偏在机体系x、y、z轴上的分量；i＝1,2,3,4；

采用下式预测速度信息：

其中，为k时刻加速度计读取的无人机节点i机体系相对于导航系的加速度在机体系x、y、z轴上的分量；为k时刻加速度计零偏在机体系x、y、z轴上的分量；g＝[00g]^t，g为当地重力加速度值；为k时刻机体系相对于导航系的线速度在导航系x、y、z轴上的分量；

为k-1时刻机体系相对于导航系的线速度在导航系x、y、z轴上的分量；i＝1,2,3,4；为机体系到导航系之间的姿态矩阵，通过下式计算：

采用下式预测位置信息：

其中，分别为k时刻无人机节点i在导航系x、y、z轴上的位置坐标；k-1时刻的位置分别为k-1时刻无人机节点i在导航系x、y、z轴上的位置坐标。

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

(401)确定无人机节点1导航系统的状态向量，无人机节点1～4的状态向量相同，如下：

其中，为无人机节点i的平台误差角，δvei,δvni,δvui为无人机节点i的陀螺东向、北向、天向速度误差，δli,δλi,δhi为无人机节点i陀螺纬度、经度、高度误差，li,λi,hi无人机节点i陀螺纬度、经度、高度值，εbxi,εbyi,εbzi为无人机节点i陀螺随机常数误差，εrxi,εryi,εrzi为无人机节点i陀螺一阶马尔科夫随机噪声，为无人机节点i加速度计一阶马尔科夫过程；

当i取1时即为无人机节点1导航系统的状态向量；

(402)建立惯性导航系统的误差方程，包括平台误差角方程、速度误差方程和位置误差方程；

平台误差角方程：

其中，wie为地球自转角速度，vei、vni为无人机节点i惯性导航系统东向、北向速度，δvei、δvni为对应的速度误差，εei、εni、εui为无人机节点i东、北、天向陀螺噪声角，rm＝re(1-2f+3fsin²li)，rn＝re(1+fsin²li)，re＝6378137米，f＝1/298.257；

速度误差方程：

其中，vui为无人机节点i惯性导航系统天向速度，δvui为对应的速度误差，fei、fni、fui为无人机节点i加速度计测得的加速度值，为无人机节点i东向、北向、天向加速度计一阶马尔可夫过程白噪声，r为地球半径，g为当地重力加速度值；

位置误差方程：

当i取1时即为无人机节点1的惯性导航系统的误差方程；

(403)建立无人机节点1导航系统的状态方程，无人机节点1～4的状态方程相同，如下：

其中，gi为系统噪声系数矩阵，wi为噪声控制向量，fni为平台误差角、速度误差、位置误差构成的系统矩阵，tgxi、tgyi、tgzi为三轴陀螺相关时间常数，taxi、tayi、tazi为三轴加速度计相关时间常数；

当i取1时即为无人机节点1导航系统的状态方程；

(404)建立无人机节点1导航系统的量测方程：

无人机节点1导航系统的速度量测矩阵hv1和量测噪声vv1：

hv1＝[03×3diag[111]03×12]

vv1＝[me1mn1mu1]

上式中，me1,mn1,mu1分别为无人机节点1的gps接收机沿东、北、天向的测速误差；

无人机节点1导航系统的位置量测矩阵hp1和量测噪声vp1：

hp1＝[03×6diag[rmrncosl11]03×9]

vp1＝[ne1nn1nu1]

上式中，ne1,nn1,nu1分别为无人机节点1上的gps接收机在东、北、天向的位置误差；

(405)采用卡尔曼滤波器对无人机节点1的导航系统进行更新。

进一步地，采用下式解算出无人机节点2和无人机节点3的绝对位置信息(xi,yi,zi)：

其中，x、y、z为无人机节点1组合导航系统得到的高精度经度、纬度、高度信息，di为无人机节点i相对于无人机节点1的距离，αi、βi为无人机节点i相对于无人机节点1的俯仰角、方位角，νie,νin,νiu为无人机节点i的视觉导航设备解算的东向位置误差、北向位置误差、天向位置误差；i＝2,3。

进一步地，在步骤(7)中，首先建立无人机节点2和无人机节点3的导航系统量测方程：

无人机节点2和无人机节点3的导航系统速度量测矩阵hvi和量测噪声vvi：

hvi＝[03×3diag[110]03×12]

vvi＝[mdeimdni0]

上式中，mdei,mdni为无人机节点i的光流传感器在东、北向上的均值为零且方差为定值的量测白噪声；

无人机节点2和无人机节点3的导航系统位置量测矩阵hpi和量测噪声vpi：

vpi＝[νieνinνiu]

上式中，i＝2,3；

然后采用卡尔曼滤波器对无人机节点2和无人机节点3的导航系统进行更新。

进一步地，在步骤(9)中，首先建立无人机节点4的导航系统量测方程：

无人机节点4的导航系统速度量测矩阵hv4和量测噪声vv4：

hv4＝[03×3diag[110]03×12]

vv4＝[me4mn40]

上式中，me4,mn4为无人机节点4的光流传感器在东向、北向满足均值为零且方差为定值的速度量测白噪声；

无人机节点4的导航系统位置量测矩阵hp4和量测噪声vp4：

vp4＝[ν4eν4nν4u]

上式中，ν4e,ν4n,ν4u为无人机节点4的视觉导航设备解算的东向位置误差、北向位置误差、天向位置误差；

然后采用卡尔曼滤波器无人机节点4的导航系统进行更新。

进一步地，在步骤(8)中，设无人机节点1的位置为(x1,y1,z1)，无人机节点2的位置为(x2,y2,z2)，无人机节点3的位置为(x3,y3,z3)，通过无人机节点4上的视觉导航设备获得与无人机节点1的距离d4、与无人机节点2的距离d2、与无人机节点3的距离d3，则：

通过最小二乘法解算上式求出无人机节点4的位置(x4,y4,z4)。

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

无人机编队协同飞行时，受限于成本、体积、重量等方面的限制，每个无人机节点不可能都搭载高精度的导航传感器以实现无人机编队高精度的导航定位，本发明采用主从式的无人机编队协同导航方式，通过编队中长机搭载高精度的导航传感器实现长机的高精度的导航定位，各从机节点通过视觉导航设备获得从机无人机节点相对长机的距离，方位信息数学解算出各从无人机节点的绝对位置信息，同时各从无人机节点通过视觉导航设备实时获得无人机节点相对地面的速度，从而实现各从无人机节点高精度导航定位，提高了导航系统的稳定性、鲁棒性以及抗干扰性能。

附图说明

图1是本发明的总体框架图；

图2是本发明中无人机节点1的导航解算流程图；

图3是本发明中无人机节点2和3的导航解算流程图；

图4是本发明中无人机节点4的导航解算流程图。

具体实施方式

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

如图1所示，本发明设计的一种无人机编队协同导航方法，无人机编队包含4架无人机节点：无人机节点1～4，包括以下步骤：

步骤1：建立机体坐标系和导航坐标系；

步骤2：在无人机节点1上搭载惯性导航系统和gps接收机，周期读取k时刻无人机节点1的机载传感器信息，包括速度信息、位置信息，陀螺仪信息和加速度计信息；

步骤3：预测k时刻无人机节点i的姿态四元数、速度和位置信息，其中i＝1,2,3,4；

步骤4：利用卡尔曼滤波器，对k时刻的无人机节点1的姿态四元数、速度、位置信息进行校正，得到无人机节点1的高精度导航信息，如图2所示；

步骤5：在无人机节点i上搭载惯性导航系统、视觉导航设备和光流传感器，周期性读取k时刻无人机节点i的机载传感器信息，包括相对于无人机节点1的距离信息、俯仰角和方位角，以及通过光流解算出的速度信息、陀螺仪信息和加速度计信息，其中i＝2,3,4；

步骤6：根据相对于无人机节点1的距离信息、俯仰角和方位角解算出无人机节点2和无人机节点3的绝对位置信息；

步骤7：利用卡尔曼滤波器，根据无人机节点2和无人机节点3的位置信息和速度信息对k时刻的姿态四元数、速度、位置信息进行校正，得到无人机节点2和无人机节点3的高精度导航信息，如图3所示；

步骤8：根据无人机节点1～3的位置信息推导出无人机节点4的绝对位置信息；

步骤9：利用卡尔曼滤波器，将无人机节点4的位置信息和速度信息作为量测信息，对k时刻的姿态四元数、速度、位置信息进行校正，得到无人机节点4的高精度导航信息，如图4所示。

在本实施例中，步骤1采用如下优选技术方案：

机体坐标系的x、y、z轴分别为无人机的右向、机头方向、天向，导航坐标系的x、y、z轴分别为东向、北向、天向。

在本实施例中，步骤3采用如下优选技术方案：

采用下式预测姿态四元数：

其中，qi(k)＝[qi0(k)qi1(k)qi2(k)qi3(k)]^t为k时刻的姿态四元数，上标t表示矩阵的转置；qi(k-1)＝[qi0(k-1)qi1(k-1)qi2(k-1)qi3(k-1)]^t为k-1时刻的姿态四元数；δt为离散采样周期；通过下式计算：

其中通过下式计算：

其中，为k时刻陀螺仪读取的无人机节点i机体系相对于导航系的角速度在机体系x、y、z轴上的分量，为k时刻陀螺仪的零偏在机体系x、y、z轴上的分量；i＝1,2,3,4；

采用下式预测速度信息：

其中，为k时刻加速度计读取的无人机节点i机体系相对于导航系的加速度在机体系x、y、z轴上的分量；为k时刻加速度计零偏在机体系x、y、z轴上的分量；g＝[00g]^t，g为当地重力加速度值；为k时刻机体系相对于导航系的线速度在导航系x、y、z轴上的分量；为k-1时刻机体系相对于导航系的线速度在导航系x、y、z轴上的分量；i＝1,2,3,4；为机体系到导航系之间的姿态矩阵，通过下式计算：

采用下式预测位置信息：

其中，分别为k时刻无人机节点i在导航系x、y、z轴上的位置坐标；k-1时刻的位置分别为k-1时刻无人机节点i在导航系x、y、z轴上的位置坐标。

在本实施例中，步骤4采用如下技术方案：

由于4个无人机节点的状态量和状态方程都相同，因此用i＝1,2,3,4统一表示。

1)无人机节点一组合导航系统的状态量：

其中，为无人机节点i的平台误差角，δvei,δvni,δvui为无人机节点i的陀螺东向、北向、天向速度误差，δli,δλi,δhi为无人机节点i陀螺纬度、经度、高度误差，li,λi,hi无人机节点i陀螺纬度、经度、高度值，εbxi,εbyi,εbzi为无人机节点i陀螺随机常数误差，εrxi,εryi,εrzi为无人机节点i陀螺一阶马尔科夫随机噪声，为无人机节点i加速度计一阶马尔科夫过程。

2)建立惯性导航系统的误差方程，包括平台误差角方程、速度误差方程和位置误差方程；

平台误差角方程：

其中，wie为地球自转角速度，vei、vni为无人机节点i惯性导航系统东向、北向速度，δvei、δvni为对应的速度误差，εei、εni、εui为无人机节点i东、北、天向陀螺噪声角，rm＝re(1-2f+3fsin²li)，rn＝re(1+fsin²li)，re＝6378137米，f＝1/298.257；

速度误差方程：

其中，vui为无人机节点i惯性导航系统天向速度，δvui为对应的速度误差，fei、fni、fui为无人机节点i加速度计测得的加速度值，为无人机节点i东向、北向、天向加速度计一阶马尔可夫过程白噪声，r为地球半径，g为当地重力加速度值；

位置误差方程：

3)建立无人机节点1导航系统的状态方程，无人机节点1～4的状态方程相同，如下：

其中，gi为系统噪声系数矩阵，wi为噪声控制向量，fni为平台误差角、速度误差、位置误差构成的系统矩阵，tgxi、tgyi、tgzi为三轴陀螺相关时间常数，taxi、tayi、tazi为三轴加速度计相关时间常数。

3)无人机节点1的系统量测方程：

k时刻gps接收机获得无人机载体的速度和位置信息可表示为：

上式中，λt1,lt1,ht1,ne1,nn1,nu1,ve1,vn1,vu1,me1,mn1,mu1分别为gps测得载体沿地理坐标系下各轴下的位置真值、gps接收机在东、北、天向的位置误差、gps测得载体在地理坐标系下沿各轴的速度真值以及gps接收机沿东、北、天向的测速误差。将gps接收机获得载体的速度信息、位置信息与惯导解算出载体的速度、位置信息作差分并写成系统量测方程形式z＝hx+v，可得：

无人机节点1导航系统的速度量测矩阵hv1和量测噪声vv1：

hv1＝[03×3diag[111]03×12]

vv1＝[me1mn1mu1]

上式中，me1,mn1,mu1分别为无人机节点1的gps接收机沿东、北、天向的测速误差；

无人机节点1导航系统的位置量测矩阵hp1和量测噪声vp1：

hp1＝[03×6diag[rmrncosl11]03×9]

vp1＝[ne1nn1nu1]

上式中，ne1,nn1,nu1分别为无人机节点1上的gps接收机在东、北、天向的位置误差。

4)采用卡尔曼滤波器对无人机节点一导航系统进行更新：

xk+1＝fk+1,kxk+gkwk

zk+1＝hk+1xk+1+vk+1

上式中，xk+1为系统k+1时刻的状态向量，wk为k时刻的噪声控制向量，fk+1,k为k到k+1时刻的系统状态转移矩阵，gk为系统噪声系数矩阵；zk+1为k+1时刻的量测向量，hk+1为k+1时刻系统的量测矩阵，vk+1为系统k+1时刻系统的量测噪声。

首先进行系统一步状态预测：

其次求解一步预测的均方差：

上式中，为k时刻状态变量xk的估计值，为利用计算得到的预测k+1时刻状态变量xk+1的值，pk为k时刻估计均方差误差，qk为系统的噪声方差矩阵，pk+1/k为状态向量的均方差。

然后求解滤波增益：

上式中，rk+1为噪声的方差矩阵，通过以上各式完成对无人机节点1的状态估值，从而得到滤波结果：

同时更新估计系统的均方差误差：

pk+1＝[1-kk+1hk+1]pk+1/k

在本实施例中，步骤6采用如下优选技术方案：

采用下式解算出无人机节点2和无人机节点3的绝对位置信息(xi,yi,zi)：

其中，x、y、z为无人机节点1组合导航系统得到的高精度经度、纬度、高度信息，di为无人机节点i相对于无人机节点1的距离，αi、βi为无人机节点i相对于无人机节点1的俯仰角、方位角，νie,νin,νiu为无人机节点i的视觉导航设备解算的东向位置误差、北向位置误差、天向位置误差；i＝2,3。

在本实施例中，步骤7采用如下优选技术方案：

首先建立无人机节点2和无人机节点3的导航系统量测方程：

无人机节点2和无人机节点3的导航系统速度量测矩阵hvi和量测噪声vvi：

hvi＝[03×3diag[110]03×12]

vvi＝[mdei,mdni,0]

上式中，mdei,mdni为无人机节点i的光流传感器在东、北向上的均值为零且方差为定值的量测白噪声；

无人机节点2和无人机节点3的导航系统位置量测矩阵hpi和量测噪声vpi：

vpi＝[νieνinνiu]

上式中，i＝2,3；

然后采用卡尔曼滤波器对无人机节点2和无人机节点3的导航系统进行更新。

在本实施例中，步骤8采用如下优选技术方案：

设无人机节点1的位置为(x1,y1,z1)，无人机节点2的位置为(x2,y2,z2)，无人机节点3的位置为(x3,y3,z3)，通过无人机节点4上的视觉导航设备获得与无人机节点1的距离d4、与无人机节点2的距离d2、与无人机节点3的距离d3，则：

通过最小二乘法解算上式求出无人机节点4的位置(x4,y4,z4)。

在本实施例中，步骤9采用如下优选技术方案：

k时刻无人机节点4解算得到的位置信息与光流解算得到的速度信息可用如下公式表示：

无人机节点4惯导解算出的位置在地球地固坐标系(ecef)可表示为：

上式中xi,yi,zi为无人机节点4机载惯导系统解算出的载体位置在地球地固坐标系下的位置，xt,yt,zt为惯导解算出的位置在地球地固坐标下的位置真值，δx,δy,δz为地球地固坐标系下的三个方向的位置误差。

无人机节点4在地球地固(ecef)坐标系解算出的位置可表示为：

上式中，x、y、z为无人机节点4在地球地固坐标系下三个方向上的位置，xt,yt,zt为三个方向上的位置真值，nx,ny,nz为三个方向上的白噪声。

可得：

无人机节点4机载光流传感器解算得到的无人机水平方向的速度可表示为：

上式中，vde4,vdn4为无人机节点4搭载的光流传感器获得无人机水平方向上对地速度，vdet4,vdnt4为水平方向对地速度真值。

将无人机节点4位置、速度信息与惯导解算出来的位置、速度信息作差分得到无人机节点四的量测量并改写成量测方程形式：

无人机节点4的导航系统速度量测矩阵hv4和量测噪声vv4：

hv4＝[03×3,diag[1,1,1],03×3]

vv4＝[me4mn40]

上式中，me4,mn4为无人机节点4的光流传感器在东向、北向满足均值为零且方差为定值的速度量测白噪声；

无人机节点4的导航系统位置量测矩阵hp4和量测噪声vp4：

vp4＝[ν4eν4nν4u]

上式中，ν4e,ν4n,ν4u为无人机节点4的视觉导航设备解算的东向位置误差、北向位置误差、天向位置误差。

然后采用卡尔曼滤波器无人机节点4的导航系统进行更新。

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