粒子濾波在衛(wèi)星姿態(tài)確定中的應(yīng)用

張 健，孫兆偉

（哈爾濱工業(yè)大學(xué)衛(wèi)星技術(shù)研究所，150001 哈爾濱）

粒子濾波在衛(wèi)星姿態(tài)確定中的應(yīng)用

張 健，孫兆偉

（哈爾濱工業(yè)大學(xué)衛(wèi)星技術(shù)研究所，150001 哈爾濱）

為了驗(yàn)證以星敏感器和速率陀螺作為衛(wèi)星測(cè)量元件時(shí)，粒子濾波算法在姿態(tài)確定中的有效性.采用修正的羅德里格參數(shù)作為姿態(tài)參數(shù)建立了有陀螺和無陀螺兩種模式下的系統(tǒng)狀態(tài)方程和測(cè)量方程，并利用粒子濾波(PF)算法進(jìn)行了姿態(tài)估計(jì).和擴(kuò)展的卡爾曼濾波(EKF)算法進(jìn)行比較，仿真結(jié)果表明:PF算法在小初始估計(jì)誤差下能夠收斂，且具有和EKF相當(dāng)?shù)木?大初始估計(jì)誤差時(shí)，EKF算法不能收斂而PF算法仍能收斂.最后驗(yàn)證了PF算法在無陀螺模式下進(jìn)行姿態(tài)確定的有效性.

粒子濾波;姿態(tài)確定;星敏感器;速率陀螺

衛(wèi)星姿態(tài)確定系統(tǒng)作為控制的前提，其精度直接影響到姿態(tài)控制的質(zhì)量.目前，EKF算法作為非線性濾波的經(jīng)典算法，在衛(wèi)星姿態(tài)確定中被廣泛的應(yīng)用.然而，EKF算法需要對(duì)非線性的狀態(tài)方程和測(cè)量方程進(jìn)行線性化，并需要計(jì)算雅克比行列式，計(jì)算復(fù)雜魯棒性低.粒子濾波算法是基于貝葉斯最優(yōu)估計(jì)理論和蒙特卡洛積分原理來對(duì)狀態(tài)的后驗(yàn)概率密度函數(shù)進(jìn)行近似，能夠克服EKF 的缺點(diǎn)［1－3］.程楊等［4］利用粒子濾波算法，結(jié)合修正的羅德里格參數(shù)對(duì)衛(wèi)星姿態(tài)進(jìn)行估計(jì)，克服了四元數(shù)用于姿態(tài)估計(jì)時(shí)需要滿足四元數(shù)歸一化的約束問題，引入變化的協(xié)方差矩陣，克服了由于觀測(cè)誤差很小引起的樣本快速枯竭的問題.Oshman等［5］將粒子濾波算法和遺傳算法相結(jié)合，用于有陀螺工作下的衛(wèi)星姿態(tài)確定，其算法結(jié)合了遺傳算法和例子濾波算法，雖然濾波精度有所提高，但計(jì)算量很大.姜雪原等［6］把 Rao-Blackwellization技術(shù)和粒子濾波結(jié)合，提出基于Marginalized的粒子濾波算法，并用于衛(wèi)星姿態(tài)估計(jì)，取得了較好的效果.本文采用由Gordon等［7］提出的Bootstrap粒子濾波(BF)算法，對(duì)衛(wèi)星的姿態(tài)確定問題進(jìn)行了仿真研究，并針對(duì)有陀螺模式下粒子濾波算法出現(xiàn)的樣本枯竭問題，對(duì)粒子濾波進(jìn)行改進(jìn)，引入擴(kuò)展的卡爾曼粒子濾波(EKPF)算法，結(jié)果證明其可行性.最后利用粒子濾波算法對(duì)無陀螺模式下的姿態(tài)確定問題進(jìn)行了仿真分析，驗(yàn)證了其有效性.

1 系統(tǒng)狀態(tài)方程

本文針對(duì)有陀螺和無陀螺兩種模式，采樣修正的羅德里格參數(shù)作為狀態(tài)參數(shù)，分別建立了系統(tǒng)的狀態(tài)方程.

假設(shè)衛(wèi)星為一單剛體，則用修正的羅德里格參數(shù)表示的運(yùn)動(dòng)學(xué)方程為

式中:ρ = ［ρx，ρy，ρz］T為修正的羅德里格參數(shù);I3×3為3階的單位陣;ωba為體系相對(duì)于參考系的角速度;ρ×為ρ的叉乘矩陣，表示為

動(dòng)力學(xué)方程為

式中:I為衛(wèi)星在星體固聯(lián)坐標(biāo)系的轉(zhuǎn)動(dòng)慣量矩陣;T為作用在星體上的內(nèi)外力矩的分量列陣.

陀螺測(cè)量模型采用簡單的Farrenkopf模型，可表示為式中:ωbi為衛(wèi)星相對(duì)于慣性空間的旋轉(zhuǎn)角速度;ωg為陀螺測(cè)量的角速度;b為陀螺常值漂移;vg為陀螺測(cè)量的白噪聲，滿足vg～N(0，σ2g);vb為驅(qū)動(dòng)陀螺常值漂移的白噪聲，滿足vb～N(0，σ2b).

1.1 有陀螺模式下的狀態(tài)方程

有陀螺模式下選擇修正的羅德里格參數(shù)和陀螺漂移作為狀態(tài)變量，即

式中:b= ［bx，by，bz］T為陀螺漂移.則根據(jù)方程(1)和(3)可得系統(tǒng)的狀態(tài)方程為

式中:W為系統(tǒng)噪聲，這里假設(shè)為零均值的高斯噪聲，f(x，t)為非線性函數(shù)，有

1.2 無陀螺模式下的狀態(tài)方程

無陀螺模式下用衛(wèi)星動(dòng)力學(xué)方程來代替陀螺測(cè)量方程，選擇修正的羅德里格參數(shù)和角速度作為狀態(tài)變量，即則根據(jù)方程(1)和方程(2)可得系統(tǒng)的狀態(tài)方程為方程(4)表示的形式，其中f(x，t)為

2 測(cè)量方程

星敏感器是一種以恒星為姿態(tài)參考的高精度光學(xué)敏感器，是航天器姿軌控系統(tǒng)中常用到的敏感元器件.由筒鏡光學(xué)系統(tǒng)和CCD面陣光敏元器件組成，來自星光的平行光經(jīng)過光學(xué)系統(tǒng)，在CCD面陣上聚焦成像圈［8］.如圖1所示，定義星敏感坐標(biāo)系Oxsyszs，zs軸沿中心光軸，xs和 ys軸沿CCD面陣的正交基準(zhǔn)，(px，py)為星光像元在面內(nèi)的坐標(biāo)，f為光學(xué)系統(tǒng)的焦距.

圖1 星敏感器測(cè)量原理圖

假設(shè)星敏感器坐標(biāo)系與星體坐標(biāo)系重合，且光軸沿星體zb軸方向.用p=［px;py］表示星光矢量ri在CCD平面上成像點(diǎn)的真實(shí)坐標(biāo)，記為

定義慣性空間中相對(duì)于某個(gè)恒星天體的兩個(gè)平行的參考矢量的觀測(cè)值分別為ui1和ui2，則可以建立星敏感器的測(cè)量模型為

式中:Cbi為慣性系到體系的坐標(biāo)變換矩陣;vg為星敏感器的測(cè)量噪聲.為便于分析，采用:ui1=［1，0，0］T，ui1= ［0，1，0］T.

3 粒子濾波算法在姿態(tài)確定中的應(yīng)用

所謂粒子濾波是指:通過尋找一組在狀態(tài)空間中傳播的隨機(jī)樣本，對(duì)概率密度函數(shù)p(xk|zk)進(jìn)行近似，以樣本均值代替積分運(yùn)算，從而獲得狀態(tài)的最小方差估計(jì)的過程，是一種基于貝葉斯估計(jì)思想的非線性濾波算法.當(dāng)粒子數(shù)目足夠大時(shí)，這些樣本點(diǎn)能非常接近地表征后驗(yàn)概率密度.

Gordon等人提出 Bootstrap粒子濾波算法(BF)，由于應(yīng)用簡單得到了廣泛應(yīng)用，其基本步驟如下:

1)初始化.從p(x0)中隨機(jī)抽取N個(gè)樣本，xi0～ N(0，^P(x0))，wi0=1/N，i=1，2，…，N，其中N為粒子數(shù).

2)遞推計(jì)算，k=1，2，….

3)重要性抽樣，從q(xk|xk－1，zk)中隨機(jī)抽取N個(gè)樣本，利用公式(7)計(jì)算權(quán)值，并利用公式(8)進(jìn)行歸一化，

4)重采樣.由公式(9)計(jì)算有效粒子數(shù)［10］，并與預(yù)定的閥值粒子數(shù)比較判斷是否需要進(jìn)行重采樣.若Neff＜Nth，則進(jìn)行重采樣;其中Nth為重采樣的閥值粒子數(shù)，取Nth=2/3N，否則轉(zhuǎn)步驟5;

5)輸出估計(jì)值.BF粒子濾波算法采用的重要性函數(shù)是次優(yōu)的，并且沒有充分利用當(dāng)前的測(cè)量信息.為得到更為精確的重要性函數(shù)，在粒子一步更新階段引入EKF過程，通過EKF對(duì)所有粒子進(jìn)行一步更新，在加權(quán)求均值和協(xié)方差，即得到擴(kuò)展的卡爾曼粒子濾波(EKPF).EKPF算法和BF算法相比，在粒子更新階段充分利用了當(dāng)前的測(cè)量信息，能得到更好的估計(jì)效果［11］.這里采用修正的羅德里格參數(shù)作為衛(wèi)星姿態(tài)參數(shù)，和四元數(shù)相比，沒有約束條件，其協(xié)方差矩陣非奇異［12］.根據(jù)有陀螺和無陀螺兩種模式下得到的狀態(tài)方程和測(cè)量方程，選擇不同的狀態(tài)估計(jì)初值和協(xié)方差矩陣，利用上述的粒子濾波算法進(jìn)行狀態(tài)估計(jì).

4 數(shù)學(xué)仿真

模式一:

有陀螺模式，仿真參數(shù)為

1)假設(shè)衛(wèi)星星體處于沒有控制力矩，只受到外界干擾力矩作用的狀態(tài)，仿真過程中假設(shè)外加的干擾力矩矢量為 Td= ［10－5;10－5;10－5］N·s;

假設(shè)衛(wèi)星星體的轉(zhuǎn)動(dòng)慣量矩陣為I=diag(100，200，200)kg·m2;

2)姿態(tài)角和角速度初值分別為φ=0°，θ=0°，Ψ =0°;wx0=0(°)/s，wy0=0(°)/s，wz0=0(°)/s;

4)星敏感器測(cè)量噪聲的均方差為σp=100 urad;星敏感器的采樣周期為1 Hz;仿真步長取1 s.

估計(jì)初值:①小初始估計(jì)誤差:姿態(tài)角估計(jì)值設(shè)為［1，1，1］°，陀螺漂移估計(jì)值設(shè)為［2，2，2］(°)/h，姿態(tài)方差為(1°)2，初始陀螺漂移的方差為(1(°)/h)2，采樣粒子數(shù)為1 000;②初始的姿態(tài)角初值設(shè)為［12，－10，170］°，其他同上;由圖 2可知，在小初始估計(jì)誤差情況下使用粒子濾波算法，姿態(tài)角估計(jì)估計(jì)誤差能夠收斂，但陀螺漂移估計(jì)誤差不能夠收斂，而是在初值附近的某個(gè)值上下波動(dòng).這是因?yàn)橥勇萜频倪f推過程是一個(gè)靜態(tài)的傳播過程，即bk+1=bk，這樣在粒子濾波過程中經(jīng)過幾次重采樣過程后，所有粒子點(diǎn)幾乎全部集中在一個(gè)粒子上，造成樣本枯竭現(xiàn)象，達(dá)不到濾波效果.

圖2 陀螺模式下小初始估計(jì)誤差PF姿態(tài)角誤差和陀螺漂移估計(jì)誤差曲線

由圖3可知，用EKPF算法對(duì)小初始估計(jì)誤差的情況狀態(tài)進(jìn)行估計(jì)，姿態(tài)角和角速度的收斂精度分為 0.002°和 0.000 05(°)/s.說明在小初始估計(jì)誤差的情況下，EKPF算法和EKF算法都能夠收斂，具有相同的精度.但EKPF算法計(jì)算量大，不適合用于實(shí)時(shí)狀態(tài)估計(jì).

由圖4和圖5可知，在大初始估計(jì)誤差的情況下，EKPF算法還能保持收斂，但EKF算法就變得發(fā)散了.這也體現(xiàn)了EKPF算法在狀態(tài)估計(jì)中的優(yōu)勢(shì).

圖3 有陀螺模式下小初始估計(jì)誤差EKPF姿態(tài)角和角速度估計(jì)誤差曲線

圖4 有陀螺模式下大初始估計(jì)誤差EKPF姿態(tài)角和陀螺漂移估計(jì)誤差范數(shù)曲線

圖5 有陀螺模式下大初始估計(jì)誤差EKF姿態(tài)角和陀螺漂移估計(jì)誤差范數(shù)曲線

模式二:無陀螺模式

仿真參數(shù)為:姿態(tài)角估計(jì)值設(shè)為［1，1，1］°，角速度估計(jì)初值設(shè)為［0.001，0.001，0.001］(°)/s，姿態(tài)方差為(1°)2，角速度的方差為(0.001(°)/s)2，采樣粒子數(shù)為1 000，其他同有陀螺模式下得仿真參數(shù)設(shè)置.

圖6表明，粒子濾波在無陀螺模式下，姿態(tài)角估計(jì)精度為0.01°，角速度估計(jì)精度為0.004(°)/s，雖然和有陀螺時(shí)相比，精度降低，但仍能滿足一般精度要求的衛(wèi)星.

圖6 無陀螺模式下BF姿態(tài)角和角速度估計(jì)誤差曲線

5 結(jié)論

本文對(duì)粒子濾波算法及其在衛(wèi)星姿態(tài)估計(jì)中的應(yīng)用進(jìn)行了研究.結(jié)果表明:在有陀螺模式下，當(dāng)直接使用粒子濾波算法時(shí)，由于重采樣過程引起了樣本枯竭，陀螺漂移不能收斂;為此采用EKPF算法，可以緩解樣本枯竭的現(xiàn)象，姿態(tài)角和陀螺漂移都可以收斂，且具有和EKF算法相同的估計(jì)精度.另外，采用粒子濾波算法的優(yōu)勢(shì)主要體現(xiàn)在初始估計(jì)誤差較大的情況下，估計(jì)結(jié)果仍然可以收斂.最后，在無陀螺情況下，用衛(wèi)星動(dòng)力學(xué)模型代替陀螺測(cè)量模型，結(jié)果表明仍能達(dá)到一定精度的估計(jì)精度，適用于一般精度要求的衛(wèi)星姿態(tài)確定.

［1］MOHD POINT KEUI A A，F(xiàn)ADLY M，SIDEK O.EKF implementation on S3CEV40 for Inno SAT attitude determination system［C］//2011 Internation Conference on Computer Applications and Industrial Electronics，Penang，MALAYSIA:IEEE，［s.n.］，2011:373 －378.

［2］HUANG Guoquan P.MOURIKIS A I，ROUMELITIOTIS S I.A First-Estimates Jacobian EKF for improving SLAM Consistency［C］//Proceedings of the International Symposium on Experimental Robotics(ISER)，Athens，Greece:［s.n.］，2008:373 －382.

［3］SOKEN H E，HAJIYEV C.REKF and RUKF development for pico satellite attitude estimation in the presence of measurement faults［C］//Recent Advances in Space Technologies(RAST)，2011 5th International Conforence on，Istanbul:IEEE，2011:891－896.

［4］CHENG Y，CRASSIDIS J.Particle filtering for sequential spacecraft attitude estimation［C］//AIAA Guidance，Navigation，and Control Conference and Exhibit，Providence，American Intstitute of Aeronautics and Astronautics.Rhode，Island:［s.n.］，2004:2004 －5337.

［5］OSHMAN Y，CARMI A.Spacecraft attitude estimation from vector observations using a fast particle filter［J］.Advances in the Astronautical Sciences，2005，Suppl 119:593－607.

［6］姜雪原，馬廣福，胡慶雷.基于Marginalized粒子濾波的衛(wèi)星姿態(tài)估計(jì)算法［J］.控制與決策，2007，22(1):39－44.

［7］GORDON N，SALMOND D.Novel approach to non-linear and non-gaussian Bayesian state estimation ［J］.Proceedings of Institude Electric Engineering，1993，40(2):107－113.

［8］孫兆偉，李暉，張世杰.僅用星敏感器的衛(wèi)星姿態(tài)估計(jì)UKF算法研究［J］.飛行力學(xué)，2006，24(3):56－60.

［9］LIU J S，CHEN R.Sequential monte carlo methods for dynamic system［J］.Journal of the American Statistical Association，1998，443(93):1032－1044.

［10］王晨，房建成.基于Unscented四元數(shù)粒子濾波的微小衛(wèi)星姿態(tài)估計(jì)［J］.北京航天航空大學(xué)學(xué)報(bào)，2007，33(5):552－556.

［11］牟忠凱，隋立芬，黃賢源.基于UPF的修正的羅德里格參數(shù)衛(wèi)星姿態(tài)估計(jì)［J］.中國空間科學(xué)技術(shù)，2009，29(6):45－50.

Satellite attitude determination based on particle filter

ZHANG Jian，SUN Zhao-wei

(Research Center of Satellite Technology，Harbin Institute of Technology，150001 Harbin，China)

In order to investigate the efficiency of attitude determination system using particle filter.The system state equation and measure equation of a satellite with star-sensors and gyros were established.And particle filter was used to estimate the attitude of satellite.The simulation results showed that the system can convergent not only for the small initial estimation error，but also for the big initial estimation error.But EKF can convergent only for the small initial estimation error.Finally，the effectiveness of PF applied to the gyro-free mode was also verified.

particle filter;attitude determination;star-sensor;rate gyro

V19

A

0367－6234(2012)11－0031－05

2012－02－13.

張 健(1988—)，男，碩士研究生;

孫兆偉(1963—)，男，教授，博士生導(dǎo)師.

張 健，zhangjian6140@126.com.

(編輯 苗秀芝)