一種基於遺傳最優request和gupf的高精度組合定姿方法

2023-06-07 04:27:21

專利名稱：一種基於遺傳最優request和gupf的高精度組合定姿方法
技術領域：
本發明涉及一種基於恆星敏感器的慣性/天文組合定姿方法，特別是一種基於遺傳最優REQUEST和GUPF的高精度組合定姿方法，可用於各種太空飛行器的高精度組合定姿。
背景技術：
為滿足天基對地觀測、武器精確打擊以及空間探索開發的迫切需求，各類地球衛星、深空探測器、載人飛船、彈道飛彈和運載火箭等太空飛行器必須具備自主運行和管理能力，而高精度的自主定姿是太空飛行器自主運行和管理的核心技術瓶頸。目前，太空飛行器的高精度自主定姿，無法依靠任何一種導航手段獨立實現。純慣性導航系統能夠自主、實時提供連續、全面的導航信息，短時精度高，但其誤差隨工作時間積累，難以滿足太空飛行器的長時間高精度定姿要求；天文導航能夠提供高精度姿態信息，誤差不隨時間積累，但易受氣候條件限制，且輸出信息不連續；將這兩者相結合、優勢互補，構成慣性/天文組合定姿系統，是實現太空飛行器長時間、高精度定姿的最為有效的手段。
在慣性/天文組合定姿技術方面，1996年Bar-Itzhack提出了一種REQUEST方法，它以K-矩陣為狀態變量，順序轉移和傳播K-矩陣，使用漸消記憶因子來處理噪聲問題，但這種方法是次優的。2001年DanielChoukroun等人在REQUEST方法的基礎上，提出了最優REQUEST方法，它將REQUEST算法嵌入卡爾曼濾波的框架來計算最優增益，可達到最優，但無法估計陀螺漂移。針對這個問題，一些學者提出了基於EKF和最優REQUEST相結合的定姿方法，雖然可估計陀螺漂移，但在非線性系統和非高斯噪聲情況下估計性能較差，因而存在陀螺漂移估計精度較低的缺點。專利申請號為200610165166.4中提出了一種基於最優REQUEST算法和UKF的慣性恆星羅盤組合定姿方法，該方法特別適用於非線性系統，但在非高斯噪聲情況下仍存在濾波精度低和速度慢的不足。

發明內容
本發明的技術解決問題是克服現有技術的不足，提出一種基於遺傳最優REQUEST和GUPF的高精度組合定姿方法，解決系統非線性和噪聲非高斯問題，以快速獲得高精度的姿態信息，並能夠準確地估計陀螺漂移，實現各種類型太空飛行器長時間、高精度的組合定姿。
本發明的技術解決方案為一種基於遺傳最優REQUEST和GUPF的高精度組合定姿方法，其特點在於利用慣性量測信息和天文量測信息，通過遺傳最優REQUEST(最優回歸四元數估計法)和GUPF(遺傳無跡粒子濾波)方法，實現太空飛行器長時間、高精度的快速組合定姿，其實現步驟如下(1)將慣性量測信息通過補償陀螺輸出數據和姿態解算得到實時的載體姿態信息；(2)利用天文量測信息，結合補償後的陀螺輸出數據，通過遺傳最優REQUEST方法快速獲得所需間隔的高精度天文姿態信息；(3)利用GUPF算法將天文姿態信息和載體姿態信息相融合，求解高精度的載體姿態信息，估計陀螺漂移，並反饋校正載體姿態和補償陀螺漂移補償；最終實現基於天文量測信息實時消除慣性/天文組合導航系統陀螺隨機誤差的在線修正，完成對太空飛行器的高精度組合定姿。
本發明的原理是首先將慣性量測信息通過補償陀螺輸出數據和姿態解算得到實時的載體姿態信息；其次利用天文量測信息，結合補償後的陀螺輸出數據，通過遺傳最優REQUEST方法快速獲得特定間隔的高精度天文姿態信息；最後利用GUPF算法將天文姿態信息和載體姿態信息相融合，解決系統非線性和噪聲非高斯問題，求解高精度載體姿態信息，估計陀螺漂移，並反饋校正載體姿態和補償陀螺漂移補償；最終實現太空飛行器長時間、高精度的組合定姿。
本發明與現有技術相比的優點在於本發明克服了傳統組合定姿方法在定姿精度和陀螺漂移估計精度低的不足，將遺傳最優REQUEST算法引入到組合定姿方法中，解決了濾波器次優的問題，提高了組合定姿的精度和速度；同時利用GUPF有效解決了系統非線性和噪聲非高斯的問題，將慣性量測信息和天文量測信息相融合，進一步提高了組合定姿的精度，實現了對陀螺漂移的精確估計，滿足了太空飛行器長時間、高精度組合定姿的要求。


圖1為本發明的一種基於遺傳最優REQUEST和GUPF的高精度組合定姿方法原理圖。
具體實施例方式
如圖1所示，本發明的具體實施步驟如下1、首先將慣性量測信息通過補償陀螺輸出數據和姿態解算得到實時的載體姿態信息，流程如下a.設定初始姿態為(0θ0γ0)，計算得出初始姿態四元數q(0)陣 b.更新矩陣為q(n+1)={cos2I+sin2[]}q(n)]]>n為第n時刻，I為單位四元數，Δφ＝[ΔφXΔφYΔφZ]為陀螺輸出角增量，[ΔΦ]為[]=0-X-Y-ZX0Z-YY-Z0XZY-X0]]>
c.由q(n+1)＝[q1q2q3q4]T，計算姿態餘弦陣C為C=C11C12C13C21C22C23C31C32C33=q42+q12-q22-q322(q1q2+q4q3)2(q1q3-q4q2)2(q1q2-q4q3)q42-q12+q22-q322(q2q3+q4q1)2(q1q3+q4q2)2(q2q3-q4q1)q42-q12-q22+q32]]>d.由方向餘弦陣C求解載體的實時姿態信息航向、俯仰和橫滾等三姿態角(，θ，γ)的解算公式如下俯仰角θ值為θ＝sin-1(C23)；航向角值的計算如下表所示

橫滾角γ值的計算如下表所示

2、利用遺傳最優REQUEST方法，將所需間隔的天文量測信息，結合補償後的陀螺輸出數據，快速求解高精度天文姿態信息的步驟為
A.定義3×3的矩陣Bk和Sk，3×1的列向量z和標量σk，k＝1...n...，Bk=wkvkT,]]>Sk=Bk+BkT]]>，z＝wk×vk，σk＝tr(Bk)；其中，wk＝[wk，xwk，ywk，z]T為k時刻觀測的星光在星敏感器坐標系中的坐標矢量，vk＝[vk，xvk，yvk，z]T為k時刻該星光在地心慣性坐標系中的參考矢量；Bk=wk,xvk,xwk,xvk,ywk,xvk,zwk,yvk,xwk,yvk,ywk,yvk,zwk,zvk,xwk,zvk,ywk,zvk,z,]]>z=-wk,zvk,y+wk,yvk,zwk,zvk,x-wk,xvk,z-wk,yvk,x+wk,xvk,y,]]>Sk=2wk,xvk,xwk,xvk,y+wk,yvk,xwk,xvk,z+wk,zvk,xwk,yvk,x+wk,xvk,y2wk,yvk,ywk,yvk,z+wk,zvk,ywk,zvk,x+wk,xvk,zwk,zvk,y+wk,yvk,z2wk,zvk,z,]]>Kk/k=kmk-1mkKk/k-1+1mkKk]]>上式中Kk/k-1為在k-1時刻估計k時刻的K矩陣值，mk和mk+1是使Kk/k的最大特徵值接近於1的標量權值係數，係數ρk是漸消記憶因子，δKk用第一個觀測向量根據下式計算Kk=Sk-kIkzkzkTk]]>B.通過遺傳算法選取最優的漸消記憶因子ρk，遺傳算法的自適應評價函數定義為f(ρk)＝min(Kk/kmk-mk-1Kk/k-1+δKk)；C.時間更新Kk+1/k=kKk/kkT]]>Pk+1/k=kPk/kkT+Qk]]>式中，Φk由補償後的陀螺輸出數據ωk計算得到，代表四元數動力學差分方程中的4×4正交轉移矩陣，k=-[k]k-kT0,k=k,1k,2k,3T,[k]=0-k,3k,2k,30-k,1-k,2k,10;]]>D.量測更新
k+1=mk2tr(Pk+1/k)mk2tr(Pk+1/k)+mk+12tr(Rk+1)]]>mk+1＝(1-ρk+1)mk+ρk+1δmk+1Kk+1/k+1=(1-k+1)mkmk+1Kk+1/k+k+1mk+1mk+1Kk+1]]>Pk+1/k+1=[(1-k+1)mkmk+1]2Pk+1/k+(k+1mk+1mk+1)2Rk+1]]>E.求解天文姿態信息qk+1＝Φkqk由qk+1＝[q1q2q3q4]T，計算姿態餘弦陣C′為C=q42+q12-q22-q322(q1q2+q4q3)2(q1q3-q4q2)2(q1q2-q4q3)q42-q12+q22-q322(q2q3+q4q1)2(q1q3+q4q2)2(q2q3-q4q1)q42-q12-q22+q32]]>F.由方向餘弦陣C′即可得出高精度天文姿態信息，求解方法同載體姿態信息的解算方法。
3、利用GUPF算法將天文姿態信息和載體姿態信息相融合，完成對太空飛行器長時間、高精度的組合定姿步驟為(1)t＝0時，初始化對初始的先驗概率密度p(x0)進行採樣，生成N個服從p(x0)分布的粒子x0(i)，i＝1，…，N，其均值和方差滿足 P0(i)=E[(x0(i)-x0(i))(x0(i)-x0(i))T]]]>(2)t≥1時，步驟如下①採樣步驟用Unscented卡爾曼濾波更新粒子{xk-1(i)，Pk-1(i)}得到{xk(i)，Pk(i)}，採樣x^k(i)~q(xk(i)|x0:k-1(i),y1:k)=N(xk(i),Pk(i)),i=1,,N]]>②計算權重
w~k(i)=w~k-1(i)p(yk|x^k(i))p(x^k(i)|xk-1(i))q(x^k(i)|x0:k-1(i)y1:k)]]>歸一化權重wk(i)=w~k(i)/i=1Nw~k(i)]]>③重採樣步驟從離散分布的{xk(i),wk(xk(i))}i=1,,N]]>中進行N次重採樣，得到一組新的粒子{xk(i*)，1/N}，仍為p(xk|y0k)的近似表示。
④通過遺傳算法從得到的這組新的粒子中選取優等粒子，剔出低等的粒子，以解決粒子枯竭問題，其自適應評價函數定義為f=(u-min(1,p(yk|xk*(i))p(yk|xk(i*))));]]>式中，u為0～1之間的一個隨機數，xk*(i)～p(xk|xk-1i)為從馬爾科夫鏈中進行的採樣，在此取u＝0.8；⑤輸出按照最小方差準則，載體姿態的最優估計值就是條件分布的均值，即x^k=i=1Nwkixki]]>pk=i=1Nwki(xki-x^k)(xki-x^k)T]]>本發明說明書中未作詳細描述的內容屬於本領域專業技術人員公知的現有技術。
權利要求
1.一種基於遺傳最優REQUEST和GUPF的高精度組合定姿方法，其特徵在於包括以下步驟(1)將慣性量測信息通過補償陀螺輸出數據和姿態解算得到實時的載體姿態信息；(2)利用天文量測信息，結合補償後的陀螺輸出數據，通過遺傳最優REQUEST方法快速獲得所需間隔的高精度天文姿態信息；(3)利用GUPF算法將天文姿態信息和載體姿態信息相融合，求解高精度的載體姿態信息，估計陀螺漂移，並反饋校正載體姿態和補償陀螺漂移補償；最終實現基於天文量測信息實時消除慣性/天文組合導航系統陀螺隨機誤差的在線修正，完成對太空飛行器的高精度組合定姿。
2.根據權利要求1所述的一種基於遺傳最優REQUEST和GUPF的高精度組合定姿方法，其特徵在於所述步驟(1)中的實時的載體姿態信息解算如下(1)設定初始姿態為(0θ0γ0)，計算得出初始姿態四元數q(0)陣 (2)更新矩陣為q(n+1)={cos2I+sin2[]}q(n)]]>n為第n時刻，I為單位四元數，Δφ＝[ΔφXΔφYΔφZ]為陀螺輸出角增量，[ΔΦ]為[]=0-X-Y-ZX0Z-YY-Z0XZY-X0]]>(3)由q(n+1)＝[q1q2q3q4]T，計算姿態餘弦陣C為C=q42+q12-q22-q322(q1q2+q4q3)2(q1q3-q4q2)2(q1q2-q4q3)q42-q12+q22-q322(q2q3+q4q1)2(q1q3+q4q2)2(q2q3-q4q1)q42-q12-q22+q32]]>(4)由方向餘弦陣C即可得出實時載體的姿態信息。
3.根據權利要求1所述的一種基於遺傳最優REQUEST和GUPF的高精度組合定姿方法，其特徵在於所述步驟(2)中利用遺傳最優REQUEST方法，將所需間隔的天文量測信息，結合補償後的陀螺輸出數據，快速求解高精度天文姿態信息的步驟為(1)定義3×3的矩陣Bk和Sk，3×1的列向量z和標量σk，k＝1...n...，Bk=wkvkT,Sk=Bk+BkT,]]>z＝wk×vk，σk＝tr(Bk)；其中，wk為k時刻觀測的星光在星敏感器坐標系中的坐標矢量，vk為k時刻該星光在地心慣性坐標系中的參考矢量；Kk/k=kmk-1mkKk/k-1+1mkKk]]>上式中Kk/k-1為在k-1時刻估計k時刻的K矩陣值，mk和mk+1是使Kk/k的最大特徵值接近於1的標量權值係數，係數ρk是漸消記憶因子，δKk用第一個觀測向量根據下式計算Kk=Sk-kIkzkzkTk]]>(2)通過遺傳算法選取最優的漸消記憶因子ρk，遺傳算法的自適應評價函數定義為f(ρk)＝min(Kk/kmk-mk-1Kk/k-1+δKk)；(3)時間更新Kk+1/k=kKk/kkT]]>Pk+1/k=kPk/kkT+Qk]]>式中，Φk由補償後的陀螺輸出數據ωk計算得到，代表四元數動力學差分方程中的4×4正交轉移矩陣，k=-[k]k-kT0,k=k+1k,2k,3T,[k]=0-k,3k,2k,30-k,1-k,2k,10;]]>(4)量測更新k+1=mk2tr(Pk+1/k)mk2tr(Pk+1/k)+mk+12tr(Rk+1)]]>mk+1＝(1-ρk+1)mk+ρk+1δmk+1Kk+1/k+1=(1-k+1)mkmk+1Kk+1/k+k+1mk+1mk+1kKk+1]]>Pk+1/k+1=[(1-k+1)mkmk+1]2Pk+1/k+(k+1mk+1mk+1)2Rk+1]]>(5)求解天文姿態信息qk+1＝Φkqk由qk+1＝[q1q2q3q4]T，計算姿態餘弦陣C為C=q42+q12-q22-q322(q1q2+q4q3)2(q1q3-q4q2)2(q1q2-q4q3)q42-q12+q22-q322(q2q3+q4q1)2(q1q3+q4q2)2(q2q3-q4q1)q42-q12-q22+q32]]>(6)由方向餘弦陣C即可得出高精度天文姿態信息。
4.根據權利要求1所述的一種基於遺傳最優REQUEST和GUPF的高精度組合定姿方法，其特徵在於所述步驟(3)中利用GUPF算法將天文姿態信息和載體姿態信息相融合，完成對太空飛行器高精度組合定姿步驟為(1)t＝0時，初始化對初始的先驗概率密度p(x0)進行採樣，生成N個服從p(x0)分布的粒子x0(i)，i＝1，...，N，其均值和方差滿足 P0(i)=E[(x0(i)-x0(i))(x0(i)-x0(i))T]]]>(2)t≥1時，步驟如下①採樣步驟用Unscented卡爾曼濾波更新粒子{xk-1(i)，Pk-1(i)}得到{xk(i)，Pk(i)}，採樣x^k(i)~q(xk(i)|x0:k-1(i),y1:k)=N(xk(i),Pk(i)),]]>i＝1，...，N②計算權重w~k(i)=w~k-1(i)p(yk|x^k(i))p(x^k(i)|xk-1(i))q(x^k(i)|x0:k-1(i),y1:k),]]>歸一化權重wk(i)=w~k(i)/i=1Nw~k(i);]]>③重採樣步驟從離散分布的{xk(i),wk(xk(i))}i=1,,N]]>中進行N次重採樣，得到一組-新的粒子{xk(i*)，1/N}，仍為p(xk|y0:k)的近似表示；④通過遺傳算法從得到的這組新的粒子中選取優等粒子，剔出低等的粒子，以解決粒子枯竭問題，其自適應評價函數定義為f=(u-min(1,p(yk|xk*(i))p(yk|xk(i*))));]]>式中，u為0～1之間的一個隨機數，xk*(i)～p(xk|xk-1i)為從馬爾科夫鏈中進行的採樣；⑤輸出按照最小方差準則，載體姿態的最優估計值就是條件分布的均值，即x^k=i=1Nwkixki]]>pk=i=1Nwki(xki-x^k)(xki-x^k)T.]]>
全文摘要
一種基於遺傳最優REQUEST和GUPF的高精度組合定姿方法，本發明涉及一種基於恆星敏感器的慣性/天文組合定姿方法。該方法首先將慣性量測信息通過補償陀螺輸出數據和姿態解算得到實時的載體姿態信息；其次利用天文量測信息，結合補償後的陀螺輸出數據，通過基於遺傳最優REQUEST方法快速獲得特定間隔的高精度天文姿態信息；最後利用Genetic Unscented Particle Filter(GUPF)算法將天文姿態信息和載體姿態信息相融合，解決系統非線性和噪聲非高斯問題，求解高精度載體姿態信息，估計陀螺漂移，並反饋校正載體姿態和補償陀螺漂移補償；最終實現基於天文量測信息實時消除慣性/天文組合導航系統陀螺隨機誤差的在線修正，完成對太空飛行器的長時間、高精度組合定姿。
文檔編號G05D1/08GK101078936SQ200710100328
公開日2007年11月28日 申請日期2007年6月8日 優先權日2007年6月8日
發明者房建成, 全偉, 鍾慧敏, 徐帆, 王科 申請人:北京航空航天大學