一種新型粒子濾波算法及其在航天器交會對接相對導航中的應用

李 科，張 騫，龔 彬，劉 偉

(西安衛星測控中心廈門測控站，廈門361023)

1 引言

激光雷達由于激光本身波束窄、相干性好、工作頻率高等優點，能在交會對接過程中，提供高精度的相對距離、速度、角度和角速度的測量，因此，激光雷達作為一種光學敏感器常被用作航天器相對導航。然而，光學敏感器的測量噪聲大多呈現厚尾分布的特點，目前相對導航中常采用的EKF［1-3］、UKF 和 GKF［4］等基于線性高斯分布的方法具有一定局限性，對于這種非線性和非高斯分布的測量模型，有可能會引入較大的模型誤差。

近年來，基于粒子濾波的方法被頻繁引入到航天器導航方面［5-7］。粒子濾波可以用一組加權的隨機粒子樣本來近似狀態的后驗密度函數，適用于任何非線性非高斯系統。相比其他方法，粒子濾波在解決航天器導航問題方面具有明顯的優勢，但是粒子濾波算法仍然存在粒子退化和貧乏問題，國內外許多學者就這方面展開了深入地研究。文獻［8］利用傳統遺傳算法對粒子濾波進行了優化，但是傳統遺傳算法容易出現早熟現象，從而影響濾波性能;文獻［9］在無跡卡爾曼濾波(Unscented Kalman Filter，UKF)中引入量子遺傳算法，以減輕粒子匱乏現象對無跡粒子濾波的影響。本文則利用一種云遺傳算法和UKF對粒子濾波進行改進，應用在航天器交會對接相對導航中，通過解決迭代過程中的粒子貧乏和退化問題來提高估計精度。

2 基于激光雷達測量的系統方程

激光雷達可以實時測量目標航天器相對于追蹤航天器的距離、仰角和方位角。為得到兩航天器的相對位置和相對速度的估計值，本文在粒子濾波器的框架內，以UKF作為建議分布，云遺傳算法進行重采樣，然后進行狀態估計。

考慮理想的二體運動情況，即假設地球為均勻圓球體，目標航天器運行于圓形軌道，不考慮軌道攝動，追蹤航天器和目標航天器均看做質點，建立如圖1所示的相對導航坐標系Oxyz(O1x1y1z1):

圖1 目標航天器和追蹤航天器的垂直坐標系Fig.1 Vertical coordinate systems of target spacecraft and tracking spacecraft

其中原點為目標航天器質心，z軸指向地球質心，x軸垂直于z軸，指向軌道角速度方向，y軸與z、y軸構成右手系。追蹤航天器相對于目標航天器的運動Hill方程如式(1)所示:

由式(1)得到系統的狀態轉移矩陣:

系統的狀態方程為:

其中w(k)為過程噪聲。

激光雷達可提供目標航天器與追蹤航天器在01x1y1z1坐標系下的相對距離ρ和相對視線仰角α以及視線方位角β，系統的測量方程為:

其中vρ(k)，vα(k)，vβ(k)為測量噪聲，服從厚尾分布:

由于系統的狀態方程和狀態變量是在01x1y1z1坐標系下，而測量方程是在01x1y1z1坐標系下，需要將兩個方程中的變量進行統一?？紤]到兩航天器之間的距離較近(1 km以內)，可以認為兩個航天器的坐標系是相互平行的，有x=-x1，y=-y1，z=-z1。則系統的非線性測量方程變為:

令z(X)=(ρ，α，β)T，v(k)=(vρ(k)，vα(k)，vβ(k)T則系統的測量方程為:

其中h(X)=(h1，h2，h3)T。

3 UKF算法

在粒子濾波器的框架內，以UKF作為建議分布，充分利用了當前觀測數據，使樣本與真實后驗分布產生的樣本更相近，即使似然函數位于系統狀態轉移概率密度函數的尾部，而且觀測精度要求較高時，也能很好地逼近真實后驗分布。假設k-1時刻系統的狀態及相應協方差的估計分別為和，計算SIGMA點:

時間更新:

其中和是第j個SIGMA點的權值，nα=nx+nv+nu。

更新預測狀態估計量:

其中K=Px～k，z～k為卡爾曼增益，按下面公式計算得到:

4 云理論與云遺傳重采樣算法

云遺傳重采樣從原理上來說，是在粒子濾波的重采樣過程中應用云遺傳算法中的選擇、交叉、變異等操作步驟，對粒子進行優化。常規的GA改進的PF算法，在遺傳迭代過程中通過個體之間隨機交叉、變異來達到優化的目的，忽略了種群整體的遺傳特征。事實上，當前優秀個體的周圍往往有存在更加優秀的個體的可能性。云遺傳算法［10-11］則利用了正態云模型的隨機性和穩定性特點，在考慮當前粒子的觀測信息的情況下，利用它發現優勝個體的規則，并利用這個規則指導進化過程，能夠提高交叉、變異操作的效率，加快遺傳進化的速度，使得遺傳進化在群體收斂性和個體多樣性之間取得平衡。

4.1 云理論

定義1:假設C屬于定量論域U內的定性概念，若定量值x∈U是C上的一次隨機實現，且x對C的確定度 μ(x)滿足:U→［0，1］，?x∈U，x→μ(x)，則在論域U上的分布成為C的隸屬云。

當μ(x)服從正態分布時，稱為正態云模型。它是隨機數的集合，隱含了三次正態分布規律，具有穩定傾向，可以用期望值Ex，熵En，超熵He三個數值特征來表示。

正態云模型是一種泛正態分布，滿足N3(Ex，En，He)，呈現“中間多，兩頭少”的特點，即隨機性和穩定傾向性，這種分布特點能更近似地描述大部分的客觀實際，李德毅等［12］證明了這種正態云模型的普適性。此外，重采樣后粒子存在的隨機性和傾向后驗概率密度峰值的特點，這與云模型云滴具有的隨機性和穩定傾向性的特點不謀而合，因此本文采用云模型來近似地對粒子集的后驗分布進行描述。

4.2 云遺傳重采樣算法

在計算交叉概率Pc時，將式(16)中條件輸入正態云發生器，生成一個云滴(xi，uc)，則這兩個個體的交叉概率為Pc=uc。

其中:favr為種群平均適應度;fmax為最大適應度;c1、c2為控制參數。

算法3:變異算法

①Ex取原個體;En=變量搜索范圍/c3;He=En/c4;μi由個體適應度值確定。其中c3和c4為控制參數，按經驗值可取c3=3～6，c4=3～5。

②執行算法2獲得和，選擇兩者中適應度值大的個體作為新個體。

從算法2中可知確定度決定了個體的搜索范圍。下面用個體的適應度值通過線性變換來獲得其相應的確定度:

式中:Fmax和Fmin分別表示種群中適應度值最高和最低的個體;Fi和μi分別為第i個個體的適應度值及其確定度;μmax和μmin分別為最大確定度和最小確定度，由經驗值決定，本文中μmax=0.95，μmin=0.2。

算法4:云遺傳重采樣

步驟2.適應度函數計算。將每個粒子的觀測概率作為適應度函數，即Fi=p(|)。

步驟3.選擇、交叉操作。

步驟4.變異操作。通過MH算法選擇可接受粒子:

①在u～［0，1］區間隨機均勻抽樣;

②隨機選取一個舊粒子執行算法3，獲得新粒子(t)，計算變異后粒子的觀測概率p(zk|(i)，計算變異粒子接受概率:

③如果u≤α(，(i))，則接受移動:=);否則拒絕移動:=。這樣獲得新粒子集合

④逐步增大參數和，重復遺傳操作，直到誤差精度符合要求，令=1/N。

5 基于激光雷達測量的粒子濾波器

假設目標航天器位于軌道高度為380 km左右的圓形近地軌道，軌道角速度很小，噪聲之間的耦合可以忽略不計，根據式(3)中給出的系統狀態轉移矩陣和式(6)給出的測量方程，給定目標的狀態空間模型如式(18)。

算法的總體流程圖如圖2所示:

圖2 基于激光雷達測量的粒子濾波總體流程圖Fig.2 Particle filter overall flow chart based on ladar measurement

具體步驟如下:

步驟1.初始化。根據先驗信息p(x0)初始化粒子狀態分布

步驟2.重要性采樣。fori=1，…，N，根據UKF算法更新粒子:

步驟2.2根據方程 (8)～(11)即式(18)，向前傳遞SIGMA點得到狀態及協方差的預測值，根據方程(10)得到狀態估計量，根據方程(12)得到預測的測量值的均值;

步驟2.3根據式(14)～(15)求取卡爾曼增益K后，根據方程(13)修正一步預測估計值，獲得修正的狀態估計。

步驟4.利用算法4進行云遺傳重采樣。

6 實驗與仿真

系統參數設置如下:目標的初始狀態X(0)=(2 500，200，5 000，10，-3，1)T，系統的過程噪聲協方差為:Q=diag(100，100，100，0.1，0.1，0.1)。

假設測量噪聲分布服從厚尾分布，滿足:

圖3 4種算法的RMSE曲線Fig.3 RMSE curves of four kinds of algorithm

表1 目標位置、速度均方根誤差Table 1 Location and speed RMSE of the target

其中，ζ=0.2。

仿真中取粒子數目N=1500;蒙特卡羅仿真次數M=100。圖 3 給出了 PF、RPF［6］、UPF、GAPF和CGAPF的相對均方根誤差(Root Mean Square Error，RMSE)曲線，表1給出了5種算法的平均RMSE性能評價，從圖3和表1可以明顯看出:CGPF性能優于PF和RPF、UPF以及GAPF。其中RMSE和平均RMSE定義分別如式(19)與(20)所示:

7 結論

本文將UKF和云遺傳粒子濾波算法相結合，采用UKF作為建議分布，得到更接近真實的近似分布，在重采樣過程中，由于云模型具有的隨機性和穩定傾向性特點，對粒子進行了較好地優化，有效地化解了粒子的有效性和多樣性之間長期存在的矛盾，提高了跟蹤精度，為交會對接相對導航問題提出了一種新的解決方法，但是，由于星載計算機計算能力有限，粒子濾波算法龐大的計算量使得將其應用于航天器自主導航等實際系統中仍然存在困難。

［1］ 馬鵬斌，王丹.基于EKF和多信息源融合的空間交會對接過程實時定軌方法［J］.載人航天，2011，17(3):7-30.

［2］ 吳偉仁，王大秩，胡海霞，等.一種環月交會對接航天器的自主導航方法［J］.中國科學，2012，42(5):548-555.

［3］ 張劍峰，曾國強.擴展卡爾曼粒子濾波在編隊衛星相對軌道確定中應用［J］.航天控制，2010，8(4):40-45.

［4］ Ienkaran A，Simon H.Discrete time nonlinear filtering algorithms using Gauss-Hermite quadrature［J］.IEEE，2007，95(5):953-977.

［5］ 劉雪奎，孫兆偉，張健，等.基于粒子濾波的通信保障航天器相對導航方法［J］.哈爾濱工業大學學報，2012，44(11):27-30.

［6］ 周翟和，劉建業，賴際舟，等.Rao-Blackwellized粒子濾波在SINS/GPS深組合導航系統中的應用研究［J］.宇航學報，2009，30(2):515-520.

［7］ 劉濤，解永春，胡海霞.粒子濾波及其在航天器交會對接相對導航中的應用［J］.空間控制技術與應用，2011，37(6):19-27.

［8］ 陳善靜，楊華，曾凱，等.基于遺傳算法的粒子濾波跟蹤算法［J］.光電工程，2010，37(10):16-21.

［9］ 李金，于虹，周璐璐，等.基于量子遺傳和無跡粒子濾波的人體運動跟蹤［J］.系統仿真學報，2008，2(18):4867-4871.

［10］ 韓勇，曹興華，楊煜普.一種改進的自適應云遺傳算法［J］.計算機仿真，2011，28(10):191-195.

［11］ 張飛舟，耿嘉洲，程鵬.基于云遺傳算法的公交車輛智能調度［J］.武漢大學學報，2010，35(8):905-908.

［12］ 李德毅，劉常昱.論正態云模型的普適性［J］.中國工程科學，2004，6(8):28-34.