岳 聰，薄煜明，吳盤(pán)龍，田夢(mèng)楚，陳志敏

（南京理工大學(xué) 自動(dòng)化學(xué)院 南京 210094）

基于模糊迭代均方根容積卡爾曼濾波的天基非合作目標(biāo)跟蹤

岳 聰，薄煜明，吳盤(pán)龍，田夢(mèng)楚，陳志敏

（南京理工大學(xué) 自動(dòng)化學(xué)院 南京 210094）

針對(duì)非合作航天器相對(duì)導(dǎo)航中測(cè)量噪聲不確定的問(wèn)題，提出了一種模糊迭代均方根容積卡爾曼濾波算法，實(shí)現(xiàn)對(duì)非合作目標(biāo)相對(duì)狀態(tài)的測(cè)量。該算法利用容積點(diǎn)均方根迭代策略和模糊推理系統(tǒng)實(shí)時(shí)調(diào)整改進(jìn)容積卡爾曼濾波的量測(cè)噪聲協(xié)方差陣權(quán)值，修正量測(cè)噪聲協(xié)方差陣，使其接近真實(shí)噪聲值，從而提高目標(biāo)跟蹤算法的自適應(yīng)能力，提高了濾波精度。通過(guò)建立數(shù)學(xué)仿真模型，分別采用擴(kuò)展卡爾曼濾波、容積卡爾曼濾波以及模糊迭代均方根容積卡爾曼濾波進(jìn)行跟蹤仿真，仿真結(jié)果表明，與標(biāo)準(zhǔn)容積卡爾曼濾波相比，該改進(jìn)算法能夠提高13.17%的跟蹤精度。

非合作目標(biāo)；容積卡爾曼濾波；模糊推理系統(tǒng)；自適應(yīng)濾波；目標(biāo)跟蹤

由于人類(lèi)對(duì)于探索未知空間的需求日益迫切，航天器的數(shù)量劇增，軌道資源愈發(fā)緊缺，促使人們對(duì)故障衛(wèi)星進(jìn)行空間監(jiān)測(cè)[1-2]。天基空間目標(biāo)監(jiān)視不受地球曲率和國(guó)家地域的局限，也不易受到復(fù)雜大氣環(huán)境的影響，使其相對(duì)于地基空間目標(biāo)監(jiān)視系統(tǒng)有較大優(yōu)勢(shì)。由于在軌目標(biāo)受到軌道攝動(dòng)力、誤差模型等復(fù)雜因素的影響，往往無(wú)法準(zhǔn)確地確定量測(cè)噪聲。因此，研究自適應(yīng)濾波算法成為空間目標(biāo)的跟蹤定軌發(fā)展的重要方向[3-5]。

針對(duì)航天器呈現(xiàn)高度非線性的特點(diǎn)，精密軌道的確定實(shí)質(zhì)上就是解決非線性系統(tǒng)濾波的問(wèn)題，即在貝葉斯濾波框架下可以尋求最優(yōu)解。然而實(shí)際應(yīng)用中后驗(yàn)概率密度函數(shù)難以獲得，因此，學(xué)者在此基礎(chǔ)上尋找次優(yōu)方法以逼近后驗(yàn)概率密度函數(shù)的統(tǒng)計(jì)特性。常用的處理方法包括擴(kuò)展卡爾曼濾波方法(EKF)[6]、容積卡爾曼濾波(CKF)[7-8]、無(wú)跡卡爾曼濾波(UKF)[9]等。

針對(duì)非合作目標(biāo)的自主導(dǎo)航問(wèn)題，本文提出了基于模糊迭代均方根容積卡爾曼濾波算法(FISCKF)來(lái)實(shí)現(xiàn)非合作航天器的相對(duì)狀態(tài)測(cè)量，利用模糊控制[9]原理調(diào)整參數(shù)，從而提高精度。該方法直接對(duì)容積點(diǎn)均方根進(jìn)行迭代更新，避免通過(guò)求取高斯近似方法來(lái)產(chǎn)生容積點(diǎn)，同時(shí)引入了模糊自適應(yīng)算法[10]，實(shí)時(shí)調(diào)整濾波過(guò)程中的協(xié)方差矩陣，能夠充分利用量測(cè)信息，從而提高對(duì)狀態(tài)的估計(jì)精度。仿真結(jié)果表明，模糊迭代均方根容積卡爾曼濾波算法估計(jì)精度要高于容積卡爾曼濾波%。

1 航天器相對(duì)運(yùn)動(dòng)學(xué)方程

1.1 目標(biāo)運(yùn)動(dòng)的狀態(tài)方程

根據(jù)理論力學(xué)可知，若將地球視為一個(gè)密度均勻的正球體，則它對(duì)空間目標(biāo)的吸引可以等效于一個(gè)質(zhì)點(diǎn)，則地球與目標(biāo)構(gòu)成一個(gè)二體系統(tǒng)，觀測(cè)衛(wèi)星和目標(biāo)衛(wèi)星的幾何關(guān)系如圖1所示。

圖1 觀測(cè)衛(wèi)星和目標(biāo)衛(wèi)星的幾何關(guān)系Fig.1 Geometric relationship between observation satellite and target satellite

根據(jù)牛頓第二定律，目標(biāo)運(yùn)動(dòng)方程為

其中，μ=GM是地球引力常數(shù)，根據(jù)第16屆國(guó)際大地測(cè)量協(xié)會(huì)和國(guó)際天文聯(lián)合會(huì)推薦，μ=3.986005×1014m3/s2。

其中，J2為地球引力二階帶諧項(xiàng)系數(shù)，狀態(tài)矢量取為，Re為地球半徑，r=(x, y, z)T。

1.2 目標(biāo)運(yùn)動(dòng)的觀測(cè)方程

在t時(shí)刻，觀測(cè)量為觀測(cè)衛(wèi)星與被觀測(cè)衛(wèi)星之間的方位角θ和俯仰角。本文采用站心赤道坐標(biāo)系，得到量測(cè)方程如下：

2 模糊迭代均方根容積卡爾曼濾波

傳統(tǒng)容積卡爾曼濾波算法由高斯域貝葉斯濾波理論推導(dǎo)而來(lái)，在該理論的框架下，非線性濾波問(wèn)題歸結(jié)為非線性函數(shù)和高斯概率密度函數(shù)乘積的數(shù)學(xué)積分問(wèn)題，當(dāng)其推廣到高階容積卡爾曼濾波時(shí)，計(jì)算過(guò)程十分復(fù)雜，擴(kuò)展性比較差[12]。為了獲得更好的濾波精度及穩(wěn)定性，本文將容積卡爾曼濾波濾波改進(jìn)為模糊迭代均方根容積卡爾曼濾波[13-14,16]，利用協(xié)方差矩陣的平方根來(lái)代替協(xié)方差矩陣直接參與遞推運(yùn)算，引入模糊推理算法，提高濾波算法的計(jì)算效率和數(shù)值穩(wěn)定性[15]。

非線性濾波問(wèn)題狀態(tài)向量空間的加性噪聲方程和量測(cè)方程如下：

由于容積卡爾曼濾波需要已知系統(tǒng)噪聲和量測(cè)噪聲的方差陣Qk-1、Rk，在實(shí)際應(yīng)用中，量測(cè)噪聲Rk易受外界環(huán)境的干擾，故難以準(zhǔn)確獲得，此時(shí)容積卡爾曼濾波的性能會(huì)大大降低，甚至出現(xiàn)發(fā)散，因而須采用一種自適應(yīng)算法來(lái)調(diào)節(jié)Rk。

在容積卡爾曼濾波中，定義系統(tǒng)新息為

其中，M是累加窗口，本文中M=4。

系統(tǒng)理論新息方差陣為Pzz，定義兩者跡的比值為

當(dāng)系統(tǒng)噪聲統(tǒng)計(jì)特性準(zhǔn)確時(shí)，新息應(yīng)為零均值高斯白噪聲序列，新息方差實(shí)際值與理論值的比值αk應(yīng)在1附近。當(dāng)系統(tǒng)測(cè)量噪聲的實(shí)際值與給定值不一致時(shí)，αk將偏離1，為了使濾波穩(wěn)定，可通過(guò)調(diào)節(jié)Rk對(duì)理論殘差方差陣進(jìn)行調(diào)整：

其中：εk為調(diào)節(jié)因子。當(dāng)系統(tǒng)測(cè)量噪聲實(shí)際值大于給定值時(shí)，αk＞1，此時(shí)可增大εk，使αk回到1附近；當(dāng)系統(tǒng)測(cè)量噪聲實(shí)際值小于給定值時(shí)，αk＜1，此時(shí)可減小εk。

以αk作為模糊推理系統(tǒng)的輸入，εk為其輸出，以1 為參考，設(shè)S表示模糊集合“小”，M表示基本等于1，L表示模糊集合“大”，可建立如下的模糊推理規(guī)則：

1）如果αk∈S，那么εk∈S；

2）如果αk∈M，那么εk∈M；

3）如果αk∈L，那么εk∈L。

通過(guò)該方法得到的模糊推理容積卡爾曼濾波器[7]能夠克服小范圍內(nèi)不確定測(cè)量噪聲的干擾。

FISCKF的核心思想是在測(cè)量更新過(guò)程中，利用協(xié)方差矩陣的平方根來(lái)代替協(xié)方差矩陣來(lái)改進(jìn)線性化參考點(diǎn)，即利用測(cè)量更新得到的狀態(tài)估值來(lái)代替狀態(tài)預(yù)測(cè)值，再通過(guò)量測(cè)更新中進(jìn)行迭代，計(jì)算量測(cè)預(yù)測(cè)值、新息方差和協(xié)方差，通過(guò)模糊推理系統(tǒng)得到的調(diào)節(jié)因子ε來(lái)調(diào)節(jié)kRk，計(jì)算增益，產(chǎn)生新的，再進(jìn)行迭代更新，當(dāng)j=N時(shí)，退出迭代。

其算法流程如下：

2）量測(cè)更新

① 計(jì)算容積點(diǎn)：

② 容積點(diǎn)傳播：

③ 計(jì)算量測(cè)預(yù)測(cè)值、新息方差和協(xié)方差矩陣：

⑤ 以αk為FIS輸入，通過(guò)模糊推理系統(tǒng)計(jì)算εk。

⑥ 計(jì)算量測(cè)更新：

3 仿真實(shí)驗(yàn)與結(jié)果分析

假設(shè)系統(tǒng)噪聲W(k)均值為0，方差為Q=diag[202m,202m, 202m, 0.052m/s, 0.052m/s, 0.052m/s]，量測(cè)噪聲R=diag[0.000052rad, 0.000052rad]，其中初始狀態(tài)誤差為δX=diag[1000, 1000, 1000, 5, 5, 5]，μe=3.986005×1014m3/s2，J2=1.0826269×10–3，Re=6.378137×106。

初始軌道要素如表1所示。圖2顯示了模糊迭代均方根容積卡爾曼濾波算法的軌跡。圖3、圖4分別為EKF、CKF、FISCKF的位置誤差和速度誤差。圖5、圖6分別為1000倍R時(shí)EKF、CKF、FISCKF的位置誤差和速度誤差。表2顯示3種算法的均方根誤差（RMSE），表3顯示了當(dāng)濾波器穩(wěn)定時(shí)的速度和位置誤差。

表1 航天器初始軌道要素Tab.1 Key elements of spacecraft’s initial orbit

圖2 FISCKF算法軌跡圖Fig.2 Curve of FISCKF algorithm

通過(guò)圖3、圖4可以看出，EKF初始濾波誤差較大，魯棒性差，收斂速度上明顯低于CKF、FISCKF，其原因在于EKF算法對(duì)非線性函數(shù)采用一階線性近似化的方法，其近似精度不高，從而導(dǎo)致估計(jì)精度不高。CKF、FISCKF由于采用三階球面-相徑容積規(guī)則近似狀態(tài)后驗(yàn)均值和協(xié)方差，其精度高于一階線性近似化的EKF算法，表示其能夠逐漸靠近真實(shí)值。從圖5、圖6中可知，當(dāng)噪聲增大時(shí)，EKF算法誤差急劇增大，即使長(zhǎng)時(shí)間響應(yīng)后依舊不能穩(wěn)定，說(shuō)明其算法不能穩(wěn)定地跟蹤目標(biāo)。CKF算法的響應(yīng)時(shí)間變長(zhǎng)，差變大，表示其不能有效地快速跟蹤目標(biāo)。FISCKF的響應(yīng)時(shí)間最短，誤差最小，表明FISCKF實(shí)時(shí)性最好，能夠提高濾波精度。

圖3 EKF、CKF、FISCKF算法位置誤差Fig.3 Position errors of EKF, CKF, and FISCKF

圖4 EKF、CKF、FISCKF算法速度誤差Fig.4 Speed errors of EKF, CKF, and FISCKF

圖5 1000R時(shí)EKF、CKF、FISCKF算法位置誤差Fig.5 Position errors of EKF, CKF, and FISCKF at 1000R

圖6 1000R時(shí)EKF、CKF、FISCKF算法速度誤差Fig.6 Speed errors of EKF, CKF, and FISCKF at 1000R

表2 EKF,CKF,FISCKF的RMSETab.2 RMSE of EKF, CKF, and FISCKF

表3 EKF,CKF,FISCKF的穩(wěn)定誤差Tab.3 Stable errors of EKF, CKF, and FISCKF

由表3可以看出，F(xiàn)ISCKF的穩(wěn)態(tài)誤差最小，位置RMSE較EKF提高了42.12%，較CKF提高了13.17%，速度RMSE較EKF提高了37.04%，較CKF提高了18.49%，表明能夠提高精度。

綜上所述，F(xiàn)ISCKF算法的位置誤差低于EKF、CKF算法，F(xiàn)ISCKF算法的速度誤差明顯低于EKF、CKF算法，表示其能夠更加迅速有效地靠近真實(shí)值?？梢?jiàn)FISCKF算法能更有效地進(jìn)行跟蹤。

4 結(jié) 論

本文針對(duì)非合作航天器跟蹤中出現(xiàn)的噪聲統(tǒng)計(jì)特性不確定問(wèn)題，提出了模糊迭代均方根容積卡爾曼濾波算法，仿真證明該算法能夠有效改善跟蹤精度和時(shí)間，滿足非合作目標(biāo)相對(duì)導(dǎo)航的要求。FISCKF算法在解決非線性模型濾波問(wèn)題時(shí)，相對(duì)于EKF、CKF算法，計(jì)算量小，具有較好的跟蹤精度，而且在非線性嚴(yán)重或者高階誤差引入時(shí)，會(huì)推遲或延緩濾波發(fā)散，能夠提高精度。

（References）：

[1] 黃劍, 胡衛(wèi)東. 基于貝葉斯框架的空間群目標(biāo)跟蹤技[J]. 雷達(dá)學(xué)報(bào), 2013, 2(1): 86-96.Huang Jian, Hu Wei-dong. Tracking of group space objects within bayesian framework[J]. Journal of Radars,2013, 2(1): 86-96.

[2] Granja C, Polansky S. The SATRAM Timepix spacecraft payload in open space on board the Proba-V satellite for wide range radiation monitoring in LEO orbit[J]. Planetary and Space Science, 2016, 125: 114-129.

[3] Wu Pan-long, Cai Ya-dong, Bo Yu-ming. Passive tracking from the combined set of bearings and frequency measurements by single satellite[J]. Journal Control Theory and Applications, 2012, 10(4): 483-489.

[4] Wu Pan-long, Cai Ya-dong, Bo Yu-ming. Passive tracking from the combined set of bearings and frequency measurements by single satellite[J]. Journal Control Theory and Applications, 2012, 10(4): 483-489.

[5] Li Fan, Chao Jiang, Min Hui. Ground track maintenance for BeiDou IGSO satellites subject to tesseral resonances and the luni-solar perturbations[J]. Advances in Space Research, 2016, 59(3): 753-761.

[6] Perumal D G, Subathra B, Saravanakumar G, et al. Extended Kalman filter based path-planning algorithm for autonomous vehicles with I2V communication[J]. IFAC Papers In Line, 2016, 49(1): 652-657.

[7] 張迎春, 李璟璟. 模糊自適應(yīng)無(wú)跡卡爾曼濾波方法用于天文導(dǎo)航[J]. 哈爾濱工業(yè)大學(xué)學(xué)報(bào), 2012, 44(1): 12-16.Zhang Y C, Li J J. Fuzzy adaptive unscented Kalman filtering method for astronomical navigation[J]. Journal of Harbin Institute of Technology, 2012, 44(1): 12-16.

[8] 蔡宗平, 牛創(chuàng). 基于模糊自適應(yīng)CKF的目標(biāo)跟蹤算[J].光電與控制, 2016, 23(10): 8-12.Cai Z P, Niu C. Target tracking algorithm based on fuzzy adaptive cubature Kalman filter[J]. Electronics Optics &Control, 2016, 23(10): 8-12.

[9] 張?chǎng)未? 均方根嵌入式容積卡爾曼濾波[J]. 控制理論與應(yīng)用, 2013, 30(9): 1116-1121.Zhang C X. Square-root imbedded cubature Kalman filter[J]. Control Theory & Applications, 2013, 30(9): 1116-1121.

[10] Zhou Qi, Wu Cheng-wei, Jing Xing-jian, et al. Adaptive fuzzy backstepping dynamic surface control for nonlinear Input-delay systems[J]. Neurocomputing, 2016, 199: 58-65.

[11] 徐曉蘇. 基于改進(jìn)型CKF的SINS初始對(duì)準(zhǔn)方法[J]. 華中科技大學(xué)學(xué)報(bào), 2016,44(1): 81-86.Xu X S. Alignment method of initial SINS based on improved CKF[J]. Huazhong Univ. of Sci. & Tech., 2016,44(1): 81-86.

[12] Arasaratnam I, Haykin S. Cubature Kalman filter[J]. IEEE Transactions on Automatic Control, 2009, 54(6): 1254-1269.

[13] Naderia E, Narimanib H, Fathib M, et al. A novel fuzzy adaptive configuration of particle swarm optimization to solve large-scale optimal reactive power dispatch[J]. Applied Soft Computing, 2017, 53: 441- 456.

[14] Duan Jia-min, Shi Hui, Liu Dan, et al.Square root cubature Kalman filter-Kalman filter algorithm for intelligent vehicle position estimate[J]. Procedia Engineering, 2016, 137:267-276.

[15] Xu Xiao-su, Zou Hai-jun. Square root cubature Kalman filter-Kalman filter algorithm for intelligent vehicle position estimate[J]. Journal of Donghua University, 2015,32(5): 731-736.

[16] Tseng Chien-Hao, Lin Sheng-Fuu.Fuzzy adaptive cubature Kalman filter for integrated navigation systems[J].Sensors, 2016, 16(8): 1167.

[17] Wang Bao-bao, Wu Pan-long. Underwater bearing-only tracking based on square-root unscented Kalman filter smoothing algorithm[J]. Journal of Chinese Inertial Technology, 2016, 24(2): 180-184.

Non-cooperative space target tracking based on fuzzy iterative square-root cubature Kalman filter

YUE Cong, BO Yu-ming, WU Pan-long, TIAN Meng-chu, CHEN Zhi-min
(School of Automation, Nanjing University of Science and Technology, Nanjing 210094, China)

In view of the problem that the statistic characteristics of the measurement noise is uncertain in the relative navigation of non-cooperative spacecraft, a fuzzy iterative RMS (root mean square) cubature Kalman filtering algorithm is proposed to realize the relative state measurement of the non-cooperative target. The measurement noise covariance of cubature Kalman filtering is adjusted in real-time by using the fuzzy inference system to make it closer to the real measurement covariance. And a cubature point RMS iteration strategy is utilized to overcome the limitations of the traditional sampling based on Gaussian approximation and improve the filtering precision. Simulation tracking is conducted with different filtering algorithms, and the results show that the improved algorithm can improve the tracking accuracy by 13.17% compared with the standard cubature Kalman filter.

non-cooperative target; cubature Kalman; fuzzy inference system; adaptive filtering; target tracking

TP391.4

：A

1005-6734(2017)03-0395-04

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

2017-04-15；

：2017-06-10

國(guó)家自然科學(xué)基金（61473153)；江蘇省“六大人才高峰”項(xiàng)目(2015-XXRJ-006)；航空科學(xué)基金（2016ZC59006）

岳聰（1989—），女，博士研究生，從事目標(biāo)跟蹤技術(shù)研究。E-mail: cycongyue@hotmail.com

聯(lián) 系 人：薄煜明（1965—），男，教授，博士生導(dǎo)師。E-mail: byming@njust.edu.cn