基于自適應(yīng)均方根EKF的慣性輔助航天器僅測(cè)角導(dǎo)航方法

杜榮華，廖文和，張 翔

（南京理工大學(xué) 機(jī)械工程學(xué)院，南京 210094）

隨著航天技術(shù)的持續(xù)發(fā)展，航天器任務(wù)范圍也從傳統(tǒng)的通信、導(dǎo)航和遙感等應(yīng)用領(lǐng)域向在軌服務(wù)[1]、空間碎片清理[2]、編隊(duì)飛行[3]和深空探測(cè)[4]等新型航天器任務(wù)拓展。這些新型航天器任務(wù)都涉及了針對(duì)空間非合作目標(biāo)的交會(huì)操作，相關(guān)技術(shù)也成為了國際競(jìng)爭(zhēng)前沿科技。

為了成功實(shí)施針對(duì)空間非合作目標(biāo)的交會(huì)操作，亟需提升航天器的自主運(yùn)行能力。這其中涉及空間非合作目標(biāo)的自主檢測(cè)、自主相對(duì)導(dǎo)航和自主制導(dǎo)與控制等方面的研究?jī)?nèi)容。本文重點(diǎn)研究了其中的自主相對(duì)導(dǎo)航問題，即航天器能夠通過星載相對(duì)導(dǎo)航傳感器，自主獲取空間非合作目標(biāo)相對(duì)于航天器的相對(duì)狀態(tài)信息。目前，常用來測(cè)量空間物體之間相對(duì)狀態(tài)信息的有源主動(dòng)式傳感器（例如激光和微波雷達(dá)等），由于它們功耗、質(zhì)量和體積都比較大，使得它們無法應(yīng)用在微小衛(wèi)星平臺(tái)上，而無源被動(dòng)式傳感器（例如可見光和紅外相機(jī)等）在這一方面具有較大的優(yōu)勢(shì)[5,6]，因?yàn)樗鼈儗?duì)航天器的總質(zhì)量和功耗的設(shè)計(jì)影響很小[7]，且適用于各種航天器間距。此外，許多航天器配備了星敏感器，如果方向合適，可用于拍攝其視野范圍內(nèi)的空間目標(biāo)，并根據(jù)測(cè)得的視線矢量信息執(zhí)行相對(duì)導(dǎo)航任務(wù)[8]。其中，僅測(cè)角導(dǎo)航技術(shù)代表了多種高級(jí)分布式空間系統(tǒng)的明確使能技術(shù)，包括空間態(tài)勢(shì)感知、自主交會(huì)對(duì)接、分布式孔徑科學(xué)和涉及空間非合作目標(biāo)的在軌維修等任務(wù)[9]。

僅測(cè)角導(dǎo)航技術(shù)指的是通過單個(gè)相機(jī)在一段時(shí)間內(nèi)測(cè)量航天器和空間目標(biāo)之間的視線矢量，以推導(dǎo)出它們之間的相對(duì)狀態(tài)信息。因?yàn)閱文肯鄼C(jī)深度信息的缺失，僅測(cè)角導(dǎo)航存在星間距離不可觀測(cè)的問題[10-12]。目前，已經(jīng)有許多學(xué)者研究了提高僅測(cè)角導(dǎo)航系統(tǒng)可觀性的方法，例如軌道機(jī)動(dòng)法[13]、相機(jī)偏置法[14]、非線性動(dòng)力學(xué)法[15]和多星多傳感器法[16]。由于視覺傳感器存在視場(chǎng)角小和數(shù)據(jù)更新率低等缺點(diǎn)，而將航天器上既有的慣性測(cè)量單元（Inertial Measurement Unit,IMU）引入相對(duì)導(dǎo)航系統(tǒng)能夠克服單一測(cè)量系統(tǒng)存在的缺陷，具有功耗小、輸出頻率高、延遲低和精度高等優(yōu)勢(shì)。目前，已經(jīng)有部分學(xué)者開展了視覺和慣性組合相對(duì)導(dǎo)航方法的研究。Zhang[17]等針對(duì)空間交會(huì)對(duì)接地面混合懸浮微重力模擬實(shí)驗(yàn)中自主水下航行器的高精度實(shí)時(shí)導(dǎo)航問題，提出了一種復(fù)雜多介質(zhì)環(huán)境下視覺/慣性自適應(yīng)組合導(dǎo)航方案和算法。Cai[18]分析了CCD相機(jī)所拍攝的序列影像中的特征點(diǎn)觀測(cè)量，與慣性導(dǎo)航系統(tǒng)進(jìn)行組合，提出了基于多狀態(tài)約束的視覺/慣性組合導(dǎo)航算法，實(shí)現(xiàn)了在無GNSS環(huán)境下利用低成本慣性器件的導(dǎo)航定位。

針對(duì)航天器對(duì)空間非合作目標(biāo)交會(huì)操作中的自主相對(duì)導(dǎo)航問題，本文提出了一種基于自適應(yīng)均方根EKF的慣性輔助航天器僅測(cè)角導(dǎo)航方法。通過將慣性測(cè)量數(shù)據(jù)引入僅測(cè)角導(dǎo)航系統(tǒng)，不僅克服了視覺傳感器輸出頻率低、視場(chǎng)角小和易受外界干擾的缺陷，還可以利用視覺傳感器長(zhǎng)周期、高精度的測(cè)量性能來修正IMU的漂移累積誤差。首先，推導(dǎo)了一種基于CW方程[19]的僅測(cè)角導(dǎo)航模型。然后，基于誤差狀態(tài)推導(dǎo)了一種僅測(cè)角/慣性組合相對(duì)導(dǎo)航模型。接著，基于Cholesky分解得到組合導(dǎo)航系統(tǒng)的狀態(tài)協(xié)方差矩陣，并引入“新息”協(xié)方差匹配方法對(duì)測(cè)量噪聲進(jìn)行在線自適應(yīng)調(diào)整，構(gòu)建了一種自適應(yīng)均方根EKF，對(duì)該組合導(dǎo)航系統(tǒng)狀態(tài)進(jìn)行動(dòng)態(tài)濾波估計(jì)，并采用Fisher信息矩陣對(duì)該組合導(dǎo)航系統(tǒng)的可觀性進(jìn)行了評(píng)估。最后，在搭建的半物理仿真平臺(tái)上對(duì)提出的基于自適應(yīng)均方根EKF的慣性輔助航天器僅測(cè)角導(dǎo)航方法進(jìn)行了仿真分析與驗(yàn)證，證明了該方法的有效性。

1 僅測(cè)角導(dǎo)航模型

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

采用著名的CW方程作為主星和副星之間自主交會(huì)的相對(duì)動(dòng)力學(xué)模型。該CW方程建立在笛卡爾坐標(biāo)系“car”中，如圖1所示，其中x軸背離地心，z軸垂直主星的軌道平面，y軸在軌道平面內(nèi)且與x軸和z軸滿足右手定則。若不考慮攝動(dòng)，則CW方程為：

圖1 笛卡爾坐標(biāo)系“car”的定義Fig.1 Definition of Cartesian coordinate system “car”

其中，ω為主星的軌道角速率，x、y和z為相對(duì)位置分量，ux、uy和uz為副星的推力加速度分量。

假設(shè)副星沒有進(jìn)行軌道機(jī)動(dòng)操縱，即ux、uy和uz均為零，則對(duì)方程(1)進(jìn)行積分之后得到CW方程的狀態(tài)轉(zhuǎn)移矩陣為：

1.2 視線測(cè)量模型

在定義視線（Line-of-Sight, LoS）矢量之前，首先需要先定義相機(jī)坐標(biāo)系“cam”。為簡(jiǎn)單起見，在不失一般性的情況下，假設(shè)相機(jī)視軸與航天器反飛行方向?qū)?zhǔn)。在這個(gè)假設(shè)下，相機(jī)坐標(biāo)系“cam”下的相對(duì)位置矢量rcam和笛卡爾坐標(biāo)系“car”下的相對(duì)位置矢量r之間的關(guān)系為：

LoS矢量包括一組方位角α和俯仰角ε，可以表示為相機(jī)坐標(biāo)系“cam”下的相對(duì)位置矢量rcam的函數(shù)，其關(guān)系如圖2所示。因此，LoS測(cè)量模型可表示為：

圖2 LoS測(cè)量幾何Fig.2 LoS measurement geometry

為了計(jì)算LoS測(cè)量值相對(duì)于狀態(tài)量x的測(cè)量靈敏度矩陣H，首先需要計(jì)算測(cè)量值相對(duì)于相機(jī)坐標(biāo)系“cam”下的相對(duì)位置矢量rcam的測(cè)量靈敏度矩陣，并且有：

通過方程(8)將方程(10)得到的偏導(dǎo)數(shù)映射到笛卡爾坐標(biāo)系“car”下，進(jìn)而得到測(cè)量靈敏度矩陣H為：

2 組合相對(duì)導(dǎo)航模型

在松耦合模式下，慣性輔助航天器僅測(cè)角導(dǎo)航系統(tǒng)的組合相對(duì)導(dǎo)航模型如圖3所示。在僅測(cè)角導(dǎo)航系統(tǒng)正常工作的情況下，將慣性導(dǎo)航系統(tǒng)與僅測(cè)角導(dǎo)航系統(tǒng)的相對(duì)位置和速度之差作為量測(cè)信息輸入到卡爾曼濾波器中，然后通過濾波估計(jì)校正IMU的漂移累積誤差，最終得到組合導(dǎo)航系統(tǒng)的相對(duì)狀態(tài)信息。

2.1 誤差狀態(tài)模型

慣性輔助航天器僅測(cè)角導(dǎo)航系統(tǒng)的誤差狀態(tài)量包括相對(duì)位置誤差、相對(duì)速度誤差、副星平臺(tái)失準(zhǔn)角誤差、加速度計(jì)零偏和陀螺儀常值漂移等，后面將對(duì)各個(gè)誤差狀態(tài)量做詳細(xì)介紹。

如圖1所示，選取以主星質(zhì)心為原點(diǎn)的局部軌道坐標(biāo)系作為相對(duì)導(dǎo)航參考坐標(biāo)系。在相對(duì)導(dǎo)航參考坐標(biāo)系中，相對(duì)位置和速度的誤差狀態(tài)方程為：

其中，Δr˙為相對(duì)位置誤差的一階導(dǎo)數(shù)；Δv˙為相對(duì)速度誤差的一階導(dǎo)數(shù)；C41～C63為重力加速度相對(duì)于位置矢量的導(dǎo)數(shù)，此處假定都為零。

假定IMU采用正裝方式，并且三軸方向與副星本體坐標(biāo)系對(duì)齊，以及忽略加速度計(jì)測(cè)量誤差中的二階及二階以上量，則加速度計(jì)的測(cè)量誤差狀態(tài)方程為：

式(13)的展開形式為：

其中，a表示加速度計(jì)測(cè)量的加速度值；Θ表示副星平臺(tái)失準(zhǔn)角；表示加速度計(jì)的零偏；表示加速度計(jì)測(cè)量值的高斯白噪聲；表示從副星本體坐標(biāo)系至相對(duì)導(dǎo)航參考坐標(biāo)系的轉(zhuǎn)換矩陣。

忽略陀螺儀誤差的二階及二階以上量，副星平臺(tái)失準(zhǔn)角誤差方程可表示為：

式(15)的展開形式為：

綜合上面幾個(gè)誤差狀態(tài)方程，則慣性輔助航天器僅測(cè)角導(dǎo)航系統(tǒng)的誤差狀態(tài)方程為：

其中，誤差狀態(tài)變量δ x為：

式(17)中的F表示的是與非線性動(dòng)力學(xué)相關(guān)的雅克比矩陣，G和w表示的是與系統(tǒng)噪聲相關(guān)的矩陣，各符號(hào)的具體表達(dá)式為：

誤差狀態(tài)方程(17)的離散形式為：

其中，Φk|k-1為狀態(tài)轉(zhuǎn)移距陣，可表示為：

其中，Δt為濾波周期。

wk為系統(tǒng)噪聲，滿足高斯白噪聲過程，即：

其中，Qk為系統(tǒng)噪聲協(xié)方差矩陣，Qk具體表達(dá)式為：

2.2 測(cè)量模型

在松耦合模式下，組合相對(duì)導(dǎo)航系統(tǒng)以慣性/僅測(cè)角導(dǎo)航系統(tǒng)估計(jì)的差值作為組合導(dǎo)航濾波器的測(cè)量輸入值，則測(cè)量方程具體可表示為：

其中，

其中，下標(biāo)“Imu”表示慣性導(dǎo)航系統(tǒng)參數(shù)；下標(biāo)“Ang”表示僅測(cè)角導(dǎo)航系統(tǒng)參數(shù)；vk為測(cè)量噪聲，滿足高斯白噪聲過程，即：

其中，Rk為測(cè)量噪聲協(xié)方差矩陣；并且vk和wk不相關(guān)，即：

3 自適應(yīng)均方根EKF

EKF和均方根EKF的流程圖如圖4所示，包括狀態(tài)傳播和測(cè)量更新兩個(gè)主要步驟。其中，更新方程是標(biāo)準(zhǔn)EKF和平方根EKF的主要區(qū)別。當(dāng)協(xié)方差矩陣條件不佳時(shí)，標(biāo)準(zhǔn)EKF在更新步驟中可能會(huì)使協(xié)方差矩陣失去正定性。平方根EKF的更新方程比標(biāo)準(zhǔn)EKF的更新方程要復(fù)雜得多，但是它們可以提高數(shù)值精度并保持協(xié)方差矩陣的正定性。本文使用的平方根算法采用了Maybeck提出的方法[20]。系統(tǒng)狀態(tài)協(xié)方差矩陣的平方根可以通過Cholesky分解計(jì)算得到：

圖4 EKF和平方根EKF的流程圖Fig.4 Flowchart of EKF and square-root EKF

由于過程噪聲和測(cè)量噪聲的選擇對(duì)卡爾曼濾波器的性能至關(guān)重要，適當(dāng)?shù)剡x擇過程噪聲和測(cè)量噪聲可以提高卡爾曼濾波器的動(dòng)態(tài)濾波性能。因此，本文引入“新息”協(xié)方差匹配方法對(duì)測(cè)量噪聲進(jìn)行在線自適應(yīng)調(diào)整?！靶孪ⅰ眳f(xié)方差匹配的基本思想是利用樣本的統(tǒng)計(jì)數(shù)據(jù)及其理論值來推斷濾波器的測(cè)量噪聲，并對(duì)其進(jìn)行自適應(yīng)調(diào)整。在正常濾波情況下，濾波器的測(cè)量預(yù)測(cè)誤差（也稱“新息”或“殘差”）為：

其中，vk為零均值高斯白噪聲，它表示組合導(dǎo)航系統(tǒng)的測(cè)量噪聲。

將式(33)帶入式(32)，“新息”可以重新表示為：

通過應(yīng)用期望得到理論上Δk的協(xié)方差ΣΔk為：

其中，Rk為濾波測(cè)量噪聲協(xié)方差矩陣；k|k-1為相對(duì)于先驗(yàn)狀態(tài)估計(jì)|k-1的協(xié)方差矩陣。

另一方面，如果在滑動(dòng)窗口中收集N個(gè)樣本，則樣本的采樣協(xié)方差Σ?Δk可表示為：

根據(jù)式(35)和式(36)的等效關(guān)系來計(jì)算測(cè)量噪聲協(xié)方差矩陣的估計(jì)值為：

濾波測(cè)量噪聲協(xié)方差矩陣Rk與測(cè)量噪聲協(xié)方差矩陣的估計(jì)值之間的差值為：

4 基于Fisher信息矩陣的可觀性分析

為了評(píng)估慣性輔助航天器僅測(cè)角導(dǎo)航系統(tǒng)的可觀性大小，有必要建立僅測(cè)角導(dǎo)航可觀性的定量分析手段。Fisher信息矩陣可用于測(cè)量給定測(cè)量集所獲得的信息量大小。首先，對(duì)于一個(gè)離散非線性估計(jì)系統(tǒng)可表示為：

其中，xk是狀態(tài)向量，wk是過程噪聲，zk是測(cè)量輸出，vk是第k個(gè)時(shí)間步長(zhǎng)的傳感器測(cè)量噪聲。

令x?k|k是狀態(tài)向量xk的無偏估計(jì)量，則濾波估計(jì)問題的誤差協(xié)方差矩陣可被定義為：

式(41)中的誤差協(xié)方差矩陣的下界可表示如下：

其中，Jk稱為Fisher信息矩陣，其逆矩陣稱為Cramér-Rao下界（Cramér-Rao Lower Bound, CRLB），CRLB定義了組合導(dǎo)航濾波器估計(jì)性能的下限。Bar-Shalom[21]指出，如果濾波器的方差等于CRLB，則濾波器可以認(rèn)定為有效。在本研究中，假設(shè)實(shí)施了有效的估算，則可以直接采用CRLB來評(píng)估組合導(dǎo)航系統(tǒng)的可觀性大小。

Fisher信息矩陣可以采用遞歸形式計(jì)算得到：

其中，Φk+1|k是狀態(tài)向量從時(shí)間步長(zhǎng)k到k+1的狀態(tài)轉(zhuǎn)移矩陣，Hk是測(cè)量方程相對(duì)于估計(jì)狀態(tài)的雅可比矩陣，Rk表示在第k個(gè)時(shí)間步長(zhǎng)的傳感器測(cè)量噪聲矩陣。用初始協(xié)方差矩陣P0的逆矩陣初始化此遞歸方程，得到：

由于Fisher信息矩陣是矩陣形式的，因此以前的大多數(shù)研究都通過該矩陣建立的標(biāo)量來建立優(yōu)化問題的成本函數(shù)。其中，最常用的方法是在終端時(shí)刻處取Fisher信息矩陣的行列式，即

在式(45)中，負(fù)號(hào)用于建立最小化問題。由于對(duì)稱正定矩陣的行列式等于其特征值的乘積，因此它直接與估計(jì)不確定性橢球的體積有關(guān)。最終，基于Fisher信息矩陣的可觀度分析方法可以獲得終端時(shí)刻組合導(dǎo)航系統(tǒng)的可觀性大小。

5 仿真校驗(yàn)

5.1 半物理仿真測(cè)試平臺(tái)搭建

在完成了基于自適應(yīng)均方根EKF的慣性輔助航天器僅測(cè)角導(dǎo)航算法的設(shè)計(jì)后，搭建驗(yàn)證該算法的半物理仿真平臺(tái)，其結(jié)構(gòu)框圖如圖5所示，實(shí)物圖見圖6。該半物理仿真平臺(tái)配備了三軸直線導(dǎo)軌和三軸精密轉(zhuǎn)臺(tái)等機(jī)械微調(diào)裝置，可以調(diào)整星圖顯示裝置和星圖采集模塊同軸。

圖5 半物理仿真平臺(tái)結(jié)構(gòu)框圖Fig.5 Block diagram of the semi-physical simulation platform

圖6 半物理仿真平臺(tái)Fig.6 Semi-physical simulation platform

該半物理仿真平臺(tái)主要包括四大模塊：

(1) 姿態(tài)/軌道發(fā)生器：該模塊內(nèi)部裝有Matlab＆simulink軟件以及STK軟件，主要用于生成副星和主星的姿態(tài)/軌道數(shù)據(jù)，模擬出副星和主星的運(yùn)動(dòng)軌跡參數(shù)；

(2) 星光星圖模擬模塊：該模塊作為星光星圖模擬器，內(nèi)部裝有恒星星庫、星圖生成軟件等，主要用來模擬恒星和主星在星圖中的分布情況和星光等級(jí)；

(3) 星圖識(shí)別與目標(biāo)檢測(cè)模塊：該模塊內(nèi)部裝有星圖識(shí)別軟件、星體質(zhì)心定位軟件和空間非合作目標(biāo)檢測(cè)軟件等，主要功能是在原始圖像中找出主星，并輸出主星相對(duì)于星圖采集模塊的LoS測(cè)量信息；

(4) 僅測(cè)角導(dǎo)航模塊：該模塊的主要功能是對(duì)本文提出的慣性輔助航天器僅測(cè)角導(dǎo)航算法進(jìn)行實(shí)時(shí)運(yùn)算，并輸出主星和副星之間的相對(duì)狀態(tài)信息。

上述四大模塊中，姿態(tài)/軌道發(fā)生器和星光星圖模擬模塊為同一臺(tái)計(jì)算機(jī)，選用型號(hào)為L(zhǎng)enovo，Windows 7，Intel Core i7系列處理器；星圖識(shí)別與目標(biāo)檢測(cè)模塊和僅測(cè)角導(dǎo)航模塊為同一臺(tái)計(jì)算機(jī)，選用型號(hào)為L(zhǎng)enovo，Windows 7，Intel Core i7系列處理器。此外，星圖顯示裝置選用型號(hào)為寶萊納公司的4:3正屏工業(yè)顯示屏B170，其分辨率為1280×1024；星圖采集模塊選用型號(hào)為EV76C660ABT-EQTR的CMOS圖像傳感器，分辨率為1280×1024，像素大小為5.3 μm×5.3 μm，焦距為25 mm的相機(jī)。另外，可以將星圖顯示裝置放置于準(zhǔn)直透鏡的焦平面處，且靠近星圖采集模塊的鏡頭處。經(jīng)過準(zhǔn)直透鏡的星圖就可以模擬無窮遠(yuǎn)處的恒星和主星在圖像中的分布情況，力求模擬出的星圖成像效果能更加接近真實(shí)的場(chǎng)景。

5.2 試驗(yàn)測(cè)試與結(jié)果分析

該慣性輔助航天器僅測(cè)角導(dǎo)航算法的半物理仿真輸入條件如下：

(1) 相對(duì)位置誤差為ROE1：跡向-2000 m、徑向和法向200 m @ 30 km；ROE2：跡向-1200 m、徑向和法向120 m @ 20 km；ROE3：跡向-300 m、徑向和法向30 m @ 5 km。這三種軌道場(chǎng)景中的主星和副星的初始絕對(duì)軌道根數(shù)見表1，包括軌道半長(zhǎng)軸a、軌道偏心率e、軌道傾角i、升交點(diǎn)赤經(jīng)Ω、近地點(diǎn)幅角ω和平近點(diǎn)角M。仿真校驗(yàn)主要在三種典型的航天器交會(huì)場(chǎng)景中進(jìn)行，包括：ROE1代表主星開始接近副星的可能配置，表示遠(yuǎn)距離保持點(diǎn)相對(duì)運(yùn)動(dòng)軌跡；ROE2代表每個(gè)軌道周期主星向副星漂移近1公里，表示自然漂移相對(duì)運(yùn)動(dòng)軌跡；ROE3代表對(duì)接階段的起點(diǎn)，表示主副星隨動(dòng)相對(duì)運(yùn)動(dòng)軌跡。這三個(gè)航天器交會(huì)場(chǎng)景剛好包含了主星從遠(yuǎn)距離交會(huì)抵近至副星的各個(gè)過程；

表1 軌道參數(shù)Tab.1 Orbital parameters

(2) 相對(duì)速度誤差：0.1 m/s、0.01 m/s、0.01 m/s；

(3) 平臺(tái)失準(zhǔn)角誤差：0.3 °、0.3 °、0.3 °；

(4) 陀螺儀測(cè)量誤差：常值漂移為0.035 °/h，隨機(jī)漂移方差為σ2= (0.035 °/3 h)2；

(5) 加速度計(jì)測(cè)量誤差：常值零偏為0.00001g，隨機(jī)零偏方差為σ2= (0.00001g/3)2；

(6) 僅測(cè)角導(dǎo)航系統(tǒng)采樣周期：TAng= 10 s；

(7) 慣性導(dǎo)航系統(tǒng)采樣周期：TImu= 1/30 s；

(8) 組合導(dǎo)航系統(tǒng)濾波周期：Tfilter= 1 s；

(9) 組合導(dǎo)航系統(tǒng)總濾波時(shí)間：Ttotal= 3000 s。

首先，在這三種軌道場(chǎng)景下，對(duì)慣性輔助航天器僅測(cè)角導(dǎo)航系統(tǒng)的可觀性D值進(jìn)行了仿真分析，結(jié)果見表2。從表2中可以看出，三種軌道場(chǎng)景中的僅測(cè)角導(dǎo)航系統(tǒng)的可觀性D值相差不大，其中軌道場(chǎng)景ROE2的可觀性最好，其次是軌道場(chǎng)景ROE1和軌道場(chǎng)景ROE3。

表2 僅測(cè)角導(dǎo)航可觀性D值仿真結(jié)果Tab.2 Angles-only navigation observability D-value simulation results

此外，在搭建的半物理仿真平臺(tái)上，對(duì)提出的基于自適應(yīng)均方根EKF的慣性輔助航天器僅測(cè)角導(dǎo)航算法進(jìn)行了仿真分析，結(jié)果如圖7-9所示，為三種軌道場(chǎng)景下的慣性輔助航天器僅測(cè)角導(dǎo)航濾波器的誤差曲線圖。從圖7-9中可以看出，本文提出的慣性輔助航天器僅測(cè)角導(dǎo)航濾波器能夠成功收斂，且本文提出的自適應(yīng)均方根EKF（圖中顯示為SREKF）的收斂速率比EKF要快，基本上在400秒以內(nèi)就實(shí)現(xiàn)了收斂。此外，終端時(shí)刻采用自適應(yīng)均方根EKF比EKF的跡向?yàn)V波精度平均提高了35%，徑向和法向?yàn)V波精度平均提高了25%。因此，證明了本文提出的基于自適應(yīng)均方根EKF的慣性輔助航天器僅測(cè)角導(dǎo)航算法的有效性。

圖7 在ROE1中，慣性輔助航天器僅測(cè)角導(dǎo)航算法的誤差曲線Fig.7 Error curves of the inertial-aided spacecraft angles-only navigation algorithm in ROE1

圖8 在ROE2中，慣性輔助航天器僅測(cè)角導(dǎo)航算法的誤差曲線Fig.8 Error curves of the inertial-aided spacecraft angles-only navigation algorithm in ROE2

圖9 在ROE3中，慣性輔助航天器僅測(cè)角導(dǎo)航算法的誤差曲線Fig.9 Error curves of the inertial-aided spacecraft angles-only navigation algorithm in ROE3

6 結(jié) 論

本文提出了一種基于自適應(yīng)均方根EKF的慣性輔助航天器僅測(cè)角導(dǎo)航方法。通過將航天器上配備的IMU的測(cè)量數(shù)據(jù)引入僅測(cè)角導(dǎo)航系統(tǒng)，不僅克服了視覺傳感器輸出頻率低、視場(chǎng)角小和易受外界干擾的缺陷，而且可以利用視覺傳感器長(zhǎng)周期、高精度的測(cè)量性能來修正IMU的漂移累積誤差。最后，通過搭建的半物理仿真平臺(tái)驗(yàn)證了本文所提出的基于自適應(yīng)均方根EKF的慣性輔助航天器僅測(cè)角導(dǎo)航方法，得出所提出的自適應(yīng)均方根EKF的收斂速率比EKF要快，基本上在400 s內(nèi)就實(shí)現(xiàn)了收斂。并且，終端時(shí)刻采用自適應(yīng)均方根EKF比EKF的跡向?yàn)V波精度平均提高了35%，徑向和法向?yàn)V波精度平均提高了25%。