星敏感器姿態(tài)測量算法研究

段 輝, 周召發(fā),*, 張志利, 徐志浩, 郝詩文, 曾 進(jìn)

(1. 火箭軍工程大學(xué)導(dǎo)彈工程學(xué)院, 陜西 西安 710025;2. 火箭軍工程大學(xué)兵器發(fā)射理論和技術(shù)國家重點(diǎn)學(xué)科實驗室, 陜西 西安 710025;3. 飛行自動控制研究所慣性技術(shù)航空科技重點(diǎn)實驗室, 陜西 西安 710065)

0 引 言

星敏感器視場內(nèi)可同時觀測多顆恒星,每顆恒星在天球系與星敏系中具有不同的矢量坐標(biāo)。因此,星敏感器可同時獲得多個矢量對信息。利用這些矢量對信息,即可基于多矢量定姿算法解算出星敏系到天球系的坐標(biāo)系變換矩陣C1,進(jìn)而確定星敏感器的坐標(biāo)軸在天球系中的指向。星敏感器在載體上固定安裝,載體系到星敏系的坐標(biāo)系轉(zhuǎn)換矩陣C2(即安裝矩陣)事先已經(jīng)標(biāo)定好,載體系到天球系的坐標(biāo)系轉(zhuǎn)換矩陣則為C1C2。幾乎所有的確定性姿態(tài)算法都是基于Grace Wahba在1965年提出的一個問題,稱為Wahba問題。Wahba問題通過最小化損失函數(shù)找到最優(yōu)姿態(tài)矩陣[1],求解Wahba問題的目的是試圖找到使損失函數(shù)最小的姿態(tài)矩陣C。

Farrell等[2]從矩陣偽逆的角度出發(fā)求解Wahba問題,仿真實驗結(jié)果表明,估計出的三軸姿態(tài)穩(wěn)定性差,具有較弱的魯棒性,且計算量大,處理速度慢。Horn等[3]通過理論推導(dǎo)指出,求解矩陣特征值及其對應(yīng)的特征向量時,傳統(tǒng)的方法會同時將所有的特征值及其對應(yīng)的特征向量都求解出來,而在對三軸姿態(tài)作最優(yōu)估計時,往往只需要其中的最大特征值及其對應(yīng)的特征向量,其余值都是冗余的,因此會提高算法復(fù)雜度,浪費(fèi)計算資源。所以,為避免該問題,Horn等將瑞利商引入到最優(yōu)姿態(tài)估計中。Markley等[4]并沒有采用傳統(tǒng)的求解思路,而是通過理論推導(dǎo)將最優(yōu)姿態(tài)估計問題轉(zhuǎn)化為已知數(shù)據(jù)構(gòu)成的矩陣的奇異值分解(singular value decomposition, SVD)問題,對矩陣進(jìn)行SVD可以充分保證算法的魯棒性,但相對而言在算法執(zhí)行效率上需要作出讓步。在求解特征值及其對應(yīng)特征向量時,Yang等[5]推導(dǎo)出了一種新的求解一元四次方程根的方法,進(jìn)而求解出相應(yīng)矩陣的最大特征值及其特征向量,該方法未存在近似步驟,屬于純解析解法,因此精度高且執(zhí)行效率高,但前提是方程必須具有實根。Wu等[6]提出了線性估計算法,該方法不依賴于Davenport的q方法,而是另辟蹊徑將最優(yōu)姿態(tài)四元數(shù)的二次求解問題轉(zhuǎn)化為一次求解問題,最終通過求解矩陣的特征值與特征向量得到姿態(tài)的最優(yōu)估計。Shuster等[7]給出了一種新的姿態(tài)四元數(shù)估計(quaternion estimator, QUEST)算法,該方法首先基于哈密爾頓-凱勒定理構(gòu)造封閉形式的特征多項式,并通過理論推導(dǎo)出最接近1的特征值所對應(yīng)的特征向量就是要求的最優(yōu)姿態(tài)四元數(shù),最后,使用非解析的迭代方法求解。該方法中迭代次數(shù)無法給出一個確定值,所以迭代效果有時不太穩(wěn)定。此外,鄭振宇等[8]將無緯度支持定姿問題轉(zhuǎn)換為對地軸矢量在參考坐標(biāo)系下投影的解算問題,針對晃動基座下的地軸矢量解算問題,提出了一種基于旋轉(zhuǎn)四元數(shù)的軸向矢量優(yōu)化解算方法,并通過船載實驗驗證了該解算方法的優(yōu)越性。Yan[9]為了增強(qiáng)姿態(tài)陣優(yōu)化算法的實用性,給出了一種快速SVD優(yōu)化算法,其浮點(diǎn)乘法計算量與四元數(shù)優(yōu)化快速算法相近。

1 坐標(biāo)系定義

天球坐標(biāo)系與星敏感器坐標(biāo)系的相對姿態(tài)如圖1所示。

圖1 天球系和星敏系相對關(guān)系圖Fig.1 Relative diagram of celestial sphere system and star sensor system

天球坐標(biāo)系Oixiyizi(第二赤道坐標(biāo)系,簡稱i系):原點(diǎn)是春分點(diǎn),基圈是天赤道,始圈是春分圈。天球坐標(biāo)系的經(jīng)度稱赤經(jīng),緯度稱赤緯。其赤經(jīng)自春分點(diǎn)向東度量,屬于左旋系統(tǒng)。赤緯自天赤道起沿天體所在的赤經(jīng)圈向南北兩個方向度量,為0°～90°。按北半球習(xí)慣,天赤道以北為正,天赤道以南為負(fù)[10-11]。

星敏感器坐標(biāo)系Osxsyszs(簡稱s系):原點(diǎn)在星敏感器的透鏡中心,Osxs軸沿光軸方向,Oszs軸沿透鏡平面橫軸向右,Osys軸沿透鏡平面內(nèi)垂直于Oszs的方向,并與Osxs、Oszs軸遵循右手法則。星敏感器坐標(biāo)系與像平面坐標(biāo)系的關(guān)系如圖2所示。

圖2 星光矢量成像示意圖Fig.2 Schematic diagram of star light vector imaging

天球坐標(biāo)系與星敏感器坐標(biāo)系的相對姿態(tài)可由星敏感器的姿態(tài)角給出,星敏感器的姿態(tài)角定義為偏航角α、俯仰角δ和滾動角κ。偏航角α為Osxs軸正方向所代表的矢量在天球坐標(biāo)系下的赤經(jīng);俯仰角δ為Osxs軸正方向所代表的矢量在天球坐標(biāo)系下的赤緯;天球坐標(biāo)系繞Oizi軸旋轉(zhuǎn)偏航角α、繞Oiyi軸旋轉(zhuǎn)俯仰角δ后,Oixi軸與Osxs軸重合,此時天球坐標(biāo)系只需再沿Oixi軸順時針旋轉(zhuǎn)κ度即可與星敏坐標(biāo)系重合,這個角度κ定義為橫滾角[12-15]。

(1)

需注意的是,實際運(yùn)用中普遍是利用羅德里格斯旋轉(zhuǎn)公式得到姿態(tài)矩陣與姿態(tài)四元數(shù)的轉(zhuǎn)換關(guān)系,進(jìn)而將姿態(tài)矩陣求解問題轉(zhuǎn)化為姿態(tài)四元數(shù)求解問題以得到姿態(tài)四元數(shù)q,從而避免了引入奇異性問題并在一定程度上提高計算效率,具體的求解算法見下文。

2 星敏感器測量模型

星敏感器的姿態(tài)確定過程可簡述為:進(jìn)入星敏感器視場內(nèi)的恒星星光經(jīng)過電荷耦合器件(charge-coupled device, CCD)平面的光電轉(zhuǎn)換形成數(shù)字星圖,星圖經(jīng)去噪、星點(diǎn)提取后進(jìn)行質(zhì)心定位操作。至此,星光矢量的星敏系坐標(biāo)已得到,再經(jīng)過星圖識別,則可得到CCD平面上星點(diǎn)的星表編號與赤經(jīng)、赤緯等信息,進(jìn)而得到星光矢量在天球坐標(biāo)系中的三維坐標(biāo)[16-19]。根據(jù)多個星光矢量的矢量信息對,就可以通過確定性姿態(tài)算法解算出天球坐標(biāo)系到星敏坐標(biāo)系的坐標(biāo)變換矩陣。恒星星光矢量在CCD平面上的成像過程如圖2所示,其理想的變換關(guān)系為

(2)

恒星星光矢量的像平面系坐標(biāo)為p(u,v)[20]。

某一時刻,進(jìn)入星敏感器視場內(nèi)的恒星有n顆,經(jīng)過一系列預(yù)處理步驟后,可得這n顆恒星的矢量信息對,其在星敏系和天球系中的坐標(biāo)分別為

(3)

(4)

星敏感器的實際姿態(tài)測量模型為[25-31]

(5)

3 星敏感器姿態(tài)確定算法理論推導(dǎo)

3.1 LS算法

最小二乘(least square, LS)法主要用于參數(shù)估計,運(yùn)用于天文定姿領(lǐng)域時,則可把姿態(tài)矩陣的9個非獨(dú)立元素看作是9個參數(shù),利用星光矢量的三維坐標(biāo)信息對這9個參數(shù)進(jìn)行估計。最小二乘估計的模型可以表示為

AX=B

(6)

式中:A為n×m的數(shù)據(jù)矩陣且n≥m;X為m×1的待估參數(shù);B為n×1的參考向量,一般假設(shè)B不帶測量噪聲。式(6)兩邊左乘AT,并同時左乘(ATA)-1,即可得最小二乘解X=(ATA)-1ATB。

需要注意的是,數(shù)據(jù)矩陣A必須滿足rank(A)=m,即A為列滿秩矩陣,這樣才能保證ATA為可逆矩陣,最小二乘解才存在。

(7)

(8)

基于式(8),即可求解出最小二乘意義下的[C11C12C13]。同理,可求解出[C21C22C23]、[C31C32C33]。

3.2 雙矢量定姿算法

假設(shè)已識別出兩顆視場內(nèi)的恒星S1、S2,星敏感器坐標(biāo)系和天球坐標(biāo)系分別為Fs、Fi,S1、S2在星敏系中的坐標(biāo)為Us、Vs,在天球系中的坐標(biāo)為Ui、Vi(具體形式見式(3)),則可知下式成立:

(9)

式中:Us、Vs、Ui、Vi為3×1維坐標(biāo)向量;Fs、Fi為3×3維坐標(biāo)系矩陣。

(10)

式中:C1為Fm和Fs之間的坐標(biāo)系變換矩陣。

同理,還能得到Fm坐標(biāo)系的另一種表達(dá)形式:

(11)

式中:C2為Fm和Fi之間的坐標(biāo)系變換矩陣。

聯(lián)立式(10)和式(11),可得

C1Fs=C2Fi?Fs=CFi

(12)

3.3 SVD算法

(13)

式中：ak為非負(fù)加權(quán)因子,由傳感器的測量精度決定,一般有a1+a2+…an=1。

展開式(13)中的2范數(shù)平方項:

(14)

將式(14)代入式(13),得

(15)

(16)

對優(yōu)化函數(shù)的最優(yōu)值而言,J(C)opt≠J*(C)opt,但這兩個優(yōu)化函數(shù)的最優(yōu)解相等,都為Copt。

由于矩陣的跡具有非常良好的運(yùn)算性質(zhì),下面將式(16)改寫成矩陣乘積的求跡,可得

(17)

其中,記

(18)

通常情況下,矩陣A都滿足SVD的條件,令A(yù)=UDVT,D=diag(σ1,σ2,σ3),σi為奇異值,U為左奇異向量構(gòu)成的正交矩陣,V為右奇異向量構(gòu)成的正交矩陣,則進(jìn)一步可得:

J(C)=tr(CAT)=tr(C(UDVT)T)=

tr(CVDUT)=tr(UTCVD)=tr(C*D)

(19)

J(C)=tr(C*D)=

(20)

Copt=UVT

(21)

由此,在測量誤差加權(quán)平方和最小的意義下,可通過上述SVD估計出最優(yōu)姿態(tài)矩陣C*,進(jìn)而得到3個姿態(tài)角。

3.4 QUEST算法

(22)

式中：qv×為矢量qv的反對稱矩陣。

將式(22)代入式(17),可將優(yōu)化函數(shù)自變量轉(zhuǎn)換成q,即：

(23)

J(q) =

(24)

式中,矩陣M的形式如下:

(25)

觀察可知,J(q)是不帶一次項和常數(shù)項的二次函數(shù),具有良好的求導(dǎo)性質(zhì)。由于q為四元數(shù),其具有隱含性質(zhì)|q|=qTq=1。因此,考慮為帶約束優(yōu)化問題求解極值:

J′(q)=qTMq+λ[1-qTq]

(26)

利用矩陣求導(dǎo)的性質(zhì)對式(26)求導(dǎo),可得

Mq=λq

(27)

式中:M為4×4維矩陣,q為4×1維向量,λ為1×1維實數(shù)。顯然,λ、q恰好為矩陣M的特征值、特征向量。將式(27)代入式(24),可得

J(q)=qTMq=qTλq=λqTq=λ

(28)

由于J(q)=λ,λ需要取最大特征值才能使目標(biāo)優(yōu)化函數(shù)J(q) 最大,此時特征值對應(yīng)的特征向量即為最優(yōu)姿態(tài)四元數(shù)qopt。

可以證明,矩陣M最大特征值λmax非常接近于1,這位迭代法的解算提供了很好的先驗信息,因此,可以1為初始值基于牛頓迭代法對特征值λ進(jìn)行迭代。矩陣的特征多項式f(λ):

f(λ)=det(W-λI)=λ4+τ1λ2+τ2λ+τ3

(29)

式中:τ1、τ2、τ3由矩陣M推導(dǎo)得到,屬于已知量。接著,對f(λ)求一階導(dǎo)可得:f′(λ)=4λ3+2τ1λ+τ2,即最大特征值λmax的迭代公式為

(30)

通常情況下,在經(jīng)過2～4次迭代后,最大特征值λmax的精度就能穩(wěn)定在一個非常高的水平。獲得λmax后,即可基于矩陣等式Mq=λq,通過初等行變換得到最大特征值λmax對于的特征向量,即最優(yōu)姿態(tài)四元數(shù)qopt。

3.5 線性估計算法[6]

原始Wahba問題構(gòu)造的最優(yōu)目標(biāo)函數(shù)見式(13)。最小化式(13)等價于求下式的最優(yōu)解C,C使得ek的絕對值之和最小:

(31)

式中:ek表示第k個誤差項。理想情況下ek=0。

設(shè)某一觀測矢量對如下:

(32)

(33)

C1、C2、C3是姿態(tài)矩陣C的列向量,q為C對應(yīng)的四元數(shù)形式,則

(34)

同理,可得C2=P2q、C3=P3q,則

(35)

(36)

由此,最優(yōu)目標(biāo)函數(shù)可寫成如下形式:

(37)

假設(shè)星敏感器無測量誤差,導(dǎo)航星表也沒有赤經(jīng)、赤緯誤差,即J*(q)=0,可得

(38)

(39)

q?=qT=(q0,q1,q2,q3)

(40)

對于長方矩陣求逆,存在左偽逆矩陣、右偽逆矩陣和M-P偽逆矩陣3種類型。其中,M-P偽逆矩陣性質(zhì)最好,但要求也最高,對于某個矩陣X,其M-P偽逆矩陣X?需要滿足如下4個條件:

(41)

接著,對矩陣PΣD進(jìn)行如下變換:

UDxP1+UDyP2+UDzP3

(42)

式中:UDx,UDy,UDz是3n×3的矩陣。將式(42)代入式(41)左側(cè),可得

(43)

令，

(44)

(45)

因此,式(39)可改寫為

HxP1+HyP2+HzP3-q?=0

(46)

對式(46)進(jìn)行轉(zhuǎn)置操作并展開Hx,Hy,Hz,整理可得

(W-I)q=0

(47)

式中：矩陣W的各元素值見文獻(xiàn)[6]。

上述結(jié)果是在理想情況下,即J*(q)=0時推導(dǎo)得到的,所以需要在式(47)中添加一個四元數(shù)誤差項εq,進(jìn)而可得

Wq=(1+ε)q

(48)

式中:ε為誤差因子。顯然,1+ε是矩陣W的一個特征值,且1+ε非常接近于1。因此,求解出矩陣W最接近1的特征值λopt及其對應(yīng)的特征向量qopt即可。

4 仿真驗證及分析

本節(jié)從數(shù)值仿真的角度對SVD定姿算法、雙矢量定姿算法、LS定姿算法、QUEST定姿算法和線性估計算法的理論分析進(jìn)行驗證,仿真步驟如下:

步驟3分別采用雙矢量定姿算法、LS定姿算法和線性估計算法等求解姿態(tài),并計算不同定姿算法的姿態(tài)角誤差,雙矢量定姿算法仿真時隨機(jī)選取其中兩個矢量信息對實現(xiàn)定姿;

步驟4計算不同算法定姿需要消耗的時間。多次重復(fù)上述步驟。

仿真使用Matlab 2018b,硬件參數(shù)為Interi7核心處理器,32G內(nèi)存。

下面,以噪聲標(biāo)準(zhǔn)差為1e-3和1e-5為例展開分析。噪聲標(biāo)準(zhǔn)差為1e2和1e-4時的性能表現(xiàn)如表1和表2所示。

表1 算法的姿態(tài)角誤差標(biāo)準(zhǔn)差

表2 算法的耗時圖

圖3可粗略看出,1e-3的噪聲標(biāo)準(zhǔn)差下線性估計算法、QUEST算法和SVD算法的姿態(tài)角誤差量集中性較好,基本都未超過3e-2的限值,這3種算法的誤差量主要取決于恒星星光在天球系和星敏系中坐標(biāo)矢量所受的噪聲強(qiáng)弱;LS算法的姿態(tài)角誤差量集中性稍差一些,主要分布在于±6e-2之間,該算法的誤差量除了受恒星星光坐標(biāo)矢量的噪聲影響外,其算法本身的模型也對定姿精度產(chǎn)生了一定的限值,即該算法并不是最優(yōu)的;雙矢量定姿算法的姿態(tài)角誤差量集中性最差,定姿效果最差,這主要是因為該算法在定姿過程中只用到了兩個恒星星光的矢量信息對,所以對噪聲非常敏感。歸算仿真得到的定姿數(shù)據(jù),1e-3的噪聲標(biāo)準(zhǔn)差下不同算法的姿態(tài)角誤差標(biāo)準(zhǔn)差如表1所示,隨機(jī)生成的1 000組仿真實驗中姿態(tài)角誤差的詳細(xì)分布情況如下。

圖3 1e-3標(biāo)準(zhǔn)差下不同算法的三軸姿態(tài)角誤差Fig.3 Three-axis attitude angle error of different algorithms under 1e-3 standard deviation

同樣地,圖4可粗略看出,1e-5的噪聲標(biāo)準(zhǔn)差下線性估計算法、QUEST算法和SVD算法的姿態(tài)角誤差量集中性較好,基本都未超過6e-4的限值;LS算法的姿態(tài)角誤差量集中性稍差一些,主要分布在于±8e-4之間;雙矢量定姿算法的姿態(tài)角誤差量集中性最差,定姿效果最差。歸算仿真得到的定姿數(shù)據(jù),1e-5的噪聲標(biāo)準(zhǔn)差下不同算法的姿態(tài)角誤差標(biāo)準(zhǔn)差如表1所示,隨機(jī)生成的1 000組仿真實驗中姿態(tài)角誤差的詳細(xì)分布情況如下。由表1可知,1e-3的噪聲標(biāo)準(zhǔn)差與1e-5的噪聲標(biāo)準(zhǔn)差下算法的性能表現(xiàn)一致,即雙矢量定姿算法由于所利用的信息量少,定姿精度最差;LS算法由于并不是全局最優(yōu),定姿精度居中;其他3種算法定姿精度相當(dāng)。

圖4 1e-5標(biāo)準(zhǔn)差下不同算法的三軸姿態(tài)角誤差曲線Fig.4 Three-axis attitude angle error curve of different algorithms under 1e-5 standard deviation

不同算法的計算效率主要取決于星光矢量的數(shù)目與算法本身的復(fù)雜度,與星光矢量坐標(biāo)所受噪聲強(qiáng)度無關(guān),表2中數(shù)據(jù)處理結(jié)果也可看出?？梢钥闯?LS算法的計算效率最低,基于處于1e-4 s的水平;線性估計算法的計算效率最高,基于處于4e-5 s的水平;QUEST、SVD算法的計算效率居中,基于處于7e-5 s的水平。由此可得,各算法的計算效率由低到高依次為LS算法、QUEST算法、SVD算法、線性估計算法。

綜上所述,仿真實驗表明雙矢量定姿算法由于算法本身只采用了部分信息,其定姿精度最差;LS算法并不是全局意義上的最優(yōu)姿態(tài)估計,其定姿精度次之;QUEST算法、SVD算法和線性估計算法定姿精度最高,得益于該3種算法都是對三軸姿態(tài)角的最優(yōu)估計,區(qū)別在于推導(dǎo)方式不同而已。此外,各算法的計算效率由低到高依次為LS算法、QUEST算法、SVD算法、線性估計算法,即線性估計算法[6]的綜合性能最好。

5 結(jié) 論

本文從理論上對星敏感器定姿SVD算法、雙矢量定姿算法、LS算法、QUEST算法和線性估計算法進(jìn)行了詳細(xì)的理論推導(dǎo)。其中,線性估計算法充分利用偽逆矩陣的性質(zhì)對二次四元數(shù)矩陣方程進(jìn)行降次,有效建立了解決Wahba問題的線性理論。建立目標(biāo)優(yōu)化函數(shù),利用星光矢量在天球系和星敏系中的坐標(biāo)信息,對幾種不同算法的定姿性能進(jìn)行仿真對比分析。理論分析和仿真實驗結(jié)果表明,線性估計算法和SVD、QUEST算法的定姿精度最高,線性估計算法的計算效率最高,即線性估計算法的綜合性能最好。