[发明专利]一种系留飞艇姿态测量装置及测量方法

权利要求书

1.一种系留飞艇姿态测量装置，包括安装在系留飞艇桁架上的激光陀螺捷联惯导，其特征在于，还包括导航处理模块、GPS定位定向模块，所述导航处理模块包括FPGA、DSP，所述FPGA与所述DSP双向连接，所述FPGA与所述激光陀螺捷联惯导、GPS定位定向模块、系留飞艇控制系统连接。

2.根据权利要求1所述的系留飞艇姿态测量装置，其特征在于，所述FPGA通过数字隔离器与所述GPS定位定向模块连接，所述激光陀螺捷联惯导的输出的三路加速度信号和三路角速度信号依次通过光耦接收隔离器、电平转换电路输入所述FPGA，所述FPGA通过RS422差分驱动器将同步采集脉冲送入所述激光陀螺捷联惯导，所述FPGA通过RS422接口与所述系留飞艇控制系统连接。

3.根据权利要求1所述的系留飞艇姿态测量装置，其特征在于，所述GPS定位定向模块采用OEM模块。

4.一种利用权利要求1至3之一所述装置测量系留飞艇姿态的方法，其特征在于，该方法为：

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

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

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

(Ctb)-1=(Ctb)t=Cbt=T,]]>

即：T=cosγcosφ-sinγsinθsinψ-cosθsinψsinγcosψ+cosγsinθsinψcosγsinψ+sinγsinθcosψcosθcosψsinγsinψ-cosγsinθcosψ-sinγcosθsinθcosγcosθ]]>

=t11t12t13t21t22t23t31t32t33;]]>

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

θ=sin-1(t32)γ=tg-1(-t31/t33)ψ=tg-1(-t12/t22);]]>

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

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

δv·e=-fuφn+fnφu+(2ωiesinL)δvn-(2ωiecosL)δvu+▿eδv·n=fuφe-feφu-(2ωiesinL)δve+▿nδv·e=-fnφe+feφn+(2ωiecosL)δve+▿u,]]>

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

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

φ·e=(ωiesinL)φn-(ωiecosL)φu-δvnRyp+h-ϵeφ·n=-(ωiesinL)φe-(ωiesinL)δL-δveRxp+h-ϵnφ·u=(ωiecosL)φe+(ωiesinL)δL+δvetanLRxp+h-ϵu,]]>

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

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

δL·=δvnRyp+hδλ·=δveRxp+hδh·=δvu,]]>

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

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

▿·e=-▿e/τax+wax(t)▿·n=-▿n/τay+way(t)▿·u=-▿u/τaz+waz(t),]]>

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

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

ϵ·e=-ϵe/τgx+wgx(t)ϵ·n=-ϵn/τgy+wgy(t)ϵ·u=-ϵu/τgz+wgz(t),]]>

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

10）令Z=δVeδVnδVuδψ=ve_ins-ve_gpsvn_ins-vn_gpsvu_ins-vu_gpsψu_ins-ψu_gps,]]>其中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滤波模型维：

X·=AX+GWZ=HX+V,]]>

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

X=φeφnφuδveδvnδvuδλδLδhϵeϵnϵu▿e▿n▿uT,]]>为X的微分；

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

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