CN103196448B - 一种机载分布式惯性测姿系统及其传递对准方法 - Google Patents

Abstract

本发明为一种机载分布式惯性测姿系统及其传递对准方法。该系统包括1个主惯导系统、1个主控计算机以及6个与该主控计算机连接的子惯导系统：每个子惯导系统分别包括惯性测量单元和测姿处理板，惯性测量单元与测姿处理板的RS_422接口连接；RS_422接口通过UART控制芯片与FPGA芯片连接，FPGA芯片通过EMIF总线与DSP芯片连接，并且还通过CAN控制器、CAN收发器与主惯导系统连接；主控计算机与主惯导系统连接。该系统主、子惯导系统之间的传递对准方法为：以主、子惯导系统的速度信息误差和姿态信息误差作为量测变量，进行卡尔曼滤波迭代后对子惯导系统计算出的速度信息、姿态信息进行校正，最终得到稳定、精确的航姿信息。

Description

一种机载分布式惯性测姿系统及其传递对准方法

一、技术领域

本发明属于惯性测姿技术领域，特别是一种机载分布式惯性测姿系统及其传递对准方法。

二、背景技术

面对瞬息万变的空中态势，飞行员必须以最敏捷的方式发现目标，并选择最有效的方式发起攻击。为了增强飞行员对外界的视野观察范围，通过在飞机上安装6个用于成像的光学传感器，提供360°的全维态势感知能力，最终能够搜集360°范围内的各种信息。然而用于成像的光学探测器的瞄准线必定随载机振动而产生一定幅度的随机抖动。这种振动会使得图像产生明显的晃动，对几个探测器间的图像拼接也会产生很大影响，因此在每个光学传感器的位置加装一个专门用于测姿的IMU（惯性测量单元，InertialMeasurementUnit）系统是很必要的。IMU惯性测姿系统能够精确的实时测量到安装部位的惯性姿态，利用这些惯性数据就可以将几个不同的传感器的图像进行无缝的拼接，并且为图像的稳定性提供了基础参考标准。

如果在每个传感器位置都安装一个高精度IMU，精度能达到长时间飞行的要求，但要安装6个高精度IMU，将使成本大幅度上升，且高精度IMU体积必然很庞大，根本满足不了机载系统对有限空间的要求；如果只是在每个传感器位置都安装一个低成本、体积小的IMU，由于其精度不高，测量误差随时间积累，精度也难以达到要求。

三、发明内容

本发明的目的是提供一种稳定性好、测量精度高、成本低的机载分布式惯性测姿系统及其传递对准方法。

实现本发明目的的技术解决方案为：一种机载分布式惯性测姿系统，包括1个主惯导系统、1个主控计算机以及6个与该主控计算机连接的子惯导系统，各个子惯导系统之间相互独立，且每个子惯导系统的结构相同：

所述每个子惯导系统分别包括惯性测量单元和测姿处理板，其中：惯性测量单元包括三个MEMS陀螺仪、三个MEMS加速度计、A/D转换器和温补系统；测姿处理板集成了DSP芯片、FPGA芯片、FLASH、SDRAM、CAN控制器、CAN收发器、UART控制芯片、RS_422接口、RS_232接口和EMIF总线；所述三个MEMS陀螺仪、三个MEMS加速度计通过A/D转换器和温补系统相连，温补系统与测姿处理板的RS_422接口连接；RS_422接口通过UART控制芯片与FPGA芯片连接，FPGA芯片通过EMIF总线与DSP芯片连接，并且FLASH、SDRAM都与EMIF总线连接，FPGA芯片还通过CAN控制器与CAN收发器连接；CAN收发器与主控计算机连接；主控计算机与主惯导系统连接；UART控制芯片还与RS_232接口连接；

主惯导系统生成载体导航信息后，先发送给主控计算机，主控计算机再通过CAN收发器发送给测姿处理板，CAN收发器将物理总线上收到的差分电平数据转换为TTL电平数据发送给CAN控制器，FPGA芯片通过CAN控制器接收数据，FPGA芯片通过EMIF总线将接收到的数据上传至DSP芯片；三个MEMS陀螺仪敏感载体的三轴角速度信息，三个MEMS加速度计敏感载体的三轴加速度信息，所得的三轴角速度信息和三轴加速度信息传输到A/D转换器，A/D转换器将得到的模拟量信息转换成数字量信息，并将该数字量信息通过温补系统补偿得到稳定的数字量信息；测姿处理板通过RS_422接口读取稳定的数字量信息，并传输给UART控制芯片，UART控制芯片将串行数据转换为并行数据并发送给FPGA芯片，FPGA芯片通过EMIF总线将接收到的数据上传至DSP芯片；FPGA芯片通过EMIF总线接收DSP芯片解算得到的惯性姿态数据，并将惯性姿态数据发送给UART控制芯片，UART控制芯片将并行数据转换为串行数据并行数据后发送给RS_232接口进行电平转换，并通过RS_232接口发给外接设备。

本发明机载分布式惯性测姿系统的传递对准方法，包括以下步骤：

第一步：系统上电后完成系统初始化工作，其中包括主、子惯导系统安装误差角对应的补偿角的初始化，然后进入下一步；

第二步：主惯导系统测得载体的速度信息、姿态信息、角速度信息、比力信息和位置信息，并利用第一步所得的补偿角修正该姿态信息；

第三步：进行粗对准，把第二步主惯导系统的速度信息、位置信息、补偿角修正后的姿态信息赋给子惯导系统的对应参数，作为子惯导系统的解算初值；

第四步：子惯导系统以第三步粗对准后的导航信息为初值，进行惯导捷联解算，得到子惯导系统的位置信息、速度信息和姿态信息；

第五步：主惯导系统数据经过时间同步和杆臂补偿等处理后，采用“速度+姿态”的匹配方式建立卡尔曼滤波模型，以第二步中主惯导系统与第四步子惯导系统之间的速度差值和姿态差值作为量测变量，进行卡尔曼滤波迭代，估计出子惯导系统速度误差、平台失准角、安装误差角、陀螺常值漂移和加速度计常值偏置；

第六步：将第五步估计出的子惯导系统速度误差、平台失准角，对第四步子惯导系统计算出的速度信息、姿态信息进行校正，最终获得子惯导系统经校正后的速度信息和姿态信息。

本发明与现有技术相比，其显著优点是：

1、本发明中所用到的6个子惯导均是低成本的MEMS（微电子机械系统）惯性测量单元，它不仅体积小而且具有成本低、集成度高等优点，它克服了以往所采用的子惯导成本高、体积大的缺点，更容易满足机载系统低成本、小型化的要求。

2、本发明以一个高精度主惯导与多个低精度子惯导进行分布式安装，因此主子惯导之间必然会以大安装误差角的方式分布在飞机上的6个不同部位。对于主子惯导安装的特殊情况，引入一种大安装误差角情况下的分布式传递对准算法，克服了以往主子惯导之间只能以小安装误差角方式安装的缺点。

3、本发明将六个子惯导系统测的惯性数据传给主控计算机，经过时间同步和信息融合算法处理后，即可实现全空域图像的无缝拼接。

四附图说明

图1本发明机载分布式惯性测姿系统的主子惯导系统结构图

图2是本发明机载分布式惯性测姿系统硬件模块框图。

图3是本发明机载分布式惯性测姿系统的传递对准方法的流程图。

图4是本发明的“姿态+速度”匹配传递对准算法原理框图。

图5是本发明机载分布式惯性测姿系统的传递对准跑车实验的硬件组成结构图。

图6是本发明实施例1的传递对准跑车实验安装误差角估计曲线图。

图7是本发明实施例2的传递对准跑车实验安装误差角估计曲线图。

图8是本发明实施例3的传递对准跑车实验安装误差角估计曲线图。

图9是本发明实施例4的传递对准跑车实验安装误差角估计曲线图。

五具体实施方式

以下结合附图，详细说明本发明的实施方式。

结合图1，本发明机载分布式惯性测姿系统，包括1个主惯导系统、1个主控计算机以及6个与该主控计算机连接的子惯导系统，各个子惯导系统之间相互独立，分别安装在飞机上不同的位置，且每个子惯导系统与主惯导系统的连接结构相同。其中，主惯导系统由高精度的主惯导和GPS进行组合导航，实时提供高精度的导航信息，并发给主控计算机；主控计算机一方面负责实时接收主惯导系统数据，然后发送给各子惯导系统，另一方面还负责接收各子惯导系统的惯性数据，经过时间同步和信息融合算法处理后，即可实现全空域图像的无缝拼接；子惯导系统是由低精度MEMS惯性测量单元和测姿处理板组成。各子惯导系统都是相同的设计，主要负责主、子惯导系统间动基座传递对准算法的实现，以提高子惯导系统的导航精度，而且能够实时输出平台高精度的航向、姿态、位置和速度信息。

结合图2，本发明的机载分布式惯性测姿系统硬件模块框图，从图中可以看出子惯导系统的具体硬件结构、以及子惯导系统与主控计算机、主惯导系统之间的连接关系：每个子惯导系统分别包括惯性测量单元和测姿处理板，其中：惯性测量单元包括三个MEMS陀螺仪、三个MEMS加速度计、A/D转换器和温补系统；测姿处理板集成了DSP芯片、FPGA芯片、FLASH、SDRAM、CAN控制器、CAN收发器、UART控制芯片、RS_422接口、RS_232接口和EMIF总线；所述三个MEMS陀螺仪、三个MEMS加速度计通过A/D转换器和温补系统相连，温补系统与测姿处理板的RS_422接口连接；RS_422接口通过UART控制芯片与FPGA芯片连接，FPGA芯片通过EMIF总线与DSP芯片连接，并且FLASH、SDRAM都与EMIF总线连接，FPGA芯片还通过CAN控制器与CAN收发器连接；CAN收发器与主控计算机连接；主控计算机与主惯导系统连接；UART控制芯片还与RS_232接口连接；

主惯导系统生成载体导航信息后，先发送给主控计算机，主控计算机再通过CAN收发器发送给测姿处理板，CAN收发器将物理总线上收到的差分电平数据转换为TTL电平数据发送给CAN控制器，FPGA芯片通过CAN控制器接收数据，FPGA芯片通过EMIF总线将接收到的数据上传至DSP芯片；三个MEMS陀螺仪敏感载体的三轴角速度信息，三个MEMS加速度计敏感载体的三轴加速度信息，所得的三轴角速度信息和三轴加速度信息传输到A/D转换器，A/D转换器将得到的模拟量信息转换成数字量信息，并将该数字量信息通过温补系统补偿得到稳定的数字量信息；测姿处理板通过RS_422接口读取稳定的数字量信息，并传输给UART控制芯片，UART控制芯片将串行数据转换为并行数据并发送给FPGA芯片，FPGA芯片通过EMIF总线将接收到的数据上传至DSP芯片；FPGA芯片通过EMIF总线接收DSP芯片解算得到的惯性姿态数据，并将惯性姿态数据发送给UART控制芯片，UART控制芯片将并行数据转换为串行数据并行数据后发送给RS_232接口进行电平转换，并通过RS_232接口发给外设。

结合图3、图4，本发明机载分布式惯性测姿系统的传递对准方法，包括以下步骤：

第一步：系统上电后完成系统初始化工作。所述的初始化工作包括硬件初始化和软件初始化，其中，硬件初始化包括：定时器，CAN总线等硬件的初始化；软件初始化包括：卡尔曼滤波器初始化，即系统初始状态X、系统状态协方差阵P、量测噪声方差阵R、系统过程噪声方差阵Q等的初始化，主、子惯导系统安装误差角对应的补偿角ξ_x、ξ_y、ξ_z的初始化。

第二步：主惯导系统测得载体在导航坐标系下的导航信息，所述导航信息包括：速度信息、姿态信息、角速度信息、比力信息和位置信息。其中速度信息包括：主惯导系统的东向速度V_E、主惯导系统的北向速度V_N、主惯导系统的天向速度V_U，姿态信息包括：俯仰角θ、横滚角γ、航线角ψ，角速度信息包括：主惯导系统的东向角速度ω_E、主惯导系统的北向角速度ω_N、主惯导系统的天向角速度ω_U，比力信息包括：主惯导系统的东向比力f_E、主惯导系统的北向比力f_N、主惯导系统的天向比力f_U，位置信息包括主惯导系统的经度λ、主惯导系统的纬度L、主惯导系统的高度h。所述姿态信息为主惯导系统在大安装误差角下测得的姿态角，则其对应的姿态阵：

${\hat{C}}_b^n=\left[\begin{array}{ccc}\cos \mathrm{\γ}\cos \mathrm{\ψ}-\sin \mathrm{\γ}\sin \mathrm{\θ}\sin \mathrm{\ψ}& -\cos \mathrm{\θ}\sin \mathrm{\ψ}& \sin \mathrm{\γ}\cos \mathrm{\ψ}+\cos \mathrm{\γ}\sin \mathrm{\θ}\sin \mathrm{\ψ}\\ \cos \mathrm{\γ}\sin \mathrm{\ψ}+\sin \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}& \cos \mathrm{\θ}\cos \mathrm{\ψ}& \sin \mathrm{\γ}\cos \mathrm{\ψ}-\cos \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}\\ -\sin \mathrm{\γ}\cos \mathrm{\θ}& \sin \mathrm{\θ}& \cos \mathrm{\γ}\cos \mathrm{\θ}\end{array}\right]$

由第一步知：主、子惯导系统安装误差角对应的补偿角ξ_x、ξ_y、ξ_z，则补偿角构成的姿态阵为：

$C_m^t=\left[\begin{array}{ccc}\cos {\mathrm{\ξ}}_y\cos {\mathrm{\ξ}}_z-\sin {\mathrm{\ξ}}_y\sin {\mathrm{\ξ}}_x\sin {\mathrm{\ξ}}_z& -\cos {\mathrm{\ξ}}_x\sin {\mathrm{\ξ}}_z& \sin {\mathrm{\ξ}}_y\cos {\mathrm{\ξ}}_z+\cos {\mathrm{\ξ}}_y\sin {\mathrm{\ξ}}_x\sin {\mathrm{\ξ}}_z\\ \cos {\mathrm{\ξ}}_y\sin {\mathrm{\ξ}}_z+\sin {\mathrm{\ξ}}_y\sin {\mathrm{\ξ}}_x\cos {\mathrm{\ξ}}_z& \cos {\mathrm{\ξ}}_x\cos {\mathrm{\ξ}}_z& \sin {\mathrm{\ξ}}_y\cos {\mathrm{\ξ}}_z-\cos {\mathrm{\ξ}}_y\sin {\mathrm{\ξ}}_x\cos {\mathrm{\ξ}}_z\\ -\sin {\mathrm{\ξ}}_y\cos {\mathrm{\ξ}}_x& \sin {\mathrm{\ξ}}_x& \cos {\mathrm{\ξ}}_y\cos {\mathrm{\ξ}}_x\end{array}\right]$

经过补偿角姿态阵修正后得到新的姿态阵由可得新的姿态角：俯仰角横滚角航线角作为子惯导系统的姿态基准。

第三步：完成粗对准：把第二步的主惯导系统速度、位置以及经过补偿角姿态阵修正过的姿态信息赋给子惯导系统对应参数，作为子惯导系统解算的初值。粗对准完成后，进行下一步。

第四步：子惯导系统中三个MEMS陀螺仪敏感载体的三轴角速度信息和三个MEMS加速度计敏感载体的三轴加速度信息以粗对准后的导航信息为初值，进行惯导捷联解算，得到子惯导系统的位置信息、速度信息和姿态信息。所述子惯导系统的位置信息、速度信息和姿态信息分别包括：子惯导系统的位置信息（经度λ₀、纬度L₀、高度h₀），子惯导系统的速度在载体系X、Y、Z轴上的分量V_X、V_y、V_z，姿态信息（俯仰角θ₀、横滚角γ₀、航线角ψ₀）；

第五步：主惯导数据经过时间同步和杆臂补偿等处理后，采用“速度+姿态”的匹配方式建立卡尔曼滤波模型，以第二步中主惯导系统与第四步子惯导系统的速度差值和姿态差值作为量测变量，进行卡尔曼滤波迭代，估计出子惯导系统速度误差、平台失准角、安装误差角、陀螺常值漂移和加速度计常值偏置。卡尔曼滤波的滤波周期为1s。

所述采用“速度+姿态”的匹配方式建立卡尔曼滤波模型，其具体步骤为：

1.建立传递对准系统的状态方程

姿态误差方程为：

$\left\{\begin{array}{c}{\stackrel{\·}{\mathrm{\φ}}}_E={\mathrm{\φ}}_N{w}_{\mathrm{ie}}\sin L+{\mathrm{\φ}}_N{V}_E\tan L/(R_N+h)-{\mathrm{\φ}}_U{w}_{\mathrm{ie}}\cos L\\ -{\mathrm{\φ}}_U{V}_E/(R_N+h)-\mathrm{\δ}{V}_N/(R_M+h)+C_b^n{\mathrm{\ϵ}}_x\\ {\stackrel{\·}{\mathrm{\φ}}}_N=-{\mathrm{\φ}}_E{w}_{\mathrm{ie}}\sin L-{\mathrm{\φ}}_E{V}_E\tan L/(R_N+h)\\ -{\mathrm{\φ}}_U{V}_N/(R_M+h)+\mathrm{\δ}{V}_E/(R_N+h)+C_b^n{\mathrm{\ϵ}}_y\\ {\stackrel{\·}{\mathrm{\φ}}}_U={\mathrm{\φ}}_E{w}_{\mathrm{ie}}\cos L+{\mathrm{\φ}}_E{V}_E/(R_N+h)+{\mathrm{\φ}}_N{V}_N/(R_M+h)\\ +\mathrm{\δ}{V}_E\tan L/(R_N+h)+C_b^n{\mathrm{\ϵ}}_z\end{array}\right.---\left(1\right)$

速度误差方程为：

$\left\{\begin{array}{c}\mathrm{\δ}{\stackrel{\·}{V}}_E=-{\mathrm{\φ}}_N{f}_U+{\mathrm{\φ}}_U{f}_N+\mathrm{\δ}{V}_E{V}_N\tan L/(R_M+h)\\ -\mathrm{\δ}{V}_E{V}_U/(R_M+h)+2\mathrm{\δ}{V}_N{w}_{\mathrm{ie}}\sin L+{\mathrm{\δV}}_N{V}_E\tan L/(R_N+h)\\ -2\mathrm{\δ}{V}_U{w}_{\mathrm{ie}}\cos L-\mathrm{\δ}{V}_U{V}_E/(R_N+h)+C_b^n{\▿}_x\\ \mathrm{\δ}{\stackrel{\·}{V}}_N={\mathrm{\φ}}_E{f}_U-{\mathrm{\φ}}_U{f}_E-2\mathrm{\δ}{V}_E{w}_{\mathrm{ie}}\sin L-2\mathrm{\δ}{V}_E{V}_E\tan L/(R_N/h)\\ -\mathrm{\δ}{V}_N{V}_U/(R_M/h)-\mathrm{\δ}{V}_U{V}_N/(R_M/h)+C_b^n{\▿}_y\\ \mathrm{\δ}{\stackrel{\·}{V}}_U=-{\mathrm{\φ}}_E{f}_N+{\mathrm{\φ}}_N{f}_E+2\mathrm{\δ}{V}_E{w}_{\mathrm{ie}}\cos L+2\mathrm{\δ}{V}_E{V}_E(R_N+h)\\ +2\mathrm{\δ}{V}_N{V}_N/(R_M/h)+C_b^m{\▿}_z\end{array}\right.---\left(2\right)$

(1)、(2)式中：

φ_E是子惯导系统的东向平台失准角，φ_N是子惯导系统的北向平台失准角，φ_U是子惯导系统的天向平台失准角；

δV_E是子惯导系统的东向速度误差，δV_N是子惯导系统的北向速度误差，δV_U是子惯导系统的天向速度误差；

V_E为主惯导系统的东向速度,V_N为主惯导系统的北向速度，V_U为主惯导系统的天向速度；

f_E为主惯导系统的东向比力,f_N为主惯导系统的北向比力，f_U为主惯导系统的天向比力；

L为主惯导系统的纬度，R_M是地球子午圈主曲率半径，R_N是地球卯酉圈主曲率半径，h是高度，ω_ie是地球自转角速度；

ε_x为子惯导系统x轴陀螺常值漂移，ε_y为子惯导系统y轴陀螺常值漂移，ε_z为子惯导系统z轴陀螺常值漂移；

▽_x为子惯导系统x轴加速度计常值偏置,▽_y为子惯导系统y轴加速度计常值偏置，▽_z为子惯导系统z轴加速度计常值偏置；

惯性器件误差方程：

${\stackrel{\·}{\mathrm{\ϵ}}}_i=0,(i=x,y,z)---\left(3\right)$

${\stackrel{\·}{\▿}}_i=0,(i=x,y,z)---\left(4\right)$

安装误差角方程为：

${\stackrel{\·}{\mathrm{\λ}}}_i=0,(i=x,y,z)---\left(5\right)$

其中，λ_x为子惯导系统x轴安装误差角，λ_y为子惯导系统y轴安装误差角，λ_z为子惯导系统z轴安装误差角；

由(1)～(5)式可得系统状态方程：

$\stackrel{\·}{X}=\mathrm{FX}+\mathrm{GW}---\left(6\right)$

式中X为系统状态变量：

X＝[φ_Eφ_Nφ_UδV_EδV_NδV_Uε_xε_yε_z▽_x▽_y▽_zλ_xλ_yλ_z]^T；

F为系统状态转移矩阵：

$F={\left[\begin{array}{ccccc}& & C_b^n& \underset{3\×3}{0}& 0_{3\×3}\\ \underset{(6\×3)}{F_1}& \underset{(6\×3)}{F_2}& \underset{3\×3}{0}& C_b^n& 0_{3\×3}\\ & 0_{9\×6}& & & 0_{9\×9}\end{array}\right]}_{15\×15}$

其中，矩阵块F₁与F₂如下所示：

$F_1={\left[\begin{array}{ccc}0& {\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E}{R_N+h}\tan L& -({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+h})\\ -({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E}{R_N+h}\tan L)& 0& \frac{V_N}{R_M+h}\\ {\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+h}& \frac{V_N}{R_M+h}& 0\\ 0& -f_U& f_U\\ f_U& 0& -f_E\\ -f_N& f_E& 0\end{array}\right]}_{6\×3}$

$F_2={\left[\begin{array}{ccc}0& \frac{1}{R_M+h}& 0\\ \frac{V_N}{R_M+h}& \frac{1}{R_N+h}& 0\\ \frac{\tan L}{R_N+h}& 0& 0\\ \frac{V_N}{R_M+h}\tan L-\frac{V_U}{R_M+h}& 2{\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E}{R_N+h}\tan L& -(2{\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+h})\\ -2({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E}{R_N+h}\tan L)& \frac{V_U}{R_M+h}& -\frac{V_N}{R_M+h}\\ 2({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+h})& 0& \frac{2V_N}{R_M+h}\end{array}\right]}_{6\×3}$

G为系统噪声驱动矩阵:

$G={\left[\begin{array}{c}\begin{array}{cc}{C}_b^n& 0\\ 0& C_b^n\end{array}\\ \underset{(9\×6)}{0}\end{array}\right]}_{15\×6}$

W为系统噪声矩阵：

W＝[W_GxW_GyW_GzW_ΔxW_ΔyW_Δz]^T _6×1

W_Gx是x轴陀螺噪声，W_Gy是y轴陀螺噪声，W_Gz是z轴陀螺噪声；

W_Δx是x轴加表噪声，W_Δy是y轴加表噪声，W_Δz是z轴加表噪声；

并假设其为零均值高斯白噪声，协方差阵为E[WW^T]＝Q。

2.建立传递对准系统的量测方程

取主惯导系统与子惯导系统的姿态角差值和速度差值作为量测变量，完整的量测方程为：

Z＝HX+V(7)

式中Z为量测变量，Z＝[δθδγδΨδV′_EδV′_NδV′_U]，其中，δθ、δγ、δψ分别是主、子惯导系统间的俯仰角差值、横滚角差值和航向角差值，δV′_E、δV′_N、δV′_U分别是主、子惯导系统间的东向、北向和天向速度差值。

V是量测噪声，并假定其为均值为零的高斯白噪声，其协方差为E[VV^T]＝R。系统过程噪声方差阵Q和量测噪声方差阵R由系统误差模型及飞机机动性决定取值。

H为量测矩阵：

${H=\left[\begin{array}{ccccc}{H}_{11}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& H_{15}\\ 0_{3\×3}& I_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\end{array}\right]}_{6\×15}$

其中，

$H_{11}=\left[\begin{array}{ccc}-\frac{T_{22}}{\sqrt{1-{T_{32}}^2}}& \frac{T_{12}}{\sqrt{1-{T_{32}}^2}}& 0\\ \frac{T_{21}{T}_{33}-T_{31}{T}_{23}}{{T_{33}}^2+{T_{31}}^2}& \frac{T_{31}{T}_{13}-T_{11}{T}_{33}}{{T_{33}}^2+{T_{31}}^2}& 0\\ \frac{T_{12}{T}_{32}}{{T_{12}}^2+{T_{22}}^2}& \frac{T_{22}{T_{31}}^2}{{T_{12}}^2+{T_{22}}^2}& -1\end{array}\right]$

$H_{15}=\left[\begin{array}{ccc}\frac{T_{33}}{\sqrt{1-{T_{32}}^2}}& 0& \frac{-T_{31}}{\sqrt{1-{T_{32}}^2}}\\ \frac{-T_{31}{T}_{32}}{{T_{33}}^2+{T_{31}}^2}& 1& \frac{-T_{32}{T}_{33}}{{T_{33}}^2+{T_{31}}^2}\\ \frac{T_{12}{T}_{23}-T_{13}{T}_{22}}{{T_{12}}^2+{T_{22}}^2}& 0& \frac{T_{11}{T}_{22}-T_{12}{T}_{21}}{{T_{12}}^2+{T_{22}}^2}\end{array}\right]$

3.根据传递对准系统的状态方程和量测方程，建立离散化的系统状态方程和量测方程为：

X_k＝Φ_k,k-1X_k-1+W_k-1(8)

Z_k＝H_kX_k+V_k

其中，X_k表示k时刻的被估计状态，Φ_k,k-1为k-1时刻到k时刻的一步转移矩阵，W_k-1为k-1时刻的系统激励噪声序列，Z_k为k时刻的量测向量，H_k为k时刻的量测矩阵，V_k为k时刻的量测噪声序列。

${\mathrm{\Φ}}_{k,k-1}=I+\mathrm{F\Δt}+\frac{1}{2!}{\left(\mathrm{F\Δt}\right)}^2+\frac{1}{3!}{\left(\mathrm{F\Δt}\right)}^3+\frac{1}{4!}{\left(\mathrm{F\Δt}\right)}^4+...$

$Q_k=\stackrel{\‾}{Q}\mathrm{\Δt}+[F\stackrel{\‾}{Q}+{\left(F\stackrel{\‾}{Q}\right)}^T]\frac{{\mathrm{\Δt}}^2}{2}+\left\{F\right[F\stackrel{\‾}{Q}+{\left(F\stackrel{\‾}{Q}\right)}^T]+[F{(F\stackrel{\‾}{Q}+{\left(F\stackrel{\‾}{Q}\right)}^T)}^T\left]\right\}\frac{{\mathrm{\Δt}}^3}{3!}...$

上式中Δt为滤波更新周期，

卡尔曼滤波基本算法编排，根据上述系统状态方程和量测方程，可建立离散卡尔曼滤波方程如下：

①状态一步预测方程：

${\hat{x}}_{k|k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{x}}_{k-1}---\left(9\right)$

其中，为k时刻系统状态一步预测值，为k-1时刻系统状态估计值，Φ_k,k-1为k-1时刻到k时刻的系统状态转移矩阵；

②状态估值计算方程：

${\hat{X}}_k={\hat{X}}_{k|k-1}+K_k(Z_k-H_k{\hat{X}}_{k|k-1})---\left(10\right)$

其中，为k时刻系统状态估计值、K_k为k时刻系统增益矩阵、Z_k为k时刻量测向量、H_k为k时刻系统量测矩阵；

③滤波增益方程：

$K_k=P_{k|k-1}{H}_k^T{(H_k{P}_{k|k-1}{H}_k^T+R_k)}^{-1}---\left(11\right)$

其中，P_k|k-1为k时刻系统协方差阵的一步预测、R_k为k时刻系统量测噪声矩阵；

④一步预测均方误差方程：

$P_{k|k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+{\mathrm{\Γ}}_{k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k-1}^T---\left(12\right)$

其中，P_k|k-1为k-1时刻系统状态协方差阵、Q_k-1为k-1时刻系统噪声矩阵、Γ_k-1为k-1时刻系统噪声驱动矩阵；

⑤估计均方误差方程：

$P_k=[I-K_k{H}_k]P_{k,k-1}]I-K_k{H}_k{]}^T+K_k{R}_k{K}_k^T---\left(13\right)$

其中，P_k为k时刻系统状态协方差阵，I是单位阵。

第六步：将第五步估计出的子惯导速度误差和平台失准角，对第四步子惯导系统计算出的速度信息、姿态信息进行校正，最终获得子惯导系统经校正后的速度信息和姿态信息。

实施例1

结合图5，本发明机载分布式惯性测姿系统的传递对准跑车实验的硬件组成结构图：把主、子惯导以大安装误差角的方式安装在一块铁板上，模拟飞机上的安装位置，主惯导系统产生高精度导航参数，通过串口以一定频率发送给主控计算机，主控计算机将接收到的导航参数转换成CAN总线数据发送给子惯导系统导航计算机，与子惯导系统一起完成传递对准，实时输出高精度导航信息。

鉴于飞机上不同位置子惯导和主惯导之间的安装误差角必定很大，且实际的安装误差角未知，故先用测姿设备大概测出z轴安装误差角为32°左右。在软件初始化时，给z轴安装误差角一个对应的补偿角，每次收到的主惯导系统的姿态信息都需经过补偿角姿态矩阵修正后才能作为子惯导系统的姿态基准。

设z轴安装误差角补偿角ξ_z=32.5°，第一次传递对准跑车实验安装误差角估计曲线图如图6所示。

实施例2

与实施例1相同，设z轴安装误差角补偿角ξ_z=32.5°，第二次传递对准跑车实验安装误差角估计曲线图如图7所示。

实施例3

与实施例1相同，不同之处在于，设z轴安装误差角补偿角ξ_z=32°，传递对准跑车实验安装误差角估计曲线图如图8所示。

实施例4

与实施例1相同，不同之处在于，设z轴安装误差角补偿角ξ_z=31.5°，传递对准跑车实验安装误差角估计曲线图如图9所示。

表1

名称	z轴补偿角	x轴安装误差角	y轴安装误差角	z轴安装误差角
					实施例1	32.5°	5′	-13′	-23.3′
实施例2	32.5°	5.5′	-13.5′	-24.2′
					实施例3	32°	5′	-12′	4.2′
实施例4	31.5°	4.5′	-11.5′	38.3′

结合表1，实施例1～实施例4的主子惯导系统安装误差角估计值统计特性，实施例1、实施例2、实施例3、实施例4的z轴安装误差角分别补偿32.5°、32.5°、32°和31.5°，从实施例1、实施例2的数据结果分析可得两次实验的x、y轴安装误差角估计精度在0.5′以内，z轴安装误差角估计精度在0.9′以内，可证明该系统的稳定性很高。其中实施例1和实施例3的z轴安装误差角补偿角相差0.5°即30′，从图6和图8可以看出z轴安装误差角仅相差27.5′。其中实施例1和实施例4的z轴安装误差角补偿角相差1°即60′，从图6和图9可以看出z轴安装误差角仅相差61.6′，故表1可证明本发明机载分布式惯性测姿系统的x轴安装误差角精度在0.5′以内，y轴安装误差角精度在2′以内，z轴安装误差角测量精度在3′以内。

从本发明机载分布式惯性测姿系统的主子惯导系统传递对准跑车实验的数据分析可以看出，该系统稳定性好、测量精度高，完全满足机载光电平台的要求。

Claims (5)

1.一种机载分布式惯性测姿系统，其特征在于，包括1个主惯导系统、1个主控计算机以及6个与该主控计算机连接的子惯导系统，各个子惯导系统之间相互独立，且每个子惯导系统的结构相同：

所述每个子惯导系统分别包括惯性测量单元和测姿处理板，其中：惯性测量单元包括三个MEMS陀螺仪、三个MEMS加速度计、A/D转换器和温补系统；测姿处理板集成了DSP芯片、FPGA芯片、FLASH、SDRAM、CAN控制器、CAN收发器、UART控制芯片、RS_422接口、RS_232接口和EMIF总线；所述三个MEMS陀螺仪、三个MEMS加速度计通过A/D转换器和温补系统相连，温补系统与测姿处理板的RS_422接口连接；RS_422接口通过UART控制芯片与FPGA芯片连接，FPGA芯片通过EMIF总线与DSP芯片连接，并且FLASH、SDRAM都与EMIF总线连接，FPGA芯片还通过CAN控制器与CAN收发器连接；CAN收发器与主控计算机连接；主控计算机与主惯导系统连接；UART控制芯片还与RS_232接口连接；

主惯导系统生成载体导航信息后，先发送给主控计算机，主控计算机再通过CAN收发器发送给测姿处理板，CAN收发器将物理总线上收到的差分电平数据转换为TTL电平数据发送给CAN控制器，FPGA芯片通过CAN控制器接收数据，FPGA芯片通过EMIF总线将接收到的数据上传至DSP芯片；三个MEMS陀螺仪敏感载体的三轴角速度信息，三个MEMS加速度计敏感载体的三轴加速度信息，所得的三轴角速度信息和三轴加速度信息传输到A/D转换器，A/D转换器将得到的模拟量信息转换成数字量信息，并将该数字量信息通过温补系统补偿得到稳定的数字量信息；测姿处理板通过RS_422接口读取稳定的数字量信息，并传输给UART控制芯片，UART控制芯片将串行数据转换为并行数据并发送给FPGA芯片，FPGA芯片通过EMIF总线将接收到的数据上传至DSP芯片；FPGA芯片通过EMIF总线接收DSP芯片解算得到的惯性姿态数据，并将惯性姿态数据发送给UART控制芯片，UART控制芯片将并行数据转换为串行数据并行数据后发送给RS_232接口进行电平转换，并通过RS_232接口发给外接设备；

系统上电后完成系统初始化工作，其中包括主、子惯导系统安装误差角对应的补偿角的初始化；主惯导系统测得载体的速度信息、姿态信息、角速度信息、比力信息和位置信息，并利用所得的补偿角修正该姿态信息；进行粗对准，把主惯导系统的速度信息、位置信息、补偿角修正后的姿态信息赋给子惯导系统的对应参数，作为子惯导系统的解算初值；子惯导系统以粗对准后的导航信息为初值，进行惯导捷联解算，得到子惯导系统的位置信息、速度信息和姿态信息；主惯导系统数据经过时间同步和杆臂补偿处理后，采用“速度+姿态”的匹配方式建立卡尔曼滤波模型，以主惯导系统与子惯导系统之间的速度差值和姿态差值作为量测变量，进行卡尔曼滤波迭代，估计出子惯导系统速度误差、平台失准角、安装误差角、陀螺常值漂移和加速度计常值偏置；将估计出的子惯导系统速度误差、平台失准角，对子惯导系统计算出的速度信息、姿态信息进行校正，最终获得子惯导系统经校正后的速度信息和姿态信息。

2.一种基于权利要求1所述的机载分布式惯性测姿系统的传递对准方法，其特征在于，包括以下步骤：

第一步：系统上电后完成系统初始化工作，其中包括主、子惯导系统安装误差角对应的补偿角的初始化，然后进入下一步；

第二步：主惯导系统测得载体的速度信息、姿态信息、角速度信息、比力信息和位置信息，并利用第一步所得的补偿角修正该姿态信息；

第三步：进行粗对准，把第二步主惯导系统的速度信息、位置信息、补偿角修正后的姿态信息赋给子惯导系统的对应参数，作为子惯导系统的解算初值；

第四步：子惯导系统以第三步粗对准后的导航信息为初值，进行惯导捷联解算，得到子惯导系统的位置信息、速度信息和姿态信息；

第五步：主惯导系统数据经过时间同步和杆臂补偿处理后，采用“速度+姿态”的匹配方式建立卡尔曼滤波模型，以第二步中主惯导系统与第四步子惯导系统之间的速度差值和姿态差值作为量测变量，进行卡尔曼滤波迭代，估计出子惯导系统速度误差、平台失准角、安装误差角、陀螺常值漂移和加速度计常值偏置；

第六步：将第五步估计出的子惯导系统速度误差、平台失准角，对第四步子惯导系统计算出的速度信息、姿态信息进行校正，最终获得子惯导系统经校正后的速度信息和姿态信息。

3.根据权利要求2所述的机载分布式惯性测姿系统的传递对准方法，其特征在于，第一步所述的系统初始化工作包括硬件初始化和软件初始化，软件初始化包括：卡尔曼滤波器的系统初始状态X、系统状态协方差阵P、量测噪声方差阵R、系统过程噪声方差阵Q的初始化，主、子惯导系统安装误差角对应的补偿角ξ_x、ξ_y、ξ_z初始化。

4.根据权利要求2所述的机载分布式惯性测姿系统的传递对准方法，其特征在于，第二步所述的速度信息包括：主惯导系统的东向速度V_E，主惯导系统的北向速度V_N，主惯导系统的天向速度V_U；姿态信息包括：俯仰角θ、横滚角γ、航线角ψ；角速度信息包括：主惯导系统的东向角速度ω_E、主惯导系统的北向角速度ω_N、主惯导系统的天向角速度ω_U；比力信息包括：主惯导系统的东向比力f_E、主惯导系统的北向比力f_N、主惯导系统的天向比力f_U；位置信息包括主惯导系统的经度λ、主惯导系统的纬度L、主惯导系统的高度h。

5.根据权利要求2所述的机载分布式惯性测姿系统的传递对准方法，其特征在于，第五步中所述的卡尔曼滤波的滤波周期为1s。