專利名稱：：基于簡化自適應(yīng)濾波的磁測微小衛(wèi)星姿態(tài)確定方法
技術(shù)領(lǐng)域：
：本發(fā)明涉及一種磁測微小衛(wèi)星姿態(tài)確定方法，尤其涉及一種基于簡化自適應(yīng)濾波的磁測微小衛(wèi)星姿態(tài)確定方法。
背景技術(shù)：
：微小衛(wèi)星技術(shù)是當(dāng)前國際空間技術(shù)研究的熱點(diǎn)。微小衛(wèi)星發(fā)射靈活，成本低、功能密度高、研制周期短，具有獨(dú)特的優(yōu)勢，發(fā)達(dá)國家在該
技術(shù)領(lǐng)域：
走在了前列，民用與軍用領(lǐng)域都從中受益。地磁場是中、低軌道環(huán)境中的特殊自然資源，三軸磁強(qiáng)計(jì)具有體積小、重量輕、成本低、性能可靠、沒有視場限制的優(yōu)點(diǎn)，并能夠提供全天侯、實(shí)時(shí)連續(xù)的自主導(dǎo)航信息，與地磁相互作用實(shí)現(xiàn)星體控制的磁力矩器也具有成熟可靠、成本低功耗小等特點(diǎn)。磁強(qiáng)計(jì)和磁力矩器是目前國內(nèi)外發(fā)射的低軌小衛(wèi)星上的最基本配置。目前，地磁場已有相當(dāng)好的磁場模型，采用高斯球諧函數(shù)來描述地磁場模型，這樣磁場的強(qiáng)度和方向是位置的函數(shù)。當(dāng)確定了衛(wèi)星軌道參數(shù)，就可求得地磁場矢量在地理坐標(biāo)系的投影，利用地理坐標(biāo)系與軌道坐標(biāo)系的轉(zhuǎn)換關(guān)系，得到地磁矢量在軌道坐標(biāo)系的分量，進(jìn)一步借助姿態(tài)方向余弦矩陣給出地磁矢量在星體坐標(biāo)系的分量，將其與三軸磁強(qiáng)計(jì)敏感到的地磁矢量的分量進(jìn)行比較，就建立起三軸磁強(qiáng)計(jì)與衛(wèi)星姿態(tài)動(dòng)力學(xué)的數(shù)學(xué)關(guān)系，采用卡爾曼濾波器遞推可以得到衛(wèi)星姿態(tài)角。考慮到小衛(wèi)星的性價(jià)比需求，要求盡可能使用較低的成本來實(shí)現(xiàn)小衛(wèi)星自主導(dǎo)航系統(tǒng)設(shè)計(jì)，因此，利用豐富的地磁場資源，并結(jié)合磁強(qiáng)計(jì)測量信息進(jìn)行低軌小衛(wèi)星自主軌道確定，將能有效降低微小衛(wèi)星自主導(dǎo)航系統(tǒng)研制成本，滿足小衛(wèi)星自主導(dǎo)航系統(tǒng)的基本需求。但是，鑒于地磁場的可變性及磁強(qiáng)計(jì)本身精度的限制，以及星載計(jì)算機(jī)的性能限制，使用單磁強(qiáng)計(jì)進(jìn)行衛(wèi)星自主導(dǎo)航的定軌精度普遍較低，耗時(shí)較長。因此需要開展磁定姿技術(shù)研究，研究出復(fù)雜度適中，適合星上計(jì)算的磁導(dǎo)航數(shù)學(xué)模型與濾波處理方法，以節(jié)約星上計(jì)算資源。
發(fā)明內(nèi)容本發(fā)明目的是針對現(xiàn)有技術(shù)存在的缺陷提供一種基于簡化自適應(yīng)濾波的磁測微小衛(wèi)星姿態(tài)確定方法。本發(fā)明為實(shí)現(xiàn)上述目的，采用如下技術(shù)方案本發(fā)明基于簡化自適應(yīng)濾波的磁測微小衛(wèi)星姿態(tài)確定方法，包括如下步驟第一步建立衛(wèi)星姿態(tài)運(yùn)動(dòng)模型四元素運(yùn)動(dòng)學(xué)微分方程為(1)q=[qiq2q3q4]T為四元素，上標(biāo)T表示轉(zhuǎn)置，《為星體坐標(biāo)系相對軌道坐標(biāo)系的角速率，$表示q的微分；求導(dǎo)，并忽略二階小量可得泌為《估計(jì)值，Ao=[AOjAo2Ao3]To誤差，6q=[66q26q3Sq4]T為四元素誤差，Sq13=[Sqi6q2Sq3]T，上標(biāo)表示微分；衛(wèi)星在外力矩的作用下，發(fā)生姿態(tài)的改變，外力矩-間擾動(dòng)力矩；-般包括衛(wèi)星的控制力矩與空其中二為(0的微分，Nt為空間擾動(dòng)力矩，Nr為控制力矩，《為d道坐標(biāo)系的角速率，J為星體慣量矩陣，h星體偏置動(dòng)量；相應(yīng)的誤差小量方程為K本坐標(biāo)系相對軌由式(2)、⑷聯(lián)合構(gòu)成狀態(tài)方程，系統(tǒng)狀態(tài)量為X=[86q26q3AcoAco2Aco3]T；第二步改進(jìn)磁強(qiáng)計(jì)量測模型量測模型為em=(BmXBb)+(Bm-Bb)(5)其中為磁場誤差，Bffl為磁強(qiáng)計(jì)測量值，Bb=臥B2B3]t為本體磁矢量估計(jì)值，同時(shí)為磁強(qiáng)計(jì)量測噪聲；磁強(qiáng)計(jì)測量殘差為Bffl=Bb+vffl(6)則量測噪聲為v'm=vmXBb+vm當(dāng)狀態(tài)量為X時(shí)，量測陣為H=[H'第三步量測噪聲模型的自適應(yīng)修正_]v'ffl=vfflXBb+vffl磁強(qiáng)計(jì)測量噪聲vm為均值Y(nT)白噪聲，則。3*3]3*6Diag表示對角矩陣，ir22分別表示觀測噪聲方差陣及=Diag量測噪聲方差陣對角線元素；[r:r2r3]T=_(BbX)[YYY]T+[YYY]T，Y為vm均值，nT表示噪聲單位納特；5第四步濾波解算采用卡爾曼濾波算法進(jìn)行濾波解算確定衛(wèi)星姿態(tài)。本發(fā)明利用三軸磁強(qiáng)計(jì)量測的地磁場矢量在儀表坐標(biāo)系中的分量，通過卡爾曼濾波器可以確定低軌道衛(wèi)星的姿態(tài)。本文提出了一種基于簡化濾波算法的磁控微小衛(wèi)星姿態(tài)確定算法。對磁強(qiáng)計(jì)量測模型做出了分析和改進(jìn)，在此基礎(chǔ)上簡化了濾波增益陣的計(jì)算。在不明顯影響精度的情況下，使得計(jì)算量得到了顯著降低，數(shù)學(xué)仿真表明算法是有效的，三軸磁強(qiáng)計(jì)定姿方案可以滿足中等姿態(tài)精度要求。具有良好的工程意義和應(yīng)用前景。圖1:滾動(dòng)角曲線；圖2:俯仰角曲線；圖3:偏航角曲線。具體實(shí)施例方式下面結(jié)合附圖對發(fā)明的技術(shù)方案進(jìn)行詳細(xì)說明本發(fā)明中，磁強(qiáng)計(jì)作為基本姿態(tài)器件，提供長期的低精度姿態(tài)信息；當(dāng)衛(wèi)星上能源緊張或處在某種不安全狀態(tài)下，磁強(qiáng)計(jì)提供基本信息，維持衛(wèi)星的生命。1.磁測微小衛(wèi)星軌道計(jì)算微小衛(wèi)星采用GPS作為軌道信息的唯一來源，全天時(shí)提供高精度軌道信息。GPS的輸出頻率為1Hz，由于姿態(tài)輸出頻率高達(dá)10Hz，或者GPS由于可見星不夠而不可用時(shí)，需要對軌道信息進(jìn)行預(yù)測，預(yù)測考慮J2攝動(dòng)項(xiàng)，以保證足夠的精度；由于GPS輸出位置和速度信息，為運(yùn)算方便，軌道參數(shù)選擇位置速度參數(shù)。2.磁強(qiáng)計(jì)姿態(tài)確定2.1衛(wèi)星姿態(tài)運(yùn)動(dòng)模型四元素運(yùn)動(dòng)學(xué)微分方程為q=[qiq2q3q4]T為四元素，上標(biāo)T表示轉(zhuǎn)置，《為星體坐標(biāo)系相對軌道坐標(biāo)系的角速率…表示q的微分。對式(1)進(jìn)行求導(dǎo)，并忽略二階小量可得1為“估計(jì)值，Ao=[AOjAo2Ao3]To誤差，6q=[66q26q3Sq4]T為四元素誤差，Sq13=[Sqi6q2Sq3]T，上標(biāo).表示微分。衛(wèi)星在外力矩的作用下，發(fā)生姿態(tài)的改變。外力矩一般包括衛(wèi)星的控制力矩與空間擾動(dòng)力矩。}(3)其中二為(0的微分，Nt為空間擾動(dòng)力矩；為控制力矩，《為(道坐標(biāo)系的角速率，J為星體慣量矩陣，h星體偏置動(dòng)量。相應(yīng)的誤差小量方程為A0)=J1{[(J+x]-(Sx)jjAco+J'1Nt(4)(2)、(4)聯(lián)合構(gòu)成狀態(tài)方程的話，系統(tǒng)狀態(tài)量為X=[66q26q3AcoAco2Aco3]T2.2改進(jìn)磁強(qiáng)計(jì)測量模型定義量測模型為K本坐標(biāo)系相對軌e=(BfflXBb)+(B-Bb)(5)其中em為磁場誤差，Bffl為磁強(qiáng)計(jì)測量值，Bb=臥B2B3]t為本體磁矢量估計(jì)值，同時(shí)定義為磁強(qiáng)計(jì)量測噪聲。磁強(qiáng)計(jì)測量殘差為Bffl=Bb+vffl(6)則effl=((2(BbX)6q13+Bb+vffl)XBb)+((2(BbX)5q13+Bb+vffl)_Bb)=-2[(BbX)(BbX)]Sq13+vmXBb+2(BbX)6q13+vm=H'8q13+v'量測噪聲為v'm=vmXBb+vm當(dāng)狀態(tài)量為X時(shí)，量測陣為H=[H'03]3。2.3觀測噪聲模型的自適應(yīng)修正v'ffl=vfflXBb+vffl假設(shè)磁強(qiáng)計(jì)測量噪聲vm為均值Y(nT)白噪聲。則Diag表示對角矩陣”2分別表示量測噪聲方差陣及=量測噪聲方差陣對角線元素；[r:r2r3]T=_(BbX)[YYY]T+[YYY]T，Y為vm均值，nT表示噪聲單位納特；2.4濾波解算卡爾曼濾波算法在建立系統(tǒng)方程、量測方程以后，就是按部就班的常規(guī)算法。本申請中，系統(tǒng)量測噪聲方差陣為自適應(yīng)計(jì)算，系統(tǒng)狀態(tài)噪聲方差陣則根據(jù)系統(tǒng)實(shí)際情況取常值就可以，并沒有特別算法。采用擴(kuò)展卡爾曼濾波器進(jìn)行濾波，建立離散的擴(kuò)展卡爾曼濾波方程為式中之.表示tk時(shí)刻的狀態(tài)對tk+1時(shí)刻的狀態(tài)的最優(yōu)預(yù)測估計(jì)值；Pk+l|k表示最優(yōu)預(yù)測估值誤差協(xié)方差陣；之+1|t+1表示tk+1時(shí)刻的狀態(tài)的最優(yōu)實(shí)時(shí)估計(jì)值；Pk+l|k+1表示最優(yōu)濾波誤差協(xié)方差陣沖k+1|k表示狀態(tài)矢量X從tk時(shí)間轉(zhuǎn)移到tk+1時(shí)刻的狀態(tài)轉(zhuǎn)移矩陣；T表示采樣周期；Hk+1表示tk+1時(shí)刻觀測矢量Zk+1與tk+1時(shí)刻的狀態(tài)矢量Xk+1間的量測系數(shù)矩陣；Qk表示系統(tǒng)狀態(tài)噪聲方差陣；&+1表示量測噪聲方差陣。解算后令只保留對角線元素，K陣簡化為對角陣，濾波無矩陣求逆運(yùn)算。仿真結(jié)果假設(shè)微小衛(wèi)星為太陽同步軌道衛(wèi)星，軌道高度約500km，姿態(tài)控制系統(tǒng)由星載計(jì)算機(jī)、三軸磁強(qiáng)計(jì)、偏置動(dòng)量輪及三軸磁力矩器等構(gòu)成。軌道測量位置誤差優(yōu)于10km、速度誤差優(yōu)于2ms。衛(wèi)星的剩磁矩為0.3Am2，在各個(gè)軸上的分量是diagAm2。其余干擾力矩設(shè)為白噪聲，量級(jí)為5e-7Nm，濾波時(shí)噪聲參數(shù)調(diào)整為5e-6Nm。衛(wèi)星的三軸轉(zhuǎn)動(dòng)慣量為diag[1.031.051.01]kgm2。磁強(qiáng)計(jì)的測量精度為300nT(1o)，輸出頻率0.5Hz。由圖1圖3與相關(guān)數(shù)據(jù)分析可得，三軸指向精度滾動(dòng)角優(yōu)于2.0°，俯仰角優(yōu)于2.0°，偏航角優(yōu)于2.0°。權(quán)利要求一種基于簡化自適應(yīng)濾波的磁測微小衛(wèi)星姿態(tài)確定方法，其特征在于包括如下步驟第一步建立衛(wèi)星姿態(tài)運(yùn)動(dòng)模型四元素運(yùn)動(dòng)學(xué)微分方程為<mrow><mover><mi>q</mi><mo>·</mo></mover><mo>=</mo><mfrac><mn>1</mn><mn>2</mn></mfrac><mo>[</mo><mi>q</mi><mo>×</mo><mo>]</mo><mi>ω</mi><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>1</mn><mo>)</mo></mrow></mrow>q＝[q1q2q3q4]T為四元素，上標(biāo)T表示轉(zhuǎn)置，ω為星體坐標(biāo)系相對軌道坐標(biāo)系的角速率，上標(biāo)·表示微分，下同；對式(1)進(jìn)行求導(dǎo)，并忽略二階小量可得<mrow><mfencedopen='{'close=''><mtable><mtr><mtd><mi>δ</mi><msub><mover><mi>q</mi><mo>·</mo></mover><mn>13</mn></msub><mo>=</mo><mo>-</mo><mo>[</mo><mover><mi>ω</mi><mo>^</mo></mover><mo>×</mo><mo>]</mo><mi>δ</mi><msub><mi>q</mi><mn>13</mn></msub><mo>+</mo><mfrac><mn>1</mn><mn>2</mn></mfrac><mi>Δω</mi></mtd></mtr><mtr><mtd><mi>δ</mi><msub><mover><mi>q</mi><mo>·</mo></mover><mn>4</mn></msub><mo>=</mo><mn>0</mn></mtd></mtr></mtable></mfenced><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>2</mn><mo>)</mo></mrow></mrow>為ω估計(jì)值，Δω＝[Δω1Δω2Δω3]T為ω誤差，δq＝[δq1δq2δq3δq4]T為四元素誤差，δq13＝[δq1δq2δq3]T；衛(wèi)星在外力矩的作用下，發(fā)生姿態(tài)的改變，外力矩包括衛(wèi)星的控制力矩Nr與空間擾動(dòng)力矩Nτ；<mrow><mover><mi>ω</mi><mo>·</mo></mover><mo>=</mo><msup><mi>J</mi><mrow><mo>-</mo><mn>1</mn></mrow></msup><mo>{</mo><msub><mi>N</mi><mi>r</mi></msub><mo>-</mo><mi>ω</mi><mo>×</mo><mrow><mo>(</mo><mi>Jω</mi><mo>+</mo><mi>h</mi><mo>)</mo></mrow><mo>+</mo><msub><mi>N</mi><mi>τ</mi></msub><mo>}</mo><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>3</mn><mo>)</mo></mrow></mrow>其中J為星體慣量矩陣，h星體偏置動(dòng)量；星體坐標(biāo)系相對軌道坐標(biāo)系的角速率相應(yīng)的誤差小量方程為<mrow><mi>Δ</mi><mover><mi>ω</mi><mo>·</mo></mover><mo>=</mo><msup><mi>J</mi><mrow><mo>-</mo><mn>1</mn></mrow></msup><mo>{</mo><mo>[</mo><mrow><mo>(</mo><mi>J</mi><mover><mi>ω</mi><mo>^</mo></mover><mo>+</mo><mi>h</mi><mo>)</mo></mrow><mo>×</mo><mo>]</mo><mo>-</mo><mrow><mo>(</mo><mover><mi>ω</mi><mo>^</mo></mover><mo>×</mo><mo>)</mo></mrow><mi>J</mi><mo>}</mo><mo>·</mo><mi>Δω</mi><mo>+</mo><msup><mi>J</mi><mrow><mo>-</mo><mn>1</mn></mrow></msup><mo>·</mo><msub><mi>N</mi><mi>τ</mi></msub><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>4</mn><mo>)</mo></mrow></mrow>由式(2)、(4)聯(lián)合構(gòu)成狀態(tài)方程，系統(tǒng)狀態(tài)量為X＝[δq1δq2δq3Δω1Δω2Δω3]T；第二步改進(jìn)磁強(qiáng)計(jì)量測模型量測模型為em＝(Bm×Bb)+(Bm-Bb)(5)其中em為磁場誤差，Bm為磁強(qiáng)計(jì)測量值，Bb＝[B1B2B3]T為本體磁矢量估計(jì)值，同時(shí)vm為磁強(qiáng)計(jì)量測噪聲；磁強(qiáng)計(jì)測量殘差為Bm＝Bb+vm(6)則em＝((2(Bb×)δq13+Bb+vm)×Bb)+((2(Bb×)δq13+Bb+vm)-Bb)＝-2[(Bb×)(Bb×)]δq13+vm×Bb+2(Bb×)δq13+vm＝H′δq13+v′m<mrow><msup><mi>H</mi><mo>′</mo></msup><mo>=</mo><mo>-</mo><mn>2</mn><mfencedopen='['close=']'><mtable><mtr><mtd><mo>-</mo><mrow><mo>(</mo><msubsup><mi>B</mi><mn>2</mn><mn>2</mn></msubsup><mo>+</mo><msubsup><mi>B</mi><mn>3</mn><mn>2</mn></msubsup><mo>)</mo></mrow></mtd><mtd><mn>0</mn></mtd><mtd><mn>0</mn></mtd></mtr><mtr><mtd><mn>0</mn></mtd><mtd><mo>-</mo><mrow><mo>(</mo><msubsup><mi>B</mi><mn>1</mn><mn>2</mn></msubsup><mo>+</mo><msubsup><mi>B</mi><mn>3</mn><mn>2</mn></msubsup><mo>)</mo></mrow></mtd><mtd><mn>0</mn></mtd></mtr><mtr><mtd><mn>0</mn></mtd><mtd><mn>0</mn></mtd><mtd><mo>-</mo><mrow><mo>(</mo><msubsup><mi>B</mi><mn>1</mn><mn>2</mn></msubsup><mo>+</mo><msubsup><mi>B</mi><mn>2</mn><mn>2</mn></msubsup><mo>)</mo></mrow></mtd></mtr></mtable></mfenced></mrow>量測噪聲為v′m＝vm×Bb+vm當(dāng)狀態(tài)量為X時(shí)，量測陣為H＝[H′03*3]3*6；第三步量測噪聲模型的自適應(yīng)修正v′m＝vm×Bb+vm磁強(qiáng)計(jì)測量噪聲vm為均值Y(nT)白噪聲，則量測噪聲方差陣Diag表示對角矩陣，分別表示量測噪聲方差陣對角線元素；[r1r2r3]T＝-(Bb×)[YYY]T+[YYY]T，Y為vm均值，nT表示噪聲單位納特；第四步濾波解算采用卡爾曼濾波算法進(jìn)行濾波解算確定衛(wèi)星姿態(tài)。FSA00000155810600013.tif,FSA00000155810600022.tif,FSA00000155810600023.tif全文摘要本發(fā)明公布了一種基于簡化自適應(yīng)濾波的磁測微小衛(wèi)星姿態(tài)確定方法，包括如下步驟第一步建立衛(wèi)星姿態(tài)運(yùn)動(dòng)模型，第二步改進(jìn)磁強(qiáng)計(jì)測量模型，第三步量測噪聲模型的自適應(yīng)修正，第四步濾波解算。本發(fā)明對磁強(qiáng)計(jì)量測模型做出了分析和改進(jìn)，在此基礎(chǔ)上簡化了濾波增益陣的計(jì)算。在不明顯影響精度的情況下，使得計(jì)算量得到了顯著降低，數(shù)學(xué)仿真表明算法是有效的，三軸磁強(qiáng)計(jì)定姿方案可以滿足中等姿態(tài)精度要求。具有良好的工程意義和應(yīng)用前景。文檔編號(hào)G01C21/00GK101852605SQ20101019785公開日2010年10月6日申請日期2010年6月10日優(yōu)先權(quán)日2010年6月10日發(fā)明者華冰,康國華,熊智,程月華,郁豐申請人:南京航空航天大學(xué)