基于波門預(yù)估遞推剔除大跳動星點快速求解姿態(tài)算法研究

劉騰駿,林榮峰,朱晏慶,周 宇,肖東東

(上海航天控制技術(shù)研究所,上海 201109)

?

基于波門預(yù)估遞推剔除大跳動星點快速求解姿態(tài)算法研究

劉騰駿,林榮峰,朱晏慶,周 宇,肖東東

(上海航天控制技術(shù)研究所,上海 201109)

考慮星敏感器對快速性、穩(wěn)定性和高精度的需求，針對傳統(tǒng)星敏感器軟件在采集波門出現(xiàn)大跳動采集星點時而出現(xiàn)星跟蹤失敗和姿態(tài)四元數(shù)信息無法輸出的缺點，對一種基于波門預(yù)估遞推剔除大跳動星點快速求解姿態(tài)的方法進行了研究。根據(jù)四元數(shù)及姿態(tài)矩陣，采用基于距離的有效數(shù)據(jù)提取方法以剔除無效和跳變的星點數(shù)據(jù)；針對不同天區(qū)的不同星點運動幅度各異的特征，采用預(yù)估遞推波門大小可根據(jù)不同星點確定各自的波門半徑，以減少因波門半徑產(chǎn)生的無法跟蹤和波門多星識別失敗現(xiàn)象。改進了波門參數(shù)設(shè)定，并對波門跟蹤進行優(yōu)化：根據(jù)算得的旋轉(zhuǎn)角度與真實轉(zhuǎn)過角度的差異剔除無效星點并進行更新，由更新后的旋轉(zhuǎn)角和姿態(tài)四元數(shù)估計新的四元數(shù)。數(shù)學(xué)仿真結(jié)果表明：該法可有效減小星點大跳動對姿態(tài)求解的影響，提高星敏感器輸出四元數(shù)姿態(tài)的精度。

星敏感器； 波門跟蹤； 多矢量定姿； 估計遞推算法； 星點跳動； 星點剔除； 四元數(shù)； 波門參數(shù)

0 引言

星敏感器的工作原理是通過拍攝星圖、圖像預(yù)處理、星點質(zhì)心提取、計算觀測星在星敏感器測量坐標系中的分量，實現(xiàn)星圖特征提取、全天識別、星跟蹤、姿態(tài)解算等，得到星敏感器相對慣性系的姿態(tài)四元數(shù)。國內(nèi)星敏感器應(yīng)用軟件開發(fā)普遍基于DSP或AT697處理平臺，主要完成與FPGA數(shù)據(jù)交互、星敏感器工作模式管理、功能模塊實現(xiàn)，從而以恒星測量為基礎(chǔ)，進行姿態(tài)確定[1]。工程實踐中，星跟蹤匹配成功后，在進行姿態(tài)解算時一般會采用計算量較大的多矢量確定姿態(tài)的算法求解姿態(tài)[2-3]。因矢量確定姿態(tài)求解過程存在需對中間矩陣進行代數(shù)平均求解的缺陷，故在星點跳動、毛刺、干擾時會出現(xiàn)姿態(tài)輸出跳變，進而輸入至控制系統(tǒng)，造成衛(wèi)星姿態(tài)的抖動并降低相關(guān)載荷的工作效率和質(zhì)量[4-6]。針對上述問題，國內(nèi)有研究單從星敏感器姿態(tài)確定算法，分別采用q-方法、QUEST算法、ESOQ2算法、SVD算法和FOAM算法進行姿態(tài)確定求解，但由于計算均較復(fù)雜，不便于工程實現(xiàn)，在應(yīng)對星點跳動、毛刺時會出現(xiàn)姿態(tài)跳動，效果并不理想[4，7-8]。后續(xù)有文獻基于星敏感器采用多矢量確定姿態(tài)算法結(jié)合卡爾曼濾波算法以改進在外干擾下的姿態(tài)估計，但解決大跳動星點的效果也不理想[9-11]。為此，本文結(jié)合星敏感器星跟蹤時的多矢量定姿算法的特性和工程實際采集存在誤差跳動的特點，針對性地設(shè)計波門預(yù)估遞推提前剔除大跳動星點環(huán)節(jié)，提出了一種星敏感器基于波門預(yù)估遞推剔除大跳動星點快速求解姿態(tài)的算法。

1 理論分析

1.1 四元數(shù)及姿態(tài)矩陣

姿態(tài)四元數(shù)表示法不包括三角函數(shù)，無奇點，約束條件簡單，應(yīng)用廣泛，尤其適于描述大角度姿態(tài)機動問題[6]。

設(shè)坐標系O-xoyozo圍繞ON軸轉(zhuǎn)過角δ與坐標系O-xbybzb重合，ON軸與Oxo，Oyo，Ozo軸(即Oxb，Oyb，Ozb軸)間的角分別為β1，β2，β3。則O-xbybzb系相對O-xoyozo系的姿態(tài)可由δ，β1，β2，β3完全確定，即用四元數(shù)完全確定，有

q=q0+q1i+q2j+q3k

式中：i，j，k為四元數(shù)單位矢量；q0，q1，q2，q3為四元數(shù)。q的向量形式為

式中：

(1)

此處：i=1，2，3。顯然滿足約束條件

(2)

其中：代表旋轉(zhuǎn)的四元數(shù)中只有3個是獨立的。

用三角公式

根據(jù)歐拉旋轉(zhuǎn)及四元數(shù)定義，可將歐拉軸/角姿態(tài)矩陣化為四元數(shù)姿態(tài)矩陣

(3)

式中：

A12=2(q1q2+q0q3)

A13=2(q1q3-q0q2)

A21=2(q1q2-q0q3)

A23=2(q2q3+q0q1)

A31=2(q1q3+q0q2)

A32=2(q2q3-q0q1)

1.2 有效數(shù)據(jù)提取模式識別原理

有效數(shù)據(jù)提取的模式識別，實際上是剔除無效的離群點，有基于統(tǒng)計、距離、密度、深度、偏離等多種經(jīng)典方法。本文采用算法上易于工程實現(xiàn)和適合嵌入式軟件同時具備辨識能力的基于距離的有效數(shù)據(jù)提取的模式識別方法。

因星敏感器的有效數(shù)據(jù)是針對采集星點信息有效性進行提取，設(shè)由采集星點A∈(x1,…,xn)組成，其距離關(guān)系矩陣D∈(d11,…,dnn)已知。此處：xi為一個三維坐標系中的星點；dij為星點i與星點j的歐氏距離，且

(4)

由于是比較相鄰拍間的星點的信息，姿態(tài)變化很小，因此采用比較各星點變動距離之和。

1.3 星敏感器定姿原理

首先，星敏感器成像是將遠處的星點進行成像投影到星敏感器CCD平面，如圖1所示。采用小孔模型，V=[V1…VN]，U=[U1…UN]分別為慣性系中星點矢量集合和相對星敏感器體坐標系中的觀測單位矢量集合[6]。此處：Vi為第i個星點在慣性系中的星點坐標矢量；Ui為第i個星點在星敏感器體坐標系中的觀測單位矢量。圖1中：點O對應(yīng)星敏感器系統(tǒng)的成像中心，假設(shè)星敏感器Z軸與系統(tǒng)光軸重合，用多矢量確定姿態(tài)方法[1]。令此時姿態(tài)矩陣為A(即此時星敏感器體坐標系相對慣性系的姿態(tài)矩陣)，則多矢量的觀測方程為

U=AV

(5)

或

V=BU

(6)

此觀測方程的代數(shù)解為

(7)

此時求出的B為非正交矩陣。優(yōu)化后可得正交的最優(yōu)矩陣

(8)

式中：I為單位矩陣。由此，可解出姿態(tài)矩陣A。

圖1 星敏感器成像Fig.1 Star sensor imaging

1.4 波門跟蹤原理

1.4.1 傳統(tǒng)波門跟蹤方式

波門是星敏感器中用于跟蹤的一個小跟蹤視窗，以快速定位所跟蹤的星，從而快速計算出當前的姿態(tài)。傳統(tǒng)的波門大小設(shè)定時直接基于某個參數(shù)，不同跟蹤星點的跟蹤波門相同。此設(shè)計的缺陷是不同星點在不同天區(qū)時的運動幅度不一致，可能會出現(xiàn)不能跟蹤或單波門多星問題。

1.4.2 優(yōu)化波門跟蹤方式

針對不同天區(qū)的不同星點運動幅度不同的特征，本文提出一種預(yù)估遞推波門大小的方法。該法可針對不同的星點確定各自的波門半徑，可減少因波門半徑而產(chǎn)生的無法跟蹤和波門多星識別失敗的現(xiàn)象，在此基礎(chǔ)上，還對當前采集的星點進行無效數(shù)據(jù)和跳變數(shù)據(jù)剔除處理。與傳統(tǒng)波門跟蹤方式相比，本文方法的抗干擾和輸出魯棒性更強，提高了星敏感器的安全可靠性。流程如圖2所示。

圖2 星敏感器優(yōu)化后流程Fig.2 Star sensor’s process after optimized

2 星點跟蹤波門建模

2.1 經(jīng)典波門參數(shù)設(shè)定

經(jīng)典波門參數(shù)根據(jù)星敏感器CCD成像原理，從像面像素層面設(shè)定波門大小，所確定的波門大小隨不同的相機成像參數(shù)而定，同時跟蹤效果也因不同的相機成像參數(shù)而各異，易誤出現(xiàn)跟蹤波門失敗。不同星點在繞某軸運動時的運動如圖3所示。圖3中：oE軸為旋轉(zhuǎn)軸；轉(zhuǎn)過角度為θ；星點A、B、C為轉(zhuǎn)前的位置，星點A′、B′、C′為轉(zhuǎn)后的位置，各點體坐標系中坐標分別為(xA,yA,zA)，(xB,yB,zB)，(xC,yC,zC)，(xA′,yA′,zA′)，(xB′,yB′,zB′)，(xC′,yC′,zC′)。由圖3可知：其各自的運動的幅度隨與旋轉(zhuǎn)軸的夾角而不同，某實例如圖4所示。

圖3 星敏感器體坐標系中星點運動Fig.3 Star point‘s movement in star sensor’s body reference

圖4 星敏感器在本體坐標系中星點運動軌跡Fig.4 Star point’s movement trace

2.2 改進波門參數(shù)設(shè)定

改進的波門可根據(jù)各自星點的位置確定更精確的波門大小。首先，確定在固定旋轉(zhuǎn)角時的星點位置變化。建模星點繞z軸旋轉(zhuǎn)θ，如圖5所示。

圖5 星點位置變化Fig.5 Star point’s position

繞oz軸轉(zhuǎn)動時，星點在ox、oy軸上投影值變化較大，可得此時星點A′的x，y的范圍分別為

式中：φx=arctan 2(yA/xA)。同理可推導(dǎo)出z的范圍為[cos(ψ+θ),cos(ψ-θ)]。此處：ψ-θ>0。因此，在進入波門跟蹤階段設(shè)定波門，只需根據(jù)每拍間隔時間姿態(tài)變化最大角度θmax(在星敏感器能跟蹤的情況下)，選取θmax便可設(shè)定此時的波門參數(shù)為

(9)

式中：ψ>θmax。

為進一步精確跟蹤波門半徑，須確定此時的旋轉(zhuǎn)軸矢量方向。實際工程應(yīng)用中，在進入波門跟蹤時上一拍的姿態(tài)四元數(shù)信息為已知，因每拍間的運動相當小，故可用上一拍的姿態(tài)四元數(shù)中旋轉(zhuǎn)軸信息作為當前的旋轉(zhuǎn)軸。由圖5可得

(10)

oA′=

[cos(φx+θ)sinψsin(φx+θ)sinψcosψ]

(11)

則

cos(∠AoA′)=oA·oA′=

cos(φx+θ)cosφxsin2ψ+sin(φx+θ)×

sinφxsin2ψ+cos2ψ=

cosθsin2ψ+cos2ψ

(12)

同理可推得繞任意軸旋轉(zhuǎn)(如圖6所示，圖6中點C、D為A、A′在其旋轉(zhuǎn)軸垂面下的投影點)時，夾角間滿足關(guān)系

cos∠AoA′=cos∠CoDsin2∠AoE+

(13)

圖6 星點繞任意軸旋轉(zhuǎn)Fig.6 Star point’s position after rotating e vector

此時，基于波門可記錄相鄰拍時間的星點信息，由此可求出每個星點運動的夾角。對圖3所示，運動夾角

(14)

若運動夾角在旋轉(zhuǎn)軸矢量垂直平面上的夾角為真實繞四元數(shù)旋轉(zhuǎn)角δ，設(shè)oA、oB、oC軸與旋轉(zhuǎn)軸oE的夾角分別為ψA,ψB,ψC，則滿足關(guān)系

(15)

因計算時，θA,θB,θC，ψA,ψB,ψC均已知，則可得各自的相對旋轉(zhuǎn)轉(zhuǎn)過的角度

(16)

將解得的δA,δB,δC代入式(9)，可得A，B，C相應(yīng)波門的范圍。

2.3 優(yōu)化波門跟蹤設(shè)計

用上述方法解得的δA,δB,δC理論上與真實轉(zhuǎn)過角度δ一致。星敏感器鏡頭的缺陷、電路及其他噪聲的影響有可能產(chǎn)生跳變誤差較大的星點，會使δA,δB,δC與真實δ不一致，但相鄰一拍間隔較短，正常情況下計算值應(yīng)與真實值差異保持在一定的范圍內(nèi)，基于此特征，可進行星點剔除辨識。

計算中，此時的旋轉(zhuǎn)軸oE采用上一拍推導(dǎo)出的姿態(tài)四元數(shù)計算出來的旋轉(zhuǎn)軸，設(shè)上一拍的姿態(tài)四元數(shù)

(17)

旋轉(zhuǎn)軸為

(18)

針對當前進入波門跟蹤狀態(tài)，令此時的當前跟蹤星有n個，則基于優(yōu)化原則剔除數(shù)據(jù)無效的點，其代價函數(shù)

(19)

因跟蹤波門星點出現(xiàn)無效跳動數(shù)據(jù)的概率較小，考慮僅存在單一跳變數(shù)據(jù)，當Jj取最大值時，剔除此時為j的星點信息。再對剔除該星點后的旋轉(zhuǎn)角度進行更新，更新后的最佳估計旋轉(zhuǎn)角

(20)

(21)

式中：

(22)

3 優(yōu)化波門驗證

設(shè)數(shù)學(xué)仿真的基本參數(shù)為：星點矢量

真實的旋轉(zhuǎn)軸

oE=

姿態(tài)機動每拍(200 ms)機動的角度0.003 rad。對機動后的oA，oB，oC的測量誤差引入0.001 rad的白噪聲進行干擾，仿真所得多矢量姿態(tài)與估計遞推算法輸出誤差如圖7所示。

圖7 多矢量姿態(tài)與估計遞推算法輸出誤差Fig 7 Multi vector calculating attitude comparing with predict and estimating attitude method

由圖7可知：用本文波門遞推預(yù)估四元數(shù)方法鎖的結(jié)果不僅與真實姿態(tài)四元數(shù)誤差很小，而且抖動小于傳統(tǒng)用多矢量確定姿態(tài)的星敏感器輸出，其輸出曲線相對平滑。

為驗證本文方法在星敏感器采集星點出現(xiàn)大跳動誤差或錯誤時的有效性，引入第4個星點

oD=

[0.308 244 50 0.450 036 0 0.838 124 6]

其中針對該星點的測量時引入隨機誤差閾值0.01 rad的白噪聲，分別用波門遞推預(yù)估四元數(shù)、傳統(tǒng)多矢量定姿、改進剔除傳統(tǒng)多矢量定姿，以及不剔除誤差大星點波門遞推預(yù)估四元數(shù)4種方法進行處理，結(jié)果如圖8所示。

圖8 四種方法仿真結(jié)果比較Fig.8 Simulation results of four methods

由圖8可知：如未剔除誤差較大的星點，即使采用波門遞推預(yù)估四元數(shù)方法也會出現(xiàn)很大的抖動，從而使星敏感器的性能下降；如采用未剔除誤差較大星點，傳統(tǒng)多矢量方法的抖動相對較大，采用波門預(yù)測剔除誤差較大星點的改進多矢量方法，姿態(tài)輸出抖動也很大，但優(yōu)于無改進的傳統(tǒng)多矢量方法；采用剔除誤差較大星點的波門遞推預(yù)估方法，抖動較小，相對真實姿態(tài)誤差更小。

4 結(jié)束語

為使星敏感器在進行波門跟蹤時遇到采集星點存在大跳動誤差時也能快速進行星跟蹤和完成姿態(tài)輸出，本文給出了一種星敏感器無效星點數(shù)據(jù)優(yōu)先剔除算法和基于波門有效姿態(tài)快速遞推算法的解決方案。由仿真數(shù)學(xué)結(jié)果可知：該算法滿足有效剔除大跳動星點，能有效削弱在進行波門跟蹤時采集星點大跳動產(chǎn)生的對星敏感器的影響，同時改善星敏感器的輸出四元數(shù)姿態(tài)的精度，可明顯提高星敏感器的性能和安全可靠性。本文創(chuàng)新地利用波門遞推進行提前剔除大跳動星點，同時結(jié)合卡爾曼濾波又進一步增加姿態(tài)輸出的魯棒性。但由于波門跟蹤基于姿態(tài)遞推，在快速運動時，該方法會存在星跟蹤跟不上的現(xiàn)象，這也是本文方法的不足之處。后續(xù)，將對在快速機動下星跟蹤的快速和穩(wěn)定輸出準備和穩(wěn)定的姿態(tài)進行研究。

[1] 章仁為. 衛(wèi)星軌道姿態(tài)動力學(xué)與控制[M]. 北京: 北京航空航天大學(xué)出版社, 2011.

[2] 朱長征. 基于星敏感器的星模式識別算法及空間飛行器姿態(tài)確定技術(shù)研究[D]. 長沙: 國防科學(xué)技術(shù)大學(xué), 2004.

[3] 徐櫻， 吳德安， 汪禮成， 等. 星敏感器慢變誤差校準方法研究[J]. 上海航天， 2016， 33(4)： 63-69.

[4] 張力軍. 基于多視場星敏感器的航天姿態(tài)確定方法研究[D]. 長沙: 國防科學(xué)技術(shù)大學(xué), 2011.

[5] 姜雪原. 衛(wèi)星姿態(tài)確定及敏感器誤差修正的濾波算法研究[D]. 哈爾濱: 爾濱工業(yè)大學(xué)工學(xué), 2006.

[6] 楊大明. 空間飛行器姿態(tài)控制系統(tǒng)[M]. 哈爾濱: 哈爾濱工業(yè)大學(xué)出版社, 2000.

[7] 謝俊峰. 衛(wèi)星星敏感器定姿數(shù)據(jù)處理關(guān)鍵技術(shù)研究[D]. 武漢: 武漢大學(xué), 2009.

[8] 邢飛, 董瑛, 武延鵬, 等. 星敏感器參數(shù)分析與自主校正[J]. 清華大學(xué)學(xué)報(自然科學(xué)版), 2005, 45(11):1484-1488.

[9] 林玉榮, 鄧正隆. 基于星敏感器估計衛(wèi)星姿態(tài)的預(yù)測Kalman濾波算法[J]. 中國科學(xué)(E輯), 2002, 32(6): 817-823.

[10] 林玉榮, 鄧正隆. 基于星敏感器的星載慣性基準誤差的實時估計與補償[J]. 中國慣性技術(shù)學(xué)報, 1999(04).

[11] 李捷. 利用星敏感器的衛(wèi)星姿態(tài)確定的信息濾波[J]. 中國空間科學(xué)技術(shù), 1995, 15(2): 15-21.

A Rapid Calculating Attitude Method for Star Sensor Based on Tracing Window Using Predicting and Estimating

LIU Teng-jun, LIN Rong-feng, ZHU Yan-qing, ZHOU Yu, XIAO Dong-dong

(Shanghai Institute of Spaceflight Control Technology, Shanghai 201109, China)

Taking the star sensor’s demanding for rapidity and stability and higher accuracy into consideration, a rapid calculating attitude method for star sensor based on tracing window using predicting and estimating was studied in this paper, because traditional star sensor showed star tracing failure and quaternion not being able to output while dealing with star points in tracing window having a big mistake jump. According to quaternion and attitude matrix, the extraction method of effective data was applied to reject the data of the ineffective and jumping star point based on distance. The various tracing windows related to different star points would be determined by predicting and estimating because individual star had its motion characteristic, which could reduce tracing failure caused by tracing window and multi-star identification failure. The setting of the tracing window parameters was improved. The tracing window was optimized. The ineffective star point was rejected according to the difference between the rotation angular computed and real rotational angular and the data set was updated. The new attitude quaternion was estimated by updated rotational angular and attitude quaternion. The numerical simulation results showed that this method could reduce the effect of large jumping star point on attitude determination greatly and improve the output accuracy of attitude quaternion for star sensor.

star sensor; tracing window; multi vector calculating attitude; predicting and estimating; star point jump; error star points eliminating; quaternion; tracing window parameter

1006-1630(2017)03-0116-06

2016-09-15；

2016-12-08

劉騰駿(1988—)，男，碩士，主要研究方向為衛(wèi)星姿控系統(tǒng)軟件設(shè)計與開發(fā)。

V448

A

10.19328/j.cnki.1006-1630.2017.03.016