CN101660914B - 耦合惯性位置误差的机载星光和惯性组合的自主导航方法 - Google Patents

Abstract

本发明公布了一种耦合惯性位置误差的机载星光和惯性组合的自主导航方法，该方法首先对航空机载地理系下导航中的姿态组合观测原理进行了理论分析，建立了地理系下姿态观测线性化量测方程；然后分析了星敏感器(星光)测量输出的惯性系下姿态到地理系下姿态之间的相互转换关系，通过引入惯性导航位置误差转换矩阵，建立了星敏感器测量输出的惯性系下姿态到惯性导航计算输出的地理系系下姿态之间的耦合误差模型；最后设计完成了基于耦合惯性位置误差建模思想的航空机载星光和惯性组合导航系统数学模型，采用KF滤波方法对惯性导航的误差状态进行了最优估计。本发明导航精度高，能充分发挥地理系下姿态量测方程对航空机载惯性导航系统误差状态量的估计作用。

Description

耦合惯性位置误差的机载星光和惯性组合的自主导航方法

技术领域

发明涉及一种耦合惯性位置误差的机载星光和惯性组合的自主导航方法，属于航空飞行器组合导航技术领域，可应用于高空长时间飞行的航空飞行器导航参数的确定，适用于高空长时间飞行的航空飞行器的导航定位。

背景技术

星敏感器作为一种高精度的天文姿态敏感器，越来越广泛应用于自主导航领域。星敏感器可以在不需要任何外部基准信息的前提下，直接精确测量获得飞行器相对于惯性坐标系下的姿态信息，且其测量精度在导航全程保持稳定，现有最高精度可以达到角秒级。

现阶段将星敏感器用于航空机载地理系导航领域的主要原理是利用其测量获得惯性系下的姿态，并直接和惯性导航基于惯性积分原理在惯性系下输出的姿态信息进行组合，从而可以精确补偿惯性导航系统中的陀螺漂移，以间接提高地理系下惯性导航系统的姿态和位置导航精度。但采用基于惯性系下姿态组合对惯性导航陀螺漂移误差进行补偿的原理有以下缺点：①系统误差量状态方程建立在惯性系下，仅能对相对于惯性系下定义的惯性导航系统的平台误差角状态量和陀螺漂移误差状态量进行估计，无法直接实现对地理系下所有的惯性导航误差状态量的估计；②为进行惯性系下姿态组合估计，必须维护一套独立于地理系下的惯性姿态组合算法，增加了系统实现的复杂性；③现有的姿态观测量和被估计的航空机载惯性导航系统误差状态量中的平台误差角状态量之间的线性化量测方程主要采用将姿态误差和平台误差角看为等价关系或将姿态测量误差转换为平台误差角的近似方式实现，会带来建模误差和转换误差。

因此，现有的航空机载星光和惯性组合的自主导航方法存在导航精度低，不能充分发挥地理系下姿态量测方程对航空机载惯性导航系统误差状态量的估计作用。

发明内容

本发明目的在于：克服在惯性系下进行姿态组合导航的不足，提供了一种耦合惯性位置误差的机载星光和惯性组合的自主导航方法。

本发明为实现上述目的，采用如下技术方案：

本发明耦合惯性位置误差的机载星光和惯性组合的自主导航方法，其特征在于包括以下步骤：

(1)通过建立航空机载惯性导航系统INS的误差状态量方程，得到了对航空机载惯性导航系统误差状态量的数学描述，航空机载惯性导航系统误差状态量X定义为：

$X={[{\mathrm{\φ}}_E,{\mathrm{\φ}}_N,{\mathrm{\φ}}_U,{\mathrm{\δv}}_E,{\mathrm{\δv}}_N,{\mathrm{\δv}}_U,\mathrm{\δL},\mathrm{\δ\λ},\mathrm{\δh},{\mathrm{\ϵ}}_{\mathrm{bx}},{\mathrm{\ϵ}}_{\mathrm{by}},{\mathrm{\ϵ}}_{\mathrm{bz}},{\mathrm{\ϵ}}_{\mathrm{rx}},{\mathrm{\ϵ}}_{\mathrm{ry}},{\mathrm{\ϵ}}_{\mathrm{rz}},{\▿}_x,{\▿}_y,{\▿}_z]}^T,$ φ_E，φ_N，φ_U分别表示航空机载惯性导航系统误差状态量中的东向平台误差角状态量、北向平台误差角状态量和天向平台误差角状态量；δv_E，δv_N，δv_U分别表示航空机载惯性导航系统误差状态量中的东向速度误差状态量、北向速度误差状态量和天向速度误差状态量；δL，δλ，δh分别表示航空机载惯性导航系统误差状态量中的纬度误差状态量、经度误差状态量和高度误差状态量；ε_bx，ε_by，ε_bz，ε_rx，ε_ry，ε_rz分别表示航空机载惯性导航系统误差状态量中的X轴、Y轴、Z轴方向陀螺常值漂移误差状态量和X轴、Y轴、Z轴方向陀螺一阶马尔可夫漂移误差状态量；

分别表示航空机载惯性导航系统误差状态量中的X轴、Y轴和Z轴方向加速度计零偏，T为转置；

(2)采用航空机载地理系下姿态线性化观测原理，建立航空机载地理系下姿态观测量和被估计的步骤(1)所述的航空机载惯性导航系统误差状态量中的平台误差角状态量之间的线性化量测方程；

(3)通过星敏感器测量输出的惯性系下姿态到步骤(2)所述的航空机载地理系下姿态之间的转换关系，建立航空机载地理系下的耦合了惯性导航位置误差的星敏感器姿态观测量和被估计的步骤(1)所述的航空机载惯性导航系统误差状态量中的平台误差角、纬度和经度误差状态量之间的线性化量测方程；

(4)将步骤(3)所述的平台误差角、纬度和经度误差状态量进行KF滤波，当KF滤波收敛，则估计出航空机载惯性导航系统的估计误差状态量用于修正导航误差；当KF滤波未收敛，则重新进行KF滤波。

本发明与现有技术相比克服在惯性系下进行姿态组合导航的不足，构建了一种适用于高空长时间飞行航空飞行器的地理系下星光和惯性组合的自主导航方法，它具有以下优点：(1)直接在地理系下实现星光和惯性的组合导航，避免了独立维护地理系下姿态组合算法的需要；(2)通过直接估计地理系下惯性导航系统的姿态和位置误差状态量，从而有效改善组合导航性能，充分发挥星敏感器姿态观测的作用。

附图说明

图1为本发明的机载星光和惯性组合的自主导航方法的一种实施例子的流程图；

图2为仿真的一条飞行航迹；

图3为本发明的导航横滚姿态误差与惯性导航系统导航横滚姿态误差的仿真对比图；

图4为本发明的导航俯仰姿态误差与惯性导航系统导航俯仰姿态误差的仿真对比图；

图5为本发明的导航航向姿态误差与惯性导航系统导航航向姿态误差的仿真对比图；

图6为本发明的导航纬度误差与惯性导航系统导航纬度误差的仿真对比图；

图7为本发明的导航经度误差与惯性导航系统导航经度误差的仿真对比图。

具体实施方式

下面结合附图对发明的技术方案进行详细说明：

如图1所示，本发明的原理是：从机载地理系导航的角度入手，建立了地理系下的姿态线性化量测方程，分析了星敏感器输出的惯性姿态到地理系下姿态之间的相互转换关系，建立了地理系下的耦合惯性导航位置误差的星敏感器姿态线性化量测方程，实现对惯性导航误差状态量的最优估计。具体实施方法如下：

一、建立航空机载惯性导航系统的误差状态量方程

选择导航坐标系为东北天地理水平坐标系(O_nX_nY_nZ_n)，采用线性卡尔曼滤波器进行组合，系统的状态方程为惯性导航系统的误差状态量方程，通过对惯性导航系统的性能及误差源的分析，可以获得惯导系统的误差状态量方程为：

$\stackrel{\·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)---\left(4\right)$

式中， $X={[{\mathrm{\φ}}_E,{\mathrm{\φ}}_N,{\mathrm{\φ}}_U,{\mathrm{\δv}}_E,{\mathrm{\δv}}_N,{\mathrm{\δv}}_U,\mathrm{\δL},\mathrm{\δ\λ},\mathrm{\δh},{\mathrm{\ϵ}}_{\mathrm{bx}},{\mathrm{\ϵ}}_{\mathrm{by}},{\mathrm{\ϵ}}_{\mathrm{bz}},{\mathrm{\ϵ}}_{\mathrm{rx}},{\mathrm{\ϵ}}_{\mathrm{ry}},{\mathrm{\ϵ}}_{\mathrm{rz}},{\▿}_x,{\▿}_y,{\▿}_z]}^T$

其中φ_E，φ_N，φ_U为平台误差角；δv_E，δv_N，δv_U为速度误差；δL，δλ，δh为纬度、经度和高度误差；ε_bx，ε_by，ε_bz，ε_rx，ε_ry，ε_rz分别为陀螺常值漂移误差和一阶马尔可夫漂移误差；为加速度计零偏。

二、建立航空机载地理系下姿态观测量和被估计的惯性导航系统误差状态量中的平台误差角状态量之间的线性化量测方程

1.航空机载地理系下姿态观测关系分析

根据航空机载地理导航坐标系下姿态角度定义，有导航坐标系下理想姿态转换矩阵为：

$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(5\right)$

式(5)中上标n表示导航坐标系，下标b表示载体本体坐标系。γ，θ，ψ分别表示航空飞行器相对于地理导航坐标系下的横滚姿态角度、俯仰姿态角度和航向姿态角度的真实值。

利用惯性导航计算输出姿态和惯性导航模拟的导航平台坐标系的平台误差角，也可以获得相对于导航坐标系下的理想姿态转换矩阵，其计算公式如下：

$C_b^n=C_{c1}^n\·C_b^{c1}=$

$\left[\begin{array}{ccc}1& -{\mathrm{\φ}}_U& {\mathrm{\φ}}_N\\ {\mathrm{\φ}}_U& 1& -{\mathrm{\φ}}_E\\ {-\mathrm{\φ}}_N& {\mathrm{\φ}}_E& 1\end{array}\right]\·\left[\begin{array}{ccc}{\cos \mathrm{\γ}}_I{\cos \mathrm{\ψ}}_I+{\sin \mathrm{\γ}}_I{\sin \mathrm{\θ}}_I{\sin \mathrm{\ψ}}_I& {\cos \mathrm{\θ}}_I{\sin \mathrm{\ψ}}_I& {\sin \mathrm{\γ}}_I{\cos \mathrm{\ψ}}_I-{\cos \mathrm{\γ}}_I{\sin \mathrm{\θ}}_I{\sin \mathrm{\ψ}}_I\\ {-\cos \mathrm{\γ}}_I{\sin \mathrm{\ψ}}_I+{\sin \mathrm{\γ}}_I{\sin \mathrm{\θ}}_I{\cos \mathrm{\ψ}}_I& {\cos \mathrm{\θ}}_I{\cos \mathrm{\ψ}}_I& {-\sin \mathrm{\γ}}_I{\sin \mathrm{\ψ}}_I-{\cos \mathrm{\γ}}_I{\sin \mathrm{\θ}}_I{\cos \mathrm{\ψ}}_I\\ {-\sin \mathrm{\γ}}_I{\cos \mathrm{\θ}}_I& {\sin \mathrm{\θ}}_I& {\cos \mathrm{\γ}}_I{\cos \mathrm{\θ}}_I\end{array}\right]---\left(6\right)$

式(6)中上标c1表示通过惯性导航计算获得的导航平台坐标系。则由公式(5)和公式(6)可以获得如下的计算等式：

$\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(7\right)$

$\left[\begin{array}{ccc}1& -{\mathrm{\φ}}_U& {\mathrm{\φ}}_N\\ {\mathrm{\φ}}_U& 1& -{\mathrm{\φ}}_E\\ {-\mathrm{\φ}}_N& {\mathrm{\φ}}_E& 1\end{array}\right]\·\left[\begin{array}{ccc}{\cos \mathrm{\γ}}_I{\cos \mathrm{\ψ}}_I+{\sin \mathrm{\γ}}_I{\sin \mathrm{\θ}}_I{\sin \mathrm{\ψ}}_I& {\cos \mathrm{\θ}}_I{\sin \mathrm{\ψ}}_I& {\sin \mathrm{\γ}}_I{\cos \mathrm{\ψ}}_I-{\cos \mathrm{\γ}}_I{\sin \mathrm{\θ}}_I{\sin \mathrm{\ψ}}_I\\ {-\cos \mathrm{\γ}}_I{\sin \mathrm{\ψ}}_I+{\sin \mathrm{\γ}}_I{\sin \mathrm{\θ}}_I{\cos \mathrm{\ψ}}_I& {\cos \mathrm{\θ}}_I{\cos \mathrm{\ψ}}_I& {-\sin \mathrm{\γ}}_I{\sin \mathrm{\ψ}}_I-{\cos \mathrm{\γ}}_I{\sin \mathrm{\θ}}_I{\cos \mathrm{\ψ}}_I\\ {-\sin \mathrm{\γ}}_I{\cos \mathrm{\θ}}_I& {\sin \mathrm{\θ}}_I& {\cos \mathrm{\γ}}_I{\cos \mathrm{\θ}}_I\end{array}\right]$

从式(7)可以看出，其反映了惯性导航计算输出姿态角度、平台误差角和真实姿态角度之间的相互关系，则可以利用上述等式关系获得组合观测量Δγ，Δθ，Δψ的计算表达式。但由于其为非线性方程，而为采用线性卡尔曼滤波器，还必须直接获得组合系统的线性化量测方程，则必须利用上述关系求解出组合观测量Δγ，Δθ，Δψ，并建立其和惯性导航系统误差状态量之间的线性量测关系。

2.线性化角度观测方程建立

利用在小角度姿态偏差情况下，Δγ，Δθ，Δψ为小量的假设以及三角函数关系，则有：

$\left.\begin{array}{c}{\sin \mathrm{\γ}}_T=\sin ({\mathrm{\γ}}_I+\mathrm{\Δ\γ})\≈{\sin \mathrm{\γ}}_I+{\mathrm{\Δ\γ}\cos \mathrm{\γ}}_I\\ {\sin \mathrm{\θ}}_T=\sin ({\mathrm{\θ}}_I+\mathrm{\Δ\θ})\≈{\sin \mathrm{\θ}}_I+{\mathrm{\Δ\θ}\cos \mathrm{\θ}}_I\\ {\sin \mathrm{\ψ}}_T=\sin ({\mathrm{\ψ}}_I+\mathrm{\Δ\ψ})\≈{\sin \mathrm{\ψ}}_I+{\mathrm{\Δ\ψ}\cos \mathrm{\ψ}}_I\\ {\cos \mathrm{\θ}}_T=\cos ({\mathrm{\θ}}_I+\mathrm{\Δ\θ})\≈{\cos \mathrm{\θ}}_I-{\mathrm{\Δ\θ}\sin \mathrm{\θ}}_I\end{array}\right\}---\left(8\right)$

式(8)中，γ_T，θ_T，ψ_T分别表示为由姿态敏感器直接测量获得的航空机载地理系下的横滚姿态角度、俯仰姿态角度和航向姿态角度；此外，对公式(6)中的C_b ^c1可做如下简化表达：

$\left[\begin{array}{ccc}{\cos \mathrm{\γ}}_I{\cos \mathrm{\ψ}}_I+{\sin \mathrm{\γ}}_I{\sin \mathrm{\θ}}_I{\sin \mathrm{\ψ}}_I& {\cos \mathrm{\θ}}_I{\sin \mathrm{\ψ}}_I& {\sin \mathrm{\γ}}_I{\cos \mathrm{\ψ}}_I-{\cos \mathrm{\γ}}_I{\sin \mathrm{\θ}}_I{\sin \mathrm{\ψ}}_I\\ {-\cos \mathrm{\γ}}_I{\sin \mathrm{\ψ}}_I+{\sin \mathrm{\γ}}_I{\sin \mathrm{\θ}}_I{\cos \mathrm{\ψ}}_I& {\cos \mathrm{\θ}}_I{\cos \mathrm{\ψ}}_I& {-\sin \mathrm{\γ}}_I{\sin \mathrm{\ψ}}_I-{\cos \mathrm{\γ}}_I{\sin \mathrm{\θ}}_I{\cos \mathrm{\ψ}}_I\\ {-\sin \mathrm{\γ}}_I{\cos \mathrm{\θ}}_I& {\sin \mathrm{\θ}}_I& {\cos \mathrm{\γ}}_I{\cos \mathrm{\θ}}_I\end{array}\right]=---\left(9\right)$

$\left[\begin{array}{ccc}{C}_{11}^{\′}& {\cos \mathrm{\θ}}_I{\sin \mathrm{\ψ}}_I& C_{13}^{\′}\\ C_{21}^{\′}& {\cos \mathrm{\θ}}_I{\cos \mathrm{\ψ}}_I& C_{23}^{\′}\\ {-\sin \mathrm{\γ}}_I{\cos \mathrm{\θ}}_I& {\sin \mathrm{\θ}}_I& {\cos \mathrm{\γ}}_I{\cos \mathrm{\θ}}_I\end{array}\right]$

则由公式(9)，并结合公式(7)可以获得：

$\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}1& {-\mathrm{\φ}}_U& {\mathrm{\φ}}_N\\ {\mathrm{\φ}}_U& 1& {-\mathrm{\φ}}_E\\ {-\mathrm{\φ}}_N& {\mathrm{\φ}}_E& 1\end{array}\right]\·\left[\begin{array}{ccc}{C}_{11}^{\′}& {\cos \mathrm{\θ}}_I{\sin \mathrm{\ψ}}_I& C_{13}^{\′}\\ C_{21}^{\′}& {\cos \mathrm{\θ}}_I{\cos \mathrm{\ψ}}_I& C_{23}^{\′}\\ {-\sin \mathrm{\γ}}_I{\cos \mathrm{\θ}}_I& {\sin \mathrm{\θ}}_I& {\cos \mathrm{\γ}}_I{\cos \mathrm{\θ}}_I\end{array}\right]=---\left(10\right)$

$\left[\begin{array}{ccc}...& \cos {\mathrm{\θ}}_I{\sin \mathrm{\ψ}}_I-{\mathrm{\φ}}_U{\cos \mathrm{\θ}}_I{\cos \mathrm{\ψ}}_I+{\mathrm{\φ}}_N{\sin \mathrm{\θ}}_I& ...\\ ...& ...& ...\\ {-\mathrm{\φ}}_N{C}_{11}^{\′}+{\mathrm{\φ}}_E{C}_{21}^{\′}-{\sin \mathrm{\γ}}_I\cos {\mathrm{\θ}}_I& {-\mathrm{\φ}}_N{\cos \mathrm{\θ}}_I{\sin \mathrm{\ψ}}_I+{\mathrm{\φ}}_E{\cos \mathrm{\θ}}_I{\cos \mathrm{\ψ}}_I+{\sin \mathrm{\θ}}_I& ...\end{array}\right]$

则利用式(10)中矩阵对应元素相等的关系，即可获得姿态角度观测量Δγ，Δθ，Δψ。由于只需要利用式(10)中的三个对应矩阵元素即可实现对姿态角度观测量的求解，因此对式(10)中等式右边其他矩阵元素不再进行计算，并用“...”表示。

则有如下的矩阵元素对应关系：

sinθ_T≈sinθ＝-φ_Ncosθ_Isinψ_I+φ_Ecosθ_Icosψ_I+sinθ_I

cosθ_Tsinψ_T≈cosθsinψ＝cosθ_Isinψ_I-φ_Ucosθ_Icosψ₁+φ_Nsinθ_I         (11)

-sinγ_Tcosθ_T≈-sinγcosθ＝-φ_NC′₁₁+φ_EC′₂₁-sinγ_Icosθ_I

则将公式(8)代入公式(11)，经过简化，可以获得最终的姿态角度线性化量测方程为：

Δθ＝φ_Ecosψ_I-φ_Nsinψ_I

Δψ＝φ_Esinψ_Isinθ_I/cosθ_T+φ_Nsinθ_Icosψ_I/cosθ_T-φ_Ucosθ_I/cosθ_T      (12)

Δγ＝φ_E(C′₂₁-sinγ_Isinθ_Icosψ_I)/(-cosγ_I cosθ_T)+φ_N(sinψ_I sinγ_Isinθ_I-C′₁₁)/(-cosγ_Icosθ_T)

三、建立航空机载地理系下的耦合了惯性导航位置误差的星敏感器姿态观测量和被估计的航空机载惯性导航系统误差状态中的平台误差角、纬度和经度误差状态量之间的线性化量测方程

3.星敏感器地理系下姿态测量原理

由1.～2.分析可以看出，如果能直接获得地理系下的姿态测量信息，则可以利用公式(12)实现地理系下的姿态组合滤波。但由于星敏感器测量输出的是航空飞行器相对于惯性系下的姿态输出，因此，无法直接采用上面分析给出的姿态测量方程。为此，本节将在分析惯性姿态到地理系姿态之间相互转换的关系基础上，直接建立地理系下的星光姿态测量模型。

利用星敏感器可以直接测量获得航空飞行器相对于惯性系下的姿态转换矩阵C_b ⁱ，上标i表示惯性坐标系，下标b表示载体本体坐标系，下面重点讨论如何由C_b ⁱ获得载体相对于地理导航坐标系下的姿态变换矩阵C_b ⁿ，上标n表示东北天地理导航坐标系，下标b表示载体本体坐标系。

考虑到由格林尼治恒星时角可获得惯性坐标系到地球固连坐标系的变换矩阵为C_i ^e，上标e表示地球固连坐标系；同时有地球固连坐标系到地理坐标系的变换矩阵为C_e ⁿ。

则结合上述矩阵相互转换关系，可以获得航空飞行器惯性姿态转换矩阵相对于导航坐标系下的姿态转换矩阵之间的关系，有：

$C_b^n{=C_e^{n}C}_i^e{C}_b^i=\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(13\right)$

式(13)中有如下定义：

$C_e^n=\left[\begin{array}{ccc}-\sin \mathrm{\λ}& \cos \mathrm{\λ}& 0\\ -\cos \mathrm{\λ}\sin L& -\sin \mathrm{\λ}\sin L& \cos L\\ \cos \mathrm{\λ}\cos L& \sin \mathrm{\λ}\cos L& \sin L\end{array}\right]$

则利用式(13)即可求出航空飞行器相对于地理导航坐标系下的横滚姿态角度、俯仰姿态角度和航向姿态角度的真实值，计算表达式为， $\left\{\begin{array}{c}\mathrm{\γ}={\mathrm{tg}}^{-1}(-\frac{C_{\mathrm{3,1}}}{C_{\mathrm{3,3}}})\\ \mathrm{\θ}{=\sin }^{-1}\left(C_{\mathrm{3,2}}\right)\\ \mathrm{\ψ}={\mathrm{tg}}^{-1}(C_{\mathrm{1,2}}/C_{\mathrm{2,2}})\end{array}\right.,$ 其中C_i，j表示矩阵C_b ⁿ中对应的第i行和第j列的元素。

4.基于位置耦合误差建模的星光姿态观测方程

从3.中的分析可以看出，如果要将星敏感器输出的惯性姿态转换为地理系姿态，就必须提供航空飞行器在星敏感器观测时刻的地理经度和纬度位置信息λ，L的真值，而在航空机载星光和惯性组合环境下，唯一能够提供位置信息的，就只有惯性导航输出的计算地理经度和纬度信息，但由于惯性导航受到陀螺和加速度计漂移误差的影响，其导航精度随时间而发散。因此，利用惯性导航输出所提供的计算地理经纬度位置信息λ_S，L_S必然不等同于地理经纬度位置信息λ，L的真值。因此，要直接实现地理系下的姿态组合，则必须建立耦合惯性导航位置误差的组合量测方程。

设惯性导航输出的地理经纬度信息分别为λ_S，L_S，则可以构成计算地理坐标系c2系，其与导航坐标系n系之间考虑有小角度经纬度位置偏差δλ，δL时，则有如下计算地理坐标系到导航坐标系之间的坐标转换矩阵：

$C_{c2}^n=\left[\begin{array}{ccc}1& -\mathrm{\δ\λ}\sin L_S& \mathrm{\δ\λ}{\cos L}_S\\ \mathrm{\δ\λ}{\sin L}_S& 1& \mathrm{\δL}\\ -\mathrm{\δ\λ}{\cos L}_S& -\mathrm{\δL}& 1\end{array}\right]---\left(14\right)$

从而有耦合了惯性导航位置误差的星敏感器在地理系下的姿态测量输出为：

$C_b^{c2}=C_n^{c2}{C}_e^n{C}_i^e{C}_b^i=C_n^{c2}{C}_b^n=$

$\left[\begin{array}{ccc}\cos {\mathrm{\γ}}_S\cos {\mathrm{\ψ}}_S+\sin {\mathrm{\γ}}_S\sin {\mathrm{\θ}}_S\sin {\mathrm{\ψ}}_S& \cos {\mathrm{\θ}}_S\sin {\mathrm{\ψ}}_S& \sin {\mathrm{\γ}}_S\cos {\mathrm{\ψ}}_S-\cos {\mathrm{\γ}}_S\sin {\mathrm{\θ}}_S\sin {\mathrm{\ψ}}_S\\ -\cos {\mathrm{\γ}}_S\sin {\mathrm{\ψ}}_S+\sin {\mathrm{\γ}}_S\sin {\mathrm{\θ}}_S\cos {\mathrm{\ψ}}_S& \cos {\mathrm{\θ}}_S\mathrm{\θ}\cos {\mathrm{\ψ}}_S& -\sin {\mathrm{\γ}}_S\sin {\mathrm{\ψ}}_S-\cos {\mathrm{\γ}}_S\sin {\mathrm{\θ}}_S\cos {\mathrm{\ψ}}_S\\ -\sin {\mathrm{\γ}}_S\cos {\mathrm{\θ}}_S& \sin {\mathrm{\θ}}_S& \cos {\mathrm{\γ}}_S\cos {\mathrm{\θ}}_S\end{array}\right]---\left(15\right)$

利用式(15)即可求得星敏感器在地理系下的横滚姿态角度、俯仰姿态角度和航向姿态角度的测量输出，计算表达式为， $\left\{\begin{array}{c}{\mathrm{\γ}}_S={\mathrm{tg}}^{-1}(-\frac{C_{\mathrm{3,1}}}{C_{\mathrm{3,3}}})\\ {\mathrm{\θ}}_S={\sin }^{-1}\left(C_{\mathrm{3,2}}\right)\\ {\mathrm{\ψ}}_S={\mathrm{tg}}^{-1}(C_{\mathrm{1,2}}/C_{\mathrm{2,2}})\end{array}\right.,$ 其中C_i，j表示矩阵C_b ^c2中对应的第i行和第j列的元素；可见星敏在由惯性姿态向地理系姿态转换过程中，主要由于惯性导航输出的位置误差引起了转换后的地理系姿态测量误差。此外，由式(15)还可以获得

$C_b^n=C_{c2}^n{C}_b^{c2}---\left(16\right)$

设星敏感器转换为相对于地理系下横滚、俯仰和航向姿态测量输出分别为γ_S，θ_S，ψ_S，从而与地理系下姿态观测原理推导类似，有如下的姿态观测关系：

$C_b^n=C_{c1}^n\·C_b^{c1}=C_{c2}^n\·C_b^{c2}---\left(17\right)$

由公式(17)可以看出，该姿态观测方程中已经耦合进入了惯性导航的位置误差，采用与二类似的推导思路，则有如下耦合位置误差的姿态角度观测关系如下：

-δλcosL_Scosθ_Ssinψ_S-δLcosθ_Scosψ_S+sinθ_S＝-φ_Ncosθ_Isinψ_I+φ_Ecosθ_Icosψ_I+sinθ_I

cosθ_Ssinψ_S-δλsinL_Scosθ_Scosψ_S+δλcosL_Ssinθ_S＝cosθ_Isinψ_I-φ_Ucosθ_Icosψ_I+φ_Nsinθ_I   (18)

-δλcosL_SC₁₁-δLC₂₁-sinγ_Scosθ_S＝-φ_NC′₁₁+φ_EC₂₁-sinγ_Icosθ_I

式(18)中C₁₁，C₂₁表示的是矩阵C_b ^c2中的对应元素，和式(9)中的简化定义表示思路一样，从而有最终的耦合位置误差的姿态线性化观测方程为：

Δθ＝φ_Ecosψ_I-φ_Nsinψ_I+δLcosθ_S cosψ_S/cosθ_I+δλcosL_Scosθ_Ssinψ_S/cosθ_I

Δψ＝φ_Esinψ_Isinθ_I/cosθ_S+φ_Nsinθ_Icosψ_I/cosθ_S-φ_Ucosθ_I/cosθ_S+

δLcosψ_S sinψ_Isinθ_I/(cosθ_Icosψ_I)+

δλ(sinL_Scosθ_Scosψ_S-cosL_Ssinθ_S+cosL_Scosθ_Ssinψ_Ssinψ_Isinθ_I/cosθ_I)/(cosθ_Scosψ_I)  (19)

Δγ＝φ_E(C′₂₁-sinγ_Isinθ_Icosψ_I)/(-cosγ_Icosθ_S)+φ_N(sinψ_Isinγ_Isinθ_I-C′₁₁)/(-cosγ_Icosθ_S)+

δL(C₂₁-cosθ_Scosψ_Ssinγ_Isinθ_I/cosθ_I)/(-cosγ_I cosθ_S)+

δλcosL_S(C₁₁-cosθ_Ssinψ_Ssinγ_Isinθ_I/cosθ_I)/(-cosγ_Icosθ_S)

式(19)即为耦合位置误差的地理系下星光和惯性组合姿态线性化量测方程，基于此方程，即可设计线性化卡尔曼滤波器，实现对惯性导航误差的最优估计和修正。

5.基于位置耦合误差建模的机载星光和惯性的组合导航系统线性化量测方程

由4.分析可以看出，在组合系统中，量测值应选择为：星敏感器转换为地理系下的姿态与惯性导航系统计算出的地理坐标系下的姿态的差值构成，如式(3)定义。对公式(19)进行简化表示，则有：

Δθ＝φ_Eh_θ1+θ_Nh_θ2+δLh_θ3+δλh_θ4

Δψ＝φ_Eh_ψ1+φ_Nh_ψ2+φ_Uh_ψ3+δLh_ψ4+δλh_ψ5                   (20)

Δγ＝φ_Eh_r1+φ_Nh_r2+δLh_r3+δλh_r4

由(20)式，可以获得星光和惯性的姿态组合线性化量测方程为：

式(21)中v₁，v₂，v₃分别为对应的星敏感器输出的地理系下横滚、俯仰和航向姿态角度测量误差。

此外，考虑到姿态观测方程式(21)中高度通道不可观，因此，增加了气压高度表观测方程如下：

式中h_I，h_e分别为惯性导航和气压高度表输出的高度值；v₄为气压高度表输出时的高度误差。

则由上述式(21)和(22)可以获得机载星光和惯性的组合导航系统的量测方程Z(t)＝H(t)X(t)+V(t)，其具体形式如式(23)：

四、进行KF(Kalman Filter)滤波，估计航空机载惯性导航系统的误差状态量

6.状态方程和量测方程的离散化及卡尔曼滤波器

当采用线性卡尔曼滤波器时，需要对上面连续形式的系统状态方程(4)和量测方程(23)进行离散化，从而获得离散形式的系统方程。其离散化形式如下：

$\left\{\begin{array}{c}{X}_k={\mathrm{\Φ}}_{k,k-1}{X}_{k-1}+{\mathrm{\Γ}}_{k-1}{W}_{k-1}\\ Z_k=H_k{X}_k+V_k\end{array}\right.$

式中 ${\mathrm{\Φ}}_{k,k-1}=\underset{m=0}{\overset{\∞}{\mathrm{\Σ}}}{\left[F\left(t_k\right)T\right]}^m/m!,$ ${\mathrm{\Γ}}_{k-1}=\left\{\underset{m=1}{\overset{\∞}{\mathrm{\Σ}}}\left[\frac{1}{m!}{\left(F\left(t_k\right)T\right)}^{m-1}\right]\right\}G\left(t_k\right)T,$ T为迭代周期。

从而可以获得系统的线性化卡尔曼滤波器方程如下：

${\hat{X}}_{k|k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}$

$\widehat{X_k}={\hat{X}}_{k|k-1}+K_k[Z_k-H_k{\hat{X}}_{k|k-1}]$

P_k|k-1＝Ф_k，k-1P_k-1Ф_k，k-1 ^T+Γ_k-1Q_k-1Γ_k-1 ^T

$K_k=P_{k|k-1}{H}_k^T{(H_k{P}_{k|k-1}{H}_k^T+R_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$

上式中的Q，R分别为系统的噪声方差矩阵和量测方差矩阵。

图2为采用本发明方法仿真的一条飞行航迹。

图3～图7的仿真结果表明，该方法不仅能实现对惯性导航系统姿态误差的滤波估计，还能够实现对位置误差的估计，能克服惯性导航系统姿态和位置误差随时间发散的趋势，可以取得比惯性系下姿态组合更好的滤波精度。

Claims (3)

1.一种耦合惯性位置误差的机载星光和惯性组合的自主导航方法，其特征在于包括以下步骤：

(1)通过建立航空机载惯性导航系统INS的误差状态量方程，得到了对航空机载惯性导航系统误差状态量的数学描述，航空机载惯性导航系统误差状态量X定义为：

φ_E，φ_N，φ_U分别表示航空机载惯性导航系统误差状态量中的东向平台误差角状态量、北向平台误差角状态量和天向平台误差角状态量；δv_E，δv_N，δv_U分别表示航空机载惯性导航系统误差状态量中的东向速度误差状态量、北向速度误差状态量和天向速度误差状态量；δL，δλ，δh分别表示航空机载惯性导航系统误差状态量中的纬度误差状态量、经度误差状态量和高度误差状态量；ε_bx，ε_by，ε_bz，ε_rx，ε_ry，ε_rz分别表示航空机载惯性导航系统误差状态量中的X轴、Y轴、Z轴方向陀螺常值漂移误差状态量和X轴、Y轴、Z轴方向陀螺一阶马尔可夫漂移误差状态量； 

分别表示航空机载惯性导航系统误差状态量中的X轴、Y轴和Z轴方向加速度计零偏，T为转置；

(2)采用航空机载地理系下姿态线性化观测原理，建立航空机载地理系下姿态观测量和被估计的步骤(1)所述的航空机载惯性导航系统误差状态量中的平台误差角状态量之间的线性化量测方程；

(3)通过星敏感器测量输出的惯性系下姿态到步骤(2)所述的航空机载地理系下姿态之间的转换关系，建立航空机载地理系下的耦合了惯性导航位置误差的星敏感器姿态观测量和被估计的步骤(1)所述的航空机载惯性导航系统误差状态量中的平台误差角、纬度和经度误差状态量之间的线性化量测方程；

(4)将步骤(3)所述的平台误差角、纬度和经度误差状态量进行KF滤波，当KF滤波收敛，则估计出航空机载惯性导航系统的估计误差状态量用于修正导航误差；当KF滤波未收敛，则重新进行KF滤波。

2.根据权利要求1所述的耦合惯性位置误差的机载星光和惯性组合的自主导航方法，其特征在于：步骤(2)中所述的航空机载地理系下姿态观测量和被估计的步骤(1)所述的航空机载惯性导航系统误差状态量中的平台误差角状态量之间的线性化量测方程，表达式如下：

Δθ＝φ_Ecosψ_I-φ_Nsinψ_I

Δψ＝φ_Esinψ_Isinθ_I/cosθ_T+φ_Nsinθ_Icosψ_I/cosθ_T-φ_Ucosθ_I/cosθ_T

(1)

Δγ＝φ_E(-cosγ_Isinψ_I+sinγ_Isinθ_Icosψ_I-sinγ_Isinθ_Icosψ_I)/(-cosγ_Icosθ_T)+

φ_N(sinψ_Isinγ_Isinθ_I-cosγ_Icosψ_I-sinγ_Isinθ_Isinψ_I)/(-cosγ_Icosθ_T)

式(1)中，Δγ，Δθ，Δψ分别为横滚姿态角度量测量、俯仰姿态角度量测量和航向姿态角度量测量；φ_E，φ_N，φ_U分别表示航空机载惯性导航系统误差状态量中的东向平台误差角状态量、北向平台误差角状态量和天向平台误差角状态量；θ_T表示由姿态敏感器直接测量获得的航空机载地理系下的俯仰姿态角度；γ_I，θ_I，ψ_I分别表示为由惯性导航计算输出的地理系下的横滚姿态角度、俯仰姿态角度和航向姿态角度，且有如下定义：

，γ_T，ψ_I分别表示为由姿态敏感器直接测量获得的航空机载地理系下的横滚姿态角度和航向姿态角度。

3.根据权利要求1所述的耦合惯性位置误差的机载星光和惯性组合的自主导航方法，其特征在于：步骤(3)中所述的航空机载地理系下的耦合了惯性导航位置误差的星敏感器姿态观测量和被估计的步骤(1)所述的航空机载惯性导航系统误差状态量中的平台误差角、纬度和经度误差状态量之间的线性化量测方程，表达式如下：

Δθ＝φ_Ecosψ_I-φ_Nsinψ_I+δLcosθ_Scosψ_s/cosθ_I+δλcosL_Scosθ_Ssinψ_s/cosθ_I

Δψ＝φ_Esinψ_Isinθ_I/cosθ_S+φ_Nsinθ_Icosψ_I/cosθ_S-φ_Ucosθ_I/cosθ_S+

            δLcosψ_Ssinψ_Isinθ_I/(cosθ_Icosψ_I)+

            δλ(sinL_Scosθ_Scosψ_S-cosL_Ssinθ_S+cosL_Scosθ_Ssinψ_Ssinψ_I sinθ_I/cosθ_I)/(cosθ_Scosψ_I)，(2)

Δγ＝φ_E(-cosγ_Isinψ_I+sinγ_Isinθ_Icosψ_I-sinγ_Isinθ_Icosψ_I)/(-cosγ_Icosθ_S)+

           φ_N(sinψ_Isinγ_Isinθ_I-cosγ_Icosψ_I-sinγ_Isinθ_Isinψ_I)/(-cosγ_Icosθ_S)+

           δL(-cosγ_Ssinψ_S+sinγ_Ssinθ_Scosψ_S-cosθ_Scosψ_Ssinγ_Isinθ_I/cosθ_I)/(-cosγ_I cosθ_S)+

           δλcosL_S(cosγ_Scosψ_S+sinγ_Ssinθ_Ssinψ_S-cosθ_Ssinψ_Ssinγ_Isinθ_I/cosθ_I)/(-cosγ_Icosθ_S)

式(2)中，Δγ，Δθ，Δψ分别为横滚姿态角度量测量、俯仰姿态角度量测量和航向姿态角度量测量；φ_E，φ_N，φ_U分别表示航空机载惯性导航系统误差状态量中的东向平台误差角状态量、北向平台误差角状态量和天向平台误差角状态量；θ_S表示由星敏感器直接测量获得的惯性系下姿态转换矩阵 

变换到地理坐标系下的姿态转换矩阵 

后，通过地理坐标系下的姿态转换矩阵 

间接测量获得的航空机载地理系下的俯仰姿态角度，其中上标i表示惯性坐标系，下标b表示载体本体坐标系，上标c2表示由惯性导航计算输出的地理经纬度信息构成的计算地理坐标系；γ_I，θ_I，ψ_I分别表示为由惯性导航计算输出的地理系下的横滚姿态角度、俯仰姿态角度和航向姿态角度，且有如下定义：

式(3)中，γ_S，ψ_S分别表示为由星敏感器直接测量获得的惯性系下姿态转换矩阵 

变换到地理坐标系下的姿态转换矩阵 

后，通过 间接测量获得的航空机载地理系下的横滚姿态角度和航向姿态角度；δλ，δL分别表示航空机载惯性导航系统误差状态量中的经度误差状态量和纬度误差状态量；L_S表示为由惯性导航计算输出的地理纬度。 

Images