CN111238469A - 一种基于惯性/数据链的无人机编队相对导航方法 - Google Patents

Abstract

本发明公开了一种基于惯性/数据链的无人机编队相对导航方法，属于无人机编队相对导航领域。本发明首先通过无人机节点机载惯导输出构成的惯导多边形与相对测距量测多边形根据双中心转换法得到无人机编队各无人机节点初始位置坐标；然后通过建立相对导航状态方程完成对无人机节点间相对位置、相对速度、相对加速度、相对四元数的预测，最后利用无人机节点间的相对测距信息作为量测量实现无人机编队节点间相对导航。本发明不仅仅能够得到编队中无人机节点间高精度的相对导航信息，同时能够提高无人机节点机载惯导系统的位置输出精度。本发明适用于卫星拒止环境下至少7架无人机以上的无人机编队相对导航。

Description

一种基于惯性/数据链的无人机编队相对导航方法

技术领域

本发明涉及一种基于惯性/数据链的无人机编队相对导航方法，属于编队无人机相对导航技术领域。

背景技术

无人机在现代战争中主要用于军事侦察、战略打击等任务，传统的单一无人机在执行任务时受载体承重、成本等条件的限制，同时单一的无人机在执行任务时抗毁伤能力不足，已无法满足现阶段作战需求。编队无人机在此背景下应运而生，为了提高无人机编队作战能力，无人机间应具有高精度相对导航能力就显得尤为重要。同时无人机在战场环境下容易受环境、电磁干扰等影响，GPS在战场环境不可用，迫切需要研究GPS卫星拒止环境下无人机编队相对导航方法。

现阶段编队无人机相对导航一般采用GPS卫星、测角/测距传感器辅助来实现。然而在战场环境下GPS信号易受到干扰而导致不可用，编队无人机需要大量搭载测距/测角传感器增加了项目成本，同时现阶段测距/测角设备主要用视觉/雷达传感器来实现，战场环境特征复杂对现有视觉/雷达匹配算法的实时性、可靠性要求较高，在实际情况下很难得到应用。

发明内容

本发明提出了一种基于惯性/数据链的无人机编队相对导航方法，首先通过无人机节点机载惯导输出构成的惯导多边形与相对测距量测多边形根据双中心转换法得到无人机编队各无人机节点初始位置坐标；然后通过建立相对导航状态方程完成对无人机节点间相对位置、相对速度、相对加速度、相对四元数的预测，最后利用无人机节点间的相对测距信息作为量测量实现无人机编队节点间相对导航。本发明不仅仅能够得到编队中无人机节点间高精度的相对导航信息同时能够提高无人机节点机载惯导系统的位置输出精度。

本发明为解决上述技术问题采用以下技术方案：

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

步骤一：无人机节点搭载惯性导航系统，数据链收发装置，周期性读取k时刻无人机节点传感器数据，包括陀螺仪信息

加速度计信息

其中 i＝7,8…,N以及无人机节点间相对距离d_ut，其中u,t表示不同的无人机节点；

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

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

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

步骤二中所述无人机节点机体坐标系及导航坐标系定义如下：机体坐标系的 X、Y、Z轴分别为无人机的右向、前向和上向，导航坐标系分别为东、北、天地理坐标系。

步骤二中采取如下方式建立无人机节点u,t之间的相对位置运动方程、相对速度误差方程、相对姿态四元数误差方程，陀螺噪声误差方程，加速度计噪声误差方程：

1)无人机节点u,t之间的相对位置运动方程

其中

表示无人机节点u机体坐标系相对于无人机节点t的机体坐标系的位置在无人机u坐标系下的投影，可表示为：

其中

为惯性坐标系到无人机节点u机体系的坐标转换矩阵；

为无人机节点u机体坐标系相对惯性坐标系的位置在惯性坐标系下的投影；

为无人机节点t机体坐标系相对惯性坐标系下的位置在惯性坐标系下的投影；

为无人机节点u机体系相对于惯性系的角速度在机体系上的投影；

为无人机节点u机体系下的比力；

为无人机节点t机体系下的比力；

为无人机节点t机体系到无人机节点u机体系的坐标转换矩阵；

为无人机节点u机体系相对惯性系的角速率在机体系下的投影；

为无人机节点u相对无人机节点t的速度在无人机节点u机体下的投影；

为无人机节点u相对无人机节点t的加速度在无人机节点u机体坐标系下的投影；

2)无人机节点u,t之间的相对速度误差方程如下式表示：

其中

为无人机节点u机体系相对惯性系的角速率估计值在机体系下的投影；

为无人机节点u机体系相对惯性系角速度估计值在机体下的投影；

为陀螺随机常数误差，

为陀螺一阶马尔科夫过程白噪声，

为无人机节点u,t之间的相对位置误差在无人机节点u机体系下的投影；

为无人机节点t到无人机节点u姿态转换矩阵的估计值；

为无人机节点u机体坐标系相对惯性坐标系的角速度的估计值在无人机节点u机体下的投影；

为无人机节点u,t之间的相对距离估计值在无人机节点u机体系下的投影；J_u表示无人机节点u的惯量矩阵；

为节点u惯量矩阵的逆矩阵；

表示为无人机节点u,t之间的相对速度误差在无人机节点u机体系下的投影；δε_ra表示为零均值的加速度计一阶马尔科夫过程白噪声；w_a为零均值的加速度计白噪声；

3)相对姿态四元数误差方程如下式表示：

其中

为无人机节点u相对无人机节点t的角速度估计值在无人机节点u机体坐标系下的投影；δq_rel为无人机节点间的相对四元数；δε_rg为陀螺一阶马尔科夫过程白噪声；δε_bg为陀螺随机常数；w_g为陀螺零均值的白噪声；

4)惯导噪声误差方程；

陀螺的噪声主要考虑陀螺随机常数、陀螺一阶马尔科夫驱动白噪声、陀螺白噪声，即：

ε＝ε_bg+ε_rg+w_g

其中ε_bg为陀螺的随机常数，ε_rg为陀螺一阶马尔科夫过程随机噪声，w_g为陀螺白噪声；

考虑无人机节点机载三轴陀螺误差类型相同，则陀螺随机常数误差数学表达式表示为：

陀螺一阶马尔科夫驱动白噪声误差数学表示式表示为：

其中ε_rg为陀螺一阶马尔科夫过程随机噪声，T_g为陀螺相关时间常数，w_rg为陀螺一阶马尔科夫过程驱动白噪声；

加速度计噪声误差方程

加速度计的噪声误差仅考虑一阶马尔科夫过程，同样假定加速度计三轴的误差模型相同，则加速度计噪声误差方程表示为：

其中▽_ra为加速度计一阶马尔科夫过程，T_a为加速度计相关时间常数，w_ag为加速度计一阶马尔科夫过程驱动白噪声。

步骤三的具体过程如下：

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)前三个最大特征值所对应的特征向量。

步骤四的具体过程如下：

卡尔曼滤波滤波时间更新以及量测更新过程如下所示：

状态一步预测方程：

状态估值方程：

滤波增益方程：

一步预测均方误差方程：

估计均方误差方程：

上式中Φ_k,k-1为系统k-1到k时刻状态转移矩阵；

为系统k-1到k时刻状态转移矩阵的转置；

为系统k-1时刻状态估计值；

为k时刻状态估计值；Z_k为k时刻系统量测值；P_k/k-1为k-1时刻到k时刻预测协方差；P_k-1为k-1时刻系统协方差；P_k/k为k时刻估计协方差；

为k时刻滤波增益的转置；K_k为系统k时刻滤波增益；H_k为系统k时刻量测矩阵；Q_k-1为系统k-1时刻系统噪声阵；Γ_k,k-1为系统k-1到k时刻系统输入矩阵； R_k为系统k时刻量测噪声矩阵；

为k时刻量测矩阵转置；

为系统k-1到k时刻系统输入矩阵的转置。

本发明的有益效果如下：

在本发明中，无人机编队各无人机节点搭载惯导系统及机载数据链收发装置，利用机载数据链实时得到无人机节点间的相对距离信息作为量测量，在精确相对导航建模的基础上实现无人机节点间高精度相对导航，解决GPS拒止环境下无人机编队相对导航问题。本发明所提出的方法不仅仅能够得到无人机节点高精度相对导航信息，而且还能够一定程度上抑制各无人机节点机载惯导系统发散。

附图说明

图1为基于惯导/数据链的无人机编队相对导航算法流程图。

图2为无人机节点1与无人机节点2之间的相对距离误差图。

图3为无人机节点1与无人机节点3之间的相对距离误差图。

图4为无人机节点1与无人机节点4之间的相对距离误差图。

图5为无人机节点1与无人机节点5之间的相对距离误差图。

图6为无人机节点1与无人机节点6之间的相对距离误差图。

图7为无人机节点1与无人机节点7之间的相对距离误差图。

具体实施方式

下面结合附图对本发明创造做进一步详细说明。

本发明方法的总体框架图如图1所示，其具体步骤如下：

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

步骤一：无人机节点搭载惯性导航系统，数据链收发装置。周期性读取k刻无人机节点传感器数据，包括陀螺仪信息

加速度计信息

其中i＝7,8…,N以及无人机节点间相对距离d_ut，其中u, t示不同的无人机节点。

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

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

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

相关坐标系定义为：机体系的X、Y、Z轴分别为无人机的右向、前向(无人机机头方向)、上向，导航坐标系分别为东、北、天地理坐标系。

步骤二中采取如下方式解算得到无人机节点u,t之间的相对位置运动方程、相对速度误差方程、相对姿态四元数误差方程，陀螺噪声误差方程，加速度计噪声误差方程。

1)无人机节点u,t之间的相对位置运动方程

其中

表示无人机节点u机体坐标系相对于无人机节点t的机体坐标系的位置在无人机u坐标系下的投影，可表示为：

其中

为惯性坐标系到无人机节点u机体系的坐标转换矩阵；

为无人机节点u机体坐标系相对惯性坐标系的位置在惯性坐标系下的投影；

为无人机节点t机体坐标系相对惯性坐标系下的位置在惯性坐标系下的投影；

为无人机节点u机体系相对于惯性系的角速度在机体系上的投影；

为无人机节点u机体系下的比力；

为无人机节点t机体系下的比力；

为无人机节点t机体系到无人机节点u机体系的坐标转换矩阵；

为无人机节点u机体系相对惯性系的角速率在机体系下的投影；

为无人机节点u相对无人机节点t的速度在无人机节点u机体下的投影；

为无人机节点u相对无人机节点t的加速度在无人机节点u机体坐标系下的投影；

无人机节点u,t之间的相对速度误差方程

无人机节点u,t之间的相对速度误差方程可用如下式表示：

其中

为无人机节点u机体系相对惯性系的角速率估计值在机体系下的投影；

为无人机节点u机体系相对惯性系角速度估计值在机体下的投影；

为陀螺随机常数误差，

为陀螺一阶马尔科夫过程白噪声，

为无人机节点u,t之间的相对位置误差在无人机节点u机体系下的投影；

为无人机节点t到无人机节点u姿态转换矩阵的估计值；

为无人机节点u机体坐标系相对惯性坐标系的角速度的估计值在无人机节点u机体下的投影；

为无人机节点u,t之间的相对距离估计值在无人机节点u机体系下的投影；J_u表示无人机节点u的惯量矩阵；

为节点u惯量矩阵的逆矩阵；

表示为无人机节点u,t之间的相对速度误差在无人机节点u机体系下的投影；δε_ra表示为零均值的加速度计一阶马尔科夫过程白噪声；w_a为零均值的加速度计白噪声。

2)相对姿态四元数误差方程

相对姿态四元数误差方程可用如下式表示：

其中

为无人机节点u相对无人机节点t的角速度估计值在无人机节点u机体坐标系下的投影；δq_rel为无人机节点间的相对四元数；δε_rg为陀螺一阶马尔科夫过程白噪声；δε_bg为陀螺随机常数；w_g为陀螺零均值的白噪声。

3)惯导噪声误差方程；

陀螺噪声误差方程

陀螺的噪声这里主要考虑陀螺随机常数、陀螺一阶马尔科夫驱动白噪声、陀螺白噪声。即：

ε＝ε_bg+ε_rg+w_g

其中ε_bg为陀螺的随机常数，ε_rg为陀螺一阶马尔科夫过程随机噪声，w_g为陀螺白噪声。

考虑无人机节点机载三轴陀螺误差类型相同，则陀螺随机常数误差数学表达式可表示为：

陀螺一阶马尔科夫驱动白噪声误差数学表示式可表示为：

其中ε_rg为陀螺一阶马尔科夫过程随机噪声，T_g为陀螺相关时间常数，w_rg为陀螺一阶马尔科夫过程驱动白噪声。

加速度计噪声误差方程

加速度计的噪声误差这里仅考虑一阶马尔科夫过程，同样假定加速度计三轴的误差模型相同，则加速度计噪声误差方程可表示为：

其中▽_ra为加速度计一阶马尔科夫过程，T_a为加速度计相关时间常数，w_ag为加速度计一阶马尔科夫过程驱动白噪声。

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

1)根据无人机节点惯导输出构成的惯导多边形与机载数据链相对测距构成的量测多边形利用多边形重心一致性得到双中心转换矩阵B(t_k)，可表示为：

上式中D(t_k)为无人机节点相对测距构成的相对距离矩阵

其d_ut表示无人机节点u与无人机节点t相对距离；

其中I为n维单位向量，K为n维向量的元素。

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

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。

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

由于无人机初始位置坐标为三维坐标，选取双中心变换矩阵B(t_k) 前三个最大的特征值，则构成的对角矩阵可表示为：Λ₃(t_k)＝diag(λ₁,λ₂,λ₃)，特征值所对应的特征向量为V_P(t_k)＝[ν₁,ν₂,ν₃]，其中ν₁为特征值λ₁对应的特征向量，ν₂为特征值λ₂对应的特征向量，ν₃为特征值λ₃对应的特征向量，则各无人机节点的初始坐标可由下式计算得到：

其中n≥7。

其中：

为无人机节点1k时刻三维初始位置；

为无人机节点2k时刻三维初始位置；

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

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

卡尔曼滤波滤波时间更新以及量测更新过程如下所示：

状态一步预测方程：

状态估值方程：

滤波增益方程：

一步预测均方误差方程：

估计均方误差方程：

上式中Φ_k,k-1为系统k-1到k时刻状态转移矩阵；

为系统k-1到k时刻状态转移矩阵的转置；

为系统k-1时刻状态估计值；

为k时刻状态估计值；P_k/k-1为k-1时刻到k时刻预测协方差；P_k-1为k-1时刻系统协方差；P_k/k为k时刻估计协方差；

为k时刻滤波增益的转置；K_k为系统k 时刻滤波增益；H_k为系统k时刻量测矩阵；Q_k-1为系统k-1时刻系统噪声阵；Γ_k,k-1为系统k-1到k时刻系统输入矩阵；R_k为系统k时刻量测噪声矩阵；

为k时刻量测矩阵转置；

为系统k-1到k时刻系统输入矩阵的转置；

表1无人机节点机载纯惯导递推与基于构型的无人机节点位置1h发散程度对比

由图2,3,4,5,6,7可知无人机编队中系统稳态时无人机节点1相对其它无人机节点相对距离误差保持在3米以内，其它无人机节点与无人机节点1类似。由表1可知通过本发明提出的算法各无人机节点机载惯导系统位置输出与机载惯导系统纯惯导递推位置输出精度提高了至少30％以上，有效验证了本发明所提出算法的有效性及可行性。

Claims (5)

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

步骤一：无人机节点搭载惯性导航系统，数据链收发装置，周期性读取k时刻无人机节点传感器数据，包括陀螺仪信息

加速度计信息

其中i＝7,8…,N以及无人机节点间相对距离d_ut，其中u,t表示不同的无人机节点；

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

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

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

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

3.根据权利要求1所述的一种基于惯性/数据链的无人机编队相对导航方法，其特征在于步骤二中采取如下方式建立无人机节点u,t之间的相对位置运动方程、相对速度误差方程、相对姿态四元数误差方程，陀螺噪声误差方程，加速度计噪声误差方程：

1)无人机节点u,t之间的相对位置运动方程

其中

表示无人机节点u机体坐标系相对于无人机节点t的机体坐标系的位置在无人机u坐标系下的投影，表示为：

其中

为惯性坐标系到无人机节点u机体系的坐标转换矩阵；

为无人机节点u机体坐标系相对惯性坐标系的位置在惯性坐标系下的投影；

为无人机节点t机体坐标系相对惯性坐标系下的位置在惯性坐标系下的投影；

为无人机节点u机体系相对于惯性系的角速度在机体系上的投影；

为无人机节点u机体系下的比力；

为无人机节点t机体系下的比力；

为无人机节点t机体系到无人机节点u机体系的坐标转换矩阵；

为无人机节点u机体系相对惯性系的角速率在机体系下的投影；

为无人机节点u相对无人机节点t的速度在无人机节点u机体下的投影；

为无人机节点u相对无人机节点t的加速度在无人机节点u机体坐标系下的投影；

2)无人机节点u,t之间的相对速度误差方程如下式表示：

其中

为无人机节点u机体系相对惯性系的角速率估计值在机体系下的投影；

为无人机节点u机体系相对惯性系角速度估计值在机体下的投影；

为陀螺随机常数误差，

为陀螺一阶马尔科夫过程白噪声，

为无人机节点u,t之间的相对位置误差在无人机节点u机体系下的投影；

为无人机节点t到无人机节点u姿态转换矩阵的估计值；

为无人机节点u机体坐标系相对惯性坐标系的角速度的估计值在无人机节点u机体下的投影；

为无人机节点u,t之间的相对距离估计值在无人机节点u机体系下的投影；J_u表示无人机节点u的惯量矩阵；

为节点u惯量矩阵的逆矩阵；

表示为无人机节点u,t之间的相对速度误差在无人机节点u机体系下的投影；δε_ra表示为零均值的加速度计一阶马尔科夫过程白噪声；w_a为零均值的加速度计白噪声；

3)相对姿态四元数误差方程如下式表示：

其中

为无人机节点u相对无人机节点t的角速度估计值在无人机节点u机体坐标系下的投影；δq_rel为无人机节点间的相对四元数；δε_rg为陀螺一阶马尔科夫过程白噪声；δε_bg为陀螺随机常数；w_g为陀螺零均值的白噪声；

4)惯导噪声误差方程；

陀螺的噪声主要考虑陀螺随机常数、陀螺一阶马尔科夫驱动白噪声、陀螺白噪声，即：

ε＝ε_bg+ε_rg+w_g

其中ε_bg为陀螺的随机常数，ε_rg为陀螺一阶马尔科夫过程随机噪声，w_g为陀螺白噪声；

考虑无人机节点机载三轴陀螺误差类型相同，则陀螺随机常数误差数学表达式表示为：

陀螺一阶马尔科夫驱动白噪声误差数学表示式表示为：

其中ε_rg为陀螺一阶马尔科夫过程随机噪声，T_g为陀螺相关时间常数，w_rg为陀螺一阶马尔科夫过程驱动白噪声；

加速度计噪声误差方程

加速度计的噪声误差仅考虑一阶马尔科夫过程，同样假定加速度计三轴的误差模型相同，则加速度计噪声误差方程表示为：

其中▽_ra为加速度计一阶马尔科夫过程，T_a为加速度计相关时间常数，w_ag为加速度计一阶马尔科夫过程驱动白噪声。

4.根据权利要求1所述的一种基于惯性/数据链的无人机编队相对导航方法，其特征在于，步骤三的具体过程如下：

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)前三个最大特征值所对应的特征向量。

5.根据权利要求1所述的一种基于惯性/数据链的无人机编队相对导航方法，其特征在于，步骤四的具体过程如下：

卡尔曼滤波滤波时间更新以及量测更新过程如下所示：

状态一步预测方程：

状态估值方程：

滤波增益方程：

一步预测均方误差方程：

估计均方误差方程：

上式中Φ_k,k-1为系统k-1到k时刻状态转移矩阵；

为系统k-1到k时刻状态转移矩阵的转置；

为系统k-1时刻状态估计值；

为k时刻状态估计值；Z_k为k时刻系统量测值；P_k/k-1为k-1时刻到k时刻预测协方差；P_k-1为k-1时刻系统协方差；P_k/k为k时刻估计协方差；

为k时刻滤波增益的转置；K_k为系统k时刻滤波增益；H_k为系统k时刻量测矩阵；Q_k-1为系统k-1时刻系统噪声阵；Γ_k,k-1为系统k-1到k时刻系统输入矩阵；R_k为系统k时刻量测噪声矩阵；

为k时刻量测矩阵转置；

为系统k-1到k时刻系统输入矩阵的转置。

Images