專利名稱：一種基于Euler-q算法和DD2濾波的航天器姿態(tài)確定方法
技術(shù)領(lǐng)域：
本發(fā)明涉及一種航天器的姿態(tài)確定方法，特別是一種基于Euler-q(Euler-quaternion)算法和DD2(Divided Difference 2)濾波的航天器姿態(tài)確定方法，用于各種中高精度的慣性/天文組合導(dǎo)航系統(tǒng)的姿態(tài)確定。

背景技術(shù)：
航天器的姿態(tài)確定任務(wù)是利用航天器上的姿態(tài)敏感器測(cè)量所得到的信息，經(jīng)過適當(dāng)?shù)奶幚恚蟮霉踢B于航天器本體坐標(biāo)系相對(duì)空間某一參考坐標(biāo)系的信息，航天器由于各種任務(wù)的要求，需要高精度姿態(tài)信息，這是航天器發(fā)展和廣泛應(yīng)用的關(guān)鍵技術(shù)之一。
慣性/天文，即INS(Inertial Navigation System)/CNS(CelestialNavigation System)姿態(tài)確定系統(tǒng)是一種完全自主的姿態(tài)確定導(dǎo)航系統(tǒng)，它利用陀螺測(cè)量的載體角速度信息和星敏感器測(cè)量視場(chǎng)內(nèi)的星光信息，在初始信息的基礎(chǔ)上進(jìn)行姿態(tài)解算和濾波，可以連續(xù)、實(shí)時(shí)地提供姿態(tài)，具有自主性強(qiáng)、隱蔽性好、不受氣候條件限制等優(yōu)點(diǎn)，因而廣泛應(yīng)用于航天等領(lǐng)域。目前使用的航天器姿態(tài)確定模型，大多以姿態(tài)四元數(shù)為狀態(tài)，由于三軸姿態(tài)的自由度為3，因此姿態(tài)四元數(shù)存在冗余，而如果單純?nèi)∽藨B(tài)四元數(shù)的矢量為估計(jì)的狀態(tài)，則必須對(duì)估計(jì)模型線性化簡(jiǎn)化，造成模型的精度降低。應(yīng)用于航天器姿態(tài)確定方法中，單純使用確定性矢量觀測(cè)，例如Davenport q方法由于需要計(jì)算特征值和特征矩陣，在線計(jì)算困難，因而難以得到應(yīng)用。而在Davenport q方法上改進(jìn)的QuEsT(Quaternion Estimator)方法，雖然在算法上得到了極大的改進(jìn)，但是仍然存在過于依賴星敏感器精度且不能修正陀螺漂移并輸出實(shí)時(shí)姿態(tài)的缺點(diǎn)。SVD(the Singular ValueDecomposition)魯棒性很強(qiáng)但卻需要奇異值分解，由于奇異值分解是一項(xiàng)運(yùn)算量很大的復(fù)雜工作，因此不是很有效。FOAM(A Fast Optimal MatrixAlgurithm)算法也不能估計(jì)陀螺漂移使陀螺輸出獲得在線標(biāo)定；而使用其它狀態(tài)估計(jì)法，例如，擴(kuò)展卡爾曼濾波EKF(Extend Kalman Filter)，由于使用泰勒展開的一階非線性處理，雖然可以估計(jì)非線性模型，但是對(duì)非線性模型估計(jì)精度不高，而UKF(Unsented Kalman Filter)則利用一系列近似高斯分布的采樣點(diǎn)，通過Unscented變換來進(jìn)行狀態(tài)與誤差協(xié)方差的遞推和更新，雖然精度有所提高，但是計(jì)算繁瑣，實(shí)時(shí)性不高，因而很難滿足姿態(tài)確定實(shí)時(shí)性的要求。


發(fā)明內(nèi)容
本發(fā)明的技術(shù)解決問題是克服現(xiàn)有技術(shù)的不足，提供一種基于Euler-q算法和DD2濾波的航天器姿態(tài)確定方法，該方法由Euler-q算法計(jì)算矢量觀測(cè)得到的航天器姿態(tài)，同時(shí)由DD2濾波估計(jì)姿態(tài)誤差和陀螺的常值漂移，不僅提供了高精度的姿態(tài)信息，而且校正了陀螺角速率輸出。
本發(fā)明的技術(shù)解決方案為一種基于Euler-q算法和DD2濾波的航天器姿態(tài)確定方法，其特征在于使用Euler-q算法和DD2濾波估計(jì)模型，利用星敏感器和陀螺儀，既可得到高精度實(shí)時(shí)的航天器姿態(tài)，又可以估計(jì)出陀螺儀的常值漂移，其具體步驟如下 (1)由陀螺儀敏感航天器姿態(tài)，輸出三軸角速率，并使用角增量算法更新姿態(tài)陣，獲得航天器在地心慣性坐標(biāo)系下的實(shí)時(shí)姿態(tài)航向角，俯仰角θ，橫滾角γ； (2)利用星敏感器敏感星光，經(jīng)過星圖預(yù)處理得到觀測(cè)星光矢量s在航天器本體坐標(biāo)系下的坐標(biāo)；并經(jīng)過星圖匹配識(shí)別之后得到與之對(duì)應(yīng)的參考星光矢量v在地心慣性坐標(biāo)系下的坐標(biāo)； (3)使用Euler-q算法，利用觀測(cè)星光矢量和與之對(duì)應(yīng)的參考星光矢量，獲得航天器姿態(tài)四元數(shù)； (4)以星光觀測(cè)得到的航天器姿態(tài)四元數(shù)與步驟(1)中陀螺儀解算的航天器姿態(tài)之差作為觀測(cè)量，對(duì)航天器姿態(tài)誤差和陀螺漂移進(jìn)行DD2濾波，狀態(tài)量為(Δq1 Δq2 Δq3 b1 b2 b3)； (5)由DD2濾波得到的航天器姿態(tài)誤差和陀螺儀常值漂移，反饋校正陀螺儀輸出姿態(tài)和陀螺儀輸出角速率，得到高精度的航天器姿態(tài)以及高精度的陀螺輸出； (6)反饋校正后，重復(fù)(1)-(5)步驟。
本發(fā)明的原理是使用陀螺儀輸出角速率解算姿態(tài)作為短期參考，提供實(shí)時(shí)的姿態(tài)，短時(shí)間內(nèi)有較高的精度，但是姿態(tài)信息不斷發(fā)散，由星敏感器獲得的星光矢量通過Euler-q算法以一定的頻率提供高精度的長(zhǎng)期參考航天器姿態(tài)，用這個(gè)長(zhǎng)期參考姿態(tài)作為觀測(cè)量，通過DD2濾波器，修正陀螺輸出姿態(tài)，糾正發(fā)散信息，同時(shí)估計(jì)陀螺常值漂移，反饋校正陀螺輸出，對(duì)陀螺進(jìn)行在線標(biāo)定，保持高精度的姿態(tài)實(shí)時(shí)輸出。
本發(fā)明與現(xiàn)有技術(shù)相比的優(yōu)點(diǎn)在于本發(fā)明提供由Euler軸和Euler角的旋轉(zhuǎn)特性來計(jì)算姿態(tài)陣的Euler q方法，計(jì)算速度更快，同時(shí)采用以姿態(tài)四元數(shù)矢量和陀螺誤差為狀態(tài)的模型，提高了模型的精度，避免了計(jì)算中的奇異性；并且在組合定姿濾波過程中采用DD2濾波估計(jì)四元數(shù)矢量誤差和陀螺漂移，以多項(xiàng)式逼近的新的濾波器考慮了新模型狀態(tài)估計(jì)的不確定性，更容易實(shí)現(xiàn)且不需要求導(dǎo)，精度優(yōu)于EKF且不低于UKF，在保證定姿精度的同時(shí)大大提高的計(jì)算速度，同時(shí)估計(jì)出陀螺的常值漂移，大大方便了工程實(shí)際應(yīng)用。



圖1為本發(fā)明的使用Euler-q算法和DD2濾波的航天器姿態(tài)確定方法流程圖； 圖2為航天器本體坐標(biāo)系與地心慣性坐標(biāo)系之間的關(guān)系。

具體實(shí)施例方式 如圖1、2所示，本發(fā)明的具體實(shí)施方法如下 1、由陀螺儀敏感航天器姿態(tài)，輸出三軸角速率，并使用角增量算法更新姿態(tài)陣，獲得航天器在地心慣性坐標(biāo)系，即地球地心為原點(diǎn)，指向天頂為zi，指向春分點(diǎn)為xi，yi與xi、zi成右手螺旋下的實(shí)時(shí)姿態(tài)航向角，俯仰角θ，橫滾角γ。陀螺輸出姿態(tài)頻率由實(shí)際陀螺采樣速率決定，本例中采用輸出頻率為100HZ； 由初始姿態(tài)計(jì)算得出初始姿態(tài)四元數(shù)q陣[q1 q2 q3 q4]，下標(biāo)0為初始值
更新矩陣為 Δθ＝[Δθ1 Δθ2 Δθ3]為陀螺輸出角增量。Δθ為Δθ的斜對(duì)稱陣 計(jì)算姿態(tài)余弦陣為 由方向余弦陣計(jì)算得出地心慣性坐標(biāo)系下的實(shí)時(shí)姿態(tài)角。
姿態(tài)角解算公式如下 俯仰角為 θ＝sin-1(C23) (8) 偏航角如下表 橫滾角值如下表 2、利用星敏感器敏感星光，經(jīng)過星圖預(yù)處理得到觀測(cè)星光矢量s在航天器本體坐標(biāo)系下的坐標(biāo)，即航天器質(zhì)心為原點(diǎn)，與地心連線反向?yàn)閦b軸，與天緯相切指向東為xb，yb與zb、xb成右手螺旋；并經(jīng)過星圖匹配識(shí)別之后得到與之對(duì)應(yīng)的參考星光矢量v在地心慣性坐標(biāo)系下的坐標(biāo)； 3、使用Euler-q算法，利用觀測(cè)星光矢量和與之對(duì)應(yīng)的參考星光矢量，獲得航天器姿態(tài)四元數(shù)； 星敏感器相關(guān)精度 計(jì)算B矩陣 計(jì)算向量ZZ＝{b23-b32 b31-b13 b12-b21}T (11) 計(jì)算相關(guān)權(quán)重 其中下標(biāo)i為第i個(gè)星光，βi為星敏感器觀測(cè)第i個(gè)星光時(shí)的精度，n為星光個(gè)數(shù)，以星圖識(shí)別星光的個(gè)數(shù)為準(zhǔn)。
引入單位向量 計(jì)算對(duì)稱矩陣 Euler軸即為H陣的最小特征值的特征向量。H陣的最小特征值的特征向量的計(jì)算公式如下 λmin為最小特征值，

為最優(yōu)特征矢量。
可以直接從H陣3階特征等式中直接解算出來 λ3+aλ2+bλ+c＝0 (16) 這里a，b，c系數(shù)可以根據(jù)H陣的元素來表示 a＝-tr[H]＝-h11-h22-h33 設(shè)定參數(shù)為 p2＝(a/3)2-(b/3) q＝[(b/3)-(a/3)2](a/3)-(c/3)(18) 三個(gè)實(shí)根解為 λ1＝-p(sinw+cosw)-a/3 λ2＝q(sinw-cosw)-a/3 (19) λ3＝2pcosw-a/3 由于0≤w≤π/3，得出λi滿足條件 0≤λ1≤λ2≤λ3(20) λmin即為最小λ值。一下計(jì)算λmin的特征向量，即Euler q軸 給出三個(gè)可選擇的解 所有的

(k＝1，2，3)都是平行的，計(jì)算各個(gè)矢量的模為 選擇最接近單位值的pk(k＝1，2，3)，然后由得出最優(yōu)的Euler-q軸。
計(jì)算得出相關(guān)Euler-q角Φopt 其中 航天器姿態(tài)矩陣A可以由Euler-q 軸和Euler-q角來表示

是

的叉乘斜對(duì)稱矩陣。由此解算航天器姿態(tài)。
4、以星光觀測(cè)得到的航天器姿態(tài)四元數(shù)與陀螺儀解算的航天器姿態(tài)之差，作為觀測(cè)量，對(duì)航天器姿態(tài)誤差和陀螺漂移進(jìn)行DD2濾波，濾波周期由星圖識(shí)別間隔時(shí)間決定，本例中頻率為1HZ，狀態(tài)量為(δq1 δq2 δq3 b1 b2 b3) 狀態(tài)方程 觀測(cè)方程 y＝[I3 0]X+w (27) 離散化后可得 xk+1＝f(xk，vk)(28) yk＝g(xk，wk) (29) 以下各式中，上標(biāo)-為量的均值，上標(biāo)^為量的估值，上標(biāo)~為量的誤差。
引入四個(gè)cholesky因子分解 其中， Q為狀態(tài)噪聲方差陣，R為量測(cè)噪聲方差陣，Yk-1＝[y0 y1…yk-1]，為過去的量測(cè)矩陣。
使用Sx，j表示Sx矩陣的j列元素。
可以得到以下各量 假設(shè)估計(jì)誤差是高斯且無偏的，設(shè)間隔長(zhǎng)度為h2＝3。
一步更新 從復(fù)合矩陣中得到 增益矩陣為 Kk＝Pxy(k)[Sy(k)Sy(k)T]-1 (38) 協(xié)方差陣 5、由DD2濾波得到的航天器姿態(tài)誤差，反饋校正陀螺儀輸出姿態(tài)，得到高精度的航天器姿態(tài)；由濾波得到的陀螺儀常值漂移，反饋校正陀螺儀輸出角速率，得到校正后的陀螺輸出 由濾波得出的狀態(tài)中[δq1 δq2 δq3]，獲得誤差四元數(shù) 計(jì)算出姿態(tài)誤差陣Cgc，修正由陀螺輸出的姿態(tài)陣 由此計(jì)算得出反饋校正后的姿態(tài)。
由濾波得出狀態(tài)中的修正陀螺輸出 式中，wout為修正前的陀螺輸出。
6、反饋校正后，重復(fù)1-5步驟。
權(quán)利要求
1.一種基于Euler-q算法和DD2濾波的航天器姿態(tài)確定方法，其特征在于包括以下步驟
(1)由陀螺儀敏感航天器姿態(tài)，輸出三軸角速率，獲得航天器在地心慣性坐標(biāo)系下的實(shí)時(shí)姿態(tài)；
(2)利用星敏感器敏感星光，經(jīng)過星圖預(yù)處理得到觀測(cè)星光矢量s在航天器本體坐標(biāo)系下的坐標(biāo)；并經(jīng)過星圖匹配識(shí)別之后得到與之對(duì)應(yīng)的參考星光矢量v在地心慣性坐標(biāo)系下的坐標(biāo)；
(3)使用Euler-q算法，利用觀測(cè)星光矢量和與之對(duì)應(yīng)的參考星光矢量，獲得航天器姿態(tài)四元數(shù)；
(4)以星光觀測(cè)得到的航天器姿態(tài)四元數(shù)與陀螺儀解算的航天器姿態(tài)之差，作為觀測(cè)量，對(duì)航天器姿態(tài)誤差和陀螺漂移進(jìn)行DD2濾波；
(5)由DD2濾波得到的航天器姿態(tài)誤差和陀螺儀常值漂移，反饋校正陀螺儀輸出姿態(tài)和陀螺儀輸出角速率，得到航天器本體坐標(biāo)系相對(duì)于地心慣性坐標(biāo)系的姿態(tài)和陀螺輸出；
(6)反饋校正后，重復(fù)(1)-(5)步驟。
2.根據(jù)權(quán)利要求1所述的基于Euler-q算法和DD2濾波的航天器姿態(tài)確定方法，其特征在于所述的步驟(4)中，對(duì)航天器姿態(tài)誤差和陀螺漂移的估計(jì)是通過DD2濾波器實(shí)現(xiàn)的，且姿態(tài)確定模型如下
狀態(tài)量為(δq1 δq2 δq3 b1 b2 b3)，其中δq1 δq2 δq3是姿態(tài)誤差四元數(shù)的矢量部分，b1 b2 b3分別是陀螺三軸輸出的常值漂移；
狀態(tài)方程，觀測(cè)方程分別為
y=[I3 0]X+W (2)
其中，y為觀測(cè)量，ε1，ε2為高斯白噪聲，w為陀螺輸出角速率，W為量測(cè)噪聲，I3為3×3單位矢量。
3.根據(jù)權(quán)利要求1所述的基于Euler-q算法和DD2濾波的航天器姿態(tài)確定方法，其特征在于所述的步驟(5)中，對(duì)航天器姿態(tài)誤差和陀螺漂移的反饋校正是通過以下步驟實(shí)現(xiàn)的
由濾波得出的狀態(tài)中[δq1 δq2 δq3]，獲得誤差四元數(shù)
計(jì)算出姿態(tài)誤差陣Cgc，修正由陀螺輸出的姿態(tài)陣C，得到反饋校正后的姿態(tài)陣
由濾波得出狀態(tài)中的修正陀螺輸出
式中，wout為修正前的陀螺輸出。
全文摘要
一種基于Euler-q(Euler-quaternion)算法和DD2(Divided Difierence2)濾波的航天器姿態(tài)確定方法，本發(fā)明涉及一種基于矢量觀測(cè)的狀態(tài)估計(jì)姿態(tài)確定方法。其特征在于根據(jù)參考星歷和由星敏感器觀測(cè)得到的參考矢量和觀測(cè)矢量，通過Euler-q算法獲得航天器姿態(tài)四元數(shù)，經(jīng)過DD2濾波，一方面反饋校正陀螺解算獲得的姿態(tài)陣，另一方面，修正三軸陀螺角速度輸出，對(duì)陀螺儀的常值漂移進(jìn)行補(bǔ)償。這樣既可以得到高精度實(shí)時(shí)的航天器姿態(tài)信息，又可以在線標(biāo)定陀螺儀的常值漂移。本發(fā)明是一種自主式姿態(tài)確定方法，具有精度高，實(shí)時(shí)性好，簡(jiǎn)便易行的特點(diǎn)，可應(yīng)用于各種航天器的姿態(tài)確定系統(tǒng)。
文檔編號(hào)G01C21/24GK101196398SQ200710301279
公開日2008年6月11日 申請(qǐng)日期2007年12月19日 優(yōu)先權(quán)日2007年5月25日
發(fā)明者房建成, 鐘慧敏, 偉 全, 帆 徐, 科 王 申請(qǐng)人:北京航空航天大學(xué)