技术领域
[0001] 本
发明属于多
飞行器协同编队飞行技术领域，具体涉及多机协同编队飞行的分布式相对导航方法。技术背景
[0002] 多架飞行器为适应任务要求而进行某种队形排列和任务分配，包括航迹规划、编队设计、相对导航、编队控制及编队重构方面。在机群协同编队飞行中，在线实时地对各成员之间相对
位置及各成员相对及绝对位置进行精确的估计是编队控制、队形保持的先决条件，因此
导航系统作为编队飞行中各成员相对及绝对信息获取的一个重要途径受到了广泛的关注。
[0003] GPS是美国国防部为海、陆、空研制的卫星导航
定位系统，具有全球性、全天候、三维定位等优点，但其在动态环境中可靠性差，易受地形物遮挡而定位中断，
数据采集频率低，并且是非自主式系统。SINS(捷联惯性导航系统)是现在较为常用的惯性导航方法，它利用惯性元件来测量运动载体的
加速度经运算来求出导航参数，它完全自主，不受外界环境的干扰影响，无
信号丢失，多功能，它的导航
精度主要取决于
陀螺仪和
加速度计，但传统的惯性器件普遍体积较大，成本较高，而MEMS惯性
传感器具有体积小、成本低、重量轻的特点，但其系统精度较低，若直接使用系统误差很大，所以无法单独工作。
[0004] 现代导航技术，已经发展为一种由多种类型传感器组合成的组合导航系统，组合导航系统通过多种传感器的配合，运用数据融合
算法，克服了单一传感器不确定性与不可靠性的缺点。
发明内容
[0005] 本发明采用分布式滤波结构，长机和僚机，均采用GPS/INS组合导航系统，通过对长机和僚机的INS数据和GPS数据进行卡尔曼滤波处理，得到各自经GPS校正后的INS输出数据，然后再进行两者相对距离、相对位置的计算，提高各自输出数据的准确性。
[0006] 本发明的原理是：长机的INS数据和GPS数据进行卡尔曼滤波处理，得到长机经GPS校正后的INS输出数据，再将长机校正后INS输出通过数据链传递给僚机；同样，僚机的INS数据和GPS数据进行卡尔曼滤波处理，得到僚机经GPS校正后的INS输出数据；利用长机经GPS校正后的INS输出数据和僚机经GPS校正后的INS输出数据，计算两者的相对距离、相对位置。得到的相对距离、相对位置信息可实时提供给多机协同编队飞行系统，为实现编队集结、队形保持和重构等功能提供更加准确的数据来源。
[0007] 本发明的技术方案为面向多个飞机协同编队飞行的分布式相对导航方法，所述多个飞机包括至少一个长机和至少一个僚机，且均采用组合导航系统进行导航；所述分布式相对导航系统包括MEMS-SINS导航系统和GPS导航系统；
[0008] 所述长机的MEMS-SINS导航系统和GPS导航系统分别输出长机的INS数据和GPS数据，对所述长机的INS数据和GPS数据进行卡尔曼滤波处理，得到长机经GPS校正后的INS输出数据，并通过数据链反馈给僚机；
[0009] 所述僚机的MEMS-SINS导航系统和GPS导航系统分别输出僚机的INS数据和GPS数据，对所述僚机的INS数据和GPS数据进行卡尔曼滤波处理，得到僚机经GPS校正后的INS输出数据；
[0010] 利用长机经GPS校正后的INS输出数据和僚机经GPS校正后的INS输出数据进行相对导航计算，得到长机和僚机的相对距离、相对位置；
[0011] 将所述长机和僚机的相对距离和相对位置实时提供给多机协同编队飞行系统。
[0012] 进一步地，对长机、僚机的INS数据和GPS数据进行卡尔曼滤波处理，处理步骤均包括，
[0013] 建立组合导航系统中MEMS-SINS导航系统和GPS导航系统各自的误差模型，根据所述误差模型选取MEMS-SINS导航系统和GPS导航系统各自的状态量，并非确定组合导航系统的状态量矩阵，根据所述状态量矩阵，计算得到长机、僚机各自的组合导航系统的
状态方程；
[0014] 根据长机、僚机的组合导航系统的INS数据和GPS数据，获取长机、僚机各自的组合导航系统的量测量矩阵，根据所述量测量矩阵，计算得到长机、僚机各自的组合导航系统的量测方程；
[0015] 对长机、僚机各自的组合导航系统的状态方程和量测方程进行离散化处理，得到离散化方程，并应用离散时间利用卡尔曼滤波算法去估计组合导航系统的各种误差状态，并用误差状态的估计值去校正INS数据，以得到校正后的长机、僚机各自的INS输出数据。
[0016] 进一步地，所述状态量的选取至少包括数学平台失准
角，载体东向、北向、天向的速度误差，纬度误差、经度误差、高度误差，陀螺仪常值漂移误差、陀螺仪相关误差、加速度计系统误差中的一种或多种。
[0017] 进一步地，所述状态方程为
[0018]
[0019] 其中，X(t)表示惯导系统t时刻下导航系统状态向量；W(t)表示t时刻下的噪声向量；F(t)表示t时刻下状态转移矩阵；G(t)表示t时刻下系统噪声转移矩阵； 表示t时刻下状态导数向量。
[0020] 进一步地，所述长机的INS数据包括MEMS-SINS导航系统输出的长机的速度和位置；所述长机的GPS数据包括GPS导航系统输出的长机的速度和位置；
[0021] 将所述长机的MEMS-SINS导航系统输出的该长机的速度和位置与所述长机的GPS导航系统输出的该长机的速度和位置对应地分别进行做差，得到所述长机的组合导航系统的量测量矩阵；
[0022] 所述僚机的INS数据包括MEMS-SINS导航系统输出的僚机的速度和位置；所述僚机的GPS数据包括GPS导航系统输出的僚机的速度和位置；
[0023] 将所述僚机的MEMS-SINS导航系统输出的该僚机的速度和位置与所述僚机的GPS导航系统输出的该僚机的速度和位置对应地分别进行做差，得到所述僚机的组合导航系统的量测量矩阵。
[0024] 进一步地，长机、僚机的量测方程为，
[0025] Z(t)＝H(t)·X(t)+V(t)
[0026] 其中，H(t)表示组合导航系统t时刻下量测矩阵；X(t)表示t时刻下状态量；V(t)表示t时刻下量测噪声向量；Z(t)表示t时刻下量测向量。
[0027] 进一步地，对长机、僚机各自组合导航系统的状态方程和量测方程进行离散化处理，所述离散化方程为，
[0028]
[0029] 其中，Xk表示组合导航系统在k时刻的n维状态向量；Zk表示在k时刻的m维量测向量；Φk,k-1为k-1时刻到k时刻的系统状态转移矩阵；Hk表示k时刻的量测矩阵；Wk-1表示为k-1时刻的系统噪声向量；Γk-1为系统噪声矩阵，它表示由k-1时刻到k时刻的各个系统噪声分别影响k时刻各个状态的程度；Vk为k时刻的m维量测噪声向量。
[0030] 进一步地，所述卡尔曼滤波算法处理步骤还包括状态一步预测、状态估计、滤波增益、一步预测均方误差和估计均方误差。
[0031] 本发明的技术效果：
[0032] 本发明将GPS和MEMS-SINS二者组合，可以提高系统导航精度，增强分布式相对导航系统的抗干扰能
力；
[0033] 与只有INS导航的情况相比INS/GPS组合导航具有很好的
稳定性，可以减小位置和速度误差，具有较高的导航精度；
[0034] 通过数据链的引入，实现了长机/僚机之间的相对导航。
附图说明
[0035] 图1为本
实施例的长机、僚机的INS/GPS组合导航系统示意图；
[0036] 图2为本实施例的分布式相对导航系统示意图；
[0037] 图3为本实施例的卡尔曼滤波的两个计算回路。
具体实施方式
[0038] 本实施例，提供一种面向多个飞机协同编队飞行的分布式相对导航系统，多个飞机包括至少一个长机和至少一个僚机，且均采用组合导航系统进行导航。
[0039] 图2为本实施例的长机、僚机的INS/GPS组合导航系统；如图1所示，本实施例的组合导航系统包括MEMS-SINS导航系统、GPS导航系统、卡尔曼
滤波器和导航计算机。
[0040] MEMS-SINS导航系统使用MEMS传感器，通过MEMS传感器初始对准，获得MEMS-SINS导航系统的初始基准位置和速度信息。在系统启动阶段，通过磁罗盘进行
姿态角的初始对准，将陀螺数据通过四元数法递推得到系统的实时姿态角，并把加速度信息对时间进行二重积分，获得载体的绝对位置与绝对速度信息。MEMS-SINS导航系统对MEMS传感器获取的数据信息进行惯性导航解算，得到MEMS-SINS导航系统输出的INS数据。INS数据包括MEMS-SINS导航系统输出的载体(长机或僚机)的速度和位置。
[0041] GPS导航系统采用GPS接收机，然后进行GPS信号提取，得到GPS导航系统输出的GPS数据。GPS数据包括GPS导航系统输出的载体(长机或僚机)速度和位置。
[0042] 然后，利用导航计算机对载体(长机或僚机)的MEMS-SINS导航系统输出的INS数据和GPS导航系统输出的GPS数据进行计算，并利用卡尔曼滤波器进行卡尔曼滤波处理，得到载体(长机或僚机)经GPS校正后的INS输出数据。将长机经GPS校正后的INS输出数据和僚机经GPS校正后的INS输出数据进行做差，得到长机和僚机的相对距离和相对位置。将所述长机和僚机的相对距离和相对位置实时提供给多机协同编队飞行系统，为实现编队集结、队形保持和重构等功能提供更加准确的数据来源。
[0043] 图2为本实施例的分布式相对导航系统示意图。如图2所示，在得到长机经GPS校正后的INS输出数据后，通过数据链反馈给僚机。利用长机经GPS校正后的INS输出数据和僚机经GPS校正后的INS输出数据进行相对导航计算，得到长机和僚机的相对距离、相对位置。
[0044] 进一步地，本实施例，利用导航计算机对载体(长机或僚机)的MEMS-SINS导航系统输出的INS数据和GPS导航系统输出的GPS数据进行计算处理，具体处理步骤主要包括以下内容：
[0045] 步骤1：获取MEMS-SINS导航系统和GPS导航系统的状态方程
[0046] (1)建立MEMS-SINS导航系统和GPS导航系统各自的误差模型
[0047] 组合导航系统采用公共参考系为指北方位，传感器误差模型用东北天地理
坐标系。由捷联惯导算法原理可推导出系统的传感器误差模型。
[0048] 坐标系定义如下：i为惯性坐标系；n为导航坐标系，本文中采用东北天地理坐标系；b为惯导载体坐标系。
[0049] 在东北天地理坐标系，当考虑飞行高度h和地球为旋转椭球体时，有[0050]
[0051] 式中：
[0052] E、N、U分别代表东、北、天；
[0053] vE、vN、vU为飞行速度的分量；
[0054] φE、φN、φU为数学平台误差角的三轴分量；
[0055] εE、εN、εU为陀螺漂移的三轴分量；
[0056] 地球半径Re＝6378245m；
[0057] 子午圈半径RM＝Re(1-2f+3f sin2L)；
[0058] 卯酉圈半径RN＝Re(1+f sin2L)；
[0059] 地球扁率f＝1/298.257；
[0060] 地球自转
角速度ωie＝7.292×10-5rad/s；
[0061] δ表示误差符号；L表示纬度。
[0062] 在东北天地理坐标系下，当考虑飞行高度h和地球为旋转椭球体时：
[0063]
[0064] 式中， 为加速度计误差的三轴分量，fE、fU、fN为比力的三轴分量。在不考虑高度通道时，则有vU,δvU为零。
[0065] 在东北天地理坐标系下，当考虑飞行高度h和地球为旋转椭球体时，惯导输出的位置误差表示为
[0066]
[0067] 惯性仪表误差包括安装误差、刻度系数误差和随机误差。为简单起见，只考虑随机误差。
[0068] 式(1)中的陀螺漂移，是沿“东、北、天”地理坐标系的陀螺漂移。捷联式惯导系统，则需式(1)中的陀螺漂移为从
机体系变换到地理系的等效陀螺漂移。
[0069] 取陀螺漂移为
[0070] ε＝εb+εr+wg (4)
[0071] 式中，εb为随机常数；εr为一阶
马尔柯夫过程；wg为陀螺仪白噪声。
[0072] 假设三个轴向的陀螺漂移误差模型相同，均为
[0073]
[0074] 式中，Tr为相关时间；wr陀螺仪马尔柯夫噪声。
[0075] 考虑一阶马尔柯夫过程，且假设三个轴向加速度计的误差模型相同，均为[0076]
[0077] 式中，Ta为一阶马尔科夫相关时间； 为加速度计误差； 为加速度计误差导数；wa加速度计马尔柯夫噪声。
[0078] (2)选取组合导航系统的状态量，并计算得到状态方程
[0079] 根据步骤1确定的惯性导航系统的各误差模型，选取状态量，并确定状态量矩阵。
[0080] 综上，载机的惯导状态模型的状态量选取为
[0081]
[0082] 式中，各物理量含义为数学平台失准角，载体东向、北向、天向的速度误差，纬度误差、经度误差、高度误差，陀螺仪常值漂移误差，陀螺仪相关误差，加速度计系统误差；其中陀螺仪常值漂移误差，陀螺仪相关误差，加速度计系统误差各自的下标x、y、z，分别表示坐标轴三个方向。本实施例，采用的是导航坐标系即东北天地理坐标系下的分量(也就是之前所的东向、北向和天向)。
[0083] 系统噪声表示为
[0084] W＝[wgx wgy wgz wrx wry wrz wax way waz]T
[0085] 式中，各物理量含义为：陀螺仪白噪声，陀螺仪马尔柯夫噪声，加速度计马尔柯夫噪声。
[0086] 得到惯性导航系统在导航坐标系下的状态方程：
[0087]
[0088] 式中，X(t)表示惯导系统(MEMS-SINS导航系统)t时刻下导航系统状态向量；W(t)表示t时刻下系统噪声向量；F(t)表示t时刻下系统状态转移矩阵；G(t)表示t时刻下系统噪声转移矩阵； 表示t时刻下状态导数向量。
[0089] 系统噪声转移矩阵为G(t)，其中G和G(t)均表示噪声转移矩阵，G的表达式为[0090]
[0091] 其中，姿态转移矩阵 由轨迹信号中的姿态四元数或姿态角获得，即[0092]
[0093]
[0094] 系统状态转移矩阵F(t)，其中，F和F(t)均表示惯性导航系统状态转移矩阵，F表达式为：
[0095]
[0096] 其中，FS、FM、FN均为9x9方阵，且 步骤2：获取MEMS-SINS导航系统和GPS导航系统的量测方程
[0097] 根据载机的MEMS-SINS导航系统输出的速度和位置与载机的GPS导航系统输出的速度和位置信息做差，得到载机的组合导航系统的量测量矩阵，计算得到载机的组合导航系统的量测方程，表示为
[0098]
[0099] 式中，L，λ，h分别为沿东北天三个方向的位置和速度；上标INS和GPS表示不同导航系统。
[0100] 组合导航系统的量测方程为
[0101] Z(t)＝H(t)·X(t)+V(t) (10)
[0102] 式中，其中，H(t)表示连续系统t时刻下量测矩阵；X(t)表示连续系统t时刻下状态量；V(t)表示连续系统t时刻下量测噪声向量；Z(t)表示连续系统t时刻下量测向量。
[0103] H为量测矩阵，且H(1,7)＝1，H(2,8)＝1，H(3,9)＝1，H阵中的其余元素均为零。
[0104] 步骤3：对组合导航系统进行卡尔曼滤波算法处理
[0105] 对长机、僚机各自组合导航系统的状态方程和量测方程进行离散化处理，得到离散化方程，并应用离散时间卡尔曼滤波算法去估计组合导航系统的各种误差状态，并用误差状态的估计值去校正INS数据，以得到校正后的长机、僚机各自的INS输出数据。本实施例，具体包括以下内容：
[0106] 在获得连续时间域的INS/GPS组合导航系统的状态方程和量测方程之后，需要进行离散化处理，从而应用离散时间卡尔曼滤波算法，进行状态信息的估计。
[0107] 把状态方程(7)式和量测方程(10)离散化可得
[0108]
[0109] 式中，Xk表示系统在k时刻的n维状态向量，也是要求的被估计的向量；Zk表示在k时刻的m维量测向量；Φk,k-1为k-1时刻到k时刻的系统状态转移矩阵；Hk表示k时刻的量测矩阵；Wk-1表示伪k-1时刻的系统噪声向量；Γk-1为系统噪声矩阵，它表示由k-1时刻到k时刻的各个系统噪声分别影响k时刻各个状态的程度；Vk为k时刻的m维量测噪声向量。且[0110]
[0111]
[0112] 式中，T为
迭代周期，n在实际计算时取有限项即可。
[0113] 状态方程和量测方程中的系统噪声和量测噪声具有如下性质：
[0114] E[Wk]＝0，
[0115] E[Vk]＝0，
[0116] Cov[Wk,Vj]＝E[WkVjT]＝0
[0117] 式中，Qk为系统噪声序列Wk的方差阵；Rk为量测噪声序列Vk的方差阵。
[0118] 离散量Qk、Rk和连续量Q(t)、R(t)的关系可近似表示为
[0119]
[0120] 卡尔曼滤波器在组合和导航系统中应用较为普遍。在组合导航系统中应用卡尔曼滤波技术的主要方法为：在导航系统某些测量输出量的
基础上，利用卡尔曼滤波去估计系统的各种误差状态，并用误差状态的估计值去校正系统，以达到系统组合的目的。用误差状态的估计值去校正INS数据，以得到校正后的长机、僚机各自的INS输出数据。
[0121] 所谓滤波就是从混合在一起的诸多信号中提取出所需的信号，卡尔曼滤波从被提取信号有关的量测量中通过算法估计所需信号。其中被估计信号是由白噪声激励引起的随机响应，激励源与响应之间的传递结构(系统方程)已知，量测量与被估计量之间的函数关系(量测方程)也已知。估计过程中利用了如下信息：系统方程、量测方程、白噪声激励的统计特性、量测误差的统计特性。由于所用信息都是时域内的量，所以卡尔曼滤波器是在时域内设计的，且适用于多维情况。
[0122] 卡尔曼滤波具有以下特点：
[0123] (1)卡尔曼滤波处理的对象是随机信号；
[0124] (2)被处理信号无有用和干扰之分，滤波的目的是要估计出所有被处理信号；
[0125] (3)系统的白噪声激励和量测噪声并不是需要滤除的对向，它们的统计特性正是估计过程中需要利用的信息。
[0126] 卡尔曼滤波实质上是一套由数字计算机实现的递推算法，每个递推周期中包含对被估计量的时间更新和量测更新两个过程。时间更新由上一步的量测更新结果和设计卡尔曼滤波器时的先验信息确定，量测更新则在时间更新的基础上根据实时获得的量测值确定。因此量测量可看作卡尔曼滤波器的输入，估计值可看作输出，输入与输出之间由时间更新和量测更新算法联系。
[0127] 假设系统的初始状态X0也是正态随机向量，其均值和协方差分别为[0128]
[0129] 离散卡尔曼滤波算法的基本步骤如下：
[0130] 状态一步预测
[0131]
[0132] 状态估计
[0133]
[0134] 滤波增益
[0135] Kk＝Pk·HkT·Rk-1 (17)
[0136] 一步预测均方误差
[0137] Pk|k-1＝Φk,k-1·Pk-1·Φk,k-1T+Γk-1·Qk·Γk-1T (18)
[0138] 估计均方误差
[0139] Pk＝(I-Kk·Hk)Pk|k-1 (19)
[0140] 只要给定滤波初值 和P0，根据k时刻的量测Zk，就可递推计算得到k时刻的状态估计
[0141] 离散卡尔曼滤波算法的计算过程可以用图3表示。
[0142] 图3中可以明显看出卡尔曼滤波具有两个计算回路：增益计算回路和滤波计算回路。其中增益计算回路是独立计算回路，而滤波计算回路依赖于增益计算回路。
