本发明涉及一种基于惯性/数据链的无人机编队相对导航方法，属于编队无人机相对导航技术领域。
背景技术：
无人机在现代战争中主要用于军事侦察、战略打击等任务，传统的单一无人机在执行任务时受载体承重、成本等条件的限制，同时单一的无人机在执行任务时抗毁伤能力不足，已无法满足现阶段作战需求。编队无人机在此背景下应运而生，为了提高无人机编队作战能力，无人机间应具有高精度相对导航能力就显得尤为重要。同时无人机在战场环境下容易受环境、电磁干扰等影响，gps在战场环境不可用，迫切需要研究gps卫星拒止环境下无人机编队相对导航方法。
现阶段编队无人机相对导航一般采用gps卫星、测角/测距传感器辅助来实现。然而在战场环境下gps信号易受到干扰而导致不可用，编队无人机需要大量搭载测距/测角传感器增加了项目成本，同时现阶段测距/测角设备主要用视觉/雷达传感器来实现，战场环境特征复杂对现有视觉/雷达匹配算法的实时性、可靠性要求较高，在实际情况下很难得到应用。
技术实现要素：
本发明提出了一种基于惯性/数据链的无人机编队相对导航方法，首先通过无人机节点机载惯导输出构成的惯导多边形与相对测距量测多边形根据双中心转换法得到无人机编队各无人机节点初始位置坐标；然后通过建立相对导航状态方程完成对无人机节点间相对位置、相对速度、相对加速度、相对四元数的预测，最后利用无人机节点间的相对测距信息作为量测量实现无人机编队节点间相对导航。本发明不仅仅能够得到编队中无人机节点间高精度的相对导航信息同时能够提高无人机节点机载惯导系统的位置输出精度。
本发明为解决上述技术问题采用以下技术方案：
一种基于惯性/数据链的无人机编队相对导航方法，包括如下步骤：
步骤一：无人机节点搭载惯性导航系统，数据链收发装置，周期性读取k时刻无人机节点传感器数据，包括陀螺仪信息
步骤二：定义无人机节点机体坐标系及导航坐标系并建立无人机节点u,t之间相对位置、相对速度、相对姿态四元数误差方程、陀螺噪声误差方程，加速度计噪声误差方程；
步骤三：利用无人机节点惯导输出构成的惯导多边型与机载数据链相对测距构成的量测多边形计算k时刻编队中各无人机节点初始位置坐标；
步骤四：利用卡尔曼滤波器对k时刻无人机节点间的相对位置信息进行校正。
步骤二中所述无人机节点机体坐标系及导航坐标系定义如下：机体坐标系的x、y、z轴分别为无人机的右向、前向和上向，导航坐标系分别为东、北、天地理坐标系。
步骤二中采取如下方式建立无人机节点u,t之间的相对位置运动方程、相对速度误差方程、相对姿态四元数误差方程，陀螺噪声误差方程，加速度计噪声误差方程：
1)无人机节点u,t之间的相对位置运动方程
其中
2)无人机节点u,t之间的相对速度误差方程如下式表示：
其中
3)相对姿态四元数误差方程如下式表示：
其中
4)惯导噪声误差方程；
陀螺的噪声主要考虑陀螺随机常数、陀螺一阶马尔科夫驱动白噪声、陀螺白噪声，即：
ε＝εbg εrg wg
其中εbg为陀螺的随机常数，εrg为陀螺一阶马尔科夫过程随机噪声，wg为陀螺白噪声；
考虑无人机节点机载三轴陀螺误差类型相同，则陀螺随机常数误差数学表达式表示为：
陀螺一阶马尔科夫驱动白噪声误差数学表示式表示为：
其中εrg为陀螺一阶马尔科夫过程随机噪声，tg为陀螺相关时间常数，wrg为陀螺一阶马尔科夫过程驱动白噪声；
加速度计噪声误差方程
加速度计的噪声误差仅考虑一阶马尔科夫过程，同样假定加速度计三轴的误差模型相同，则加速度计噪声误差方程表示为：
其中▽ra为加速度计一阶马尔科夫过程，ta为加速度计相关时间常数，wag为加速度计一阶马尔科夫过程驱动白噪声。
步骤三的具体过程如下：
a)解算获得惯导多边形与量测多边形双中心转换矩阵b(tk)，表示为：
上式中d(tk)为无人机节点相对测距构成的相对距离矩阵
b)解算双中心转换矩阵的特征值和特征向量
b(tk)＝v(tk)λ(tk)v(tk)t
其中λ(tk)＝diag(λ1,λ2,...λn)为矩阵b(tk)的特征值对应的对角矩阵，λ1,λ2,...λn矩阵b(tk)的特征值，v(tk)＝[ν1,ν2,...,νn]为特征值所对应的特征向量，其中λ1特征值对应的特征向量为ν1，λ2特征值对应的特征向量为ν2，λn特征值对应的特征向量为νn，且λ1≥λ2≥…≥λn≥0；
c)解算编队中各无人机节点初始位置坐标：
各无人机节点的初始坐标由下式计算得到：
其中：
步骤四的具体过程如下：
卡尔曼滤波滤波时间更新以及量测更新过程如下所示：
状态一步预测方程：
状态估值方程：
滤波增益方程：
一步预测均方误差方程：
估计均方误差方程：
上式中φk,k-1为系统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刻无人机节点传感器数据，包括陀螺仪信息
步骤二：定义无人机节点机体坐标系及导航坐标系并建立无人机节点u,t间相对位置、相对速度、相对姿态四元数误差方程、陀螺噪声误差方程，加速度计噪声误差方程。
步骤三：利用无人机节点惯导输出构成的惯导多边型与机载数据链相对测距构成的量测多边形计算k时刻编队中各无人机节点初始位置坐标。
步骤四：利用卡尔曼滤波器对k时刻无人机节点间的相对位置信息进行校正。
相关坐标系定义为：机体系的x、y、z轴分别为无人机的右向、前向(无人机机头方向)、上向，导航坐标系分别为东、北、天地理坐标系。
步骤二中采取如下方式解算得到无人机节点u,t之间的相对位置运动方程、相对速度误差方程、相对姿态四元数误差方程，陀螺噪声误差方程，加速度计噪声误差方程。
1)无人机节点u,t之间的相对位置运动方程
其中
无人机节点u,t之间的相对速度误差方程
无人机节点u,t之间的相对速度误差方程可用如下式表示：
其中
2)相对姿态四元数误差方程
相对姿态四元数误差方程可用如下式表示：
其中
3)惯导噪声误差方程；
陀螺噪声误差方程
陀螺的噪声这里主要考虑陀螺随机常数、陀螺一阶马尔科夫驱动白噪声、陀螺白噪声。即：
ε＝εbg εrg wg
其中εbg为陀螺的随机常数，εrg为陀螺一阶马尔科夫过程随机噪声，wg为陀螺白噪声。
考虑无人机节点机载三轴陀螺误差类型相同，则陀螺随机常数误差数学表达式可表示为：
陀螺一阶马尔科夫驱动白噪声误差数学表示式可表示为：
其中εrg为陀螺一阶马尔科夫过程随机噪声，tg为陀螺相关时间常数，wrg为陀螺一阶马尔科夫过程驱动白噪声。
加速度计噪声误差方程
加速度计的噪声误差这里仅考虑一阶马尔科夫过程，同样假定加速度计三轴的误差模型相同，则加速度计噪声误差方程可表示为：
其中▽ra为加速度计一阶马尔科夫过程，ta为加速度计相关时间常数，wag为加速度计一阶马尔科夫过程驱动白噪声。
步骤三利用无人机节点惯导输出构成的惯导多边形与机载数据链相对测距构成的量测多边形计算k时刻编队中各无人机节点的初始位置坐标。
1)根据无人机节点惯导输出构成的惯导多边形与机载数据链相对测距构成的量测多边形利用多边形重心一致性得到双中心转换矩阵b(tk)，可表示为：
上式中d(tk)为无人机节点相对测距构成的相对距离矩阵
2)解算双中心转换矩阵的特征值和特征向量
b(tk)＝v(tk)λ(tk)v(tk)t
其中λ(tk)＝diag(λ1,λ2,...λn)为矩阵b(tk)的特征值对应的对角矩阵，λ1,λ2,...λn矩阵b(tk)的特征值，v(tk)＝[ν1,ν2,...,νn]为特征值所对应的特征向量，其中λ1特征值对应的特征向量为ν1，λ2特征值对应的特征向量为ν2，λn特征值对应的特征向量为νn，且λ1≥λ2≥...≥λn≥0。
3)解算编队中各无人机节点初始位置坐标：
由于无人机初始位置坐标为三维坐标，选取双中心变换矩阵b(tk)前三个最大的特征值，则构成的对角矩阵可表示为：λ3(tk)＝diag(λ1,λ2,λ3)，特征值所对应的特征向量为vp(tk)＝[ν1,ν2,ν3]，其中ν1为特征值λ1对应的特征向量，ν2为特征值λ2对应的特征向量，ν3为特征值λ3对应的特征向量，则各无人机节点的初始坐标可由下式计算得到：
其中：
步骤四中利用卡尔曼滤波器对k时刻无人机节点间的相对位置信息进行校正。
卡尔曼滤波滤波时间更新以及量测更新过程如下所示：
状态一步预测方程：
状态估值方程：
滤波增益方程：
一步预测均方误差方程：
估计均方误差方程：
上式中φk,k-1为系统k-1到k时刻状态转移矩阵；
表1无人机节点机载纯惯导递推与基于构型的无人机节点位置1h发散程度对比
由图2,3,4,5,6,7可知无人机编队中系统稳态时无人机节点1相对其它无人机节点相对距离误差保持在3米以内，其它无人机节点与无人机节点1类似。由表1可知通过本发明提出的算法各无人机节点机载惯导系统位置输出与机载惯导系统纯惯导递推位置输出精度提高了至少30％以上，有效验证了本发明所提出算法的有效性及可行性。
1.一种基于惯性/数据链的无人机编队相对导航方法，其特征在于，包括如下步骤：
步骤一：无人机节点搭载惯性导航系统，数据链收发装置，周期性读取k时刻无人机节点传感器数据，包括陀螺仪信息
步骤二：定义无人机节点机体坐标系及导航坐标系并建立无人机节点u,t之间相对位置、相对速度、相对姿态四元数误差方程、陀螺噪声误差方程，加速度计噪声误差方程；
步骤三：利用无人机节点惯导输出构成的惯导多边型与机载数据链相对测距构成的量测多边形计算k时刻编队中各无人机节点初始位置坐标；
步骤四：利用卡尔曼滤波器对k时刻无人机节点间的相对位置信息进行校正。
2.根据权利要求1所述的一种基于惯性/数据链的无人机编队相对导航方法，其特征在于步骤二中所述无人机节点机体坐标系及导航坐标系定义如下：机体坐标系的x、y、z轴分别为无人机的右向、前向和上向，导航坐标系分别为东、北、天地理坐标系。
3.根据权利要求1所述的一种基于惯性/数据链的无人机编队相对导航方法，其特征在于步骤二中采取如下方式建立无人机节点u,t之间的相对位置运动方程、相对速度误差方程、相对姿态四元数误差方程，陀螺噪声误差方程，加速度计噪声误差方程：
1)无人机节点u,t之间的相对位置运动方程
其中
2)无人机节点u,t之间的相对速度误差方程如下式表示：
其中
3)相对姿态四元数误差方程如下式表示：
其中
4)惯导噪声误差方程；
陀螺的噪声主要考虑陀螺随机常数、陀螺一阶马尔科夫驱动白噪声、陀螺白噪声，即：
ε＝εbg εrg wg
其中εbg为陀螺的随机常数，εrg为陀螺一阶马尔科夫过程随机噪声，wg为陀螺白噪声；
考虑无人机节点机载三轴陀螺误差类型相同，则陀螺随机常数误差数学表达式表示为：
陀螺一阶马尔科夫驱动白噪声误差数学表示式表示为：
其中εrg为陀螺一阶马尔科夫过程随机噪声，tg为陀螺相关时间常数，wrg为陀螺一阶马尔科夫过程驱动白噪声；
加速度计噪声误差方程
加速度计的噪声误差仅考虑一阶马尔科夫过程，同样假定加速度计三轴的误差模型相同，则加速度计噪声误差方程表示为：
其中▽ra为加速度计一阶马尔科夫过程，ta为加速度计相关时间常数，wag为加速度计一阶马尔科夫过程驱动白噪声。
4.根据权利要求1所述的一种基于惯性/数据链的无人机编队相对导航方法，其特征在于，步骤三的具体过程如下：
a)解算获得惯导多边形与量测多边形双中心转换矩阵b(tk)，表示为：
上式中d(tk)为无人机节点相对测距构成的相对距离矩阵
b)解算双中心转换矩阵的特征值和特征向量
b(tk)＝v(tk)λ(tk)v(tk)t
其中λ(tk)＝diag(λ1,λ2,...λn)为矩阵b(tk)的特征值对应的对角矩阵，λ1,λ2,...λn矩阵b(tk)的特征值，v(tk)＝[ν1,ν2,...,νn]为特征值所对应的特征向量，其中λ1特征值对应的特征向量为ν1，λ2特征值对应的特征向量为ν2，λn特征值对应的特征向量为νn，且λ1≥λ2≥...≥λn≥0；
c)解算编队中各无人机节点初始位置坐标：
各无人机节点的初始坐标由下式计算得到：
其中：
5.根据权利要求1所述的一种基于惯性/数据链的无人机编队相对导航方法，其特征在于，步骤四的具体过程如下：
卡尔曼滤波滤波时间更新以及量测更新过程如下所示：
状态一步预测方程：
状态估值方程：
滤波增益方程：
一步预测均方误差方程：
估计均方误差方程：
上式中φk,k-1为系统k-1到k时刻状态转移矩阵；
