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

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

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

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

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

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

螢火蟲算法；粒子濾波；粒子貧化；相對導航

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

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

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

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

1 相對導航問題狀態(tài)模型與觀測模型

1.1 相對運動狀態(tài)模型

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

圖1 相對運動示意圖Fig.1 Relative motion diagram

由牛頓第二定律得追蹤星和目標星對慣性空間的運動方程為

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

1.2 觀測模型確定

本文采用“光學測角相機+激光測距儀”的觀測模式，如圖2所示。

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

圖2 觀測模型示意圖Fig.2 Observation model diagram

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

式中：

觀測方程可寫成如下形式：

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

概率密度分布為

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

2 粒子濾波分析

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

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

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

3.1 螢火蟲算法

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

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

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

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

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

1）螢火蟲的相對熒光亮度為

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

2）螢火蟲吸引度公式為

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

3）在t時刻，螢火蟲個體i被個體j吸引并向后者移動的位置更新公式為

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

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

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

1）熒光亮度重定義

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

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

2）改進吸引度更新公式

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

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

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

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

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

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

6）歸一化

7）狀態(tài)輸出

4 仿真實驗與分析

為進一步驗證FA-PF算法對解決空間非合作目標相對導航問題的有效性，本文進行了仿真試驗。仿真條件如表1所示。

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

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

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

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

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

圖5 濾波時間對比Fig.5 Comparison on filtering times

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

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

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

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

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

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

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

5 結(jié) 論

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

（References）：

[1] 王楷, 湯亮, 李克行, 等. 航天器相對導航與控制技術(shù)的典型任務(wù)[J]. 空間控制技術(shù)與應用, 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] 李文躍, 徐貴力, 周龍, 等. 空間非合作目標相對導航技術(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] 朱志宇. 粒子濾波算法及其應用[M]. 北京: 科學出版社, 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

國家自然科學基金（61304108）

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

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