空間非合作目標的相對導航粒子濾波算法*

金煌煌，溫奇詠，夏紅偉，王常虹

（哈爾濱工業大學控制工程系，哈爾濱150001）

空間非合作目標的相對導航粒子濾波算法*

金煌煌，溫奇詠，夏紅偉，王常虹

（哈爾濱工業大學控制工程系，哈爾濱150001）

針對激光交會雷達作為在空間交會對接過程中的相對導航敏感器，研究了未來特殊背景下，非合作目標航天器對雷達信號發射干擾的情況下的相對導航濾波算法.針對高維模型計算量大的特點對傳統粒子濾波算法進行了改進，并與擴展卡爾曼濾波器進行了仿真比較，仿真結果表明改進后的粒子濾波器的濾波效果遠遠優于擴展卡爾曼濾波器，且計算量優于傳統粒子濾波.

非合作目標；自主相對導航；粒子濾波；激光雷達

針對空間合作目標的交會對接技術研究已經進入了實際應用階段，包括空間站的組建、航天飛機與空間站對接等，諸如美國的阿波羅計劃、美國的“阿特蘭蒂斯”號航天飛機與“和平”號空間站的對接等交會對接計劃的成功實施也標志了交會對接過程中的相對導航逐漸由有人參與到自主導航過渡.

隨著人類對空間的不斷探索和深入，對航天技術提出了更高的要求.交會對接技術的應用也逐漸由空間合作目標推廣到空間非合作目標.空間非合作目標包括故障或失效衛星、空間垃圾等.在追蹤航天器對非合作目標的交會對接、捕捉過程中需要得到準確的相對距離、相對速度等信息.針對非合作目標的精確相對導航存在以下可能的困難：觀測量不完備、缺少相對定位標志、敵對目標發射的雷達干擾等，因此近年來針對空間非合作目標的相對導航問題已經成為一個研究的熱點［1］.

本文研究了在非合作目標對激光交會雷達的觀測進行干擾下的濾波問題.將干擾視為非高斯噪聲，此時已經無法滿足傳統的卡爾曼濾波器關于系統噪聲和觀測噪聲都服從高斯分布的假設，且相對導航中的狀態方程和觀測方程都為非線性，因此需要尋找一種合適的濾波方法.近年來興起的粒子濾波正是解決非線性非高斯背景下的狀態估計的近似最優方法［2］.其基本思想是用一組帶權值的離散采樣值去逼近一種分布，這些采樣值類似于一個個“粒子”，然后在觀測的基礎上通過調節樣本的權值大小和樣本位置來獲得服從實際分布的樣本，并以最終樣本的均值為系統狀態估計值［3］.但由于傳統粒子濾波存在計算量過大的問題，使其很難在實際項目中得到應用，本文將采用一種改進的高斯粒子濾波器，對近地圓軌道上的非合作目標相對導航進行仿真，并通過與傳統粒子濾波器以及擴展卡爾曼濾波器的濾波效果進行比較，證明本方法的有效性.

1 相對導航狀態方程

記追蹤航天器為 c，目標航天器為 t.假設 c在近圓軌道上運動，取目標航天器的軌道坐標系txyz作為相對運動坐標系，其原點與t的質心固連并隨其沿軌道運動，x軸與目標航天器的地心矢量重合，由地心指向 t，y軸在追蹤航天器的軌道面內垂直于 x軸，并指向運動方向，z軸由右手規則確定，亦即z軸與追蹤航天器軌道動量矩矢量的方向一致.軌道坐標系 t－xyz與地心慣性坐標系 OEXYZ的關系如圖1所示.

圖1 相對運動坐標系與地心慣性坐標系的關系

在地心慣性坐標系中，目標航天器和追蹤航天器的動力學方程分別如下：

上式可進一步表示為下列兩個等效的關系式

為了建立追蹤航天器與目標航天器在動坐標系t－xyz中的相對運動方程，有

將式（5）代入式（7）可得

對于目標航天器和追蹤航天器的近距離相對運動情況，航天器間距是小量，特別是當相對高度不大（即 rt／rc接近 1）時.從式（6）可以看出，其右端第一項為中心引力加速度差表達式，通過取一次近似可以進行簡化，可得與式（8）比較可知，通過簡化將相對運動動力學方程轉化為一組常系數線性微分方程［4］.

2 觀測模型及非高斯觀測噪聲建模

利用星載激光雷達可以測量航天器的相對距離ρ和目標航天器相對追蹤航天器的方位角α以及俯仰角β，一般可以假設傳感器坐標系和追蹤航天器軌道坐標系重合，同時基于上述假設可以認為追蹤航天器軌道坐標系和目標航天器軌道坐標系平行，基于這兩個前提假設可以省略坐標系轉換過程.于是在目標航天器軌道坐標系中可以建立觀測方程［5］y＝h（x）＋v，其中觀測量 y＝（ρ，α，β），h（x）可由下式表示：

v代表觀測噪聲采用的閃爍分布.考慮到非合作相對導航過程中傳感器觀測量可能受到的非合作干擾，假設觀測噪聲為角閃爍噪聲，角閃爍是光學區跟蹤雷達在近距離上面臨的主要誤差源，其產生機理與擴展目標的散射中心理論密切聯系.在光學區，目標可以看作由許多散射中心組成，目標回波可以看作由這些散射中心的回波反射合成的，合成結果等效為一個視在中心，這個中心的位置也就是實際測得的位置.隨著目標航天器和追蹤航天器間的相對運動，散射中心的反射也在不斷變化，導致視在中心發生變化，從而產生測角誤差，這種現象稱為角閃爍，嚴重時角閃爍可以導致跟蹤點偏離到目標之外［6］，閃爍噪聲的概率密度函數為

式中，ε為一小于1的正數，pG（w）為高斯密度函數，pt（w）表示厚尾分布，常用的厚尾分布有拉普拉斯分布、t分布、均勻分布、大方差的高斯分布等，選取pt（w）為拉普拉斯分布.

3 粒子濾波算法

針對模型中存在的非線性以及非高斯噪聲干擾，需要選取合適的濾波算法.粒子濾波算法是一種基于蒙特卡羅仿真的近似貝葉斯濾波算法.它的核心思想為用一批有相應權重的離散隨機采樣點來近似狀態變量的后驗概率密度函數（pdf），這批采樣點被稱為粒子，并根據這些粒子以及它們的權重來計算估計值.當粒子數很多的時候這種濾波方法就可以接近最優貝葉斯估計.狀態變量的概率密度函數提供了有關狀態變量分布的所有信息，一旦獲得了概率密度函數，就可以依照不同的準則函數計算得到狀態變量的極大似然估計、最小方差估計、最大后驗估計等等.相對于EKF和UKF算法，粒子濾波算法能夠處理任意的非線性函數和非高斯分布，根據采樣點就能夠得到均值、協方差和其他統計量.

基本粒子濾波算法基于Gordan等人在20世紀90年代提出的“采樣-重要性-重采樣”（SIR，sampling importance resampling）濾波算法［7］，與其他粒子濾波算法而言，它具有簡單和易于實現的特點.下面簡要介紹一下貝葉斯估計理論和SIR算法.

3.1 貝葉斯估計理論

假設一般非線性動力學系統的狀態方程和測量方程分別為

貝葉斯估計的目標是構建狀態變量的后驗概率密度函數 p（xk＋1｜Yk＋1）.為此有貝葉斯遞推濾波算法，該算法每一步遞推過程由預測和更新兩步組成［8］.

預測：當已知 k時刻的概率密度函數 p（xk｜Yk）時，利用給定的系統狀態方程（12）與過程噪聲的統計特性 p（wk），可以預測狀態的前驗概率密度p（xk＋1｜Yk），具體算式為

更新：當k＋1時刻的測量 yk＋1獲得時，則可利用系統的測量值、測量模型及測量噪聲統計特性，對前驗概率密度函數p（xk＋1｜Yk）進行修正并給出后驗概率密度 p（xk＋1｜Yk＋1），具體算式為

以上為嚴格的貝葉斯濾波公式，對于線性高斯系統，pdf可由均值和方差來表示，狀態的最小方差估計可以由卡爾曼濾波得到，而對于非線性、非高斯系統，則無法求得上述概率密度函數的解析解.為此需要尋找適于更多實際系統應用的狀態概率密度函數的近似求解法.其中粒子濾波則為一種有效的近似方法.

3.2 SIR粒子濾波方法

根據蒙特卡羅近似方法，后驗概率密度函數p（xk｜Yk）可由一批離散的采樣點｛，｝近似，i＝1，…，N，其中稱為粒子，為相應的權值

＝1.下標k代表時間，于是k時刻的后驗概率密度可以離散的加權近似為

其中權值可以通過重要性采樣法進行選擇.如果能直接從p（x）中產生粒子，則每一粒子將被賦予1／Ns的權值.當無法直接從 p（x）中產生粒子時，選擇從分布 q（x）中產生粒子，這里 q（x）稱為重要性函數.這樣，可以根據下式分配權值重要性函數在粒子濾波器的性能中起著關鍵的作用.一般的，q（x）越接近于 p（x）越好.最常用的兩個重要性函數是先驗重要性密度函數與最佳重要性密度函數.先驗密度函數取為p（xk），基于更新表達式（14），它意味著權值的更新為而最佳重要性函數取為p（xk，y0：k），權值更新則依據

可見，實現先驗重要性密度函數比較容易，因為選最佳重要性函數時計算－）需要積分.順序的重要性采樣任務就是從重要性函數中采樣生成樣本集和遞歸地計算重要性權重.

然而，隨著迭代次數的增加，重要性權值的分布變得越來越傾斜，這就是粒子濾波容易出現的粒子退化現象，即除了一個粒子外，所有粒子只具有微小的權值［9］.為了避免粒子退化，Gordan等人提出了重采樣方法，其主要思想是去除那些權值小的粒子，復制權值大的粒子.

基本SIR粒子濾波算法步驟參見參考文獻［3］.

3.3 高斯粒子濾波

傳統的基本SIR粒子濾波存在計算量大、權值退化以及多樣性匱乏等缺點，很難在實時性要求很高的自主相對導航中得到實際應用，針對以上缺點，一些改進的粒子濾波算法層出不窮.為了獲得更好的重要性密度函數，先后提出了輔助粒子濾波算法（auxiliary particle filter）；擴展卡爾曼粒子濾波算法（extended Kalman particle filter）等一系列有效的方法.為了解決重采樣步驟帶來的粒子多樣性匱乏問題，又進一步在粒子濾波中引入了馬爾可夫鏈蒙特卡洛移動 （MCMC，Markov chain monto carlo）步驟［10］.

本文應用了一種新的濾波器即高斯粒子濾波器［11］.該濾波器基于粒子濾波方法得到一高斯分布來近似估計未知狀態變量的后驗分布.并通過仿真結果表明，該濾波器濾波精度高于 EKF，并且由于該濾波器將后驗分布近似成高斯分布，因此不需要重采樣，也不需要重采樣，不存在粒子退化現象，極大的降低了粒子濾波的復雜性和計算量.高斯粒子濾波的基本思想是通過一個高斯分布近似濾波概率分布，具體的高斯粒子濾波算法步驟［12］如下：

1）判斷是否是初始化步驟，若是則根據初始化方法從 p（x0）采樣粒子，若不是則根據 N（，Pk－1）采樣得 N個粒子（i＝1，…，N）；

3）輸出：

4）判斷濾波是否結束，若沒結束則返回到1）.

4 仿真實例

4.1 狀態方程離散化

離散化后的狀態方程為

X（k）＝ΦX（k－1）＋Γw（k），

式中T為采樣周期，s和c分別代表sinωT和cosωT.Γ為過程噪聲方差陣，

式中w（k）為系統過程噪聲，在仿真中假設為高斯白噪聲.

4.2 仿真結果與分析

為了驗證方法的有效性，本文進行了仿真試驗，仿真條件如下：目標航天器的軌道參數取為：軌道高度為780 km圓軌道，激光雷達的測距精度為0.6 m（3σ），對視線角的測量精度為 0.1°（3σ）；相對軌道狀態初始條件如下：X0＝［－520，10，20，－0.0324，－0.02，0.02］T，濾波器初始條件為X＾0＝［－510，0，1，0，0，0，0］T；采樣周期為 1 s.

對EKF、PF和 GPF三種濾波器進行了仿真比較，PF和GPF粒子數為1 000時運行100個周期的誤差曲線如圖2～圖4所示.

圖2 x軸相對距離誤差曲線

圖3 y軸相對距離誤差曲線

圖4 z軸相對距離誤差曲線

表1 GPF（粒子數 N＝1 000）與 PF、EKF的性能對比

從圖2～圖4以及表1可以看出，對非線性系統存在非高斯噪聲干擾的情況下，GPF比EKF更快速地逼近到真值，最終的誤差值也收斂到允許范圍內；并且在高維模型的背景下，GPF比基本粒子濾波實時性要好得多，能基本符合實際應用的要求，從粒子濾波的粒子權重更新值上也證明了GPF不存在粒子退化現象，從而不需要重采樣步驟，大大節省了計算量.

5 結束語

本文應用一種改進的粒子濾波算法——高斯粒子濾波（GPF），解決了基本粒子濾波在處理高維模型時的計算量大、粒子多樣性匱乏等問題，而且處理非高斯噪聲的能力優于EKF，具有重要的應用前景和進一步研究價值.

［1］張秋華，韓琦，孫毅.空間非合作目標交會運動狀態估計的卡爾曼濾波方法［A］.全國測控、計量與儀器儀表學術年會論文集（下冊），2004

［2］李保國，肖懷鐵，王遠模，等.角閃爍背景下基于粒子濾波器的跟蹤研究［J］.現代雷達，2006，28（2）：54-55

［3］胡士強，敬忠良.粒子濾波算法綜述［J］.控制與決策，2005，20（4）：361-365

［4］郗曉寧，王威，高玉東.近地航天器軌道基礎［M］.長沙：國防科技大學出版社，2003：244-247

［5］何英姿，諶穎，韓冬.基于交會雷達測量的相對導航濾波器［J］.航天控制，2004，22（6）：17－20

［6］劉勇，徐世杰.航天器相對軌道自主確定算法研究［C］.全國第十二屆空間及運動體控制技術年會，桂林，2007

［7］黃培康.雷達目標特征信號［M］.北京：宇航出版社，1993

［8］Gordan N J，Salmond D J.Novel approach to nonlinear and non-gaussian bayesian state estimation［J］.Proc.of Institute Electric Engineering，1993，140（2）：107-113

［9］Arulampalam M S，Maskell S，Gordan N，et al.A tutorial on particle filters for online nonlinear non-Gaussian Bayesian tracking［J］.IEEE Transactions on Signal Processing，2002，50（2）：174-175

［10］胡洪濤，敬忠良，李安平，等.非高斯條件下基于粒子濾波的目標跟蹤［J］.上海交通大學學報，2004，38（12）：1998

［11］戴丁樟.粒子濾波算法研究及其在目標跟蹤中的應用［D］.哈爾濱工業大學，2006，11：32-33

［12］王寧，王從慶.高斯粒子濾波器及其在非線性估計中的應用［J］.南京航空航天大學學報，2006，38（增刊 1）：132-135

［13］Kotecha J H，Djuric P M.Gaussian particle filtering［J］.IEEE Transactions on Signal Processing，2003，51（10）：2592-2601

Particle Filter Algorithm for Relative Navigation of Space Non-Cooperative Target

JIN Huanghuang，WEN Qiyong，XIA Hongwei，WANG Changhong
（Dept.Control Engineering，Harbin Institute of Technology，Harbin 150001，China）

Taking a laser radar as the relative navigation sensor during space rendezvous and docking in space，a relative navigation filter algorithm for a non-cooperative target spacecraft eradiating radar jamming signal is investigated for a special situation in the future.Considering large computational effort for high-dimensional model，traditional particle filter algorithm is improved，and compared with EKF through simulation.The simulation results show that the filtering effect of improved particle filter is much better than the EKF，and the computational effort is largely reduced compared with traditional particle filter.

non-cooperative target；autonomous relative navigation；particle filter；laser radar

V4

A

1674-1579（2009）04-0006-05

*黑龍江省青年科學技術專項資金（QC07C15）資助項目.

2009-03-15

金煌煌（1986—），男，安徽人，碩士研究生，研究方向為空間相對導航 （e-mail：jinhh10＠gmail.com）.