耦合惯性位置误差的机载星光和惯性组合的自主导航方法 【技术领域】
发明涉及一种耦合惯性位置误差的机载星光和惯性组合的自主导航方法，属于航空飞行器组合导航技术领域，可应用于高空长时间飞行的航空飞行器导航参数的确定，适用于高空长时间飞行的航空飞行器的导航定位。
背景技术
星敏感器作为一种高精度的天文姿态敏感器，越来越广泛应用于自主导航领域。星敏感器可以在不需要任何外部基准信息的前提下，直接精确测量获得飞行器相对于惯性坐标系下的姿态信息，且其测量精度在导航全程保持稳定，现有最高精度可以达到角秒级。
现阶段将星敏感器用于航空机载地理系导航领域的主要原理是利用其测量获得惯性系下的姿态，并直接和惯性导航基于惯性积分原理在惯性系下输出的姿态信息进行组合，从而可以精确补偿惯性导航系统中的陀螺漂移，以间接提高地理系下惯性导航系统的姿态和位置导航精度。但采用基于惯性系下姿态组合对惯性导航陀螺漂移误差进行补偿的原理有以下缺点：①系统误差量状态方程建立在惯性系下，仅能对相对于惯性系下定义的惯性导航系统的平台误差角状态量和陀螺漂移误差状态量进行估计，无法直接实现对地理系下所有的惯性导航误差状态量的估计；②为进行惯性系下姿态组合估计，必须维护一套独立于地理系下的惯性姿态组合算法，增加了系统实现的复杂性；③现有的姿态观测量和被估计的航空机载惯性导航系统误差状态量中的平台误差角状态量之间的线性化量测方程主要采用将姿态误差和平台误差角看为等价关系或将姿态测量误差转换为平台误差角的近似方式实现，会带来建模误差和转换误差。
因此，现有的航空机载星光和惯性组合的自主导航方法存在导航精度低，不能充分发挥地理系下姿态量测方程对航空机载惯性导航系统误差状态量的估计作用。
【发明内容】
本发明目的在于：克服在惯性系下进行姿态组合导航的不足，提供了一种耦合惯性位置误差的机载星光和惯性组合的自主导航方法。
本发明为实现上述目的，采用如下技术方案：
本发明耦合惯性位置误差的机载星光和惯性组合的自主导航方法，其特征在于包括以下步骤：
(1)通过建立航空机载惯性导航系统INS的误差状态量方程，得到了对航空机载惯性导航系统误差状态量的数学描述，航空机载惯性导航系统误差状态量X定义为：
X=[φE,φN,φU,δvE,δvN,δvU,δL,δλ,δh,ϵbx,ϵby,ϵbz,ϵrx,ϵry,ϵrz,▿x,▿y,▿z]T,]]>φE，φN，φU分别表示航空机载惯性导航系统误差状态量中的东向平台误差角状态量、北向平台误差角状态量和天向平台误差角状态量；δvE，δvN，δvU分别表示航空机载惯性导航系统误差状态量中的东向速度误差状态量、北向速度误差状态量和天向速度误差状态量；δ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所示，本发明的原理是：从机载地理系导航的角度入手，建立了地理系下的姿态线性化量测方程，分析了星敏感器输出的惯性姿态到地理系下姿态之间的相互转换关系，建立了地理系下的耦合惯性导航位置误差的星敏感器姿态线性化量测方程，实现对惯性导航误差状态量的最优估计。具体实施方法如下：
一、建立航空机载惯性导航系统的误差状态量方程
选择导航坐标系为东北天地理水平坐标系(OnXnYnZn)，采用线性卡尔曼滤波器进行组合，系统的状态方程为惯性导航系统的误差状态量方程，通过对惯性导航系统的性能及误差源的分析，可以获得惯导系统的误差状态量方程为：
X·(t)=F(t)X(t)+G(t)W(t)---(4)]]>
式中，X=[φE,φN,φU,δvE,δvN,δvU,δL,δλ,δh,ϵbx,ϵby,ϵbz,ϵrx,ϵry,ϵrz,▿x,▿y,▿z]T]]>
其中φE，φN，φU为平台误差角；δvE，δvN，δvU为速度误差；δL，δλ，δh为纬度、经度和高度误差；εbx，εby，εbz，εrx，εry，εrz分别为陀螺常值漂移误差和一阶马尔可夫漂移误差；为加速度计零偏。
二、建立航空机载地理系下姿态观测量和被估计的惯性导航系统误差状态量中的平台误差角状态量之间的线性化量测方程
1.航空机载地理系下姿态观测关系分析
根据航空机载地理导航坐标系下姿态角度定义，有导航坐标系下理想姿态转换矩阵为：
Cbn=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θ---(5)]]>
式(5)中上标n表示导航坐标系，下标b表示载体本体坐标系。γ，θ，ψ分别表示航空飞行器相对于地理导航坐标系下的横滚姿态角度、俯仰姿态角度和航向姿态角度的真实值。
利用惯性导航计算输出姿态和惯性导航模拟的导航平台坐标系的平台误差角，也可以获得相对于导航坐标系下的理想姿态转换矩阵，其计算公式如下：
Cbn=Cc1n·Cbc1=]]>
1-φUφNφU1-φE-φNφE1·cosγIcosψI+sinγIsinθIsinψIcosθIsinψIsinγIcosψI-cosγIsinθIsinψI-cosγIsinψI+sinγIsinθIcosψIcosθIcosψI-sinγIsinψI-cosγIsinθIcosψI-sinγIcosθIsinθIcosγIcosθI---(6)]]>
式(6)中上标c1表示通过惯性导航计算获得的导航平台坐标系。则由公式(5)和公式(6)可以获得如下的计算等式：
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θ=---(7)]]>
1-φUφNφU1-φE-φNφE1·cosγIcosψI+sinγIsinθIsinψIcosθIsinψIsinγIcosψI-cosγIsinθIsinψI-cosγIsinψI+sinγIsinθIcosψIcosθIcosψI-sinγIsinψI-cosγIsinθIcosψI-sinγIcosθIsinθIcosγIcosθI]]>
从式(7)可以看出，其反映了惯性导航计算输出姿态角度、平台误差角和真实姿态角度之间的相互关系，则可以利用上述等式关系获得组合观测量Δγ，Δθ，Δψ的计算表达式。但由于其为非线性方程，而为采用线性卡尔曼滤波器，还必须直接获得组合系统的线性化量测方程，则必须利用上述关系求解出组合观测量Δγ，Δθ，Δψ，并建立其和惯性导航系统误差状态量之间的线性量测关系。
2.线性化角度观测方程建立
利用在小角度姿态偏差情况下，Δγ，Δθ，Δψ为小量的假设以及三角函数关系，则有：
sinγT=sin(γI+Δγ)≈sinγI+ΔγcosγIsinθT=sin(θI+Δθ)≈sinθI+ΔθcosθIsinψT=sin(ψI+Δψ)≈sinψI+ΔψcosψIcosθT=cos(θI+Δθ)≈cosθI-ΔθsinθI---(8)]]>
式(8)中，γT，θT，ψT分别表示为由姿态敏感器直接测量获得的航空机载地理系下的横滚姿态角度、俯仰姿态角度和航向姿态角度；此外，对公式(6)中的Cbc1可做如下简化表达：
cosγIcosψI+sinγIsinθIsinψIcosθIsinψIsinγIcosψI-cosγIsinθIsinψI-cosγIsinψI+sinγIsinθIcosψIcosθIcosψI-sinγIsinψI-cosγIsinθIcosψI-sinγIcosθIsinθIcosγIcosθI=---(9)]]>
C11′cosθIsinψIC13′C21′cosθIcosψIC23′-sinγIcosθIsinθIcosγIcosθI]]>
则由公式(9)，并结合公式(7)可以获得：
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θ=]]>
1-φUφNφU1-φE-φNφE1·C11′cosθIsinψIC13′C21′cosθIcosψIC23′-sinγIcosθIsinθIcosγIcosθI=---(10)]]>
...cosθIsinψI-φUcosθIcosψI+φNsinθI............-φNC11′+φEC21′-sinγIcosθI-φNcosθIsinψI+φEcosθIcosψI+sinθI...]]>
则利用式(10)中矩阵对应元素相等的关系，即可获得姿态角度观测量Δγ，Δθ，Δψ。由于只需要利用式(10)中的三个对应矩阵元素即可实现对姿态角度观测量的求解，因此对式(10)中等式右边其他矩阵元素不再进行计算，并用“...”表示。
则有如下的矩阵元素对应关系：
sinθT≈sinθ＝-φNcosθIsinψI+φEcosθIcosψI+sinθI
cosθTsinψT≈cosθsinψ＝cosθIsinψI-φUcosθIcosψ1+φNsinθI (11)
-sinγTcosθT≈-sinγcosθ＝-φNC′11+φEC′21-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′21-sinγIsinθIcosψI)/(-cosγI cosθT)+φN(sinψI sinγIsinθI-C′11)/(-cosγIcosθT)
三、建立航空机载地理系下的耦合了惯性导航位置误差的星敏感器姿态观测量和被估计的航空机载惯性导航系统误差状态中的平台误差角、纬度和经度误差状态量之间的线性化量测方程
3.星敏感器地理系下姿态测量原理
由1.～2.分析可以看出，如果能直接获得地理系下的姿态测量信息，则可以利用公式(12)实现地理系下的姿态组合滤波。但由于星敏感器测量输出的是航空飞行器相对于惯性系下的姿态输出，因此，无法直接采用上面分析给出的姿态测量方程。为此，本节将在分析惯性姿态到地理系姿态之间相互转换的关系基础上，直接建立地理系下的星光姿态测量模型。
利用星敏感器可以直接测量获得航空飞行器相对于惯性系下的姿态转换矩阵Cbi，上标i表示惯性坐标系，下标b表示载体本体坐标系，下面重点讨论如何由Cbi获得载体相对于地理导航坐标系下的姿态变换矩阵Cbn，上标n表示东北天地理导航坐标系，下标b表示载体本体坐标系。
考虑到由格林尼治恒星时角可获得惯性坐标系到地球固连坐标系的变换矩阵为Cie，上标e表示地球固连坐标系；同时有地球固连坐标系到地理坐标系的变换矩阵为Cen。
则结合上述矩阵相互转换关系，可以获得航空飞行器惯性姿态转换矩阵相对于导航坐标系下的姿态转换矩阵之间的关系，有：
Cbn=CenCieCbi=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θ---(13)]]>
式(13)中有如下定义：
Cen=-sinλcosλ0-cosλsinL-sinλsinLcosLcosλcosLsinλcosLsinL]]>
则利用式(13)即可求出航空飞行器相对于地理导航坐标系下的横滚姿态角度、俯仰姿态角度和航向姿态角度的真实值，计算表达式为，γ=tg-1(-C3,1C3,3)θ=sin-1(C3,2)ψ=tg-1(C1,2/C2,2),]]>其中Ci，j表示矩阵Cbn中对应的第i行和第j列的元素。
4.基于位置耦合误差建模的星光姿态观测方程
从3.中的分析可以看出，如果要将星敏感器输出的惯性姿态转换为地理系姿态，就必须提供航空飞行器在星敏感器观测时刻的地理经度和纬度位置信息λ，L的真值，而在航空机载星光和惯性组合环境下，唯一能够提供位置信息的，就只有惯性导航输出的计算地理经度和纬度信息，但由于惯性导航受到陀螺和加速度计漂移误差的影响，其导航精度随时间而发散。因此，利用惯性导航输出所提供的计算地理经纬度位置信息λS，LS必然不等同于地理经纬度位置信息λ，L的真值。因此，要直接实现地理系下的姿态组合，则必须建立耦合惯性导航位置误差的组合量测方程。
设惯性导航输出的地理经纬度信息分别为λS，LS，则可以构成计算地理坐标系c2系，其与导航坐标系n系之间考虑有小角度经纬度位置偏差δλ，δL时，则有如下计算地理坐标系到导航坐标系之间的坐标转换矩阵：
Cc2n=1-δλsinLSδλcosLSδλsinLS1δL-δλcosLS-δL1---(14)]]>
从而有耦合了惯性导航位置误差的星敏感器在地理系下的姿态测量输出为：
Cbc2=Cnc2CenCieCbi=Cnc2Cbn=]]>
cosγScosψS+sinγSsinθSsinψScosθSsinψSsinγScosψS-cosγSsinθSsinψS-cosγSsinψS+sinγSsinθScosψScosθSθcosψS-sinγSsinψS-cosγSsinθScosψS-sinγScosθSsinθScosγScosθS---(15)]]>
利用式(15)即可求得星敏感器在地理系下的横滚姿态角度、俯仰姿态角度和航向姿态角度的测量输出，计算表达式为，γS=tg-1(-C3,1C3,3)θS=sin-1(C3,2)ψS=tg-1(C1,2/C2,2),]]>其中Ci，j表示矩阵Cbc2中对应的第i行和第j列的元素；可见星敏在由惯性姿态向地理系姿态转换过程中，主要由于惯性导航输出的位置误差引起了转换后的地理系姿态测量误差。此外，由式(15)还可以获得
Cbn=Cc2nCbc2---(16)]]>
设星敏感器转换为相对于地理系下横滚、俯仰和航向姿态测量输出分别为γS，θS，ψS，从而与地理系下姿态观测原理推导类似，有如下的姿态观测关系：
Cbn=Cc1n·Cbc1=Cc2n·Cbc2---(17)]]>
由公式(17)可以看出，该姿态观测方程中已经耦合进入了惯性导航的位置误差，采用与二类似的推导思路，则有如下耦合位置误差的姿态角度观测关系如下：
-δλcosLScosθSsinψS-δLcosθScosψS+sinθS＝-φNcosθIsinψI+φEcosθIcosψI+sinθI
cosθSsinψS-δλsinLScosθScosψS+δλcosLSsinθS＝cosθIsinψI-φUcosθIcosψI+φNsinθI (18)
-δλcosLSC11-δLC21-sinγScosθS＝-φNC′11+φEC21-sinγIcosθI
式(18)中C11，C21表示的是矩阵Cbc2中的对应元素，和式(9)中的简化定义表示思路一样，从而有最终的耦合位置误差的姿态线性化观测方程为：
Δθ＝φEcosψI-φNsinψI+δLcosθS cosψS/cosθI+δλcosLScosθ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)+
δλ(sinLScosθScosψS-cosLSsinθS+cosLScosθSsinψSsinψIsinθI/cosθI)/(cosθScosψI) (19)
Δγ＝φE(C′21-sinγIsinθIcosψI)/(-cosγIcosθS)+φN(sinψIsinγIsinθI-C′11)/(-cosγIcosθS)+
δL(C21-cosθScosψSsinγIsinθI/cosθI)/(-cosγI cosθS)+
δλcosLS(C11-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)
Δγ＝φEhr1+φNhr2+δLhr3+δλhr4
由(20)式，可以获得星光和惯性的姿态组合线性化量测方程为：
式(21)中v1，v2，v3分别为对应的星敏感器输出的地理系下横滚、俯仰和航向姿态角度测量误差。
此外，考虑到姿态观测方程式(21)中高度通道不可观，因此，增加了气压高度表观测方程如下：
式中hI，he分别为惯性导航和气压高度表输出的高度值；v4为气压高度表输出时的高度误差。
则由上述式(21)和(22)可以获得机载星光和惯性的组合导航系统的量测方程Z(t)＝H(t)X(t)+V(t)，其具体形式如式(23)：
四、进行KF(Kalman Filter)滤波，估计航空机载惯性导航系统的误差状态量
6.状态方程和量测方程的离散化及卡尔曼滤波器
当采用线性卡尔曼滤波器时，需要对上面连续形式的系统状态方程(4)和量测方程(23)进行离散化，从而获得离散形式的系统方程。其离散化形式如下：
Xk=Φk,k-1Xk-1+Γk-1Wk-1Zk=HkXk+Vk]]>
式中Φk,k-1=Σm=0∞[F(tk)T]m/m!,]]>Γk-1={Σm=1∞[1m!(F(tk)T)m-1]}G(tk)T,]]>T为迭代周期。
从而可以获得系统的线性化卡尔曼滤波器方程如下：
X^k|k-1=Φk,k-1X^k-1]]>
Xk^=X^k|k-1+Kk[Zk-HkX^k|k-1]]]>
Pk|k-1＝Фk，k-1Pk-1Фk，k-1T+Γk-1Qk-1Γk-1T
Kk=Pk|k-1HkT(HkPk|k-1HkT+Rk)-1]]>
Pk=(I-KkHk)Pk|k-1(I-KkHk)T+KkRkKkT]]>
上式中的Q，R分别为系统的噪声方差矩阵和量测方差矩阵。
图2为采用本发明方法仿真的一条飞行航迹。
图3～图7的仿真结果表明，该方法不仅能实现对惯性导航系统姿态误差的滤波估计，还能够实现对位置误差的估计，能克服惯性导航系统姿态和位置误差随时间发散的趋势，可以取得比惯性系下姿态组合更好的滤波精度。