失效衛(wèi)星救援過程中相對(duì)導(dǎo)航算法研究

楊雪勤，王洪宇，馮 剛

（上海宇航系統(tǒng)工程研究所，上海 201109）

0 引言

太空中存在大量的失效衛(wèi)星，這些衛(wèi)星或是服務(wù)期已過，因一個(gè)或數(shù)個(gè)系統(tǒng)失效而成為廢棄衛(wèi)星，或某載荷失效而不能正常工作。對(duì)此類衛(wèi)星，如無其他措施，將可能成為空間碎片或垃圾，其存在會(huì)威脅空間其他衛(wèi)星的正常飛行。因此，空間救援與維護(hù)顯得尤為重要。針對(duì)此類失效衛(wèi)星的空間救援措施一般為：施援衛(wèi)星軌道機(jī)動(dòng)至救援目標(biāo)附近，用其攜帶的機(jī)械臂實(shí)施救援，或用機(jī)械臂回收廢棄衛(wèi)星以減少空間垃圾或更換救援目標(biāo)的載荷模塊以重新使用。這些空間救援與維護(hù)策略，均需對(duì)救援目標(biāo)實(shí)施近程逼近過程，施援衛(wèi)星需裝備近程導(dǎo)航設(shè)備實(shí)現(xiàn)對(duì)求援目標(biāo)的相對(duì)導(dǎo)航與控制［1］。

非線性條件下的擴(kuò)展卡爾曼濾波（EKF）對(duì)非線性的系統(tǒng)方程或觀測(cè)方程進(jìn)行泰勒展開，并取其一階近似項(xiàng)，這樣不可避免會(huì)引入線性化誤差，當(dāng)線性化假設(shè)不成立時(shí)，EKF算法會(huì)導(dǎo)致濾波器性能下降甚至造成發(fā)散［2］。在空間目標(biāo)跟蹤過程中，不能排除目標(biāo)飛行器存在發(fā)生非合作性機(jī)動(dòng)或大幅擾動(dòng)的可能性。此時(shí)，目標(biāo)將偏離原運(yùn)行軌道，且在地心引力場(chǎng)的作用下這種位置和速度的偏離會(huì)呈現(xiàn)復(fù)雜的非線性特征。與此同時(shí)，傳統(tǒng)的最優(yōu)或次優(yōu)濾波算法并不具備良好的抗擾動(dòng)性，致使在突變擾動(dòng)下難以持續(xù)對(duì)系統(tǒng)狀態(tài)進(jìn)行準(zhǔn)確跟蹤，從而導(dǎo)致跟蹤算法呈現(xiàn)跟蹤快速性的不足，甚至因算法不收斂而引起失跟。因此，研究適于軌道機(jī)動(dòng)目標(biāo)跟蹤的濾波估計(jì)算法顯得尤為必要。適于非線性系統(tǒng)的濾波器無味卡爾曼濾波（UKF）通過確定性采樣得到的一組Sigma點(diǎn)，可獲得更多的觀測(cè)假設(shè)，對(duì)系統(tǒng)狀態(tài)的均值和協(xié)方差的估計(jì)更準(zhǔn)確，同時(shí)因采用非線性的狀態(tài)方程或觀測(cè)方程而避免了線性化誤差［3］。在救援目標(biāo)進(jìn)行連續(xù)軌道機(jī)動(dòng)時(shí)，對(duì)其進(jìn)行軌道跟蹤控制，觀測(cè)量必然包含控制加速度信息，加速度偏差可等效為目標(biāo)軌道機(jī)動(dòng)過程中的動(dòng)力學(xué)模型誤差。對(duì)此，可將目標(biāo)機(jī)動(dòng)的控制加速度視作一不確定性干擾因素，用強(qiáng)跟蹤性的UKF濾波算法削弱不確定性干擾因素的影響，實(shí)現(xiàn)對(duì)機(jī)動(dòng)目標(biāo)的自主隨動(dòng)跟蹤過程中的高精度相對(duì)導(dǎo)航。針對(duì)近距離的空間救援目標(biāo)出現(xiàn)機(jī)動(dòng)狀況，本文提出了施援衛(wèi)星的自主跟蹤過程中相對(duì)導(dǎo)航方案及其優(yōu)化策略。

1 相對(duì)軌道動(dòng)力學(xué)模型

對(duì)救援目標(biāo)近程跟蹤與逼近控制來說，建立兩個(gè)飛行器間的相對(duì)軌道動(dòng)力學(xué)模型是控制的基礎(chǔ)，也是相對(duì)導(dǎo)航技術(shù)的基礎(chǔ)［4－6］。

在慣性參考系中，目標(biāo)器T和施援衛(wèi)星S的軌道動(dòng)力學(xué)方程分別為

T，S的相對(duì)幾何關(guān)系如圖1所示。位置關(guān)系可表示為

式中：為相對(duì)救援目標(biāo)的編隊(duì)向量；ΔrT／S為施援衛(wèi)星相對(duì)編隊(duì)向量點(diǎn)的位置矢量；lT／S為施援衛(wèi)星相對(duì)救援目標(biāo)的位置矢量。

圖1 空間兩飛行器相對(duì)幾何關(guān)系Fig.1 Relative geometry between two spacecrafts

式（2）減去式（1），并由式（3）、（4）可得

在目標(biāo)軌道系中，由于兩飛行器相距僅數(shù)十千米，可簡(jiǎn)化攝動(dòng)影響，對(duì)兩器間的引力差進(jìn)行線性化簡(jiǎn)化處理。定義狀態(tài)向量

將相對(duì)軌道動(dòng)力學(xué)方程寫成狀態(tài)空間形式

式中：w為干擾量，且w＝［03×4fd3×1］T；uctrl為相對(duì)動(dòng)力學(xué)方程中的加速度矢量，且uctrl＝uS－uT－；

此處：fd3×1為干擾加速度向量為與跟隨位置矢量有 關(guān) 的 加 速 度 矢 量；A41＝。其中分別為救援目標(biāo)角速度和角加速度；ω0為救援目標(biāo)平均軌道角速度。

對(duì)救援目標(biāo)自主跟蹤問題，因目標(biāo)的機(jī)動(dòng)未知，為能有效跟蹤控制，將目標(biāo)機(jī)動(dòng)隱含在擾動(dòng)項(xiàng)內(nèi)，即將uT隱含在w中，則uctrl＝uctrl＋。

2 相對(duì)導(dǎo)航設(shè)計(jì)

2.1 相對(duì)導(dǎo)航系統(tǒng)結(jié)構(gòu)

施援衛(wèi)星一般均裝有近程跟瞄測(cè)量系統(tǒng)，可對(duì)救援目標(biāo)進(jìn)行視線距離、視線速度及視線方位角的相對(duì)測(cè)量。施援衛(wèi)星的近程相對(duì)導(dǎo)航系統(tǒng)如圖2所示。

圖2 跟瞄相對(duì)導(dǎo)航系統(tǒng)Fig.2 Tracking and pointing relative navigation system

施援衛(wèi)星攜帶的近程測(cè)量設(shè)備可以是主動(dòng)式微波測(cè)量設(shè)備、被動(dòng)式光學(xué)測(cè)量設(shè)備或主備組合一體的組合測(cè)量設(shè)備，能對(duì)求援目標(biāo)進(jìn)行相對(duì)測(cè)量。相對(duì)導(dǎo)航系統(tǒng)一般根據(jù)近程目標(biāo)測(cè)量設(shè)備輸出的兩星間的視線距離、視線距離變化率、視線方位角和視線方位角變化率等測(cè)量信息，選取合理的測(cè)量信息，根據(jù)兩星間的動(dòng)力學(xué)模型，建立合理的測(cè)量方程，設(shè)計(jì)相對(duì)導(dǎo)航濾波器實(shí)現(xiàn)對(duì)兩星間相對(duì)狀態(tài)的估計(jì)，為目標(biāo)近距離跟蹤控制提供信息輸入。

2.2 近程相對(duì)導(dǎo)航系統(tǒng)觀測(cè)量與觀測(cè)方程

2.2.1 近程跟瞄系統(tǒng)觀測(cè)量

假設(shè)導(dǎo)航測(cè)量系統(tǒng)測(cè)量坐標(biāo)系（如圖3所示）與施援衛(wèi)星的體軸系重合。星上跟瞄系統(tǒng)的測(cè)量輸出為：目標(biāo)相對(duì)施援衛(wèi)星的視線距離及其變化率ρ，；視線在施援衛(wèi)星跟瞄測(cè)量坐標(biāo)系中的高低角α及其變化率（定義為視線與其在測(cè)量坐標(biāo)系xy平面的投影間的夾角，抬頭為正）；視線在施援衛(wèi)星跟瞄測(cè)量坐標(biāo)系中的方位角β及其變化率（定義為視線在測(cè)量坐標(biāo)系xy平面的投影與x軸的夾角，偏向＋y軸向?yàn)檎?/p>

圖3 施援星上跟瞄設(shè)備測(cè)量坐標(biāo)系Fig.3 Measuring coordinate system of tracking and pointing on satellite

式中：wρ，wα，wβ，w，w，w為跟瞄測(cè)量噪聲，可視為相互獨(dú)立的高斯分布的白噪聲。

直接測(cè)得ρ，α，β，，，，則可得求援目標(biāo)相對(duì)位置和速度在施援衛(wèi)星體軸系中的分量為

2.2.2 導(dǎo)航系統(tǒng)測(cè)量方程建立

式中：RSb／T為救援目標(biāo)軌道坐標(biāo)系至施援衛(wèi)星體軸系的方向余弦陣；Vk為量測(cè)方程噪聲陣。

因觀測(cè)方程為非線性，有

此處：ωc為施援衛(wèi)星軌道角速度；

綜合可得濾波觀測(cè)方程

2.3 基于目標(biāo)機(jī)動(dòng)的相對(duì)導(dǎo)航濾波器優(yōu)化設(shè)計(jì)

目標(biāo)不機(jī)動(dòng)時(shí)，一般救援星上相對(duì)導(dǎo)航系統(tǒng)方案多采用EKF設(shè)計(jì)，針對(duì)非線性的測(cè)量方程［式（26）］進(jìn)行線性化處理，得到線性化觀測(cè)方程［式（27）］［7］。因此，對(duì)非機(jī)動(dòng)空間目標(biāo)的常規(guī)相對(duì)導(dǎo)航算法，可認(rèn)為觀測(cè)量偏差主要是跟瞄設(shè)備測(cè)量噪聲。對(duì)機(jī)動(dòng)空間目標(biāo)進(jìn)行實(shí)時(shí)檢測(cè)和相對(duì)運(yùn)動(dòng)狀態(tài)估計(jì)，則還需考慮模型的不確定性。因控制加速度的量級(jí)遠(yuǎn)大于其他攝動(dòng)加速度的誤差量級(jí)，觀測(cè)量中還包含了反映目標(biāo)機(jī)動(dòng)過程中的動(dòng)力學(xué)模型誤差。在擾動(dòng)環(huán)境中，傳統(tǒng)濾波算法會(huì)出現(xiàn)收斂慢甚至不收斂的狀況，無法對(duì)實(shí)際相對(duì)運(yùn)動(dòng)狀態(tài)進(jìn)行準(zhǔn)確的估計(jì)和預(yù)測(cè)。為此，需對(duì)星上EKF濾波算法進(jìn)行優(yōu)化，采用一系列對(duì)外部擾動(dòng)具一定容忍度的強(qiáng)跟蹤濾波算法，確保相對(duì)導(dǎo)航算法可在內(nèi)部模型誤差和外部擾動(dòng)持續(xù)存在條件下保持穩(wěn)定，并能在強(qiáng)擾動(dòng)環(huán)境中對(duì)施援衛(wèi)星與目標(biāo)器間的相對(duì)運(yùn)動(dòng)狀態(tài)做出準(zhǔn)確估計(jì)。

另外，采用強(qiáng)跟蹤的UKF濾波算法，可不用對(duì)非線性的測(cè)量方程進(jìn)行線性化，能顯著簡(jiǎn)化算法。

2.3.1 基于強(qiáng)跟蹤的相對(duì)導(dǎo)航UKF濾波算法

UKF法采樣的粒子點(diǎn)（一般稱Sigma點(diǎn)）的個(gè)數(shù)很少，具體數(shù)量取決于選擇的采樣策略［8］。具體步驟為：考慮一個(gè)L維的隨機(jī)變量x，且滿足非線性方程y＝g（x），假設(shè)x有均值和方差Px，為計(jì)算y的統(tǒng)計(jì)特性，現(xiàn)生成2L＋1個(gè)Sigma點(diǎn)xi及相應(yīng)權(quán)值Wi，輸入維數(shù)j＝2，…，n時(shí)，迭代公式為

對(duì)非線性系統(tǒng)

式中：xk為狀態(tài)量；yk為觀測(cè)量；uk為控制量；wk為狀態(tài)過程噪聲；vk為觀測(cè)噪聲。假設(shè)wk，vk為高斯白噪聲，且wk∈N（0，Q），vk∈N（0，R），wk，vk互不相關(guān)。則UKF濾波算法步驟如下。

a）初始化

b）計(jì)算采樣點(diǎn)。用均值－1和方差Pk－1，計(jì)算2L＋1個(gè)采樣點(diǎn)xi，k－1，有

c）時(shí)間更新

一步預(yù)測(cè)方差

系統(tǒng)預(yù)測(cè)觀測(cè)值

d）測(cè)量更新

2.3.2 STUKF

對(duì)定義的非線性系統(tǒng)，使濾波器成為強(qiáng)跟蹤濾波器的一個(gè)充分條件是在線選擇時(shí)變?cè)鲆骊嘖（k＋1），使

式中：k＝0，1，2，…；j＝1，2，…［8-9］。

其中要求殘差序列處處保持相互正交，此處正交性原理的實(shí)質(zhì)是在狀態(tài)殘差估計(jì)最小方差性能指標(biāo)的基礎(chǔ)上再附加一個(gè)輸出殘差序列處處保持相互正交的性能指標(biāo)，物理意義是當(dāng)系統(tǒng)參數(shù)失配時(shí)，可通過殘差序列表現(xiàn)出，通過在線調(diào)整K（k＋1），迫使殘差序列具正交性，從而保持對(duì)實(shí)際系統(tǒng)狀態(tài)的跟蹤。

STUKF算法是在通過在協(xié)方差陣中引入一時(shí)變的漸消因子，一步預(yù)測(cè)誤差協(xié)方差陣變?yōu)?/p>

式中：λk為時(shí)變因子，λk≥1，且

此處：

其中：γk為殘差序列，且γk＝y(tǒng)k－（k＝1，2，3，…）；ζ為遺忘因子，且0＜ζ≤1。ζ能進(jìn)一步提高濾波器的快速跟蹤能力，其值越大，則時(shí)刻k以前的信息所占比例就越小，當(dāng)前殘差向量的影響越突出。

該方法有極強(qiáng)的關(guān)于突變狀態(tài)的跟蹤能力，并在濾波達(dá)到穩(wěn)態(tài)時(shí)，仍保持對(duì)緩變狀態(tài)以及突變狀態(tài)的跟蹤能力。

具體處理時(shí)，對(duì)λk可設(shè)定一上限值λmax，即

3 仿真分析與驗(yàn)證

設(shè)施援衛(wèi)星位于目標(biāo)飛行器后方10km處穩(wěn)定伴飛，跟瞄系統(tǒng)對(duì)目標(biāo)進(jìn)行穩(wěn)定測(cè)量，仿真開始3 000s后，求援目標(biāo)器x軸軌道機(jī)動(dòng)100s后停止機(jī)動(dòng)穩(wěn)定飛行。因本文不涉及跟蹤控制，只是優(yōu)化了原相對(duì)導(dǎo)航算法，仿真過程中相對(duì)控制仍為原設(shè)計(jì)。取仿真條件如下。

a）施援衛(wèi)星和目標(biāo)器在J2000系中的位置速度見表1。

b）跟瞄單機(jī)的測(cè)量精度為（3σ）：測(cè)距15m；測(cè)角0.2°；測(cè)速0.1m／s。近程相對(duì)導(dǎo)航的解算周期為0.4s，軌控周期為0.4s。

c）救援目標(biāo)整器質(zhì)量1 000kg，用于軌道控制機(jī)動(dòng)的發(fā)動(dòng)機(jī)推力5N，軌道控制加速度0.005m／s2。

用優(yōu)化的STUKF濾波算法，施援衛(wèi)星在目標(biāo)衛(wèi)星后方10km跟蹤伴飛及目標(biāo)突然機(jī)動(dòng)過程中，仿真結(jié)果如圖4～7所示。

由圖4、5可知：穩(wěn)定跟蹤約3 000s后，目標(biāo)突然＋x軸機(jī)動(dòng)，軌道抬高，＋x軸相對(duì)狀態(tài)增大，約600s后相對(duì)狀態(tài)又恢復(fù)至10km附近，說明相對(duì)導(dǎo)航采用強(qiáng)跟蹤UKF濾波算法優(yōu)化后，對(duì)目標(biāo)機(jī)動(dòng)時(shí)兩星相對(duì)狀態(tài)進(jìn)行了正確估計(jì)，使相對(duì)控制能在目標(biāo)機(jī)動(dòng)后仍能對(duì)目標(biāo)進(jìn)行跟蹤伴飛。

表1 初始位置和速度Tab.1 Initial position and velocity

圖4 兩星相對(duì)位置Fig.4 Relation position of two spacecrafts

圖5 兩星相對(duì)速度Fig.5 Relation velocity of two spacecrafts

由圖6、7可知：相對(duì)導(dǎo)航對(duì)三軸的相對(duì)位置估計(jì)和相對(duì)速度估計(jì)精度較穩(wěn)定，三軸相對(duì)位置估計(jì)精度優(yōu)于±20m，三軸相對(duì)速度估計(jì)精度約±0.1m／s。

圖6 近程兩星相對(duì)導(dǎo)航位置估計(jì)誤差Fig.6 Relative position estimation error of two spacecrafts

圖7 近程兩星相對(duì)速度估計(jì)誤差Fig.7 Relative velocity estimation error of two spacecrafts

綜上，可認(rèn)為用STUKF強(qiáng)跟蹤濾波器，對(duì)突發(fā)增大的外部干擾具一定的強(qiáng)跟蹤能力，相對(duì)導(dǎo)航濾波精度穩(wěn)定，并未出現(xiàn)振蕩。

4 結(jié)束語

針對(duì)施援衛(wèi)星的近程相對(duì)系統(tǒng)，本文對(duì)常規(guī)使用的近程相對(duì)導(dǎo)航EKF算法進(jìn)行了濾波器的優(yōu)化設(shè)計(jì)。將星上近程跟瞄設(shè)備作為量測(cè)，對(duì)目標(biāo)進(jìn)行視線角距的測(cè)量，自主獲取目標(biāo)量測(cè)信息，并對(duì)兩星的相對(duì)狀態(tài)進(jìn)行精確跟蹤和估計(jì)，以作為近程跟蹤控制的信息輸入。設(shè)計(jì)了具強(qiáng)跟蹤性的STUKF濾波器，優(yōu)化了近程相對(duì)導(dǎo)航算法，目標(biāo)機(jī)動(dòng)時(shí)相對(duì)導(dǎo)航系統(tǒng)在不確定干擾影響下，仍能根據(jù)跟瞄系統(tǒng)的測(cè)量對(duì)兩器的相對(duì)狀態(tài)進(jìn)行高精度估計(jì)。采用STUKF濾波算法，無需對(duì)非線性測(cè)量方程進(jìn)行線性化處理，簡(jiǎn)化了運(yùn)算，有一定的工程參考價(jià)值。

［1］ 李元?jiǎng)P.空間救援目標(biāo)自主隨動(dòng)跟蹤與控制研究［D］.上海：上海交通大學(xué)，2010.

［2］ KALMAN R E.A new approach to linear filtering and prediction theory［J］.Trans ASME Journal of Basic Eng，1960，82D：35-46.

［3］ JULIER S J，UHLMANN J K.A general method for approving nonlinear transformations of probability distributions［C］／／Proceeding of the 1996American Control COnference.［s.l.］：［s.n.］，1996：1628-1632.

［4］ 邢光謙，馮 剛，張寶恒，等.一種追蹤飛行器近程逼近目標(biāo)及保持伴飛的實(shí)現(xiàn)方法［P］.中國：ZL 200910125447.

［5］ 車汝才，張洪華，跟蹤空間非合作目標(biāo)的一種相對(duì)軌道確定方法研究［J］.中國空間科學(xué)技術(shù)，2007，27（4）：7-13.

［6］ 林 健，林曉輝，曹喜濱.基于自適應(yīng)卡爾曼濾波的機(jī)動(dòng)目標(biāo)自主軌道確定［J］.上海航天，2008，25（2）：14-18.

［7］ 劉建業(yè)，趙 偉，熊 智.導(dǎo)航系統(tǒng)理論及應(yīng)用［M］.南京：南京航空航天大學(xué)出版社，2005：248-281.

［8］ 劉 錚.UKF算法及其改進(jìn)算法的研究［D］.長(zhǎng)沙：中南大學(xué)，2009.

［9］ 劉華普，孔金生，董文麗.改進(jìn)的強(qiáng)跟蹤自適應(yīng)濾波信息融合方法［J］.傳感器與儀器儀表，2007，23（7）：141-143.