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

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

（哈爾濱工業大學 航天學院，哈爾濱 150001）

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

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

（哈爾濱工業大學 航天學院，哈爾濱 150001）

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

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

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

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

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

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

1 相對導航問題狀態模型與觀測模型

1.1 相對運動狀態模型

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

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

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

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

1.2 觀測模型確定

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

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

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

以上參數與相對軌道坐標的關系可以表示為

式中：

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

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

概率密度分布為

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

2 粒子濾波分析

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

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

3 螢火蟲群優化粒子濾波算法

3.1 螢火蟲算法

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

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

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

3）熒光亮度由要優化的目標函數值決定。

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

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

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

2）螢火蟲吸引度公式為

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

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

3.2 優化算法改進

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

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

1）熒光亮度重定義

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

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

2）改進吸引度更新公式

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

3.3 算法具體實現和步驟

2）數據處理。粒子經狀態模型和觀測模型處理后分別得到k時刻的狀態值和預測值。

3）權值更新。根據次優概率密度函數，結合預測值和實際觀測值對權重進行更新。具體為

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

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

6）歸一化

7）狀態輸出

4 仿真實驗與分析

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

表1 航天器軌道根數Tab.1 Orbit elements of spacecraft

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

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

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

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

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

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

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

由于設置仿真步數較多，為避免圖像丟幀導致結果不直觀，取曲線中 56個點來繪制相對位置和相對速度誤差曲線。圖 6和圖 7顯示，在相同條件下，FA-PF算法表現較PF算法穩定。為了更清晰地展示FA-PF算法對粒子貧化現象的抑制作用，給出狀態輸出前的粒子分布圖，如圖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 粒子相對位置狀態分布Fig.8 Relative position state distribution of particles

圖6～8中，實線表示真實狀態，“*”表示PF算法粒子分布，“。”表示FA-PF算法粒子分布。可以看出，標準粒子濾波算法粒子多樣性表現一般，在有些位置分布較為集中，不利于濾波器的狀態估計。而FA-PF算法粒子在整體向高似然區域移動的同時，在低似然區域也合理的保留了部分粒子，使得這部分粒子所包含的狀態信息得以在估計結果中體現，保證了粒子的多樣性。

5 結 論

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

（References）：

[1] 王楷, 湯亮, 李克行, 等. 航天器相對導航與控制技術的典型任務[J]. 空間控制技術與應用, 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] 李文躍, 徐貴力, 周龍, 等. 空間非合作目標相對導航技術研究[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

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