[发明专利]一种无人机编队协同导航方法

权利要求书

1.一种无人机编队协同导航方法，其特征在于，无人机编队包含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的位置信息和速度信息对无人机节点2和无人机节点3的k时刻的姿态四元数、速度、位置信息进行校正，得到无人机节点2和无人机节点3的高精度导航信息；

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

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

2.根据权利要求1所述无人机编队协同导航方法，其特征在于，在步骤(1)中，机体坐标系的X、Y、Z轴分别为无人机的右向、机头方向、天向，导航坐标系的X、Y、Z轴分别为东向、北向、天向。

3.根据权利要求2所述无人机编队协同导航方法，其特征在于，在步骤(3)中，采用下式预测姿态四元数：

其中，Q_i(k)＝[q_i0(k) q_i1(k) q_i2(k) q_i3(k)]^T为k时刻的姿态四元数，上标T表示矩阵的转置；Q_i(k-1)＝[q_i0(k-1) q_i1(k-1) q_i2(k-1) q_i3(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＝[0 0 g]^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轴上的位置坐标。