CN104457748A - 一种嵌入式瞄准吊舱测姿系统及其传递对准方法 - Google Patents

Abstract

本发明公开了一种嵌入式瞄准吊舱测姿系统及其传递对准方法。该系统包括主惯导系统、主控计算机和子惯导系统，其中主惯导系统包括主惯导、第一GPS接收机和主导航计算机，子惯导系统包括子惯导、第二GPS接收机、子导航计算机和监控系统。传递对准方法为：主导航计算机接收主惯导和第一GPS接收机的数据，进行组合导航获得基本导航信息并发送给主控计算机；主控计算机对接收到的数据进行处理后发送给子惯导系统；子导航计算机接收主控计算机和子惯导发送的数据，建立卡尔曼滤波模型并采用“速度+姿态”匹配方式进行动基座传递对准，同时将导航信息发送给监控系统。本发明嵌入式瞄准吊舱测姿系统的实时性强、精度高、稳定性好。

Description

一种嵌入式瞄准吊舱测姿系统及其传递对准方法

一技术领域

本发明属于惯性导航领域，特别是一种嵌入式瞄准吊舱测姿系统及其传递对准方法。

二背景技术

瞄准吊舱（Targeting Pod）是挂在飞机专用挂梁处，供驾驶员实施对红外目标的图像识别、跟踪、瞄准和激光测距，以及导引激光半主动制导武器的目标照射的专用舱形装置，在飞行器大地勘探、遥测、灾难救援等领域也得到了广泛的应用。

军用瞄准吊舱是一种融合了光电探测器、捷联惯性导航系统和复杂的数据融合算法等为一体的轻小型、远程目标探测的空中武器作战装备。在战斗机上安装了嵌入式瞄准吊舱测姿系统，大大地提高了其跟踪能力及对地攻击精度，同时具备一定的自主导航能力，从而可实现对海、陆、空目标的快速搜索，实时跟踪和精确定位，并能引导精确制导武器或常规武器对目标实时精确打击。但瞄准吊舱上装备的的惯性测姿装置(子惯导)一般精度较低，测量误差随时间积累，需要利用战机上的高精度惯性导航系统（主惯导）信息来进行动基座传递对准。

瞄准吊舱测姿系统功能复杂，在接收处理大量数据的同时，需要周期性进行复杂的卡尔曼滤波算法计算，并且还需要实时向机载主控计算机提供导航参数，传统的单任务软件不能满足传递对准的实时性要求，在战机剧烈机动后，产生的新瞄准线将有延误，且瞄准的误差大、可靠性低。

三发明内容

本发明的目的是提供一种稳定性好、测量精度高、实时性强的瞄准吊舱测姿系统及其传递对准方法。

实现本发明目的的技术解决方案为：一种嵌入式瞄准吊舱测姿系统，包括主惯导系统、主控计算机和子惯导系统，其中主惯导系统包括主惯导、第一GPS接收机和主导航计算机，子惯导系统包括子惯导、第二GPS接收机、子导航计算机和监控系统；所述主惯导通过RS422接口与主导航计算机连接，第一GPS接收机通过RS232与主导航计算机连接，主导航计算机的输出端通过RS422接口接入主控计算机；主控计算机与子惯导系统的子导航计算机通过RS422接口连接，子惯导和第二GPS接收机均通过RS422接口与子导航计算机连接，子导航计算机的输出端通过RS422接口接入监控系统；

主导航计算机接收主惯导和第一GPS接收机的数据，进行组合导航获得基本导航信息，并将生成的基本导航信息发送给主控计算机；主控计算机对主惯导系统实时发送来的数据进行处理，然后发送给子惯导系统；子导航计算机一方面接收主控计算机发来的主惯导系统数据，另一方面接收子惯导发送的数据，并将接收到的数据进行捷联惯导解算，然后利用主惯导信息进行动基座传递对准，得到稳定平台的速度、姿态和位置信息并通过RS422接口发送给监控系统；第二GPS接收机负责子导航计算机内部时钟授时、以及主惯导和子惯导之间的时间同步。

一种嵌入式瞄准吊舱测姿系统的传递对准方法，包括以下步骤：

第1步，对系统的软件进行初始化，其中包括主惯导系统和子惯导系统安装误差角对应的补偿角的初始化，然后进入下一步；

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

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

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

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

第6步，将第5步得到的子惯导系统速度误差、平台失准角，对第4步子惯导系统的速度信息、姿态信息进行校正，最终获得子惯导系统经校正后的速度信息和姿态信息，并通过RS422接口发送给监控系统。

本发明与现有技术相比，其显著优点是：（1）在VxWorks嵌入式实时操作系统下，设计了传递对准多任务软件，实时性强、稳定性好；（2）对于主、子惯导安装的特殊情况，能够进行大安装误差角情况下的传递对准。

四附图说明

图1是本发明嵌入式瞄准吊舱测姿系统的硬件结构示意图。

图2是本发明嵌入式瞄准吊舱测姿系统中多任务软件的流程图。

图3是本发明嵌入式瞄准吊舱测姿系统的传递对准方法流程图。

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

图5是本发明嵌入式瞄准吊舱测姿系统的传递对准跑车实验硬件结构示意图。

图6是本发明实施例1中传递对准跑车实验失准角估计曲线图。

图7是本发明实施例2中传递对准跑车实验失准角估计曲线图。

图8是本发明实施例3中传递对准跑车实验失准角估计曲线图。

图9是本发明实施例4中传递对准跑车实验失准角估计曲线图。

图10是本发明实施例5中传递对准跑车实验失准角估计曲线图。

图11是本发明实施例6中传递对准跑车实验失准角估计曲线图。

表1是实施例1～6中子惯导数据统计表。

五具体实施方式

下面结合附图及具体实施例对本发明作进一步详细说明。

结合图1，本发明嵌入式瞄准吊舱测姿系统，包括主惯导系统、主控计算机和子惯导系统，其中主惯导系统包括主惯导、第一GPS接收机和主导航计算机，子惯导系统包括子惯导、第二GPS接收机、子导航计算机和监控系统；所述主惯导通过RS422接口与主导航计算机连接，第一GPS接收机通过RS232与主导航计算机连接，主导航计算机的输出端通过RS422接口接入主控计算机；主控计算机与子惯导系统的子导航计算机通过RS422接口连接，子惯导和第二GPS接收机均通过RS422接口与子导航计算机连接，子导航计算机的输出端通过RS422接口接入监控系统；所述子惯导系统中设有与子导航计算机相接的调试接口，该调试接口为RS232接口或以太网接口。所述子导航计算机为嵌入式MPC8245计算机系统，其软件使用VxWorks操作系统。

主导航计算机接收主惯导和第一GPS接收机的数据，进行组合导航获得基本导航信息，并将生成的基本导航信息发送给主控计算机；主控计算机对主惯导系统实时发送来的数据进行处理，然后发送给子惯导系统；子导航计算机一方面接收主控计算机发来的主惯导系统数据，另一方面接收子惯导发送的数据，并将接收到的数据进行捷联惯导解算，然后利用主惯导信息进行动基座传递对准，得到稳定平台的速度、姿态和位置信息并通过RS422接口发送给监控系统；第二GPS接收机负责子导航计算机内部时钟授时、以及主惯导和子惯导之间的时间同步。

结合图2，本发明嵌入式瞄准吊舱测姿系统采用嵌入式MPC8245计算机系统作为子导航计算机，导航计算机采用VxWorks操作系统，软件开发环境为Tornado2.2。VxWorks是一个运行在目标机上的高性能、可裁减的嵌入式实时操作系统，它以其良好的可靠性和卓越的实时性被广泛应用在通信、军事、航空、航天等高精尖技术及实时性要求极高的领域中。

在实时多任务操作系统VxWorks中，任务是指能够独立执行，实现某一功能或某一处理过程，是竞争系统资源的最小运行单元，也是操作系统调度的最基本单元。本系统是一个多路通信并含有复杂数学运算的多任务应用系统，采用模块化和层次化的划分原则，把本系统的软件模块划分成了四个任务：授时任务、主任务、子任务和发送任务，其中授时任务优先级最高，负责接收第二GPS接收机的数据，根据秒脉冲对子导航计算机内部时钟授时，授时成功则任务挂起；主任务负责接收主控计算机发来的主惯导系统数据；子任务负责接收子惯导数据，并进行惯导解算和传递对准；发送任务负责把运行结果发送给监控系统。

具体多任务调度软件流程如图2所示：

第一步：系统上电后，硬件初始化，多任务初始化，然后启动多任务调度；

第二步：各任务根据优先级的高低抢占CPU资源，授时任务首先获得CPU资源，接收第二GPS数据，根据秒脉冲对系统进行授时，授时成功后任务挂起；

第三步：主任务、子任务和发送任务抢占CPU资源，主任务主要负责接收主惯导系统的导航数据，发送任务主要负责把数据融合后的高精度导航参数发送给外设，子任务是该系统的核心，一方面实时接收子惯导数据，另一方面要负责传递对准算法的执行。

结合图3～4，本发明嵌入式瞄准吊舱测姿系统的传递对准方法，包括以下步骤：

第1步，对系统的软件进行初始化，具体包括：卡尔曼滤波器初始化即系统初始状态X、系统状态协方差阵P、量测噪声方差阵R、系统过程噪声方差阵Q的初始化，以及主惯导系统和子惯导系统安装误差角对应的补偿角在载体系x、y、z轴的分量ξ_x、ξ_y、ξ_z的初始化。

第2步，由主任务得到主惯导系统测得的载体速度信息、姿态信息、角速度信息、比力信息和位置信息，并利用第1步所得的补偿角修正该姿态信息；所述导航信息包括速度信息、姿态信息、角速度信息、比力信息和位置信息，其中速度信息包括：主惯导系统的东向速度V_E，主惯导系统的北向速度V_N，主惯导系统的天向速度V_U；姿态信息包括：主惯导系统的俯仰角θ、主惯导系统的横滚角γ、主惯导系统的航向角ψ；角速度信息包括：主惯导系统的东向角速度ω_E、主惯导系统的北向角速度ω_N、主惯导系统的天向角速度ω_U；比力信息包括：主惯导系统的东向比力f_E、主惯导系统的北向比力f_N、主惯导系统的天向比力f_U；位置信息包括主惯导系统的经度λ、主惯导系统的纬度L、主惯导系统的高度h。所述姿态信息为主惯导系统在大安装误差角下测得的姿态角，则其对应的姿态阵：

${\hat{C}}_a^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]$

由第1步知：主惯导系统和子惯导系统安装误差角对应的补偿角在载体系x、y、z轴的分量ξ_x、ξ_y、ξ_z，则补偿角构成的姿态阵为：

$C_m^s=\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]$

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

第3步，进行粗对准，将第2步主惯导系统测得的速度信息、位置信息、补偿角修正后的姿态信息赋给子惯导系统的对应参数，作为子惯导系统的解算初值。

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

第5步，主惯导系统的数据经过时间同步和杆臂补偿处理后，采用“速度+姿态”的匹配方式建立卡尔曼滤波模型，以第2步中主惯导系统与第4步子惯导系统之间的速度差值和姿态差值作为量测变量，进行卡尔曼滤波迭代，得到子惯导系统的速度误差、平台失准角、安装误差角、陀螺常值漂移和加速度计常值偏置；卡尔曼滤波的滤波周期为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)+{\mathrm{\ϵ}}_x^n\\ {\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)+{\mathrm{\ϵ}}_y^n\\ {\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)+{\mathrm{\ϵ}}_z^n\\ {\[{\mathrm{\ϵ}}_x^n{\mathrm{\ϵ}}_y^n{\mathrm{\ϵ}}_z^n\]}^T=C_b^n{\[{\mathrm{\ϵ}}_x^b{\mathrm{\ϵ}}_y^b{\mathrm{\ϵ}}_z^b\]}^T\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)+{\▿}_x^n\\ \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)+{\▿}_y^n\\ \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)+{\▿}_z^n\\ {\[{\▿}_x^n{\▿}_y^n{\▿}_z^n\]}^T=C_b^n{\[{\▿}_x^b{\▿}_y^b{\▿}_z^b\]}^T\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轴陀螺常值漂移，为子惯导系统y轴陀螺常值漂移，为子惯导系统z轴陀螺常值漂移；

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

为子惯导姿态阵；

惯性器件误差方程：

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

${\stackrel{.}{\▿}}_i^b=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为系统状态变量：

${\[{\mathrm{\φ}}_E{\mathrm{\φ}}_N{\mathrm{\φ}}_U\mathrm{\δ}{V}_E\mathrm{\δ}{V}_N\mathrm{\δ}{V}_U{\mathrm{\ϵ}}_x^b{\mathrm{\ϵ}}_y^b{\mathrm{\ϵ}}_z^b{\▿}_x^b{\▿}_y^b{\▿}_z^b{\mathrm{\λ}}_x{\mathrm{\λ}}_y{\mathrm{\λ}}_z\]}^T;$

F为系统状态转移矩阵：

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

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

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

$F_2=\left[\begin{array}{ccc}0& -\frac{1}{R_M+h}& 0\\ \frac{1}{R_N+h}& 0& 0\\ \frac{\tan L}{R_N+h}& 0& 0\\ \frac{V_N}{R_M+h}\tan L-\frac{V_U}{R_M+h}& 2w_{\mathrm{ie}}\sin L+\frac{V_E}{R_N+h}\tan L& -(2w_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+h})\\ -2(w_{\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(w_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+h})& \frac{2V_N}{R_M+h}& 0\end{array}\right]$

G为系统噪声驱动矩阵:

$\left[\begin{array}{cc}{C}_b^n& 0_{3\×3}\\ 0_{3\×3}& C_b^n\\ 0_{9\×6}& \end{array}\right]$

W为系统噪声矩阵：

W＝[W_GxW_GyW_GzW_ΔxW_ΔyW_Δz]^T

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为量测矩阵：

$\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]$

令 $C_a^n=T$

$\left[\begin{array}{ccc}{T}_{11}& T_{12}& T_{13}\\ T_{21}& T_{22}& T_{23}\\ T_{31}& T_{32}& T_{33}\end{array}\right]$

则

$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}_{32}}{{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_{33}{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

Z_k＝H_kX_k+V_k           (8)

其中，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}+\{F\[F\stackrel{-}{Q}+{\left(F\stackrel{-}{Q}\right)}^T\]+\[F{(F\stackrel{-}{Q}+{\left(F\stackrel{-}{Q}\right)}^T)}^T\]\}\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_kk-1为k时刻系统协方差阵的一步预测、R_k为k时刻系统量测噪声矩阵；

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

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

其中，P_kk-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是单位阵。

第6步，将第5步得到的子惯导系统速度误差、平台失准角，对第4步子惯导系统的速度信息、姿态信息进行校正，最终获得子惯导系统经校正后的速度信息和姿态信息，并通过RS422接口发送给监控系统。

下面结合具体实施例对本发明作进一步说明。

实施例1

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

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

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

实施例2

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

实施例3

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

实施例4

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

实施例5

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

实施例6

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

结合表1，实施例1～6的子惯导数据统计表：各实验的主子惯导x,y轴安装误差角真实值均为0°左右，z轴安装误差角的真实值经测量为17°左右，给z轴安装误差角对应的补偿角分别为16.00°、16.00°、16.50°、17.00°、17.50°和18.00°。

从实施例1、实施例2的跑车实验数据结果分析可知：相同的补偿角下估计的z轴失准角估计值仅相差0.20′，由此可以证明该系统的稳定性很高；

从实施例1～实施例6数据结果分析可知：(1)x轴失准角估计值分别为：4.30′、4.50′、2.70′、2.60′、3.56′和3.30′，取它们的均值3.49′作为x轴失准角的真实值，则它们的估计值与真实值之间的误差分别为：0.81′、1.01′、-0.79′、-0.89′、0.07′和-0.19′，由此可见：x轴失准角估计误差在1.01′以内；(2)同理：y轴失准角估计误差在0.83′以内；(3)z轴补偿角分别为16.00°、16.00°、16.50°、17.00°、17.50°和18.00°，z轴安装误差角测量值分别为16.96°、16.97°、16.99°、16.96°、16.98°和16.97°，z轴补偿角与z轴安装误差角测量值的差值分别为-57.60′、-58.20′、-29.40′、2.40′、31.20′和61.80′，而z轴失准角估计值分别为-58.00′、-58.20′、-29.70′、2.36′、30.65′和61.40′，由此可见：z轴失准角估计误差在0.55′以内。

表1

综合以上实施例1～实施例6的实验结果可以证明：(1)本发明嵌入式瞄准吊舱测姿系统的稳定性很高；(2)该系统的三轴失准角估计精度很高，分别在1.01′、0.83′、0.55′以内；(3)同时也证明了基于VxVorks操作系统下多任务设计的稳定性和高效性，最终完全满足机载平台的要求。

Claims (7)

1.一种嵌入式瞄准吊舱测姿系统，其特征在于，包括主惯导系统、主控计算机和子惯导系统，其中主惯导系统包括主惯导、第一GPS接收机和主导航计算机，子惯导系统包括子惯导、第二GPS接收机、子导航计算机和监控系统；所述主惯导通过RS422接口与主导航计算机连接，第一GPS接收机通过RS232与主导航计算机连接，主导航计算机的输出端通过RS422接口接入主控计算机；主控计算机与子惯导系统的子导航计算机通过RS422接口连接，子惯导和第二GPS接收机均通过RS422接口与子导航计算机连接，子导航计算机的输出端通过RS422接口接入监控系统；

主导航计算机接收主惯导和第一GPS接收机的数据，进行组合导航获得基本导航信息，并将生成的基本导航信息发送给主控计算机；主控计算机对主惯导系统实时发送来的数据进行处理，然后发送给子惯导系统；子导航计算机一方面接收主控计算机发来的主惯导系统数据，另一方面接收子惯导发送的数据，并将接收到的子惯导数据进行捷联惯导解算，然后建立卡尔曼滤波模型，并采用“速度+姿态”匹配方式进行动基座传递对准，得到稳定平台的速度、姿态和位置信息并通过RS422接口发送给监控系统；第二GPS接收机负责子导航计算机内部时钟授时、以及主惯导和子惯导之间的时间同步。

2.根据权利要求1所述的嵌入式瞄准吊舱测姿系统，其特征在于，所述子惯导系统中设有与子导航计算机相接的调试接口，该调试接口为RS232接口或以太网接口。

3.根据权利要求1所述的嵌入式瞄准吊舱测姿系统，其特征在于，所述子导航计算机为嵌入式MPC8245计算机系统，其软件使用VxWorks操作系统。

4.根据权利要求3所述的嵌入式瞄准吊舱测姿系统，其特征在于，所述VxWorks操作系统的软件模块划分为四个任务：授时任务、主任务、子任务和发送任务，其中授时任务优先级最高，负责接收第二GPS接收机的数据，根据秒脉冲对子导航计算机内部时钟授时，授时成功则任务挂起；主任务负责接收主控计算机发来的主惯导系统数据；子任务负责接收子惯导数据，并进行惯导解算和传递对准；发送任务负责把运行结果发送给监控系统。

5.一种基于权利要求1所述嵌入式瞄准吊舱测姿系统的传递对准方法，其特征在于，包括以下步骤：

第1步，对系统的软件进行初始化，其中包括主惯导系统和子惯导系统安装误差角对应的补偿角的初始化，然后进入下一步；

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

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

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

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

第6步，将第5步得到的子惯导系统速度误差、平台失准角，对第4步子惯导系统的速度信息、姿态信息进行校正，最终获得子惯导系统经校正后的速度信息和姿态信息，并通过RS422接口发送给监控系统。

6.根据权利要求5所述的嵌入式瞄准吊舱测姿系统的传递对准方法，其特征在于，第1步中所述对系统的软件进行初始化，具体包括：卡尔曼滤波器初始化即系统初始状态X、系统状态协方差阵P、量测噪声方差阵R、系统过程噪声方差阵Q的初始化，以及主惯导系统和子惯导系统安装误差角对应的补偿角在载体系x、y、z轴的分量ξ_x、ξ_y、ξ_z的初始化。

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