专利名称：一种基于自适应扩展卡尔曼滤波的地球卫星自主天文导航方法
技术领域：
本发明涉及一种基于自适应扩展卡尔曼滤波的地球卫星自主天文导航方法，可用于资源、通讯、侦察、气象等地球应用卫星自主导航定位。
背景技术：
天文导航系统是一种不与外界进行信息传输和交换，不依赖于地面设备的完全自主的导航系统，是一种利用光学敏感器测得的天体信息进行载体位置计算的一种定位导航方法。其基本原理是利用天体量测信息结合轨道动力学方程，利用最优估计方法估计空间飞行器的位置速度等导航信息。也就是说天文导航方法可分为三部分，一是轨道动力学的精确建模，二是量测量的选择，三是滤波(估计)方法的设计。
基于轨道动力学方程的天文导航包括直接敏感地平和利用星光折射间接敏感地平的两种导航方式。间接敏感地平要求必须一颗恒星发生大气折射，把测得的星光折射角作为量测信息，这种导航方式要求星光折射角与大气密度之间建立较精确的函数关系，而实际上很难得到大气密度的精确模型。
直接敏感地平是基于轨道动力学的另一种自主天文导航方式，它的原理是利用红外地球仪敏感地球边缘的切线方向，进而得到地球卫星与地心的连线方向，在利用星敏感器测得导航恒星的星光矢量方向，地心方向矢量与恒星的矢量构成星光角距作为量测量，如图2所示。这种导航方式具有结构简单、成本低廉、运行可靠、技术成熟且易于实现等特点。
在卫星导航估计中，目前常采用的最优估计方法是卡尔曼滤波方法，这种方法是假设在一种理想条件下进行的，即要求系统模型为线性或弱非线性，系统噪声和量测噪声统计特性为零均值的高斯白噪声，而实际的卫星导航系统不仅系统模型具有强非线性，系统噪声和量测噪声也很难满足要求，显然在实际工程应用中卡尔曼滤波方法的假设前提是十分苛刻的，存在自身无法克服的缺陷，这些都是导致滤波器的发散的主要原因，滤波器发散将导致导航精度下降，严重的将无法输出正确的导航结果，因此说传统的基于卡尔曼滤波方法存在自身难以克服的因噪声估计不准确而造成滤波器发散，导航精度下降的缺陷。
发明内容
本发明解决的技术问题是克服传统的扩展卡尔曼滤波方法中假设系统噪声和量测噪声为高斯白噪声，造成导航系统精度低的不足，提出一种基于自适应扩展卡尔曼滤波的地球卫星自主天文导航方法，通过直接敏感地平的方式实现地球卫星自主天文导航，根据自适应扩展卡尔曼滤波每次计算出的更新信息，推导出实际模型的系统噪声和量测噪声统计特性，克服了因噪声统计特性确定不准确造成的噪声滤波器发散问题，大大提高地球导航系统精度和适用范围。
本发明的技术方案为一种基于自适应扩展卡尔曼滤波的地球卫星自主天文导航方法，其特征以星光角距为量测信息，结合地球卫星的轨道动力学方程，利用自适应扩展卡尔曼滤波方法得到高精度的位置、速度估计，首先通过扩展卡尔曼滤波方法计算滤波残差的大小来判断滤波器是否发散，进而启动自适应扩展卡尔曼滤波程序，利用极大后验估计器推算出系统噪声和量测噪声的统计特性，极大程度克服了传统的卡尔曼滤波器在处理系统噪声和量测噪声为非高斯白噪声时导致滤波器发散的问题，进一步提高地球卫星的导航精度。
具体步骤如下(1)建立基于直角坐标系的地球卫星轨道运动学方程，即状态方程，通过解微分方程计算出地球卫星的位置(x，y，z)和速度(vx，vv，vz)信息；
(2)建立以星光角距为量测量的量测方程(3)建立离散型扩展卡尔曼滤波方程，将状态方程和量测方程离散化后再线性化，根据最小方差估计原则推导出离散性推广卡尔曼滤波的递推方程；(4)判定扩展卡尔曼滤波是否发散，利用对预测滤波残差判断扩展卡尔曼滤波器是否发散，若满足发散条件则进行噪声统计特性的估计，否则按照标准的扩展卡尔曼滤波程序进行计算；(5)对地球卫星导航系统中噪声统计特性的估计，包括地球卫星导航系统状态模型的系统噪声和量测方程的量测噪声统计特性估计，采用扩展卡尔曼滤波根据每次计算出的更新信息，推算出实际系统中系统噪声和量测噪声统计特性，即利用滤波器估计值和预报值近似代替平滑估计值，可得到次优的极大后验估计值，并使扩展卡尔曼滤波成为最优；(6)按照上述步骤(1)～(5)，输出为地球卫星状态矢量估计值 及其方差矩阵P，其中状态估计值 包括地球卫星位置和速度矢量[x，y，z，vx，vy，vz]T，状态估计方差矩阵P包括地球卫星位置和速度估计方差[Px，Py，Pz，Pvx，Pvy，Pvz]T。
本发明的基本原理是利用星敏感器观测导航恒星得到该星光在星敏感测量坐标系的方向，通过星敏感器安装矩阵的转换，可算得星光在地球卫星本体坐标系中的方向；再利用红外地球敏感器直接测得地球卫星至地球边缘的切线方向或法线方向，得到地心矢量在地球卫星本体坐标系中的方向；继而得到天文量测信息如星光角距，如图2所示等，再结合轨道动力学方程和先进的滤波技术即可估计出地球卫星的位置信息。自适应卡尔曼滤波方法原理是在扩展卡尔曼滤波的基础上，即根据每次扩展卡尔曼滤波计算出的前k乡更新信息，推导出实际模型的系统噪声和量测噪声统计特性，解决了传统扩展卡尔曼滤波中直接假设系统噪声和量测噪声为高斯白噪声的缺陷，提高自适应扩展卡尔曼滤波器收敛性以及地球卫星导航系统导航定位精度。
本发明与现有技术相比的优点在于解决了传统卡尔曼滤波应用于非线性较强的地球卫星导航系统中由于假设系统噪声和量侧噪声为非高斯白噪声所造成的缺陷，设计了自适应扩展卡尔曼滤波器。利用滤波器中前k步的更新信息推算系统噪声和量测噪声的真实统计特性的特点，解决了传统扩展卡尔曼滤波器因噪声统计特性估计不准确造成的滤波器发散，影响滤波精度的问题，提高了地球卫星的导航精度及其适用范围，使其更加适用于对导航定位要求较高的资源、通讯、侦察、气象等地球卫星。
图1为本发明的流程图；图2为本发明中的量测信息-星光角距示意图。
具体实施例方式
本发明具体实施的流程如图1所示，先由星敏感器测得的星光矢量和地平仪测量的地心方向矢量构成的星光角距作为量测量，结合地球卫星的状态方程，利用扩展卡尔曼滤波方法估计预测方差是否满足滤波器发散的要求，就利用更新信息推算系统噪声和量测噪声的统计特性，即利用滤波估计值和预测值近似代替平滑估计值，得到次极大后验估计值，再进行扩展卡尔曼滤波，估计地球卫星的位置、速度信息；否则滤波器不发散就直接估计出地球卫星的位置、速度信息。
具体的实施步骤如下。
1、产生标称的轨道数据①坐标系J2000.0地心赤道惯性坐标系②标称轨道参数设定，目的是产生标准的轨道。
半长轴a＝7136.635km偏心率e＝1.809×10-3轨道倾角i＝65°
升交点赤经Ω＝30.00°近升角距ω＝30.00°2、建立基于直角坐标系的地球卫星轨道运动学方程在研究地球卫星的运动时，选取历元(J2000.0)地心赤道坐标系。此时，通常选用的卫星导航系统状态模型(轨道动力学模型)为dxdt=vxdydt=vydzdt=vzdvxdt=-μxr3[1-J2(Rer)(7.5z2r2-1.5)]+ΔFxdvydt=-μyr3[1-J2(Rer)(7.5z2r2-1.5)]+ΔFydvydt=-μzr3[1-J2(Rer)(7.5z2r2-4.5)]+ΔFz---(6)]]>式中，r=x2+y2+z2,]]>简写为X(t)＝f(X，t)+w(t)(7)式中，状态矢量X＝[x y z vxvyvz]T，x，y，z，vx，vy，vz分别为卫星在X、Y、Z三个方向的位置和速度；μ是地心引力常数；r是卫星位置参数矢量；J2为地球引力系数；ΔFx，ΔFy，ΔFz为地球非球形摄动的高阶摄动项和日、月摄动以及太阳光压摄动和大气摄动等摄动力的影响，在简化模型中这些摄动力的影响通常用系统噪声w(t)表示。
上述方程为连续系统状态方程，将其离散化表示为Xk+1＝Φk+1kXk+Wkk＝1，2，3...(8)式中，Φk+1，k为k时刻至k+1时刻的一步转移矩阵；Wk为系统噪声矩阵。
对于系统噪声为零均值白噪声有E{W(k)}＝0，E{W(k)WT(j)}＝Qkδkjj＝1，2，3...(9)3、以星光角距为量测量建立量测方程星光角距是天文导航中经常使用的一种量测量，星光角距α指从卫星上观测到的导航恒星星光的矢量方向与地心矢量方向之间的夹角，如图2所示。从图中所示的几何关系，可得到星光角距α的表达式和相应的量测方程分别为α=arccos(-r·sr)---(10)]]>z(k+1)=h[X(k+1),k+1]+V(k+1)=α+Vk+1=arccos(-r·sr)+Vk+1---(11)]]>式中，r是卫星在地心惯性球坐标系中的位置矢量，由地平敏感器获得；s是导航星星光方向的单位矢量，由星敏感器识别。
对于量测噪声为零均值白噪声有E{V(k)}＝0，E{V(k)VT(j)}＝Rkδkj(12)4、建立离散型扩展卡尔曼滤波方程扩展的卡尔曼滤波是先将随机非线性系统模型中的非线性向量函数和h围绕滤波值线性化，得到系统线性化模型，然后应用卡尔曼滤波基本方程，解决非线性滤波问题的。将离散随机非线性系统(8)的状态方程和量测方程(11)中的非线性向量函数和h围绕滤波值 展开成泰勒级数，并略去二次以上项，分别得 Z(k+1)=h[X^(k+1|k),k+1]+∂h[X^(k+1),k+1]∂X(k+1)|X(k)=X^(k+1|k)[X(k+1)-X^(k+1|k)]+V(k+1)---(14)]]>离散性推广卡尔曼滤波的递推方程为 K(k+1)＝P(k+1|k)HT(k+1)[H(k+1)P(k+1|k)HT(k+1)+Rk+1]-1(16)
P(k+1|k)=Φ[k+1,k]P(k|k)ΦT[k+1,k]+QkΓT[X^(k|k),k]---(17)]]>P(k+1|k+1)＝[I-K(k+1)H(k+1)]P(k+1|k)(18)初始值为X^(0|0)=E{X(0)}=μx(0);]]>P(0|0)＝Var{X(0)}＝Px(0)；(20)式中， H(k+1)=∂h[X(k+1),k+1]∂X(k+1)|X(k)=X^(k+1|k).]]>P=[PxPyPzPvxPvyPvz]T]]>为状态矢量X估计方差矩阵。
5、判定扩展卡尔曼滤波是否发散，利用对预测滤波残差判断扩展卡尔曼滤波器是否发散，若满足发散条件则进行噪声统计特性的估计，否则按照标准的扩展卡尔曼滤波程序进行计算。
计算滤波残差Vk/k＝Zk-HkXk/(k-1)，以及其方差Sk=HkPk/(k-1)HkT+Rk,]]>判断滤波器是否发散条件为Vk/kTVk/k≤t×Tr(Sk)(21)式中，t为可调系数(t≥1)，若不满足条件，则判定扩展卡尔曼滤波发散，，下一步进行地球卫星导航系统中噪声统计特性的估计，即估计系统噪声和量测噪声统计特性。
6、对地球卫星导航系统中噪声统计特性的估计，包括地球卫星导航系统状态模型的系统噪声和量测方程的量测噪声统计特性估计，采用扩展卡尔曼滤波根据每次计算出的更新信息，推算出实际系统中系统噪声和量测噪声统计特性，即利用滤波器估计值和预报值近似代替平滑估计值，可得到次优的极大后验估计值，并使扩展卡尔曼滤波成为最优。
①系统噪声的统计特性为
式中， 为预测残差， 为系统噪声误差均值和协方差。
②量测噪声的统计特性为 式中， 为量测残差， 为量测噪声误差均值和协方差。
7、利用步骤1产生的标称轨迹，利用步骤2求解状态方程，利用步骤3建立量测方程，步骤2与步骤3之间的相互关系代入步骤4中的离散型扩展卡尔曼滤波方程中，估计出地球卫星位置和速度矢量[x，y，z，vx，vy，vz]T，而且在估计过程要通过步骤5计算滤波残差，据此判定扩展卡尔曼滤波器是否发散，如果滤波器发散通过步骤6估计模型噪声的统计特性，将估计出来的统计特性再代入步骤4中，估计出地球卫星状态矢量估计值 及其方差矩阵P，其中状态估计值 包括地球卫星位置和速度矢量[x，y，z，vx，vy，vz]T，状态估计方差矩阵P包括地球卫星位置和速度估计方差[Px，Py，Pz，Pvx，Pvy，Pvz]T。
本发明说明书中未作详细描述的内容属于本领域专业技术人员公知的现有技术。
权利要求
1.一种基于自适应扩展卡尔曼滤波的地球卫星自主天文导航方法，其特征在于包括下列步骤(1)建立基于直角坐标系的地球卫星轨道运动学方程，即状态方程，通过解微分方程计算出地球卫星的位置(x，y，z)和速度(vx，vy，vz)信息；(2)建立以星光角距为量测量的量测方程；(3)建立离散型扩展卡尔曼滤波方程，将状态方程和量测方程离散化后再线性化，根据最小方差估计原则推导出离散性推广卡尔曼滤波的递推方程；(4)判定扩展卡尔曼滤波是否发散，利用对预测滤波残差判断扩展卡尔曼滤波器是否发散，若满足发散条件则进行噪声统计特性的估计，否则按照标准的扩展卡尔曼滤波程序进行计算；(5)对地球卫星导航系统中噪声统计特性的估计，包括地球卫星导航系统状态模型的系统噪声和量测方程的量测噪声统计特性估计，采用扩展卡尔曼滤波根据每次计算出的更新信息，推算出实际系统中系统噪声和量测噪声统计特性，即利用滤波器估计值和预报值近似代替平滑估计值，可得到次优的极大后验估计值，并使扩展卡尔曼滤波成为最优；(6)按照上述步骤(1)～(5)，输出为地球卫星状态矢量估计值 及其方差矩阵P，其中状态估计值 包括地球卫星位置和速度矢量[x，y，z，vx，vy，vz]T，状态估计方差矩阵P包括地球卫星位置和速度估计方差[Px，Py，Pz，Pvx，Pvy，Pvz]T。
2.根据权利要求1所述的一种基于自适应扩展卡尔曼滤波的地球卫星自主天文导航方法，其特征在于所述步骤(4)中的扩展卡尔曼滤波是否发散的判定为计算滤波残差Vk/k＝Zk-HkXk/(k-1)，以及其方差Sk=HkPk/(k-1)HkT+Rk,]]>判断滤波器是否发散条件为Vk/kTVk/k≤t×Tr(Sk)---(1)]]>式中，t为可调系数(t≥1)，Xk，Zk，Hk，Vk分别k时刻的状态量，量测量，量测矩阵和量测残差；Sk，Pk，Rk分别为滤波残差的方差，估计均方误差，量测噪声的协方差，Tr为求Sk矩阵的迹，若不满足条件，则判定传统卡尔曼滤波发散，启动对地球卫星导航系统中噪声统计特性的估计，分别估计系统噪声与量测噪声统计特性。
3.根据权利要求1所述的一种基于自适应扩展卡尔曼滤波的地球卫星自主天文导航方法，其特征在于所述步骤(5)中对系统噪声与量测噪声统计特性的估计，采用根据扩展卡尔曼滤波根据每次计算出的更新信息推算系统噪声和量测噪声特性，即利用滤波估计值 和预测值 近似代替平滑估计值，得到次极大后验估计值，该方法的具体步骤为①系统噪声的统计特性为q^k=1kΣj=1kXj%---(2)]]>Q^k=1kΣj=1k(Xj%-q^j)(Xj%-q^j)T---(3)]]>式中，Xj%=X^j-X^j/(j-1)=X^j-Φk,k-1X^j-1]]>为预测残差， 为系统噪声误差均值和协方差，Φk，k-1为tk-1时刻的一步转移矩阵；②量测噪声的统计特性为r^k=1kΣj=1kZj%---(4)]]>R^k=1kΣj=1k(Zj%-r^j)(Zj%-r^j)T---(5)]]>式中，Zj%=Zj-HkX^j]]>为量测残差， 为量测噪声误差均值和协方差。
全文摘要
一种基于自适应扩展卡尔曼滤波的地球卫星自主天文导航方法，建立基于直角坐标系的地球卫星轨道运动学方程，建立以星光角距为量测量建立量测方程，建立离散型扩展卡尔曼滤波方程，判定扩展卡尔曼滤波是否发散，利用对预测滤波残差判断扩展卡尔曼滤波器是否发散，若满足发散条件则进行噪声统计特性的估计，否则按照标准的扩展卡尔曼滤波程序进行计算。本发明解决了因噪声统计特性确定不准确造成的噪声滤波器发散，影响导航精度的问题，具有自主、灵活简单、精度高的特点，更适用于对导航精度要求较高的资源、通讯、侦查、气象等地球应用卫星。
文档编号G01C21/20GK1987355SQ20061016557
公开日2007年6月27日 申请日期2006年12月22日 优先权日2006年12月22日
发明者房建成, 杨照华, 宁晓琳, 武瑾媛, 宋婷婷 申请人:北京航空航天大学