宋 亮，李 志，馬興瑞

(1．中國(guó)空間技術(shù)研究院錢學(xué)森空間技術(shù)實(shí)驗(yàn)室，北京100094;2．廣東省政府，廣州510031)

式中:

0 引言

航天技術(shù)經(jīng)過(guò)半個(gè)多世紀(jì)的發(fā)展，已逐漸成熟并在人類的生產(chǎn)生活中發(fā)揮著愈加重要的作用。但與此同時(shí)，頻繁的航天活動(dòng)也造成近地軌道日漸擁擠。美國(guó)空間監(jiān)視網(wǎng)(Space Surveillance Network，SSN)的數(shù)據(jù)顯示，在軌可跟蹤空間物體的數(shù)量已超過(guò)21000個(gè)［1］，其中絕大部分為空間碎片?？臻g碎片，特別是大體積空間碎片占據(jù)了稀缺的軌道資源并對(duì)在軌航天器構(gòu)成了極大的威脅。為消除空間碎片帶來(lái)的碰撞風(fēng)險(xiǎn)，采用服務(wù)航天器進(jìn)行空間碎片清除的研究項(xiàng)目已經(jīng)提上日程。

對(duì)空間碎片執(zhí)行清除操作的前提是通過(guò)相對(duì)測(cè)量手段獲取空間碎片與服務(wù)航天器間的相對(duì)位置與姿態(tài)信息(相對(duì)位姿)，基于相對(duì)位姿信息進(jìn)行位姿控制實(shí)現(xiàn)對(duì)目標(biāo)的繞飛觀測(cè)與逼近?？?，進(jìn)而完成復(fù)雜的碎片清除操作。作為一種成熟的視覺(jué)測(cè)量敏感器，單目相機(jī)在相對(duì)位姿測(cè)量領(lǐng)域得到了廣泛的應(yīng)用［2］。相比于功能更完備但也更復(fù)雜的雙目相機(jī)和激光雷達(dá)，單目相機(jī)具有技術(shù)實(shí)現(xiàn)簡(jiǎn)單、體積質(zhì)量小、功耗低等優(yōu)勢(shì)，在應(yīng)用上更具靈活性和經(jīng)濟(jì)性。

空間碎片的幾何、質(zhì)量參數(shù)一般不可知(包括質(zhì)量、轉(zhuǎn)動(dòng)慣量、質(zhì)心、形狀和尺寸)，且可能處于姿態(tài)翻滾狀態(tài)，是典型的姿態(tài)翻滾未知目標(biāo)。因此，無(wú)法將適用于合作目標(biāo)的相對(duì)位姿估計(jì)算法應(yīng)用在空間碎片上。同時(shí)，當(dāng)空間碎片由于某種原因處于姿態(tài)翻滾狀態(tài)時(shí)，其參數(shù)的未知性更是對(duì)基于單目相機(jī)的相對(duì)位姿估計(jì)算法［3－6］提出了挑戰(zhàn)。

在目前的研究中，對(duì)于沒(méi)有任何先驗(yàn)知識(shí)的未知目標(biāo)，單獨(dú)使用單目相機(jī)進(jìn)行相對(duì)位姿估計(jì)仍是一個(gè)難題。其困難主要在于距離相關(guān)量的解算。一些研究通過(guò)引入額外信息，使單目相機(jī)對(duì)未知目標(biāo)的相對(duì)位姿估計(jì)成為可能［7－9］。針對(duì)單目相機(jī)無(wú)法直接測(cè)距的問(wèn)題，文獻(xiàn)［7］在單目相機(jī)測(cè)量信息的基礎(chǔ)上引入激光測(cè)距信息，采用擴(kuò)展卡爾曼濾波(Extended Kalman Filter，EKF)建立了相對(duì)位姿估計(jì)算法，解決了對(duì)未知目標(biāo)的相對(duì)位姿估計(jì)問(wèn)題。文獻(xiàn)［8］使用結(jié)構(gòu)光提供額外的信息，結(jié)合相機(jī)測(cè)量方程，完成了對(duì)未知目標(biāo)局部區(qū)域的相對(duì)位姿解算。這兩種方式的缺點(diǎn)在于增加了系統(tǒng)的復(fù)雜性。文獻(xiàn)［10］通過(guò)對(duì)靜態(tài)目標(biāo)不同角度成像所得的多幅圖像，基于相機(jī)的散焦信息進(jìn)行了相對(duì)位姿解算，但該方法難以應(yīng)用在動(dòng)態(tài)目標(biāo)上。可見(jiàn)，必須引入額外的信息解決單目相機(jī)在距離測(cè)量上的不足。此外，當(dāng)以未知、姿態(tài)翻滾的空間碎片作為目標(biāo)時(shí)，對(duì)其未知慣量參數(shù)的估計(jì)也是一項(xiàng)關(guān)鍵技術(shù)。文獻(xiàn)［11－12］采用不同的模型對(duì)該問(wèn)題進(jìn)行了研究。

本文針對(duì)單獨(dú)使用單目相機(jī)對(duì)未知目標(biāo)進(jìn)行相對(duì)位姿估計(jì)的問(wèn)題，以幾何、質(zhì)量參數(shù)未知的空間碎片為目標(biāo)，基于單目相機(jī)獲得的目標(biāo)特征視線測(cè)量，在不依賴結(jié)構(gòu)光、無(wú)需距離測(cè)量敏感器/慣性測(cè)量單元輔助且不使用散焦信息的前提下，給出了一種基于EKF的相對(duì)位姿估計(jì)算法。

1 基本知識(shí)

1．1基本假設(shè)

在相對(duì)位姿估計(jì)中涉及兩個(gè)空間物體，服務(wù)航天器和空間碎片?？臻g碎片作為一個(gè)未知目標(biāo)，其外形、尺寸、質(zhì)量、質(zhì)心位置和轉(zhuǎn)動(dòng)慣量等參數(shù)均是未知的，且處于姿態(tài)翻滾狀態(tài)。服務(wù)航天器作為主動(dòng)航天器具有姿態(tài)控制與軌道控制能力。服務(wù)航天器上裝備一部單目相機(jī)作為唯一的相對(duì)測(cè)量敏感器，通過(guò)單目相機(jī)可以獲得目標(biāo)特征點(diǎn)的視線測(cè)量信息。假設(shè)目標(biāo)上具有固定數(shù)量的特征，這些目標(biāo)特征間的相對(duì)位置關(guān)系對(duì)于估計(jì)算法而言是未知的。通過(guò)對(duì)圖像特征的識(shí)別，相機(jī)在對(duì)目標(biāo)成像時(shí)可以獲得這些特征的像素坐標(biāo)，即視線測(cè)量。通過(guò)對(duì)目標(biāo)特征的跟蹤，可以進(jìn)一步得到相同特征在序列圖像中的視線測(cè)量。圖像特征相關(guān)技術(shù)不在本文研究范圍內(nèi)，其過(guò)程將由數(shù)學(xué)模型模擬。

1．2坐標(biāo)系定義

相對(duì)運(yùn)動(dòng)坐標(biāo)系(Local Vertical/Local Horizontal，LVLH){H}:定義同文獻(xiàn)［6］。目標(biāo)與服務(wù)航天器間的相對(duì)位置r、相對(duì)速度v及相應(yīng)的運(yùn)動(dòng)方程均在此坐標(biāo)系中表示，qH→I表示該坐標(biāo)系相對(duì)慣性系的姿態(tài)。

服務(wù)航天器主軸坐標(biāo)系{A}:坐標(biāo)系原點(diǎn)位于航天器質(zhì)心，坐標(biāo)軸沿慣量主軸方向。相機(jī)位置偏置TB在此坐標(biāo)系中表示，qA→I表示服務(wù)航天器的絕對(duì)姿態(tài)。

目標(biāo)主軸坐標(biāo)系{B}:坐標(biāo)系原點(diǎn)位于目標(biāo)質(zhì)心，坐標(biāo)軸沿慣量主軸方向。目標(biāo)絕對(duì)角速度ωB和目標(biāo)特征位置ρi(如圖1所示)均在此坐標(biāo)系中表示，qB→I表示目標(biāo)的絕對(duì)姿態(tài)。

相機(jī)測(cè)量坐標(biāo)系{C}:坐標(biāo)系原點(diǎn)位于{A}中的TB位置，z軸沿相機(jī)視線軸方向，qC→A為該坐標(biāo)系相對(duì)于服務(wù)航天器主軸系{A}的姿態(tài)。

圖1 相對(duì)測(cè)量幾何Fig．1 Relative measurement geometry

1．3相機(jī)測(cè)量方程

相機(jī)的直接測(cè)量為目標(biāo)特征在成像平面上的像素坐標(biāo)(Xi，Yi)。但如將像素坐標(biāo)作為濾波器的測(cè)量輸入，將導(dǎo)致復(fù)雜的測(cè)量方程和濾波敏感矩陣。因此一般采用與其等價(jià)的視線測(cè)量形式作為濾波器測(cè)量輸入。

單目相機(jī)測(cè)量幾何如圖1所示。其中，第i個(gè)目標(biāo)特征相對(duì)于相機(jī)(將相機(jī)視為一個(gè)點(diǎn))的位置矢量li可表示為

式中:·表示四元數(shù)乘。

由式(1)可得第i個(gè)目標(biāo)特征的視線測(cè)量為

式(4)即為本文提出算法的測(cè)量輸入。下面推導(dǎo)測(cè)量協(xié)方差矩陣。根據(jù)小孔成像模型，第i個(gè)特征的視線測(cè)量bi與對(duì)應(yīng)的像素坐標(biāo)(Xi，Yi)間有如下關(guān)系

式中:f為相機(jī)的焦距。對(duì)式(5)求像素坐標(biāo)(Xi，Yi)的偏導(dǎo)，可得測(cè)量誤差傳遞關(guān)系如下

式中:

由式(6)可得視線測(cè)量bi的協(xié)方差矩陣為

2 運(yùn)動(dòng)方程

2．1相對(duì)位置運(yùn)動(dòng)方程

考慮到空間碎片所處軌道的多樣性，研究中將采用與文獻(xiàn)［6］類似的、適用于非圓軌道的相對(duì)位置運(yùn)動(dòng)方程。令xt=［rTvT］T，則有相對(duì)位置運(yùn)動(dòng)方程如下所示

式中:Bt=［03×3I3×3］T/mA為軌控矩陣，mA為服務(wù)航天器質(zhì)量;ut為服務(wù)航天器軌道控制量;wt～N(0)是均值為零、方差為的高斯噪聲;Γt=［03×3I3×3］T為噪聲系數(shù)矩陣。ft形式如下

式中:θ為軌道真近點(diǎn)角，rc為軌道半徑，pc為半正交弦。絕對(duì)軌道參量θ、rc和pc可以通過(guò)服務(wù)航天器的軌道確定系統(tǒng)較為精確地得到，將作為已知量用于濾波方程中。

2．2姿態(tài)運(yùn)動(dòng)方程

選擇四元數(shù)作為姿態(tài)參數(shù)，建立目標(biāo)的絕對(duì)姿態(tài)動(dòng)力學(xué)方程和姿態(tài)運(yùn)動(dòng)學(xué)方程?，F(xiàn)有文獻(xiàn)對(duì)于相對(duì)姿態(tài)估計(jì)問(wèn)題中濾波狀態(tài)的選取各有不同。文獻(xiàn)［12］選擇相對(duì)姿態(tài)和目標(biāo)絕對(duì)角速度作為濾波狀態(tài)，但假設(shè)服務(wù)航天器保持對(duì)地定向，即服務(wù)航天器的姿態(tài)角速度與軌道角速度一致。文獻(xiàn)［5］選擇相對(duì)姿態(tài)和相對(duì)角速度作為濾波狀態(tài)，但假設(shè)目標(biāo)的姿態(tài)角速度與軌道角速度一致。本研究不對(duì)目標(biāo)和服務(wù)航天器的姿態(tài)和角速度做任何限制，在采用服務(wù)航天器姿態(tài)確定系統(tǒng)提供自身絕對(duì)姿態(tài)的前提下，將目標(biāo)絕對(duì)姿態(tài)和絕對(duì)角速度作為濾波狀態(tài)。

目標(biāo)的姿態(tài)運(yùn)動(dòng)學(xué)方程和動(dòng)力學(xué)方程分別為

式中:IB=diag(Ix，Iy，Iz)為目標(biāo)的主慣量矩陣;wr～N(0)是均值為零、方差為的高斯噪聲;為ωB的叉乘矩陣。由于目標(biāo)的慣量矩陣是未知的，因此不能直接使用式(11)建立濾波方程。為此，定義慣量比p=［p1p2］T=［Iy/IxIz/Ix］T，用慣量比p替換式(11)中的Ix、Iy和Iz，得到新的姿態(tài)動(dòng)力學(xué)方程如下

式中:

慣量比p是常量，因此有

2．3目標(biāo)特征

將目標(biāo)特征位置ρi作為估計(jì)狀態(tài)是對(duì)未知目標(biāo)進(jìn)行相對(duì)位姿估計(jì)的關(guān)鍵。假設(shè)目標(biāo)為剛體，則特征位置 ρi為常數(shù)，所以有。令Π=為目標(biāo)特征的集合向量，則有

3 濾波器設(shè)計(jì)

本節(jié)將基于EKF方法給出濾波器的具體實(shí)現(xiàn)。選取系統(tǒng)狀態(tài)如下

式中:qv，B→I為目標(biāo)絕對(duì)姿態(tài)的矢部，δqv，B→I為qv，B→I的誤差形式;δωB為目標(biāo)絕對(duì)角速度的誤差形式。δqv，B→I的運(yùn)動(dòng)方程為［12］

對(duì)式(12)進(jìn)行變分運(yùn)算，可得

式中:

根據(jù)式(9)、(13)、(14)、(16)和(17)可得線性化的系統(tǒng)狀態(tài)方程如下

式中:

由系統(tǒng)矩陣F可得濾波狀態(tài)轉(zhuǎn)移矩陣的一階近似Φ=I26×26+FΔt，Δt為濾波周期。由狀態(tài)轉(zhuǎn)移矩陣Φ可得過(guò)程噪聲矩陣為

下面將給出濾波測(cè)量方程。對(duì)于N個(gè)特征視線測(cè)量{b1，…，bN}，測(cè)量方程為

對(duì)式(20)求系統(tǒng)狀態(tài)(15)的偏導(dǎo)，可得濾波敏感矩陣如下

式中:

EKF算法流程可參考文獻(xiàn)［6］。

4 算法核心思想

算法基于EKF實(shí)現(xiàn)，現(xiàn)對(duì)濾波狀態(tài)的選取進(jìn)行說(shuō)明。首先，對(duì)于相對(duì)位姿估計(jì)問(wèn)題，相對(duì)位置/速度、姿態(tài)/角速度必須作為濾波狀態(tài)。而對(duì)于未知空間碎片，由于其轉(zhuǎn)動(dòng)慣量未知，因此需將目標(biāo)轉(zhuǎn)動(dòng)慣量以某種形式引入濾波狀態(tài)，本研究中采用了慣量比形式。此外，由于采用目標(biāo)特征的視線測(cè)量作為濾波測(cè)量輸入，而對(duì)于未知目標(biāo)，特征位置是未知的，因此必須將目標(biāo)特征位置作為濾波狀態(tài)。完整的濾波狀態(tài)包括了以上討論的所有物理量。然而，由于單目相機(jī)視線測(cè)量缺少距離維度信息，如果以上述方式建立濾波算法，則僅能對(duì)姿態(tài)、姿態(tài)角速度和慣量比等姿態(tài)相關(guān)狀態(tài)進(jìn)行有效估計(jì)。對(duì)相對(duì)位置、相對(duì)速度和目標(biāo)特征位置等距離相關(guān)狀態(tài)是不可觀。下面給出兩個(gè)關(guān)鍵條件可以保證新算法對(duì)距離相關(guān)狀態(tài)也是可觀的，即全部位姿狀態(tài)均可被有效估計(jì)出。

4．1關(guān)鍵條件

條件1．將相機(jī)位置偏置TB引入濾波測(cè)量方程中。目前，基于濾波方法、采用視覺(jué)測(cè)量敏感器(不局限于單目相機(jī))進(jìn)行相對(duì)位姿估計(jì)研究時(shí)，一般假設(shè)測(cè)量敏感器位于所搭載航天器的質(zhì)心上，如文獻(xiàn)［3，6，12－13］。因此在動(dòng)力學(xué)意義上，即相對(duì)位置運(yùn)動(dòng)方程中，敏感器和航天器被視為一體。這種建模上的簡(jiǎn)化會(huì)在估計(jì)結(jié)果中引入系統(tǒng)誤差，損失一定的估計(jì)精度。文獻(xiàn)［5］在建立相對(duì)位置運(yùn)動(dòng)方程和測(cè)量方程時(shí)，考慮了相機(jī)相對(duì)于航天器質(zhì)心偏置安裝的問(wèn)題，通過(guò)引入相機(jī)安裝位置矢量TB，建立了非質(zhì)點(diǎn)形式的相對(duì)位置運(yùn)動(dòng)模型。通過(guò)仿真，分析了上述兩種建模方式間的差別，并得到非質(zhì)點(diǎn)形式相對(duì)位置運(yùn)動(dòng)模型可以給出更好估計(jì)結(jié)果的結(jié)論。這一結(jié)論無(wú)疑是正確的，但該文作者并沒(méi)有發(fā)掘出其蘊(yùn)含的更為重要的一個(gè)特性。即通過(guò)引入相機(jī)的位置偏置，結(jié)合相對(duì)位姿運(yùn)動(dòng)的耦合關(guān)系，使得僅基于單目相機(jī)的視線測(cè)量，即可完成對(duì)未知目標(biāo)的相對(duì)位姿估計(jì)。

條件2．服務(wù)航天器進(jìn)行軌道機(jī)動(dòng)。若不引入相機(jī)位置偏置，為了提供額外信息解決位置相關(guān)狀態(tài)的可觀性問(wèn)題，需要服務(wù)航天器進(jìn)行軌道機(jī)動(dòng)。引入服務(wù)航天器的軌道機(jī)動(dòng)是有明確物理意義的。軌道機(jī)動(dòng)既可以是使服務(wù)航天器對(duì)目標(biāo)進(jìn)行受迫繞飛的軌道機(jī)動(dòng)，也可以是使服務(wù)航天器逼近/遠(yuǎn)離目標(biāo)的軌道機(jī)動(dòng)。引入軌道機(jī)動(dòng)是視線導(dǎo)航中解決位置狀態(tài)可觀性問(wèn)題的常規(guī)方法［14］，但視線導(dǎo)航中不考慮姿態(tài)，因此本研究中的問(wèn)題更加復(fù)雜。

4．2可觀性分析

本節(jié)將進(jìn)行可觀性分析證明第4．1節(jié)給出條件的作用。首先采用基于Lie導(dǎo)數(shù)的可觀性秩條件法進(jìn)行可觀性分析，具體方法可參考文獻(xiàn)［15］。由于本文建立的濾波器維數(shù)大，難以人工寫(xiě)出求解可觀性秩過(guò)程中涉及到的復(fù)雜矩陣。故采用Matlab符號(hào)函數(shù)工具箱建立可觀性分析方程并求解，最終得到可觀性矩陣的秩為26，為滿秩。因此根據(jù)第4．1節(jié)提出的兩個(gè)條件建立的濾波算法是可觀的?；诳捎^性秩條件給出的可觀性結(jié)果并不能形象說(shuō)明本文提出的算法是如何有效解決可觀性問(wèn)題的，下面通過(guò)解析方法進(jìn)行可觀性分析。

首先需要指出的是，單目相機(jī)對(duì)未知目標(biāo)相對(duì)姿態(tài)的測(cè)量是不存在可觀性問(wèn)題的［16］，文獻(xiàn)［11］所研究的即為基于單目相機(jī)視線測(cè)量的姿態(tài)/角速度估計(jì)問(wèn)題。因此，下面對(duì)可觀性的分析僅針對(duì)位置相關(guān)狀態(tài)。為方便推導(dǎo)，在分析過(guò)程中假設(shè)姿態(tài)相關(guān)狀態(tài)都是可知的。

本文算法的可觀性問(wèn)題與視線導(dǎo)航中存在的可觀性問(wèn)題非常相似，都是基于單目視線測(cè)量進(jìn)行相對(duì)狀態(tài)估計(jì)的可觀性問(wèn)題。事實(shí)上，本節(jié)給出的可觀性分析就是基于Geller和Klein［17］在視線導(dǎo)航領(lǐng)域的研究成果進(jìn)行的。本節(jié)分析采用與文獻(xiàn)［17］類似的可觀性判定條件:通過(guò)對(duì)目標(biāo)特征進(jìn)行一段時(shí)間的測(cè)量，根據(jù)測(cè)量信息，若可以唯一確定一組初始的相對(duì)位置、相對(duì)速度以及目標(biāo)特征位置，則算法對(duì)位置相關(guān)狀態(tài)是可觀的。

首先證明當(dāng)?shù)?．1節(jié)提出的條件不滿足時(shí)，算法對(duì)位置相關(guān)狀態(tài)是不可觀的。根據(jù)式(4)，對(duì)于第i個(gè)特征點(diǎn)，其t0時(shí)刻的視線測(cè)量如下(為方便討論，將變量在LVLH坐標(biāo)系中表示)

式中:

不失一般性，以線性形式表示相對(duì)位置運(yùn)動(dòng)方程，如文獻(xiàn)［17］。因此有

式中:

將式(23)代入式(22)中，可以得到以r(t0)、v(t0)、ρi(t0)表示的特征視線測(cè)量bi。其中項(xiàng)包含了相機(jī)位置偏置表示軌道機(jī)動(dòng)項(xiàng)。若第4．1節(jié)提出的條件均不滿足，則和均為零，目標(biāo)特征視線測(cè)量可表示為

根據(jù)可觀性判定條件，若算法是可觀的，則通過(guò)一系列{bi(t0)，…，bi(tN)}，可以唯一確定一組初值r(t0)、v(t0)、ρi(t0)。但對(duì)于式(24)，對(duì)于任意正常數(shù) α，αr(t0)、αv(t0)、αρi(t0)同樣使式(24)成立，即{bi(t0)，…，bi(tN)}不能唯一確定一組狀態(tài)初值。因此不滿足可觀性條件，此時(shí)估計(jì)算法對(duì)位置相關(guān)狀態(tài)不可觀。

下面使用文獻(xiàn)［17］中的方法證明，當(dāng)?shù)?．1節(jié)提出的條件滿足時(shí)，估計(jì)算法是可觀的。假設(shè)除r(t0)、v(t0)、ρi(t0)，存在另外一組相對(duì)位置、相對(duì)速度以及目標(biāo)特征位置初值可以產(chǎn)生與式(22)相同的視線測(cè)量，如下(以相機(jī)位置偏置為例進(jìn)行證明)

為使式(25)成立，必有

式中:

同理，對(duì)于ti時(shí)刻可得

式中:

將式(27)中的ti分別代之以t1、t2并與式(26)聯(lián)立，得到如下方程組

式中:

進(jìn)一步可得

將式(27)中的ti分別代之以t3、t4，并將由式(29)求得的代入，可得如下形式的方程

式中:

對(duì)式(30)、(31)聯(lián)立方程組，如下

式中:

當(dāng)TB不為零時(shí)，根據(jù)Ai和Bi的表達(dá)式可知矩陣M是列滿秩的。因此式(32)中的K必為零向量。由此證得，通過(guò)一系列的測(cè)量{bi(t0)…bi(tN)}可以唯一確定相對(duì)位置、相對(duì)速度和目標(biāo)特征的初值r(t0)，v(t0)，ρi(t0)，滿足可觀性判定條件，所以本文提出的估計(jì)算法對(duì)位置相關(guān)狀態(tài)是可觀的。結(jié)合姿態(tài)相關(guān)狀態(tài)的可觀性，可得本文算法對(duì)所有狀態(tài)都是可觀的，證畢。

以上可觀性分析過(guò)程以相機(jī)安裝位置TB為例，證明了TB對(duì)算法可觀性的關(guān)鍵作用。軌道機(jī)動(dòng)的作用與相機(jī)位置偏置TB完全相同，證明過(guò)程相似，不再給出。

5 數(shù)值仿真

5．1仿真算例

給出三個(gè)仿真算例，分別為相機(jī)位置偏置和軌道機(jī)動(dòng)單獨(dú)作用的情況，以及相機(jī)位置偏置和軌道控制均不作用的情況。

算例1．服務(wù)航天器對(duì)目標(biāo)進(jìn)行自然繞飛(相機(jī)位置偏置單獨(dú)作用，無(wú)軌道機(jī)動(dòng))

選擇目標(biāo)軌道參數(shù)為，半長(zhǎng)軸a=6698455 m，偏心率e=0．05。服務(wù)航天器與目標(biāo)的相對(duì)軌道參數(shù)為

式中:A0=30 m，α=π/4，=0．0012 rad，t=0。相機(jī)位置偏置TB=［0．5 0．5－1］Tm。仿真過(guò)程中服務(wù)航天器不進(jìn)行軌道機(jī)動(dòng)，但需進(jìn)行姿態(tài)控制保證單目相機(jī)的視線方向?qū)?zhǔn)目標(biāo)。算例2、3中對(duì)服務(wù)航天器姿態(tài)的要求同算例1。

算例2．服務(wù)航天器對(duì)目標(biāo)進(jìn)行受迫繞飛(軌道機(jī)動(dòng)單獨(dú)作用，相機(jī)位置偏置為零)

軌道初始條件同算例1，但將相機(jī)位置偏置TB設(shè)置為零，即相機(jī)與服務(wù)航天器質(zhì)心重合。服務(wù)航天器通過(guò)軌道控制進(jìn)行受迫繞飛。為了進(jìn)行對(duì)比，選擇與算例1繞飛軌道相近的受迫繞飛軌道。選取方法如下:根據(jù)式(33)選取繞飛制導(dǎo)點(diǎn)，取t=600i，i=1，…，10，其他參數(shù)同算例1，獲得10個(gè)制導(dǎo)點(diǎn)。由這10個(gè)制導(dǎo)點(diǎn)組成的繞飛軌道與算例1中的自然繞飛軌道形狀相似，但周期不同。

算例3．服務(wù)航天器對(duì)目標(biāo)進(jìn)行自然繞飛(相機(jī)位置偏置為零且無(wú)軌道機(jī)動(dòng))

軌道初始條件同算例1，但將相機(jī)位置偏置TB設(shè)置為零，且無(wú)軌道控制。

以上三個(gè)算例中，目標(biāo)均處于姿態(tài)翻滾、無(wú)控狀態(tài)。目標(biāo)的姿態(tài)參數(shù)初值為

在濾波器中，狀態(tài)相關(guān)的測(cè)量協(xié)方差矩陣根據(jù)式(7)計(jì)算。初始協(xié)方差估計(jì)為P0=diag(I3×3，I3×3，2I2×2，500I3×3，I3×3，25I12×12)，在濾波器中設(shè)置初始狀態(tài)估計(jì)值為仿真值的150%。目標(biāo)特征位置(目標(biāo)主軸系{B}中表示)設(shè)置如表1所示。

表1 目標(biāo)特征位置Table 1 Feature locations

5．2仿真結(jié)果與分析

受篇幅所限，僅將算例1的全部仿真結(jié)果列出。圖2～圖7給出算例1全部狀態(tài)估計(jì)結(jié)果(姿態(tài)以歐拉角的形式給出)，所有結(jié)果均以誤差形式給出，即估計(jì)值與真值之差(圖中實(shí)線)，并且同時(shí)給出相應(yīng)的3σ誤差界(圖中虛線)。圖7給出一個(gè)特征位置的估計(jì)結(jié)果，其他特征位置估計(jì)的收斂趨勢(shì)具有相似性，不逐一給出。仿真結(jié)果顯示，誤差界可以較為準(zhǔn)確地描述估計(jì)偏差，所有估計(jì)狀態(tài)均得到較為準(zhǔn)確的估計(jì)。因此當(dāng)條件1滿足時(shí)，算法有效。算例2的結(jié)果與算例1相似，不再給出。

圖2 姿態(tài)估計(jì)誤差曲線與誤差界(3σ)Fig．2 Estimated errors for the target＇s attitude and 3σbounds

圖3 角速度估計(jì)誤差曲線與誤差界(3σ)Fig．3 Estimated errors for the target＇s angular velocity and 3σbounds

圖4 慣量比估計(jì)誤差曲線與誤差界(3σ)Fig．4 Estimated errors for the inertia ratios and 3σbounds

算例3中所有姿態(tài)相關(guān)狀態(tài)均收斂且收斂趨勢(shì)與算例1、2相似，說(shuō)明算例3條件下的算法對(duì)姿態(tài)是可以有效估計(jì)的。但是算例3中的算法無(wú)法對(duì)位置相關(guān)狀態(tài)進(jìn)行有效估計(jì)，所有位置相關(guān)狀態(tài)均不收斂。

圖5 相對(duì)位置估計(jì)誤差曲線與誤差界(3σ)Fig．5 Estimated errors for relative position and 3σbounds

圖6 相對(duì)速度估計(jì)誤差曲線與誤差界(3σ)Fig．6 Estimated errors for relative velocity and 3σbounds

圖7 特征位置估計(jì)誤差曲線與誤差界(3σ)Fig．7 Estimated errors for the feature locations and 3σbounds

由仿真結(jié)果可得，相機(jī)位置偏置(算例1)和服務(wù)航天器的軌道機(jī)動(dòng)(算例2)均能保證濾波狀態(tài)的完全可觀。如果沒(méi)有這兩者之一，單目相機(jī)僅能對(duì)姿態(tài)相關(guān)狀態(tài)進(jìn)行有效估計(jì)，對(duì)距離相關(guān)狀態(tài)不可觀。

6 結(jié)論

本文基于單目相機(jī)視線測(cè)量，采用EKF建立了針對(duì)未知空間碎片的相對(duì)位姿估計(jì)算法。提出了兩個(gè)可以保證算法可觀性的關(guān)鍵條件，并通過(guò)可觀性分析進(jìn)行了證明。最后對(duì)三個(gè)典型算例進(jìn)行了數(shù)值仿真，校驗(yàn)了算法的有效性。

［1］National Aeronautics and Space Administration．Space debris and human spacecraft［EB/OL］．2013［2014］．http://www．nasa．gov/mission_pages/station/news/orbital_debris．html#．VCzdNnExjBg．

［2］Florian S，Toralf B，J?rn S，et al．On－orbit servicing missions:challenges and solutions for spacecraft operations［C］．SpaceOps 2010 Conference，Huntsville，Alabama，April 25－30，2010．

［3］Zhang L，Yang H，Zhang S，et al．Kalman filtering for relative spacecraft attitude and position estimation:a revisit［J］．Journal of Guidance，Control，and Dynamics，2014，37(5):1706－1711．

［4］Xing Y，Cao X，Zhang S，et al．Relative position and attitude estimation for satellite formation with coupled translational and rotational dynamics［J］．Acta Astronautica，2010，67(3):455－467．

［5］ 邢艷軍，曹喜濱，張世杰，等．非質(zhì)心特征點(diǎn)相對(duì)位置和姿態(tài)估計(jì)方法研究［J］．宇航學(xué)報(bào)，2010，31(9):2129－2137．［Xing Yan－jun，Cao Xi－bin，Zhang Shi－jie，et al．Relative position and attitude estimation of feature point deviating from center of mass［J］．Journal of Astronautics，2010，31(9):2129－2137．］

［6］Kim S，Crassidis J L，Cheng Y，et al．Kalman filtering for relative spacecraft attitude and position estimation［J］．Journal of Guidance，Control，and Dynamics，2007，30(1):133－143．

［7］Padial J，Hammond M，Augenstein S，et al．Tumbling target reconstruction and pose estimation through fusion of monocular vision and sparse－pattern range data［C］．2012 IEEE Conference on Multisensor Fusion and Integration for Intelligent Systems，Hamburg，Germany，September 13－15，2012．

［8］ 高學(xué)海，梁斌，潘樂(lè)，等．非合作大目標(biāo)位姿測(cè)量的線結(jié)構(gòu)光視覺(jué)方法［J］．宇航學(xué)報(bào)，2012，33(6):728－735．［Gao Xue－h(huán)ai，Liang Bin，Pan Le，et al．Pose measurement of large non－cooperative target using line structured light vision［J］．Journal of Astronautics，2012，33(6):728－735．］

［9］Augenstein S，Rock SM．Improved frame－to－frame pose tracking during vision－only SLAM/SFM with a tumbling target［C］．2011 IEEE International Conference on Robotics and Automation，Shanghai，China，May 9－13，2011．

［10］Kuhl A，W?hler C，Krüger L，et al．Monocular 3D scene reconstruction at absolute scales by combination of geometric and real－aperture methods［C］．Pattern Recognition，Berlin，Germany，September 12－14，2006．

［11］ 王峰，陳雪芹，曹喜濱．在軌服務(wù)航天器對(duì)失控航天器參數(shù)估計(jì)算法研究［J］．宇航學(xué)報(bào)，2009，30(4):1396－1403．［Wang Feng，Chen Xue－qin，Cao Xi－bin．The research of onorbit parameters estimation for on－orbit－servicing spacecraft relative to out－of－control spacecraft［J］．Journal of Astronautics，2009，30(4):1396－1403．］

［12］Aghili F，Parsa K．Motion and parameter estimation of space objects using laser－vision data［J］．Journal of Guidance，Control，and Dynamics，2009，32(2):538－550．

［13］Segal S，Carmi A，Gurfil P．Vision－based relative state estimation of non－cooperative spacecraft under modeling uncertainty［C］．IEEE Aerospace Conference，Big Sky，USA，March 5－12，2011．

［14］Woffinden D C，Geller D K．Observability criteria for anglesonly navigation［J］．Aerospace and Electronic Systems，IEEE Transactions on，2009，45(3):1194－1208．

［15］ 宋亮．星敏感器陀螺姿態(tài)確定系統(tǒng)在軌標(biāo)定研究［D］．哈爾濱:哈爾濱工業(yè)大學(xué)，2011．［Song Liang．Research on on－orbit calibration methods for star tracker－gyro attitude determination system［D］．Harbin:Harbin Institute of Technology，2011．］

［16］Hartley R，Zisserman A．Multiple view geometry in computer vision［M］．2nd Edition．Cambridge:Cambridge University Press，2003．

［17］Geller D K，Klein I．Angles－only navigation state observability during orbital proximity operations［J］．Journal of Guidance，Control，and Dynamics，2014，37(6):1976－1983．