專利名稱：一種基于自適應擴展卡爾曼濾波的地球衛(wèi)星自主天文導航方法
技術領域：
本發(fā)明涉及一種基于自適應擴展卡爾曼濾波的地球衛(wèi)星自主天文導航方法，可用于資源、通訊、偵察、氣象等地球應用衛(wèi)星自主導航定位。
背景技術：
天文導航系統是一種不與外界進行信息傳輸和交換，不依賴于地面設備的完全自主的導航系統，是一種利用光學敏感器測得的天體信息進行載體位置計算的一種定位導航方法。其基本原理是利用天體量測信息結合軌道動力學方程，利用最優(yōu)估計方法估計空間飛行器的位置速度等導航信息。也就是說天文導航方法可分為三部分，一是軌道動力學的精確建模，二是量測量的選擇，三是濾波(估計)方法的設計。
基于軌道動力學方程的天文導航包括直接敏感地平和利用星光折射間接敏感地平的兩種導航方式。間接敏感地平要求必須一顆恒星發(fā)生大氣折射，把測得的星光折射角作為量測信息，這種導航方式要求星光折射角與大氣密度之間建立較精確的函數關系，而實際上很難得到大氣密度的精確模型。
直接敏感地平是基于軌道動力學的另一種自主天文導航方式，它的原理是利用紅外地球儀敏感地球邊緣的切線方向，進而得到地球衛(wèi)星與地心的連線方向，在利用星敏感器測得導航恒星的星光矢量方向，地心方向矢量與恒星的矢量構成星光角距作為量測量，如圖2所示。這種導航方式具有結構簡單、成本低廉、運行可靠、技術成熟且易于實現等特點。
在衛(wèi)星導航估計中，目前常采用的最優(yōu)估計方法是卡爾曼濾波方法，這種方法是假設在一種理想條件下進行的，即要求系統模型為線性或弱非線性，系統噪聲和量測噪聲統計特性為零均值的高斯白噪聲，而實際的衛(wèi)星導航系統不僅系統模型具有強非線性，系統噪聲和量測噪聲也很難滿足要求，顯然在實際工程應用中卡爾曼濾波方法的假設前提是十分苛刻的，存在自身無法克服的缺陷，這些都是導致濾波器的發(fā)散的主要原因，濾波器發(fā)散將導致導航精度下降，嚴重的將無法輸出正確的導航結果，因此說傳統的基于卡爾曼濾波方法存在自身難以克服的因噪聲估計不準確而造成濾波器發(fā)散，導航精度下降的缺陷。

發(fā)明內容
本發(fā)明解決的技術問題是克服傳統的擴展卡爾曼濾波方法中假設系統噪聲和量測噪聲為高斯白噪聲，造成導航系統精度低的不足，提出一種基于自適應擴展卡爾曼濾波的地球衛(wèi)星自主天文導航方法，通過直接敏感地平的方式實現地球衛(wèi)星自主天文導航，根據自適應擴展卡爾曼濾波每次計算出的更新信息，推導出實際模型的系統噪聲和量測噪聲統計特性，克服了因噪聲統計特性確定不準確造成的噪聲濾波器發(fā)散問題，大大提高地球導航系統精度和適用范圍。
本發(fā)明的技術方案為一種基于自適應擴展卡爾曼濾波的地球衛(wèi)星自主天文導航方法，其特征以星光角距為量測信息，結合地球衛(wèi)星的軌道動力學方程，利用自適應擴展卡爾曼濾波方法得到高精度的位置、速度估計，首先通過擴展卡爾曼濾波方法計算濾波殘差的大小來判斷濾波器是否發(fā)散，進而啟動自適應擴展卡爾曼濾波程序，利用極大后驗估計器推算出系統噪聲和量測噪聲的統計特性，極大程度克服了傳統的卡爾曼濾波器在處理系統噪聲和量測噪聲為非高斯白噪聲時導致濾波器發(fā)散的問題，進一步提高地球衛(wèi)星的導航精度。
具體步驟如下(1)建立基于直角坐標系的地球衛(wèi)星軌道運動學方程，即狀態(tài)方程，通過解微分方程計算出地球衛(wèi)星的位置(x，y，z)和速度(vx，vv，vz)信息；
(2)建立以星光角距為量測量的量測方程(3)建立離散型擴展卡爾曼濾波方程，將狀態(tài)方程和量測方程離散化后再線性化，根據最小方差估計原則推導出離散性推廣卡爾曼濾波的遞推方程；(4)判定擴展卡爾曼濾波是否發(fā)散，利用對預測濾波殘差判斷擴展卡爾曼濾波器是否發(fā)散，若滿足發(fā)散條件則進行噪聲統計特性的估計，否則按照標準的擴展卡爾曼濾波程序進行計算；(5)對地球衛(wèi)星導航系統中噪聲統計特性的估計，包括地球衛(wèi)星導航系統狀態(tài)模型的系統噪聲和量測方程的量測噪聲統計特性估計，采用擴展卡爾曼濾波根據每次計算出的更新信息，推算出實際系統中系統噪聲和量測噪聲統計特性，即利用濾波器估計值和預報值近似代替平滑估計值，可得到次優(yōu)的極大后驗估計值，并使擴展卡爾曼濾波成為最優(yōu)；(6)按照上述步驟(1)～(5)，輸出為地球衛(wèi)星狀態(tài)矢量估計值 及其方差矩陣P，其中狀態(tài)估計值 包括地球衛(wèi)星位置和速度矢量[x，y，z，vx，vy，vz]T，狀態(tài)估計方差矩陣P包括地球衛(wèi)星位置和速度估計方差[Px，Py，Pz，Pvx，Pvy，Pvz]T。
本發(fā)明的基本原理是利用星敏感器觀測導航恒星得到該星光在星敏感測量坐標系的方向，通過星敏感器安裝矩陣的轉換，可算得星光在地球衛(wèi)星本體坐標系中的方向；再利用紅外地球敏感器直接測得地球衛(wèi)星至地球邊緣的切線方向或法線方向，得到地心矢量在地球衛(wèi)星本體坐標系中的方向；繼而得到天文量測信息如星光角距，如圖2所示等，再結合軌道動力學方程和先進的濾波技術即可估計出地球衛(wèi)星的位置信息。自適應卡爾曼濾波方法原理是在擴展卡爾曼濾波的基礎上，即根據每次擴展卡爾曼濾波計算出的前k鄉(xiāng)更新信息，推導出實際模型的系統噪聲和量測噪聲統計特性，解決了傳統擴展卡爾曼濾波中直接假設系統噪聲和量測噪聲為高斯白噪聲的缺陷，提高自適應擴展卡爾曼濾波器收斂性以及地球衛(wèi)星導航系統導航定位精度。
本發(fā)明與現有技術相比的優(yōu)點在于解決了傳統卡爾曼濾波應用于非線性較強的地球衛(wèi)星導航系統中由于假設系統噪聲和量側噪聲為非高斯白噪聲所造成的缺陷，設計了自適應擴展卡爾曼濾波器。利用濾波器中前k步的更新信息推算系統噪聲和量測噪聲的真實統計特性的特點，解決了傳統擴展卡爾曼濾波器因噪聲統計特性估計不準確造成的濾波器發(fā)散，影響濾波精度的問題，提高了地球衛(wèi)星的導航精度及其適用范圍，使其更加適用于對導航定位要求較高的資源、通訊、偵察、氣象等地球衛(wèi)星。


圖1為本發(fā)明的流程圖；圖2為本發(fā)明中的量測信息-星光角距示意圖。
具體實施例方式
本發(fā)明具體實施的流程如圖1所示，先由星敏感器測得的星光矢量和地平儀測量的地心方向矢量構成的星光角距作為量測量，結合地球衛(wèi)星的狀態(tài)方程，利用擴展卡爾曼濾波方法估計預測方差是否滿足濾波器發(fā)散的要求，就利用更新信息推算系統噪聲和量測噪聲的統計特性，即利用濾波估計值和預測值近似代替平滑估計值，得到次極大后驗估計值，再進行擴展卡爾曼濾波，估計地球衛(wèi)星的位置、速度信息；否則濾波器不發(fā)散就直接估計出地球衛(wèi)星的位置、速度信息。
具體的實施步驟如下。
1、產生標稱的軌道數據①坐標系J2000.0地心赤道慣性坐標系②標稱軌道參數設定，目的是產生標準的軌道。
半長軸a＝7136.635km偏心率e＝1.809×10-3軌道傾角i＝65°
升交點赤經Ω＝30.00°近升角距ω＝30.00°2、建立基于直角坐標系的地球衛(wèi)星軌道運動學方程在研究地球衛(wèi)星的運動時，選取歷元(J2000.0)地心赤道坐標系。此時，通常選用的衛(wèi)星導航系統狀態(tài)模型(軌道動力學模型)為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)式中，狀態(tài)矢量X＝[x y z vxvyvz]T，x，y，z，vx，vy，vz分別為衛(wèi)星在X、Y、Z三個方向的位置和速度；μ是地心引力常數；r是衛(wèi)星位置參數矢量；J2為地球引力系數；ΔFx，ΔFy，ΔFz為地球非球形攝動的高階攝動項和日、月攝動以及太陽光壓攝動和大氣攝動等攝動力的影響，在簡化模型中這些攝動力的影響通常用系統噪聲w(t)表示。
上述方程為連續(xù)系統狀態(tài)方程，將其離散化表示為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、以星光角距為量測量建立量測方程星光角距是天文導航中經常使用的一種量測量，星光角距α指從衛(wèi)星上觀測到的導航恒星星光的矢量方向與地心矢量方向之間的夾角，如圖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是衛(wèi)星在地心慣性球坐標系中的位置矢量，由地平敏感器獲得；s是導航星星光方向的單位矢量，由星敏感器識別。
對于量測噪聲為零均值白噪聲有E{V(k)}＝0，E{V(k)VT(j)}＝Rkδkj(12)4、建立離散型擴展卡爾曼濾波方程擴展的卡爾曼濾波是先將隨機非線性系統模型中的非線性向量函數和h圍繞濾波值線性化，得到系統線性化模型，然后應用卡爾曼濾波基本方程，解決非線性濾波問題的。將離散隨機非線性系統(8)的狀態(tài)方程和量測方程(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]]>為狀態(tài)矢量X估計方差矩陣。
5、判定擴展卡爾曼濾波是否發(fā)散，利用對預測濾波殘差判斷擴展卡爾曼濾波器是否發(fā)散，若滿足發(fā)散條件則進行噪聲統計特性的估計，否則按照標準的擴展卡爾曼濾波程序進行計算。
計算濾波殘差Vk/k＝Zk-HkXk/(k-1)，以及其方差Sk=HkPk/(k-1)HkT+Rk,]]>判斷濾波器是否發(fā)散條件為Vk/kTVk/k≤t×Tr(Sk)(21)式中，t為可調系數(t≥1)，若不滿足條件，則判定擴展卡爾曼濾波發(fā)散，，下一步進行地球衛(wèi)星導航系統中噪聲統計特性的估計，即估計系統噪聲和量測噪聲統計特性。
6、對地球衛(wèi)星導航系統中噪聲統計特性的估計，包括地球衛(wèi)星導航系統狀態(tài)模型的系統噪聲和量測方程的量測噪聲統計特性估計，采用擴展卡爾曼濾波根據每次計算出的更新信息，推算出實際系統中系統噪聲和量測噪聲統計特性，即利用濾波器估計值和預報值近似代替平滑估計值，可得到次優(yōu)的極大后驗估計值，并使擴展卡爾曼濾波成為最優(yōu)。
①系統噪聲的統計特性為
式中， 為預測殘差， 為系統噪聲誤差均值和協方差。
②量測噪聲的統計特性為 式中， 為量測殘差， 為量測噪聲誤差均值和協方差。
7、利用步驟1產生的標稱軌跡，利用步驟2求解狀態(tài)方程，利用步驟3建立量測方程，步驟2與步驟3之間的相互關系代入步驟4中的離散型擴展卡爾曼濾波方程中，估計出地球衛(wèi)星位置和速度矢量[x，y，z，vx，vy，vz]T，而且在估計過程要通過步驟5計算濾波殘差，據此判定擴展卡爾曼濾波器是否發(fā)散，如果濾波器發(fā)散通過步驟6估計模型噪聲的統計特性，將估計出來的統計特性再代入步驟4中，估計出地球衛(wèi)星狀態(tài)矢量估計值 及其方差矩陣P，其中狀態(tài)估計值 包括地球衛(wèi)星位置和速度矢量[x，y，z，vx，vy，vz]T，狀態(tài)估計方差矩陣P包括地球衛(wèi)星位置和速度估計方差[Px，Py，Pz，Pvx，Pvy，Pvz]T。
本發(fā)明說明書中未作詳細描述的內容屬于本領域專業(yè)技術人員公知的現有技術。
權利要求
1.一種基于自適應擴展卡爾曼濾波的地球衛(wèi)星自主天文導航方法，其特征在于包括下列步驟(1)建立基于直角坐標系的地球衛(wèi)星軌道運動學方程，即狀態(tài)方程，通過解微分方程計算出地球衛(wèi)星的位置(x，y，z)和速度(vx，vy，vz)信息；(2)建立以星光角距為量測量的量測方程；(3)建立離散型擴展卡爾曼濾波方程，將狀態(tài)方程和量測方程離散化后再線性化，根據最小方差估計原則推導出離散性推廣卡爾曼濾波的遞推方程；(4)判定擴展卡爾曼濾波是否發(fā)散，利用對預測濾波殘差判斷擴展卡爾曼濾波器是否發(fā)散，若滿足發(fā)散條件則進行噪聲統計特性的估計，否則按照標準的擴展卡爾曼濾波程序進行計算；(5)對地球衛(wèi)星導航系統中噪聲統計特性的估計，包括地球衛(wèi)星導航系統狀態(tài)模型的系統噪聲和量測方程的量測噪聲統計特性估計，采用擴展卡爾曼濾波根據每次計算出的更新信息，推算出實際系統中系統噪聲和量測噪聲統計特性，即利用濾波器估計值和預報值近似代替平滑估計值，可得到次優(yōu)的極大后驗估計值，并使擴展卡爾曼濾波成為最優(yōu)；(6)按照上述步驟(1)～(5)，輸出為地球衛(wèi)星狀態(tài)矢量估計值 及其方差矩陣P，其中狀態(tài)估計值 包括地球衛(wèi)星位置和速度矢量[x，y，z，vx，vy，vz]T，狀態(tài)估計方差矩陣P包括地球衛(wèi)星位置和速度估計方差[Px，Py，Pz，Pvx，Pvy，Pvz]T。
2.根據權利要求1所述的一種基于自適應擴展卡爾曼濾波的地球衛(wèi)星自主天文導航方法，其特征在于所述步驟(4)中的擴展卡爾曼濾波是否發(fā)散的判定為計算濾波殘差Vk/k＝Zk-HkXk/(k-1)，以及其方差Sk=HkPk/(k-1)HkT+Rk,]]>判斷濾波器是否發(fā)散條件為Vk/kTVk/k≤t×Tr(Sk)---(1)]]>式中，t為可調系數(t≥1)，Xk，Zk，Hk，Vk分別k時刻的狀態(tài)量，量測量，量測矩陣和量測殘差；Sk，Pk，Rk分別為濾波殘差的方差，估計均方誤差，量測噪聲的協方差，Tr為求Sk矩陣的跡，若不滿足條件，則判定傳統卡爾曼濾波發(fā)散，啟動對地球衛(wèi)星導航系統中噪聲統計特性的估計，分別估計系統噪聲與量測噪聲統計特性。
3.根據權利要求1所述的一種基于自適應擴展卡爾曼濾波的地球衛(wèi)星自主天文導航方法，其特征在于所述步驟(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]]>為量測殘差， 為量測噪聲誤差均值和協方差。
全文摘要
一種基于自適應擴展卡爾曼濾波的地球衛(wèi)星自主天文導航方法，建立基于直角坐標系的地球衛(wèi)星軌道運動學方程，建立以星光角距為量測量建立量測方程，建立離散型擴展卡爾曼濾波方程，判定擴展卡爾曼濾波是否發(fā)散，利用對預測濾波殘差判斷擴展卡爾曼濾波器是否發(fā)散，若滿足發(fā)散條件則進行噪聲統計特性的估計，否則按照標準的擴展卡爾曼濾波程序進行計算。本發(fā)明解決了因噪聲統計特性確定不準確造成的噪聲濾波器發(fā)散，影響導航精度的問題，具有自主、靈活簡單、精度高的特點，更適用于對導航精度要求較高的資源、通訊、偵查、氣象等地球應用衛(wèi)星。
文檔編號G01C21/20GK1987355SQ20061016557
公開日2007年6月27日 申請日期2006年12月22日 優(yōu)先權日2006年12月22日
發(fā)明者房建成, 楊照華, 寧曉琳, 武瑾媛, 宋婷婷 申請人:北京航空航天大學