柱面坐標(biāo)系下航天器僅測角相對導(dǎo)航算法

龔柏春，張德港，張偉夫，苑艷華，陳修橋

（1. 南京航空航天大學(xué)，南京210016；2. 上海衛(wèi)星工程研究所，上海 201109；

3. 北京控制與電子技術(shù)研究所，北京100038；4. 中國人民解放軍32032部隊，北京100094）

當(dāng)前，地球同步軌道的環(huán)境越來越復(fù)雜，大量增加的失效衛(wèi)星、故障衛(wèi)星、碎片以及太空武器等（統(tǒng)稱為空間非合作目標(biāo)）給在軌現(xiàn)役衛(wèi)星帶來各種主被動安全威脅。為了應(yīng)對空間非合作目標(biāo)的安全威脅，各國都在大力發(fā)展包括空間態(tài)勢感知、在軌服務(wù)等在內(nèi)的空間安全技術(shù)，而對目標(biāo)進(jìn)行及時、長時、精確的相對導(dǎo)航（或稱相對軌道確定）則是關(guān)鍵前提技術(shù)。

通常，能用于對空間非合作目標(biāo)進(jìn)行相對測量的敏感器包括微波雷達(dá)、激光雷達(dá)、光學(xué)相機等。其中，微波雷達(dá)、激光雷達(dá)等系統(tǒng)因為系統(tǒng)復(fù)雜、造價昂貴、能耗大等缺點難以在快速響應(yīng)的中小型衛(wèi)星上配備。而光學(xué)相機因為其具有簡單可靠、體積小、重量輕、功耗低、全自主等特點已經(jīng)被在軌衛(wèi)星廣泛應(yīng)用。同時，光學(xué)相機的無源測量也具有很好的隱蔽性，相比雷達(dá)更加適合在空間攻防領(lǐng)域的應(yīng)用。然而，也正是因為光學(xué)相機的無源測量，使得其只能獲取目標(biāo)的視線角信息，缺少測距信息，這就產(chǎn)生了僅測角相對導(dǎo)航的狀態(tài)不可觀測/弱可觀測的問題[1]。國內(nèi)外的學(xué)者從多個角度對該問題進(jìn)行了研究。

Wang[2]和Chen[3]等人提出了一種雙視線測量的僅測角相對導(dǎo)航實現(xiàn)方法，Han等[4]對此方法提出了顯著提高距離狀態(tài)可觀測度和估計性能的優(yōu)化方法,該方法通過配置輔助測量航天器形成測量基線，從而來引入距離信息，雙星編隊獲取測量基線“邊”，然后與雙星同時測量的視線角一起以“角邊角”的方式確定三角形的形狀，也就解決了相對距離在可觀測性方面的問題，但是這種方法需要至少2顆衛(wèi)星，增加了成本支出。Anjaly[4]提出了利用軌道機動信息進(jìn)行距離估計的思想，研究了可觀測性最優(yōu)的機動方式，但是軌道機動法約束了實際操作任務(wù)中相對軌道制導(dǎo)的自由度，同時也帶來了更多的燃料消耗與安全風(fēng)險，Gao等[6]驗證了軌道機動對可觀測性的影響，Klein[7]提出了一種利用測量相機安裝存在偏離航天器質(zhì)心的現(xiàn)象，解決距離可觀測性的新思路和新方法——相機偏置法，在相機偏置法的基礎(chǔ)上，Gong等[8]建立了基于無跡卡爾曼濾波的僅測角相對導(dǎo)航算法，Du等[9]提出了快速獲得僅測角相對定軌解的方法。相機偏心距離足夠大的時候就可以提供距離的可觀測性，但是相機偏置法有效作用范圍取決于相機偏離航天器質(zhì)心的距離，通常這個距離比較小，因此只適用于近距離探測的情況。Kaufman等[10]從笛卡爾坐標(biāo)系下二階非線性相對運動動力學(xué)出發(fā)，通過采用高階李導(dǎo)數(shù)研究了僅測角相對軌道確定的非線性可觀測性問題。Li等[11]說明了通過非線性模型可以解決原本僅測角相對導(dǎo)航在笛卡爾坐標(biāo)系下不可觀測的問題。Gaias等[12]在相對軌道要素模型下探討了僅測角相對導(dǎo)航問題，分離了不可觀測的軌道要素。Han等[12]建立了球坐標(biāo)下的狀態(tài)方程和觀測方程，采用了UKF算法，推導(dǎo)了濾波參數(shù)的計算方法。然而，非線性動力學(xué)模型的非線性強弱會直接決定可觀性的強弱，如果非線性太弱，則產(chǎn)生的效果很容易淹沒在測量誤差之中。

綜上，現(xiàn)有方法主要分成多敏感器協(xié)同測量、軌道機動輔助、敏感器桿臂效應(yīng)輔助以及非線性動力學(xué)等四類。多敏感器法需要至少兩顆衛(wèi)星協(xié)同測量，且對構(gòu)型有一定要求，增加了經(jīng)濟成本；軌道機動輔助法增加了燃料消耗，也帶來了碰撞風(fēng)險；敏感器桿臂效應(yīng)法限于桿臂長度僅能適用于公里級的近程場景；非線性動力學(xué)法從模型的角度提供可觀測性，犧牲一定的計算量來實現(xiàn)僅測角導(dǎo)航，但這對于目前高速發(fā)展的計算能力來說已經(jīng)不是問題。

因此，本文從利用非線性動力學(xué)解決僅測角導(dǎo)航可觀測性的角度出發(fā)，研究在軌道曲率捕獲能力更強的曲線坐標(biāo)系下建立相對運動動力學(xué)模型并用于僅測角相對導(dǎo)航。下面將先建立柱面坐標(biāo)系下的航天器相對運動動力學(xué)模型以及視線角測量模型，然后進(jìn)行相對軌道狀態(tài)的可觀測性分析，接著引入平方根容積卡爾曼濾波建立非線性濾波算法、并設(shè)計長航時導(dǎo)航方案，最后對所提出的算法進(jìn)行數(shù)值仿真驗證。

1 相對運動動力學(xué)模型

美國學(xué)者Geller等[14]在柱面坐標(biāo)系下建立了二維平面情況下的航天器相對動力學(xué)模型，本文在此基礎(chǔ)上進(jìn)行拓展，在柱面坐標(biāo)系下建立三維空間的相對運動動力學(xué)模型。如圖1和圖2所示，首先，建立一個與主星（Chief）初始軌道平面重合的固定參考軌道平面，這個平面的法線將用單位矢量iz表示，在參考平面上的分量用2個極坐標(biāo)的單位矢量iρc、iθc表示。因此，三維空間的位置矢量可由iρc、iz和iθc線性組合表示。

圖1 柱面坐標(biāo)系下的X-Y平面示意圖Fig.1 Schematic diagram of 2D in cylindrical coordinate

圖2 柱面坐標(biāo)系下的三維示意圖Fig.2 Schematic diagram of 3D in cylindrical coordinate

以主星為例，將地心與衛(wèi)星的連線用矢量表示，并求1階和2階導(dǎo)數(shù)后可得：

將這個運動方程與二體動力學(xué)方程在柱面坐標(biāo)系下的表示形式做比對即可得到柱面坐標(biāo)系下的軌道動力學(xué)方程：

類似的，可以得到從星（deputy）在柱面坐標(biāo)系下的軌道動力學(xué)方程。

定義相對運動狀態(tài)為：

那么相對狀態(tài)的二階導(dǎo)數(shù)為：

令柱面坐標(biāo)系下的相對軌道運動狀態(tài)量為：

則相對運動狀態(tài)模型可以寫成如下的非線性形式。

后續(xù)的導(dǎo)航濾波估計中將采用式(8)進(jìn)行相對軌道狀態(tài)的演化。

2 量測方程

設(shè)相機測量得到的視線角為α和β，根據(jù)圖1、圖2所示的幾何關(guān)系可以得到測量角與相對狀態(tài)的關(guān)系式如下：

最終的測量方程建模為：

其中，α和β相對角度的真實值，vθ、wθ是對應(yīng)的測量噪聲，通常假設(shè)為零均值高斯白噪聲，d是主星和從星在主星軌道平面上投影的距離，可由其他參數(shù)計算得到，即。

對于主星軌道有傾角的情況，為了方便計算可按圖3所示關(guān)系對慣性系與近焦點系進(jìn)行坐標(biāo)轉(zhuǎn)化，從而與柱面坐標(biāo)系建立聯(lián)系。其中，近焦點坐標(biāo)系是以主星軌道所在平面為XY平面建立近焦點坐標(biāo)系，該坐標(biāo)系的坐標(biāo)原點位于地心，Z軸與軌道角動量方向重合，X軸指向近地點，Y軸與XZ平面垂直構(gòu)成右手系；地心慣性坐標(biāo)系是X軸指向春分點，Z軸與地球旋轉(zhuǎn)軸重合，向北為正，Y軸與XZ平面垂直構(gòu)成右手系。

圖3 慣性系與近焦點系的關(guān)系示意圖Fig.3 Relation of the inertial system and the near focal system

3 可觀測性分析

通常，對非線性系統(tǒng)進(jìn)行可觀測性分析可采用李導(dǎo)數(shù)的方式進(jìn)行。但是如式(8)(10)所示的柱面坐標(biāo)系下相對導(dǎo)航模型是包含三角函數(shù)在內(nèi)的強非線性模型，利用高階李導(dǎo)數(shù)證明可觀測性分析時會包含大量復(fù)雜的三角函數(shù)計算，很難在不進(jìn)行大量線性化的情況下獲得解析解，而線性化又會略去用來提供僅測角可觀測性的非線性項。因此，下面只對柱面系下的僅測角導(dǎo)航模型的可觀測性進(jìn)行定性分析。

文獻(xiàn)[1]中總結(jié)了僅測角相對軌道不可觀測的四個原因，其中之一便是線性化動力學(xué)模型假設(shè)，也就是說采用線性化動力學(xué)模型進(jìn)行軌道演化時僅測角相對導(dǎo)航是不可觀測的。而本文中柱面系下的僅測角導(dǎo)航模型是強非線性的，那么理論上可觀測性也越強。接下來分析模型對軌道曲率的捕獲能力。測量角有α和β，α和β的測量方程如式(10)所示，將α和β分別對ρrel、θrel、zrel求導(dǎo)，可以得到：

由式(11)-(13)可以看出，與采用常規(guī)的直角坐標(biāo)系下線性動力學(xué)模型進(jìn)行僅測角相對導(dǎo)航不同，當(dāng)在柱面動力學(xué)模型下進(jìn)行ρrel和θrel、zrel演化時，基于α和β角的視線矢量與狀態(tài)量之間的關(guān)系是強非線性的，所以根據(jù)α、β的變化就可以捕捉到ρrel和θrel、zrel的改變，這在一定程度上反應(yīng)了相對狀態(tài)的可觀測性。

另外，主星位于從星低軌和高軌時，具有不同的觀測效果，如圖4和圖5所示。

圖4 低軌觀測高軌示意圖Fig.4 Case of low orbit observes high orbit

圖5 高軌觀測低軌示意圖Fig.5 Case of high orbit observes low orbit

由圖4和圖5可以看到當(dāng)主星位于低軌道時，從主星觀測從星的視線矢量與從星的軌道只有一個交點。而當(dāng)主星位于高軌道時，觀測位于低軌道的從星，則視線會在從星軌道上有兩個交點（相位角不同），且當(dāng)主星位于高軌道時，從星在軌道上的位置和觀測角的變化關(guān)系更為復(fù)雜。也就是說主星位于低軌道時，系統(tǒng)具有更好的觀測性。

4 基于SCKF的長航時濾波估計方案

由式(8)和(10)表示的僅測角導(dǎo)航系統(tǒng)具有強非線性，常用的擴展卡爾曼濾波器（Extended Kalman Filter, EKF）存在線性化誤差累積導(dǎo)致的精度不高、穩(wěn)定性差等問題，難以適用于強非線性系統(tǒng)[15]；無跡卡爾曼濾波(Unscented Kalman Filter, UKF)能夠適用于非線性導(dǎo)航系統(tǒng)[16,17]，但是只有二階精度。因此，本文采用適用于非線性系統(tǒng)的、精度更高的平方根容積卡爾曼濾波算法(SCKF)。

下面先簡單介紹SCKF的主要計算步驟，然后給出長航時條件下的僅測角相對導(dǎo)航濾波方案。

4.1 SCKF濾波算法

容積卡爾曼濾波（Cubature Kalman Filter, CKF）算法是Arasaratnam和Haykin 2009年提出的一種非線性濾波算法[18]，其核心是采用三階球面-相徑容積規(guī)則近似非線性函數(shù)傳遞的后驗均值和協(xié)方差，從數(shù)值積分的角度來進(jìn)行近似高斯積分。在采用CKF對模型進(jìn)行仿真時，協(xié)方差矩陣在運算過程中可能會出現(xiàn)非正定從而導(dǎo)致算法出錯，而采用CKF的平方根形式SCKF，則可以避免這種問題[18]。

在CKF的平方根形式中，誤差協(xié)方差的平方根Sk-1∣k-1是可用的，SCKF的算法流程如下所示：

時間更新：

1）求解容積點(i=1,2…m)

2）求解傳播容積點(i=1,2…m)

3）估計狀態(tài)預(yù)測值

4）估計預(yù)測誤差協(xié)方差的平方根因子

其中，SQ,k-1是Qk-1的平方根因子，關(guān)系式如下：

測量更新：

1）求解容積點(i=1,2 …m)

2）求解傳播容積點(i=1,2…m)

3）計算測量預(yù)測值

4）計算新的協(xié)方差矩陣平方根

其中，SR,k是Rk的平方根因子，關(guān)系式如下：

5）計算互協(xié)方差矩陣

6）計算濾波增益

7）狀態(tài)的測量更新

8）誤差協(xié)方差的平方根因子更新

這里將一般的三角化算法(如QR分解)表示為S=Tria(A)，其中S是下三角矩陣。矩陣A和S的關(guān)系如下：假設(shè)R是通過對AT的QR分解得到的上三角矩陣，則有S=RT。

4.2 長航時濾波方案

在導(dǎo)航時長較長的情況下，誤差協(xié)方差的平方根形式的矩陣S會隨著時間推移逐漸趨近于0，從而使得濾波增益也逐漸趨于零，那么測量更新將失去對預(yù)測狀態(tài)的校正作用。也就是說，在長航時的情況下，采用常規(guī)濾波方案相對軌道狀態(tài)誤差最終會因為初值誤差、模型噪聲的累積而發(fā)散，這與系統(tǒng)可觀測性無關(guān)，是濾波算法使然。同時，當(dāng)主從星軌道存在高度差時，隨著長時間的推移，主從星間的相位角之差會減小至零，根據(jù)測量模型(10)可知，當(dāng)相位角之差趨于零時，視線角也將會處于奇異狀態(tài)，必將使估計誤差發(fā)散。

為了實現(xiàn)對非合作目標(biāo)的長航時穩(wěn)定跟蹤導(dǎo)航，必須解決上述兩個問題。有研究表明在長時間導(dǎo)航中濾波器定時重新啟動會提升性能[19]，因此本文設(shè)計如下兩步操作方案：

（1）設(shè)置測量更新開關(guān)，當(dāng)相對角度過小時（具體設(shè)置由仿真經(jīng)驗設(shè)置）關(guān)閉測量更新，直到超出該范圍時重新打開測量更新開關(guān)；

（2）設(shè)置濾波器定時重啟開關(guān)，即濾波器定時重新初始化，以開關(guān)開啟上一時刻的狀態(tài)值作為當(dāng)前時刻的濾波初值，重新設(shè)置估計誤差協(xié)方差平方根形式的矩陣S。濾波器重啟周期也將通過數(shù)值仿真進(jìn)行優(yōu)選。

5 數(shù)值仿真與分析

5.1 參數(shù)設(shè)置

本文以GEO軌道鄰域的態(tài)勢感知為背景設(shè)置相對運動。假設(shè)主星運行在近圓軌道上，軌道參數(shù)如表1所示。從星在主星上方70公里高的軌道上，軌道偏心率、軌道傾角、升交點赤經(jīng)和近地點幅角等參數(shù)和主星相同，僅通過改變真近點角設(shè)置兩種相對運動工況。第一種工況從星初始時刻的真近點角落后主星8 °，初始時刻主從星間距離大約為5900 km，仿真周期內(nèi)雙星之間的距離一直增大；第二種工況從星初始時刻真近點角領(lǐng)先主星2 °，初始時刻主從星間距離大約為1500 km，仿真周期內(nèi)星間距離先減小后增大。

表1 主星的軌道參數(shù)設(shè)置Tab.1 Chief’s orbit parameter settings

仿真中選用差、中、好三種不同精度的光學(xué)相機進(jìn)行算法驗證，對應(yīng)的測角誤差標(biāo)準(zhǔn)差分別為10-3rad、10-4rad、10-5rad。參考軌道由軌道動力學(xué)模型積分得到，積分步長為1 s，總仿真時長為5個軌道周期。濾波系統(tǒng)協(xié)方差平方根形式的矩陣重置周期設(shè)置為1~24小時，每一種增加1小時，重置的S矩陣為初始狀態(tài)與參考狀態(tài)之差的絕對值所構(gòu)成的對角矩陣。設(shè)定初始距離的不確定性為5 km，測量更新關(guān)閉的相對角度θrel邊界條件設(shè)置為 ± 0.01 °。

此外，由于僅測角相對導(dǎo)航的核心問題是距離的可觀測性問題，因此后續(xù)的仿真結(jié)果分析中將以相對距離估計誤差的百分比作為指標(biāo)。

5.2 仿真結(jié)果與分析

第一種工況仿真的相對距離估計誤差百分比如圖6所示，光學(xué)相機的測角誤差的標(biāo)準(zhǔn)差為10-5rad，其中1~24 h表示S矩陣重置周期。由圖6可知，在重置周期為1 h的條件下，仿真結(jié)束時相對距離估計誤差接近20%。而重置周期為2 h時，終端時刻估計誤差下降到約3%，其他情況下的估計誤差均在1%以內(nèi)。這里必須說明的是，圖6中誤差百分比曲線之所以顯示出一定的發(fā)散情況并非是系統(tǒng)可觀測性問題，而是因為兩星之間的距離一直在增加，同樣的測角誤差對應(yīng)的相對位置誤差會更大。當(dāng)相對距離減少時將不會出現(xiàn)這種情況，工況二的仿真結(jié)果將證明這個結(jié)論。

圖6 工況1的相對距離估計百分比誤差Fig.6 The percent error of range estimate for Case 1

不同測角精度下的終端時刻估計誤差百分比統(tǒng)計如表2所示（5~10 h重置周期的情況）。顯然，測角精度越高時導(dǎo)航性能越好。由表2可知，重置周期為8 h（約1/3個主星軌道周期）時估計精度最高，10-3、10-4、10-5rad的測角精度時距離估計誤差分別為0.181%、0.061%和0.005%。此外，8 周期下三種精度相機的全程估計誤差曲線如圖7所示，可見10-5rad測角精度時估計誤差收斂非常穩(wěn)定，沒有出現(xiàn)緩慢增長趨勢，收斂后距離誤差始終維持在星間距離的0.01%以下。

表2 工況1距離估計百分比誤差統(tǒng)計Tab.2 The statistic of percent error for Case 1

圖7 工況1：不同測角精度的估計誤差（重置周期8 h）Fig.7 Case 1：Estimate error for different camera level (8 h)

第二種工況仿真的相對距離估計誤差百分比如圖8所示(測角精度10-5rad)。由圖可知，正如工況一中分析的一樣，工況二的相對距離估計誤差整體上呈現(xiàn)收斂趨勢，但是從約2個軌道周期后開始有發(fā)散增長的趨勢，這是因為約經(jīng)過2個周期后主星與從星的相位角之差趨近為0，觸發(fā)了長航時導(dǎo)航濾波方案中設(shè)定的測量更新關(guān)閉開關(guān)，此時相對軌道單純由相對軌道動力學(xué)模型進(jìn)行演化。而在此之后相位角開始逐漸增大，大于設(shè)定的閾值之后測量更新開啟，增長的估計誤差又被重新拉回到收斂趨勢。收斂后相對距離估計誤差整體上在0.4%以下，對應(yīng)的距離誤差約為200 m以內(nèi)。

圖8 工況2的相對距離估計百分比誤差Fig.8 The percent error of range estimate for Case 2

此外，與工況一相關(guān)，重置周期為8 h時估計精度最高，如表3所示。對應(yīng)的三種精度相機的全程估計誤差曲線如圖9所示，可見10-5rad測角精度時估計誤差收斂非常穩(wěn)定。

圖9 工況2：不同測角精度的估計誤差（重置周期8 h）Fig.9 Case 2：Estimate error for different camera level (8 h)

表3 工況2距離估計百分比誤差統(tǒng)計Tab.3 The statistic of percent error for Case 2

綜上，采用本文設(shè)計的相對導(dǎo)航方案，可以實現(xiàn)非合作目標(biāo)無源探測的長航時跟蹤導(dǎo)航。采用10-5rad量級的測角相機時，距離估計精度可達(dá)星間距0.01%；10-4rad量級的測角相機時，距離估計精度可保持在星間距0.1%以內(nèi)。

6 結(jié) 論

本文面向空間非合作目標(biāo)無源探測任務(wù)，針對笛卡爾坐標(biāo)系下僅測角定軌存在相對軌道狀態(tài)不可觀測的問題，利用柱面坐標(biāo)系的軌道曲率捕獲能力，建立了柱面坐標(biāo)系下的相對軌道動力學(xué)模型，并基于該模型研究了僅測角導(dǎo)航算法，實現(xiàn)了基于僅有角度測量的空間非合作目標(biāo)長航時跟蹤定軌。在滿足可觀測條件時，即使感知衛(wèi)星與目標(biāo)之間的距離達(dá)到數(shù)千公里，長航時相對定軌誤差依然可以保持在公里級至幾百米的范圍之內(nèi)。下一步將分別針對高軌和低軌情況引入不同的攝動項，建立更加貼近工程實際的動力學(xué)模型，并研究僅測角相對導(dǎo)航的性能。