CN108387227A - 机载分布式pos的多节点信息融合方法及系统 - Google Patents

Abstract

一种机载分布式POS的多节点信息融合方法及系统，该方法包括：建立传递对准误差模型；针对每一个子IMU，分别计算该子IMU所处的导航坐标系下表示的主POS的姿态与该子IMU的姿态之差、主POS的速度与该子IMU的速度之差并建立该子IMU传递对准的数学模型；对该子IMU传递对准的数学模型进行传递对准，确定姿态误差、速度误差和位置误差，并得到安装误差角、挠曲变形角和挠曲变形角速率；修正该子IMU的捷联解算结果，得到该子IMU的修正位置、修正速度和修正姿态；利用所估计的各子IMU的安装误差角、挠曲变形角和挠曲变形角速率补偿杆臂效应，分别将各子IMU的修正位置、修正速度和修正姿态进行信息融合，确定机载分布式POS的测量信息，提高分布式POS系统的整体测量精度。

Description

机载分布式POS的多节点信息融合方法及系统

技术领域

本发明涉及多任务遥感载荷节点信息融合技术领域，特别是涉及一种机载分布式POS(Position and Orientation System，位置姿态测量系统)的多节点信息融合方法及系统。

背景技术

多任务遥感载荷是目前机载对地观测的重要发展方向之一，如集成高分辨率测绘相机、成像光谱仪、大视场红外扫描仪、合成孔径雷达(Synthetic Aperture Radar，SAR)于同一载机的多任务载荷，机载分布式阵列天线SAR等。对于装备多任务遥感载荷的综合航空遥感系统，需要对各载荷分布点的运动参数进行高精度测量。

分布式POS是目前获取载机多点位置、速度、姿态等运动参数的有效手段。分布式POS的组成主要包括一个高精度主位置姿态测量系统(主POS)、多个子惯性测量单元(Inertial Measurement Unit，IMU)、一个导航计算机和一套后处理软件。其中主POS由高精度主IMU和全球导航卫星系统(Global Navigation Satellite System，GNSS)组成，主IMU一般安装在机舱内或机腹部，子IMU一般分布在载机两侧的机翼上，形成一个多节点的惯性测量系统。

而应用于机载导航系统的信息融合算法主要有集中滤波和联邦滤波两种方法。理论上讲，集中滤波器可获得最优估计，但是随着子系统数量的增加算法复杂度增加，造成“维数灾难”，甚至导致滤波发散。分级分散的联邦滤波采用各子滤波器并行滤波的方法，克服了集中滤波带来的计算量大的问题。联邦滤波各系统有主次之分，主系统又叫参考系统，它与各子系统的估计值进行融合后反馈到子系统，对子系统的估计值进行重置。

然而，目前针对导航系统的联邦滤波多传感器信息融合方法主要用来提高单一节点的测量精度，针对多节点分布式POS的信息融合，主要采用的方法是将主POS输出的高精度位置、速度、姿态等运动参数信息分别与各子IMU输出的信息进行融合，实现子IMU所在处运动信息的精确测量，即传递对准。

理想情况下，经过传递对准的各子IMU的测量精度应当一致，但是实际飞行中，由于各子IMU安置点的机体变形、杆臂位置、惯性器件精度等因素存在差异，导致各子IMU传递对准精度的差异。一般而言，靠近机体中心的子IMU安置点的传递对准精度较高，容易满足成像运动补偿的精度要求，远离机体中心的子IMU安置点的挠曲变形情况更复杂，传递对准精度较低，不能满足成像运动补偿的精度要求。为了使分布式POS每个节点均能够满足精度要求，提高分布式POS系统的整体测量精度，必须综合利用所有子IMU的输出信息进行信息融合，均衡各个节点的精度。

但是目前一般假设各子IMU所处的导航坐标系相同，均为主POS的导航坐标系，从而直接对各IMU进行信息融合，但是位置、速度、姿态均通过各子IMU载体坐标系之间的方向余弦矩阵转换，缺乏合理性。另外，也没有考虑各IMU之间杆臂效应、安装误差等因素，对于高精度的分布式POS系统而言，这些因素会带来巨大的测量误差，因此必须予以考虑。

发明内容

本发明的目的是提供一种机载分布式POS的多节点信息融合方法及系统，可提高分布式POS系统的整体测量精度。

为实现上述目的，本发明提供了如下方案：

一种机载分布式位置姿态测量系统POS的多节点信息融合方法，所述机载分布式POS包括一个主POS和多个子惯性测量单元IMU，所述多节点信息融合方法包括：

建立传递对准误差模型，所述传递对准误差模型包括子IMU惯导误差模型、主POS与各子IMU的主子系统间的角误差模型；

针对每一个子IMU，分别计算该子IMU所处的导航坐标系下表示的主POS的姿态与该子IMU的姿态之差、主POS的速度与该子IMU的速度之差，分别得到的对应的姿态差和速度差，将姿态差和速度差作为系统量测量，建立该子IMU传递对准的数学模型；

采用卡尔曼滤波方法，对该子IMU传递对准的数学模型进行传递对准，确定该子IMU的姿态误差、速度误差和位置误差，并得到该子IMU的安装误差角、挠曲变形角和挠曲变形角速率；

根据该子IMU的姿态误差、速度误差和位置误差修正该子IMU的捷联解算结果，得到该子IMU的修正位置、修正速度和修正姿态；

利用所估计的各子IMU的安装误差角、挠曲变形角和挠曲变形角速率补偿杆臂效应，分别将各子IMU的修正位置、修正速度和修正姿态进行信息融合，确定所述机载分布式POS的测量信息。

为实现上述目的，本发明提供了如下方案：

一种机载分布式POS的多节点信息融合系统，所述机载分布式POS包括一个主POS和多个子惯性测量单元IMU，所述多节点信息融合方法系统：

第一建模单元，用于建立传递对准误差模型，所述传递对准误差模型包括子IMU惯导误差模型、主POS与各子IMU的主子系统间的角误差模型；

第二建模单元，用于针对每一个子IMU，分别计算该子IMU所处的导航坐标系下表示的主POS的姿态与该子IMU的姿态之差、主POS的速度与该子IMU的速度之差，分别得到的对应的姿态差和速度差，将姿态差和速度差作为系统量测量，建立该子IMU传递对准的数学模型；

传递对准单元，用于采用卡尔曼滤波方法，对该子IMU传递对准的数学模型进行传递对准，确定该子IMU的姿态误差、速度误差和位置误差，并得到该子IMU的安装误差角、挠曲变形角和挠曲变形角速率；

修正单元，用于根据该子IMU的姿态误差、速度误差和位置误差修正该子IMU的捷联解算结果，得到该子IMU的修正位置、修正速度和修正姿态；

信息融合单元，用于利用所估计的各子IMU的安装误差角、挠曲变形角和挠曲变形角速率补偿杆臂效应，分别将各子IMU的修正位置、修正速度和修正姿态进行信息融合，确定所述机载分布式POS的测量信息。

根据本发明提供的具体实施例，本发明公开了以下技术效果：

本发明通过建立传递对准误差模型、子IMU传递对准的数学模型，采用卡尔曼滤波方法，对该子IMU传递对准的数学模型进行传递对准，获得子IMU的姿态误差、速度误差和位置误差以及该子IMU的安装误差角、挠曲变形角和挠曲变形角速率，根据子IMU的姿态误差、速度误差和位置误差对各子IMU的捷联解算结果进行修正，并对修正结果进行信息融合，从而获得准确的机载分布式POS的测量信息，提升整体的测量精度。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。

图1为本发明实施例机载分布式POS的多节点信息融合方法的流程图；

图2为各子IMU信息融合的结构图；

图3为本发明实施例机载分布式POS的多节点信息融合系统的模块结构示意图。

符号说明：

第一建模单元—1，第二建模单元—2，传递对准单元—3，修正单元—4，信息融合单元—5。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本发明的目的是提供一种机载分布式POS的多节点信息融合方法，通过建立传递对准误差模型、子IMU传递对准的数学模型，采用卡尔曼滤波方法，对该子IMU传递对准的数学模型进行传递对准，获得子IMU的姿态误差、速度误差和位置误差以及该子IMU的安装误差角、挠曲变形角和挠曲变形角速率，根据子IMU的姿态误差、速度误差和位置误差对各子IMU的捷联解算结果进行修正，并对修正结果进行信息融合，从而获得准确的机载分布式POS的测量信息，提升整体的测量精度。

其中，所述机载分布式POS包括一个主POS和多个子IMU。

为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。

如图1所示，本发明机载分布式位置姿态测量系统POS的多节点信息融合方法包括：

步骤100：建立传递对准误差模型，所述传递对准误差模型包括子IMU惯导误差模型、主POS与各子IMU的主子系统间的角误差模型；

步骤200：针对每一个子IMU，分别计算该子IMU所处的导航坐标系下表示的主POS的姿态与该子IMU的姿态之差、主POS的速度与该子IMU的速度之差，分别得到的对应的姿态差和速度差，将姿态差和速度差作为系统量测量，建立该子IMU传递对准的数学模型；

步骤300：采用卡尔曼滤波方法，对该子IMU传递对准的数学模型进行传递对准，确定该子IMU的姿态误差、速度误差和位置误差，并得到该子IMU的安装误差角、挠曲变形角和挠曲变形角速率；

步骤400：根据该子IMU的姿态误差、速度误差和位置误差修正该子IMU的捷联解算结果，得到该子IMU的修正位置、修正速度和修正姿态；

步骤500：利用所估计的各子IMU的安装误差角、挠曲变形角和挠曲变形角速率补偿杆臂效应，分别将各子IMU的修正位置、修正速度和修正姿态进行信息融合，确定所述机载分布式POS的测量信息。

其中，在步骤100中，所述建立传递对准误差模型，具体包括：

步骤110：建立子IMU惯导误差模型。

所述子IMU惯导误差模型包括姿态角误差微分方程、速度误差微分方程、位置误差微分方程和惯性仪表误差微分方程。

其中，o为地心惯性坐标系；e为地球坐标系；主POS和子IMU导航坐标系均为东北天地理坐标系，主POS的导航坐标系用n表示，第i个子IMU的导航坐标系用ni表示，i＝1,2,…,N，N为子IMU的个数，计算导航坐标系用ni′表示；载体坐标系原点为载体重心，x轴沿载体横轴向右，y轴沿载体纵轴向前，z轴沿载体竖轴向上，该坐标系固定在载体上，称为右前上载体坐标系，用m和bi分别代表主POS和第i个子IMU的载体坐标系。

具体根据以下步骤分别确定姿态角误差微分方程、速度误差微分方程、位置误差微分方程和惯性仪表误差微分方程：

步骤111：根据以下公式，确定姿态角误差微分方程：

其中，为第i个子IMU的姿态失准角，和分别为东向、北向、天向失准角，下标E、N和U分别表示东向、北向和天向；为第i个子IMU导航坐标系相对地心惯性坐标系的角速度；为的误差；为第i个子IMU载体坐标系到其导航坐标系的方向余弦矩阵的估计值；为第i个子IMU的陀螺仪常值漂移，其中和分别为第i个子IMU载体坐标系x轴、y轴和z轴陀螺仪常值漂移。

步骤112：根据以下公式，确定速度误差微分方程：

其中，为第i个子IMU的速度，为第i个子IMU的速度误差，其中和分别为东向、北向和天向速度，和分别为东向、北向和天向速度误差；是第i个子IMU的比力，其中和分别为东向、北向和天向比力；和分别为第i个子IMU导航坐标系相对地球坐标系的角速度及其误差；和分别为第i个子IMU导航坐标系相对地球坐标系的角速度及其误差；为加速度计常值偏置，其中和分别为第i个子IMU载体坐标系x轴、y轴和z轴加速度计常值偏置。

步骤113：根据以下公式，确定位置误差微分方程：

其中，L_i、λ_i、h_i和δL_i、δλ_i、δh_i分别为第i个子IMU纬度、经度、高度和纬度误差、经度误差、高度误差；为纬度的一阶导数，为经度的一阶导数；和分别为第i个子IMU沿子午圈和卯酉圈的主曲率半径。

步骤114：根据以下公式，确定惯性仪表误差微分方程：

步骤120：建立主POS与各子IMU的主子系统间的角误差模型。

所述主子系统间的角误差模型包括：固定安装误差角的微分方程、弹性变形角的微分方程。

具体根据以下步骤分别确定固定安装误差角的微分方程、弹性变形角的微分方程：

步骤121：根据以下公式，确定固定安装误差角的微分方程：

其中，为第i个子IMU固定安装误差角，和分别为第i个子IMU载体坐标系x轴、y轴和z轴安装误差角。

步骤122：根据以下公式，确定弹性变形角的微分方程：

其中，为第i个子IMU载体坐标系第j轴上的弹性变形角，j＝x,y,z，为弹性变形角；为二阶马尔科夫过程相关时间；为零均值白噪声，其方差满足：

为弹性变形角的方差，和为描述弹性变形角θ_i的二阶马尔科夫过程的参数。

在步骤200中，根据以下公式，建立各子IMU传递对准的数学模型：

其中，系统状态变量X_i为：

系统转移矩阵F_i可由第i个子IMU的传递对准误差模型确定；系统噪声其中和分别为第i个子IMU载体坐标系x轴、y轴、z轴陀螺仪和第i个子IMU载体坐标系x轴、y轴、z轴加速度计的随机误差，不包括随机常值误差；系统噪声W_i为零均值的高斯白噪声，其方差阵Q_i由陀螺仪常值漂移、加速度计常值偏置和二阶马尔科夫过程参数决定；系统噪声阵G_i的表达式为：

其中，为第i个子IMU载体坐标系到导航坐标系的方向余弦矩阵；

系统量测变量Z_i＝[δψ_i δθ_i δγ_i δV′_iE δV′_iN δV′_iU]^T，其中δψ_i、δθ_i、δγ_i和δV′_iE、δV′_iN、δV′_iU分别为第i个子IMU与主POS相对于第i个子IMU导航坐标系的航向角、俯仰角、横滚角之差和东向、北向、天向速度之差；量测噪声其中分别为主POS航向角、俯仰角、横滚角的量测噪声，分别为主POS东向、北向、天向速度的量测噪声；V_i为零均值的高斯白噪声，其方差阵R_i由主POS的姿态精度和速度精度决定；量测矩阵H_i为：

主POS载体坐标系相对于第i个子IMU导航坐标系的方向余弦矩阵为：

其中，为主POS载体坐标系相对于主POS导航坐标系的方向余弦矩阵，为主POS导航坐标系到第i个子IMU导航坐标系的方向余弦矩阵：

其中，为主POS导航坐标系相对于地球坐标系的方向余弦矩阵，L_m和λ_m分别为主POS的纬度和经度；为第i个子IMU导航坐标系相对于地球坐标系的方向余弦矩阵；

令记为矩阵T_i中第l行、第m列的元素，l＝1,2,3，m＝1,2,3；则量测矩阵中和的表达式为：

在步骤400中，所述根据该子IMU的姿态误差、速度误差和位置误差修正该子IMU的捷联解算结果，得到该子IMU的修正位置、修正速度和修正姿态，具体包括：根据t_k时刻估计出的失准角速度误差 和位置误差δL_i、δλ_i、δh_i，并根据步骤410-步骤430分别修正第i个子IMU的姿态、速度和位置。

步骤410：根据以下公式，得到修正速度：

其中，和分别为第i个子IMU修正后的东向、北向和天向速度；和分别为第i个子IMU捷联解算得到的东向、北向和天向速度；和分别为t_k时刻卡尔曼滤波估计出的第i个子IMU捷联解算东向、北向和天向速度误差；

步骤420：根据以下公式，得到修正位置：

其中，和分别为第i个子IMU捷联解算得到的纬度、经度和高度；和分别为第i个子IMU修正后的纬度、经度和高度；δL_i、δλ_i和δh_i分别为t_k时刻卡尔曼滤波估计出的第i个子IMU捷联解算纬度、经度和高度误差；

步骤430：根据以下步骤，得到修正姿态，所述修正姿态包括航向角、俯仰角和横滚角：

步骤431：计算t_k时刻第i个子IMU导航坐标系n_i与计算导航坐标系n_i′间的转换矩阵和第i个子IMU载体坐标系b_i与真实导航坐标系n_i之间的转换矩阵

其中，为t_k时刻第i个子IMU捷联解算得到的姿态的方向余弦矩阵；

步骤432：由被更新后的第i个子IMU的方向余弦矩阵计算t_k时刻第i个子IMU的航向角ψ_s、俯仰角θ_s和横滚角γ_s：

其中，

T_lm为矩阵中第l行、第m列的元素，l＝1,2,3，m＝1,2,3；则第i个子IMU航向角ψ_i、俯仰角θ_i和横滚角γ_i的主值，即ψ_i主、θ_i主和γ_i主分别为：

由于航向角ψ_i、俯仰角θ_i和横滚角γ_i的取值范围分别定义为[0，2π]、[-π，+π]；那么，ψ_i、θ_i和γ_i的真值由以下公式确定：

通过对第i个子IMU的速度、位置和姿态进行修正，能够得到第i个子IMU安装点的更加准确的速度、位置和姿态信息。重复以上步骤2～4，完成所有子IMU的传递对准，并保存各子IMU的估计协方差矩阵P_i(i＝1,2,…,N)。

如图2所示，在步骤500中，所述利用所估计的各子IMU的安装误差角、挠曲变形角和挠曲变形角速率补偿杆臂效应，分别将各子IMU的修正位置、修正速度和修正姿态进行信息融合，确定所述机载分布式POS的测量信息，具体包括：

步骤510：根据以下公式，确定位置信息融合：

第i个子IMU所在节点与主POS之间的动态杆臂r_i ⁿ为：

其中，和分别为主POS沿子午圈和卯酉圈的主曲率半径，为主POS载体坐标系到主POS导航坐标系的方向余弦矩阵，为没有挠曲变形时第i个子IMU所在节点的杆臂长度在主POS载体坐标系下的投影，为第i个子IMU所在节点的挠曲变形角在主POS载体坐标系下的投影；

其他N-1个子IMU所在节点与主POS之间的动态杆臂为：

其中，为没有挠曲变形时第c个子IMU所在节点的杆臂长度在主载体坐标系下的投影，为第c个子IMU所在节点的挠曲变形角在主载体坐标系下的投影；

第c个子IMU所在节点与第i个子IMU所在节点之间的杆臂之差为：

用第c个子IMU的位置表示第i个子IMU的位置：

其中，S_c＝[L_c λ_c h_c]为第c个子IMU的位置，为主POS导航坐标系相对于第i个子IMU导航坐标系的方向余弦矩阵，的协方差矩阵为：

其中，为的协方差矩阵，为与S_c的互协方差矩阵，假设每个节点的挠曲运动均不相关，和的计算方法分别为：

其中，E[·]表示求取期望， P_c为传递对准中第c个子IMU的估计协方差矩阵，P_c(19:21,19:21)_3×3表示P_c第19行到第21行、第19列到21列的3×3的子矩阵，P_c(19:21,7:9)_3×3表示P_c第19行到第21行、第7列到9列的3×3的子矩阵，P_i(19:21,19:21)_3×3表示P_i第19行到21行、第19列到21列的3×3的子矩阵。

由于传递对准得到的协方差矩阵一定程度上可以反映传递对准的精度，传递对准精度越高，协方差矩阵的逆越大。将第c个IMU传递对准得到的协方差矩阵的逆作为权重矩阵，确定第c个IMU在信息融合中所占的比重。利用第c个IMU经过传递对准得到的位置信息、安装误差角和挠曲变形角等信息推导得到第i个IMU的位置估计值借鉴最小方差估计的思想，令与第i个IMU位置真值S_i之差的平方(方差)尽可能小。对于第i个IMU经过传递对准得到的位置信息直接将其协方差矩阵的逆作为权重矩阵，令与S_i之差的平方尽可能小。将N-1个IMU和第i个IMU的信息按照上述方法求和，得到第i个子IMU位置信息融合的二次目标函数并令其最小。

具体的，将第c个IMU传递对准得到的协方差矩阵的逆作为权重矩阵，确定第c个IMU在信息融合中所占的比重；

利用第c个IMU经过传递对准得到的位置信息、安装误差角和挠曲变形角信息，确定第i个IMU的位置估计值

令与第i个IMU位置真值S_i的方差尽可能小，对于第i个IMU经过传递对准得到的位置信息直接将其协方差矩阵的逆作为权重矩阵，令与S_i的方差尽可能小，计算N-1个IMU和第i个IMU的信息的求和，得到第i个子IMU位置信息融合的二次目标函数并令其最小：

其中，为经过传递对准的估计值，为第i个IMU的位置S_i经过传递对准的估计值；

对求偏导并令其为零：

得到第i个子IMU的位置融合结果为：

步骤520：根据以下公式，确定速度信息融合：

第i个子IMU所在节点相对于主POS的杆臂速度为：

第c个子IMU所在节点相对于主POS的杆臂速度为：

第c个子IMU所在节点与第i个子IMU所在节点之间的杆臂速度之差为：

用第c个子IMU的速度V_c表示第i个子IMU的速度及其协方差矩阵分别为：

其中，分别为V_c、的协方差矩阵，为与V_c的互协方差矩阵，为第c个子IMU导航坐标系到第i个子IMU导航坐标系的方向余弦矩阵，的计算过程如下：

则的计算式为：

其中， P_c(22:24,22:24)_3×3为P_c的第22行到24行、第22列到24列的3×3的子矩阵，P_c(19:21,22:24)_3×3为P_c的第19行到21行、第22列到24列的3×3的子矩阵，P_c(19:21,4:6)_3×3为P_c的第19行到21行、第4列到6列的3×3的子矩阵，P_c(22:24,4:6)_3×3为P_c的第22行到24行、第4列到6列的3×3的子矩阵；P_i(22:24,22:24)_3×3为P_i的第22行到24行、第22列到24列的3×3的子矩阵，P_i(19:21,22:24)_3×3为P_i的第19行到21行、第22列到24列的3×3的子矩阵；

设计第i个子IMU速度信息融合的二次目标函数并令其最小：

其中，为经过传递对准的估计值，为V_i经过传递对准的估计值；

对求导并令偏导为零：

得到V_i的融合值为：

步骤530：根据以下公式，确定姿态信息融合：

第i个子IMU所在节点的角误差ζ_i为：

ζ_i＝θ_i+ρ_i；

第c个子IMU所在节点的角误差ζ_c为：

ζ_c＝θ_c+ρ_c；

第i个子IMU所在节点姿态的方向余弦矩阵为：

其中，

第c个子IMU的载体坐标系到其导航坐标系的方向余弦矩阵为：

其中，

建立第i个子IMU载体坐标系与其他子IMU载体坐标系之间的方向余弦矩阵由于各子IMU所在节点之间的安装误差角和挠曲变形角均为小角度，故忽略二阶小量的结果如下：

设

第i个子IMU载体坐标系与其导航坐标系之间的方向余弦矩阵为

解得：

其中，A_c＝[ψ_c′ θ_c′ γ_c′]^T，ψ_c′、θ_c′、γ_c′分别为第c个子IMU在第i个子IMU导航坐标系下解算的欧拉角，M_c的表达式为：

由于M_c矩阵由方向余弦矩阵的元素计算得出，包含三角函数、开平方等复杂运算，因此无法准确得出其对协方差矩阵的影响。由于方程第一项已经考虑了节点姿态误差的影响，这里可以忽略M_c中的误差，可得的协方差矩阵为：

其中，A_c的协方差矩阵P_c(1:3,1:3)_3×3为P_c第1行到3行、第1列到3列的3×3的子矩阵，ζ_c的协方差的计算式为：

ζ_c与A_c的互协方差矩阵的计算式为：

其中， P_c(16:18,16:18)_3×3为P_c的第16行到18行、第16列到18列的3×3的子矩阵，P_c(19:21,16:18)_3×3为P_c的第19行到21行、第16列到18列的3×3的子矩阵，P_c(19:21,1:3)_3×3为P_c的第19行到21行、第1列到3列的3×3的子矩阵，P_c(16:18,1:3)_3×3为P_c的第16行到18行、第1列到3列的3×3的子矩阵，P_i(16:18,16:18)_3×3为P_i的第16行到18行、第16列到18列的3×3的子矩阵，P_i(19:21,16:18)_3×3为P_i的第18行到21行、第16列到18列的3×3的子矩阵；

定义第i个子IMU姿态信息融合的二次目标函数

得第i个子IMU姿态的融合值为：

针对分布式POS中各节点子IMU的运动信息经传递对准后精度存在差异导致系统整体精度下降的问题，本发明提供一种机载分布式POS多节点信息融合方法，充分利用各子IMU经过传递对准得到的位置、速度、姿态以及安装误差角、挠曲变形角等信息对杆臂效应进行补偿，经过准确的坐标系变换，分别推导了位置信息融合、速度信息融合、姿态信息融合的目标函数，将子IMU传递对准得到的协方差矩阵的逆作为信息融合的权重矩阵，传递对准精度越高其协方差矩阵的逆越大，该子IMU在信息融合中所占的比重则越大，使得传递对准精度较低的子IMU通过信息融合提高精度，从而使得分布式POS系统的整体测量精度得到提升。

此外，本发明还提供一种机载分布式POS的多节点信息融合系统。如图3所示，所述机载分布式POS的多节点信息融合系统包括第一建模单元1、第二建模单元2、传递对准单元3、修正单元4及信息融合单元5。

所述第一建模单元1用于建立传递对准误差模型，所述传递对准误差模型包括子IMU惯导误差模型、主POS与各子IMU的主子系统间的角误差模型。

所述第二建模单元2用于针对每一个子IMU，分别计算该子IMU所处的导航坐标系下表示的主POS的姿态与该子IMU的姿态之差、主POS的速度与该子IMU的速度之差，分别得到的对应的姿态差和速度差，将姿态差和速度差作为系统量测量，建立该子IMU传递对准的数学模型。

所述传递对准单元3用于采用卡尔曼滤波方法，对该子IMU传递对准的数学模型进行传递对准，确定该子IMU的姿态误差、速度误差和位置误差，并得到该子IMU的安装误差角、挠曲变形角和挠曲变形角速率。

所述修正单元4用于根据该子IMU的姿态误差、速度误差和位置误差修正该子IMU的捷联解算结果，得到该子IMU的修正位置、修正速度和修正姿态。

所述信息融合单元5用于利用所估计的各子IMU的安装误差角、挠曲变形角和挠曲变形角速率补偿杆臂效应，分别将各子IMU的修正位置、修正速度和修正姿态进行信息融合，确定所述机载分布式POS的测量信息。

相对于现有技术，本发明机载分布式POS的多节点信息融合系统与上述机载分布式POS的多节点信息融合方法的有益效果相同，在此不再赘述。

本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。

本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上所述，本说明书内容不应理解为对本发明的限制。

Claims (6)

1.一种机载分布式位置姿态测量系统POS的多节点信息融合方法，所述机载分布式POS包括一个主POS和多个子惯性测量单元IMU，其特征在于，所述多节点信息融合方法包括：

建立传递对准误差模型，所述传递对准误差模型包括子IMU惯导误差模型、主POS与各子IMU的主子系统间的角误差模型；

针对每一个子IMU，分别计算该子IMU所处的导航坐标系下表示的主POS的姿态与该子IMU的姿态之差、主POS的速度与该子IMU的速度之差，分别得到的对应的姿态差和速度差，将姿态差和速度差作为系统量测量，建立该子IMU传递对准的数学模型；

采用卡尔曼滤波方法，对该子IMU传递对准的数学模型进行传递对准，确定该子IMU的姿态误差、速度误差和位置误差，并得到该子IMU的安装误差角、挠曲变形角和挠曲变形角速率；

根据该子IMU的姿态误差、速度误差和位置误差修正该子IMU的捷联解算结果，得到该子IMU的修正位置、修正速度和修正姿态；

利用所估计的各子IMU的安装误差角、挠曲变形角和挠曲变形角速率补偿杆臂效应，分别将各子IMU的修正位置、修正速度和修正姿态进行信息融合，确定所述机载分布式POS的测量信息。

2.根据权利要求1所述的机载分布式POS的多节点信息融合方法，其特征在于，所述建立传递对准误差模型，具体包括：

步骤11：建立子IMU惯导误差模型，所述子IMU惯导误差模型包括姿态角误差微分方程、速度误差微分方程、位置误差微分方程和惯性仪表误差微分方程；其中，o为地心惯性坐标系；e为地球坐标系；主POS和子IMU导航坐标系均为东北天地理坐标系，主POS的导航坐标系用n表示，第i个子IMU的导航坐标系用n_i表示，i＝1,2,…,N，N为子IMU的个数，计算导航坐标系用n′_i表示；载体坐标系原点为载体重心，x轴沿载体横轴向右，y轴沿载体纵轴向前，z轴沿载体竖轴向上，该坐标系固定在载体上，称为右前上载体坐标系，用m和b_i分别代表主POS和第i个子IMU的载体坐标系；

步骤111：根据以下公式，确定姿态角误差微分方程：

其中，为第i个子IMU的姿态失准角，和分别为东向、北向、天向失准角，下标E、N和U分别表示东向、北向和天向；为第i个子IMU导航坐标系相对地心惯性坐标系的角速度；为的误差；为第i个子IMU载体坐标系到其导航坐标系的方向余弦矩阵的估计值；为第i个子IMU的陀螺仪常值漂移，其中和分别为第i个子IMU载体坐标系x轴、y轴和z轴陀螺仪常值漂移；

步骤112：根据以下公式，确定速度误差微分方程：

其中，为第i个子IMU的速度，为第i个子IMU的速度误差，其中和分别为东向、北向和天向速度，和分别为东向、北向和天向速度误差；是第i个子IMU的比力，其中和分别为东向、北向和天向比力；和分别为第i个子IMU导航坐标系相对地球坐标系的角速度及其误差；和分别为第i个子IMU导航坐标系相对地球坐标系的角速度及其误差；为加速度计常值偏置，其中和分别为第i个子IMU载体坐标系x轴、y轴和z轴加速度计常值偏置；

步骤113：根据以下公式，确定位置误差微分方程：

其中，L_i、λ_i、h_i和δL_i、δλ_i、δh_i分别为第i个子IMU纬度、经度、高度和纬度误差、经度误差、高度误差；为纬度的一阶导数，为经度的一阶导数；和分别为第i个子IMU沿子午圈和卯酉圈的主曲率半径；

步骤114：根据以下公式，确定惯性仪表误差微分方程：

步骤12：建立主POS与各子IMU的主子系统间的角误差模型，所述主子系统间的角误差模型包括：固定安装误差角的微分方程、弹性变形角的微分方程、

步骤121：根据以下公式，确定固定安装误差角的微分方程：

其中，为第i个子IMU固定安装误差角，和分别为第i个子IMU载体坐标系x轴、y轴和z轴安装误差角；

步骤122：根据以下公式，确定弹性变形角的微分方程：

其中，为第i个子IMU载体坐标系第j轴上的弹性变形角，j＝x,y,z，为弹性变形角； 为二阶马尔科夫过程相关时间；为零均值白噪声，方差满足：

为弹性变形角的方差，和为描述弹性变形角θ_i的二阶马尔科夫过程的参数。

3.根据权利要求1所述的机载分布式POS的多节点信息融合方法，其特征在于，根据以下公式，建立各子IMU传递对准的数学模型：

其中，系统状态变量X_i为：

系统转移矩阵F_i可由第i个子IMU的传递对准误差模型确定；系统噪声其中和分别为第i个子IMU载体坐标系x轴、y轴、z轴陀螺仪和第i个子IMU载体坐标系x轴、y轴、z轴加速度计的随机误差，不包括随机常值误差；系统噪声W_i为零均值的高斯白噪声，其方差阵Q_i由陀螺仪常值漂移、加速度计常值偏置和二阶马尔科夫过程参数决定；系统噪声阵G_i的表达式为：

其中，为第i个子IMU载体坐标系到导航坐标系的方向余弦矩阵；

系统量测变量Z_i＝[δψ_i δθ_i δγ_i δV′_iE δV′_iN δV′_iU]^T，其中δψ_i、δθ_i、δγ_i和δV′_iE、δV′_iN、δV′_iU分别为第i个子IMU与主POS相对于第i个子IMU导航坐标系的航向角、俯仰角、横滚角之差和东向、北向、天向速度之差；量测噪声其中分别为主POS航向角、俯仰角、横滚角的量测噪声，分别为主POS东向、北向、天向速度的量测噪声；V_i为零均值的高斯白噪声，其方差阵R_i由主POS的姿态精度和速度精度决定；量测矩阵H_i为：

主POS载体坐标系相对于第i个子IMU导航坐标系的方向余弦矩阵为：

其中，为主POS载体坐标系相对于主POS导航坐标系的方向余弦矩阵，为主POS导航坐标系到第i个子IMU导航坐标系的方向余弦矩阵：

其中，为主POS导航坐标系相对于地球坐标系的方向余弦矩阵，L_m和λ_m分别为主POS的纬度和经度；为第i个子IMU导航坐标系相对于地球坐标系的方向余弦矩阵；

令记为矩阵T_i中第l行、第m列的元素，l＝1,2,3，m＝1,2,3；则量测矩阵中和的表达式为：

4.根据权利要求1所述的机载分布式POS的多节点信息融合方法，其特征在于，根据该子IMU的姿态误差、速度误差和位置误差修正该子IMU的捷联解算结果，得到该子IMU的修正位置、修正速度和修正姿态，具体包括：

根据t_k时刻估计出的失准角速度误差和位置误差δL_i、δλ_i、δh_i，修正第i个子IMU的姿态、速度和位置：

步骤41：根据以下公式，得到修正速度：

其中，和分别为第i个子IMU修正后的东向、北向和天向速度；和分别为第i个子IMU捷联解算得到的东向、北向和天向速度；和分别为t_k时刻卡尔曼滤波估计出的第i个子IMU捷联解算东向、北向和天向速度误差；

步骤42：根据以下公式，得到修正位置：

其中，和分别为第i个子IMU捷联解算得到的纬度、经度和高度；和分别为第i个子IMU修正后的纬度、经度和高度；δL_i、δλ_i和δh_i分别为t_k时刻卡尔曼滤波估计出的第i个子IMU捷联解算纬度、经度和高度误差；

步骤43：根据以下步骤，得到修正姿态，所述修正姿态包括航向角、俯仰角和横滚角：

步骤431：计算t_k时刻第i个子IMU导航坐标系n_i与计算导航坐标系n′_i间的转换矩阵和第i个子IMU载体坐标系b_i与真实导航坐标系n_i之间的转换矩阵

其中，为t_k时刻第i个子IMU捷联解算得到的姿态的方向余弦矩阵；

步骤432：由被更新后的第i个子IMU的方向余弦矩阵计算t_k时刻第i个子IMU的航向角ψ_s、俯仰角θ_s和横滚角γ_s：

其中，

T_lm为矩阵中第l行、第m列的元素，l＝1,2,3，m＝1,2,3；则第i个子IMU航向角ψ_i、俯仰角θ_i和横滚角γ_i的主值，即ψ_i主、θ_i主和γ_i主分别为：

由于航向角ψ_i、俯仰角θ_i和横滚角γ_i的取值范围分别定义为[0，2π]、[-π，+π]；那么，ψ_i、θ_i和γ_i的真值由以下公式确定：

θ_i＝θ_i主，

5.根据权利要求1-4中任一项所述的机载分布式POS的多节点信息融合方法，其特征在于，所述利用所估计的各子IMU的安装误差角、挠曲变形角和挠曲变形角速率补偿杆臂效应，分别将各子IMU的修正位置、修正速度和修正姿态进行信息融合，确定所述机载分布式POS的测量信息，具体包括：

步骤51：根据以下公式，确定位置信息融合：

第i个子IMU所在节点与主POS之间的动态杆臂r_i ⁿ为：

其中， 和分别为主POS沿子午圈和卯酉圈的主曲率半径，为主POS载体坐标系到主POS导航坐标系的方向余弦矩阵，为没有挠曲变形时第i个子IMU所在节点的杆臂长度在主POS载体坐标系下的投影，为第i个子IMU所在节点的挠曲变形角在主POS载体坐标系下的投影；

其他N-1个子IMU所在节点与主POS之间的动态杆臂为：

其中，为没有挠曲变形时第c个子IMU所在节点的杆臂长度在主载体坐标系下的投影，为第c个子IMU所在节点的挠曲变形角在主载体坐标系下的投影；

第c个子IMU所在节点与第i个子IMU所在节点之间的杆臂之差为：

用第c个子IMU的位置表示第i个子IMU的位置：

其中，S_c＝[L_c λ_c h_c]为第c个子IMU的位置，为主POS导航坐标系相对于第i个子IMU导航坐标系的方向余弦矩阵，的协方差矩阵为：

其中，为的协方差矩阵，为与S_c的互协方差矩阵，假设每个节点的挠曲运动均不相关，和的计算方法分别为：

其中，E[·]表示求取期望， P_c为传递对准中第c个子IMU的估计协方差矩阵，P_c(19:21,19:21)_3×3表示P_c第19行到第21行、第19列到21列的3×3的子矩阵，P_c(19:21,7:9)_3×3表示P_c第19行到第21行、第7列到9列的3×3的子矩阵，P_i(19:21,19:21)_3×3表示P_i第19行到21行、第19列到21列的3×3的子矩阵；

将第c个IMU传递对准得到的协方差矩阵的逆作为权重矩阵，确定第c个IMU在信息融合中所占的比重；

利用第c个IMU经过传递对准得到的位置信息、安装误差角和挠曲变形角信息，确定第i个IMU的位置估计值

令与第i个IMU位置真值S_i的方差尽可能小，对于第i个IMU经过传递对准得到的位置信息直接将其协方差矩阵的逆作为权重矩阵，令与S_i的方差尽可能小，计算N-1个IMU和第i个IMU的信息的求和，得到第i个子IMU位置信息融合的二次目标函数并令其最小：

其中，为经过传递对准的估计值，为第i个IMU的位置S_i经过传递对准的估计值；

对求偏导并令其为零：

得到第i个子IMU的位置融合结果为：

步骤52：根据以下公式，确定速度信息融合：

第i个子IMU所在节点相对于主POS的杆臂速度为：

第c个子IMU所在节点相对于主POS的杆臂速度为：

第c个子IMU所在节点与第i个子IMU所在节点之间的杆臂速度之差为：

用第c个子IMU的速度V_c表示第i个子IMU的速度V_i ^c及其协方差矩阵分别为：

其中，分别为V_c、的协方差矩阵，为与V_c的互协方差矩阵，为第c个子IMU导航坐标系到第i个子IMU导航坐标系的方向余弦矩阵，的计算过程如下：

则的计算式为：

其中， P_c(22:24,22:24)_3×3为P_c的第22行到24行、第22列到24列的3×3的子矩阵，P_c(19:21,22:24)_3×3为P_c的第19行到21行、第22列到24列的3×3的子矩阵，P_c(19:21,4:6)_3×3为P_c的第19行到21行、第4列到6列的3×3的子矩阵，P_c(22:24,4:6)_3×3为P_c的第22行到24行、第4列到6列的3×3的子矩阵；P_i(22:24,22:24)_3×3为P_i的第22行到24行、第22列到24列的3×3的子矩阵，P_i(19:21,22:24)_3×3为P_i的第19行到21行、第22列到24列的3×3的子矩阵；

设计第i个子IMU速度信息融合的二次目标函数并令其最小：

其中，为V_i ^c经过传递对准的估计值，为V_i经过传递对准的估计值；

对求导并令偏导为零：

得到V_i的融合值为：

步骤53：根据以下公式，确定姿态信息融合：

第i个子IMU所在节点的角误差ζ_i为：

ζ_i＝θ_i+ρ_i；

第c个子IMU所在节点的角误差ζ_c为：

ζ_c＝θ_c+ρ_c；

第i个子IMU所在节点姿态的方向余弦矩阵为：

其中，

第c个子IMU的载体坐标系到其导航坐标系的方向余弦矩阵为：

其中，

建立第i个子IMU载体坐标系与其他子IMU载体坐标系之间的方向余弦矩阵由于各子IMU所在节点之间的安装误差角和挠曲变形角均为小角度，故忽略二阶小量的结果如下：

设

第i个子IMU载体坐标系与其导航坐标系之间的方向余弦矩阵为

解得：

其中，A_c＝[ψ′_c θ′_c γ′_c]^T，ψ′_c、θ′_c、γ′_c分别为第c个子IMU在第i个子IMU导航坐标系下解算的欧拉角，M_c的表达式为：

忽略M_c中的误差，可得的协方差矩阵为：

其中，A_c的协方差矩阵P_c(1:3,1:3)_3×3为P_c第1行到3行、第1列到3列的3×3的子矩阵，ζ_c的协方差的计算式为：

ζ_c与A_c的互协方差矩阵的计算式为：

其中， P_c(16:18,16:18)_3×3为P_c的第16行到18行、第16列到18列的3×3的子矩阵，P_c(19:21,16:18)_3×3为P_c的第19行到21行、第16列到18列的3×3的子矩阵，P_c(19:21,1:3)_3×3为P_c的第19行到21行、第1列到3列的3×3的子矩阵，P_c(16:18,1:3)_3×3为P_c的第16行到18行、第1列到3列的3×3的子矩阵，P_i(16:18,16:18)_3×3为P_i的第16行到18行、第16列到18列的3×3的子矩阵，P_i(19:21,16:18)_3×3为P_i的第18行到21行、第16列到18列的3×3的子矩阵；

定义第i个子IMU姿态信息融合的二次目标函数

得第i个子IMU姿态的融合值为：

6.一种机载分布式POS的多节点信息融合系统，所述机载分布式POS包括一个主POS和多个子IMU，其特征在于，所述多节点信息融合方法系统：

第一建模单元，用于建立传递对准误差模型，所述传递对准误差模型包括子IMU惯导误差模型、主POS与各子IMU的主子系统间的角误差模型；

第二建模单元，用于针对每一个子IMU，分别计算该子IMU所处的导航坐标系下表示的主POS的姿态与该子IMU的姿态之差、主POS的速度与该子IMU的速度之差，分别得到的对应的姿态差和速度差，将姿态差和速度差作为系统量测量，建立该子IMU传递对准的数学模型；

传递对准单元，用于采用卡尔曼滤波方法，对该子IMU传递对准的数学模型进行传递对准，确定该子IMU的姿态误差、速度误差和位置误差，并得到该子IMU的安装误差角、挠曲变形角和挠曲变形角速率；

修正单元，用于根据该子IMU的姿态误差、速度误差和位置误差修正该子IMU的捷联解算结果，得到该子IMU的修正位置、修正速度和修正姿态；

信息融合单元，用于利用所估计的各子IMU的安装误差角、挠曲变形角和挠曲变形角速率补偿杆臂效应，分别将各子IMU的修正位置、修正速度和修正姿态进行信息融合，确定所述机载分布式POS的测量信息。