基于容積濾波的自適應(yīng)相對(duì)導(dǎo)航算法

李明明 馮 燁 于欣欣 王 瀛

北京控制工程研究所，北京100190

航天器的相對(duì)導(dǎo)航是指確定航天器之間的相對(duì)位置以及相對(duì)速度的過程。目前追蹤飛行器與目標(biāo)飛行器在軌道上交會(huì)、伴飛和編隊(duì)飛行是航天器導(dǎo)航、制導(dǎo)與控制領(lǐng)域的一項(xiàng)重要研究?jī)?nèi)容，而高精度的相對(duì)導(dǎo)航技術(shù)正是自主交會(huì)、伴飛及編隊(duì)飛行的基礎(chǔ)，其性能會(huì)直接影響最終的制導(dǎo)和控制精度。

相對(duì)導(dǎo)航最直接的方法是利用絕對(duì)導(dǎo)航結(jié)果計(jì)算兩航天器間的相對(duì)位置，文獻(xiàn)［1］詳細(xì)介紹了GPS 相對(duì)導(dǎo)航的技術(shù)和算法。但是，當(dāng)兩航天器距離較近時(shí)，存在著相互遮擋GPS 信號(hào)的現(xiàn)象，造成兩者無法保證選用相同的GPS 衛(wèi)星，使得相對(duì)導(dǎo)航精度下降，且由于GPS 更新速率的限制導(dǎo)致其在最后靠攏對(duì)接時(shí)具有固有缺陷，因此目前相對(duì)導(dǎo)航研究的重點(diǎn)集中在直接利用航天器上的諸多相對(duì)導(dǎo)航傳感器完成相對(duì)導(dǎo)航任務(wù)［2］。航天器相對(duì)導(dǎo)航一般選用激光雷達(dá)或視覺傳感器作為相對(duì)導(dǎo)航傳感器。其中激光雷達(dá)由于具有作用體積小、精度高、可在幾十千米到幾米的過程中提供較高精度的相對(duì)距離和角度測(cè)量值等特點(diǎn)，是中、遠(yuǎn)程相對(duì)導(dǎo)航中最常使用的導(dǎo)航敏感器。本文主要研究基于激光雷達(dá)的導(dǎo)航濾波算法，基于視覺傳感器及兩者數(shù)據(jù)融合的導(dǎo)航算法將在后續(xù)研究中進(jìn)行討論。

鑒于激光雷達(dá)直接測(cè)量值為相對(duì)位置的非線性函數(shù)，因此需要利用非線性濾波算法設(shè)計(jì)導(dǎo)航濾波器。在非線性濾波算法中擴(kuò)展卡爾曼濾波(EKF，extended Kalman filter，)是較為經(jīng)典的濾波方法，但其利用狀態(tài)估計(jì)值計(jì)算Jacobian 矩陣時(shí)，容易導(dǎo)致濾波器有偏，甚至發(fā)散;近期學(xué)者Arasaratnam 通過利用三階容積法則的數(shù)值積分方法對(duì)高斯加權(quán)積分進(jìn)行近似，推導(dǎo)出了一種容積卡爾曼濾波算法(CKF，cubature Kalman filter )［3］，該算法不僅可以避免線性化非線性方程等近似方法帶來的誤差，且在估計(jì)精度和收斂速度方面較EKF 也有顯著提高［4］。

本文采用激光交會(huì)雷達(dá)測(cè)量?jī)珊教炱鏖g相對(duì)距離和視線角，利用CKF 算法設(shè)計(jì)了自適應(yīng)導(dǎo)航濾波器，對(duì)追蹤航天器和目標(biāo)航天器之間的相對(duì)位置和速度進(jìn)行估計(jì)，并給出數(shù)學(xué)仿真結(jié)果，表明該算法是可行的。

1 基于激光雷達(dá)的相對(duì)運(yùn)動(dòng)導(dǎo)航模型

具體的導(dǎo)航模型是設(shè)計(jì)導(dǎo)航濾波器的基礎(chǔ)，因此本節(jié)將主要對(duì)相對(duì)運(yùn)動(dòng)方程及激光雷達(dá)的測(cè)量方程進(jìn)行描述。為方便闡述，在下面各節(jié)中追蹤飛行器簡(jiǎn)稱追蹤器，目標(biāo)飛行器簡(jiǎn)稱為目標(biāo)器。

1.1 相對(duì)運(yùn)動(dòng)方程

追蹤器與目標(biāo)器在軌道上的相對(duì)位置示意圖如圖1 所示。圖中OXYZ 為目標(biāo)軌道坐標(biāo)系，OCXCYCZC為追蹤器軌道坐標(biāo)系。

將目標(biāo)器與追蹤器的相對(duì)運(yùn)動(dòng)定義在目標(biāo)器軌道坐標(biāo)系中，則當(dāng)目標(biāo)器運(yùn)行軌道為近圓軌道時(shí)，兩航天器的質(zhì)心相對(duì)運(yùn)動(dòng)方程可采用CW 方程進(jìn)行描述［5］。具體方程如下所示:

圖1 追蹤器與目標(biāo)器相對(duì)位置

式中，x，y，z 分別為追蹤器在目標(biāo)器軌道中的相對(duì)位置;ω 為目標(biāo)的軌道角速度;ux，uy，uz為追蹤器的控制沖量在目標(biāo)器軌道系中的投影;wx，wy，wz為系統(tǒng)誤差，主要用來描述追蹤器控制執(zhí)行機(jī)構(gòu)產(chǎn)生的隨機(jī)沖量噪聲，和目標(biāo)軌道角速率ω 的隨機(jī)偏移誤差產(chǎn)生的影響，一般可設(shè)wi～N(0，σi)。取相對(duì)位置、速度為系統(tǒng)狀態(tài)，將式(1)～(3)離散化可得離散的相對(duì)運(yùn)動(dòng)方程為

1.2 基于激光雷達(dá)的相對(duì)運(yùn)動(dòng)測(cè)量方程

目前相對(duì)導(dǎo)航任務(wù)中，通常將相對(duì)導(dǎo)航用的激光雷達(dá)設(shè)備安裝于追蹤器上，不失一般性，可令其測(cè)量坐標(biāo)系與追蹤器本體坐標(biāo)系重合，即測(cè)量值在追蹤器本體坐標(biāo)系ObXbYbZb描述。則激光雷達(dá)的相對(duì)運(yùn)動(dòng)測(cè)量方程可表示為

式中，ρk，αk，βk分別為k 時(shí)刻由激光雷達(dá)測(cè)量得到的目標(biāo)器相對(duì)于追蹤器本體系的視線距離、仰角和方位角;xbk，ybk，zbk為目標(biāo)器在追蹤器本體系下的坐標(biāo);Vk為激光雷達(dá)的量測(cè)噪聲，一般可假設(shè)其為零均值的高斯白噪聲，量測(cè)噪聲協(xié)方差陣為

由式(5)可見，激光雷達(dá)測(cè)量輸出是在追蹤器本體系ObXbYbZb描述，而系統(tǒng)狀態(tài)方程(4)是在目標(biāo)軌道坐標(biāo)系OXYZ 描述，因此在設(shè)計(jì)導(dǎo)航濾波算法前有必要將狀態(tài)方程和觀測(cè)方程中的狀態(tài)變量統(tǒng)一起來。易知二者的關(guān)系可表示為

2 容積濾波導(dǎo)航算法

由于激光雷達(dá)的測(cè)量方程具有嚴(yán)重的非線性，因此設(shè)計(jì)相對(duì)導(dǎo)航濾波器時(shí)需針對(duì)該特性進(jìn)行特殊處理。傳統(tǒng)方法以直接測(cè)量值作為濾波器輸入，利用EKF 設(shè)計(jì)濾波器。但由于EKF 在推導(dǎo)過程中不得不引入如兩飛行器共面、追蹤器零姿態(tài)等假設(shè)，以保證偏導(dǎo)數(shù)求取過程的可實(shí)現(xiàn)性［6］;而文獻(xiàn)［6］提出的利用間接量測(cè)值設(shè)計(jì)濾波器的方法雖然無需偏導(dǎo)數(shù)計(jì)算，但需對(duì)量測(cè)協(xié)方差矩陣進(jìn)行一階近似，且誤差特性不明確，不便于設(shè)計(jì)自適應(yīng)濾波算法，為該濾波算法的進(jìn)一步優(yōu)化設(shè)計(jì)造成了障礙。因此鑒于CKF 無需對(duì)非線性方程線性化處理，且相比于UKF算法簡(jiǎn)單易行的優(yōu)點(diǎn)，并考慮克服激光雷達(dá)測(cè)量值不準(zhǔn)確對(duì)導(dǎo)航精度的影響，利用CKF 設(shè)計(jì)自適應(yīng)容積濾波算法。

2.1 容積卡爾曼濾波算法

考慮具有加性噪聲的離散非線性動(dòng)態(tài)系統(tǒng)

式中，xk∈Rn為系統(tǒng)狀態(tài)向量;Γk為系統(tǒng)過程噪聲驅(qū)動(dòng)陣;Wk系統(tǒng)過程噪聲序列，為均值為零協(xié)方差陣為Qk的白噪聲;zk+1∈Rm為觀測(cè)向量;Vk量測(cè)噪聲序列，為均值為零協(xié)方差陣為Rk的白噪聲。則容積卡爾曼濾波算法可表示為［3］:

1)計(jì)算基本的容積點(diǎn)和對(duì)應(yīng)的權(quán)值

式中:n 為系統(tǒng)狀態(tài)維數(shù);［1］j表示點(diǎn)集［1］中的第j個(gè)點(diǎn)，其中符號(hào)［1］為完整全對(duì)稱點(diǎn)集，表示為n 維單位向量e =［1，0，…，0］T的元素進(jìn)行全排列和改變?cè)胤?hào)所產(chǎn)生的點(diǎn)集［7］。

2)初始化

3)時(shí)間更新

其中，Si為對(duì)方差陣Pi進(jìn)行下三角Cholesky 分解運(yùn)算所得到的平方根矩陣。

4)量測(cè)更新

5)狀態(tài)估計(jì)

2.2 基于容積濾波的自適應(yīng)算法

鑒于相對(duì)運(yùn)動(dòng)狀態(tài)方程(4)為線性方程，因此可將上述容積濾波方程進(jìn)行簡(jiǎn)化，并考慮克服噪聲統(tǒng)計(jì)特性不準(zhǔn)確等工程測(cè)量問題的影響，本文所設(shè)計(jì)的基于容積濾波的自適應(yīng)相對(duì)導(dǎo)航濾波器如下:

1)按式(10)～(13)計(jì)算容積點(diǎn)及權(quán)值，并初始化

2)時(shí)間更新

3)量測(cè)更新

按式(18)～(20)對(duì)觀測(cè)量進(jìn)行下一步預(yù)測(cè)。

4)自適應(yīng)設(shè)置量測(cè)噪聲協(xié)方差陣

考慮到量測(cè)值ρk與αk，βk為不同類型參數(shù)，所以無法利用單因子自適應(yīng)方案，因此本文針對(duì)其設(shè)計(jì)了多因子自適應(yīng)方案。

計(jì)算觀測(cè)殘差

按測(cè)量值類型標(biāo)準(zhǔn)化殘差向量

采用Huber 權(quán)函數(shù)構(gòu)造自適應(yīng)因子［8］

式中，c 可取為c=2.5 ～3.0。

5)按式(21)～(25)計(jì)算協(xié)方差矩陣并進(jìn)行狀態(tài)估計(jì)。

需要說明的是:本文利用測(cè)量狀態(tài)不符值(式(28)所示)與標(biāo)稱協(xié)方差陣間的關(guān)系及Huber 代價(jià)函數(shù)構(gòu)建自適應(yīng)因子(關(guān)于Huber 權(quán)函數(shù)構(gòu)造方法的詳細(xì)推導(dǎo)可參見文獻(xiàn)［8］)。式(30)中c 為可調(diào)參數(shù)，在Huber 估計(jì)中體現(xiàn)為統(tǒng)計(jì)量取中值或均值。而從診斷異常并自適應(yīng)放大協(xié)方差陣的角度而言，c選擇的過大將會(huì)導(dǎo)致無法診斷出量測(cè)噪聲異常;c選擇的過小又會(huì)使得噪聲協(xié)方差無故增大，影響導(dǎo)航精度。因此一般考慮到正態(tài)分布原則，式(30)中可取c=2.5 ～3.0［9］。實(shí)際工程應(yīng)用時(shí)應(yīng)按照工程需要及傳感器特性給出相應(yīng)的經(jīng)驗(yàn)值。

3 數(shù)學(xué)仿真

為驗(yàn)證本文提出的基于容積濾波的自適應(yīng)導(dǎo)航算法在兩航天器相對(duì)運(yùn)動(dòng)導(dǎo)航中的性能，本節(jié)設(shè)計(jì)了一個(gè)簡(jiǎn)單的交會(huì)過程給予驗(yàn)證。

仿真時(shí)設(shè)目標(biāo)器以圓軌道飛行在400km 高空，軌道角速度ω=0.0012rad/s。在t=0 時(shí)刻，追蹤器在目標(biāo)軌道坐標(biāo)系中的初始位置、速度及設(shè)置的相應(yīng)誤差如表1 所示。追蹤航天器經(jīng)過一次20s 的尋的脈沖及一次15s 的制動(dòng)脈沖控制后，于1800s 時(shí)位于目標(biāo)器后方400m 保持點(diǎn)處。兩次脈沖產(chǎn)生的加速度如圖2 所示。激光交會(huì)雷達(dá)以5Hz 的頻率給出測(cè)量信息，設(shè)正常工作情況下，激光雷達(dá)測(cè)量得到的相對(duì)距離具有3m(1σ)的隨機(jī)誤差、相對(duì)姿態(tài)角具有隨機(jī)誤差為0.05°(1σ)［6］。

為驗(yàn)證本文提出的多因子自適應(yīng)方案的有效性，仿真過程中按表2 所示在激光交會(huì)雷達(dá)的測(cè)量值中加入異常干擾，其中增大隨機(jī)誤差異常干擾可視為因地面測(cè)試時(shí)無法完全模擬激光交會(huì)雷達(dá)在太

表1 仿真初始數(shù)據(jù)

圖2 控制加速度曲線

空中的工作環(huán)境，或發(fā)生了太空磁暴輻射等突發(fā)情況，導(dǎo)致地面得到的測(cè)量噪聲統(tǒng)計(jì)特性不準(zhǔn)確造成的;而常值測(cè)量偏差可視為由激光交會(huì)雷達(dá)部件故障造成。

表2 激光雷達(dá)測(cè)量值干擾策略

圖3 相對(duì)位置估計(jì)誤差比較

圖4 相對(duì)速度估計(jì)誤差比較

圖3 和4 分別給出了利用CKF 算法和本文提出的基于CKF 的自適應(yīng)算法仿真得出的相對(duì)位置和相對(duì)速度估計(jì)誤差的對(duì)比圖。仿真結(jié)果表明雖然CKF 算法可以用來對(duì)兩航天器間的相對(duì)距離和速度進(jìn)行估計(jì)(如t ＞1800s 時(shí)誤差曲線所示)，但當(dāng)激光交會(huì)雷達(dá)的測(cè)量值存在干擾時(shí)，CKF 算法的導(dǎo)航誤差將明顯增大，無法準(zhǔn)確完成導(dǎo)航任務(wù)。而從圖3 及4 可見，本文提出的基于CKF 的自適應(yīng)算法對(duì)激光交會(huì)雷達(dá)的測(cè)量噪聲具有容錯(cuò)能力，在其測(cè)量值發(fā)生異常時(shí)仍可準(zhǔn)確完成導(dǎo)航任務(wù):

1)當(dāng)測(cè)量噪聲隨機(jī)誤差增大(可視為噪聲統(tǒng)計(jì)特性不準(zhǔn)確)時(shí)，由于該算法可以自適應(yīng)的調(diào)整相關(guān)測(cè)量值的協(xié)方差，因此保證了此種異常情況下算法的導(dǎo)航精度(如圖中200 ～400s，500 ～700s，800 ～1000s 所示);

2)當(dāng)測(cè)量值短時(shí)間內(nèi)具有常偏誤差時(shí)，該算法亦可以通過自適應(yīng)增大相關(guān)測(cè)量值協(xié)方差的方式，降低異常測(cè)量值對(duì)導(dǎo)航系統(tǒng)的影響，保證了算法的導(dǎo)航精度，并且可為組合導(dǎo)航系統(tǒng)的后續(xù)重組提供判斷依據(jù)和重組時(shí)間(如圖中1100 ～1130s，1400 ～1430s，1700 ～1730s 所示);

3)通過仿真圖可見，文中設(shè)計(jì)的多因子自適應(yīng)方案可以很好的解決不同類型量測(cè)值的自適應(yīng)問題。

4 結(jié)論

本文針對(duì)航天器相對(duì)導(dǎo)航時(shí)常用的激光交會(huì)雷達(dá)量測(cè)方程為非線性方程的特點(diǎn)，將CKF 算法應(yīng)用于航天器相對(duì)導(dǎo)航中。并結(jié)合實(shí)際工程需要，考慮到測(cè)量噪聲統(tǒng)計(jì)特性不準(zhǔn)確以及測(cè)量信息中出現(xiàn)常偏等現(xiàn)象，設(shè)計(jì)了基于CKF 的自適應(yīng)相對(duì)導(dǎo)航算法。仿真結(jié)果表明，該算法解決了不同類型量測(cè)值的協(xié)方差自適應(yīng)問題，且當(dāng)激光交會(huì)雷達(dá)的測(cè)量值存在隨機(jī)誤差增大(統(tǒng)計(jì)特性不準(zhǔn)確)和具有常值偏差異常時(shí)，仍然可以通過自適應(yīng)調(diào)整測(cè)量協(xié)方差陣保證獲得較高的導(dǎo)航精度。

［1］Walton W，Jeehong M. A Comparison of State Space，Range Space，and Carrier Phase Differential GPS/INS Relative Navigation［C］. Proceedings of the American Control Conference，June 2000:2932-2938.

［2］Hablani H B，Tapper M . Guidance Algorithms for Autonomous Rendezvous of Spacecraft with a Target Vehicle in Circular Orbit［C］. AIAA Guidance，Navigation，and Control Conference and Exhibit，6-9 August 2001，AIAA 2001-4393.

［3］Arasaratnam I，Haykin S. Cubature Kalman Filters［J］.IEEE Transactions on Automatic Control，2009，54(6):1254-1269.

［4］魏喜慶.航天器相對(duì)導(dǎo)航中的非線性濾波問題研究［D］.哈爾濱工業(yè)大學(xué)航天學(xué)院，2013:20-33. (WEI Xiqing. Research on Nonlinear Filtering for Relative Navigation of Spacecraft［D］. School of Astronautics，Harbin Institute of Technology，2013:20-33.)

［5］Philip N K，Ananthasayanam M R. Relative Position and Attitude Estimation and Control Schemes for The Final Phase of an Autonomous Docking Mission of Spacecraft［J］. Acta Astronautica，2003，52(7):511-522.

［6］何英姿，諶穎，韓冬. 基于交會(huì)雷達(dá)測(cè)量的相對(duì)導(dǎo)航濾波器［J］.航天控制，2004，22(6):17-20.(He Yingzi，Chen Ying，Han Dong. Rendezvous Radar Observation-based Relative Navigation Filter［J］. Aerospace Control，2004，22(6):17-20.)

［7］穆靜，蔡遠(yuǎn)利，張俊敏. 容積粒子濾波算法及其應(yīng)用［J］.西安交通大學(xué)學(xué)報(bào)，2011，8(145):13-16. (Mu Jing，Cai Yuanli，Zhang Junmin. Cubature Particle Filter and Its Application［J］. Journal of Xian Jiaotong University，2011，8(145):13-16.)

［8］Karlgaard C D. Robust Adaptive Estimation for Autonomous Rendezvous in Elliptical Orbit［D］. Virginia:Virginia Polytechnic Institute and State University，2010:20-29.

［9］楊元喜.自適應(yīng)動(dòng)態(tài)導(dǎo)航定位［M］. 北京:測(cè)繪出版社，2006:99-100. (Yang Yuanxi. Adaptive Navigation and Kinematic Positioning［M］. Beijing:Surveying and Mapping Press，2006:99-100.)