張大力，夏紅偉，馬廣程，王常虹

（哈爾濱工業(yè)大學(xué) 航天學(xué)院，哈爾濱 150001）

基于螢火蟲群優(yōu)化的空間非合作目標(biāo)相對(duì)導(dǎo)航粒子濾波算法

張大力，夏紅偉，馬廣程，王常虹

（哈爾濱工業(yè)大學(xué) 航天學(xué)院，哈爾濱 150001）

研究了空間非合作目標(biāo)相對(duì)導(dǎo)航算法，針對(duì)標(biāo)準(zhǔn)粒子濾波的重采樣過程導(dǎo)致的粒子貧化現(xiàn)象及其造成的相對(duì)導(dǎo)航精度下降問題，分析了螢火蟲優(yōu)化算法的運(yùn)行機(jī)制，提出一種基于螢火蟲智能優(yōu)化算法的改進(jìn)粒子濾波算法。改進(jìn)算法通過優(yōu)化粒子濾波的重采樣過程，使粒子群智能的向高似然區(qū)域移動(dòng)，同時(shí)在低似然區(qū)域也合理保留了部分粒子，保證了粒子的多樣性，提高了樣本的整體質(zhì)量。仿真結(jié)果表明，改進(jìn)算法導(dǎo)航精度較標(biāo)準(zhǔn)算法提高了39.35%，達(dá)到穩(wěn)定精度所需粒子數(shù)較少，有效抑制了粒子貧化問題。

螢火蟲算法；粒子濾波；粒子貧化；相對(duì)導(dǎo)航

航天器的相對(duì)導(dǎo)航是實(shí)現(xiàn)在軌服務(wù)和編隊(duì)飛行任務(wù)的基礎(chǔ)，直接影響控制和制導(dǎo)精度[1]。目前對(duì)空間合作目標(biāo)的相對(duì)導(dǎo)航研究基本趨于成熟，對(duì)非合作目標(biāo)如故障、失效衛(wèi)星或空間碎片等的研究較少，是目前航天領(lǐng)域研究的熱點(diǎn)[2]。經(jīng)典方法基于線性系統(tǒng)和高斯噪聲假設(shè)，利用目標(biāo)的反饋信息，通過擴(kuò)展卡爾曼濾波(EKF)估計(jì)目標(biāo)的真實(shí)相對(duì)狀態(tài)[3]。

非合作目標(biāo)的運(yùn)動(dòng)往往無法用線性模型描述，且實(shí)際噪聲通常不滿足高斯分布，需要轉(zhuǎn)化為線性問題處理，導(dǎo)致 EKF估計(jì)效果急劇下降。無跡卡爾曼濾波較 EKF具有更好的非線性濾波性能，但對(duì)于非高斯噪聲問題依然缺乏適應(yīng)性[4]。粒子濾波是近年來逐漸發(fā)展成熟的基于蒙特卡洛思想的濾波技術(shù)，不受系統(tǒng)非線性和非高斯條件的限制[5]，得到國(guó)內(nèi)外學(xué)者的青睞，取得了很多研究成果。然而標(biāo)準(zhǔn)粒子濾波采用重采樣技術(shù)，復(fù)制大權(quán)值樣本，小權(quán)值粒子被直接舍棄，這導(dǎo)致了粒子的貧化問題?？陀^來講，雖然小權(quán)值粒子對(duì)濾波估計(jì)貢獻(xiàn)有限，但其中仍包含了一定的狀態(tài)信息，直接舍棄勢(shì)必影響濾波精度，這限制了粒子濾波的發(fā)展和應(yīng)用前景。

針對(duì)這種粒子貧化問題，國(guó)內(nèi)外學(xué)者進(jìn)行了大量研究。Zhao[6]利用MCMC方法通過構(gòu)造Markov鏈產(chǎn)生隨機(jī)樣本，增加了粒子多樣性，但增加了計(jì)算量；Sun[7]引入輔助變量近似后驗(yàn)密度，減少了權(quán)值方差，但對(duì)噪聲適應(yīng)性較差；Li[8]利用遺傳算法的進(jìn)化思想對(duì)粒子濾波進(jìn)行改進(jìn)，一定程度上提高了濾波實(shí)時(shí)性；Liu[9]利用粒子群算法的全局尋優(yōu)能力優(yōu)化粒子濾波，減少了濾波所需粒子數(shù)，但容易陷入局部最優(yōu)。

LI[10]利用人工魚群算法對(duì)粒子濾波進(jìn)行優(yōu)化，收斂速度快，但精度不高。受此啟發(fā)，本文引入螢火蟲群智能優(yōu)化思想，在對(duì)其運(yùn)行機(jī)制進(jìn)行改進(jìn)的基礎(chǔ)上優(yōu)化粒子濾波的重采樣過程，改善粒子貧化問題，提高導(dǎo)航精度，并通過仿真結(jié)果驗(yàn)證算法的有效性。

1 相對(duì)導(dǎo)航問題狀態(tài)模型與觀測(cè)模型

1.1 相對(duì)運(yùn)動(dòng)狀態(tài)模型

考慮空間兩在軌目標(biāo)的近距離相對(duì)運(yùn)動(dòng)，推導(dǎo)相對(duì)運(yùn)動(dòng)動(dòng)力學(xué)方程，為跟蹤濾波算法提供狀態(tài)方程。在可觀測(cè)條件下，兩星在軌運(yùn)行情況如圖1所示，其中：C為追蹤星(Chaser)，運(yùn)行于近圓軌道；T為目標(biāo)星(Target)，為非合作目標(biāo)。令追蹤星軌道坐標(biāo)系為o-xyz，原點(diǎn)o位于追蹤星質(zhì)心，ox軸沿地心矢徑方向指向衛(wèi)星，oy軸指向追蹤星飛行的切線方向，oz軸垂直于軌道平面向上，三軸滿足右手定則。

圖1 相對(duì)運(yùn)動(dòng)示意圖Fig.1 Relative motion diagram

由牛頓第二定律得追蹤星和目標(biāo)星對(duì)慣性空間的運(yùn)動(dòng)方程為

式中：rC、rT為地心到C和T的矢量；μ為地心引力常數(shù)；fC、fT為控制力和空間攝動(dòng)力作用下C和T的加速度矢量。

1.2 觀測(cè)模型確定

本文采用“光學(xué)測(cè)角相機(jī)+激光測(cè)距儀”的觀測(cè)模式，如圖2所示。

光學(xué)測(cè)角相機(jī)可以給出T在o-xyz系下方位角(A)和高度角(E)，激光測(cè)距儀可提供C與T的相對(duì)距離ρ。

圖2 觀測(cè)模型示意圖Fig.2 Observation model diagram

以上參數(shù)與相對(duì)軌道坐標(biāo)的關(guān)系可以表示為

式中：

觀測(cè)方程可寫成如下形式：

式中：Y(k)為k時(shí)刻的觀測(cè)量；v(k)為閃爍噪聲。

概率密度分布為

公式(3)和(7)組成了相對(duì)導(dǎo)航問題的狀態(tài)方程和觀測(cè)方程。需要補(bǔ)充的是，本文不考慮導(dǎo)航過程中姿態(tài)變化等因素導(dǎo)致的設(shè)備測(cè)量坐標(biāo)系與軌道坐標(biāo)系不重合現(xiàn)象，避免了繁瑣的坐標(biāo)轉(zhuǎn)換。

2 粒子濾波分析

通常情況下，先驗(yàn)密度函數(shù)被選為重要密度函數(shù)，即：

當(dāng)用重要性函數(shù)替代后驗(yàn)概率分布作為采樣函數(shù)時(shí)，理想情況是重要性函數(shù)非常接近后驗(yàn)概率分布。而在實(shí)際問題中，通常使用式(10)中的次優(yōu)重要性概率密度代替。當(dāng)觀測(cè)值較準(zhǔn)確或似然函數(shù)位于先驗(yàn)概率密度尾部時(shí)，似然概率與先驗(yàn)概率分布的重疊部分就會(huì)很小，權(quán)重更新后許多粒子的權(quán)重會(huì)變得很小，經(jīng)重采樣過程，多數(shù)粒子就會(huì)被篩選掉，嚴(yán)重時(shí)甚至僅剩單一粒子，從而導(dǎo)致粒子貧乏問題，影響濾波精度。

3 螢火蟲群優(yōu)化粒子濾波算法

3.1 螢火蟲算法

螢火蟲算法是模擬螢火蟲群的群體行為的隨機(jī)優(yōu)化算法，由劍橋?qū)W者yang[11]于2009年提出，是最新的智能優(yōu)化算法之一，具有良好的收斂速度和精度，實(shí)現(xiàn)簡(jiǎn)單，易于工程實(shí)現(xiàn)。其仿生原理為：搜索空間中的點(diǎn)抽象為螢火蟲個(gè)體，尋優(yōu)過程模擬個(gè)體間的吸引和移動(dòng)過程，優(yōu)化目標(biāo)抽象為個(gè)體位置的優(yōu)劣。算法遵循下列理想行為規(guī)則：

1）所有螢火蟲無性別，每個(gè)螢火蟲均可吸引其他螢火蟲而與它們的性別無關(guān)。

2）吸引度與它們的亮度成正比，并隨著距離的增加而減少，若兩個(gè)個(gè)體亮度完全相同，則自由移動(dòng)。

3）熒光亮度由要優(yōu)化的目標(biāo)函數(shù)值決定。

算法包含兩個(gè)要素：亮度和吸引度。亮度體現(xiàn)了個(gè)體所處位置的優(yōu)劣并決定其移動(dòng)方向，個(gè)體受到的吸引度決定了其移動(dòng)的距離。通過對(duì)這兩個(gè)核心要素的更新實(shí)現(xiàn)迭代尋優(yōu)過程。其參數(shù)化表述如下：

1）螢火蟲的相對(duì)熒光亮度為

其中：I0代表光源的光強(qiáng)；γ近似代表固定的光吸收系數(shù)，典型取值范圍(0.1, 10)；rij代表螢火蟲個(gè)體間的空間距離。

2）螢火蟲吸引度公式為

其中，β0為最大吸引度，通常取值為1。

3）在t時(shí)刻，螢火蟲個(gè)體i被個(gè)體j吸引并向后者移動(dòng)的位置更新公式為

3.2 優(yōu)化算法改進(jìn)

針對(duì)粒子濾波的存在的問題，本文擬采用螢火蟲算法的尋優(yōu)能力優(yōu)化粒子濾波的重采樣過程。由螢火蟲算法的原理可知，螢火蟲算法與粒子濾波算法有一定相似之處，前者通過更新個(gè)體熒光亮度和位置來尋求最優(yōu)值，后者通過更新粒子權(quán)值和位置來逼近系統(tǒng)真實(shí)后驗(yàn)概率密度。

由于螢火蟲算法本身運(yùn)行機(jī)制的特殊性，直接進(jìn)行算法融合會(huì)帶來一系列問題，如循環(huán)嵌套和粒子交互導(dǎo)致算法復(fù)雜度大幅增加等，因此本文對(duì)螢火蟲算法中的熒光亮度進(jìn)行重定義，對(duì)吸引度更新機(jī)制進(jìn)行改進(jìn)。

1）熒光亮度重定義

在粒子濾波中，權(quán)值ω是評(píng)價(jià)粒子的重要參量，越接近真實(shí)值權(quán)重越大；在螢火蟲算法中，個(gè)體越接近最優(yōu)值亮度越高。將熒光亮度定義為粒子權(quán)值，即

重新定義的熒光亮度公式意義更為明確，同時(shí)避免了不必要的運(yùn)算，降低了運(yùn)算復(fù)雜度。

2）改進(jìn)吸引度更新公式

利用螢火蟲算法吸引度和位置更新公式模擬不同權(quán)重粒子的移動(dòng)過程，低權(quán)值粒子受高權(quán)值粒子吸引，向后者移動(dòng)，自身權(quán)重提高。權(quán)重較小的粒子所攜帶的觀測(cè)信息在估計(jì)結(jié)果中得到體現(xiàn)，避免了標(biāo)準(zhǔn)粒子濾波重采樣過程簡(jiǎn)單復(fù)制替換帶來的粒子退化現(xiàn)象。

3.3 算法具體實(shí)現(xiàn)和步驟

2）數(shù)據(jù)處理。粒子經(jīng)狀態(tài)模型和觀測(cè)模型處理后分別得到k時(shí)刻的狀態(tài)值和預(yù)測(cè)值。

3）權(quán)值更新。根據(jù)次優(yōu)概率密度函數(shù)，結(jié)合預(yù)測(cè)值和實(shí)際觀測(cè)值對(duì)權(quán)重進(jìn)行更新。具體為

4）根據(jù)螢火蟲算法，通過式(14)和式(15)得到螢火蟲個(gè)體的亮度和相對(duì)吸引度。亮度的對(duì)比結(jié)果決定螢火蟲移動(dòng)方向，吸引度值代入式(13)得到個(gè)體移動(dòng)距離。位置更新后進(jìn)行亮度更新，并繼續(xù)向更高亮度個(gè)體位置移動(dòng)，形成迭代過程。引入最大迭代次數(shù)MaxG和終止閾值D，當(dāng)亮度值大于D或達(dá)到最大迭代次數(shù)時(shí)停止優(yōu)化。D的引入減少了在權(quán)值相近的情況下依然迭代至最大迭代次數(shù)的概率，提高了算法靈活性和效率。

經(jīng)補(bǔ)償，粒子集在理論上服從同一分布，從而保證了算法的合理性。

6）歸一化

7）狀態(tài)輸出

4 仿真實(shí)驗(yàn)與分析

為進(jìn)一步驗(yàn)證FA-PF算法對(duì)解決空間非合作目標(biāo)相對(duì)導(dǎo)航問題的有效性，本文進(jìn)行了仿真試驗(yàn)。仿真條件如表1所示。

表1 航天器軌道根數(shù)Tab.1 Orbit elements of spacecraft

給定初值[0.173 176, 0, 0.100 001, 0, -0.000 205, 0]；根據(jù)某實(shí)際系統(tǒng)，標(biāo)稱測(cè)距精度 15 m (3σ)，測(cè)角精度0.3° (3σ)。采樣周期為1 s，對(duì)10 800個(gè)時(shí)刻的相對(duì)狀態(tài)信息進(jìn)行濾波估計(jì)。

將初值分別代入PF和FA-PF算法，仿真時(shí)間設(shè)置為 10 800 s。設(shè)置間隔為25，給出粒子數(shù)在 25到200時(shí)的濾波結(jié)果，并分別對(duì)相對(duì)位置誤差、相對(duì)速度誤差和仿真時(shí)間進(jìn)行對(duì)比。目標(biāo)航天器在O-XYZ坐標(biāo)系下的位置和速度為所需的相對(duì)軌道數(shù)據(jù)。

首先給出在不同粒子數(shù)下PF和FA-PF算法相對(duì)位置、相對(duì)速度和濾波時(shí)間的對(duì)比條形圖，如圖3～5所示。

圖3 相對(duì)位置誤差對(duì)比Fig.3 Comparison on relative position errors

圖5 濾波時(shí)間對(duì)比Fig.5 Comparison on filtering times

由圖5可知，隨著粒子數(shù)的增加，仿真時(shí)間在不斷延長(zhǎng)。相同粒子數(shù)下，PF和FA-PF算法仿真時(shí)間基本一致，前者略高于后者。當(dāng) PF算法在取150個(gè)粒子，仿真時(shí)間達(dá)到 115.564 15 s的情況下，才能達(dá)到最高精度，而 FA-PF算法在取 100個(gè)粒子，仿真71.152 52 s即可達(dá)到最高精度。

綜合考慮，F(xiàn)A-PF在精度和時(shí)間上均優(yōu)于PF算法。更直觀的，給出取100粒子時(shí)PF和FA-PF算法的相對(duì)位置和速度誤差曲線，如圖6、圖7所示。

由于設(shè)置仿真步數(shù)較多，為避免圖像丟幀導(dǎo)致結(jié)果不直觀，取曲線中 56個(gè)點(diǎn)來繪制相對(duì)位置和相對(duì)速度誤差曲線。圖 6和圖 7顯示，在相同條件下，F(xiàn)A-PF算法表現(xiàn)較PF算法穩(wěn)定。為了更清晰地展示FA-PF算法對(duì)粒子貧化現(xiàn)象的抑制作用，給出狀態(tài)輸出前的粒子分布圖，如圖8所示。

圖6 100粒子相對(duì)位置誤差對(duì)比Fig.6 Comparison on relative position errors of 100 particles

圖7 100粒子相對(duì)速度誤差對(duì)比Fig.7 Comparison on relative velocity errors of 100 particles

圖8 粒子相對(duì)位置狀態(tài)分布Fig.8 Relative position state distribution of particles

圖6～8中，實(shí)線表示真實(shí)狀態(tài)，“*”表示PF算法粒子分布，“?！北硎綟A-PF算法粒子分布?？梢钥闯?，標(biāo)準(zhǔn)粒子濾波算法粒子多樣性表現(xiàn)一般，在有些位置分布較為集中，不利于濾波器的狀態(tài)估計(jì)。而FA-PF算法粒子在整體向高似然區(qū)域移動(dòng)的同時(shí)，在低似然區(qū)域也合理的保留了部分粒子，使得這部分粒子所包含的狀態(tài)信息得以在估計(jì)結(jié)果中體現(xiàn)，保證了粒子的多樣性。

5 結(jié) 論

本文提出了一種基于螢火蟲群優(yōu)化的空間非合作目標(biāo)相對(duì)導(dǎo)航粒子濾波算法，以改善標(biāo)準(zhǔn)粒子濾波的粒子貧化現(xiàn)象。文中給出了兩星相對(duì)導(dǎo)航問題的狀態(tài)模型和觀測(cè)模型，分析了標(biāo)準(zhǔn)粒子濾波的缺陷，指出為解決粒子退化問題而引入的重采樣技術(shù)會(huì)引起粒子貧化現(xiàn)象。結(jié)合螢火蟲群優(yōu)化思想，通過引入改進(jìn)的螢火蟲算法的熒光亮度和吸引度更新機(jī)制來解決粒子濾波的粒子貧化問題，并給出算法步驟。仿真結(jié)果表明，改進(jìn)算法有效抑制了粒子貧化問題，明顯提高了相對(duì)導(dǎo)航精度，有很高的工程應(yīng)用價(jià)值。

（References）：

[1] 王楷, 湯亮, 李克行, 等. 航天器相對(duì)導(dǎo)航與控制技術(shù)的典型任務(wù)[J]. 空間控制技術(shù)與應(yīng)用, 2016, 42(1): 7-12. Wang Kai, Tang Liang, Li Ke-xing. Application of relative navigation and control technology in specific space missions[J]. Aerospace Control and Application, 2016, 42(1): 7-12.

[2] 李文躍, 徐貴力, 周龍, 等. 空間非合作目標(biāo)相對(duì)導(dǎo)航技術(shù)研究[J]. 航空兵器, 2012(3): 14-17. Li Wen-yue, Xu Gui-li, Zhou Long, et al. Research on mea- surement of relative poses between two non-cooperative spacecrafts[J]. Aero Weaponry, 2012(3): 14-17.

[3] Hablani H B, Tapper M L, Danabashian D J. Guidance and relative navigation for autonomous rendezvous in a circular orbit[J]. Journal of Guidance Control and Dynamics, 2012, 25(3): 553-562.

[4] Wu Jin-yuan, Fang Jian-cheng, Yang Zhao-hua. ASUKF based relative navigation method for mars probe using pulsar[J]. Chinese Journal of Scientific Instrument, 2013, 34(8): 1711-1716.

[5] 朱志宇. 粒子濾波算法及其應(yīng)用[M]. 北京: 科學(xué)出版社, 2010: 27-29.

[6] Zhao Z N, Kumar M. An MCMC-based particle filter for multiple target tracking[C]//15thInternational Conference on Information Fusion. Singapore, 2012: 1676-1682.

[7] Sun Yun, Wang Guo-hong, Tan Shun-cheng, et al. A TBD algorithm for maneuvering stealthy target based on auxiliary particle filtering[J]. Electronics Optics & Control, 2013, 20(7): 28-31.

[8] Li Ming, Pang Bo, He Yong-feng, et al. Particle filter improved by genetic algorithm and particle swarm optimazation algorithm[J]. Journal of Software, 2013, 8(3): 666-672.

[9] Liu D, Liu G, Yu M. An improved FastSLAM framework based on particle swarm optimization and unscented particle filter[J]. Journal of Computational Information Systems, 2012, 8(7): 2859-2866.

[10] Li J S, Xi Z H, Zhao Y Q. Application of the Beidou/INS integrated navigation artificial fish-swarm particle filter [J]. Electronic Science and Technology, 2013, 26(6): 157-161.

[11] Yang X S. Firefly algorithm, stochastic test functions and design optimisation[J]. International Journal of Bio-Inspired Computation, 2010, 2(2): 78-84.

[12] Shan C F, Tan T N, Wei Y C. Real-time hand tracking using a mean shift embedded particle filter[J]. Pattern Recognition, 2007, 40(7): 1958 -1970.

Particle filter based on firefly algorithm optimization for relative navigation of non-cooperative target

ZHANG Da-li, XIA Hong-wei, MA Guang-cheng, WANG Chang-hong
(School of Astronautics, Harbin Institute of Technology, Harbin 150001, China)

The particle-filter resampling technology in the relative navigation of the non-cooperative target may cause the particle impoverishment phenomenon and decrease the relative navigation precision. To solve this problem, the operation mechanism of the firefly optimization algorithm is analyzed, and then an improved particle filter algorithm based on the firefly intelligent optimization algorithm is proposed. With the purpose of ensuring the diversity of particles and improving the integral quality of particle swarm, the improved algorithm enables the particle swarm to move towards the high likelihood region by optimizing the re-sampling process of particle filter, and reasonably remains some particles in low likelihood region at the same time. The simulation results show that, compared with the standard algorithm, the improved algorithm improves the navigation accuracy by about 39.35%, requires less quantity of the particles to achieve stable precision, and effectively restrain the problem of particle impoverishment.

firefly algorithm; particle filter; particle impoverishment; relative navigation

V448.22+4

A

1005-6734(2017)02-0269-06

10.13695/j.cnki.12-1222/o3.2017.02.025

2017-01-04；

2017-03-28

國(guó)家自然科學(xué)基金（61304108）

張大力（1991—），男，博士研究生，從事導(dǎo)航、濾波與控制研究。E-mail: 15b904016@hit.edu.cn

聯(lián) 系 人：王常虹（1961—），男，教授，博士生導(dǎo)師，從事導(dǎo)航、制導(dǎo)與控制研究。E-mail: cwang@hit.edu.cn