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

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

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

0 引 言

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

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

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

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

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

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

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

在利用區(qū)間理論描述衛(wèi)星姿態(tài)時(shí)，在狀態(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]，同樣一個(gè)區(qū)間[x]函數(shù)[f]可以表示為[f]([x])，則姿態(tài)區(qū)間[q]的姿態(tài)運(yùn)動(dòng)學(xué)方程為：

(6)

(7)

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

(8)

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

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

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

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

改進(jìn)BPF姿態(tài)確定算法首先通過(guò)雙矢量算法對(duì)敏感器量測(cè)進(jìn)行預(yù)處理，將解算出的姿態(tài)四元數(shù)作為量測(cè)值輸入到BPF中，以降低敏感器噪聲對(duì)估計(jì)精度的影響，從而獲得更加精確的衛(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時(shí)刻狀態(tài)量區(qū)間的下界和上界。且有：

(12)

由于姿態(tài)運(yùn)動(dòng)學(xué)曲線變化平緩，當(dāng)區(qū)間選擇較小時(shí)，定義在區(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方法求解微分方程得到下一時(shí)刻的狀態(tài)，得到:

(14)

2) 量測(cè)方程

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

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

(15)

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

(16)

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

1)盒粒子初始化

(17)

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

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

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

(18)

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

(19)

3)狀態(tài)更新

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

(1)預(yù)測(cè)

在預(yù)測(cè)過(guò)程，粒子濾波和BPF都是通過(guò)量測(cè)方程來(lái)推算第k步的預(yù)測(cè)值。BPF的預(yù)測(cè)方程可以寫(xiě)為：

(20)

(21)

(2)殘差

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

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

(22)

(23)

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

(24)

(25)

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

(26)

(3)似然函數(shù)

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

(27)

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

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

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

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

(28)

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

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

(29)

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

(30)

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

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

(31)

(32)

4)重要性重采樣

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

其中，令：

(33)

(34)

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

(35)

3 仿真結(jié)果

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

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

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

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

(1)BPF姿態(tài)確定算法與PF姿態(tài)確定算法的對(duì)比實(shí)驗(yàn)。

(2)量測(cè)噪聲對(duì)BPF姿態(tài)確定精度的影響實(shí)驗(yàn)。

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

1)基準(zhǔn)場(chǎng)景下BPF姿態(tài)確定算法與PF姿態(tài)確定算法的對(duì)比實(shí)驗(yàn)

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

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

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

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

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

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

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

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

2)量測(cè)噪聲對(duì)BPF姿態(tài)確定的影響實(shí)驗(yàn)

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

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

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

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

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

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

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

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

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

表4給出了不同盒粒子數(shù)目下改進(jìn)BPF姿態(tài)確定算法的估計(jì)精度與運(yùn)行時(shí)間。統(tǒng)計(jì)結(jié)果進(jìn)一步表明，粒子數(shù)目由100減少至50時(shí)，改進(jìn)BPF姿態(tài)確定算法運(yùn)行時(shí)間縮短了25.86%，估計(jì)精度降低了41.51%。算法運(yùn)行時(shí)間雖然隨著粒子數(shù)目的減少而縮短，但兩者不是正比關(guān)系，這是因?yàn)槌跏己辛Ｗ訑?shù)目減少時(shí)，濾波過(guò)程中盒粒子區(qū)間衰減明顯，因而算法因重采樣次數(shù)增多而耗費(fèi)了一些時(shí)間。由此得出，改進(jìn)BPF姿態(tài)確定精度與盒粒子數(shù)目呈正相關(guān)，算法運(yùn)行時(shí)間與盒粒子數(shù)目負(fù)相關(guān)。因此，在實(shí)際選擇粒子數(shù)目時(shí)，應(yīng)綜合考慮精度和時(shí)間的影響，以滿足實(shí)際工程問(wèn)題的指標(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)確定算法，并通過(guò)數(shù)字仿真校驗(yàn)了算法的有效性，同時(shí)探究了不同參數(shù)設(shè)置對(duì)濾波器性能的影響。目前，該姿態(tài)確定算法應(yīng)用于基于手機(jī)平臺(tái)的小衛(wèi)星的項(xiàng)目研究中，即手機(jī)衛(wèi)星姿態(tài)確定系統(tǒng)。該算法既適用于當(dāng)前課題的應(yīng)用也為其他手機(jī)衛(wèi)星研究提供了借鑒和經(jīng)驗(yàn)，為今后BPF在工程項(xiàng)目中的應(yīng)用奠定了基礎(chǔ)。