李 科，張 騫，龔 彬，劉 偉

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

1 引言

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

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

2 基于激光雷達(dá)測量的系統(tǒng)方程

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

考慮理想的二體運(yùn)動(dòng)情況，即假設(shè)地球?yàn)榫鶆驁A球體，目標(biāo)航天器運(yùn)行于圓形軌道，不考慮軌道攝動(dòng)，追蹤航天器和目標(biāo)航天器均看做質(zhì)點(diǎn)，建立如圖1所示的相對導(dǎo)航坐標(biāo)系Oxyz(O1x1y1z1):

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

其中原點(diǎn)為目標(biāo)航天器質(zhì)心，z軸指向地球質(zhì)心，x軸垂直于z軸，指向軌道角速度方向，y軸與z、y軸構(gòu)成右手系。追蹤航天器相對于目標(biāo)航天器的運(yùn)動(dòng)Hill方程如式(1)所示:

由式(1)得到系統(tǒng)的狀態(tài)轉(zhuǎn)移矩陣:

系統(tǒng)的狀態(tài)方程為:

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

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

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

由于系統(tǒng)的狀態(tài)方程和狀態(tài)變量是在01x1y1z1坐標(biāo)系下，而測量方程是在01x1y1z1坐標(biāo)系下，需要將兩個(gè)方程中的變量進(jìn)行統(tǒng)一。考慮到兩航天器之間的距離較近(1 km以內(nèi))，可以認(rèn)為兩個(gè)航天器的坐標(biāo)系是相互平行的，有x=-x1，y=-y1，z=-z1。則系統(tǒng)的非線性測量方程變?yōu)?

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

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

3 UKF算法

在粒子濾波器的框架內(nèi)，以UKF作為建議分布，充分利用了當(dāng)前觀測數(shù)據(jù)，使樣本與真實(shí)后驗(yàn)分布產(chǎn)生的樣本更相近，即使似然函數(shù)位于系統(tǒng)狀態(tài)轉(zhuǎn)移概率密度函數(shù)的尾部，而且觀測精度要求較高時(shí)，也能很好地逼近真實(shí)后驗(yàn)分布。假設(shè)k-1時(shí)刻系統(tǒng)的狀態(tài)及相應(yīng)協(xié)方差的估計(jì)分別為和，計(jì)算SIGMA點(diǎn):

時(shí)間更新:

其中和是第j個(gè)SIGMA點(diǎn)的權(quán)值，nα=nx+nv+nu。

更新預(yù)測狀態(tài)估計(jì)量:

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

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

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

4.1 云理論

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

當(dāng)μ(x)服從正態(tài)分布時(shí)，稱為正態(tài)云模型。它是隨機(jī)數(shù)的集合，隱含了三次正態(tài)分布規(guī)律，具有穩(wěn)定傾向，可以用期望值Ex，熵En，超熵He三個(gè)數(shù)值特征來表示。

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

4.2 云遺傳重采樣算法

在計(jì)算交叉概率Pc時(shí)，將式(16)中條件輸入正態(tài)云發(fā)生器，生成一個(gè)云滴(xi，uc)，則這兩個(gè)個(gè)體的交叉概率為Pc=uc。

其中:favr為種群平均適應(yīng)度;fmax為最大適應(yīng)度;c1、c2為控制參數(shù)。

算法3:變異算法

①Ex取原個(gè)體;En=變量搜索范圍/c3;He=En/c4;μi由個(gè)體適應(yīng)度值確定。其中c3和c4為控制參數(shù)，按經(jīng)驗(yàn)值可取c3=3～6，c4=3～5。

②執(zhí)行算法2獲得和，選擇兩者中適應(yīng)度值大的個(gè)體作為新個(gè)體。

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

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

算法4:云遺傳重采樣

步驟2.適應(yīng)度函數(shù)計(jì)算。將每個(gè)粒子的觀測概率作為適應(yīng)度函數(shù)，即Fi=p(|)。

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

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

①在u～［0，1］區(qū)間隨機(jī)均勻抽樣;

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

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

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

5 基于激光雷達(dá)測量的粒子濾波器

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

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

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

具體步驟如下:

步驟1.初始化。根據(jù)先驗(yàn)信息p(x0)初始化粒子狀態(tài)分布

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

步驟2.2根據(jù)方程 (8)～(11)即式(18)，向前傳遞SIGMA點(diǎn)得到狀態(tài)及協(xié)方差的預(yù)測值，根據(jù)方程(10)得到狀態(tài)估計(jì)量，根據(jù)方程(12)得到預(yù)測的測量值的均值;

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

步驟4.利用算法4進(jìn)行云遺傳重采樣。

6 實(shí)驗(yàn)與仿真

系統(tǒng)參數(shù)設(shè)置如下:目標(biāo)的初始狀態(tài)X(0)=(2 500，200，5 000，10，-3，1)T，系統(tǒng)的過程噪聲協(xié)方差為:Q=diag(100，100，100，0.1，0.1，0.1)。

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

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

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

其中，ζ=0.2。

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

7 結(jié)論

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

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

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

［3］ 張劍峰，曾國強(qiáng).擴(kuò)展卡爾曼粒子濾波在編隊(duì)衛(wèi)星相對軌道確定中應(yīng)用［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］ 劉雪奎，孫兆偉，張健，等.基于粒子濾波的通信保障航天器相對導(dǎo)航方法［J］.哈爾濱工業(yè)大學(xué)學(xué)報(bào)，2012，44(11):27-30.

［6］ 周翟和，劉建業(yè)，賴際舟，等.Rao-Blackwellized粒子濾波在SINS/GPS深組合導(dǎo)航系統(tǒng)中的應(yīng)用研究［J］.宇航學(xué)報(bào)，2009，30(2):515-520.

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

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

［9］ 李金，于虹，周璐璐，等.基于量子遺傳和無跡粒子濾波的人體運(yùn)動(dòng)跟蹤［J］.系統(tǒng)仿真學(xué)報(bào)，2008，2(18):4867-4871.

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

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

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