專利名稱：：一種基于esoqpf和ukf主從濾波的微小衛(wèi)星姿態(tài)確定方法
技術(shù)領(lǐng)域：
：本發(fā)明涉及一種基于ESOQPF(EstimatoroftheQuaternionParticleFilter,四元數(shù)粒子濾波估計(jì)器)和UKF(UnscentedKalmanFilter，Unscented卡爾曼濾波)主從濾波的微小衛(wèi)星姿態(tài)確定方法，適用于有陀螺配置模式下，基于矢量觀測的微小衛(wèi)星姿態(tài)確定。本發(fā)明屬于航天器姿態(tài)確定領(lǐng)域。
背景技術(shù)：
：姿態(tài)確定是微小衛(wèi)星系統(tǒng)技術(shù)中的重要問題之一，其精度和實(shí)時(shí)性是影響姿態(tài)控制系統(tǒng)精度的重要因素。姿態(tài)確定的精度和實(shí)時(shí)性不僅取決于姿態(tài)敏感器的性能，還與姿態(tài)確定方法有關(guān)。姿態(tài)確定方法分為確定性算法和狀態(tài)估計(jì)法兩類。常見的確定性算法有q_method、QUEST(QuaternionEstimator,四元數(shù)估計(jì)器)、ES0Q(EstimatoroftheOptimalQuaternion，最優(yōu)四元數(shù)估計(jì)器)等。常見的基于狀態(tài)估計(jì)的姿態(tài)確定方法有KF(KalmanFilter，卡爾曼濾波)、EKF(ExtendedKalmanFilter，擴(kuò)展卡爾曼濾波)、UKF和PF(ParticleFilter，粒子濾波)等。其中，UKF采用U變換來處理狀態(tài)方程和觀測方程的非線性問題，不需要計(jì)算Jacobian矩陣，計(jì)算的狀態(tài)均值和方差可以精確到三階，但UKF對(duì)于具有較強(qiáng)非線性、非高斯系統(tǒng)的狀態(tài)估計(jì)就達(dá)不到期望的效果。針對(duì)上述不足，出現(xiàn)了PF濾波方法。PF是一種利用隨機(jī)樣本或粒子來表示系統(tǒng)狀態(tài)變量的后驗(yàn)概率分布的遞推貝葉斯濾波方法，將PF應(yīng)用于姿態(tài)確定，估計(jì)精度高。但PF存在計(jì)算量大從而使定姿過程長的缺點(diǎn)，為了達(dá)到要求的估計(jì)精度，需要上百個(gè)甚至上千個(gè)粒子，且隨著狀態(tài)維數(shù)的增加，計(jì)算量會(huì)劇增，嚴(yán)重影響姿態(tài)確定的實(shí)時(shí)性。近幾年，已有學(xué)者在提高姿態(tài)確定實(shí)時(shí)性方面進(jìn)行了研究。其中，YaakovOshman提出了GA-QPF(GeneticsAlgorithm-QuaternionParticleFilter，遺傳算法-四元數(shù)粒子濾波)算法，該算法將姿態(tài)四元數(shù)與陀螺漂移分開估計(jì)，利用QPF(QuaternionParticleFilter，四元數(shù)粒子濾波)算法估計(jì)四元數(shù)，GA(GeneticsAlgorithm，遺傳算法)算法估計(jì)陀螺漂移，進(jìn)而降低了粒子濾波狀態(tài)維數(shù)，提高實(shí)時(shí)性。其中，QPF算法直接利用加權(quán)四元數(shù)粒子集進(jìn)行時(shí)間更新和觀測更新處理，并結(jié)合q_method計(jì)算姿態(tài)四元數(shù)。但是qjiiethod估計(jì)姿態(tài)四元數(shù)時(shí)需要求出Davenport(達(dá)文波特)矩陣的所有特征值，計(jì)算量大，從而定姿過程長。
發(fā)明內(nèi)容本發(fā)明的技術(shù)解決問題是(1)克服了基于粒子濾波的定姿方法計(jì)算量大從而定姿過程長，姿態(tài)確定實(shí)時(shí)性差的不足，利用ESOQPF和UKF進(jìn)行主從濾波，將姿態(tài)四元數(shù)與陀螺漂移分開估計(jì)，在保證姿態(tài)確定精度的同時(shí)提高實(shí)時(shí)性；(2)克服了QPF在估計(jì)姿態(tài)四元數(shù)時(shí)采用q_method算法引起的計(jì)算量大從而定姿過程長的不足，ESOQPF在估計(jì)姿態(tài)四元數(shù)時(shí)采用ES0Q2算法，進(jìn)一步降低計(jì)算量，縮短了定姿過程。本發(fā)明的技術(shù)解決方案是基于ES0QPF和UKF主從濾波的微小衛(wèi)星姿態(tài)確定方法，將ES0QPF作為主濾波器估計(jì)姿態(tài)四元數(shù)，UKF作為從濾波器估計(jì)陀螺漂移，即姿態(tài)四元數(shù)與陀螺漂移分開估計(jì)。但是，主從濾波器之間又存在相互聯(lián)系主濾波器估計(jì)第k時(shí)刻姿態(tài)四元數(shù)時(shí)，要利用從濾波器第k-1時(shí)刻估計(jì)的陀螺漂移；從濾波器估計(jì)第k時(shí)刻陀螺漂移時(shí)，要利用主濾波器第k-1時(shí)刻估計(jì)的姿態(tài)四元數(shù)。此外，ES0QPF主濾波器估計(jì)姿態(tài)四元數(shù)時(shí)采用一種計(jì)算量小的確定性算法ES0Q2算法，進(jìn)一步提高姿態(tài)確定的實(shí)時(shí)性。本發(fā)明的具體步驟如下(1)建立微小衛(wèi)星系統(tǒng)狀態(tài)空間模型a.姿態(tài)四元數(shù)運(yùn)動(dòng)學(xué)方程qk+1=Q(wk)qk式中，qk和qk+1分別為第k時(shí)刻和第k+1時(shí)刻衛(wèi)星本體坐標(biāo)系相對(duì)于參考坐標(biāo)系的姿態(tài)四元數(shù)，gA=[qkTq4kf&和q4k分別為四元數(shù)的矢量部分和標(biāo)量部分；《k為第k時(shí)刻本體坐標(biāo)系相對(duì)參考坐標(biāo)系姿態(tài)角速度。假設(shè)在采樣周期At內(nèi)《k保持不變，則rn、cos(0.5||^||A)/3><3-[^X]￥kO.{cok)=T,丨丨丨丨、式中，I3X3*3X3單位陣；V.7；[￥kX]^；…的反對(duì)稱矩陣。wb.陀螺數(shù)學(xué)模型cbk=cok+pk+]v’k，3k+1=3k+nu,k式中，錢為陀螺的測量輸出值；《k為真實(shí)的姿態(tài)角速度4k和3k+1分別為第k時(shí)刻和第k+i時(shí)刻的陀螺漂移；nv,k與nu,k是互不相關(guān)的零均值高斯白噪聲，nv,k和nu,k的方差分別為ov2與ou2。c.矢量觀測模型bk=A(qk)rk+8bk式中，bk為觀測矢量；rk為參考矢量；Sbk為測量噪聲，且Sbk服從概率分布Sbk~psbt；A(qk)為參考坐標(biāo)系到衛(wèi)星本體坐標(biāo)系的姿態(tài)轉(zhuǎn)移矩陣。(2)基于步驟⑴建立的系統(tǒng)狀態(tài)空間模型，進(jìn)行濾波初始化濾波初始化要基于步驟(1)建立的系統(tǒng)狀態(tài)空間模型，包括ES0QPF主濾波器初始化和UKF從濾波器初始化兩部分。ES0QPF主濾波器的初始化主要是初始化N個(gè)粒子及粒子權(quán)值；UKF從濾波器的初始化主要完成陀螺漂移估計(jì)值和估計(jì)誤差方差陣的初始化。(3)以姿態(tài)四元數(shù)為狀態(tài)變量，基于步驟(1)中的四元數(shù)運(yùn)動(dòng)學(xué)方程和矢量觀測模型建立主濾波器的狀態(tài)空間模型，利用ES0QPF估計(jì)姿態(tài)四元數(shù)。ES0QPF主濾波器估計(jì)姿態(tài)四元數(shù)的基本步驟包括從第k-1時(shí)刻到第k時(shí)刻的時(shí)間更新，測量更新即粒子的權(quán)值更新，計(jì)算Davenport矩陣K，利用ES0Q2算法估計(jì)第k時(shí)刻姿態(tài)四元數(shù)么。此外，主濾波器估計(jì)第k時(shí)刻姿態(tài)四元數(shù)么時(shí)用到從濾波器第k-1時(shí)刻估計(jì)出的陀螺漂移；^,。(4)四元數(shù)轉(zhuǎn)換成姿態(tài)角根據(jù)四元數(shù)與姿態(tài)角之間的關(guān)系，將步驟(3)估計(jì)出的姿態(tài)四元數(shù)么轉(zhuǎn)換為姿態(tài)四元數(shù)對(duì)應(yīng)的姿態(tài)角橫滾角外，俯仰角ek，偏航角Vk。(5)以陀螺漂移為狀態(tài)變量，基于步驟(1)中的陀螺漂移數(shù)學(xué)模型和矢量觀測模型建立從濾波器的狀態(tài)空間模型，利用UKF估計(jì)陀螺漂移。UKF從濾波器估計(jì)陀螺漂移的基本步驟包括計(jì)算Sigma點(diǎn)，計(jì)算Sigma點(diǎn)權(quán)值，時(shí)間更新，測量更新。此外，從濾波器估計(jì)第k時(shí)刻陀螺漂移及時(shí)用到主濾波器第k-1時(shí)刻估計(jì)出的姿態(tài)四元數(shù)么。(6)當(dāng)有新的觀測值時(shí)，重復(fù)步驟(3)(5)進(jìn)行姿態(tài)確定。本發(fā)明的原理是微小衛(wèi)星姿態(tài)確定的主要任務(wù)是利用星上的姿態(tài)敏感器測量所得到的信息，經(jīng)過適當(dāng)?shù)奶幚?，求得固連于衛(wèi)星本體的坐標(biāo)系相對(duì)于空間參考坐標(biāo)系的姿態(tài)。由于陀螺存在漂移，無法將陀螺的輸出值直接用于姿態(tài)估計(jì)，所以在四元數(shù)估計(jì)過程中要考慮陀螺漂移。本發(fā)明原理步驟是(1)建立微小衛(wèi)星系統(tǒng)狀態(tài)空間模型；(2)濾波初始化；(3)以姿態(tài)四元數(shù)為狀態(tài)變量，將ES0QPF作為主濾波器估計(jì)姿態(tài)四元數(shù)；(4)將估計(jì)出來的四元數(shù)轉(zhuǎn)換為姿態(tài)角；(5)以陀螺漂移為狀態(tài)變量，將UKF作為從濾波器估計(jì)陀螺漂移。此外，主從濾波器之間又存在相互聯(lián)系，主濾波器估計(jì)第k時(shí)刻姿態(tài)四元數(shù)時(shí)，要利用從濾波器第k-1時(shí)刻估計(jì)的陀螺漂移；從濾波器估計(jì)第k時(shí)刻陀螺漂移時(shí)，要利用主濾波器第k-1時(shí)刻估計(jì)的姿態(tài)四元數(shù)。本發(fā)明與現(xiàn)有技術(shù)相比的優(yōu)點(diǎn)在于本發(fā)明涉及一種基于ES0QPF和UKF主從濾波的微小衛(wèi)星姿態(tài)確定方法，在保證姿態(tài)估計(jì)精度的同時(shí)，從兩個(gè)方面降低了計(jì)算量，縮短了定姿過程。(1)從整體上講，將ES0QPF作為主濾波器估計(jì)姿態(tài)四元數(shù)，UKF作為從濾波器估計(jì)陀螺漂移，即姿態(tài)四元數(shù)與陀螺漂移分開估計(jì)，避免了粒子濾波狀態(tài)維數(shù)高而引起計(jì)算量大從而定姿過程長的問題，在保證姿態(tài)確定精度的同時(shí)提高姿態(tài)確定方法的實(shí)時(shí)性；(2)對(duì)于主濾波器來講，ES0QPF估計(jì)姿態(tài)四元數(shù)時(shí)，將QPF算法與ES0Q2算法相結(jié)合，采用ES0Q2算法直接計(jì)算Davenport矩陣的最大特征值所對(duì)應(yīng)的特征向量，克服了QPF中采用q_method計(jì)算Davenport矩陣所有特征值引起的計(jì)算量大從而定姿過程長的不足，進(jìn)一步提高姿態(tài)確定的實(shí)時(shí)性。圖1為本發(fā)明的實(shí)現(xiàn)流程圖；圖2為ES0QPF與UKF主從濾波器在第k時(shí)刻的計(jì)算流程圖。具體實(shí)施例方式如圖1所示，本發(fā)明采用ES0QPF和UKF主從濾波確定微小衛(wèi)星姿態(tài)，具體步驟如下1.建立微小衛(wèi)星系統(tǒng)狀態(tài)空間模型a.姿態(tài)運(yùn)動(dòng)學(xué)方程姿態(tài)四元數(shù)運(yùn)動(dòng)學(xué)方程的離散形式為qk+1=Q(cok)qk(1)式中，qk和qk+1分別為第k時(shí)刻和第k+1時(shí)刻衛(wèi)星本體坐標(biāo)系相對(duì)于參考坐標(biāo)系的姿態(tài)四元數(shù)，&=[qkTq4k]T&和q4k分別為四元數(shù)的矢量部分和標(biāo)量部分；《k是本體坐標(biāo)系相對(duì)參考坐標(biāo)系姿態(tài)角速度。假設(shè)在采樣周期At內(nèi)《k保持不變，則rn、「cos(0.5吟A,)4<3-^]QK)=；,式中，I3X3*3X3單位陣；V.‘；；[￥kX]^；…的反對(duì)稱矩陣。Wb.陀螺數(shù)學(xué)模型陀螺的數(shù)學(xué)模型采用下式描述&k^(Dk+Pk+T]vk，旦k+i=旦k+nuk(2)式中，而為陀螺的測量輸出值；《k為真實(shí)的姿態(tài)角速度4k和3k+1分別為第k時(shí)刻和第k+i時(shí)刻的陀螺漂移；nv,k與nu,k是互不相關(guān)的零均值高斯白噪聲，nv,k和nu,k的方差分別為ov2與ou2。c.矢量觀測模型bk=A(qk)rk+8bk式中，bk為測量矢量；rk為參考矢量；Sbk為測量噪聲，且Sbk服從概率分布Sbk~psh；A(qk)為參考坐標(biāo)系到衛(wèi)星本體坐標(biāo)系的姿態(tài)轉(zhuǎn)移矩陣，表示為A{qk)=[(q,kf-qkrqk]hx3+2qkqkT-2qik[qkx](3)式中，[^x]為四元數(shù)矢量部分&的反對(duì)稱矩陣。2.基于步驟(1)建立的系統(tǒng)狀態(tài)空間模型，進(jìn)行濾波初始化濾波初始化要基于步驟(1)建立的系統(tǒng)狀態(tài)空間模型，包括ES0QPF主濾波器的初始化和UKF從濾波器的初始化兩部分。a.ES0QPF主濾波器初始化四元數(shù)粒子初始化為qQ(i)，i=1，…，N；各個(gè)粒子的權(quán)值初始化為WQ(i)=1/N，i=1，…，N;N為粒子個(gè)數(shù)。b.UKF從濾波器初始化=E[J%]’Pfio=E[{P0-成)(爲(wèi)-A)"](4)式中，0^和&分別為初始時(shí)刻陀螺漂移的真值和估計(jì)值；&。為初始估計(jì)誤差方差陣；E[*]表示數(shù)學(xué)期望。3.ES0QPF估計(jì)姿態(tài)四元數(shù)以姿態(tài)四元數(shù)為狀態(tài)變量，基于步驟(1)中的四元數(shù)運(yùn)動(dòng)學(xué)方程和矢量觀測模型建立主濾波器的狀態(tài)空間模型狀態(tài)方程&⑶觀測方程bk=A(qk)rk+6bk(6)由于陀螺存在漂移，其輸出值不能直接用于姿態(tài)估計(jì)，所以在四元數(shù)估計(jì)過程中要將陀螺漂移估計(jì)出來。實(shí)際計(jì)算過程中，主濾波器估計(jì)第k時(shí)刻的姿態(tài)四元數(shù)時(shí)要利用從濾波器第k-1時(shí)刻估計(jì)的陀螺漂移。ES0QPF主濾波器(1)估計(jì)姿態(tài)四元數(shù)基本步驟如下a.QPF算法計(jì)算Davenport矩陣K利用第k_l時(shí)刻的四元數(shù)粒子qnG)、歸一化的粒子權(quán)值和第k時(shí)刻的觀測值bk計(jì)算Davenport矩陣K。1)時(shí)間更新根據(jù)式(5)由第k_l時(shí)刻的四元數(shù)粒子c^^i)求第k時(shí)刻各粒子的預(yù)測值2)測量更新測量更新即粒子權(quán)值更新，利用第k_l時(shí)刻的歸一化的粒子權(quán)值和第k時(shí)刻的觀測值bk計(jì)算第k時(shí)刻的粒子權(quán)值Wk(i)。Wk(i)ccpbM{bk\qk^(i)Wk-X(})=Psh(bk-A{qk{k_x(i))rk)Wk,x(i)(7)式中，i=1，…，N；A▲仇!“)為‘的似然概率；;X汍-氺知—々))。)為h-成么|^(0)燦勺概率。將粒子權(quán)值Wk⑴歸一化Wk(0=Wk(0/…,N(8)4)計(jì)算Davenport矩陣KBk+Bkr~I3xMBk)z(9)其中，代矻(10)/=1厶z=(11)Bk{3,2)~Bk(2,3)叢(1,3)-叢(3,1)Bk(2,l)-Bk(l,2)_式中，tr(Bk)為矩陣Bk的跡。b.利用ES0Q2估計(jì)姿態(tài)四元數(shù)么第k時(shí)刻四元數(shù)估計(jì)值么為矩陣K最大特征值所對(duì)應(yīng)的特征向量，即Kqk=AmJk1)求矩陣K的最大特征值入(12)(13)式中，b=-2tr(Bk)+tr(adj(Bk+BkT))-zTz；d=det(K)；adj(Bk+BkT)為矩陣(Bk+BkT)的伴隨矩陣；tr(adj(Bk+BkT))為矩陣adj(Bk+BkT)的跡；det⑷為矩陣K的行列式c2)求第k時(shí)刻姿態(tài)四元數(shù)估計(jì)值么姿態(tài)四元數(shù)與歐拉軸/角的關(guān)系q=[eTsin(0/2)cos(0/2)]T式中，e為旋轉(zhuǎn)軸矢量，O為繞旋轉(zhuǎn)軸的轉(zhuǎn)角(將式(14)代入式(12)并消去①得[(tr(Bk)-入隱)S-zzT]e=Me=0(14)(15)9式中，S=811+8111-(廿低)+人_)13><3，由上式可知，矩陣1為對(duì)稱陣，即mmrm、,m=M=[^！m2w3]=ymhmsmymzmc(16)由式(15)可知，旋轉(zhuǎn)軸e與矩陣M的各行/列向量都正交，所以可通過兩向量叉乘求取旋轉(zhuǎn)軸e。因此，旋轉(zhuǎn)軸e的取值有三種情況e}=m2xm3=[mbmc-mz2mymz-mxmcmxmz-mymj7e2=fh3xmx(17)=[mymz-mxmcmamc-my2mjny-mzmafe3=Wjxin2=[mxm2-mymbmxmy-mzmamamb-mz2f式(17)中的ei(i=1，2，3)是相互平行的，但模值不同，選取4=maX{||e,’||}f=1。則ek最大特征值對(duì)應(yīng)的特征向量為qk=Tzek(18)將^歸一化得第k時(shí)刻姿態(tài)四元數(shù)估計(jì)值么為qk=qj^k(19)4.四元數(shù)轉(zhuǎn)換成姿態(tài)角根據(jù)四元數(shù)與姿態(tài)角之間的關(guān)系，將步驟(3)估計(jì)出的姿態(tài)四元數(shù)么轉(zhuǎn)換為姿態(tài)四元數(shù)對(duì)應(yīng)的姿態(tài)角橫滾角約，俯仰角ek，偏航角Vk。5.UKF估計(jì)陀螺漂移由于陀螺存在漂移，無法將陀螺的輸出值直接用于姿態(tài)估計(jì)，所以在四元數(shù)估計(jì)過程中要將陀螺漂移估計(jì)出來。以陀螺漂移為狀態(tài)變量，基于步驟(1)中的陀螺漂移數(shù)學(xué)模型和矢量觀測模型建立從濾波器的狀態(tài)空間模型。狀態(tài)方程3k=3H+nu,H(20)觀測方程bk=A、qk、rk+Sbk(21)=A(Q(3k_,—jBk_x)qk_])rk+5bk由狀態(tài)空間模型可知狀態(tài)方程為線性，觀測方程為非線性。從濾波器估計(jì)第k時(shí)刻陀螺漂移時(shí)要利用主濾波器第k_l時(shí)刻估計(jì)的姿態(tài)四元數(shù)。假設(shè)狀態(tài)方程和觀測方程的噪聲方差陣分別為Q和R，UKF從濾波器(2)估計(jì)陀螺漂移的基本步驟為a計(jì)算Sigma點(diǎn)Xo,k-\=Pk-\x刺=1”(22)式中，x。,^，Xi.H，x^H分別是戎_,對(duì)應(yīng)的第0個(gè)、第i個(gè)、第i+n個(gè)Sigma點(diǎn)，及q對(duì)應(yīng)的Sigma點(diǎn)共有2n+l個(gè)別為第k_l時(shí)刻陀螺漂移的估計(jì)值和估計(jì)誤差方差陣；n為狀態(tài)維數(shù)，這里n=3；T為合成比例參數(shù)；將估計(jì)誤差方差陣分解，(D,表示A的第i列。^AA10113]b.計(jì)算Sigma點(diǎn)對(duì)應(yīng)的權(quán)值0114]0118]0128]0129]0130]W0=T/(n+r)W,=l/[2(w+r)]i=l,...,nWl+n=l/[2(n+r)](23)0115]式中，W^，W^Wh分別是第0個(gè)、第i個(gè)、第i+n個(gè)Sigma點(diǎn)對(duì)應(yīng)的權(quán)值c0116]c.時(shí)間更新0117]第k時(shí)刻各Sigma點(diǎn)的預(yù)測值,kk-l=Xi=0,1,…，2n(24)0119]第k時(shí)刻陀螺漂移的預(yù)測均值爲(wèi)和預(yù)測誤差方差陣々t分別為0120]p-k==P,+Q,=oAA-丨^0121]式中，為第k-l時(shí)刻陀螺漂移的預(yù)測誤差方差陣。0122]第k時(shí)刻各Sigma點(diǎn)對(duì)應(yīng)的測量預(yù)測值0123]=~Xi,k\k-i)Qk-\)rk‘Z=0，l”..，20124]測量的預(yù)測均值&為aIn0125]b￥=L_々)(25)(26)(27)0126]d.測量更新0127]測量的預(yù)測誤差方差陣和測量與狀態(tài)變量之間的協(xié)方差陣分別為pbA=-h)(V-丨(0-+Ri=0(28)(29)增frff.矩o3陣c=PpkbkPbkbk0131]則第k時(shí)刻陀螺漂移的估計(jì)值&和陀螺漂移估計(jì)誤差方差陣々t分別為0132]Pk=P-k+Kk(bk-b-k)(31)權(quán)利要求一種基于ESOQPF和UKF主從濾波的微小衛(wèi)星姿態(tài)確定方法，其特征在于包括以下步驟(1)建立微小衛(wèi)星系統(tǒng)狀態(tài)空間模型微小衛(wèi)星系統(tǒng)狀態(tài)空間模型包括姿態(tài)四元數(shù)運(yùn)動(dòng)學(xué)方程、陀螺數(shù)學(xué)模型、矢量觀測模型；a.姿態(tài)四元數(shù)運(yùn)動(dòng)學(xué)方程qk+1＝Ω(ωk)qk式中，qk和qk+1分別為第k時(shí)刻和第k+1時(shí)刻衛(wèi)星本體坐標(biāo)系相對(duì)于參考坐標(biāo)系的姿態(tài)四元數(shù)，和q4k分別為四元數(shù)的矢量部分和標(biāo)量部分；ωk為第k時(shí)刻本體坐標(biāo)系相對(duì)參考坐標(biāo)系的姿態(tài)角速度。假設(shè)在采樣周期Δt內(nèi)ωk保持不變，則<mrow><mi>Ω</mi><mrow><mo>(</mo><msub><mi>ω</mi><mi>k</mi></msub><mo>)</mo></mrow><mo>=</mo><msub><mfencedopen='['close=']'><mtable><mtr><mtd><mi>cos</mi><mrow><mo>(</mo><mn>0.5</mn><mo>|</mo><mo>|</mo><msub><mi>ω</mi><mi>k</mi></msub><mo>|</mo><mo>|</mo><mi>Δt</mi><mo>)</mo></mrow><msub><mi>I</mi><mrow><mn>3</mn><mo>×</mo><mn>3</mn></mrow></msub><mo>-</mo><mo>[</mo><msub><mi>ψ</mi><mi>k</mi></msub><mo>×</mo><mo>]</mo></mtd><mtd><msub><mi>ψ</mi><mi>k</mi></msub></mtd></mtr><mtr><mtd><mo>-</mo><msubsup><mi>ψ</mi><mi>k</mi><mi>T</mi></msubsup></mtd><mtd><mi>cos</mi><mrow><mo>(</mo><mn>0.5</mn><mo>|</mo><mo>|</mo><msub><mi>ω</mi><mi>k</mi></msub><mo>|</mo><mo>|</mo><mi>Δt</mi><mo>)</mo></mrow></mtd></mtr></mtable></mfenced><mrow><mn>4</mn><mo>×</mo><mn>4</mn></mrow></msub></mrow>式中，I3×3為3×3的單位陣；[ψk×]為ψk的反對(duì)稱矩陣；b.陀螺數(shù)學(xué)模型βk+1＝βk+ηu，k式中，為陀螺的測量輸出值；ωk為真實(shí)的姿態(tài)角速度；βk和βk+1分別為第k時(shí)刻和第k+1時(shí)刻的陀螺漂移；ηv，k與ηu，k是互不相關(guān)的零均值高斯白噪聲，ηv，k和ηu，k的方差分別為σv2與σu2；c.矢量觀測模型bk＝A(qk)rk+δbk式中，bk為觀測矢量；rk為參考矢量；δbk為測量噪聲，且δbk服從概率分布A(qk)為參考坐標(biāo)系到衛(wèi)星本體坐標(biāo)系的姿態(tài)轉(zhuǎn)移矩陣；(2)基于步驟(1)建立的系統(tǒng)狀態(tài)空間模型，進(jìn)行濾波初始化濾波初始化要基于步驟(1)建立的系統(tǒng)狀態(tài)空間模型，包括ESOQPF主濾波器初始化和UKF從濾波器初始化兩部分。ESOQPF主濾波器的初始化主要是初始化N個(gè)粒子及粒子權(quán)值；UKF從濾波器的初始化是對(duì)陀螺漂移估計(jì)值和估計(jì)誤差方差陣進(jìn)行初始化；(3)以姿態(tài)四元數(shù)為狀態(tài)變量，基于步驟(1)中的四元數(shù)運(yùn)動(dòng)學(xué)方程和矢量觀測模型建立主濾波器的狀態(tài)空間模型，利用ESOQPF主濾波器估計(jì)第k時(shí)刻的姿態(tài)四元數(shù)此外，主濾波器估計(jì)第k時(shí)刻姿態(tài)四元數(shù)時(shí)，要利用從濾波器第k1時(shí)刻估計(jì)的陀螺漂移(4)四元數(shù)轉(zhuǎn)換為姿態(tài)角根據(jù)四元數(shù)與姿態(tài)角之間的關(guān)系，將步驟(3)估計(jì)出的姿態(tài)四元數(shù)轉(zhuǎn)換為姿態(tài)四元數(shù)對(duì)應(yīng)的姿態(tài)角橫滾角俯仰角θk，偏航角ψk；(5)以陀螺漂移為狀態(tài)變量，基于步驟(1)中的陀螺漂移數(shù)學(xué)模型和矢量觀測模型建立從濾波器的狀態(tài)空間模型，利用UKF從濾波器估計(jì)第k時(shí)刻的陀螺漂移此外，從濾波器估計(jì)第k時(shí)刻陀螺漂移時(shí)，要利用主濾波器第k1時(shí)刻估計(jì)的姿態(tài)四元數(shù)(6)當(dāng)有新的觀測值時(shí)，重復(fù)步驟(3)～(5)進(jìn)行姿態(tài)確定，得到姿態(tài)角估計(jì)值和陀螺漂移估計(jì)值。FSA00000270286200011.tif,FSA00000270286200012.tif,FSA00000270286200014.tif,FSA00000270286200015.tif,FSA00000270286200016.tif,FSA00000270286200017.tif,FSA00000270286200018.tif,FSA00000270286200021.tif,FSA00000270286200022.tif,FSA00000270286200023.tif,FSA00000270286200024.tif,FSA00000270286200025.tif,FSA00000270286200026.tif,FSA00000270286200027.tif,FSA00000270286200028.tif2.根據(jù)權(quán)利要求1所述的一種基于ES0QPF和UKF主從濾波的微小衛(wèi)星姿態(tài)確定方法，其特征在于所述步驟(3)中ES0QPF主濾波器將QPF算法與ES0Q2算法相結(jié)合來估計(jì)姿態(tài)四元數(shù)；ES0QPF主濾波器首先利用QPF算法計(jì)算出Davenport矩陣，然后利用ES0Q2算法直接計(jì)算出該矩陣的最大特征值，最后計(jì)算出最大特征值對(duì)應(yīng)的特征向量，即姿態(tài)四元數(shù)；具體實(shí)現(xiàn)步驟如下(1)QPF算法計(jì)算Davenport矩陣K利用第k-1時(shí)刻的四元數(shù)粒子Qh(i)、歸一化的粒子權(quán)值巧乂/)、陀螺漂移估計(jì)值&q和第k時(shí)刻的觀測值bk計(jì)算Davenport矩陣K；a.時(shí)間更新根據(jù)姿態(tài)四元數(shù)運(yùn)動(dòng)學(xué)方程，由第k-1時(shí)刻的四元數(shù)粒子qk—Ji)求各粒子第k時(shí)刻的預(yù)測值知i=l,...,N-,b.測量更新測量更新即粒子權(quán)值更新，利用第k-1時(shí)刻的歸一化的粒子權(quán)值和第k時(shí)刻的觀測值bk計(jì)算第k時(shí)刻的各粒子權(quán)值Wk(i)為■xPbih隊(duì)^HW^ii)=Psbk(bk-A{qk^{i))rkW^(i)式中，i=1，...，N;A▲汍1%-,(0)為么丨w(0的似然概率；Pshk(bk-A{qk]k_{{i))rk)為氏-成的概率；將上式各粒子權(quán)值Wk(i)歸一化為Wk{i)=Wk(i)/fjWk{i),i^\,...,N/；=1c.計(jì)算Davenport矩陣KKABk+BkT-I^Xx{Bk)z‘其中，(么M(/))/=1'Bk{3,2)-Bk(2,3)戰(zhàn)(1,3)-代(3,1)Bk(2,l)-Bk(l,2)_式中，tr(Bk)為矩陣Bk的跡；(2)ES0Q2算法估計(jì)姿態(tài)四元數(shù)么a.求矩陣K的最大特征值入.；lmax=去(^2^/d-b+式中，b=-2tr(Bk)+tr(adj(Bk+BkT))-zTz；d=det(K)；adj(Bk+BkT)為矩陣(Bk+BkT)的伴隨矩陣；tr(adj(Bk+BkT))為矩陣adj(Bk+BkT)的跡；det(K)為矩陣K的行列式；b.求姿態(tài)四元數(shù)估計(jì)值也矩陣K最大特征值對(duì)應(yīng)的特征向量么為全文摘要本發(fā)明涉及一種基于ESOQPF和UKF主從濾波的微小衛(wèi)星姿態(tài)確定方法。首先建立微小衛(wèi)星系統(tǒng)狀態(tài)空間模型，進(jìn)行姿態(tài)確定濾波初始化；然后以姿態(tài)四元數(shù)為狀態(tài)變量，將ESOQPF作為主濾波器估計(jì)姿態(tài)四元數(shù)，將估計(jì)出來的四元數(shù)轉(zhuǎn)換為相應(yīng)的姿態(tài)角；以陀螺漂移為狀態(tài)變量，將UKF作為從濾波器估計(jì)陀螺漂移。本發(fā)明在保證姿態(tài)估計(jì)精度的同時(shí)，從兩個(gè)方面降低了計(jì)算量，縮短了定姿過程。一方面，利用ESOQPF和UKF進(jìn)行主從濾波，即將姿態(tài)四元數(shù)與陀螺漂移分開估計(jì)；另一方面，ESOQPF估計(jì)姿態(tài)四元數(shù)時(shí)將QPF算法與ESOQ2算法相結(jié)合，利用QPF算法計(jì)算出Davenport矩陣，然后利用ESOQ2算法直接計(jì)算出該矩陣最大特征值所對(duì)應(yīng)的特征向量。該方法適用于有陀螺配置模式下，基于矢量觀測的微小衛(wèi)星姿態(tài)確定。文檔編號(hào)G01C1/00GK101982732SQ201010281990公開日2011年3月2日申請日期2010年9月14日優(yōu)先權(quán)日2010年9月14日發(fā)明者全偉,崔培玲,張會(huì)娟,房建成,郭雷申請人:北京航空航天大學(xué)