CN103389092B - 一种系留飞艇姿态测量装置及测量方法 - Google Patents

Abstract

本发明公开了一种系留飞艇姿态测量装置及测量方法，系留飞艇姿态测量装置包括激光陀螺捷联惯导、导航处理模块、GPS定位定向模块，所述导航处理模块包括FPGA、DSP，所述FPGA与所述DSP双向连接，所述FPGA与所述激光陀螺捷联惯导、GPS定位定向模块、系留飞艇控制系统连接。本发明的测量装置结构简单，精度高、功耗低、环境适用性好、可靠性高；通过同步采集GPS定位定向模块的方位和速度信息，GPS的方位和速度信息与导航解算的方位和速度信息之间的差值作为Kalman滤波器的输入，对惯性导航的误差进行实时估计，同时进行反馈校正，实现导航信息的长时间精度保持。

Description

一种系留飞艇姿态测量装置及测量方法

技术领域

本发明涉及组合导航与制导、姿态测量与控制领域,特别是一种系留飞艇姿态测量装置及测量方法。

背景技术

系留艇升空后长期漂浮在空中进行工作，需要实时获得系留艇的高精度姿态信息，用于对系留艇进行姿态控制。为此需要研制姿态测量装置用于对系留艇的姿态等信息进行高精度的测量。

目前通常在系留艇上使用捷联惯性导航系统或多天线GPS测姿系统两种方式进行。捷联惯导不依赖任何外界信息，能测量出载体的真北航向、俯仰和滚动，但在初始对准期间要求载体相对静止，在一定时间内测量精度高，随着时间的推移精度会逐渐下降。多天线GPS测姿系统精度高，但完全依赖于GPS信息，易受干扰，GPS信息中断则测量中断。

发明内容

本发明所要解决的技术问题是，针对现有技术不足，提供一种系留飞艇姿态测量装置及测量方法，解决系留艇在空中长时间工作的姿态测量问题，提高测量精度。

为解决上述技术问题，本发明所采用的技术方案是：一种系留飞艇姿态测量装置，包括安装在系留飞艇桁架上的激光陀螺捷联惯导，还包括导航处理模块、GPS定位定向模块，所述导航处理模块包括FPGA、DSP，所述FPGA与所述DSP双向连接，所述FPGA与所述激光陀螺捷联惯导、GPS定位定向模块、系留飞艇控制系统连接。

作为优选方案，所述FPGA通过数字隔离器与所述GPS定位定向模块连接，所述激光陀螺捷联惯导的输出的三路加速度信号和三路角速度信号依次通过光耦接收隔离器、电平转换电路输入FPGA，所述FPGA通过RS422差分驱动器将同步采集脉冲送入所述激光陀螺捷联惯导，所述FPGA通过RS422接口与所述系留飞艇控制系统连接。

作为优选方案，所述GPS定位定向模块采用OEM模块。

系留飞艇姿态测量方法为：

1）选用系留飞艇所在地东北天地理坐标系为参考坐标系，利用激光陀螺捷联惯导测量的角速度和加速度，计算得到参考坐标系到系留飞艇坐标系的方向余弦矩阵

其中，θ、γ、ψ分别为系留飞艇俯仰角、横滚角和方位角；

2）定义方向余弦矩阵的转置矩阵为姿态矩阵T，则：

${\left(C_t^b\right)}^{-1}={\left(C_t^b\right)}^t=C_b^t=T,$

即： $T=\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{\γ}\sin \mathrm{\ψ}-\cos \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}\\ -\sin \mathrm{\γ}\cos \mathrm{\θ}& \sin \mathrm{\θ}& \cos \mathrm{\γ}\cos \mathrm{\θ}\end{array}\right]$

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

3）由步骤2）确定ψ、θ、γ，完成系留飞艇姿态的初始对准：

$\left\{\begin{array}{c}\mathrm{\θ}={\sin }^{-1}\left(t_{32}\right)\\ \mathrm{\γ}={\mathrm{tg}}^{-1}({-t}_{31}/t_{33})\\ \mathrm{\ψ}={\mathrm{tg}}^{-1}(-t_{12}/t_{22})\end{array}\right.;$

4）利用激光陀螺捷联惯导测量的角速度和加速度，计算得到系留飞艇在东北天地理坐标系上的速度v_{e_ins}、v_{n_ins}、v_{u_ins}、方位角ψ_{u_ins}和系留飞艇所在位置的经度λ、纬度L、高度h，其中ψ_{u_ins}=ψ；

5）建立激光陀螺捷联惯导速度误差模型：

$\left\{\begin{array}{c}\mathrm{\δ}{\stackrel{\·}{v}}_e=-f_u{\mathrm{\φ}}_n+f_n{\mathrm{\φ}}_u+\left(2{\mathrm{\ω}}_{\mathrm{ie}}\sin L\right){\mathrm{\δv}}_n-\left({2\mathrm{\ω}}_{\mathrm{ie}}\cos L\right){\mathrm{\δv}}_u+{\▿}_e\\ \mathrm{\δ}{\stackrel{\·}{v}}_n=f_u{\mathrm{\φ}}_e-f_e{\mathrm{\φ}}_u-\left({2\mathrm{\ω}}_{\mathrm{ie}}\sin L\right){\mathrm{\δv}}_e+{\▿}_n\\ \mathrm{\δ}{\stackrel{\·}{v}}_e=-f_n{\mathrm{\φ}}_e+f_e{\mathrm{\φ}}_n+\left({2\mathrm{\ω}}_{\mathrm{ie}}\cos L\right){\mathrm{\δv}}_e+{\▿}_u\end{array}\right.,$

其中，f_e、f_n、f_u分别为加表在东向、北向、天向测量的比力；δv_e、δv_n、δv_u分别为惯导系统东向、北向、天向速度误差，分别为δv_e、δv_n、δv_u的微分，φ_e、φ_n、φ_u分别为系留飞艇俯仰、横滚、方位角误差，ω_ie为地球自转角速度，分别为东向加速度计、北向加速度计、天向加速度计的零偏；

6）建立激光陀螺捷联惯导姿态误差模型：

$\left\{\begin{array}{c}{\stackrel{\·}{\mathrm{\φ}}}_e=\left({\mathrm{\ω}}_{\mathrm{ie}}\sin L\right){\mathrm{\φ}}_n-\left({\mathrm{\ω}}_{\mathrm{ie}}\cos L\right){\mathrm{\φ}}_u-\frac{{\mathrm{\δv}}_n}{R_{\mathrm{yp}}+h}-{\mathrm{\ϵ}}_e\\ {\stackrel{\·}{\mathrm{\φ}}}_n=-\left({\mathrm{\ω}}_{\mathrm{ie}}\sin L\right){\mathrm{\φ}}_e-\left({\mathrm{\ω}}_{\mathrm{ie}}\sin L\right)\mathrm{\δL}-\frac{{\mathrm{\δv}}_e}{R_{\mathrm{xp}}+h}-{\mathrm{\ϵ}}_n\\ {\stackrel{\·}{\mathrm{\φ}}}_u=\left({\mathrm{\ω}}_{\mathrm{ie}}\cos L\right){\mathrm{\φ}}_e+\left({\mathrm{\ω}}_{\mathrm{ie}}\sin L\right)\mathrm{\δL}+\frac{{\mathrm{\δv}}_e\tan L}{R_{\mathrm{xp}}+h}-{\mathrm{\ϵ}}_u\end{array}\right.,$

其中，分别为φ_e、φ_n、φ_u的微分，R_xp为地球纬度圈半径，R_yp为地球经度圈半径，ω_ie为地球自转角速度，δλ、δL、δh分别为激光陀螺捷联惯导计算的系留飞艇位置的经度误差、纬度误差、高度误差；ε_e、ε_n、ε_u分别为东向陀螺、北向陀螺、天向陀螺的零漂；

7）建立激光陀螺捷联惯导位置误差模型：

$\left\{\begin{array}{c}\mathrm{\δ}\stackrel{\·}{L}=\frac{{\mathrm{\δv}}_n}{R_{\mathrm{yp}}+h}\\ \mathrm{\δ}\stackrel{\·}{\mathrm{\λ}}=\frac{{\mathrm{\δv}}_e}{R_{\mathrm{xp}}+h}\\ \mathrm{\δ}\stackrel{\·}{h}={\mathrm{\δv}}_u\end{array}\right.,$

分别为δλ、δL、δh的微分；

8）建立激光陀螺捷联惯导三个加速度计的零偏误差模型：

$\left\{\begin{array}{c}{\stackrel{\·}{\▿}}_e=-{\▿}_e/{\mathrm{\τ}}_{\mathrm{ax}}+w_{\mathrm{ax}}\left(t\right)\\ {\stackrel{\·}{\▿}}_n=-{\▿}_n/{\mathrm{\τ}}_{\mathrm{ay}}+w_{\mathrm{ay}}\left(t\right)\\ {\stackrel{\·}{\▿}}_u=-{\▿}_u/{\mathrm{\τ}}_{\mathrm{az}}+w_{\mathrm{az}}\left(t\right)\end{array}\right.,$

其中，分别为的微分，τ_ai,i=x,y,z为三个加速度计零偏相关时间，w_ax、w_ay、w_az分别为三个加速度计的零均值高斯白噪声；

9）建立激光陀螺捷联惯导三个陀螺的零偏误差模型：

$\left\{\begin{array}{c}{\stackrel{\·}{\mathrm{\ϵ}}}_e=-{\mathrm{\ϵ}}_e/{\mathrm{\τ}}_{\mathrm{gx}}+w_{\mathrm{gx}}\left(t\right)\\ {\stackrel{\·}{\mathrm{\ϵ}}}_n=-{\mathrm{\ϵ}}_n/{\mathrm{\τ}}_{\mathrm{gy}}+w_{\mathrm{gy}}\left(t\right)\\ {\stackrel{\·}{\mathrm{\ϵ}}}_u=-{\mathrm{\ϵ}}_u/{\mathrm{\τ}}_{\mathrm{gz}}+w_{\mathrm{gz}}\left(t\right)\end{array}\right.,$

其中，ε_e、ε_n、ε_u分别为东向陀螺，北向陀螺，天向陀螺的零漂，分别为ε_e、ε_n、ε_u的微分，τ_gi,i=x,y,z为三个陀螺零漂相关时间，w_gx、w_gy、w_gz分别为三个陀螺的零均值高斯白噪声；

10）令 $Z=\left[\begin{array}{c}{\mathrm{\δV}}_e\\ {\mathrm{\δV}}_n\\ {\mathrm{\δV}}_u\\ \mathrm{\δ\ψ}\end{array}\right]=\left[\begin{array}{c}{v}_{e\_\mathrm{ins}}-v_{e\_\mathrm{gps}}\\ v_{n\_\mathrm{ins}}-v_{n\_\mathrm{gps}}\\ v_{u\_\mathrm{ins}}-v_{u\_\mathrm{gps}}\\ {\mathrm{\ψ}}_{u\_\mathrm{ins}}-{\mathrm{\ψ}}_{u\_\mathrm{gps}}\end{array}\right],$ 其中v_{e_gps}、v_{n_gps}、v_{u_gps}、ψ_{u_gps}分别为GPS定位定向模块测量的系留飞艇在东北天地理坐标系上的速度和方位角，δV_e、δV_n、δV_u、δψ分别为捷联惯导与GPS定位定向模块测量的系留飞艇的东向速度、北向速度、天向速度和方位角的差值，将Z作为kalman滤波器的输入；

11）根据步骤5）～10），得到kalman滤波模型维：

$\left\{\begin{array}{c}\stackrel{\·}{X}=\mathrm{AX}+\mathrm{GW}\\ Z=\mathrm{HX}+V\end{array}\right.,$

由kalman滤波模型维解算出X，从而对激光陀螺捷联惯导的各项导航误差进行实时估计；其中A为15×15维状态转移矩阵；H为4×15维观测矩阵；G为15×6维噪声输入矩阵；W为6维噪声向量，由陀螺和加速度计噪声构成；V为4维观测噪声，

$X={\left[\begin{array}{ccccccccccccccc}{\mathrm{\φ}}_e& {\mathrm{\φ}}_n& {\mathrm{\φ}}_u& {\mathrm{\δv}}_e& {\mathrm{\δv}}_n& {\mathrm{\δv}}_u& \mathrm{\δ\λ}& \mathrm{\δL}& \mathrm{\δh}& {\mathrm{\ϵ}}_e& {\mathrm{\ϵ}}_n& {\mathrm{\ϵ}}_u& {\▿}_e& {\▿}_n& {\▿}_u\end{array}\right]}^T,$ 为X的微分；

12）对系留飞艇在东北天地理坐标系上的速度v_{e_ins}、v_{n_ins}、v_{u_ins}、方位角ψ_{u_ins}进行反馈校正，克服姿态角误差发散；

13）导航处理模块将当前时刻系留飞艇的位置、速度、姿态信息发送至系留飞艇控制系统，并返回5）。

其中，A可以表示为：

$A\left(t\right)={\left[\begin{array}{cc}{A}_N& A_S\\ 0_{6\×9}& A_M\end{array}\right]}_{15\×15}---\left(12\right)$

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

$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{\γ}\sin \mathrm{\ψ}-\cos \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}\\ -\sin \mathrm{\γ}\cos \mathrm{\θ}& \sin \mathrm{\θ}& \cos \mathrm{\γ}\cos \mathrm{\θ}\end{array}\right]---\left(14\right)$

其中，ψ、θ、γ分别为惯导测量的飞艇方位角、俯仰角、滚动角。

$A_N=$

$\left[\begin{array}{ccccccccc}0& {\mathrm{\ω}}_{\mathrm{ie}}\sin L& -{\mathrm{\ω}}_{\mathrm{ie}}\cos L& 0& -\frac{1}{R_{\mathrm{yp}}+h}& 0& 0& 0& 0\\ -{\mathrm{\ω}}_{\mathrm{ie}}\sin L& 0& 0& \frac{1}{R_{\mathrm{xp}}+h}& 0& 0& -{\mathrm{\ω}}_{\mathrm{ie}}\sin L& 0& 0\\ {\mathrm{\ω}}_{\mathrm{ie}}\cos L& 0& 0& \frac{\tan L}{R_{\mathrm{xp}}+h}& 0& 0& {\mathrm{\ω}}_{\mathrm{ie}}\cos L& 0& 0\\ 0& -f_U& f_N& 0& {2\mathrm{\ω}}_{\mathrm{ie}}\sin L& -{2\mathrm{\ω}}_{\mathrm{ie}}\cos L& 0& 0& 0\\ f_U& 0& -f_E& -{2\mathrm{\ω}}_{\mathrm{ie}}\sin L& 0& 0& 0& 0& 0\\ -f_N& f_E& 0& {2\mathrm{\ω}}_{\mathrm{ie}}\cos L& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& \frac{1}{R_{\mathrm{yp}}+h}& 0& 0& 0& 0\\ 0& 0& 0& \frac{1}{R_{\mathrm{xp}}+h}\sec L& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0\end{array}\right]---\left(15\right)$

$A_M=\left[\begin{array}{cccccc}-\frac{1}{{\mathrm{\τ}}_{\mathrm{gx}}}& 0& 0& 0& 0& 0\\ 0& -\frac{1}{{\mathrm{\τ}}_{\mathrm{gy}}}& 0& 0& 0& 0\\ 0& 0& -\frac{1}{{\mathrm{\τ}}_{\mathrm{gz}}}& 0& 0& 0\\ 0& 0& 0& -\frac{1}{{\mathrm{\τ}}_{\mathrm{ax}}}& 0& 0\\ 0& 0& 0& 0& -\frac{1}{{\mathrm{\τ}}_{\mathrm{ay}}}& 0\\ 0& 0& 0& 0& 0& -\frac{1}{{\mathrm{\τ}}_{\mathrm{az}}}\end{array}\right]---\left(16\right)$

W(t)=[w_gxw_gyw_gzw_axw_ayw_az]^T（17)

$G\left(t\right)={\left[\begin{array}{cc}{C}_b^n& 0_{3\×3}\\ 0_{6\×3}& 0_{6\×3}\\ I_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& I_{3\×3}\end{array}\right]}_{15\×6}---\left(18\right)$

$H=\left[\begin{array}{ccccccccccccccc}0& 0& 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\end{array}\right]---\left(19\right)$

V=[M_vEM_vNM_vUM_φU]（20)

其中，M_vE,M_vN,M_vU分别为GPS定位定向模块速度测量白噪声，M_φU为GPS定位定向模块方位测量白噪声。

0_3×3定义为3×3维的零矩阵，I_3×3定义为3×3维的单位矩阵，依次类推，以0_6×9和I_3×3为例：

$0_{6\×9}={\left[\begin{array}{ccccccccc}0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0\end{array}\right]}_{6\×9}---\left(21\right)$

$I_{3\×3}=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]---\left(22\right)$

用滤波得到的误差估计量来校正导航方程的位置、速度、姿态以及惯性元件误差。捷联惯导的位置、速度、姿态角等信号都是数字量，可以直接从惯导输出中减去相应的滤波值。

与现有技术相比，本发明所具有的有益效果为：本发明的装置结合了捷联惯导与GPS定位定向模块，结构简单，精度高、功耗低、环境适用性好、可靠性高；本发明的方法采用捷联惯导/GPS组合导航技术来克服惯导系统误差的发散，保证系留飞艇长时间保持导航精度；采用Kalman滤波技术进行捷联惯导/GPS的信息融合技术研究，考虑到GPS定位定向设备能实时输出准确的速度和方位信息，选择捷联惯导的速度、方位和GPS的速度、方位之差作为kalman滤波器的观测量，对激光陀螺捷联惯导的各项导航误差进行实时估计和反馈校正，有效提高了系留飞艇姿态测量精度。

附图说明

图1为本发明一实施例系留飞艇姿态测量装置结构示意图；

图2为本发明一实施例导航处理模块结构框图；

图3为导航处理模块计算系留飞艇的位置、速度、姿态信息原理图。

具体实施方式

如图1和图2所示，本发明一实施例系留飞艇姿态测量装置包括激光陀螺捷联惯导、导航处理模块、GPS定位定向模块，所述导航处理模块包括FPGA、DSP，所述FPGA与所述DSP双向连接，所述FPGA与所述激光陀螺捷联惯导、GPS定位定向模块、系留飞艇控制系统连接。

激光陀螺捷联惯导为现有的成熟产品，其核心器件为三只石英挠性加速度计和三只50型二频机抖激光陀螺。陀螺和加速度计正交安装在基座上，用于测量运载器沿载体坐标系的三路角速度和三路加速度信息，经惯导DSP电路误差补偿后传输给导航处理模块。

角速率信息的测量是通过在惯组台体上安装三个敏感轴相互正交的50型激光陀螺仪，利用Sagnac原理将绕弹体三个轴转动的角速度信息转换成光电流信号，其信号频率与角速度信息成正比，该光电流信号经过电流－电压转换、宽带低噪高增益放大、过零比较、脉冲整形后，变成TTL电平的脉冲信号，供给DSP系统鉴相计数器计数。

加速度信息的测量是通过三个敏感轴相互正交的石英挠性加速度计来实现的，石英挠性加速度计将弹体三个轴的视加速度信息转换成电流信号，其电流信号与视加速度信息成正比，该电流信号经过I/F转换电路和数字信号处理板（DSP）测量，还原加速度信息。DSP系统以2KHz的采样速率对激光陀螺输出的鉴相脉冲采集，并通过数字低通滤波器消除信号中的抖动分量，还原角速度信息；再将角速度和加速度信息进行数字处理、对称性补偿、非线性补偿和系统误差补偿后，传送导航处理模块。

GPS定位定向模块主要用于测量两个GPS接收天线之间的方位信息，测量点的速度、位置信息等。GPS接收天线分别测量其收到的同一组GPS卫星GPS载波信号的瞬时相位，并比较其相位差同步观测，以求解出基线（即两个GPS天线之间的连线）端点的相对位置或基线向量。

导航处理模块完成数据采集和导航解算任务，主要包括：

a)FPGA主要完成各种信息的交互，时序控制等；

b)通过RS422实时采集激光捷联惯导的加速度和角速度信息，同步采集GPS定位定向模块的方位和速度信息，并进行组合导航解算；

c)DSP板通过2路RS422接口与控制系统设备通讯。

本发明姿态测量方法如下：

(1)在姿态测量装置系统开机后的前6min，组合导航处理盒根据捷联惯导输出的3路加速度信息以及当地重力加速度，计算出载体的俯仰角和滚转角信息，并通过GPS定位定向设备对天线基线的方位角信息进行测量，于是得到飞艇的初始方位角ψ、俯仰角θ、滚动角γ，实现飞艇姿态的初始对准。

载体的初始俯仰角θ和滚转角γ的计算方法为：

首先要选用当地东北天地理坐标系（ENU）为参考坐标系n。而载体坐标系定义为b，载体（即系留飞艇）的捷联惯导上安装有三个激光陀螺和三个加表，由于捷联惯导是固联在载体上的，所以激光陀螺和加速度计测量得到的角速度和加速度都是在载体坐标系下的投影。

利用捷联惯导测量的角速度和加速度，计算得到地理坐标系到载体坐标系的方向余弦矩阵

在捷联式惯导系统中，我们定义由载体坐标系至地理坐标系的转换矩阵为姿态矩阵（我们有时也称为捷联矩阵）T。

由矩阵理论知，为正交矩阵，则有下式成立：

${\left(C_t^b\right)}^{-1}={\left(C_t^b\right)}^t=C_b^t=T---\left(2\right)$

因此，我们可以得出捷联姿态矩阵为：

$T=\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{\γ}\sin \mathrm{\ψ}-\cos \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}\\ -\sin \mathrm{\γ}\cos \mathrm{\θ}& \sin \mathrm{\θ}& \cos \mathrm{\γ}\cos \mathrm{\θ}\end{array}\right]$

$=\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]---\left(3\right)$

由上式可以看出，姿态矩阵T是ψ、θ、γ的函数，根据T的元素即可单值地确定ψ、θ、γ，即确定了载体的姿态角。

从上式可知：

式中t₃₂、t₃₁、t₃₃、t₁₂、t₂₂等分别为姿态矩阵T的相应的元素。

初始对准时，载体的俯仰角θ和滚动角γ取捷联惯导输出信息计算值，但方位角ψ不取以捷联惯导输出信息计算的方位角，而是取GPS定位定向设备所测量的方位角。

完成初始对准以后，导航处理模块根据捷联惯导输出的3路角速度信息和3路加速度信息对系留飞艇的位置、速度、姿态信息进行实时的计算，得到系留飞艇在东北天地理坐标系上的速度v_{e_ins}、v_{n_ins}、v_{u_ins}和系留飞艇所在位置的经度λ、纬度L、高度h，具体实现过程见图3，其中，f^b为捷联惯导输出的3路加速度信息，ω^b为捷联惯导输出的3路角速度信息；V_x、V_y、V_z为以捷联惯导输出信息计算的飞艇的3个方向的速度分量，λ、L、h分别为以捷联惯导输出信息计算的飞艇所在位置的经度、纬度、高度；ψ、θ、γ分别为以捷联惯导输出信息计算的飞艇姿态的方位角、俯仰角、滚动角。Q为四元数，T为姿态矩阵，C为位置矩阵，g为地球重力加速度，ω_ie为地球自转角速度。

为了提高卡尔曼滤波的精度，取SINS输出的导航参数的误差作为组合导航的状态量，因此SINS的误差方程就是组合导航系统状态方程的主要组成部分。在本发明中，采用东北天坐标系（ENU），以方位／速度组合模式进行组合，并采用反馈校正的方式对惯导系统进行校正。

捷联惯导速度误差模型：

$\left\{\begin{array}{c}\mathrm{\δ}{\stackrel{\·}{v}}_e=-f_u{\mathrm{\φ}}_n+f_n{\mathrm{\φ}}_u+\left(2{\mathrm{\ω}}_{\mathrm{ie}}\sin L\right){\mathrm{\δv}}_n-\left({2\mathrm{\ω}}_{\mathrm{ie}}\cos L\right){\mathrm{\δv}}_u+{\▿}_e\\ \mathrm{\δ}{\stackrel{\·}{v}}_n=f_u{\mathrm{\φ}}_e-f_e{\mathrm{\φ}}_u-\left({2\mathrm{\ω}}_{\mathrm{ie}}\sin L\right){\mathrm{\δv}}_e+{\▿}_n\\ \mathrm{\δ}{\stackrel{\·}{v}}_e=-f_n{\mathrm{\φ}}_e+f_e{\mathrm{\φ}}_n+\left({2\mathrm{\ω}}_{\mathrm{ie}}\cos L\right){\mathrm{\δv}}_e+{\▿}_u\end{array}\right.---\left(5\right)$

捷联惯导姿态误差模型：

$\left\{\begin{array}{c}{\stackrel{\·}{\mathrm{\φ}}}_e=\left({\mathrm{\ω}}_{\mathrm{ie}}\sin L\right){\mathrm{\φ}}_n-\left({\mathrm{\ω}}_{\mathrm{ie}}\cos L\right){\mathrm{\φ}}_u-\frac{{\mathrm{\δv}}_n}{R_{\mathrm{yp}}+h}-{\mathrm{\ϵ}}_e\\ {\stackrel{\·}{\mathrm{\φ}}}_n=-\left({\mathrm{\ω}}_{\mathrm{ie}}\sin L\right){\mathrm{\φ}}_e-\left({\mathrm{\ω}}_{\mathrm{ie}}\sin L\right)\mathrm{\δL}-\frac{{\mathrm{\δv}}_e}{R_{\mathrm{xp}}+h}-{\mathrm{\ϵ}}_n\\ {\stackrel{\·}{\mathrm{\φ}}}_u=\left({\mathrm{\ω}}_{\mathrm{ie}}\cos L\right){\mathrm{\φ}}_e+\left({\mathrm{\ω}}_{\mathrm{ie}}\sin L\right)\mathrm{\δL}+\frac{{\mathrm{\δv}}_e\tan L}{R_{\mathrm{xp}}+h}-{\mathrm{\ϵ}}_u\end{array}\right.---\left(6\right)$

捷联惯导位置误差模型：

$\left\{\begin{array}{c}\mathrm{\δ}\stackrel{\·}{L}=\frac{{\mathrm{\δv}}_n}{R_{\mathrm{yp}}+h}\\ \mathrm{\δ}\stackrel{\·}{\mathrm{\λ}}=\frac{{\mathrm{\δv}}_e}{R_{\mathrm{xp}}+h}\\ \mathrm{\δ}\stackrel{\·}{h}={\mathrm{\δv}}_u\end{array}\right.---\left(7\right)$

捷联惯导加表零偏误差模型：

$\left\{\begin{array}{c}{\stackrel{\·}{\▿}}_e=-{\▿}_e/{\mathrm{\τ}}_{\mathrm{ax}}+w_{\▿}\left(t\right)\\ {\stackrel{\·}{\▿}}_n=-{\▿}_n/{\mathrm{\τ}}_{\mathrm{ay}}+w_{\▿}\left(t\right)\\ {\stackrel{\·}{\▿}}_u=-{\▿}_u/{\mathrm{\τ}}_{\mathrm{az}}+w_{\▿}\left(t\right)\end{array}\right.---\left(8\right)$

捷联惯导陀螺零漂误差模型：

$\left\{\begin{array}{c}{\stackrel{\·}{\mathrm{\ϵ}}}_e=-{\mathrm{\ϵ}}_e/{\mathrm{\τ}}_{\mathrm{gx}}+w_{\mathrm{\ϵ}}\left(t\right)\\ {\stackrel{\·}{\mathrm{\ϵ}}}_n=-{\mathrm{\ϵ}}_n/{\mathrm{\τ}}_{\mathrm{gy}}+w_{\mathrm{\ϵ}}\left(t\right)\\ {\stackrel{\·}{\mathrm{\ϵ}}}_u=-{\mathrm{\ϵ}}_u/{\mathrm{\τ}}_{\mathrm{gz}}+w_{\mathrm{\ϵ}}\left(t\right)\end{array}\right.---\left(9\right)$

其中：δv_e、δv_n、δv_u分别为惯导系统东向、北向、天向速度误差， 分别为其微分；φ_e、φ_n、φ_u为俯仰、横滚、方位角误差，分别为其微分；为东向加表（即加速度计）、北向加表、天向加表的零偏，分别为其微分；τ_ai(i=x,y,z)为加表零偏相关时间；ε_e、ε_n、ε_u分别为东向陀螺、北向陀螺、天向陀螺的零漂，分别为其微分；τ_gi(i=x,y,z)为陀螺零漂相关时间；w_ε分别为加速度计和陀螺的马尔可夫噪声项；ω_ie为地球自转角速度；R_xp为地球纬度圈半径，R_yp为地球经度圈半径。

滤波器的状态包括捷联惯导的速度误差δv_e、δv_n、δv_u，姿态误差φ_e、φ_n、φ_u，位置误差δλ、δL、δh，加表零偏陀螺零漂ε_e、ε_n、ε_u，即得到滤波器的状态向量（15维）和观测向量为：

$X={\left[\begin{array}{ccccccccccccccc}{\mathrm{\φ}}_e& {\mathrm{\φ}}_n& {\mathrm{\φ}}_u& {\mathrm{\δv}}_e& {\mathrm{\δv}}_n& {\mathrm{\δv}}_u& \mathrm{\δ\λ}& \mathrm{\δL}& \mathrm{\δh}& {\mathrm{\ϵ}}_e& {\mathrm{\ϵ}}_n& {\mathrm{\ϵ}}_u& {\▿}_e& {\▿}_n& {\▿}_u\end{array}\right]}^T$

$Z=\left[\begin{array}{c}{\mathrm{\δv}}_e\\ {\mathrm{\δv}}_n\\ {\mathrm{\δv}}_u\\ \mathrm{\δ\ψ}\end{array}\right]=\left[\begin{array}{c}{v}_{e\_\mathrm{ins}}-v_{e\_\mathrm{gps}}\\ v_{n\_\mathrm{ins}}-v_{n\_\mathrm{gps}}\\ v_{u\_\mathrm{ins}}-v_{u\_\mathrm{gps}}\\ {\mathrm{\ψ}}_{u\_\mathrm{ins}}-{\mathrm{\ψ}}_{u\_\mathrm{gps}}\end{array}\right]$

根据式(5)～(10)得到组合导航系统kalman滤波模型维：

$\left\{\begin{array}{c}\stackrel{\·}{X}=\mathrm{AX}+\mathrm{GW}\\ Z=\mathrm{HX}+V\end{array}\right.---\left(11\right)$

其中，A为15×15维状态转移矩阵；H为4×15维观测矩阵；G为15×6维噪声输入矩阵；W为6维噪声向量，由陀螺和加速度噪声构成；V为4维观测噪声，v_{e_ins}、v_{n_ins}、v_{u_ins}、ψ_{u_ins}分别为捷联惯导测量的载体的速度和方位，v_{e_gps}、v_{n_gps}、v_{u_gps}、ψ_{u_gps}分别为GPS测量的载体的速度和方位。

解算出误差项X，即得到速度误差δv_e、δv_n、δv_u，姿态误差φ_e、φ_n、φ_u，将速度误差和姿态误差叠加到系留飞艇的速度、俯仰角、横滚角和方位角上，然后以校正后的速度、俯仰角、横滚角和方位角为参考，不断地进行误差校正，直至误差最小。

Claims (1)

1.一种测量系留飞艇姿态的方法，其特征在于，该方法为：

1)选用系留飞艇所在地东北天地理坐标系为参考坐标系，利用激光陀螺捷联惯导测量的角速度和加速度，计算得到参考坐标系到系留飞艇坐标系的方向余弦矩阵

其中，θ、γ、ψ分别为系留飞艇俯仰角、横滚角和方位角；

2)定义方向余弦矩阵的转置矩阵为姿态矩阵T，则：

${\left(C_t^b\right)}^{-1}={\left(C_t^b\right)}^t=C_b^t=T,$

即： $\begin{array}{c}T=\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{\γ}\sin \mathrm{\ψ}-\cos \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}\\ -\sin \mathrm{\γ}\cos \mathrm{\θ}& \sin \mathrm{\θ}& \cos \mathrm{\γ}\cos \mathrm{\θ}\end{array}\right]\\ =\left[\begin{array}{ccc}{t}_{11}& t_{12}& t_{13}\\ t_{21}& t_{\mathrm{22}}& t_{\mathrm{23}}\\ t_{31}& t_{\mathrm{31}}& t_{\mathrm{33}}\end{array}\right]\end{array};$

3)由步骤2)确定ψ、θ、γ，完成系留飞艇姿态的初始对准：

$\left\{\begin{array}{c}\mathrm{\θ}=si{n}^{-1}\left(t_{32}\right)\\ \mathrm{\γ}=t{g}^{-1}(-t_{31}/t_{33})\\ \mathrm{\ψ}=t{g}^{-1}(-t_{12}/t_{22})\end{array}\right.;$

4)利用激光陀螺捷联惯导测量的角速度和加速度，计算得到系留飞艇在东北天地理坐标系上的速度v_{e_ins}、v_{n_ins}、v_{u_ins}、方位角ψ_{u_ins}和系留飞艇所在位置的经度λ、纬度L、高度h，其中ψ_{u_ins}＝ψ；

5)建立激光陀螺捷联惯导速度误差模型：

$\{\begin{array}{c}\mathrm{\δ}{\stackrel{\·}{v}}_e=-f_u{\mathrm{\φ}}_n+f_n{\mathrm{\φ}}_u+\left(2{\mathrm{\ω}}_{ie}\sin L\right){\mathrm{\δv}}_n-\left(2{\mathrm{\ω}}_{ie}\cos L\right){\mathrm{\δv}}_n+{\▿}_e\\ \mathrm{\δ}{\stackrel{\·}{v}}_n=f_u{\mathrm{\φ}}_e-f_e{\mathrm{\φ}}_u-\left(2{\mathrm{\ω}}_{ie}\sin L\right){\mathrm{\δv}}_e+{\▿}_n\\ \mathrm{\δ}{\stackrel{\·}{v}}_e=-f_n{\mathrm{\φ}}_e+f_e{\mathrm{\φ}}_n+\left(2{\mathrm{\ω}}_{ie}\cos L\right){\mathrm{\δv}}_e+{\▿}_u\end{array},$

其中，f_e、f_n、f_u分别为加表在东向、北向、天向测量的比力；δv_e、δv_n、δv_u分别为惯导系统东向、北向、天向速度误差，分别为δv_e、δv_n、δv_u的微分，φ_e、φ_n、φ_u分别为系留飞艇俯仰、横滚、方位角误差，ω_ie为地球自转角速度，分别为东向加速度计、北向加速度计、天向加速度计的零偏；

6)建立激光陀螺捷联惯导姿态误差模型：

$\left\{\begin{array}{c}{\stackrel{\·}{\mathrm{\φ}}}_e=\left({\mathrm{\ω}}_{ie}\sin L\right){\mathrm{\φ}}_n-\left({\mathrm{\ω}}_{ie}\cos L\right){\mathrm{\φ}}_u-\frac{{\mathrm{\δv}}_n}{R_{yp}+h}-{\mathrm{\ϵ}}_e\\ {\stackrel{\·}{\mathrm{\φ}}}_n=-\left({\mathrm{\ω}}_{ie}\sin L\right){\mathrm{\φ}}_e-\left({\mathrm{\ω}}_{ie}\sin L\right)\mathrm{\δ}L-\frac{{\mathrm{\δv}}_e}{R_{xp}+h}-{\mathrm{\ϵ}}_n\\ {\stackrel{\·}{\mathrm{\φ}}}_u=\left({\mathrm{\ω}}_{ie}\cos L\right){\mathrm{\φ}}_e+\left({\mathrm{\ω}}_{ie}\sin L\right)\mathrm{\δ}L+\frac{{\mathrm{\δv}}_e\tan L}{R_{xp}+h}-{\mathrm{\ϵ}}_u\end{array}\right.,$

其中，分别为φ_e、φ_n、φ_u的微分，R_xp为地球纬度圈半径，R_yp为地球经度圈半径，ω_ie为地球自转角速度，δλ、δL、δh分别为激光陀螺捷联惯导计算的系留飞艇位置的经度误差、纬度误差、高度误差；ε_e、ε_n、ε_u分别为东向陀螺、北向陀螺、天向陀螺的零漂；

7)建立激光陀螺捷联惯导位置误差模型：

$\left\{\begin{array}{c}\mathrm{\δ}\stackrel{\·}{L}=\frac{{\mathrm{\δv}}_n}{R_{yp}+h}\\ \mathrm{\δ}\stackrel{\·}{\mathrm{\λ}}=\frac{{\mathrm{\δv}}_e}{R_{xp}+h}\\ \mathrm{\δ}\stackrel{\·}{h}=\mathrm{\δ}{v}_u\end{array}\right.,$

分别为δλ、δL、δh的微分；

8)建立激光陀螺捷联惯导三个加速度计的零偏误差模型：

$\left\{\begin{array}{c}{\stackrel{\·}{\▿}}_e=-{\▿}_e/{\mathrm{\τ}}_{ax}+w_{ax}\left(t\right)\\ {\stackrel{\·}{\▿}}_n=-{\▿}_n/{\mathrm{\τ}}_{ay}+w_{ay}\left(t\right)\\ {\stackrel{\·}{\▿}}_u=-{\▿}_u/{\mathrm{\τ}}_{az}+w_{az}\left(t\right)\end{array}\right.,$

其中，分别为的微分，τ_ai,i＝x,y,z为三个加速度计零偏相关时间，w_ax、w_ay、w_az分别为三个加速度计的零均值高斯白噪声；

9)建立激光陀螺捷联惯导三个陀螺的零偏误差模型：

$\left\{\begin{array}{c}{\stackrel{\·}{\mathrm{\ϵ}}}_e=-{\mathrm{\ϵ}}_e/{\mathrm{\τ}}_{gx}+w_{gx}\left(t\right)\\ {\stackrel{\·}{\mathrm{\ϵ}}}_n=-{\mathrm{\ϵ}}_n/{\mathrm{\τ}}_{gy}+w_{gy}\left(t\right),\\ {\stackrel{\·}{\mathrm{\ϵ}}}_u=-{\mathrm{\ϵ}}_u/{\mathrm{\τ}}_{gz}+w_{gz}\left(t\right)\end{array}\right.$

其中，ε_e、ε_n、ε_u分别为东向陀螺，北向陀螺，天向陀螺的零漂，分别为ε_e、ε_n、ε_u的微分，τ_gi,i＝x,y,z为三个陀螺零漂相关时间，w_gx、w_gy、w_gz分别为三个陀螺的零均值高斯白噪声；

10)令 $Z=\left[\begin{array}{c}\mathrm{\δ}{V}_e\\ {\mathrm{\δV}}_n\\ {\mathrm{\δV}}_u\\ \mathrm{\δ}\mathrm{\ψ}\end{array}\right]=\left[\begin{array}{c}{v}_{e\_ins}-v_{e\_gps}\\ v_{n\_ins}-v_{n\_gps}\\ v_{u\_ins}-v_{u\_gps}\\ {\mathrm{\ψ}}_{u\_ins}-{\mathrm{\ψ}}_{u\_gps}\end{array}\right],$ 其中v_{e_gps}、v_{n_gps}、v_{u_gps}、ψ_{u_gps}分别为GPS定位定向模块测量的系留飞艇在东北天地理坐标系上的速度和方位角，δV_e、δV_n、δV_u、δψ分别为捷联惯导与GPS定位定向模块测量的系留飞艇的东向速度、北向速度、天向速度和方位角的差值，将Z作为kalman滤波器的输入；

11)根据步骤5)～10)，得到kalman滤波模型维：

$\{\begin{array}{c}\stackrel{\·}{X}=AX+GW\\ Z=HX+V\end{array},$

由kalman滤波模型维解算出X，从而对激光陀螺捷联惯导的各项导航误差进行实时估计；其中A为15×15维状态转移矩阵；H为4×15维观测矩阵；G为15×6维噪声输入矩阵；W为6维噪声向量，由陀螺和加速度计噪声构成；V为4维观测噪声，

$X={\left[\begin{array}{ccccccccccccccc}{\mathrm{\φ}}_e& {\mathrm{\φ}}_n& {\mathrm{\φ}}_u& {\mathrm{\δv}}_e& {\mathrm{\δv}}_n& {\mathrm{\δv}}_u& \mathrm{\δ}\mathrm{\λ}& \mathrm{\δ}L& \mathrm{\δ}h& {\mathrm{\ϵ}}_e& {\mathrm{\ϵ}}_n& {\mathrm{\ϵ}}_u& {\▿}_e& {\▿}_n& {\▿}_u\end{array}\right]}^T,$ 为X的微分；

12)对系留飞艇在东北天地理坐标系上的速度v_{e_ins}、v_{n_ins}、v_{u_ins}、方位角ψ_{u_ins}进行反馈校正，克服姿态角误差发散；

13)导航处理模块将当前时刻系留飞艇的位置、速度、姿态信息发送至系留飞艇控制系统，并返回5)。