一種空間非合作目標(biāo)相對位姿緊耦合估計(jì)方法

劉 柯，汪 玲，劉寒寒，張 翔

(1.南京航空航天大學(xué) 電子信息工程學(xué)院，南京 211106; 2. 南京理工大學(xué) 機(jī)械工程學(xué)院，南京 210094)

0 引言

隨著空間技術(shù)的發(fā)展，頻繁的航天任務(wù)導(dǎo)致對在軌服務(wù)航天器實(shí)現(xiàn)復(fù)雜任務(wù)的需求日益增加，例如對目標(biāo)的繞飛觀測和逼近停靠，對失效衛(wèi)星和空間碎片進(jìn)行清理，還有維修故障航天器等，這些任務(wù)都需要服務(wù)航天器近距離準(zhǔn)確得到目標(biāo)的相對位置與姿態(tài)信息[1-4]。根據(jù)空間目標(biāo)能否反饋有用的合作信息，如能直接進(jìn)行通信或已安裝光學(xué)標(biāo)志等，可以分為合作目標(biāo)和非合作目標(biāo)[5-6]。針對空間合作目標(biāo)的近距離相對位姿測量技術(shù)，國內(nèi)外學(xué)者已經(jīng)進(jìn)行了大量研究，提出的許多方法都已成功在軌驗(yàn)證[7-10]。而空間非合作目標(biāo)由于其缺乏先驗(yàn)信息，相對位姿測量問題變得相當(dāng)具有挑戰(zhàn)性，而且在已編目的在軌目標(biāo)中，又有大量被歸類為非合作目標(biāo)，所以空間非合作目標(biāo)的相對位姿測量問題越來越受到國內(nèi)外學(xué)者的重視。

目前空間非合作目標(biāo)的相對位姿獲取方法主要有：光學(xué)成像測量與激光雷達(dá)測量[11]，具體傳感器的選擇必須考慮任務(wù)航天器上的可用資源，例如功率、質(zhì)量、體積和計(jì)算資源等，另外還必須考慮到具體的任務(wù)設(shè)想和整體的費(fèi)用。相比激光雷達(dá)測量技術(shù)，基于光學(xué)成像測量的位姿估計(jì)技術(shù)因其具有低成本、高精度等優(yōu)勢，已成為航天器近距離導(dǎo)航中獲取目標(biāo)相對位姿的重要途徑。

此外小型衛(wèi)星由于研發(fā)快、成本低等優(yōu)勢，在航天活動(dòng)中越來越受到青睞，應(yīng)用領(lǐng)域不斷拓展。在衛(wèi)星小型化發(fā)展的過程中，對星載傳感器功率、質(zhì)量、體積等的要求也越來越苛刻。單目相機(jī)有功耗低、質(zhì)量輕、體積小、結(jié)構(gòu)簡單等優(yōu)勢，恰恰滿足了小型衛(wèi)星對星載傳感器的要求，是小型衛(wèi)星近距離導(dǎo)航中獲取目標(biāo)相對位姿的有效途徑，所以基于單目相機(jī)的空間非合作目標(biāo)位姿估計(jì)技術(shù)越來越受到國內(nèi)外學(xué)者的重視。文獻(xiàn)[6]基于單目相機(jī)估計(jì)了非合作航天器的相對位姿，但假設(shè)目標(biāo)航天器特征點(diǎn)模型已知。文獻(xiàn)[12]利用移動(dòng)機(jī)器人中同步定位與地圖構(gòu)建(SLAM，simultaneous localization and mapping)方法和運(yùn)動(dòng)中結(jié)構(gòu)恢復(fù)(SFM，structure from motion)方法來解決空間未知非合作目標(biāo)的相對位姿估計(jì)問題，該文獻(xiàn)提出了一種實(shí)時(shí)估計(jì)未知翻滾目標(biāo)幀間位姿的SLAM/SFM算法，只使用單個(gè)相機(jī)，該算法融合了貝葉斯估計(jì)方法和測量反演法來估計(jì)相對位姿參數(shù)，可用于檢查和維修故障或損壞的衛(wèi)星，但該方法假定目標(biāo)初始位姿已知。文獻(xiàn)[13]針對此方法中的相對位姿估計(jì)部分進(jìn)行改進(jìn)，提出聯(lián)合無跡卡爾曼濾波(UKF，unscented kalman filter)和粒子濾波 (PF，particle filter)來估計(jì)目標(biāo)的位姿參數(shù)，但該方法計(jì)算量大，實(shí)時(shí)性較差。

由于單目相機(jī)的局限性，無法測得空間非合作目標(biāo)深度，由此一些學(xué)者提出利用單目視覺與激光測距儀混合的系統(tǒng)來估計(jì)空間非合作目標(biāo)的位姿參數(shù)。文獻(xiàn)[14]基于單目手眼相機(jī)和激光測距儀， 提出了一種尺寸未知的空間目標(biāo)的位姿測量算法，但該方法只適用于矩形目標(biāo)或者自帶矩形標(biāo)志點(diǎn)的目標(biāo)。文獻(xiàn)[15]提出了一種基于可見光相機(jī)與激光測距儀的空間碎片在軌識別與定位方法，由單目相機(jī)提供測角信息，以優(yōu)化后的測角信息驅(qū)動(dòng)激光測距儀完成測距，之后結(jié)合兩個(gè)傳感器的測量信息，采用擴(kuò)展卡爾曼濾波算法獲得較高精度的目標(biāo)位置信息，該方法適用于完全未知的空間非合作目標(biāo)，但該方法只求得了目標(biāo)的位置信息，而沒有解算目標(biāo)的姿態(tài)信息。

本文針對空間任務(wù)中服務(wù)航天器實(shí)時(shí)估計(jì)空間未知非合作目標(biāo)的相對位置和姿態(tài)這一難題，提出了一種融合單目相機(jī)與激光測距儀的相對位姿估計(jì)方法。采用單目相機(jī)獲取目標(biāo)序列圖像，在初始化時(shí)構(gòu)建了真實(shí)尺度下的世界坐標(biāo)，在連續(xù)位姿估計(jì)時(shí)以緊耦合的形式融合相機(jī)數(shù)據(jù)與激光測距儀數(shù)據(jù)來優(yōu)化相對位姿，得到了較高精度的相對位姿信息。最后使用Blender軟件生成空間非合作目標(biāo)序列圖像，驗(yàn)證了本文算法能穩(wěn)健地得到較高精度的空間非合作目標(biāo)的相對位姿，且擁有較好的實(shí)時(shí)性。

1 系統(tǒng)基本知識

1.1 基本假設(shè)

在相對位姿估計(jì)中涉及兩個(gè)空間物體，服務(wù)航天器和空間非合作目標(biāo)?？臻g非合作目標(biāo)作為一個(gè)未知目標(biāo)，其外形、質(zhì)量、質(zhì)心位置和轉(zhuǎn)動(dòng)慣量等所有參數(shù)均是未知的。服務(wù)航天器作為主動(dòng)航天器具有姿態(tài)控制與軌道控制能力。服務(wù)航天器上裝備一部單目相機(jī)和一個(gè)激光測距儀作為測量傳感器，假設(shè)相機(jī)視場中只出現(xiàn)目標(biāo)或圖像背景中的地球已被濾除?？紤]到我們感興趣的是服務(wù)航天器與空間非合作目標(biāo)間的相對位姿問題，若相機(jī)視場中只存在非合作目標(biāo)，則可將非合作目標(biāo)的運(yùn)動(dòng)等效看作為服務(wù)航天器的運(yùn)動(dòng)，比如非合作目標(biāo)靠近服務(wù)航天器的運(yùn)動(dòng)可等效為服務(wù)航天器遠(yuǎn)離非合作目標(biāo)的運(yùn)動(dòng)，這種運(yùn)動(dòng)轉(zhuǎn)換在定義完坐標(biāo)系之后進(jìn)行詳述。

1.2 坐標(biāo)系定義

圖1給出了各個(gè)坐標(biāo)系的定義，設(shè)oC-xC-yC-zC為相機(jī)坐標(biāo)系，其中oC為攝像機(jī)光心，也是針孔模型中的針孔，zC軸穿過相機(jī)光心，與成像平面垂直指向相機(jī)前方，xC向右，yC向下。設(shè)oB-xB-yB-zB為目標(biāo)本體坐標(biāo)系，O-X-Y為物理成像坐標(biāo)系，o-u-v為像素坐標(biāo)系。物理成像平面與相機(jī)光心的距離為相機(jī)焦距f，激光測距儀激光發(fā)射點(diǎn)L距相機(jī)光心的距離為基線長度d，發(fā)射方向與相機(jī)坐標(biāo)系zC軸平行，記激光測距儀照在目標(biāo)上的點(diǎn)為激光測距點(diǎn)。非合作目標(biāo)上的空間點(diǎn)P，小孔成像后落在物理成像平面上的成像點(diǎn)為p。ow-xw-yw-zw為世界坐標(biāo)系，在此坐標(biāo)系下估計(jì)服務(wù)航天器與空間非合作目標(biāo)的相對位姿。相機(jī)坐標(biāo)系與目標(biāo)本體坐標(biāo)系會(huì)隨相機(jī)與目標(biāo)的運(yùn)動(dòng)而不停變動(dòng)，圖1是初始時(shí)刻時(shí)，相機(jī)坐標(biāo)系與世界坐標(biāo)系重合，目標(biāo)本體坐標(biāo)系與世界坐標(biāo)系坐標(biāo)指向相同。

圖1 坐標(biāo)系定義

1.3 運(yùn)動(dòng)轉(zhuǎn)換

測量空間非合作目標(biāo)的相對位姿時(shí)，在剔除背景干擾的情況下，可以將空間非合作目標(biāo)的運(yùn)動(dòng)全部轉(zhuǎn)換成相機(jī)的運(yùn)動(dòng)，如圖2所示，目標(biāo)向相機(jī)移動(dòng)to等同于相機(jī)向目標(biāo)移動(dòng)-to：

圖2 運(yùn)動(dòng)轉(zhuǎn)換示意圖

除了圖2這種簡單的平移運(yùn)動(dòng)，非合作目標(biāo)的旋轉(zhuǎn)可以轉(zhuǎn)化為相機(jī)的旋轉(zhuǎn)加平移運(yùn)動(dòng)，例如非合作目標(biāo)自旋一周，可以轉(zhuǎn)換為相機(jī)將鏡頭一直面向目標(biāo)繞飛一周，有了這些轉(zhuǎn)換情況，就可以構(gòu)建一個(gè)只含有任務(wù)航天器的單目相機(jī)和空間非合作目標(biāo)的系統(tǒng)：

在初始化時(shí)確定目標(biāo)在世界坐標(biāo)系下的位姿，然后將后面所有的目標(biāo)運(yùn)動(dòng)都轉(zhuǎn)換為相機(jī)的運(yùn)動(dòng)，即目標(biāo)在世界坐標(biāo)系下的位姿始終不發(fā)生改變，這樣根據(jù)目標(biāo)在相機(jī)圖像中的變化，就可以求得世界坐標(biāo)系下相機(jī)自身的運(yùn)動(dòng)疊加轉(zhuǎn)換的相機(jī)運(yùn)動(dòng)，之后根據(jù)目標(biāo)在世界坐標(biāo)系下的位姿就能得到相機(jī)與目標(biāo)的相對位姿。

后文中再出現(xiàn)的相機(jī)運(yùn)動(dòng)，都是世界坐標(biāo)系下相機(jī)自身的運(yùn)動(dòng)與轉(zhuǎn)換的相機(jī)運(yùn)動(dòng)疊加后的運(yùn)動(dòng)，當(dāng)目標(biāo)位姿在初始化中固定后，再求得相機(jī)在世界坐標(biāo)系下的位姿相當(dāng)于求得了相機(jī)與目標(biāo)之間的相對位姿。

2 相對位姿初始化

選取初始兩幀圖像與第一幀時(shí)刻激光測距儀數(shù)據(jù)，對服務(wù)航天器與空間非合作目標(biāo)的相對位姿進(jìn)行初始化。圖3為初始化流程圖，下面對其中的關(guān)鍵步驟做介紹。

圖3 位姿測量初始化流程圖

2.1 特征點(diǎn)檢測與匹配

考慮到空間環(huán)境的光照環(huán)境復(fù)雜，非合作目標(biāo)運(yùn)動(dòng)的不確定性，本文在特征檢測時(shí)選取加速魯棒特征(SURF， speeded up robust features)[16]作為目標(biāo)特征點(diǎn)，該特征點(diǎn)具有良好的旋轉(zhuǎn)和尺度不變性，且在光照變化時(shí)仍能保持較好的檢測效果。

后續(xù)在恢復(fù)尺度時(shí)使用KLT(kanade-lucas-tomasi)光流法跟蹤求解激光測距點(diǎn)的偽測量點(diǎn)，在求解相對位姿時(shí)，使用暴力匹配的基礎(chǔ)上，增加FLANN算法[17]進(jìn)行篩選，使匹配更為精確。

2.2 激光測距點(diǎn)偽測量值的計(jì)算與匹配

由于激光測距點(diǎn)可能沒有直接在像平面成像，或者成像了也無法確定像平面哪個(gè)點(diǎn)是激光測距點(diǎn)，所以其在第二幀圖像上的投影點(diǎn)坐標(biāo)無法像其他特征點(diǎn)一樣直接觀測得到。而是根據(jù)兩幀中的其他特征點(diǎn)的觀測值利用光流法計(jì)算出激光測距點(diǎn)在第二幀的偽觀測點(diǎn)。

針孔模型的像為倒像，而我們實(shí)際在使用相機(jī)時(shí)，看到的一般是正著的像，因?yàn)橄鄼C(jī)內(nèi)自身的軟件會(huì)翻轉(zhuǎn)輸出的圖像，所以將圖1中成像平面對稱挪到相機(jī)光心前方，如圖4所示。

圖4 帶對稱虛擬成像平面的投影模型

激光測距點(diǎn)為非合作目標(biāo)上空間點(diǎn)PL，激光測距儀距光心距離為d，讀數(shù)為l，則在初始狀態(tài)，空間點(diǎn)PL的世界坐標(biāo)為[x,d,l]T，成像點(diǎn)PL的世界坐標(biāo)為[X,Y,f]T，則有：

(1)

激光測距儀發(fā)射方向與相機(jī)坐標(biāo)系zc軸平行，落在ycoczc平面上，所以x為0，則點(diǎn)PL在物理成像坐標(biāo)系中的坐標(biāo)[X,Y]T為：

(2)

在求解非合作目標(biāo)相對位姿問題時(shí)，我們能得到的觀測值為特征點(diǎn)的像素坐標(biāo)，根據(jù)點(diǎn)在物理成像坐標(biāo)系中的坐標(biāo)求得該點(diǎn)在第一幀的像素坐標(biāo)系中坐標(biāo)為：

(3)

其中:dx、dv為單位距離內(nèi)像素的個(gè)數(shù)，當(dāng)焦距f單位為米時(shí)，1/dx和1/dv的單位為像素/米，[cx,cv]T是圖片中心點(diǎn)像素坐標(biāo)，fx、fv單位為像素，這些都是相機(jī)內(nèi)參，寫成矩陣形式為：

(4)

式中,l為激光測距儀讀數(shù)，矩陣K就是相機(jī)內(nèi)參矩陣，通過相機(jī)標(biāo)定獲得，向量PL為激光測距點(diǎn)PL坐標(biāo)。

以像素坐標(biāo)[u1,v1]T為中心，以200 pixels×200 picels的矩形為感興趣區(qū)域檢測匹配上的特征點(diǎn)，假設(shè)匹配到n對特征點(diǎn)，它們在第一幀圖像中的像素坐標(biāo)為[u1,i,v1,i]T，使用KLT光流法跟蹤得到它們在第二幀圖像中的像素坐標(biāo)為[u2,i,v2,i]T，其中i=1,2,3,...,n，則激光測距點(diǎn)投影在第二幀圖像中像素坐標(biāo)[ux2,v2]T為：

(5)

然后將第一幀圖像中的像素點(diǎn)[u1,v1]T與第二幀圖像中的像素點(diǎn)[u2,v2]T加入2.1中使用特征點(diǎn)描述子進(jìn)行暴力匹配的匹配特征對p1,i-p2,i中，一起進(jìn)行后續(xù)操作。

2.3 估計(jì)歸一化相機(jī)位姿與特征點(diǎn)空間坐標(biāo)

以旋轉(zhuǎn)矩陣R和平移向量t描述第二幀相機(jī)坐標(biāo)系相對于第一幀相機(jī)坐標(biāo)系的位姿關(guān)系，在初始化時(shí)，旋轉(zhuǎn)矩陣R和平移向量t也是第二幀相機(jī)相對于世界坐標(biāo)系的位姿。記非合作目標(biāo)上一空間點(diǎn)世界坐標(biāo)為P，該點(diǎn)投影在兩幀圖像中的像素坐標(biāo)分別為p1，p2，則有：

(6)

式中，z1和z2分別為該特征點(diǎn)在兩幀相機(jī)坐標(biāo)系下深度，根據(jù)目標(biāo)空間點(diǎn)與兩幀圖像對應(yīng)的相機(jī)光心共面的對極幾何關(guān)系，可以得到下面的對極約束：

(7)

其中:t∧表示位移向量t對應(yīng)的反對稱矩陣，記本質(zhì)矩陣E=t∧R。可利用五點(diǎn)法[18]來估計(jì)E，然后通過奇異值分解恢復(fù)出旋轉(zhuǎn)矩陣R和帶尺度的平移向量ts，在實(shí)際操作中，通常將ts進(jìn)行歸一化，即不論前面兩張圖片對應(yīng)的相機(jī)運(yùn)動(dòng)中的平移量為多少，都認(rèn)為相機(jī)平移了距離1，并以此為基準(zhǔn)計(jì)算特征點(diǎn)坐標(biāo)和更新后續(xù)的相對位姿。

然后根據(jù)三角測量原理求得兩個(gè)深度z1和z2，再根據(jù)式(6)，就得到了該特征點(diǎn)空間坐標(biāo)P，根據(jù)此方法可求得個(gè)特征點(diǎn)的帶尺度的空間坐標(biāo)Psi，(i=1,2,…,n)。此時(shí)求得的R是我們最終要得到的旋轉(zhuǎn)矩陣，但是向量ts與坐標(biāo)Psi的尺度都是模糊的。

將微膠囊分為3份相同質(zhì)量的樣品和硅膠放在一起,置于4、25和40 ℃的條件下避光貯藏,于0、10、20、30 d后,測定其精油殘留量,觀察精油的釋放規(guī)律,以此可驗(yàn)證預(yù)期壽命的準(zhǔn)確性。若初始精油量為Mo,精油殘留量為M,產(chǎn)品保留率公式如下:

2.4 相機(jī)位姿與特征點(diǎn)空間坐標(biāo)的尺度恢復(fù)

(8)

此時(shí)已知匹配特征點(diǎn)的像素坐標(biāo)和對應(yīng)的世界坐標(biāo)，可利用EPnP(efficient perspective-n-point)算法[19]求解世界坐標(biāo)系下的相機(jī)外參，即旋轉(zhuǎn)矩陣R和平移向量t。

此時(shí)第i個(gè)特征點(diǎn)的初始三維坐標(biāo)為Pi，相機(jī)的第二幀初始外參數(shù)為[Rt]，選取激光測距點(diǎn)為非合作目標(biāo)本體坐標(biāo)系原點(diǎn)，且目標(biāo)本體坐標(biāo)系的坐標(biāo)軸方向與第一幀相機(jī)坐標(biāo)系平行，則相機(jī)在第一幀與目標(biāo)初始相對姿態(tài)用ZYX旋轉(zhuǎn)的歐拉角表示為[0,0,0]T，初始相對位置為[0,d,l]T。

3 連續(xù)相對位姿估計(jì)

3.1 構(gòu)建緊耦合形式的聯(lián)合代價(jià)函數(shù)

(9)

上式中的RkPi+tk和TkPi中，Pi分別是非齊次坐標(biāo)和齊次坐標(biāo)。在第k幀可使用EPnP算法求解相機(jī)外參Tk，然后使用三角測量更新特征點(diǎn)三維坐標(biāo)，但是該方法只使用當(dāng)前幀的信息，隨時(shí)間推移存在估計(jì)漂移，累積誤差會(huì)越來越大，且沒有充分利用激光測距儀數(shù)據(jù)。為了解決估計(jì)漂移問題，充分利用激光測距儀數(shù)據(jù)和所有幀數(shù)據(jù)，本文在上述求解的相機(jī)外參和特征點(diǎn)三維坐標(biāo)基礎(chǔ)上，基于光束法平差(BA，bundle adjustment)優(yōu)化算法，根據(jù)特征點(diǎn)和激光測距點(diǎn)的估計(jì)值與觀測值構(gòu)建緊耦合形式的聯(lián)合代價(jià)函數(shù)，然后調(diào)整相機(jī)位姿最小化該代價(jià)函數(shù)，調(diào)整結(jié)束就得到優(yōu)化后的相機(jī)位姿。

記自變量x=[1,...,m]T為待優(yōu)化的變量，其中k為第k幀時(shí)相機(jī)外參對應(yīng)的李代數(shù)。記為第i個(gè)特征點(diǎn)在第k幀圖像中的觀測值，為其估計(jì)值，pi為第i個(gè)特征點(diǎn)世界坐標(biāo)，只考慮目標(biāo)特征點(diǎn)時(shí)，有以下代價(jià)函數(shù)：

(10)

只考慮激光測距儀時(shí)，有以下代價(jià)函數(shù)：

(11)

(12)

因?yàn)樘卣鼽c(diǎn)數(shù)量n遠(yuǎn)大于1，所以在代價(jià)函數(shù)式(12)中，激光測距儀信息相對于圖像信息權(quán)重占比極小，可以忽略不計(jì)，使得式(12)解算的結(jié)果同式(10)基本一致。為充分利用激光測距儀數(shù)據(jù)，考慮增加激光測距儀數(shù)據(jù)權(quán)重，優(yōu)化后構(gòu)建最終的聯(lián)合代價(jià)函數(shù)為：

(13)

3.2 聯(lián)合代價(jià)函數(shù)求解

對于式(13)，需要求解自變量x=[1,...,m]T，使得該代價(jià)函數(shù)最小?？梢詮腅PnP算法給定的初值開始，不斷地尋找自變量x沿代價(jià)函數(shù)梯度下降方向的增量Δx，使得f(x+Δx)更小，直到某個(gè)時(shí)刻自變量x的增量Δx非常小，無法再使函數(shù)下降，此時(shí)算法收斂，得到優(yōu)化后的自變量x，具體數(shù)學(xué)推導(dǎo)過程如下。

因?yàn)榇鷥r(jià)函數(shù)中有關(guān)激光測距儀的觀測誤差與無關(guān)，則可將代價(jià)函數(shù)變成以下形式：

(14)

(15)

(16)

然后可繼續(xù)將代價(jià)函數(shù)簡化為：

(17)

求上式關(guān)于Δx的導(dǎo)數(shù)，并令其為0，J為雅克比矩陣，可構(gòu)造關(guān)于Δx的增量方程為：

JTJΔx=-JTe

(18)

不斷迭代求解該增量方程直到Δx足夠小，無法再使函數(shù)下降，則得到了優(yōu)化后的相機(jī)外參。

4 實(shí)驗(yàn)結(jié)果與分析

為了驗(yàn)證本文算法的有效性，利用Blender軟件建立空間非合作目標(biāo)的三維模型，并生成該模型投影在單目相機(jī)中的序列圖像。

假設(shè)空間非合作目標(biāo)是一個(gè)失效衛(wèi)星，該衛(wèi)星主體是一個(gè)高1.88 m，寬1.14 m的八棱柱，兩個(gè)太陽能電池板平行于主體棱柱的軸線在主體兩側(cè)展開。設(shè)置光源為無限遠(yuǎn)處點(diǎn)光源，光線方向?yàn)閇1,1,0]T，來模擬空間中太陽光照。任務(wù)航天器的單目相機(jī)焦距f=0.069 8 m，視場角為28.91°×28.91°，圖像分辨率為800 pixels×600 pixels。在世界坐標(biāo)系下任務(wù)航天器的初始位置為[0,0,0]Tm，初始姿態(tài)為[0,0,0]T度，空間非合作目標(biāo)的初始位置為[0,0.1,24.43]Tm，初始姿態(tài)為[0,0,0]T度。

令服務(wù)航天器相對空間非合作目標(biāo)做繞飛觀測運(yùn)動(dòng)，飛行半徑為25 m，相對姿態(tài)以ω=[0,-1,0]T°/S的角速度繞世界坐標(biāo)中的Y軸旋轉(zhuǎn)，相對線速度為0.436 3 m/s，圖5給出了該相對運(yùn)動(dòng)的示意圖。服務(wù)航天器的相機(jī)采樣速率為1幀/秒，共采集100幀序列圖像，并在采樣圖像中添加高斯噪聲。假設(shè)激光測距儀采樣速率與相機(jī)采樣速率相同且同步，激光測距儀輸出的測量值為真實(shí)距離值加上高斯噪聲。圖6為利用Blender生成的部分圖像，由第一幀開始，從上到下，從左到右依次間隔30幀。

圖5 相機(jī)與目標(biāo)間相對運(yùn)動(dòng)

圖6 空間非合作目標(biāo)仿真圖像

選擇前兩幀圖像使用本文提出的方法初始化，圖7給出了初始化時(shí)圖像的特征檢測與匹配結(jié)果，將前兩幀灰度圖重合，紅色圓圈是第一幀上檢測到的SURF特征點(diǎn)，綠色加號是第二幀上檢測到的SURF特征點(diǎn)，可見匹配效果很好，沒有誤匹配，兩幀之間特征點(diǎn)移動(dòng)極小。

圖7 特征點(diǎn)檢測與匹配

為充分驗(yàn)證初始化算法的有效性和魯棒性，初始化仿真100次，結(jié)果如表1、圖8和圖9所示。分析可見本文提出的初始化方法的精度較高，相對位置估計(jì)的均方根誤差不超過0.02 m，最大誤差不超過0.07 m，相對姿態(tài)估計(jì)的均方根誤差不超過0.03°，最大誤差不超過0.10°。

表1 初始化時(shí)相對位姿估計(jì)誤差

圖8 初始化時(shí)相對位置估計(jì)誤差

圖9 初始化時(shí)相對姿態(tài)估計(jì)誤差

在初始化之后繼續(xù)用后續(xù)圖像序列進(jìn)行連續(xù)位姿估計(jì)，為充分驗(yàn)證算法有效性和魯棒性，連續(xù)進(jìn)行5次仿真實(shí)驗(yàn)，實(shí)驗(yàn)結(jié)果如表2、圖10和圖11所示。分析可見本文提出的連續(xù)位姿估計(jì)算法的精度較高，相對位置估計(jì)的均方根誤差不超過0.12 m，最大誤差不超過0.50 m，相對姿態(tài)估計(jì)的均方根誤差不超過0.29°，最大誤差不超過1.02°。

表2 連續(xù)測量時(shí)相對位姿估計(jì)誤差

圖10 連續(xù)相對位置估計(jì)誤差

圖11 連續(xù)相對姿態(tài)估計(jì)誤差

以上的結(jié)果能表明本文提出算法能很好地估計(jì)服務(wù)航天器與空間非合作目標(biāo)間的相對位姿，此外，為了評估算法的實(shí)時(shí)性，平均100次初始化用時(shí)和平均100幀連續(xù)位姿估計(jì)用時(shí)，可得初始化平均用時(shí)為0.07 s，連續(xù)位姿估計(jì)時(shí)平均每幀圖像用時(shí)為0.12 s，這說明了該算法能滿足實(shí)時(shí)要求。

5 結(jié)束語

針對空間任務(wù)中服務(wù)航天器實(shí)時(shí)估計(jì)空間非合作目標(biāo)的相對位置和姿態(tài)這一難題，提出了一種融合單目相機(jī)和激光測距儀的空間非合作目標(biāo)相對位姿緊耦合估計(jì)方法。該算法分為初始化和連續(xù)位姿估計(jì)兩部分，在初始化部分恢復(fù)了所有參數(shù)的真實(shí)尺度，構(gòu)建了真實(shí)尺度下的各個(gè)坐標(biāo)系；在連續(xù)位姿估計(jì)部分，以緊耦合的形式融合相機(jī)數(shù)據(jù)與激光測距儀數(shù)據(jù)來優(yōu)化相對位姿，即激光測距儀的信息不僅用于實(shí)時(shí)優(yōu)化特征點(diǎn)三維坐標(biāo)，還與圖像信息一起構(gòu)建目標(biāo)函數(shù)來優(yōu)化全局位姿，解決估計(jì)漂移問題。最后使用Blender軟件仿真了任務(wù)航天器以25 m的半徑相對空間非合作目標(biāo)做繞飛觀測的圖像，以此對算法進(jìn)行驗(yàn)證。結(jié)果顯示初始化時(shí)相對位置和姿態(tài)估計(jì)的均方根誤差分別不超過0.02 m和0.03°，連續(xù)位姿估計(jì)時(shí)相對位置和姿態(tài)估計(jì)的均方根誤差分別不超過0.12 m和0.29°。初始化平均用時(shí)為0.07 s，連續(xù)位姿估計(jì)時(shí)平均每幀圖像用時(shí)為0.12 s。表明該算法估計(jì)精度與實(shí)時(shí)性較高，理論上可以為自主交會(huì)、空間態(tài)勢感知、空間碎片清理等空間任務(wù)提供所需的空間非合作目標(biāo)位姿信息。