CN109631883A - 一种基于节点信息共享的载机局部姿态精确估计方法 - Google Patents

Abstract

本发明公开了一种基于节点信息共享的载机局部姿态精确估计方法，步骤包括：1)对于单个子节点的精度优化策略，将每个子节点IMU与主节点IMU进行传递对准，得到较高精度的子节点导航数据；2)结合离散卡尔曼滤波方程进行局部卡尔曼滤波算法编排，并引入逆向卡尔曼滤波以及双向融合算法，提高单节点的传递对准结果；3)利用子节点之间相距位置较近以及运动的连续性特点，对各子节点之间进行信息转换计算，将不同节点信息转换至某一节点，并对该节点的导航信息进行融合，得到更高精度的状态估计结果。本发明的方法，过程简单，结果准确。

Description

一种基于节点信息共享的载机局部姿态精确估计方法

技术领域

本发明属于机载惯性导航技术领域，涉及一种基于节点信息共享的载机局部姿态精确估计方法。

背景技术

由于惯性导航系统是一种隐蔽性好、可全天候全时间地工作的自主式导航系统，因此其在军用和民用领域都有很多方面的应用。随着现代战争中飞机需要执行机载瞄准和武器投放等任务，这需要精确获得载机的导航信息。采用高精度惯性导航仪器获取载机所需高可靠性和准确性的导航信息成为了首选，但是考虑到飞机和弹群的造价成本以及飞机载重等因素，大量使用高精度惯性导航仪器有许多现实制约因素。采用惯性网络技术不仅可以降低整个网络的成本，还能很好的实现惯性网络中传感器故障诊断、隔离与重构，可低成本、高精度和高可靠性获取载机所需要的局部惯性状态信息。

惯性网络技术是利用滤波和数据融合技术，使少数高精度惯导组和多数低精度惯导组构成的惯性网络的导航精度接近全部使用高精度惯导组的弹群或者机群的导航精度。目前先进战机一般都在机身重心处安装有高精度惯导系统，而在重要机载任务传感器附近安装有低成本MSME惯导，能够提供惯性测量信息如加速度、角速度信息，这也为采用惯性网络技术的实现提供了应用条件。

发明内容

本发明的目的是提供一种基于节点信息共享的载机局部姿态精确估计方法，首先建立一个多节点惯性网络系统，包括一个主IMU节点和多个子IMU节点，主节点位于机身重心位置，各子节点分布于机翼及其余机身位置，然后，载机局部姿态估计精度的提高策略如图3所示，具体按照以下步骤实施：

第一部分：对于单个子节点的精度优化策略，将每个子节点IMU与主节点IMU进行传递对准，得到较高精度的子节点导航数据；

第二部分：结合离散卡尔曼滤波方程进行局部卡尔曼滤波算法编排，并引入逆向卡尔曼滤波以及双向融合算法，提高单节点的传递对准结果；

第三部分：利用子节点之间相距位置较近以及运动的连续性特点，对各子节点之间进行信息转换计算，将不同节点信息转换至某一节点，并对该节点的导航信息进行融合，得到更高精度的状态估计结果。

本发明的有益效果是，针对子节点导航精度不高的问题，从两个方面对其进行精度优化策略，以此达到对载机局部姿态的精确估计。首先，对于单节点的精度优化策略，本发明利用主节点IMU和各子节点IMU分别进行数据匹配，并通过离线双向卡尔曼滤波进行处理，得到速度和姿态的最优融合值。其次，通过各个不同节点获得的导航信息进行滤波和数据融合处理，实时的修正导航信息从而实现局部惯性信息的准确获取，具体包括：

1)由于采用了双向滤波以及融合的方法，主子节点之间的传递对准速度和效率会大大提高，并且可以改善传递对准的精度。

2)具有良好的系统故障容错性和鲁棒性，确保在任何节点或者元器件的发生故障后，不会引起机体导航性能的退化，有效的进行故障检测与隔离。

3)准确的局部导航信息估计，通过几个步骤，惯性网络中的各个子节点都可以融合整体系统的量测信息，进而推导局部状态的最优估计结果。

附图说明

图1是本发明方法中的分布式惯性网络简图；

图2是本发明方法中不同节点之间的转换关系示意图；

图3是本发明方法中的基于节点信息共享的精度优化简图。

具体实施方式

下面结合附图和具体实施方式对本发明进行详细说明。

参照图1，本发明的载机局部姿态精确估计方法，假设将各惯导测量单元分别安装在机翼及机身(重心)位置处，以此安装位置为基础组成惯性网络结构。首先对于单节点的精度提高策略，本发明利用机身位置处的高精度惯导测量结果对各子节点进行传递对准，采用速度+姿态的匹配方法，建立卡尔曼滤波器模型，卡尔曼滤波原理简单易于实现，但收敛需要一定的时间，收敛之前滤波精度较低。而实际需要是在整个飞行过程中都具有较高精度的估计结果，以此建立逆向卡尔曼滤波模型，利用正逆向滤波结合的方法优化单节点传递对准的估计结果，使得估计结果在任何时间内都具有良好的性能。其次，对于多节点的精度提高方法，本发明基于上一步的估计结果，虽然主节点的精度较高，但在传递对准中由于杆臂效应以及机翼形变等原因，使得传递对准中出现较大的误差，而子节点之间元器件相同，相隔距离较近，其误差也具有共性，因此提出多节点信息融合的思路，进一步优化每个节点的导航信息结果，使得整体导航信息更为准确。

本发明方法，基于上述的结构及原理，参照图3所示流程，按照以下步骤实施：

第一部分：对于单个子节点的精度优化策略，由于子节点的精度不高，因此将每个子节点IMU与主节点IMU进行传递对准，得到较高精度的子节点导航数据；

即建立分布式惯导系统模型对子惯导进行传递对准，采用速度+姿态的对准匹配方式，具体包括：

步骤1：确定主节点、子节点传递对准匹配方式，

传递对准是采用已对准的主惯导信息修正子惯导信息，从而获得子惯导初始姿态的一种方法，根据所选参考信息的不同，可得出不同的基本匹配方案。

目前采用最多且效果较好的就是“速度+姿态”的匹配方式，它是一种组合参数匹配法，结合了速度匹配和姿态匹配两种方式的优点，并且对机动要求不高。

步骤2：构造“速度+姿态”传递对准的量测方程和状态方程：

2.1)选取滤波状态量，表达式为：

其中，为子惯导姿态误差；为子惯导速度误差；为子惯导陀螺常值漂移；为子惯导加速度计常值偏置；μ^T为子惯导常值安装误差角；为子惯导所在处机翼的挠曲变形角，为子惯导所在处机翼的变形角速率，各个变量及表达式右上角的T表示转置；

2.2)采用二阶马尔科夫过程描述机翼的挠曲变形，表达式为：

其中，指的是对θ求导数，κ＝2.146/τ，τ为二阶马尔科夫过程的相关时间；λ为方差是4κ³σ²的白噪声，σ为挠曲变形角的均方差；

2.3)构建系统状态空间模型,表达式为：

式中，

其中，为n系(导航坐标系)相对于i系(惯性坐标系)的运动角速率，为的反对称矩阵(下同)，为地球自转角速率在n系分量，为运载体绕地球表面运动引起的角速率在n系的分量，为子惯导(s系)转换至n系的矩阵(即子惯导输出姿态矩阵)，f^s为子惯导的比力输出，为子惯导陀螺随机游走，为子惯导加计随机漂移；

2.4)进行滤波量测量，选取速度匹配量测量与姿态匹配量测量，

滤波量测量的表达式为：

Z_vθ＝[Z_v Z_θ]^T (5)

其中，速度匹配量测量与姿态匹配量测量分别表示为：

其中，为子惯导输出速度；为主惯导输出速度；为子惯导处杆臂速度；为主惯导解算姿态矩阵；为子惯导解算姿态矩阵；

上述的式(1)、式(4)、式(5)、式(6)一起构成了速度+姿态匹配传递对准的卡尔曼滤波系统状态方程和量测方程。

第二部分：结合离散卡尔曼滤波方程进行局部卡尔曼滤波算法编排，并引入逆向卡尔曼滤波以及双向融合算法，提高单节点的传递对准结果；

存储正向卡尔曼滤波的结果，将正向滤波的结果作为逆向卡尔曼滤波的初始信息，如此一来逆向滤波就有了“准确”的先验知识，一开始就具有较高精度的估计结果，将逆向滤波的结果作为最终结果进行输出。此方法为事后处理过程，对于飞行过程中各个时间段机体局部导航信息都有良好的估计结果。在导航系统中，滤波算法是通过导航计算机实现的，卡尔曼滤波的应用主要采用的是离散的表达形式，本发明在离散线性卡尔曼滤波的基础上，推导了逆向卡尔曼滤波方程，在分析平滑算法的基础上，提出了创新的正/逆向双向融合算法。

步骤3：根据得到的卡尔曼滤波系统状态方程以及量测方程，构建离散型卡尔曼滤波方程，由以下五个基本方程构成：

3.1)状态一步预测方程：

其中，X为步骤2.1)中的状态量，右下标k表示在k时刻的状态量，上标表示对X的状态一步估值(下同)，Φ_k/k-1为t_k-1时刻至t_k时刻一步转移矩阵，是根据计算得到的对X_k的一步状态预测；

3.2)状态估计方程：

其中，K_k为滤波增益方程，Z_k为步骤2.4)中求得的量测方程，H_k为量测阵；

3.3)一步预测的均方误差阵：

其中，Γ_k-1为系统噪声驱动阵，Q_k为系统噪声序列的方差阵，为非负定阵，P_k表示k时刻的均方误差，P_k-1表示k-1时刻的均方误差，P_k/k-1为由t_k-1时刻至t_k时刻的均方误差，Q对应表示与P相同。

3.4)滤波增益矩阵：

其中，R_k为量测噪声序列的方差阵，为正定阵；

3.5)估计均方误差方程是：

P_k＝(I-K_kH_k)P_k/k-1 (12)

其中，I为单位阵；

步骤4：构建逆向卡尔曼滤波方程，

逆向滤波器的表达式跟正向滤波器的表达式类似，不同的仅仅是逆向滤波器中的时间更新是逆向的，考虑到与正向滤波器相同的状态空间模型和时间序列的逆向处理，得到逆向滤波器的方程式为：

其中，上标箭头←表示逆向处理；与步骤3中变量不变，下标k-1/k为由t_k时刻逆向推导出t_k-1时刻的对应方程。

步骤5：加权融合过程，

将正向滤波、逆向滤波得到的状态参数估计向量及协方差进行加权平滑，得到平滑状态参数向量及协方差阵，表达式如下：

其中，下标s,k表示平滑后的结果。

第三部分：从系统的角度出发，利用子节点之间相距位置较近以及运动的连续性特点，对各子节点之间进行信息转换计算，将不同节点信息转换至某一节点，并对该节点的导航信息进行融合，得到更高精度的状态估计结果，具体过程是：

步骤6：对各节点之间进行转换矩阵的计算，

参照图2，以三个IMU节点i，j，cg为例，选择IMU节点cg的坐标系作为参考坐标系，则IMU节点i，j的角速率相对惯性坐标系(I系)矢量在IMU节点主坐标系cg下的投影。

以i节点为例，可得出表达式为：

其中，Ω是对应角速率矢量ω的反对称矩阵，为i系相对于I系(惯性坐标系)的运动角速率在cg系下的投影，为cg系相对于I系的运动角速率在cg系下的投影，为i系相对于cg系的运动角速率在cg系下的投影。

由于姿态阵微分方程将方程式(15)改写为：

其中，为cg系至i系的转换矩阵，为的导数。

由此得到IMU节点i相对IMU节点cg转换阵的微分方程为：

其中，为i系至cg系的转换矩阵，为i系相对于I系(惯性坐标系)的运动角速率在i系下的投影。

同理，得到IMU节点j相对IMU节点cg转换阵的微分方程，求解微分方程得到和则IMU节点i，j之间的动态转换阵表达式为：

式中，为i系至j系的转换矩阵。

步骤7：利用节点之间的信息转换矩阵将其他节点信息转换至各节点，并进行各节点信息的量测融合；

IMU节点i和IMU节点j根据式(18)的旋转矩阵实现该两个IMU节点间相似状态的相互转换：

其中，x_l,l＝i,j,cg为IMU节点l的待估计加速度或角速度输出，为步骤6中所求的i节点与j节点之间的转换矩阵；

则IMU节点i的量测方程表达式为：

其中，m_l,l＝i,j,cg表示l节点吸收其余节点惯性测量信息后，得到l点完整的的惯性测量信息，y_l,l＝i,j,cg是IMU节点l的加速度或角速度输出值，v_l,l＝i,j,cg是节点l的观测误差矩阵，H_l,l＝i,j,cg是IMU节点l的观测阵；

由最小二乘估计法得到IMU节点l的待估计加速度或角速度及其估计误差协方差阵P_x,l，表达式为：

其中，表示对状态量x的估计,R为量测噪声序列的方差阵，P_x,l为误差协方差阵。

选取最大条件概率密度函数作为优化指标，由于所有IMU节点测量是相互独立的，得到IMU节点i,j,cg通过量测融合后待估计加速度或角速度x_l,l＝i,j,cg，表达式为：

步骤8：对各节点信息进行状态融合，

在各个节点通过局部卡尔曼滤波器对局部状态进行估计后，设计局部状态融合滤波器，吸收其他节点的局部状态估计，进行各节点姿态信息融合处理，完成局部状态估计的更新，获得导航状态信息的高精度估计。

对于节点l＝(i,j,cg)，首先参考加权最小二乘估计指标的设计，使得在当前节点的局部状态估计值与局部状态实际值(x_on)之差的加权平方和最小，其中的权重设置为局部状态估计的协方差阵的逆矩阵最后再用求和函数Σ求取各个节点加权平方和的总和。

定义一个二次方的代价函数如下：

其中，x_on为局部状态的实际量，为节点l＝(i,j,cg)的导航状态估计，P_ol为节点l＝(i,j,cg)的协方差,表示节点l＝(i,j,cg)所在坐标系至导航坐标系(n系)的转换矩阵，同理，表示n系至节点l＝(i,j,cg)所在坐标系的转换矩阵。

令代价函数最小，即可求得局部状态融合器的状态更新。得到节点i估计协方差矩阵如下：

上式中即为其估计协方差阵。

则由此得到状态融合估计表达式为：

即为经过精度优化后的节点i导航状态输出。

经仿真实验后得出结论，通过单节点传递对准精度优化以及多节点信息融合两个步骤，大大提高机体局部位置的导航信息的精度，进一步估计出机翼在飞行过程中的形变情况。

本发明的方法，在前两部分传递对准中，采用速度+姿态匹配方式，建立卡尔曼滤波模型；在常规卡尔曼滤波模型的基础上，通过现代计算机的存储设备，对卡尔曼滤波的结果进行逆向处理，最后再对双向滤波的结果进行融合，其本质上也属于信息融合的一种方式。通过此方法提高传递对准中子节点的精度。

在第三部分多节点信息共享中，在上一步中已经得到由主子节点传递对准后较精确的子节点信息，但由于主子节点之间杆臂距离以及机翼挠曲变形等的影响，会使得主子节点之间的相对位置发生变化，从而导致对子节点导航信息的估计有较大的误差。而相邻较近的节点之间由于机体运动的连续性，使得其之间相对变化较小，因此可以利用此相对位置关系来将其余子IMU处的导航信息转换至某一节点，至此已经得到由多节点估计的单节点的导航信息。对单节点的多维导航信息进行融合，通过对多个相同信息的融合处理，达到一个更优的估计结果。

Claims (4)

1.一种基于节点信息共享的载机局部姿态精确估计方法，其特征在于：首先建立一个多节点惯性网络系统，包括一个主IMU节点和多个子IMU节点，主节点位于机身重心位置，各子节点分布于机翼及其余机身位置，

然后，按照以下步骤实施：

第一部分：对于单个子节点的精度优化策略，将每个子节点IMU与主节点IMU进行传递对准，得到较高精度的子节点导航数据；

第二部分：结合离散卡尔曼滤波方程进行局部卡尔曼滤波算法编排，并引入逆向卡尔曼滤波以及双向融合算法，提高单节点的传递对准结果；

第三部分：利用子节点之间相距位置较近以及运动的连续性特点，对各子节点之间进行信息转换计算，将不同节点信息转换至某一节点，并对该节点的导航信息进行融合，得到更高精度的状态估计结果。

2.根据权利要求1所述的基于节点信息共享的载机局部姿态精确估计方法，其特征在于：所述的第一部分中，具体过程是，

步骤1：确定主节点、子节点传递对准匹配方式，

步骤2：构造“速度+姿态”传递对准的量测方程和状态方程：

2.1)选取滤波状态量，表达式为：

其中，为子惯导姿态误差；为子惯导速度误差；为子惯导陀螺常值漂移；为子惯导加速度计常值偏置；μ^T为子惯导常值安装误差角；为子惯导所在处机翼的挠曲变形角，为子惯导所在处机翼的变形角速率，各个变量及表达式右上角的T表示转置；

2.2)采用二阶马尔科夫过程描述机翼的挠曲变形，表达式为：

其中，指的是对θ求导数，κ＝2.146/τ，τ为二阶马尔科夫过程的相关时间；λ为方差是4κ³σ²的白噪声，σ为挠曲变形角的均方差；

2.3)构建系统状态空间模型,表达式为：

式中，

其中，为n系相对于i系的运动角速率，为的反对称矩阵，为地球自转角速率在n系分量，为运载体绕地球表面运动引起的角速率在n系的分量，为子惯导转换至n系的矩阵，f^s为子惯导的比力输出，为子惯导陀螺随机游走，为子惯导加计随机漂移；

2.4)进行滤波量测量，选取速度匹配量测量与姿态匹配量测量，

滤波量测量的表达式为：

Z_vθ＝[Z_v Z_θ]^T (5)

其中，速度匹配量测量与姿态匹配量测量分别表示为：

其中，为子惯导输出速度；为主惯导输出速度；为子惯导处杆臂速度；为主惯导解算姿态矩阵；为子惯导解算姿态矩阵；

上述的式(1)、式(4)、式(5)、式(6)一起构成了速度+姿态匹配传递对准的卡尔曼滤波系统状态方程和量测方程。

3.根据权利要求2所述的基于节点信息共享的载机局部姿态精确估计方法，其特征在于：所述的第二部分中，具体过程是，

步骤3：根据得到的卡尔曼滤波系统状态方程以及量测方程，构建离散型卡尔曼滤波方程，由以下五个基本方程构成：

3.1)状态一步预测方程：

其中，X为步骤2.1)中的状态量，右下标k表示在k时刻的状态量，上标表示对X的状态一步估值，Φ_k/k-1为t_k-1时刻至t_k时刻一步转移矩阵，是根据计算得到的对X_k的一步状态预测；

3.2)状态估计方程：

其中，K_k为滤波增益方程，Z_k为步骤2.4)中求得的量测方程，H_k为量测阵；

3.3)一步预测的均方误差阵：

其中，Γ_k-1为系统噪声驱动阵，Q_k为系统噪声序列的方差阵，为非负定阵，P_k表示k时刻的均方误差，P_k-1表示k-1时刻的均方误差，P_k/k-1为由t_k-1时刻至t_k时刻的均方误差，Q对应表示与P相同；

3.4)滤波增益矩阵：

其中，R_k为量测噪声序列的方差阵，为正定阵；

3.5)估计均方误差方程是：

P_k＝(I-K_kH_k)P_k/k-1 (12)

其中，I为单位阵；

步骤4：构建逆向卡尔曼滤波方程，

逆向滤波器的方程式为：

其中，上标箭头←表示逆向处理；与步骤3中变量不变，下标k-1/k为由t_k时刻逆向推导出t_k-1时刻的对应方程；

步骤5：加权融合过程，

将正向滤波、逆向滤波得到的状态参数估计向量及协方差进行加权平滑，得到平滑状态参数向量及协方差阵，表达式如下：

其中，下标s,k表示平滑后的结果。

4.根据权利要求3所述的基于节点信息共享的载机局部姿态精确估计方法，其特征在于：所述的第三部分中，具体过程是，

步骤6：对各节点之间进行转换矩阵的计算，

以三个IMU节点i，j，cg为例，选择IMU节点cg的坐标系作为参考坐标系，则IMU节点i，j的角速率相对惯性坐标系矢量在IMU节点主坐标系cg下的投影，

以i节点为例，可得出表达式为：

其中，Ω是对应角速率矢量ω的反对称矩阵，为i系相对于I系的运动角速率在cg系下的投影，为cg系相对于I系的运动角速率在cg系下的投影，为i系相对于cg系的运动角速率在cg系下的投影，

由于姿态阵微分方程将方程式(15)改写为：

其中，为cg系至i系的转换矩阵，为的导数，

由此得到IMU节点i相对IMU节点cg转换阵的微分方程为：

其中，为i系至cg系的转换矩阵，为i系相对于I系的运动角速率在i系下的投影，

同理，得到IMU节点j相对IMU节点cg转换阵的微分方程，求解微分方程得到和则IMU节点i，j之间的动态转换阵表达式为：

式中，为i系至j系的转换矩阵；

步骤7：利用节点之间的信息转换矩阵将其他节点信息转换至各节点，并进行各节点信息的量测融合；

IMU节点i和IMU节点j根据式(18)的旋转矩阵实现该两个IMU节点间相似状态的相互转换：

其中，x_l,l＝i,j,cg为IMU节点l的待估计加速度或角速度输出，为步骤6中所求的i节点与j节点之间的转换矩阵；

则IMU节点i的量测方程表达式为：

其中，m_l,l＝i,j,cg表示l节点吸收其余节点惯性测量信息后，得到l点完整的的惯性测量信息，y_l,l＝i,j,cg是IMU节点l的加速度或角速度输出值，v_l,l＝i,j,cg是节点l的观测误差矩阵，H_l,l＝i,j,cg是IMU节点l的观测阵；

由最小二乘估计法得到IMU节点l的待估计加速度或角速度及其估计误差协方差阵P_x,l，表达式为：

其中，表示对状态量x的估计,R为量测噪声序列的方差阵，P_x,l为误差协方差阵，

选取最大条件概率密度函数作为优化指标，得到IMU节点i,j,cg通过量测融合后待估计加速度或角速度x_l,l＝i,j,cg，表达式为：

步骤8：对各节点信息进行状态融合，

在各个节点通过局部卡尔曼滤波器对局部状态进行估计后，设计局部状态融合滤波器，吸收其他节点的局部状态估计，进行各节点姿态信息融合处理，完成局部状态估计的更新，获得导航状态信息的高精度估计。