基于盒粒子濾波的低成本皮納衛(wèi)星高效姿態(tài)確定算法

侯曉磊，張聰哲，劉 勇，潘 泉，李毅蘭

(西北工業(yè)大學(xué)自動化學(xué)院，西安 710072)

0 引 言

皮納衛(wèi)星是近年來興起的一種新概念微小衛(wèi)星[1]，質(zhì)量通常在0.1～10 kg之間，其具有發(fā)射方式靈活、功能密度高、研制周期短、成本低等優(yōu)點(diǎn)。

姿態(tài)確定系統(tǒng)是衛(wèi)星飛行控制系統(tǒng)中的重要組成部分，其定姿精度直接影響衛(wèi)星運(yùn)行的穩(wěn)定性甚至決定任務(wù)成敗。目前，衛(wèi)星姿態(tài)確定系統(tǒng)多采用光電太陽敏感器及磁強(qiáng)計作為主要傳感器，并已在大衛(wèi)星上得到了應(yīng)用，如COMPASS-1，AAUSa1[2]，同時，近年來MEMS、MOEMS等先進(jìn)加工技術(shù)的發(fā)展實現(xiàn)了傳感器的微小型化，諸如MEMS微型陀螺和微型加速度計，以及MOEMS太陽敏感器和三軸微型磁強(qiáng)計等[3-4]傳感器開始在皮納衛(wèi)星上得到廣泛的應(yīng)用。然而，微型傳感器受到體積、測量原理等方面的限制，導(dǎo)致其量測性能相對較弱。同時，皮納衛(wèi)星的設(shè)計存在體積、功耗、成本、運(yùn)算能力和存儲空間等方面的約束。同時，為滿足衛(wèi)星姿態(tài)控制精度，皮納衛(wèi)星對姿態(tài)確定算法以及衛(wèi)星的運(yùn)算能力提出了較高的要求[5]。

目前衛(wèi)星姿態(tài)確定算法主要包括兩大類，即靜態(tài)確定性算法和狀態(tài)空間估計算法。TRIAD法[8]是一種典型的靜態(tài)確定性算法，在無需姿態(tài)的先驗知識的情況下，通過有姿態(tài)敏感器測量值構(gòu)建的兩個非平行矢量測量信息確定衛(wèi)星的姿態(tài)矩陣[6]，算法簡單且計算量較小，但其姿態(tài)確定結(jié)果易受測量精度影響導(dǎo)致較大誤差，并且至少需要兩個矢量才能確定姿態(tài)，其應(yīng)用范圍有一定的局限性[7]；衛(wèi)星姿態(tài)確定的狀態(tài)空間估計算法，是利用傳感器量測的歷史信息，結(jié)合航天器姿態(tài)運(yùn)動模型，得到統(tǒng)計意義下的最優(yōu)姿態(tài)估計[6-7,9]。在早期工程應(yīng)用領(lǐng)域中主要采用擴(kuò)展卡爾曼濾波(EKF)[10-11]，但由于航天器姿態(tài)觀測的噪聲類型復(fù)雜、狀態(tài)量分布各異，導(dǎo)致EKF濾波器設(shè)計復(fù)雜且可能無法完成姿態(tài)估計，后來逐漸發(fā)展出可在任意分布下實現(xiàn)非線性近似的粒子濾波(PF)方法[12-13]。

皮納衛(wèi)星姿態(tài)確定過程是高非線性的，基于PF的高精度姿態(tài)確定方法計算復(fù)雜度高，皮納衛(wèi)星的CPU無法提供高效的計算能力，而基礎(chǔ)的EKF方法精度不夠。在某些實際應(yīng)用中，為了保證結(jié)果的精確性，粒子濾波所采用的重要性重采樣(SIR)方法可能需要數(shù)千個粒子。于是為了降低采樣粒子個數(shù)，Abdllah等[14]通過將持續(xù)蒙特卡羅(SMC)和區(qū)間分析相結(jié)合，提出了以描述狀態(tài)空間中占據(jù)一個體積小且可控制的非零矩形區(qū)域的盒粒子的概念，并在此基礎(chǔ)上提出了盒粒子濾波(Box particle filter,BPF)[15-16]。BPF濾波器通過區(qū)間的概念有效地減少了粒子數(shù)量、降低了計算的復(fù)雜度，并且較適合分布式過濾。一般BPF濾波器只需幾個或者幾十個盒粒子即可達(dá)到與粒子濾波相同或者相近的精度。

本文基于區(qū)間理論，提出了區(qū)間化皮納衛(wèi)星姿態(tài)描述方法，并建立了姿態(tài)敏感器的數(shù)學(xué)模型，又在此基礎(chǔ)上，推導(dǎo)出基于BPF的皮納衛(wèi)星姿態(tài)確定算法，通過數(shù)字仿真校驗了算法的有效性，同時探究了不同參數(shù)設(shè)置對濾波器參數(shù)影響。

1 衛(wèi)星姿態(tài)區(qū)間化描述

1.1 衛(wèi)星姿態(tài)區(qū)間化描述

在許多應(yīng)用中，區(qū)間理論是一個很好的處理非白噪聲和偏差的方法。本文提出一個涉及區(qū)間數(shù)據(jù)移動定位的PF策略。使用區(qū)間數(shù)據(jù)可以有效降低PF算法所使用的粒子的數(shù)量，從而提高計算效率。

在利用區(qū)間理論描述衛(wèi)星姿態(tài)時，在狀態(tài)空間Rn內(nèi)，定義區(qū)間[x]，[x]是空間Rn內(nèi)封閉連續(xù)集合，則盒[x]可表示為區(qū)間的笛卡爾積，即：

(1)

其中，[xi]表示空間R內(nèi)的封閉連續(xù)子集。則衛(wèi)星姿態(tài)四元數(shù)可以區(qū)間化表示為：

[q]=[η]×[ε1]×[ε2]×[ε3]

(2)

根據(jù)四元數(shù)的約束條件，

[q]?[-1,1]4

(3)

令[q]=[ql,qh]，其中，[ql]為[q]下界，[qh]為[q]上界。根據(jù)歐拉角定義，姿態(tài)角速度區(qū)間化表述為：

[ω]=[ωx]×[ωy]×[ωz]

(4)

1.2 衛(wèi)星數(shù)學(xué)模型區(qū)間化描述

(5)

根據(jù)區(qū)間分析理論，定義f的區(qū)間化表示為[f]，同樣一個區(qū)間[x]函數(shù)[f]可以表示為[f]([x])，則姿態(tài)區(qū)間[q]的姿態(tài)運(yùn)動學(xué)方程為：

(6)

(7)

同樣得到區(qū)間化的動力學(xué)方程為:

(8)

由此得到了衛(wèi)星姿態(tài)以及動力學(xué)模型的區(qū)間化描述，并基于此描述提出了改進(jìn)的BPF姿態(tài)確定方法。

2 改進(jìn)BPF姿態(tài)確定算法

本文針對皮納衛(wèi)星所裝備傳感器的特點(diǎn)，提出了將雙矢量算法與BPF相結(jié)合的改進(jìn)BPF姿態(tài)確定算法，圖1為其算法流程圖。

圖1 改進(jìn)BPF姿態(tài)確定算法流程圖

改進(jìn)BPF姿態(tài)確定算法首先通過雙矢量算法對敏感器量測進(jìn)行預(yù)處理，將解算出的姿態(tài)四元數(shù)作為量測值輸入到BPF中，以降低敏感器噪聲對估計精度的影響，從而獲得更加精確的衛(wèi)星姿態(tài)數(shù)據(jù)。

2.1 區(qū)間化描述

1)狀態(tài)方程

選擇區(qū)間化四元數(shù)與區(qū)間化角速度作為狀態(tài)量。則：

[ε3]×[ωx]×[ωy]×[ωz]

(9)

定義[F]([X])為非線性區(qū)間化狀態(tài)函數(shù)，即

(10)

結(jié)合式(9)與式(10)可以得到:

(11)

定義lkl和lkh分別表示第k時刻狀態(tài)量區(qū)間的下界和上界。且有：

(12)

由于姿態(tài)運(yùn)動學(xué)曲線變化平緩，當(dāng)區(qū)間選擇較小時，定義在區(qū)間內(nèi)的狀態(tài)轉(zhuǎn)移函數(shù)基本為線性，由此可以得到：

[Xk]=[F]([Xk-1])=[lkl,lkh]=[min(F(l(k-1)l),

F(l(k-1)h)),max(F(l(k-1)l),F(l(k-1)h))]

(13)

采用四階Runge-Kutta方法求解微分方程得到下一時刻的狀態(tài)，得到:

(14)

2) 量測方程

當(dāng)姿態(tài)測量僅使用磁強(qiáng)計和太陽敏感器時，量測量為磁強(qiáng)計和太陽敏感器的輸出。但是這種方法在每一步濾波時會將得到的姿態(tài)旋轉(zhuǎn)矩陣進(jìn)行坐標(biāo)轉(zhuǎn)換，從而增加了計算量。為此，采用雙矢量方法對量測進(jìn)行預(yù)處理，降低量測維度，減少計算量，從而得到三維量測為:

[Z]=[ε]=[ε1]×[ε2]×[ε3]

(15)

同樣，定義zkl和zkh分別表示第k步量測量區(qū)間的下界和上界。由此得到系統(tǒng)量測方程為：

(16)

2.2 從PF到BPF的姿態(tài)確定算法過程推導(dǎo)

1)盒粒子初始化

(17)

2)狀態(tài)轉(zhuǎn)移

在此步驟中，狀態(tài)量盒粒子會根據(jù)狀態(tài)方程進(jìn)行更新。在這個過程中粒子濾波與盒粒子濾波之間的顯著區(qū)別是對噪聲的處理。在盒粒子濾波過程中，會根據(jù)有界誤差模型對建模和量測誤差進(jìn)行處理[16]。

BPF的狀態(tài)轉(zhuǎn)移方程可以表示為：

(18)

又根據(jù)式(12)、式(13)，狀態(tài)方程可以寫成

(19)

3)狀態(tài)更新

BPF的狀態(tài)更新包括以下步驟：預(yù)測、殘差計算、似然函數(shù)求解、盒式粒子濾波收縮、權(quán)重更新、歸一化及狀態(tài)估計過程[7]。

(1)預(yù)測

在預(yù)測過程，粒子濾波和BPF都是通過量測方程來推算第k步的預(yù)測值。BPF的預(yù)測方程可以寫為：

(20)

(21)

(2)殘差

使用BPF時，可預(yù)測的值可以比作一個真正的盒子測量，因此殘差要求能夠反應(yīng)真實量測與預(yù)測量的接近程度，由此可以將殘差定義為量測區(qū)間與預(yù)測區(qū)間的交叉點(diǎn)[7]。本文中，量測噪聲近似于高斯分布，即為高斯白噪聲，符合正態(tài)分布。由正態(tài)分布的置信度理論可以得到，在3倍標(biāo)準(zhǔn)差±σ以內(nèi)，隨機(jī)誤差出現(xiàn)的概率為99.73%。再結(jié)合邊界誤差理論對量測建模。高斯白噪聲vk區(qū)間化后為[vk]=[-3σv, 3σv]，則可以得到量測區(qū)間為：

[yk]=[yk-3σ,yk+3σ]=[ykl,ykh]

(22)

(23)

下式用以保證盒粒子滿足區(qū)間特性：

(24)

(25)

則殘差盒粒子的區(qū)間長度為：

(26)

(3)似然函數(shù)

對于粒子濾波，根據(jù)量測噪聲的概率模型可以求出其似然函數(shù)，由于量測噪聲w符合高斯分布。根據(jù)邊界誤差方法，很顯然對于一個盒粒子，預(yù)測盒子和量測盒子沒有交集的盒子應(yīng)該被舍棄，僅保留那些同時包括實際量測信息與預(yù)測信息的盒子。由此，基本的盒式粒子濾波的似然函數(shù)可以設(shè)計如下[7]：

(27)

式中:q表示量測的維數(shù)，即為3。

(4)盒式粒子濾波收縮過程

盒粒子收縮過程旨在利用Waltz理論[7]，通過殘差縮小狀態(tài)量預(yù)測與估計區(qū)間，從而加速濾波過程，提高運(yùn)行效率，使得BPF相較于PF更加快速高效。

收縮器可以定義為用于縮小CSP基礎(chǔ)定義域的算法，從而產(chǎn)生新的S∈[x′]滿足的盒[x′]∈[x]。根據(jù)Waltz理論，BPF收縮方程為：

(28)

式中:[B]為Waltz算法的函數(shù)表示。

(5)權(quán)重更新

(29)

而BPF和粒子濾波的單位化過程相同，都是為了保證這些盒粒子的權(quán)重之和為1，方程如下：

(30)

(6)狀態(tài)估計

粒子濾波中，根據(jù)粒子的權(quán)重可以估計出系統(tǒng)當(dāng)前時刻的狀態(tài)，這個估計結(jié)果的可信度可以通過協(xié)方差矩陣來進(jìn)行表述[7]。而盒粒子的估計值則是通過N維的中心粒子來計算，可以表示為：

(31)

(32)

4)重要性重采樣

濾波過程中，區(qū)間收縮和概率選擇的過程會使粒子多樣性減小，從而導(dǎo)致濾波算法精度下降。解決這一問題的一般方法為重采樣[7]，重采樣的方法有很多種，本文采用的是重要性重采樣。

其中，令：

(33)

(34)

重采樣結(jié)束后，完成盒粒子更新及權(quán)重歸一化：

(35)

3 仿真結(jié)果

由于本文設(shè)計的算法的輸入為雙矢量姿態(tài)確定結(jié)果，量測噪聲非高斯，而濾波器設(shè)計中所用到的噪聲模型要求為高斯分布，因此雙矢量姿態(tài)確定算法的姿態(tài)誤差模型的建立是整個仿真實驗的先決條件[7]。根據(jù)實際需求，本文首先仿真統(tǒng)計了不同量測噪聲下雙矢量姿態(tài)確定算法的精度。統(tǒng)計結(jié)果如下：

(1)雙矢量姿態(tài)確定的三軸精度均值基本為0。

(2)雙矢量姿態(tài)確定的三軸精度標(biāo)準(zhǔn)差統(tǒng)計如表1所示。

表1 雙矢量算法姿態(tài)確定精度統(tǒng)計表[7]

(1)BPF姿態(tài)確定算法與PF姿態(tài)確定算法的對比實驗。

(2)量測噪聲對BPF姿態(tài)確定精度的影響實驗。

(3)盒粒子數(shù)量對BPF姿態(tài)確定的影響實驗。

1)基準(zhǔn)場景下BPF姿態(tài)確定算法與PF姿態(tài)確定算法的對比實驗

為驗證BPF姿態(tài)確定算法的有效性，如下給出了BPF姿態(tài)確定算法與PF姿態(tài)確定算法的實驗對比結(jié)果?；鶞?zhǔn)場景參數(shù)設(shè)計如下：

(1)BPF盒粒子數(shù)為100，PF粒子數(shù)為1000。

(2)兩個濾波器的重采樣閾值均為20。

(3)磁強(qiáng)計噪聲為10-7/T，太陽敏感器噪聲為0.01。根據(jù)表1中的結(jié)果，雙矢量姿態(tài)確定算法估計精度的標(biāo)準(zhǔn)差為0.96。

圖2給出了BPF與PF姿態(tài)確定誤差對比結(jié)果。從圖2可以看出，在姿態(tài)確定初期，BPF姿態(tài)確定算法相較于PF方法具有較快的收斂速度，姿態(tài)誤差曲線在500 s左右進(jìn)入穩(wěn)態(tài)。由于BPF方法具有盒粒子收縮過程，即通過量測值與量測預(yù)測的交集反解出狀態(tài)量以縮短狀態(tài)量的區(qū)間，加快收斂速度，圖中的跳變就是因為量測噪聲突變導(dǎo)致反解出的狀態(tài)量大幅度偏移的結(jié)果；PF姿態(tài)確定方法在700 s以后收斂速度降低直至進(jìn)入穩(wěn)態(tài)，且在穩(wěn)態(tài)時有較好的估計精度。

圖2 PF與BPF姿態(tài)誤差對比

表2為姿態(tài)穩(wěn)態(tài)時BPF與PF姿態(tài)估計精度與運(yùn)行時間的統(tǒng)計結(jié)果。統(tǒng)計數(shù)據(jù)表明，BPF姿態(tài)確定法估計精度略低于PF方法(約為PF方法的73.3%)，但其運(yùn)行時間僅為PF方法的22.9%。由此可以得到，在控制精度相近時，BPF姿態(tài)確定方法相較于傳統(tǒng)的PF方法能夠有效提高算法運(yùn)行效率，減少系統(tǒng)能耗。

表2 姿態(tài)穩(wěn)定時BPF與PF濾波結(jié)果比較

2)量測噪聲對BPF姿態(tài)確定的影響實驗

為了進(jìn)一步校驗BPF姿態(tài)確定算法的可靠性，如下仿真實驗給出了不同量測噪聲下BPF姿態(tài)確定估計結(jié)果。其中對比實驗的磁強(qiáng)計噪聲為10-6T，太陽敏感器噪聲為0.01，根據(jù)表2中的結(jié)果，雙矢量姿態(tài)確定算法估計精度的標(biāo)準(zhǔn)差為3.64，其余參數(shù)與基準(zhǔn)場景下的濾波器參數(shù)設(shè)置相同。仿真結(jié)果如圖3所示。

圖3給出了量測標(biāo)準(zhǔn)差分別為0.96和3.64時的BPF姿態(tài)確定估計結(jié)果?？梢姡?dāng)量測噪聲較大時，姿態(tài)估計精度曲線初始估計精度變差，收斂速度顯著降低。這是因為當(dāng)量測噪聲較大時，改進(jìn)BPF姿態(tài)確定算法中量測區(qū)間范圍明顯增大，初始估計精度隨之變差。同時，通過盒粒子反解過程得到的新的狀態(tài)區(qū)間也隨之變大，因而收斂速度和最后的估計精度都受到了影響。

圖3 不同量測噪聲下BPF姿態(tài)確定估計結(jié)果

表3給出了不同量測噪聲下改進(jìn)BPF姿態(tài)確定算法的估計精度與運(yùn)行時間。統(tǒng)計結(jié)果進(jìn)一步表明，隨著量測噪聲增大，改進(jìn)BPF姿態(tài)確定算法運(yùn)行時間相差不大，但估計精度明顯降低，即改進(jìn)BPF姿態(tài)確定精度與量測噪聲的大小呈負(fù)相關(guān)。

表3 不同量測噪聲下BPF濾波結(jié)果比較

3)盒粒子數(shù)量對BPF姿態(tài)確定的影響實驗

本節(jié)主要通過仿真實驗校驗盒粒子數(shù)量與BPF姿態(tài)確定精度的關(guān)系。對比實驗的盒粒子數(shù)目為50。其余參數(shù)均與實驗1)中的濾波器參數(shù)設(shè)置相同。仿真實驗結(jié)果如圖4所示。

圖4 不同盒粒子數(shù)量下的BPF姿態(tài)確定估計結(jié)果

圖4給出了盒粒子數(shù)目分別為50和100時的BPF姿態(tài)確定估計結(jié)果?？梢?，當(dāng)盒粒子數(shù)目減少時，姿態(tài)估計精度曲線初始估計精度顯著變差，但收斂速度較之變化不大。

表4給出了不同盒粒子數(shù)目下改進(jìn)BPF姿態(tài)確定算法的估計精度與運(yùn)行時間。統(tǒng)計結(jié)果進(jìn)一步表明，粒子數(shù)目由100減少至50時，改進(jìn)BPF姿態(tài)確定算法運(yùn)行時間縮短了25.86%，估計精度降低了41.51%。算法運(yùn)行時間雖然隨著粒子數(shù)目的減少而縮短，但兩者不是正比關(guān)系，這是因為初始盒粒子數(shù)目減少時，濾波過程中盒粒子區(qū)間衰減明顯，因而算法因重采樣次數(shù)增多而耗費(fèi)了一些時間。由此得出，改進(jìn)BPF姿態(tài)確定精度與盒粒子數(shù)目呈正相關(guān)，算法運(yùn)行時間與盒粒子數(shù)目負(fù)相關(guān)。因此，在實際選擇粒子數(shù)目時，應(yīng)綜合考慮精度和時間的影響，以滿足實際工程問題的指標(biāo)要求。

表4 不同盒粒子數(shù)目下BPF濾波結(jié)果比較

4 結(jié) 論

本文基于區(qū)間理論，提出了區(qū)間化皮納衛(wèi)星姿態(tài)描述方法，建立了姿態(tài)敏感器的數(shù)學(xué)模型。在此基礎(chǔ)上，推導(dǎo)出基于改進(jìn)BPF的皮納衛(wèi)星姿態(tài)確定算法，并通過數(shù)字仿真校驗了算法的有效性，同時探究了不同參數(shù)設(shè)置對濾波器性能的影響。目前，該姿態(tài)確定算法應(yīng)用于基于手機(jī)平臺的小衛(wèi)星的項目研究中，即手機(jī)衛(wèi)星姿態(tài)確定系統(tǒng)。該算法既適用于當(dāng)前課題的應(yīng)用也為其他手機(jī)衛(wèi)星研究提供了借鑒和經(jīng)驗，為今后BPF在工程項目中的應(yīng)用奠定了基礎(chǔ)。