柱面坐標系下航天器僅測角相對導航算法

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

（1. 南京航空航天大學，南京210016；2. 上海衛星工程研究所，上海 201109；

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

當前，地球同步軌道的環境越來越復雜，大量增加的失效衛星、故障衛星、碎片以及太空武器等（統稱為空間非合作目標）給在軌現役衛星帶來各種主被動安全威脅。為了應對空間非合作目標的安全威脅，各國都在大力發展包括空間態勢感知、在軌服務等在內的空間安全技術，而對目標進行及時、長時、精確的相對導航（或稱相對軌道確定）則是關鍵前提技術。

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

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

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

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

1 相對運動動力學模型

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

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

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

以主星為例，將地心與衛星的連線用矢量表示，并求1階和2階導數后可得：

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

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

定義相對運動狀態為：

那么相對狀態的二階導數為：

令柱面坐標系下的相對軌道運動狀態量為：

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

后續的導航濾波估計中將采用式(8)進行相對軌道狀態的演化。

2 量測方程

設相機測量得到的視線角為α和β，根據圖1、圖2所示的幾何關系可以得到測量角與相對狀態的關系式如下：

最終的測量方程建模為：

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

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

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

3 可觀測性分析

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

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

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

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

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

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

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

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

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

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

4.1 SCKF濾波算法

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

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

時間更新：

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

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

3）估計狀態預測值

4）估計預測誤差協方差的平方根因子

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

測量更新：

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

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

3）計算測量預測值

4）計算新的協方差矩陣平方根

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

5）計算互協方差矩陣

6）計算濾波增益

7）狀態的測量更新

8）誤差協方差的平方根因子更新

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

4.2 長航時濾波方案

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

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

（1）設置測量更新開關，當相對角度過小時（具體設置由仿真經驗設置）關閉測量更新，直到超出該范圍時重新打開測量更新開關；

（2）設置濾波器定時重啟開關，即濾波器定時重新初始化，以開關開啟上一時刻的狀態值作為當前時刻的濾波初值，重新設置估計誤差協方差平方根形式的矩陣S。濾波器重啟周期也將通過數值仿真進行優選。

5 數值仿真與分析

5.1 參數設置

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

表1 主星的軌道參數設置Tab.1 Chief’s orbit parameter settings

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

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

5.2 仿真結果與分析

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

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

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

表2 工況1距離估計百分比誤差統計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)。由圖可知，正如工況一中分析的一樣，工況二的相對距離估計誤差整體上呈現收斂趨勢，但是從約2個軌道周期后開始有發散增長的趨勢，這是因為約經過2個周期后主星與從星的相位角之差趨近為0，觸發了長航時導航濾波方案中設定的測量更新關閉開關，此時相對軌道單純由相對軌道動力學模型進行演化。而在此之后相位角開始逐漸增大，大于設定的閾值之后測量更新開啟，增長的估計誤差又被重新拉回到收斂趨勢。收斂后相對距離估計誤差整體上在0.4%以下，對應的距離誤差約為200 m以內。

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

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

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

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

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

6 結 論

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