一種基于改進(jìn)的自適應(yīng)平方根ukf算法的衛(wèi)星姿態(tài)確定方法
【專利摘要】一種基于改進(jìn)的自適應(yīng)平方根UKF算法的衛(wèi)星姿態(tài)確定方法，屬于衛(wèi)星姿態(tài)確定【技術(shù)領(lǐng)域】。本發(fā)明解決了在衛(wèi)星姿態(tài)確定系統(tǒng)受到不確定性干擾和噪聲的影響時(shí)，由于現(xiàn)有的EKF,UKF和SRUKF算法數(shù)值計(jì)算的舍入誤差太大所引起的衛(wèi)星姿態(tài)確定系統(tǒng)的不穩(wěn)定以及對(duì)衛(wèi)星姿態(tài)估計(jì)的精度低和衛(wèi)星實(shí)際狀態(tài)跟蹤能力弱等問題。該衛(wèi)星姿態(tài)確定方法的主要實(shí)現(xiàn)過程為：利用改進(jìn)的自適應(yīng)平方根UKF估計(jì)誤差四元數(shù)和陀螺漂移誤差；利用陀螺測(cè)量值和估計(jì)出來(lái)的陀螺漂移誤差代入姿態(tài)運(yùn)動(dòng)學(xué)方程計(jì)算姿態(tài)四元數(shù)；利用估計(jì)出的誤差四元數(shù)對(duì)解算出的姿態(tài)四元數(shù)進(jìn)行修正；利用修正的姿態(tài)四元數(shù)進(jìn)行姿態(tài)解算，確定衛(wèi)星的姿態(tài)。本發(fā)明適用于衛(wèi)星姿態(tài)確定【技術(shù)領(lǐng)域】。
【專利說(shuō)明】一種基于改進(jìn)的自適應(yīng)平方根UKF算法的衛(wèi)星姿態(tài)確定方法
【技術(shù)領(lǐng)域】
[0001]本發(fā)明涉及星敏感器和陀螺的高精度衛(wèi)星姿態(tài)確定方法，屬于衛(wèi)星姿態(tài)確定【技術(shù)領(lǐng)域】。
【背景技術(shù)】
[0002]星敏感器和陀螺組成的姿態(tài)測(cè)量系統(tǒng)，由于其精度較高，因而被廣泛用于衛(wèi)星姿態(tài)確定系統(tǒng)中。針對(duì)其組成的非線性系統(tǒng)，采用非線性濾波技術(shù)確定姿態(tài)的方法被廣泛應(yīng)用。擴(kuò)展卡爾曼濾波器(EKF)因?yàn)槠浞椒ê?jiǎn)單，容易實(shí)現(xiàn)等優(yōu)點(diǎn)，在處理非線性估計(jì)的工程問題中被廣泛應(yīng)用。但是EKF是對(duì)非線性方程做一階近似處理，忽略其余高階項(xiàng)，從而將非線性問題轉(zhuǎn)化為線性問題。當(dāng)系統(tǒng)非線性較強(qiáng)時(shí)，EKF違背局部線性假設(shè)，被忽略的高階項(xiàng)會(huì)帶來(lái)大的誤差，導(dǎo)致EKF算法精度下降，甚至造成EKF算法的濾波發(fā)散。另外EKF在線性化處理時(shí)需要計(jì)算雅克比(Jacobian)矩陣，其計(jì)算過程繁瑣復(fù)雜且容易出錯(cuò)。
[0003]針對(duì)上述問題，Julier等人提出了無(wú)忌卡爾曼濾波器(UKF)算法。相對(duì)于EKF，UKF采用UT變換對(duì)非線性概率密度分布進(jìn)行近似，具有不需要計(jì)算Jacobian矩陣，估計(jì)精度更高，近年來(lái)在飛行器姿態(tài)估計(jì)問題當(dāng)中獲得廣泛應(yīng)用。然而UKF在數(shù)值計(jì)算中往往會(huì)存在舍入誤差，可能會(huì)破壞系統(tǒng)估計(jì)誤差協(xié)方差矩陣的非負(fù)定性和對(duì)稱性，導(dǎo)致算法的收斂速度慢，甚至造成算法的不穩(wěn)定。針對(duì)該問題平方根UKF(SRUKF)濾波算法可以較好的解決，該方法借鑒kalman濾波中平方根分解濾波思想，在濾波的過程中采用矩陣的QR分解和Cholesky分解結(jié)果直接傳播并更新協(xié)方差矩陣的平方根，解決了 UKF算法中計(jì)算誤差可能引起的誤差協(xié)方差的負(fù)定性問題，提高了濾波算法的計(jì)算效率和數(shù)值穩(wěn)定性。但是，當(dāng)數(shù)值累積誤差太大或者權(quán)值I/選擇不合適的時(shí)候，標(biāo)準(zhǔn)的平方根UKF仍然存在濾波發(fā)散問題。此外，EKF, UKF和SRUKF都要求系統(tǒng)模型精確和噪聲統(tǒng)計(jì)特性已知。當(dāng)系統(tǒng)存在不確定性干擾和噪聲作用時(shí)，上述方法都不具備較好的估計(jì)精度、跟蹤能力及魯棒性。

【發(fā)明內(nèi)容】

[0004]本發(fā)明的目的是提出一種基于改進(jìn)的自適應(yīng)平方根UKF算法的衛(wèi)星姿態(tài)確定方法，以解決在衛(wèi)星姿態(tài)確定系統(tǒng)受到不確定性干擾和噪聲的影響時(shí)，由于現(xiàn)有的EKF，UKF和SRUKF算法數(shù)值計(jì)算的舍入誤差太大所引起的衛(wèi)星姿態(tài)確定系統(tǒng)的不穩(wěn)定以及對(duì)衛(wèi)星姿態(tài)估計(jì)的精度低和衛(wèi)星實(shí)際狀態(tài)跟蹤能力弱等問題。
[0005]本發(fā)明為解決上述技術(shù)問題所采用的技術(shù)方案是:
[0006]本發(fā)明所述的一種基于改進(jìn)的自適應(yīng)平方根UKF算法的衛(wèi)星姿態(tài)確定方法，是按照以下步驟實(shí)現(xiàn)的:步驟一、建立陀螺測(cè)量模型；步驟二、建立衛(wèi)星姿態(tài)運(yùn)動(dòng)學(xué)方程；步驟三、建立基于誤差四元數(shù)和陀螺漂移誤差組成的狀態(tài)變量的系統(tǒng)狀態(tài)方程；步驟四、建立誤差系統(tǒng)觀測(cè)方程；步驟五、利用改進(jìn)的自適應(yīng)平方根UKF估計(jì)誤差四元數(shù)和陀螺漂移誤差；步驟六、利用陀螺測(cè)量值和估計(jì)出來(lái)的陀螺漂移誤差代入姿態(tài)運(yùn)動(dòng)學(xué)方程計(jì)算姿態(tài)四元數(shù)；步驟七、利用估計(jì)出的誤差四元數(shù)對(duì)解算出的姿態(tài)四元數(shù)進(jìn)行修正；步驟八、利用修正的姿態(tài)四元數(shù)進(jìn)行姿態(tài)解算，確定衛(wèi)星的姿態(tài)。
[0007]本發(fā)明的有益效果是:
[0008]一、本發(fā)明改善了由于現(xiàn)有算法的不穩(wěn)定所導(dǎo)致的衛(wèi)星姿態(tài)確定系統(tǒng)的穩(wěn)定性，使衛(wèi)星姿態(tài)確定系統(tǒng)的穩(wěn)定性更好。
[0009]二、在改進(jìn)的平方根UKF基礎(chǔ)上引入自適應(yīng)因子μ k+1，使得平方根UKF具有自適應(yīng)性，從而在系統(tǒng)具有未知干擾的情況下，提高了衛(wèi)星姿態(tài)估計(jì)精度。
[0010]三、本發(fā)明改善了 UKF和平方根UKF對(duì)未知干擾及突變狀態(tài)的跟蹤能力，使衛(wèi)星實(shí)際狀態(tài)跟蹤能力增強(qiáng)，從而在模型不確定性和噪聲統(tǒng)計(jì)特性不確定時(shí)也能夠較好的估計(jì)出衛(wèi)星的姿態(tài)，因此在復(fù)雜多變的環(huán)境當(dāng)中更適用。
[0011]四、相對(duì)于STF方法，本發(fā)明(IASRUKF)在滾轉(zhuǎn)角的估計(jì)精度方面提高了 66%?67%，在俯仰角的估計(jì)精度方面提高了 68%?69%，在偏航角的估計(jì)精度方面提高了58%?％ 59 ;相對(duì)于SRUKF方法，本發(fā)明在滾轉(zhuǎn)角的估計(jì)精度方面提高了 97%?98%，在俯仰角的估計(jì)精度方面提高了 97 %?98 %，在偏航角的估計(jì)精度方面提高了 97 %?98 %。
【專利附圖】

【附圖說(shuō)明】
[0012]圖1為本發(fā)明的流程圖；圖2為衛(wèi)星姿態(tài)誤差系統(tǒng)的改進(jìn)的自適應(yīng)平方根UKF算法流程圖；圖3為STF (強(qiáng)跟蹤濾波器)、SRUKF (平方根UKF)和本發(fā)明的IASRUKF的滾轉(zhuǎn)角誤差曲線對(duì)比圖；圖4為STF (強(qiáng)跟蹤濾波器)、SRUKF (平方根UKF)和本發(fā)明的IASRUKF的俯仰角誤差曲線對(duì)比圖；圖5為STF(強(qiáng)跟蹤濾波器)、SRUKF(平方根UKF)和本發(fā)明的IASRUKF的偏航角誤差曲線對(duì)比圖。
【具體實(shí)施方式】
[0013]【具體實(shí)施方式】一:本實(shí)施方式所述的一種基于改進(jìn)的自適應(yīng)平方根UKF算法的衛(wèi)星姿態(tài)確定方法，其特征在于所述方法包括以下步驟:
[0014]步驟一、建立陀螺測(cè)量模型；
[0015]步驟二、建立衛(wèi)星姿態(tài)運(yùn)動(dòng)學(xué)方程；
[0016]步驟三、建立基于誤差四元數(shù)和陀螺漂移誤差組成的狀態(tài)變量的系統(tǒng)狀態(tài)方程；
[0017]步驟四、建立誤差系統(tǒng)觀測(cè)方程；
[0018]步驟五、利用改進(jìn)的自適應(yīng)平方根UKF估計(jì)誤差四元數(shù)和陀螺漂移誤差；
[0019]步驟六、利用陀螺測(cè)量值和估計(jì)出來(lái)的陀螺漂移誤差代入姿態(tài)運(yùn)動(dòng)學(xué)方程計(jì)算姿態(tài)四元數(shù)；
[0020]步驟七、利用估計(jì)出的誤差四元數(shù)對(duì)解算出的姿態(tài)四元數(shù)進(jìn)行修正；
[0021]步驟八、利用修正的姿態(tài)四元數(shù)進(jìn)行姿態(tài)解算，確定衛(wèi)星的姿態(tài)。
[0022]【具體實(shí)施方式】二:本實(shí)施方式與【具體實(shí)施方式】一不同的是:步驟一所述的建立陀螺測(cè)量模型的具體過程為:在陀螺測(cè)量坐標(biāo)系與星體的坐標(biāo)系為同一坐標(biāo)系的情況下，陀螺測(cè)量模型為
【權(quán)利要求】
1.一種基于改進(jìn)的自適應(yīng)平方根UKF算法的衛(wèi)星姿態(tài)確定方法，其特征在于所述方法包括以下步驟: 步驟一、建立陀螺測(cè)量模型； 步驟二、建立衛(wèi)星姿態(tài)運(yùn)動(dòng)學(xué)方程； 步驟三、建立基于誤差四元數(shù)和陀螺漂移誤差組成的狀態(tài)變量的系統(tǒng)狀態(tài)方程； 步驟四、建立誤差系統(tǒng)觀測(cè)方程； 步驟五、利用改進(jìn)的自適應(yīng)平方根UKF估計(jì)誤差四元數(shù)和陀螺漂移誤差； 步驟六、利用陀螺測(cè)量值和估計(jì)出來(lái)的陀螺漂移誤差代入姿態(tài)運(yùn)動(dòng)學(xué)方程計(jì)算姿態(tài)四元數(shù)； 步驟七、利用估計(jì)出的誤差四元數(shù)對(duì)解算出的姿態(tài)四元數(shù)進(jìn)行修正； 步驟八、利用修正的姿態(tài)四元數(shù)進(jìn)行姿態(tài)解算，確定衛(wèi)星的姿態(tài)。
2.根據(jù)權(quán)利要求1所述的一種基于改進(jìn)的自適應(yīng)平方根UKF算法的衛(wèi)星姿態(tài)確定方法，其特征在于步驟一所述的建立陀螺測(cè)量模型的具體過程為:在陀螺測(cè)量坐標(biāo)系與星體的坐標(biāo)系為同一坐標(biāo)系的情況下，陀螺測(cè)量模型為
S(I)^oAI) + β(!) +ηΛΟ.(I)
/(O = "''⑴ 式中，g(t)為陀螺的測(cè)量輸出值，ω (t)為陀螺真實(shí)角速度，β (t)為陀螺漂移，nu(t)和nv(t)為互不相關(guān)的聞斯噪聲，滿足:< (2)
(Οη'? ⑴} = — r)/,x3 式中CT和<分別為白噪聲，nu(t)和ηνα)為均方差，δ (t-τ)為狄拉克函數(shù)。
3.根據(jù)權(quán)利要求2所述的一種基于改進(jìn)的自適應(yīng)平方根UKF算法的衛(wèi)星姿態(tài)確定方法，其特征在于步驟二所述的建立衛(wèi)星姿態(tài)運(yùn)動(dòng)學(xué)方程的具體過程為:衛(wèi)星姿態(tài)四元數(shù)被定義為 q = [qi Q2 Q3 qJT ⑶ 式中，[知％,%] = ^11(6^2)，q4 = cos( Θ/2)，A和Θ分別為單位旋轉(zhuǎn)向量和旋轉(zhuǎn)角； 四元數(shù)滿足如下約束: <Ji + cIl + cIi +cIa =I(4) 用四元數(shù)表示衛(wèi)星姿態(tài)運(yùn)動(dòng)學(xué)方程為: ^~二^￡2{(翁0)(S)
at 2 式中，ω (t)為衛(wèi)星的真實(shí)角速度，且 「I「O OJ1(I)
「―W)x]y--(/)?— ?2(…(0)= Y」廣]、,，MOx]= οκ(!)O -Ojv(I)。
-ω (t)O Λ Λ'
L W」4χ4-ων(?) ωχ{?) O
4.根據(jù)權(quán)利要求3所述的一種基于改進(jìn)的自適應(yīng)平方根UKF算法的衛(wèi)星姿態(tài)確定方法，其特征在于步驟三所述的建立基于誤差四元數(shù)和陀螺漂移誤差組成的狀態(tài)變量的系統(tǒng)狀態(tài)方程的具體過程為:采用乘性四元數(shù)定義真實(shí)四元數(shù)與四元數(shù)計(jì)算值$之間的誤差四元數(shù)為狀態(tài)變量:
5.根據(jù)權(quán)利要求4所述的一種基于改進(jìn)的自適應(yīng)平方根UKF算法的衛(wèi)星姿態(tài)確定方法，其特征在于步驟四所述的建立誤差系統(tǒng)觀測(cè)方程的具體過程為:由于采用的是將四元數(shù)q作為輸出量，將星敏感器得到的四元數(shù)誤差作為測(cè)量輸出，取其矢量部分作為觀測(cè)量，有觀測(cè)方程: z = Hx+v (14) 式中，H為觀測(cè)矩陣，V為觀測(cè)噪聲；
6.根據(jù)權(quán)利要求5所述的一種基于改進(jìn)的自適應(yīng)平方根UKF算法的衛(wèi)星姿態(tài)確定方法，其特征在于步驟五所述的利用改進(jìn)的自適應(yīng)平方根UKF估計(jì)誤差四元數(shù)和陀螺漂移誤差的具體過程為: (1)初始化狀態(tài)戈,=[Se,Shf，狀態(tài)誤差協(xié)方差矩陣平方根Stl為
7.根據(jù)權(quán)利要求6所述的一種基于改進(jìn)的自適應(yīng)平方根UKF算法的衛(wèi)星姿態(tài)確定方法，其特征在于步驟八所述的姿態(tài)解算方法為:
【文檔編號(hào)】G01C21/24GK103940433SQ201410198696
【公開日】2014年7月23日 申請(qǐng)日期:2014年5月12日 優(yōu)先權(quán)日:2014年5月12日
【發(fā)明者】李敏, 王松艷, 張迎春, 耿云海, 李華義, 謝成清 申請(qǐng)人:哈爾濱工業(yè)大學(xué)