CN103913169B - 一种飞行器的捷联惯性/星光折射组合导航方法 - Google Patents

Abstract

本发明属于组合导航的技术领域，具体涉及一种飞行器的捷联惯性/星光折射组合导航方法。包括以下步骤：星敏感器输出载体的姿态并获取星光折射角；捷联惯导通过捷联解算得到导航信息；将步骤一和步骤二中的结果带入系统模型使用卡尔曼滤波进行状态估计；利用最优估计的结果修正惯性元件误差和导航信息并得到最终的导航结果。本发明提高了加速度计误差的估计精度，抑制导航误差的发散，解决传统方法不能准确估计加速度计偏置的问题。

Description

一种飞行器的捷联惯性/星光折射组合导航方法

技术领域

本发明属于组合导航的技术领域，具体涉及一种飞行器的捷联惯性/星光折射组合导航方法。

背景技术

惯性/天文组合导航系统作为一个黄金组合系统一直以来受到各国的高度重视。其组合方式一般分为简单组合方式和基于最优估计的组合方式，前一种简单、可靠，但是精度较低。目前基于最优估计的组合导航系统主要有两类：①基于校正惯导陀螺漂移的惯性/天文组合导航系统；②基于高度角和方位角直接敏感地平的组合导航系统；第一种组合导航系统是利用星敏感器获取高精度的姿态信息，将SINS与星敏感器的姿态差作为量测，通过滤波

器估计陀螺漂移。这种方式能够较好的修正由陀螺漂移引起的姿态误差，但是由于对加速度计误差估计不准确，不能阻止速度和位置误差的发散。而第二种方法由于地球表面不规则，使得地平仪或惯性平台提供水平基准的测量精度较低，这与星敏感器的测量精度不匹配，极大的影响了系统的定位精度。

20世纪80年代发展起来的基于星光折射间接敏感地平的方法能够有效的解决地平敏感精度不高的问题。它结合大气对星光的折射模型，利用高精度星敏感器精确敏感地平，从而实现高精度定位。国内外对此进行了大量的理论研究，并进行了实验验证。研究结果表明：这种方法成本低廉、结构简单，能达到较高的精度，是一种很有前途的导航方法，目前在30km的高空飞行器上已经实现应用。

发明内容

本发明的目的是为了提高加速度计误差的估计精度，抑制导航误差的发散，解决传统方法不能准确估计加速度计偏置的问题，提出了一种飞行器的捷联惯性/星光折射组合导航方法。

本发明的目的是这样实现的：

包括以下步骤：

步骤一：星敏感器输出载体的姿态并获取星光折射角；

步骤二：捷联惯导通过捷联解算得到导航信息；

步骤三：将步骤一和步骤二中的结果带入系统模型使用卡尔曼滤波进行状态估计；

步骤四：利用最优估计的结果修正惯性元件误差和导航信息并得到最终的导航结果。

步骤三中，系统模型的建立分为如下子步骤：

步骤A：建立系统状态方程；

步骤B：建立系统的量测方程，与步骤A中的状态方程组成系统模型。

子步骤A中，系统状态方程建立的具体方法为：

飞行器的导航坐标系选取为发射点惯性坐标系，系统的状态方程为：

$\stackrel{.}{X}=\mathrm{FX}+\mathrm{Gw}$

其中，X为系统状态矢量；w为系统噪声矢量；F为系统状态矩阵；G为系统噪声驱动矩阵；系统的状态包括姿态误差角φ＝[φ_x,φ_y,φ_z]^T；速度误差δv=[δv_x,δv_y,δv_z]^T；位置误差δr=[δx_c,δy_c,δz_c]^T；陀螺常值漂移ξ＝[ε_x,ε_y,ε_z]^T；加速度计常值偏置

$X={[{\mathrm{\φ}}_x,{\mathrm{\φ}}_y,{\mathrm{\φ}}_z,{\mathrm{\δv}}_x,{\mathrm{\δv}}_y,{\mathrm{\δv}}_z,{\mathrm{\δx}}_c,{\mathrm{\δy}}_c,{\mathrm{\δz}}_c,{\mathrm{\ϵ}}_x,{\mathrm{\ϵ}}_y,{\mathrm{\ϵ}}_z,{\▿}_x,{\▿}_y,{\▿}_z]}^T;$

$w={\left[\begin{array}{cccccc}{w}_{\mathrm{\ξx}}& w_{\mathrm{\ξy}}& w_{\mathrm{\ξz}}& w_{\▿x}& w_{\▿y}& w_{\▿z}\end{array}\right]}^T$ w_ε＝[wξ_x wξ_y wξ_z]^T和 $w_{\▿}={\left[\begin{array}{ccc}{w}_{\▿x}& w_{\▿y}& w_{\▿z}\end{array}\right]}^T$ 分别代表陀螺和加速度计的随机噪声；

$G={\left[\begin{array}{ccc}{C}_b^c& 0_{3\×3}& 0_{3\×9}\\ 0_{3\×3}& C_b^c& 0_{3\×9}\end{array}\right]}^T;F=\left[\begin{array}{ccccc}{0}_{3\×3}& 0_{3\×3}& 0_{3\×3}& C_b^c& 0_{3\×3}\\ F_a& 0_{3\×3}& F_b& 0_{3\×3}& C_b^c\\ 0_{3\×3}& I_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ 0_{6\×3}& 0_{6\×3}& 0_{6\×3}& 0_{6\×3}& 0_{6\×3}\end{array}\right];$

其中，为从载体坐标系到发射点坐标系的坐标变换矩阵；

令 ${\left[\begin{array}{ccc}{\mathrm{ac}}_1& {\mathrm{ac}}_2& {\mathrm{ac}}_3\end{array}\right]}^T=C_b^c{\left[\begin{array}{ccc}{a}_1& a_2& a_3\end{array}\right]}^T,$ 其中a₁、a₂、a₃为加速度计测量的比力；0和I代表零矩阵和单位阵；F_a和F_b表示如下：

$F_a=\left[\begin{array}{ccc}0& -a{c}_3& {\mathrm{ac}}_2\\ {\mathrm{ac}}_3& 0& -{\mathrm{ac}}_1\\ -{\mathrm{ac}}_2& {\mathrm{ac}}_1& 0\end{array}\right];F_b=\left[\begin{array}{ccc}-\frac{\mathrm{\μ}}{r^3}(1-\frac{3x_c^2}{r^2})& \frac{3{\mathrm{\μx}}_c(y_c+R_e)}{r^5}& \frac{3{\mathrm{\μx}}_c{z}_c}{r^5}\\ \frac{3\mathrm{\μ}{x}_c(y_c+R_e)}{r^5}& -\frac{\mathrm{\μ}}{r^3}[1-\frac{3{(R_e+y_c)}^2}{r^2}]& \frac{3\mathrm{\μ}{z}_c(y_c+R_e)}{r^5}\\ \frac{3\mathrm{\μ}{x}_c{z}_c}{r^5}& \frac{3\mathrm{\μ}{z}_c(y_c+R_e)}{r^5}& -\frac{\mathrm{\μ}}{r^3}(1-\frac{3z_c^2}{r^2})\end{array}\right]$

其中，μ为地心引力常数；r为载体到地心的距离；R_e为地球半径；x_c、y_c、z_c为载体在发射点坐标系的位置。

子步骤B中，系统量测方程建立的具体方法如下：

系统的量测方程分为两部分：姿态误差角量测和折射视高度量测；

姿态误差量测表示如下:

$Z_1=C_i^c\left[\begin{array}{c}\mathrm{\Δ\β}\\ \mathrm{\Δ\θ}\\ \mathrm{\Δ\α}\end{array}\right]=C_i^c\left[\begin{array}{c}{\mathrm{\β}}_I-{\mathrm{\β}}_X\\ {\mathrm{\θ}}_I-{\mathrm{\θ}}_X\\ {\mathrm{\α}}_I-{\mathrm{\α}}_X\end{array}\right]=H_{1}X+v_1$

其中，为惯性坐标系到发射点惯性坐标系的坐标变换矩阵，当发射点确定以后是一个常值矩阵；假设发射点的经纬度分别为φ和γ，发射角为A，则为

$C_i^c=\left[\begin{array}{ccc}-\cos A\sin \mathrm{\γ}\cos \mathrm{\φ}-\sin A\sin \mathrm{\φ}& \cos \mathrm{\γ}\cos \mathrm{\φ}& \sin A\sin \mathrm{\γ}\cos \mathrm{\φ}-\cos A\sin \mathrm{\φ}\\ -\cos A\sin \mathrm{\γ}\sin \mathrm{\φ}+\sin A\cos \mathrm{\φ}& \cos \mathrm{\γ}\sin \mathrm{\φ}& \sin A\sin \mathrm{\γ}\sin \mathrm{\φ}+\cos A\cos \mathrm{\φ}\\ \cos A\cos \mathrm{\γ}& \sin \mathrm{\γ}& -\sin A\cos \mathrm{\γ}\end{array}\right];$

β_I、θ_I和α_I分别为由捷联惯导解算得到的横滚角、俯仰角和航向角；β_X、θ_X和α_X分别为由星敏感器输出的横滚角、俯仰角和航向角；[Δβ Δθ Δα]^T为姿态误差角；v₁为星敏感器的随机噪声；H₁＝[I_3×3 0_3×12]为姿态误差角量测的转移矩阵；

定义视高度为h_a，折射高度为h_g，R_e为地球半径；r为飞行器的位置矢量；u为位置矢量在恒星入射光线方向上的分量；R为星光折射角；

根据折射视高度与折射角之间的几何关系得：

$h_a=\sqrt{r^2-u^2}+u\tan R-R_e-a$

其中，

$\left\{\begin{array}{c}r=\left|r\right|=\sqrt{x^2+y^2+z^2}\\ u=|r\·u|=|{\mathrm{xs}}_x+{\mathrm{ys}}_y+{\mathrm{zs}}_z|\end{array}\right.,$

r＝[x y z]为载体在地心赤道惯性坐标系的位置矢量，u=[s_x s_y s_z]^T为折射前的星光矢量,s_x、s_y、s_z恒星在天球坐标系的方向矢量，星图识别成功后可通过查找星表得到；a为一个小量，通常忽略不计；根据大气密度模型得到星光折射角和视高度的关系：

h_ac＝57.081+2.531e^{[0.981ln(R)-8.689]}-6.441ln(R)

r和u中含有与地球位置相关的参数，因此h_a必定会受到捷联惯导噪声的影响而存在折射视高度误差；真实的视高度h_at＝h_ac+v_a；v_a是零均值高斯白噪声，且则折射视高度误差δh_a可以被表示为：

δh_a＝h_at-h_a＝h_ac-h_a+v_a

${\mathrm{\δh}}_a=\mathrm{\δ}(\sqrt{r^2-u^2}+u\tan R-R_e)=\frac{r\·\mathrm{\δr}-u\·\mathrm{\δu}}{\sqrt{r^2-u^2}}+\mathrm{\δu}\·\tan R+\frac{u\·\mathrm{\δR}}{1+R^2}$

载体在发射点惯性坐标系下的位置矢量为r_c，在惯性坐标系下的位置矢量r之间的关系为：

$r=C_c^i{r}_c+R_c$

其中，r_c=[x_c y_c z_c]^T；R_c＝[R_cx R_cy R_cz]^T为发射点子午圈半径在地心惯性系下的投影； $C_c^i={\left(C_i^c\right)}^{-1},$ 且令

$C_c^i=\left[\begin{array}{ccc}{c}_{11}& c_{12}& c_{13}\\ c_{21}& c_{22}& c_{23}\\ c_{31}& c_{32}& c_{33}\end{array}\right]$

得载体位置矢量在惯性系投影为

$\left\{\begin{array}{c}x=c_{11}{x}_c+c_{12}{y}_c+c_{13}{z}_c+R_{\mathrm{cx}}\\ y=c_{21}{x}_c+c_{22}{y}_c+c_{23}{z}_c+R_{\mathrm{cy}}\\ z=c_{31}{x}_c+c_{32}{y}_c+c_{33}{z}_c+R_{\mathrm{cz}}\end{array}\right.$

求微分得到载体的在地心惯性系各轴的位置误差为

$\left\{\begin{array}{c}\mathrm{\δx}=c_{11}{\mathrm{\δx}}_c+c_{12}{\mathrm{\δy}}_c+c_{13}{\mathrm{\δz}}_c\\ \mathrm{\δy}=c_{21}{\mathrm{\δx}}_c+c_{22}{\mathrm{\δy}}_c+c_{23}{\mathrm{\δz}}_c\\ \mathrm{\δz}=c_{31}{\mathrm{\δx}}_c+c_{32}{\mathrm{\δy}}_c+c_{33}{\mathrm{\δz}}_c\end{array}\right.$

可得δr、δu，

$\mathrm{\δha}=a_1\·{\mathrm{\δx}}_c+a_2\·{\mathrm{\δy}}_c+a_3\·{\mathrm{\δz}}_c+\frac{u\·\mathrm{\δR}}{1+R^2}$

当u＜0时 $m=\sqrt{r^2-u^2}$

$\left\{\begin{array}{c}{u}_1=c_{11}x+c_{21}y+c_{31}z\\ u_2=c_{12}x+c_{22}y+c_{32}z\\ u_3=c_{13}x+c_{23}y+c_{33}z\end{array}\right.$ $\left\{\begin{array}{c}{\mathrm{du}}_1=c_{11}{s}_x+c_{21}{s}_y+c_{31}{s}_z\\ {\mathrm{du}}_2=c_{12}{s}_x+c_{22}{s}_y+c_{32}{s}_z\\ {\mathrm{du}}_3=c_{13}{s}_x+c_{23}{s}_y+c_{33}{s}_z\end{array}\right.$ $\left\{\begin{array}{c}{a}_1=\frac{u_1}{m}-(\frac{u}{m}-\tan R)\·{\mathrm{du}}_1\\ a_2=\frac{u_2}{m}-(\frac{u}{m}-\tan R)\·{\mathrm{du}}_2\\ a_3=\frac{u_3}{m}-(\frac{u}{m}-\tan R)\·{\mathrm{du}}_3\end{array}\right.$

量测误差：

$\frac{u}{1+R^2}\·\mathrm{\δR}=v_b$

δR为星敏感器的量测噪声，量测方程表示为：

z＝h_ac-h_a＝hX+v_b-v_a

v_b不是零均值高斯白噪声，设那么：

$z=\mathrm{hX}+d\·\mathrm{\δR}-v_a=\mathrm{hX}+D\left[\begin{array}{c}\mathrm{\δR}\\ v_a\end{array}\right]$

其中，D＝[d-1]：

$D^{-1}z=D^{-1}\mathrm{hX}+\left[\begin{array}{c}\mathrm{\δR}\\ v_a\end{array}\right]$

其中，D^-1＝D^T(DD^T)^-1为广义逆，定义z^*＝D^-1z，h^*＝D^-1h，v^*＝[δR v_a]^T，

z^*＝h^*X+v^*

$Z_2=\left[\begin{array}{c}{h}_1^{*}\\ h_2^{*}\\ \·\\ \·\\ \·\end{array}\right]X+\left[\begin{array}{c}{v}_1^{*}\\ v_2^{*}\\ \·\\ \·\\ \·\end{array}\right]=H_{2}X+v_2$

其中，下标1/2…代表折射星的标号，得到系统总的量测方程为：

$Z=\left[\begin{array}{c}{Z}_1\\ Z_2\end{array}\right]=\left[\begin{array}{c}{H}_1\\ H_2\end{array}\right]X+\left[\begin{array}{c}{v}_1\\ v_2\end{array}\right].$

本发明的有益效果在于：本发明提高了加速度计误差的估计精度，抑制导航误差的发散，解决传统方法不能准确估计加速度计偏置的问题。

附图说明

图1是星光折射角的求解；

图2是星光折射几何图示；

图3是捷联惯性/星光折射组合导航的工作框图；

图4是使用一颗折射星时传统方法与新房法位置误差对比曲线；

图5是使用一颗折射星时传统方法与新房法姿态误差对比曲线；

图6是使用多颗折射星时新方法的导航精度对比曲线。

具体实施方式

下面将结合附图和实施例对本发明作进一步的详细说明。

表1是使用多颗折射星时新方法的导航精度统计结果；

图1中：

u_sa-非折射星的星光矢量 u_sb-折射星光发生折射前的星光矢量

P₁-与u_sa相同为非折射星的星光矢量 P₂-折射星光发生折射后的星光矢量

θ₁-u_sb与P₁的夹角 θ₂-P₁与P₂的夹角

R-星光折射角

图2中：

h_a-折射视高度 h_g-折射高度 R_e-地球半径

r-飞行器的位置矢量 O-地心 a为一个小量，通常忽略不计

u-位置矢量在恒星入射光线方向上的分量

图3中：

h_ac-为使用大气折射模型得到的折射视高度δh_a-折射视高度误差

β_I、θ_I、α_I-由捷联惯导解算得到的横滚角、俯仰角和航向角

β_X、θ_X、α_X-由星敏感器输出的横滚角、俯仰角和航向角

步骤一：星敏感器输出载体的姿态并计算星光折射角；

本发明组合导航系统需要两个星敏感器，星敏感器a对准不发生折射的恒星，通过观测结果确定出未折射的星光矢量在载体本体坐标系的坐标P₁和载体的姿态角；星敏感器b对准折射星，由观测值可以确定出折射后的星光方向矢量在本体坐标系中的坐标P₂，如图1所示，由P₁和P₂可得到两恒星星光之间的角距θ₂，它与由恒星星历查得的标称值θ₁不同，二者的差值就是星光折射角。

步骤二：捷联惯导通过捷联解算得到导航信息；

捷联惯导通过捷联解算得到飞行器的速度、位置、姿态等导航信息。

步骤三：将步骤一和步骤二中的结果带入系统模型使用卡尔曼滤波进行状态估计；

系统模型的具体建立步骤为：

步骤A：建立系统状态方程；

本文飞行器为发射型的，将其导航坐标系选取为发射点惯性坐标系，系统的状态方程表示如下：

$\stackrel{.}{X}=\mathrm{FX}+\mathrm{Gw}---\left(1\right)$

其中，X为系统状态矢量；w为系统噪声矢量；F为系统状态矩阵；G为系统噪声驱动矩阵；系统的状态包括姿态误差角φ＝[φ_x,φ_y,φ_z]^T；速度误差δv=[δv_x,δv_y,δv_z]^T；位置误差δr=[δx_c,δy_c,δz_c]^T；陀螺常值漂移ξ＝[ε_x,ε_y,ε_z]^T；加速度计常值偏置

$X={[{\mathrm{\φ}}_x,{\mathrm{\φ}}_y,{\mathrm{\φ}}_z,{\mathrm{\δv}}_x,{\mathrm{\δv}}_y,{\mathrm{\δv}}_z,{\mathrm{\δx}}_c,{\mathrm{\δy}}_c,{\mathrm{\δz}}_c,{\mathrm{\ϵ}}_x,{\mathrm{\ϵ}}_y,{\mathrm{\ϵ}}_z,{\▿}_x,{\▿}_y,{\▿}_z]}^T;$

$w={\left[\begin{array}{cccccc}{w}_{\mathrm{\ξx}}& w_{\mathrm{\ξy}}& w_{\mathrm{\ξz}}& w_{\▿x}& w_{\▿y}& w_{\▿z}\end{array}\right]}^T$ w_ε＝[wξ_x wξ_y wξ_z]^T和 $w_{\▿}={\left[\begin{array}{ccc}{w}_{\▿x}& w_{\▿y}& w_{\▿z}\end{array}\right]}^T$ 分别代表陀螺和加速度计的随机噪声；

$G={\left[\begin{array}{ccc}{C}_b^c& 0_{3\×3}& 0_{3\×9}\\ 0_{3\×3}& C_b^c& 0_{3\×9}\end{array}\right]}^T;F=\left[\begin{array}{ccccc}{0}_{3\×3}& 0_{3\×3}& 0_{3\×3}& C_b^c& 0_{3\×3}\\ F_a& 0_{3\×3}& F_b& 0_{3\×3}& C_b^c\\ 0_{3\×3}& I_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ 0_{6\×3}& 0_{6\×3}& 0_{6\×3}& 0_{6\×3}& 0_{6\×3}\end{array}\right];$

其中，为从载体坐标系到发射点坐标系的坐标变换矩阵；令 ${\left[\begin{array}{ccc}{\mathrm{ac}}_1& {\mathrm{ac}}_2& {\mathrm{ac}}_3\end{array}\right]}^T=C_b^c{\left[\begin{array}{ccc}{a}_1& a_2& a_3\end{array}\right]}^T,$ 其中a₁、a₂、a₃为加速度计测量的比力；0和I代表零矩阵和单位阵；F_a和F_b表示如下：

$F_a=\left[\begin{array}{ccc}0& -a{c}_3& {\mathrm{ac}}_2\\ {\mathrm{ac}}_3& 0& -{\mathrm{ac}}_1\\ -{\mathrm{ac}}_2& {\mathrm{ac}}_1& 0\end{array}\right];F_b=\left[\begin{array}{ccc}-\frac{\mathrm{\μ}}{r^3}(1-\frac{3x_c^2}{r^2})& \frac{3{\mathrm{\μx}}_c(y_c+R_e)}{r^5}& \frac{3{\mathrm{\μx}}_c{z}_c}{r^5}\\ \frac{3\mathrm{\μ}{x}_c(y_c+R_e)}{r^5}& -\frac{\mathrm{\μ}}{r^3}[1-\frac{3{(R_e+y_c)}^2}{r^2}]& \frac{3\mathrm{\μ}{z}_c(y_c+R_e)}{r^5}\\ \frac{3\mathrm{\μ}{x}_c{z}_c}{r^5}& \frac{3\mathrm{\μ}{z}_c(y_c+R_e)}{r^5}& -\frac{\mathrm{\μ}}{r^3}(1-\frac{3z_c^2}{r^2})\end{array}\right]$

其中，μ为地心引力常数；r为飞行器到地心的距离；R_e为地球半径；x_c、y_c、z_c为载体在发射点坐标系的位置。

步骤B：建立系统的量测方程；

系统的量测方程分为两部分：姿态误差角量测和折射视高度量测；

姿态误差量测表示如下:

$Z_1=C_i^c\left[\begin{array}{c}\mathrm{\Δ\β}\\ \mathrm{\Δ\θ}\\ \mathrm{\Δ\α}\end{array}\right]=C_i^c\left[\begin{array}{c}{\mathrm{\β}}_I-{\mathrm{\β}}_X\\ {\mathrm{\θ}}_I-{\mathrm{\θ}}_X\\ {\mathrm{\α}}_I-{\mathrm{\α}}_X\end{array}\right]=H_{1}X+v_1---\left(2\right)$

其中，为惯性坐标系到发射点惯性坐标系的坐标变换矩阵，当发射点确定以后是一个常值矩阵；假设发射点的经纬度分别为φ和γ，发射角为A，则为

$C_i^c=\left[\begin{array}{ccc}-\cos A\sin \mathrm{\γ}\cos \mathrm{\φ}-\sin A\sin \mathrm{\φ}& \cos \mathrm{\γ}\cos \mathrm{\φ}& \sin A\sin \mathrm{\γ}\cos \mathrm{\φ}-\cos A\sin \mathrm{\φ}\\ -\cos A\sin \mathrm{\γ}\sin \mathrm{\φ}+\sin A\cos \mathrm{\φ}& \cos \mathrm{\γ}\sin \mathrm{\φ}& \sin A\sin \mathrm{\γ}\sin \mathrm{\φ}+\cos A\cos \mathrm{\φ}\\ \cos A\cos \mathrm{\γ}& \sin \mathrm{\γ}& -\sin A\cos \mathrm{\γ}\end{array}\right];$

β_I、θ_I和α_I分别为由捷联惯导解算得到的横滚角、俯仰角和航向角；β_X、θ_X和α_X分别为由星敏感器输出的横滚角、俯仰角和航向角；[Δβ Δθ Δα]^T为姿态误差角；v₁为星敏感器的随机噪声；H₁＝[I_3×3 0_3×12]为姿态误差角量测的转移矩阵；

当来自恒星的星光经过大气层时，光线会发生折射从而向地心方向弯曲，若经过折射的光线被安装在载体上的星敏感器观测后，从载体上看，恒星的视位置将会比其真实位置偏高，如图2所示，其视高度为h_a，折射高度为h_g，R_e为地球半径；r为飞行器的位置矢量；u为位置矢量在恒星入射光线方向上的分量；R为星光折射角。

由图中的几何关系可得

$h_a=\sqrt{r^2-u^2}+u\tan R-R_e-a---\left(3\right)$

其中，

$\left\{\begin{array}{c}r=\left|r\right|=\sqrt{x^2+y^2+z^2}\\ u=|r\·u|=|{\mathrm{xs}}_x+{\mathrm{ys}}_y+{\mathrm{zs}}_z|\end{array}\right.,$

r＝[x y z]为载体在地心赤道惯性坐标系的位置矢量，u=[s_x s_y s_z]^T为折射前的星光矢量,s_x、s_y、s_z恒星在天球坐标系的方向矢量，星图识别成功后可通过查找星表得到；a为一个小量，通常忽略不计。

根据大气密度模型也可以得到星光折射角和视高度的关系：

h_ac＝57.081+2.531e^{[0.981ln(R)-8.689]}-6.441ln(R) (4)

对于公式(3)，r和u中含有与地球位置相关的参数，因此h_a必定会受到捷联惯导噪声的影响而存在折射视高度误差；而公式(4)是一个经验公式，假设其视高度误差为v_a，则真实的视高度h_at＝h_ac+v_a；影响大气折射模型精度的主要因素是大气密度模型的准确性，研究表明：通过深入研究大气密度模型，是可以将其模型误差控制在1%之内的，而且1%的大气密度模型误差将会引起76m的折射视高度误差，因此假设v_a是零均值高斯白噪声，且则折射视高度误差δh_a可以被表示为：

δh_a＝h_at-h_a＝h_ac-h_a+v_a (5)

根据公式(3)

${\mathrm{\δh}}_a=\mathrm{\δ}(\sqrt{r^2-u^2}+u\tan R-R_e)=\frac{r\·\mathrm{\δr}-u\·\mathrm{\δu}}{\sqrt{r^2-u^2}}+\mathrm{\δu}\·\tan R+\frac{u\·\mathrm{\δR}}{1+R^2}---\left(6\right)$

飞行器的导航坐标系为发射点惯性坐标系，而上式中与状态矢量相关的参数都在来自与惯性坐标系，因此需要将其转化到导航坐标系下，载体在发射点惯性坐标系下的位置矢量为r_c，在惯性坐标系下的位置矢量r如式(3)所示，它们之间的关系为：

$r=C_c^i{r}_c+R_c---\left(7\right)$

其中，r_c=[x_c y_c z_c]^T；R_c＝[R_cx R_cy R_cz]^T为发射点子午圈半径在地心惯性系下的投影； $C_c^i={\left(C_i^c\right)}^{-1},$ 且令

$C_c^i=\left[\begin{array}{ccc}{c}_{11}& c_{12}& c_{13}\\ c_{21}& c_{22}& c_{23}\\ c_{31}& c_{32}& c_{33}\end{array}\right]---\left(8\right)$

则由(7)式得载体位置矢量在惯性系投影为

$\left\{\begin{array}{c}x=c_{11}{x}_c+c_{12}{y}_c+c_{13}{z}_c+R_{\mathrm{cx}}\\ y=c_{21}{x}_c+c_{22}{y}_c+c_{23}{z}_c+R_{\mathrm{cy}}\\ z=c_{31}{x}_c+c_{32}{y}_c+c_{33}{z}_c+R_{\mathrm{cz}}\end{array}\right.---\left(9\right)$

对上式求微分得到载体的在地心惯性系各轴的位置误差为

$\left\{\begin{array}{c}\mathrm{\δx}=c_{11}{\mathrm{\δx}}_c+c_{12}{\mathrm{\δy}}_c+c_{13}{\mathrm{\δz}}_c\\ \mathrm{\δy}=c_{21}{\mathrm{\δx}}_c+c_{22}{\mathrm{\δy}}_c+c_{23}{\mathrm{\δz}}_c\\ \mathrm{\δz}=c_{31}{\mathrm{\δx}}_c+c_{32}{\mathrm{\δy}}_c+c_{33}{\mathrm{\δz}}_c\end{array}\right.---\left(10\right)$

同理，可得δr、δu，将其带入(6)式得

$\mathrm{\δha}=a_1\·{\mathrm{\δx}}_c+a_2\·{\mathrm{\δy}}_c+a_3\·{\mathrm{\δz}}_c+\frac{u\·\mathrm{\δR}}{1+R^2}---\left(11\right)$

上述推导是在u＞0的条件下得到的，当u＜0时推导过程类似，其中，

$\left\{\begin{array}{c}{u}_1=c_{11}x+c_{21}y+c_{31}z\\ u_2=c_{12}x+c_{22}y+c_{32}z\\ u_3=c_{13}x+c_{23}y+c_{33}z\end{array}\right.$ $\left\{\begin{array}{c}{\mathrm{du}}_1=c_{11}{s}_x+c_{21}{s}_y+c_{31}{s}_z\\ {\mathrm{du}}_2=c_{12}{s}_x+c_{22}{s}_y+c_{32}{s}_z\\ {\mathrm{du}}_3=c_{13}{s}_x+c_{23}{s}_y+c_{33}{s}_z\end{array}\right.$ $\left\{\begin{array}{c}{a}_1=\frac{u_1}{m}-(\frac{u}{m}-\tan R)\·{\mathrm{du}}_1\\ a_2=\frac{u_2}{m}-(\frac{u}{m}-\tan R)\·{\mathrm{du}}_2\\ a_3=\frac{u_3}{m}-(\frac{u}{m}-\tan R)\·{\mathrm{du}}_3\end{array}\right.$

将u代入(11)最后一项，作为量测误差：

$\frac{u}{1+R^2}\·\mathrm{\δR}=v_b---\left(12\right)$

δR可看作星敏感器的量测噪声，它是一个零均值的高斯白噪声，则量测方程表示为：

z＝h_ac-h_a＝hX+v_b-v_a (13)

(13)中vb不是零均值高斯白噪声，在这种情况下，量测扩增方法是一个很好的选择，假设 $d=\frac{u}{1+R^2},$ 那么：

$z=\mathrm{hX}+d\·\mathrm{\δR}-v_a=\mathrm{hX}+D\left[\begin{array}{c}\mathrm{\δR}\\ v_a\end{array}\right]---\left(14\right)$

其中，D＝[d-1]，因此我们得到如下方程：

$D^{-1}z=D^{-1}\mathrm{hX}+\left[\begin{array}{c}\mathrm{\δR}\\ v_a\end{array}\right]---\left(15\right)$

其中，D^-1＝D^T(DD^T)^-1为广义逆，定义z^*＝D^-1z，h^*＝D^-1h，v^*＝[δR v_a]^T，我们得到新的量测方程为;

z^*＝h^*X+v^* (16)

当使用多颗折射星时，量测方程可以表示为：

$Z_2=\left[\begin{array}{c}{h}_1^{*}\\ h_2^{*}\\ \·\\ \·\\ \·\end{array}\right]X+\left[\begin{array}{c}{v}_1^{*}\\ v_2^{*}\\ \·\\ \·\\ \·\end{array}\right]=H_{2}X+v_2---\left(17\right)$

其中，下标1/2…代表折射星的标号，将式(17)与式(2)组合得到系统总的量测方程为：

$Z=\left[\begin{array}{c}{Z}_1\\ Z_2\end{array}\right]=\left[\begin{array}{c}{H}_1\\ H_2\end{array}\right]X+\left[\begin{array}{c}{v}_1\\ v_2\end{array}\right]---\left(18\right)$

公式(1)与公式(18)组成了系统的模型，利用建立的系统模型进行卡尔曼滤波，估计系统状态。

步骤四：利用最优估计的结果修正惯性元件误差和导航信息并得到最终的导航结果；

利用状态估计的结果修正捷联惯导的陀螺漂移和加速度计偏置，系统工作的原理图如图3所示。

由于步骤三中的系统模型为线性的，因此采用了卡尔曼滤波进行状态估计，而卡尔曼滤波的估计精度有系统状态的可观测性决定，因此为了验证一种飞行器的捷联惯性/星光折射组合导航方法的性能和导航精度，首先使用分段定常系统(PWCS，Piece-wiseConstant System)可观测性分析方法对系统模型进行可观测性分析，PWCS分析方法可根据选择可观测性矩阵(SOM,Stripped Observability Matrix)的秩来反映系统可观测的状态数目，分析结果为：当不使用折射星时，也就是传统方法，SOM的秩为6；当使用一颗折射星时SOM的秩为11；使用两颗折射星SOM的秩为14；使用时三颗及以上折射星时SOM的秩为15；从分析结果看，传统方法SOM的秩只有6，当使用折射星时SOM的秩显著增加，即系统的可观测性增强，系统可观测的状态增多，当所使用的折射星数目达到三颗及以上时，系统完全可观测，说明新方法可观测性要强于传统方法，因此卡尔曼滤波后的导航精度也要高于传统方法。

同时，还通过仿真结果对一种飞行器的捷联惯性/星光折射组合导航方法的性能和导航精度进行了验证，图4为使用一颗折射星时新方法与传统方法位置误差对比曲线；图5为使用一颗折射星时新方法与传统方法姿态误差对比曲线；从图中可以看出，新方法的姿态测量精度保持了星敏感器测姿的高精度特性，跟传统方法相差不大，而新方法的位置误差要远远小于传统方法，然而由于只使用了一颗折射星，仍然不能彻底阻止速度和位置的发散；

图6和表1为新方法使用多颗折射星时的仿真结果，图或表中1、2、3代表所使用的折射星数目;由图6和表1可以看出，系统的导航精度随着所使用的折射星数目增加而提高，而且当折射星数目达到三颗时，系统导航误差收敛，这跟可观测性分析的结果相同，证明了新方法高精度的导航特性。

表1

Claims (1)

1.一种飞行器的捷联惯性/星光折射组合导航方法，其特征在于，包括以下步骤：

步骤一：星敏感器输出载体的姿态并获取星光折射角；

步骤二：捷联惯导通过捷联解算得到导航信息；

步骤三：将步骤一和步骤二中的结果带入系统模型使用卡尔曼滤波进行状态估计；

步骤四：利用最优估计的结果修正惯性元件误差和导航信息并得到最终的导航结果；

步骤三中，系统模型的建立分为如下子步骤：

步骤A：建立系统状态方程；

步骤B：建立系统的量测方程，与步骤A中的状态方程组成系统模型；

子步骤A中，系统状态方程建立的具体方法为：

飞行器的导航坐标系选取为发射点惯性坐标系，系统的状态方程为：

$\stackrel{\·}{X}=FX+Gw$

其中，X为系统状态矢量；w为系统噪声矢量；F为系统状态矩阵；G为系统噪声驱动矩阵；系统的状态包括姿态误差角φ＝[φ_x,φ_y,φ_z]^T；速度误差δv＝[δv_x,δv_y,δv_z]^T；位置误差δr＝[δx_c,δy_c,δz_c]^T；陀螺常值漂移ξ＝[ε_x,ε_y,ε_z]^T；加速度计常值偏置

w＝[w_ξx w_ξy w_ξz w_▽x w_▽y w_▽z]^T w_ε＝[w_ξx w_ξy w_ξz]^T和分别代表陀螺和加速度计的随机噪声；

$G={\left[\begin{array}{ccc}{C}_b^c& 0_{3\×3}& 0_{3\×9}\\ 0_{3\×3}& C_b^c& 0_{3\×9}\end{array}\right]}^T;F=\left[\begin{array}{ccccc}{0}_{3\×3}& 0_{3\×3}& 0_{3\×3}& C_b^c& 0_{3\×3}\\ F_a& 0_{3\×3}& F_b& 0_{3\×3}& C_b^c\\ 0_{3\×3}& I_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ 0_{6\×3}& 0_{6\×3}& 0_{6\×3}& 0_{6\×3}& 0_{6\×3}\end{array}\right];$

其中，为从载体坐标系到发射点坐标系的坐标变换矩阵；

令其中a₁、a₂、a₃为加速度计测量的比力；0和I代表零矩阵和单位阵；F_a和F_b表示如下：

$F_a=\left[\begin{array}{ccc}0& -{\mathrm{ac}}_3& {\mathrm{ac}}_2\\ {\mathrm{ac}}_3& 0& -{\mathrm{ac}}_1\\ -{\mathrm{ac}}_2& {\mathrm{ac}}_1& 0\end{array}\right];F_b=\left[\begin{array}{ccc}-\frac{\mathrm{\μ}}{r^3}(1-\frac{3x_c^2}{r^2})& \frac{3{\mathrm{\μx}}_c(y_c+R_e)}{r^5}& \frac{3{\mathrm{\μx}}_c{z}_c}{r^5}\\ \frac{3{\mathrm{\μx}}_c(y_c+R_e)}{r^5}& -\frac{\mathrm{\μ}}{r^3}\[1-\frac{3{(R_e+y_c)}^2}{r^2}\]& \frac{3{\mathrm{\μz}}_c(y_c+R_e)}{r^5}\\ \frac{3{\mathrm{\μx}}_c{z}_c}{r^5}& \frac{3{\mathrm{\μz}}_c(y_c+R_e)}{r^5}& -\frac{\mathrm{\μ}}{r^3}(1-\frac{3z_c^2}{r^2})\end{array}\right]$

其中，μ为地心引力常数；r为载体到地心的距离；R_e为地球半径；x_c、y_c、z_c为载体在发射点坐标系的位置；

子步骤B中，系统量测方程建立的具体方法如下：

系统的量测方程分为两部分：姿态误差角量测和折射视高度量测；

姿态误差角量测表示如下:

$Z_1=C_i^c\left[\begin{array}{c}\mathrm{\Δ}\mathrm{\β}\\ \mathrm{\Δ}\mathrm{\θ}\\ \mathrm{\Δ}\mathrm{\α}\end{array}\right]=C_i^c\left[\begin{array}{c}{\mathrm{\β}}_I-{\mathrm{\β}}_X\\ {\mathrm{\θ}}_I-{\mathrm{\θ}}_X\\ {\mathrm{\α}}_I-{\mathrm{\α}}_X\end{array}\right]=H_{1}X+v_1$

其中，为惯性坐标系到发射点惯性坐标系的坐标变换矩阵，当发射点确定以后是一个常值矩阵；设发射点的经纬度分别为φ和γ，发射角为A，则为

$C_i^c=\left[\begin{array}{ccc}-\cos A\sin \mathrm{\γ}\cos \mathrm{\φ}-\sin A\sin \mathrm{\φ}& \cos \mathrm{\γ}\cos \mathrm{\φ}& \sin A\sin \mathrm{\γ}\cos \mathrm{\φ}-\cos A\sin \mathrm{\φ}\\ -\cos A\sin \mathrm{\γ}\sin \mathrm{\φ}+\sin A\cos \mathrm{\φ}& \cos \mathrm{\γ}\sin \mathrm{\φ}& \sin A\sin \mathrm{\γ}\sin \mathrm{\φ}+\cos A\cos \mathrm{\φ}\\ \cos A\cos \mathrm{\γ}& \sin \mathrm{\γ}& -\sin A\cos \mathrm{\γ}\end{array}\right];$

β_I、θ_I和α_I分别为由捷联惯导解算得到的横滚角、俯仰角和航向角；β_X、θ_X和α_X分别为由星敏感器输出的横滚角、俯仰角和航向角；[Δβ Δθ Δα]^T为姿态误差角；v₁为星敏感器的随机噪声；H₁＝[I_3×3 0_3×12]为姿态误差角量测的转移矩阵；

定义视高度为h_a，折射高度为h_g，R_e为地球半径；r为飞行器的位置矢量；u为位置矢量在恒星入射光线方向上的分量；R为星光折射角；

根据折射视高度与折射角之间的几何关系得：

$h_a=\sqrt{r^2-u^2}+u\tan R-R_e-a$

其中，

$\left\{\begin{array}{c}r=\left|r\right|=\sqrt{x^2+y^2+z^2}\\ u=|r\·u|=|{\mathrm{xs}}_x+{\mathrm{ys}}_y+{\mathrm{zs}}_z|\end{array}\right.,$

r＝[x y z]为载体在地心赤道惯性坐标系的位置矢量，u＝[s_x s_y s_z]^T为折射前的星光矢量,s_x、s_y、s_z为恒星在天球坐标系的方向矢量，星图识别成功后可通过查找星表得到；a为一个小量，通常忽略不计；根据大气密度模型得到星光折射角和视高度的关系：

h_ac＝57.081+2.531e^{[0.981ln(R)-8.689]}-6.441ln(R)

r和u中含有与地球位置相关的参数，因此h_a必定会受到捷联惯导噪声的影响而存在折射视高度误差；真实的视高度h_at＝h_ac+v_a；v_a是零均值高斯白噪声，且则折射视高度误差δh_a可以被表示为：

δh_a＝h_at-h_a＝h_ac-h_a+v_a

${\mathrm{\δh}}_a=\mathrm{\δ}(\sqrt{r^2-u^2}+u\tan R-R_e)=\frac{r\·\mathrm{\δ}r-u\·\mathrm{\δ}u}{\sqrt{r^2-u^2}}+\mathrm{\δ}u\·\tan R+\frac{u\·\mathrm{\δ}R}{1+R^2}$

载体在发射点惯性坐标系下的位置矢量为r_c，在惯性坐标系下的位置矢量r之间的关系为：

$r=C_c^i{r}_c+R_c$

其中，r_c＝[x_c y_c z_c]^T；R_c＝[R_cx R_cy R_cz]^T为发射点子午圈半径在地心惯性坐标系下的投影；且令

$C_c^i=\left[\begin{array}{ccc}{c}_{11}& c_{12}& c_{13}\\ c_{21}& c_{22}& c_{23}\\ c_{31}& c_{32}& c_{33}\end{array}\right]$

得载体位置矢量在惯性坐标系投影为

$\left\{\begin{array}{c}x=c_{11}{x}_c+c_{12}{y}_c+c_{13}{z}_c+R_{cx}\\ y=c_{21}{x}_c+c_{22}{y}_c+c_{23}{z}_c+R_{cy}\\ z=c_{31}{x}_c+c_{32}{y}_c+c_{33}{z}_c+R_{cz}\end{array}\right.$

求微分得到载体的在地心惯性坐标系各轴的位置误差为

$\left\{\begin{array}{c}\mathrm{\δ}x=c_{11}\mathrm{\δ}{x}_c+c_{12}\mathrm{\δ}{y}_c+c_{13}\mathrm{\δ}{z}_c\\ \mathrm{\δ}y=c_{21}{\mathrm{\δx}}_c+c_{22}{\mathrm{\δy}}_c+c_{23}{\mathrm{\δz}}_c\\ \mathrm{\δ}z=c_{31}{\mathrm{\δx}}_c+c_{32}{\mathrm{\δy}}_c+c_{33}{\mathrm{\δz}}_c\end{array}\right.$

可得δr、δu，

$\mathrm{\δ}ha=a_1\·{\mathrm{\δx}}_c+a_2\·{\mathrm{\δy}}_c+a_3\·{\mathrm{\δz}}_c+\frac{u\·\mathrm{\δ}R}{1+R^2}$

当u＜0时

$\begin{array}{ccc}\left\{\begin{array}{c}{u}_1=c_{11}x+c_{21}y+c_{31}z\\ u_2=c_{12}x+c_{22}y+c_{32}z\\ u_3=c_{13}x+c_{23}y+c_{33}z\end{array}\right.& \left\{\begin{array}{c}{\mathrm{du}}_1=c_{11}{s}_x+c_{21}{s}_y+c_{31}{s}_z\\ {\mathrm{du}}_2=c_{12}{s}_x+c_{22}{s}_y+c_{32}{s}_z\\ {\mathrm{du}}_3=c_{13}{s}_x+c_{23}{s}_y+c_{33}{s}_z\end{array}\right.& \left\{\begin{array}{c}{a}_1=\frac{u_1}{m}-(\frac{u}{m}-\tan R)\·{\mathrm{du}}_1\\ a_2=\frac{u_2}{m}-(\frac{u}{m}-\tan R)\·{\mathrm{du}}_2\\ a_3=\frac{u_3}{m}-(\frac{u}{m}-\tan R)\·{\mathrm{du}}_3\end{array}\right.\end{array}$

量测误差：

$\frac{u}{1+R^2}\·\mathrm{\δ}R=v_b$

δR为星敏感器的量测噪声，量测方程表示为：

z＝h_ac-h_a＝hX+v_b-v_a

v_b不是零均值高斯白噪声，设那么：

$z=hX+d\·\mathrm{\δ}R-v_a=hX+D\left[\begin{array}{c}\mathrm{\δ}R\\ v_a\end{array}\right]$

其中，D＝[d -1]：

$D^{-1}z=D^{-1}hX+\left[\begin{array}{c}\mathrm{\δ}R\\ v_a\end{array}\right]$

其中，D^-1＝D^T(DD^T)^-1为广义逆，定义z^*＝D^-1z，h^*＝D^-1h，v^*＝[δR v_a]^T，

z^*＝h^*X+v^*

$Z_2=\left[\begin{array}{c}{h}_1^{*}\\ h_2^{*}\\ .\\ .\\ .\end{array}\right]X+\left[\begin{array}{c}{v}_1^{*}\\ v_2^{*}\\ .\\ .\\ .\end{array}\right]=H_{2}X+v_2$

其中，下标1、2…代表折射星的标号，得到系统总的量测方程为：

$Z=\left[\begin{array}{c}{Z}_1\\ Z_2\end{array}\right]=\left[\begin{array}{c}{H}_1\\ H_2\end{array}\right]X+\left[\begin{array}{c}{v}_1\\ v_2\end{array}\right].$