李 敏，王松艷，張迎春，2，李化義

（1.哈爾濱工業(yè)大學(xué)航天學(xué)院，黑龍江哈爾濱150001；2.深圳航天東方紅海特衛(wèi)星有限公司，廣東深圳518057）

改進(jìn)的強(qiáng)跟蹤平方根UKF在衛(wèi)星導(dǎo)航中應(yīng)用

李 敏1，王松艷1，張迎春1，2，李化義1

（1.哈爾濱工業(yè)大學(xué)航天學(xué)院，黑龍江哈爾濱150001；2.深圳航天東方紅海特衛(wèi)星有限公司，廣東深圳518057）

針對應(yīng)用于受不確定性干擾和噪聲影響的衛(wèi)星自主導(dǎo)航系統(tǒng)中的無跡卡爾曼濾波（unscented Kalman filter，UKF）存在估計(jì)精度低、跟蹤性能差和魯棒性弱等缺陷，提出一種改進(jìn)的強(qiáng)跟蹤平方根UKF（strong tracking square-root UKF，STSRUKF）導(dǎo)航方法。該方法中利用星敏感器和光學(xué)導(dǎo)航相機(jī)設(shè)計(jì)出導(dǎo)航方案，并通過轉(zhuǎn)換方程將間接量測量轉(zhuǎn)換為觀測量。針對平方根UKF（square-root UKF，SRUKF）在高階系統(tǒng)中因?yàn)閟igma點(diǎn)的零權(quán)值系數(shù)是負(fù)的或者數(shù)值計(jì)算誤差太大時(shí)而可能造成濾波器發(fā)散問題，采用一種改良的平方根分解方法，改善了濾波器的穩(wěn)定性。同時(shí)，基于強(qiáng)跟蹤濾波器理論（strong tracking filters，STF），引入多重自適應(yīng)衰減因子調(diào)節(jié)協(xié)方差矩陣，使得濾波器具有強(qiáng)跟蹤能力和克服系統(tǒng)模型不確定的魯棒性，改善了濾波器的估計(jì)精度。將該方法應(yīng)用于衛(wèi)星自主導(dǎo)航系統(tǒng)中，實(shí)驗(yàn)仿真結(jié)果表明，相對于平方根UKF和STF，該方法不僅保證了系統(tǒng)的可靠性，還提高系統(tǒng)的導(dǎo)航精度和改善系統(tǒng)的魯棒性及跟蹤能力。

衛(wèi)星導(dǎo)航系統(tǒng)；不確定性干擾；改進(jìn)的強(qiáng)跟蹤平方根無跡卡爾曼濾波；魯棒性

0 引 言

基于光學(xué)測量的衛(wèi)星自主導(dǎo)航是指衛(wèi)星利用安裝在自身身上的星敏感器及光學(xué)導(dǎo)航相機(jī)來測量星光信息及天體的邊緣信息，并在軌處理觀測到的光學(xué)圖像信息，獲取自身的位置和速度信息的過程。由于其具有自主性強(qiáng)、精度高、實(shí)時(shí)性好等優(yōu)點(diǎn)，是當(dāng)今衛(wèi)星控制技術(shù)發(fā)展的趨勢，它在減輕地面測控負(fù)擔(dān)、降低衛(wèi)星運(yùn)行費(fèi)用、提高衛(wèi)星的生存能力和擴(kuò)展衛(wèi)星的應(yīng)用潛力等方面具有重要意義。由于衛(wèi)星導(dǎo)航系統(tǒng)其是非線性的，因此需要用非線性濾波技術(shù)來得到其狀態(tài)變量的最優(yōu)估計(jì)［1］。展卡爾曼濾波器（extended Kalman filter，EKF）因?yàn)槠浞椒ê唵稳菀讓?shí)現(xiàn)等優(yōu)點(diǎn)，被廣泛應(yīng)用于衛(wèi)星自主導(dǎo)航系統(tǒng)中。但是EKF實(shí)質(zhì)仍然是一種在線線性化的算法，存在自身的理論缺陷性：①EKF在線性化處理時(shí)需要計(jì)算雅克比（Jacobian）矩陣，其計(jì)算過程繁瑣復(fù)雜且容易出錯(cuò)；②在系統(tǒng)非線性較強(qiáng)的時(shí)候，線性化誤差容易增大，估計(jì)精度會下降，甚至發(fā)散；③當(dāng)初始狀態(tài)誤差相對較大時(shí)，EKF濾波很不穩(wěn)定、收斂速度較慢、估計(jì)精度較低［2－5］。

為了解決這個(gè)問題，文獻(xiàn)［6－8］提出了無跡卡爾曼濾波（unscented Kalman filter，UKF）方法。相對于EKF，UKF采用unscented變換對非線性概率密度分布進(jìn)行近似，具有不需要計(jì)算Jacobian矩陣、估計(jì)精度更高等優(yōu)點(diǎn)，近年來在導(dǎo)航系統(tǒng)濾波問題當(dāng)中獲得廣泛應(yīng)用。然而無論是EKF，還是UKF，它們的高精度都是建立在精確的系統(tǒng)模型和精確已知的系統(tǒng)先驗(yàn)噪聲分布基礎(chǔ)之上，當(dāng)系統(tǒng)模型和系統(tǒng)噪聲統(tǒng)計(jì)特性不準(zhǔn)確的情況下，UKF的估計(jì)性能會下降，甚至出現(xiàn)濾波發(fā)散，也就是說UKF不具備能夠克服系統(tǒng)模型不確定性的魯棒性。針對模型不確定性估計(jì)問題，文獻(xiàn)［9－10］提出了強(qiáng)跟蹤濾波器（strong tracking filters，STF）的方法。強(qiáng)跟蹤濾波器通過在狀態(tài)預(yù)測協(xié)方差陣中引入漸消因子，自適應(yīng)調(diào)整增益矩陣，保證輸出殘差序列之間的相互正交，從而使得在系統(tǒng)模型不確定時(shí)，仍能保持對系統(tǒng)狀態(tài)的跟蹤能力，即STF具有關(guān)于模型不確定的魯棒性。但是STF在EKF的基礎(chǔ)上的改進(jìn)，與EKF存在類似，仍然無法克服EKF自身存在的理論局限性。為此，文獻(xiàn)［11－12］結(jié)合STF理論和UKF理論，提出了一種基于強(qiáng)跟蹤的UKF，并應(yīng)用于導(dǎo)航系統(tǒng)中，獲得了比STF和UKF更好的估計(jì)精度，具有比STF和UKF更好的魯棒性和跟蹤能力，改善系統(tǒng)的可靠性。

雖然強(qiáng)跟蹤UKF具有克服系統(tǒng)不確定的魯棒性，但強(qiáng)跟蹤UKF在數(shù)值計(jì)算的過程中存在舍入誤差，隨著迭代計(jì)算的累加，積累的舍入誤差可能會破壞系統(tǒng)估計(jì)誤差協(xié)方差矩陣的非負(fù)定性和對稱性，導(dǎo)致算法的收斂速度慢，甚至造成算法的不穩(wěn)定。平方根UKF算法采用誤差協(xié)方差矩陣的平方根矩陣進(jìn)行迭代運(yùn)算，解決了常規(guī)UKF算法中由于累積舍入誤差引起誤差協(xié)方差矩陣負(fù)定性而導(dǎo)致的濾波結(jié)果發(fā)散的問題，提高了濾波的精度和穩(wěn)定性［13－14］。但是，標(biāo)準(zhǔn)平方根UKF是基于QR分解和Cholesky分解來進(jìn)行的，當(dāng)sigma點(diǎn)的零權(quán)值系數(shù)是負(fù)的或者數(shù)值計(jì)算誤差太大時(shí)有可能造成cholesky分解失敗，造成濾波器發(fā)散。

針對以上問題描述，為了使得濾波器即具有能夠克服系統(tǒng)不確定的魯棒性、較好的跟蹤能力和估計(jì)精度，又具有較好的數(shù)值計(jì)算穩(wěn)定性以及較快的收斂速度，本文以受不確定性因素影響的星敏感器／光學(xué)導(dǎo)航相機(jī)衛(wèi)星自主導(dǎo)航系統(tǒng)為背景，提出了一種改進(jìn)的強(qiáng)跟蹤平方根UKF的衛(wèi)星自主導(dǎo)航方法。該方法中，根據(jù)強(qiáng)跟蹤濾波器原理，引入時(shí)變漸消自適應(yīng)衰減因子，自適應(yīng)調(diào)整濾波增益矩陣和預(yù)測誤差協(xié)方差平方根矩陣，保證殘差序列正交，改善濾波器的跟蹤能力和魯棒性。同時(shí)，在現(xiàn)有的基礎(chǔ)上，改進(jìn)了平方根UKF的分解操作，有效地避免了因?yàn)閟igma點(diǎn)的零權(quán)值系數(shù)是負(fù)的或者數(shù)值計(jì)算累積誤差太大時(shí)有可能造成濾波器發(fā)散問題，保證了濾波器的穩(wěn)定性。另外，相對于標(biāo)準(zhǔn)的平方根UKF，改進(jìn)的平方根分解縮短了計(jì)算量，實(shí)時(shí)性更好。在衛(wèi)星自主導(dǎo)航系統(tǒng)中的仿真結(jié)果表明，該方法是有效的。

1 改進(jìn)的平方根強(qiáng)跟蹤UKF

1.1 平方根UKF

針對一類非線性系統(tǒng)：

式中，X為狀態(tài)變量；Z為觀測量；f（uk，Xk，k）為系統(tǒng)的非線性狀態(tài)方程，具有一階連續(xù)偏導(dǎo)數(shù)；H為線性測量方程；過程噪聲wk和測量噪聲Vk均為均值為零的高斯白噪聲；其協(xié)方差矩陣分別為Qk和Rk。假設(shè)系統(tǒng)的初始狀態(tài)為X0，X0與wk及Vk相互獨(dú)立，基于標(biāo)準(zhǔn)平方根UKF的狀態(tài)估計(jì)方法［15］如下：

（1）初始化狀態(tài)＾X0，狀態(tài)誤差協(xié)方差矩陣平方根S0：

（2）對于k＝1，2，…，n實(shí)現(xiàn)步驟如下：

①計(jì)算sigma點(diǎn)

②時(shí)間更新

式中，qr和cholupdate分別表示QR分解和Cholesky一階更新，為標(biāo)準(zhǔn)的matlab指令。

③測量更新

上述過程中用到的參數(shù)計(jì)算如下：

1.2 改進(jìn)的平方根UKF

平方根UKF（square-root UKF，SRUKF）相對于UKF具有更好的魯棒性、穩(wěn)定性及估計(jì)精度［16］。在SRUKF的分解過程中，矩陣｛S±OUUT｝的Cholesky分解是用cholupdate｛S，U，±O｝表示，它要求矩陣必須是半正定的。但是在SRUKF中，按式（7）和式（13）更新協(xié)方差矩陣，實(shí)際如下：

式中，sgn為符號函數(shù)，sgn（X）＝X／abs（X），X為變量，abs為Matlab當(dāng)中的絕對值函數(shù)。

在高階系統(tǒng)中，當(dāng)系統(tǒng)噪聲為高斯白噪聲的時(shí)候，為了獲得較好的估計(jì)精度，一般取n＋λ＝3，因此λ是遠(yuǎn)小于零的。根據(jù)式（21）和式（22）可知Wc0是負(fù)的，且｜Wc0｜?Wci，這使得按式（8）和式（13）更新的矩陣很可能是負(fù)定的［6］。

另外，在SRUKF中，按照式（17）更新狀態(tài)誤差協(xié)方差矩陣，實(shí)際為

它要求等式右邊必須是正定的，但是在數(shù)值計(jì)算過程中，等式右邊很容易因?yàn)樯崛胝`差的累積而破壞協(xié)方差矩陣的正定性。

針對上述問題，為了改善SRUKF的穩(wěn)定性，同時(shí)又能夠保持SRUKF的估計(jì)精度和魯棒性，對SRUKF作如下改進(jìn)。

（1）采用改進(jìn)的狀態(tài)誤差一步預(yù)測協(xié)方差矩陣計(jì)算方法，文獻(xiàn)［6］給出了一種改進(jìn)的狀態(tài)誤差一步預(yù)測協(xié)方差矩陣的計(jì)算方法，如下：

根據(jù)式（20）可知，所有的Wmi都是正的，因此按式（24）計(jì)算的協(xié)方差矩陣Pk＋1｜k至少是半正定的，針對式（24）可直接采用QR分解即可得到狀態(tài)誤差一步預(yù)測協(xié)方差矩陣的平方根Sk＋1｜k，如式（25）所述，從而避免因?yàn)閃c0而造成矩陣負(fù)定的。

（2）針對線性測量方程，其測量估計(jì)值、輸出誤差協(xié)方差陣及互誤差協(xié)方差矩陣可以直接利用Kalman遞推公式來計(jì)算，而不必采用UT變換和加權(quán)擬合來進(jìn)行計(jì)算，減少計(jì)算量，提高計(jì)算效率，即

利用式（28）和式（29）來計(jì)算SZ，k＋1，需要進(jìn)行兩次矩陣乘法和一次Cholesky分解，根據(jù)文獻(xiàn)［17］對于矩陣A進(jìn)行QR分解的上三角部分等于矩陣AAT的Cholesky因子，因此可以采用計(jì)算量更小的式（30）和式（31）替換式（28）和式（29）。

式（31）實(shí)際描述為

很容易證明上述等式的右邊是正定的。

（3）根據(jù)式（14）、式（23）、式（27）和式（28），Pk＋1可被重

寫為

文獻(xiàn)［18］給出了式（33）的平方根分解方法，具體如下：

采用式（34）更新狀態(tài)誤差協(xié)方差矩陣，避免了數(shù)值計(jì)算過程中因誤差累積而破壞Pk＋1正定性的問題。

按照上述描述，利用式（25）代替式（7）和式（8），利用式（30）、式（31）和式（12）、式（13），利用式（34）代替式（16）和式（17）即得到改進(jìn)的SRUKF算法，相對于SRUKF，該方法在保持估計(jì)精度的同時(shí)，不僅改善了濾波器的穩(wěn)定性和魯棒性，還可以改善濾波器的計(jì)算效率，提高實(shí)時(shí)性。

1.3 改進(jìn)的強(qiáng)跟蹤平方根UKF

在系統(tǒng)模型精確的情況下，相對于UKF和SRUKF，采用改進(jìn)SRUKF可以提高系統(tǒng)的可靠性和實(shí)時(shí)性，同時(shí)也具有很好的估計(jì)精度。但是，針對具有不確定性因素干擾和噪聲統(tǒng)計(jì)特性的系統(tǒng)仍存在魯棒性差、估計(jì)精度低及對突變狀態(tài)的跟蹤能力差和收斂速度慢等缺陷，而強(qiáng)跟蹤濾波器通過引入自適應(yīng)調(diào)節(jié)因子，可以在線調(diào)整濾波增益，具有極強(qiáng)的模型失配魯棒性，獨(dú)特的強(qiáng)跟蹤能力，可以有效地改善系統(tǒng)對突變狀態(tài)的跟蹤性能［9－10］。因此，結(jié)合強(qiáng)跟蹤濾波器理論（strong tracking filters，STF），在改進(jìn)的SRUKF基礎(chǔ)上，提出改進(jìn)的強(qiáng)跟蹤SRUKF（strong tracking square-root UKF，STSRUKF）。

濾波器成為強(qiáng)跟蹤濾波器的充分條件是通過在狀態(tài)誤差一步預(yù)測協(xié)方差矩陣Pk＋1｜k引入自適應(yīng)調(diào)節(jié)因子的方法，在線實(shí)時(shí)調(diào)整增益矩陣Κk＋1，使得

①E［（Xk－＾Xk）（Xk－＾Xk）］＝min

②E［εk＋jεTk］＝0，k＝0，1，2，…；j＝1，2，…

其中，εk為殘差序列，εk＝Zk－＾Zk。條件①是實(shí)現(xiàn)濾波器的估計(jì)性能指標(biāo)最優(yōu)，條件②是要求任意時(shí)刻輸出信息殘差序列保持正交。當(dāng)模型與實(shí)際模型相匹配的時(shí)候，改進(jìn)的SRUKF的輸出殘差序列是高斯白噪聲序列，因此條件①和條件②是滿足的。當(dāng)模型存在不確定性的時(shí)候，改進(jìn)的SRUKF的狀態(tài)估計(jì)值偏離系統(tǒng)的實(shí)際狀態(tài)，而這些信息必然會被輸出殘差序列的均值和幅值表現(xiàn)出來。此時(shí)通過引入自適應(yīng)漸消因子dk＋1在線調(diào)整增益狀態(tài)Kk＋1，強(qiáng)迫條件②成立，使得殘差序列仍然保持相互正交，則就可使得濾波器保持對實(shí)際系統(tǒng)狀態(tài)的跟蹤，即改進(jìn)的強(qiáng)跟蹤SRUKF。

采用自適應(yīng)漸消因子dk＋1調(diào)節(jié)狀態(tài)誤差一步預(yù)測協(xié)方差矩陣Pk＋1｜k，其具體形式［19］如下：

自適應(yīng)漸消因子dk＋1的計(jì)算方法如下：

式中，l是一個(gè)調(diào)節(jié)因子；ρ是遺忘因子；dk＋1＝diag（dk＋1，1，dk＋1，2，…，dk＋1，n）是一個(gè)多重因子；是狀態(tài)方程的Jacobian矩陣。

從式（39）可以看出，F(xiàn)k＋1的求取將增加計(jì)算復(fù)雜度，為此，采用基于UKF與KF之間的等價(jià)描述來進(jìn)行計(jì)算，設(shè)未引入自適應(yīng)漸消因子前的狀態(tài)誤差協(xié)方差矩陣為因而有

將式（42）代入式（39），有

利用式（36）、式（37）、式（38）、式（42）、式（43）、式（40）和式（41）計(jì)算得到自適應(yīng)漸消因子后，按式（35）計(jì)算狀態(tài)誤差協(xié)方差矩陣Pk＋1｜k，對其進(jìn)行平方根分解得到Sk＋1｜k具體計(jì)算方式如下：

根據(jù)上述描述及改進(jìn)的平方根UKF，由此得到基于改進(jìn)的強(qiáng)跟蹤平方根UKF狀態(tài)估計(jì)方法如下：

（2）對于k＝1，2，3，…，n實(shí)現(xiàn)步驟如下：

步驟1 按照式（4）計(jì)算sigma點(diǎn)；

步驟3 按式（36）、式（37）、式（38）、式（40）、式（41）、式（42）和式（43）計(jì)算自適應(yīng)衰減調(diào)節(jié)因子dk＋1；

步驟4 按式（44）計(jì)算狀態(tài)預(yù)測協(xié)方差矩陣的平方根Sk＋1｜k，利用代替Pk＋1｜k，按式（27）計(jì)算Pxz，k＋1，按式（30）和式（31）計(jì)算Gk＋1及輸出協(xié)方差矩陣的平方根Sz，k＋1，按式（14）和式（15）計(jì)算狀態(tài)估計(jì)，按式（34）計(jì)算狀態(tài)協(xié)方差矩陣的平方根Sk＋1。

2 衛(wèi)星自主導(dǎo)航系統(tǒng)模型

2.1 系統(tǒng)狀態(tài)方程

在J2000.0地心慣性赤道坐標(biāo)系下，衛(wèi)星自主導(dǎo)航系統(tǒng)模型（即軌道動(dòng)力學(xué)模型）［20－21］如下：為航天器位置參數(shù)矢量的模；J2為地球引力系數(shù)；Re是地球的平均赤道半徑；ΔFx、ΔFy和ΔFz為地球非球形高階攝動(dòng)項(xiàng)和日月攝動(dòng)、太陽光壓攝動(dòng)以及大氣壓攝動(dòng)等其他攝動(dòng)力的影響；w為高斯白噪聲。

2.2 間接量測量方程

衛(wèi)星導(dǎo)航原理如圖1所示，圖中A1，A2為地心與衛(wèi)星及兩恒星平星光方向的夾角，A3是衛(wèi)星相對于地球的張角。根據(jù)文獻(xiàn)［22－23］，這3個(gè)角度可由星敏感器和星光相機(jī)測得，它們分別為

式（45）可簡寫為

式中，X＝［x y z vxvyvz］T為衛(wèi)星在地心慣性坐標(biāo)系下3個(gè)方向的位置和速度；μ為引力常數(shù)；r＝

式中，θ為已知的兩恒星平行光的夾角。通過測量到的A1，A2則可以確定以地球?yàn)轫旤c(diǎn)的兩個(gè)圓錐面，再利用已知的兩恒星平行光的單位矢量i1，i2則可確定衛(wèi)星相對于地心的矢量r的方向，最后結(jié)合測量到的衛(wèi)星相對于地球的張角A3及地球半徑Re即可確定位置矢量r，其數(shù)學(xué)描述如下：

圖1 衛(wèi)星導(dǎo)航原理圖

根據(jù)以上描述，完全可以由幾何關(guān)系得到衛(wèi)星關(guān)于地球的相對位置。根據(jù)式（47）和式（48）有

將x，y，z作為間接量觀測，可得觀測方程為

式中，Z＝［x y z］T；V為高斯白噪聲；

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

為了驗(yàn)證本文提出的改進(jìn)的強(qiáng)跟蹤UKF方法的有效性，分別將改進(jìn)的STSRUKF、STF和SRUKF對由式（42）和式（47）組成的導(dǎo)航系統(tǒng)，將其離散化，然后進(jìn)行仿真。仿真環(huán)境為Matlab R2010a，仿真參數(shù)設(shè)置如下［24］：坐標(biāo)系為J2000.0地心赤道慣性坐標(biāo)系，半軸長為a＝7 136.635km，偏心率e＝0.001，軌道傾角i＝65°，升交點(diǎn)赤經(jīng)Ω＝30°，近地點(diǎn)角距為ω＝30°，光學(xué)導(dǎo)航相機(jī)精度為0.005°，星敏感器精度為3″，測量噪聲均為白噪聲，采樣時(shí)間間隔為T＝1s，系統(tǒng)的初始誤差為X＝［4.5 4.3 3.2－0.46 0.056 0.59］T。仿真分別在理想情況、系統(tǒng)受到干擾的情況和系統(tǒng)存在模型不確定型3種環(huán)境下進(jìn)行，在理想情況下，仿真時(shí)長為t＝100s，主要是為了驗(yàn)證改進(jìn)的強(qiáng)跟蹤平方根UKF的估計(jì)精度；在系統(tǒng)受到干擾的情況下，仿真時(shí)長為t＝600s，在系統(tǒng)存在模型不確定性的情況下，仿真時(shí)長為t＝400s，這兩種情況下主要是為了驗(yàn)證改進(jìn)的強(qiáng)跟蹤平方根的強(qiáng)跟蹤能力和魯棒性。

（1）理想環(huán)境下的仿真

在理想情況下的仿真結(jié)果如圖2和圖3所示，從圖2和圖3可以看出，在理想情況下，3種方法都能很好地跟蹤系統(tǒng)的狀態(tài)變化，3種方法的收斂速度和估計(jì)精度差不多。

圖2 位置估計(jì)絕對誤差

圖3 速度估計(jì)絕對誤差

（2）系統(tǒng)受到干擾的情況下仿真

在實(shí)際的空間環(huán)境中，系統(tǒng)受到的干擾及測量機(jī)構(gòu)發(fā)生故障是不確定的范圍，為了驗(yàn)證改進(jìn)的強(qiáng)跟蹤平方根UKF對系統(tǒng)不確定性的跟蹤能力、估計(jì)精度及魯棒性。假設(shè)系統(tǒng)存在以下幾種不確定性，設(shè)

仿真實(shí)驗(yàn)結(jié)果如圖4和圖5所示，從圖4和圖5可以看出，SRUKF的估計(jì)性能很差，受系統(tǒng)的不確定性因素影響很大，即不具有對系統(tǒng)不確定性的魯棒性和強(qiáng)跟蹤能力。而STF針對系統(tǒng)受到常值干擾、有界正弦時(shí)變干擾和有界周期性方波干擾等不確定性因素影響時(shí)具有較好的魯棒性和強(qiáng)跟蹤能力，估計(jì)性能幾乎不受影響，但是當(dāng)系統(tǒng)受到隨機(jī)無規(guī)律有界干擾不確定性因素影響時(shí)，STF的魯棒性和跟蹤能力變差，估計(jì)誤差變大。但是，在上述4種的任何一種干擾不確定性因素的影響下，本文提出的STSRUKF的估計(jì)性能都幾乎不受影響，具有良好的估計(jì)性能，表現(xiàn)出優(yōu)良的魯棒性和強(qiáng)跟蹤能力。

圖4 位置估計(jì)絕對誤差

圖5 速度估計(jì)絕對誤差

（3）系統(tǒng)存在模型不確定性的情況下仿真

在實(shí)際航天器導(dǎo)航系統(tǒng)中，大多數(shù)時(shí)候，由于外界復(fù)雜多變的環(huán)境，在系統(tǒng)建模的時(shí)候存在未知的建模動(dòng)態(tài)特性，即建立的系統(tǒng)模型存在不確定性，為了進(jìn)一步驗(yàn)證改進(jìn)的強(qiáng)跟蹤平方根UKF的魯棒性、強(qiáng)跟蹤能力和穩(wěn)定性。假設(shè)，系統(tǒng)模型存在如下的不確定性：

仿真實(shí)驗(yàn)結(jié)果如圖6和圖7所示，從圖6和圖7可以看出，無論是位置曲線還是速度曲線，SRUKF的估計(jì)性能都很差，嚴(yán)重偏離真實(shí)的狀態(tài)，跟蹤能力弱，對系統(tǒng)模型存在的不確定性很敏感，即不具備克服系統(tǒng)不確定性的魯棒性。STF估計(jì)精度明顯要好于SRUKF，但是當(dāng)系統(tǒng)模型的不確定性變大的時(shí)候，STF的估計(jì)性能也開始變差，不過變化范圍不是很大。而本文提出的改進(jìn)的強(qiáng)跟蹤平方根UKF，相對于上述兩種方法，估計(jì)精度是最好的，而且針對兩種假設(shè)的系統(tǒng)模型不確定性的情況下，其估計(jì)性能也幾乎不受影響，即具有很好的克服系統(tǒng)模型不確定的魯棒性。

圖6 位置估計(jì)絕對誤差

圖7 速度估計(jì)絕對誤差

式中，N為蒙特卡羅仿真次數(shù)；j表示第j次仿真；X（k）及分別表示第k時(shí)刻狀態(tài)真實(shí)值及濾波估計(jì)值。

為了更好地比較3種算法在穩(wěn)態(tài)的估計(jì)性能，評價(jià)算法的有效性，采用均方根誤差（root-mean-square-error，RMSE）來描述估計(jì)質(zhì)量，分別針對兩種情況下，進(jìn)行100次蒙特卡羅仿真。仿真結(jié)果如表1～表3所示，RMSE的表達(dá)式如下：

表1 理想情況下的均方根誤差

表2 系統(tǒng)受干擾影響下的均方根誤差

表3 系統(tǒng)存在模型不確定情況下的均方根誤差

從表1中可以看出，在理想情況下，3種估計(jì)方法得到的均方根誤差相差不大，說明3種方法的的估計(jì)精度差不多，在同一個(gè)數(shù)量級上。從表2和表3可以看出，在系統(tǒng)受到干擾不確定性因素影響或者系統(tǒng)存在模型不確定的時(shí)候，SRUKF得到的均方根誤差最大，與STF和STSRUKF相比較，要高出一個(gè)數(shù)量級以上，其次是STF得到的，最好的是STSRUKF，這說明STSRUKF估計(jì)的結(jié)果的精度是最好的。比較表1～表3中的數(shù)據(jù)，也可知系統(tǒng)存在的模型不確定情況下，3種方法的估計(jì)結(jié)果的精度最差。另外，根據(jù)表1～表3中的數(shù)據(jù)還可知，在系統(tǒng)受到不確定因素影響和系統(tǒng)存在模型不確定兩種情況下，SRUKF的估計(jì)精度變差了很多，而STF和STSRUKF則變化相對較小，這說明通過引入自適應(yīng)調(diào)節(jié)因子可以很好地改善濾波器對系統(tǒng)不確定性的魯棒性，克服干擾的影響。

4 結(jié) 論

本文針對衛(wèi)星導(dǎo)航系統(tǒng)，設(shè)計(jì)了基于星敏感器和光學(xué)導(dǎo)航相機(jī)的導(dǎo)航方案，針對標(biāo)準(zhǔn)平方根UKF存在的缺陷，給出了一種改進(jìn)的平方根UKF方法，改善了平方根UKF的數(shù)值穩(wěn)定性，針對衛(wèi)星導(dǎo)航系統(tǒng)受不確定性干擾因素影響，采用強(qiáng)跟蹤濾波方法，提出了改進(jìn)的強(qiáng)跟蹤平方根UKF方法。將改進(jìn)的STSRUKF、STF和SRUKF分別用于衛(wèi)星導(dǎo)航系統(tǒng)中進(jìn)行實(shí)驗(yàn)仿真驗(yàn)證，仿真時(shí)分別將4種不同的干擾信號引入系統(tǒng)中，仿真實(shí)驗(yàn)結(jié)果表明，相對于SRUKF和STF，STSRUKF對系統(tǒng)受不確定性干擾因素影響所具有的魯棒性和強(qiáng)跟蹤能力都是最好的，能夠準(zhǔn)確的估計(jì)和跟蹤系統(tǒng)的狀態(tài)變化，提高估計(jì)精度。

［1］Fesq L.Spacecraft autonomy in the new millennium［C］∥Proc.of the Annual AAS Rocky Mountain Guidance and Control Conference，1996.

［2］Psiaki M L.Backward-smoothing extended kalman filter［J］.Journal of Guidance Control and Dynamics，2005，28（5）：885－894.

［3］Norgaard M，Poulsen N K，Ravn O.New development in state estimation systems［J］.Automatica，2000，36（11）：1627－1638.

［4］Okita F，Ozaki S.Nonlineaity-compensation extended Kalman filter and its application to target motion［J］.Oki Technical Review，1997，63（159）：1－12.

［5］Joseph J，LaViola J.A comparison of unscented and extended Kalman filtering for estimating quaternion motion［C］∥Proc.of the American Control Conference，2003.

［6］Julier S J，Uhlamnn J K，Durrant-Whyte H F.A new method for the nonlinear transformation of means and co variances in filters and estimaters［J］.IEEE Trans.on Automatic Control，2000，45（3）：477－482.

［7］Zhang Y，F(xiàn)ang J C.Study of the satellite autonomous celestial navigation based on the unscented Kalman filter［J］.Journal of Astronautics，2010，31（3）：739－747.（張瑜，房建成.基于Unscented卡爾曼濾波器的衛(wèi)星自主天文導(dǎo)航研究［J］.宇航學(xué)報(bào)，2010，31（3）：739－747.）

［8］Liu T，Xie Y C.Stability analysis of UKF and its application in relative navigation［J］.Journal of Astronautics，2010，31（3）：739－747.（劉濤，解永春.UKF穩(wěn)定性研究及其在相對導(dǎo)航中的應(yīng)用［J］.宇航學(xué)報(bào)，2010，31（3）：739－747.）

［9］Zhou D H，Xi Y G，Zhang Z J.Suboptimal fading extended Kalman filtering for nonlinear systems［J］.Control and Decision，1990，5（5）：1－6.（周東華，席裕庚，張鐘俊.非線性系統(tǒng)次優(yōu)漸消因子的擴(kuò)展卡爾曼濾波［J］.控制與決策，1990，5（5）：1－6.）

［10］Zhou D H，Xi Y G，Zhang Z J.A syboptimal multiple fading extended Kalman filter［J］.Acta Automatica Sinica，1991，17（6）：689－696.（周東華，席裕庚，張鐘俊.一種帶多重次優(yōu)漸消因子的擴(kuò)展卡爾曼濾波器［J］.自動(dòng)化學(xué)報(bào)，1991，17（6）：689－696.）

［11］Lu H Y，Zhao W，Xiong J.Exploration of strong tracking UKF used in the SINS／GPS integrated navigation［J］.Avionics Technology，2008，39（4）：5－10.（陸海勇，趙偉，熊劍.強(qiáng)跟蹤UKF濾波在SINS＿GPS組合導(dǎo)航中的應(yīng)用研究［J］.航空電子技術(shù)，2008，39（4）：5－10.）

［12］Yang W B，Li S Y.Autonomous navigation filtering algrithm for spacecraft based on strong tracking UKF［J］.Systems Engineering and Electronics，2011，33（11）：2485－2491.（楊文博，李少遠(yuǎn).基于強(qiáng)跟蹤UKF的航天器自主導(dǎo)航間接量測濾波算法［J］.系統(tǒng)工程與電子技術(shù)，2011，33（11）：2485－2491.）

［13］Chen Y Z，Sun Z G，Ma H B.Square－root unscented Kalman filter for vehicle integrated navigation［J］.Systems Engineering and Electronics，2008，30（5）：926－929.（陳陽舟，孫章固，馬海波.基于平方根UKF的車輛組合導(dǎo)航［J］.系統(tǒng)工程與電子技術(shù)，2008，30（5）：926－929.）

［14］Fan L W，Zheng W，Tang G J.Autonomous navigation method for orbital maneuver vehicle based on square-root unscented Kalman filter［J］.Journal of Chinese Inertial Technology，2008，16（6）：667－675.（范利濤，鄭偉，湯國建.基于平方根UKF濾波的軌道機(jī)動(dòng)飛行器自主導(dǎo)航方法［J］.中國慣性技術(shù)學(xué)報(bào)，2008，16（6）：667－675.）

［15］Fu M Y，Deng Z H，Zhang J W.Kalman filtering with applications to navigation systems［M］.Beijing：Science Press，2003.（付夢印，鄧志紅，張繼偉.Kalman濾波及其在導(dǎo)航系統(tǒng)中的應(yīng)用［M］.北京：科學(xué)出版社，2003.）

［16］Jafarzadeh S，Lascu C，F(xiàn)adali S.Square root unscented Kalman filters for state estimation of induction motor drives［J］.IEEE Trans.on Industry Applications，2013，49（1）：92－99.

［17］Merwe V D，R.Wan E A.The square-root unscented Kalmanfilter for state and parameter-estimation［C］∥Proc.of the IEEE International Conference on Acoustics，Speech and Signal Processing，2001：3461－3464.

［18］Kaminski P G，Bryson A E，Schmidt S F.Discrete square root filtering：a survey of current techniques［J］.IEEE Trans.on Automatic Control，1970，16（6）：727－736.

［19］Qian M H，Huang W，Sun L.Attitude estimation of strong tracking UKF based on multiplie fading factors［J］.Systems Engineering and Electronics，2103，35（3）：580－585.（錢明華，黃蔚，孫龍.基于多重次漸消因子的強(qiáng)跟蹤UKF姿態(tài)估計(jì)［J］.系統(tǒng)工程與電子技術(shù)，2013，35（3）：580－585）

［20］Li P，Cui H T，Cui P Y.UPF based autonomous navigation scheme for deep space probe［J］.Journal of Systems Engineering and Electronics，2008，19（3）：529－536.

［21］Shang L，Liu G H，Zhang R.An information fusion algorithm for integrated autonomous orbit determination of navigation satellites［J］.Acta Astronautica，2013，85：33－40.

［22］Gan Q B，Ma J B.Autonomous satellite constellation orbit determination using the star sensor and inter-satellite links data［J］.Science China-Physics Mechanics ＆Astronomy，2010，53（5）：966－974.

［23］Qian H M，Sun L，Cai J N.A star light refraction scheme with single star sensor used in autonomous satellite navigation system［J］.Acta Astronautica，2014，96：45－52.

［24］Zhang G Y，Chen Y M，Yang F.Applying of forecast-revise EKF algorithm in autonomous navigation system［J］.Journal of Astronautics，2009，30（6）：2227－2230.（張共愿，程詠梅，楊峰.預(yù)測－校正EKF算法在自主導(dǎo)航中的應(yīng)用［J］.宇航學(xué)報(bào)，2009，30（6）：2227－2230.）

Satellite autonomous navigation filtering algorithm based on improved strong tracking square-root UKF

LI Min1，WANG Song－yan1，ZHANG Ying-chun1，2，LI Hua-yi1
（1.College of Astronautics，Harbin Institute of Technology，Harbin 150001，China；2.Aerospace Dongfanghong Development Ltd，Shenzhen 518057，China）

For the satellite autonomous navigation system subjects to model uncertainties，external disturbances and noises，the unscented Kalman filter（UKF）method has low accuracy，poor tracking ability and poor robustness.An improved strong tracking square-root unscented Kalman filter（STSRUKF）-based autonomous navigation method is proposed.For the navigation purpose，star sensors and optical navigation cameras are used in this method，and the indirect measurement vector is transformed to observables through a transition equation.To avoid the problem that negative zero weights of sigma points and great calculation errors in square-root UKF（SRUKF）design for high-order systems，a modified square-root decomposition method is applied for the SRUKF design to improve the stability of the SRUKF.In addition，based on strong tracking filters（STF），multiple adaptive fading factors in adjustment covariance matrix are adopted so that the STSRUKF has better tracking ability，better robustness against model uncertainties and better estimation accuracy.Finally，the STSRUKF-based method is applied to the satellite autonomous navigation systems，and simulation results are provided to verify the effectiveness and practicability of the proposed approach.

satellite autonomous navigation system；uncertain disturbance；improved strong tracking square-root unscented Kalman filter（STSRUKF）；robustness

V 19

A

10.3969／j.issn.1001-506X.2015.08.22

李 敏（1989－），男，碩士研究生，主要研究方向?yàn)樾l(wèi)星姿態(tài)估計(jì)及控制、非線性濾波方法在導(dǎo)航當(dāng)中的應(yīng)用。

E-mail：liminhit＠126.com

王松艷（1976－），女，副教授，博士，主要研究方向?yàn)轱w行器控制。

E-mail：wangsy＠hit.edu.cn

張迎春（1961－），男，教授，博士研究生導(dǎo)師，主要研究方向?yàn)樾l(wèi)星控制系統(tǒng)設(shè)計(jì)、衛(wèi)星總體設(shè)計(jì)、衛(wèi)星導(dǎo)航。

E-mail：zhang＠hit.edu.cn

李化義（1978－），男，講師，博士，主要研究方向?yàn)樾l(wèi)星編隊(duì)。

E-mail：lihuayi＠hit.edu.cn

1001－506X201508－1858－08

網(wǎng)址：www.sys－ele.com

2014－07－01；

2014－10－23；網(wǎng)絡(luò)優(yōu)先出版日期：2015－01－06。

網(wǎng)絡(luò)優(yōu)先出版地址：http：／／www.cnki.net／kcms／detail／11.2422.TN.20150106.1201.005.html

國家自然科學(xué)基金（61304237）；國防重點(diǎn)實(shí)驗(yàn)室微小型航天器技術(shù)開放基金（20090450126）資助課題