[发明专利]一种基于惯性/数据链的无人机编队相对导航方法

权利要求书

1.一种基于惯性/数据链的无人机编队相对导航方法，其特征在于，包括如下步骤：

步骤一：无人机节点搭载惯性导航系统，数据链收发装置，周期性读取k时刻无人机节点传感器数据，包括陀螺仪信息加速度计信息其中i＝7,8…,N以及无人机节点间相对距离d_ut，其中u,t表示不同的无人机节点；

步骤二：定义无人机节点机体坐标系及导航坐标系并建立无人机节点u,t之间相对位置、相对速度、相对姿态四元数误差方程、陀螺噪声误差方程，加速度计噪声误差方程；

步骤三：利用无人机节点惯导输出构成的惯导多边型与机载数据链相对测距构成的量测多边形计算k时刻编队中各无人机节点初始位置坐标；

步骤四：利用卡尔曼滤波器对k时刻无人机节点间的相对位置信息进行校正；

其中，步骤三的具体过程如下：

a)解算获得惯导多边形与量测多边形双中心转换矩阵B(t_k)，表示为：

上式中D(t_k)为无人机节点相对测距构成的相对距离矩阵其中d_ut表示无人机节点u与无人机节点t相对距离；其中I为n维单位向量，K为n维向量的元素；

b)解算双中心转换矩阵的特征值和特征向量

B(t_k)＝V(t_k)Λ(t_k)V(t_k)^T

其中Λ(t_k)＝diag(λ₁,λ₂,...λ_n)为矩阵B(t_k)的特征值对应的对角矩阵，λ₁,λ₂,...λ_n矩阵B(t_k)的特征值，V(t_k)＝[ν₁,ν₂,...,ν_n]为特征值所对应的特征向量，其中λ₁特征值对应的特征向量为ν₁，λ₂特征值对应的特征向量为ν₂，λ_n特征值对应的特征向量为ν_n，且λ₁≥λ₂≥...≥λ_n≥0；

c)解算编队中各无人机节点初始位置坐标：

各无人机节点的初始坐标由下式计算得到：

其中n≥7

其中：为无人机节点1k时刻三维初始位置；为无人机节点2k时刻三维初始位置；为无人机节点n k时刻初始位置；Λ₃(t_k)为k时刻双中心变换矩阵B(t_k)前三个最大特征值构成的对角矩阵；V_P(t_k)＝[ν₁,ν₂,ν₃]为k时刻双中心变换矩阵B(t_k)前三个最大特征值所对应的特征向量。

2.根据权利要求1所述的一种基于惯性/数据链的无人机编队相对导航方法，其特征在于步骤二中所述无人机节点机体坐标系及导航坐标系定义如下：机体坐标系的X、Y、Z轴分别为无人机的右向、前向和上向，导航坐标系分别为东、北、天地理坐标系。