張東林，曹一凡，段戰(zhàn)勝，王鵬程，郭 明，張永合

（1.西安交通大學(xué) 自動(dòng)化科學(xué)與工程學(xué)院，西安 710049；2.中國(guó)科學(xué)院 微小衛(wèi)星創(chuàng)新研究院，上海 201204）

引言

近年來(lái)，引力波探測(cè)已經(jīng)成為當(dāng)代物理學(xué)重要的前沿領(lǐng)域之一。其中空間引力波探測(cè)是指在太空中利用衛(wèi)星編隊(duì)或星座構(gòu)建大型激光干涉儀，實(shí)現(xiàn)對(duì)中低頻引力波信號(hào)的探測(cè)[1]。歐洲航天局（European Space Agency，ESA） 于1993年最早提出了空間引力波探測(cè)計(jì)劃“激光干涉測(cè)量空間天線”（Laser Interferometer Space Antenna，LISA）項(xiàng)目，中國(guó)在2008年開(kāi)始探討空間引力波探測(cè)的可行性，先后開(kāi)展了概念與方案設(shè)計(jì)、關(guān)鍵科學(xué)載荷研制等，并于2014年和2016年提出了“天琴計(jì)劃”和“太極計(jì)劃”[2-3]。

激光干涉測(cè)量對(duì)衛(wèi)星無(wú)拖曳與姿態(tài)控制系統(tǒng)提出了極高的要求。引力波信號(hào)在空間中極其微弱，而探測(cè)環(huán)境存在著強(qiáng)噪聲的干擾，因此探測(cè)系統(tǒng)必須保持足夠的穩(wěn)定才有可能捕獲引力波信號(hào)[4-5]。此外，由于激光器功率有限且測(cè)量光束發(fā)散角很小，需要衛(wèi)星進(jìn)行精確的姿態(tài)控制實(shí)現(xiàn)高精度、高穩(wěn)定度指向。衛(wèi)星高精度姿態(tài)估計(jì)作為控制律設(shè)計(jì)和執(zhí)行機(jī)構(gòu)高精度執(zhí)行的前提，顯得尤為重要[6-7]。

衛(wèi)星姿態(tài)確定方法通?？梢苑譃榇_定性方法和狀態(tài)估計(jì)法兩類(lèi)[8-9]。確定性方法無(wú)需姿態(tài)的先驗(yàn)知識(shí)和動(dòng)態(tài)變化信息，直接利用各個(gè)時(shí)刻的矢量觀測(cè)進(jìn)行姿態(tài)確定[10]。TRIAD（TRIaxial Attitude Determination）姿態(tài)確定方法是第一個(gè)使用兩個(gè)矢量觀測(cè)確定航天器姿態(tài)的廣泛普及方法[11]。除此之外還有QUEST （QUaternion ESTimator） 法[11]、SVD （Singular Value Decomposition） 法[12]、FOAM （Fast Optimal Attitude Matrix）法[12]以及Euler-q法[13]等。確定性方法具有明確的物理或幾何意義，但該類(lèi)方法要求參考矢量的參數(shù)足夠精確。狀態(tài)估計(jì)法是結(jié)合傳感器測(cè)量方程和衛(wèi)星狀態(tài)方程，基于一定的估計(jì)準(zhǔn)則，實(shí)時(shí)得到衛(wèi)星狀態(tài)變量的最優(yōu)估計(jì)方法[14]。與確定性方法不同，狀態(tài)估計(jì)法通常能提供被估計(jì)量的統(tǒng)計(jì)最優(yōu)解。在一定程度上抑制了某些不確定性因素的影響，提高姿態(tài)確定的精度，因此在高精度姿態(tài)確定系統(tǒng)中多采用狀態(tài)估計(jì)法。

卡爾曼濾波器已經(jīng)在航天器姿態(tài)估計(jì)領(lǐng)域得到了廣泛的應(yīng)用[15]。為了更好地適應(yīng)航天器動(dòng)力學(xué)和量測(cè)模型的非線性，擴(kuò)展卡爾曼濾波器得到了推廣。隨著四元數(shù)被用于姿態(tài)描述參數(shù)，Lefferts[16]提出了基于四元數(shù)的擴(kuò)展卡爾曼姿態(tài)估計(jì)方法。然而使用四元數(shù)參數(shù)化的傳統(tǒng)卡爾曼濾波器存在協(xié)方差矩陣奇異的問(wèn)題。隨著星載計(jì)算機(jī)性能的提高，一些先進(jìn)的非線性濾波算法應(yīng)用到了衛(wèi)星姿態(tài)估計(jì)系統(tǒng)中。無(wú)跡卡爾曼濾波[17-18]，中心差分卡爾曼濾波算法[19]以及粒子濾波算法[20]相較于擴(kuò)展卡爾曼方法具有更快的收斂速度和更高的精度。此外，由于基于容積卡爾曼濾波[21]的容積四元數(shù)估計(jì)器更適用于高維的非線性系統(tǒng)，因此它也被廣泛應(yīng)用于航天器系統(tǒng)狀態(tài)估計(jì)中。

基于星敏感器和慣性傳感器測(cè)量的空間引力波探測(cè)航天器系統(tǒng)狀態(tài)估計(jì)本質(zhì)上是一個(gè)非線性約束估計(jì)融合問(wèn)題。上述已有的常規(guī)非線性濾波算法無(wú)法直接應(yīng)用于該航天器系統(tǒng)狀態(tài)估計(jì)問(wèn)題。在本文中，提出了一種線性化四元數(shù)量測(cè)的卡爾曼濾波算法，以實(shí)現(xiàn)空間引力波探測(cè)任務(wù)中航天器狀態(tài)的高精度實(shí)時(shí)估計(jì)。該方法結(jié)合平臺(tái)的超穩(wěn)超靜特性，通過(guò)對(duì)四元數(shù)姿態(tài)測(cè)量在航天器小角度變化下的近似變形來(lái)滿足卡爾曼濾波器的線性假設(shè)條件，并結(jié)合航天器系統(tǒng)離散時(shí)間狀態(tài)空間模型和多源異構(gòu)數(shù)據(jù)實(shí)現(xiàn)航天器系統(tǒng)狀態(tài)的高精度估計(jì)。在空間引力波探測(cè)任務(wù)中的捕獲建鏈階段，指向估計(jì)精度應(yīng)達(dá)到0.1 μrad量級(jí)。航天器系統(tǒng)具有高精度的測(cè)量傳感器，狀態(tài)估計(jì)任務(wù)則需要滿足航天器姿態(tài)量測(cè)誤差壓縮率高于40%。設(shè)計(jì)的狀態(tài)估計(jì)器將系統(tǒng)傳感器噪聲進(jìn)一步壓低了至少一個(gè)數(shù)量級(jí)，滿足空間引力波探測(cè)航天器系統(tǒng)狀態(tài)高精度實(shí)時(shí)估計(jì)的需要，是引力波探測(cè)重大科學(xué)實(shí)驗(yàn)計(jì)劃順利實(shí)施的前提。

1 狀態(tài)空間建模

1.1 狀態(tài)方程建模

應(yīng)用牛頓–歐拉公式，可得到空間引力波探測(cè)中航天器系統(tǒng)動(dòng)力學(xué)模型，其線性化后相應(yīng)的二階微分方程為

由于M陣可逆，將等式兩側(cè)同乘M-1，得到

由上述狀態(tài)空間建模得到系統(tǒng)連續(xù)時(shí)間的狀態(tài)空間描述，已知這是一個(gè)近似線性時(shí)不變的系統(tǒng)，假設(shè)系統(tǒng)采樣周期為T(mén)，由線性系統(tǒng)理論可知，連續(xù)時(shí)間的LTI （Linear Time Invariant）系統(tǒng)的非齊次方程的解為

對(duì)于式（4），考慮從t0=kT到t=(k+1)T之間的時(shí)間段，并由于這一時(shí)間段內(nèi)u(t)=u(kT)，d(t)=d(kT)以及w(t)=w(kT)，可以得到

對(duì)于積分項(xiàng)，令t=(k+1)T-τ，則dτ=-dt。由于在一個(gè)采樣周期內(nèi)系統(tǒng)可看作LTI系統(tǒng)，因此可以將積分項(xiàng)內(nèi)的B′、F′、G′項(xiàng)移到積分項(xiàng)外。通過(guò)化簡(jiǎn)整理可得到

將式（6）簡(jiǎn)記為

備注：由于航天器系統(tǒng)動(dòng)力學(xué)模型中質(zhì)量矩陣M是非對(duì)角矩陣，因此航天器本體姿態(tài)和兩個(gè)檢驗(yàn)質(zhì)量塊的位姿存在耦合關(guān)系。如果不考慮兩個(gè)檢驗(yàn)質(zhì)量塊的位姿信息，就無(wú)法實(shí)現(xiàn)航天器姿態(tài)的高精度估計(jì)。此外，在空間引力波探測(cè)任務(wù)中，星間激光鏈路的建立和保持需要協(xié)調(diào)控制衛(wèi)星及兩個(gè)檢驗(yàn)質(zhì)量塊來(lái)共同完成。因此，在航天器姿態(tài)估計(jì)中，需要同時(shí)估計(jì)兩個(gè)檢驗(yàn)質(zhì)量塊的位姿信息。

1.2 量測(cè)方程建模

首先利用姿態(tài)四元數(shù)定義衛(wèi)星本體相對(duì)地面慣性參考坐標(biāo)系的姿態(tài)為qib∈R4，記為qib=[qib,0,qib,1,qib,2,qib,3]T，有如下形式

其中：e=[e1,e2,e3]T表示歐拉軸的三維方向向量且滿足||e||2=1，θ表示歐拉角參數(shù)。姿態(tài)四元數(shù)的4個(gè)參數(shù)非獨(dú)立，滿足以下條件

2 量測(cè)線性化

從式（8）中可以看出系統(tǒng)關(guān)于衛(wèi)星本體姿態(tài)四元數(shù)的量測(cè)是非線性的，由于系統(tǒng)動(dòng)力學(xué)方程復(fù)雜且估計(jì)狀態(tài)維數(shù)高、初始化難度大，直接應(yīng)用非線性濾波方法進(jìn)行狀態(tài)估計(jì)會(huì)出現(xiàn)收斂緩慢、容易發(fā)散等問(wèn)題。現(xiàn)將衛(wèi)星本體姿態(tài)四元數(shù)的量測(cè)部分進(jìn)行線性化處理。

根據(jù)歐拉定理，剛體繞固定點(diǎn)的位移也可以是繞該點(diǎn)的若干次有限位移的合成，在用姿態(tài)四元數(shù)表示姿態(tài)參數(shù)的方法中，連續(xù)兩次轉(zhuǎn)動(dòng)后的姿態(tài)四元數(shù)等于各次轉(zhuǎn)動(dòng)的四元數(shù)的乘積?；谛敲舾衅髯x出，可以得到偏差四元數(shù)測(cè)量

展開(kāi)形式為

其中：qr為設(shè)定參考姿態(tài)：q為衛(wèi)星的真實(shí)姿態(tài)；qn為噪聲。

將量測(cè)方程中姿態(tài)四元數(shù)的部分進(jìn)行線性化處理，衛(wèi)星本體的角度參數(shù)與姿態(tài)四元數(shù)量測(cè)之間的相互轉(zhuǎn)換關(guān)系如下

將式（12）進(jìn)一步展開(kāi)

由于四元數(shù)的參數(shù)非獨(dú)立且滿足條件||q||2=1，在歐拉角為小角度（Δq0≈1）的情況下，有以下近似關(guān)系

將式（12）代入式（13）重寫(xiě)為

展開(kāi)可得

由此得到系統(tǒng)關(guān)于姿態(tài)四元數(shù)的非線性量測(cè)部分的線性化結(jié)果。

3 濾波算法

其中：H∈R15×36是線性化后的測(cè)量矩陣；v(t)∈R15是量測(cè)系統(tǒng)的讀出噪聲。在相鄰的兩個(gè)采樣時(shí)刻之間，讀出噪聲v(t)通過(guò)零階保持器保持不變。此時(shí)量測(cè)方程是狀態(tài)向量和讀出噪聲的加性組合，即得到離散時(shí)間量測(cè)方程為

通過(guò)系統(tǒng)離散化和量測(cè)線性化處理，將基于星敏傳感器非線性量測(cè)的衛(wèi)星動(dòng)力學(xué)系統(tǒng)和量測(cè)系統(tǒng)轉(zhuǎn)換為滿足卡爾曼濾波線性高斯假設(shè)的離散狀態(tài)空間模型，即

基于卡爾曼濾波原理，可以得到線性化四元數(shù)量測(cè)的高性能狀態(tài)估計(jì)算法。預(yù)測(cè)

更新

此外，進(jìn)一步分析了設(shè)計(jì)的濾波器的計(jì)算復(fù)雜度。定義n為系統(tǒng)狀態(tài)x(k)的維數(shù)，nd為執(zhí)行器擾動(dòng)d(k)的維數(shù)，nw為系統(tǒng)噪聲w(k)的維數(shù)，以及m為傳感器量測(cè)z(k)的維數(shù)。然后可以得到線性化四元數(shù)卡爾曼濾波算法的計(jì)算復(fù)雜度

相較于標(biāo)準(zhǔn)的卡爾曼濾波器（盡管其不能應(yīng)用到該任務(wù)中），本文中設(shè)計(jì)的卡爾曼濾波器并沒(méi)有額外增加計(jì)算復(fù)雜度。因此，在有限星載計(jì)算條件下，設(shè)計(jì)的基于線性化四元數(shù)量測(cè)的卡爾曼濾波算法可以用于空間引力波探測(cè)航天器系統(tǒng)狀態(tài)的高精度估計(jì)。

4 仿真驗(yàn)證

設(shè)置仿真采樣時(shí)間為T(mén)=0.1 s，仿真時(shí)長(zhǎng)為N=50 000 s。初始的衛(wèi)星狀態(tài)設(shè)置為x(0)=[0′;0;3e-5;0;e-5;0;0;0;3e-5;0′],P0=e-25×I。建立MATLAB/Simulink仿真模型，對(duì)天基引力波系統(tǒng)進(jìn)行狀態(tài)估計(jì)與結(jié)果分析。在實(shí)驗(yàn)結(jié)果中，我們將濾波估計(jì)結(jié)果分別與真實(shí)值和測(cè)量值進(jìn)行比較。為了便于比較分析，定義性能指標(biāo)：平均量測(cè)誤差、平均估計(jì)誤差、誤差減小率（Error Reduction Rate，ERR）。

使用最大短時(shí)抖動(dòng)指標(biāo)衡量狀態(tài)估計(jì)與量測(cè)的控制效果。最大短時(shí)抖動(dòng)性能的計(jì)算式如下

比對(duì)濾波前后控制誤差的頻譜分析濾波的性能。這里控制誤差是指估計(jì)值/量測(cè)值減去引導(dǎo)率信號(hào)的誤差，即可表示為

從圖1所示的狀態(tài)仿真結(jié)果可以看出狀態(tài)估計(jì)方案抑制了多源復(fù)雜噪聲的影響，實(shí)現(xiàn)了非線性量測(cè)下衛(wèi)星位姿的實(shí)時(shí)估計(jì)。與系統(tǒng)傳感器的量測(cè)值相比，估計(jì)值更為平滑且精確地跟蹤到衛(wèi)星的真實(shí)狀態(tài)。從圖2所示的結(jié)果可以更直觀地看出估計(jì)誤差明顯小于量測(cè)誤差的波動(dòng)范圍，在數(shù)值上更接近0。從表1可以看出單個(gè)測(cè)試質(zhì)量的位姿以及衛(wèi)星的三維姿態(tài)的估計(jì)結(jié)果誤差都小于量測(cè)結(jié)果誤差，姿態(tài)的估計(jì)誤差成功降低了一個(gè)數(shù)量級(jí)，且非線性量測(cè)的衛(wèi)星姿態(tài)估計(jì)結(jié)果的誤差減小率ERR均達(dá)到了45%及以上，因此基于非線性量測(cè)線性化設(shè)計(jì)的卡爾曼濾波器可以得到更高精度的衛(wèi)星狀態(tài)，估計(jì)結(jié)果代替測(cè)量結(jié)果為整個(gè)系統(tǒng)中控制器模塊提供更高精度的衛(wèi)星實(shí)時(shí)狀態(tài)信息。通過(guò)計(jì)算衛(wèi)星姿態(tài)估計(jì)值與量測(cè)值的最大短時(shí)抖動(dòng)來(lái)衡量系統(tǒng)控制誤差的RMSE性能。從圖3可以看出，在時(shí)間窗長(zhǎng)度內(nèi)衛(wèi)星姿態(tài)角度估計(jì)值的最大短時(shí)擾動(dòng)指標(biāo)均小于量測(cè)值的最大短時(shí)擾動(dòng)指標(biāo)。同時(shí)通過(guò)控制誤差頻譜圖反映狀態(tài)估計(jì)對(duì)系統(tǒng)控制誤差的影響。從圖4可以看出，使用狀態(tài)估計(jì)值的控制誤差頻譜相較于量測(cè)值在高頻處幅值顯著衰減，在高頻處其控制誤差的方差更小。因此使用基于非線性量測(cè)線性化設(shè)計(jì)的卡爾曼濾波器使得系統(tǒng)有更好的控制性能。

表1 狀態(tài)估計(jì)性能比較Table 1 Performance comparison of state estimation

圖1 αB(x)狀態(tài)仿真結(jié)果圖Fig.1 αB(x)State simulation results

圖2 αB(x)估計(jì)誤差對(duì)比圖Fig.2 αB(x)Estimation error comparison

圖3 αB(x)最大短時(shí)抖動(dòng)Fig.3 αB(x)Maximum short-time jitter

圖4 αB(x)濾波前后控制誤差頻譜Fig.4 αB(x)Control error spectrum

5 結(jié)論

本文針對(duì)空間引力波探測(cè)衛(wèi)星對(duì)準(zhǔn)階段狀態(tài)實(shí)時(shí)估計(jì)任務(wù)，基于星敏感器和慣性傳感器等多源異構(gòu)信息設(shè)計(jì)了一種線性化四元數(shù)量測(cè)的高性能狀態(tài)估計(jì)器。首先對(duì)系統(tǒng)動(dòng)力學(xué)模型進(jìn)行離散時(shí)間狀態(tài)空間建模；其次利用參考四元數(shù)信息對(duì)星敏感器非線性量測(cè)模型線性化處理，提出了基于卡爾曼濾波器的高性能狀態(tài)估計(jì)算法，從而實(shí)現(xiàn)空間引力波探測(cè)航天器系統(tǒng)狀態(tài)的在軌高精度估計(jì)；最后建立MATLAB/Simulink閉環(huán)仿真系統(tǒng)。仿真結(jié)果表明，在保證較低計(jì)算復(fù)雜度的前提下，所設(shè)計(jì)的估計(jì)方法能夠達(dá)到很好的估計(jì)效果，滿足引力波探測(cè)任務(wù)中航天器狀態(tài)高精度實(shí)時(shí)估計(jì)的需要，進(jìn)一步驗(yàn)證了提出的狀態(tài)估計(jì)器的可行性。