聯(lián)合EKF和EKPF的空間非合作目標(biāo)單目位姿估計(jì)

金澤明，汪 玲，劉 柯，杜榮華，張 翔

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

0 引 言

空間技術(shù)發(fā)展對完成復(fù)雜任務(wù)的需求日益增加，如抓捕或轉(zhuǎn)移空間碎片和廢棄衛(wèi)星、維修或更換有故障的在軌航天器、通過加注燃料延長衛(wèi)星的壽命等，這些任務(wù)要求追蹤任務(wù)航天器近距離精確估計(jì)目標(biāo)的相對位置和姿態(tài)[1-3]。目標(biāo)在非合作狀態(tài)下，沒有合作標(biāo)識以及星間鏈路等輔助進(jìn)行位姿測量，相對位姿估計(jì)具有挑戰(zhàn)性，難度大。根據(jù)非合作目標(biāo)的幾何模型是否已知，非合作目標(biāo)又可被分為模型已知非合作目標(biāo)和模型未知非合作目標(biāo)。依賴于非合作目標(biāo)的明顯的幾何特征或已知的三維模型，模型已知非合作目標(biāo)的相對位姿估計(jì)問題已經(jīng)得到了很好的解決[4-6]。相反，模型未知非合作目標(biāo)先驗(yàn)信息缺失、運(yùn)動(dòng)狀態(tài)不確定，其相對位姿估計(jì)問題更為復(fù)雜。

近年來，國內(nèi)外研究者開始關(guān)注利用移動(dòng)機(jī)器人同步定位與地圖構(gòu)建(Simultaneous localization and mapping，SLAM)[7]方法來解決模型未知非合作目標(biāo)的相對位姿估計(jì)問題。SLAM研究在靜止場景中估計(jì)運(yùn)動(dòng)相機(jī)的位姿，同時(shí)重建三維場景地圖，未知非合作目標(biāo)位姿估計(jì)則關(guān)注在相機(jī)位姿已知的情況下估計(jì)運(yùn)動(dòng)目標(biāo)的位姿，同時(shí)重建目標(biāo)的三維結(jié)構(gòu)。文獻(xiàn)[8]提出利用若干同步協(xié)作的三維視覺傳感器在不同方位對目標(biāo)拍攝，同時(shí)在軌估計(jì)目標(biāo)運(yùn)動(dòng)狀態(tài)、幾何尺寸以及質(zhì)量信息。文獻(xiàn)[9]提出一種基于特征的SLAM方法，該方法將目標(biāo)上一組已知的三維特征作為觀測輸入，利用擴(kuò)展卡爾曼濾波(Extended Kalman filter，EKF)作為導(dǎo)航濾波估計(jì)追蹤航天器與未知非合作目標(biāo)的相對狀態(tài)。文獻(xiàn)[10]針對空間失效航天器的自主維修問題，提出結(jié)合EKF-SLAM和隨機(jī)一致性采樣(Random sample consensus，RANSAC)算法來估計(jì)目標(biāo)相對位姿，恢復(fù)目標(biāo)三維結(jié)構(gòu)。文獻(xiàn)[11-12]提出利用增量式平滑建圖(Incremental smoothing and mapping，ISAM)[13]方法解決未知空間目標(biāo)繞其任意主慣性軸旋轉(zhuǎn)的SLAM問題，通過立體相機(jī)獲取的三維測量數(shù)據(jù)作為觀測輸入，該方法能夠估計(jì)目標(biāo)的整個(gè)相對狀態(tài)。相對于ISAM，使用迭代擴(kuò)展卡爾曼(Iterated extended kalman filter，IEKF)方法可以有效的提高算法的計(jì)算效率。對此，文獻(xiàn)[14]中提出利用IEKF算法估計(jì)未知空間非合作目標(biāo)的相對運(yùn)動(dòng)狀態(tài)。最后，文獻(xiàn)[15]提出了一種基于立體視覺EKF-SLAM的模型未知非合作翻滾目標(biāo)實(shí)時(shí)相對狀態(tài)估計(jì)方法。盡管卡爾曼濾波及其各種擴(kuò)展廣泛應(yīng)用于SLAM和相對導(dǎo)航領(lǐng)域，但仍有一些研究通過非線性優(yōu)化方法來解決未知非合作目標(biāo)的SLAM問題，文獻(xiàn)[16]提出一種基于先驗(yàn)子圖檢測改進(jìn)的SLAM算法，通過激光雷達(dá)采集目標(biāo)及周圍環(huán)境的點(diǎn)云數(shù)據(jù)，基于圖優(yōu)化SLAM技術(shù)估計(jì)失效航天器的相對位姿。文獻(xiàn)[17]提出一種基于位姿圖優(yōu)化的SLAM方法，該方法利用激光雷達(dá)采集的3D點(diǎn)云估計(jì)非合作旋轉(zhuǎn)目標(biāo)的相對位姿并重建其三維模型。

上述研究大多集中在利用三維視覺傳感器，如立體相機(jī)和激光雷達(dá)等估計(jì)未知非合作目標(biāo)的相對位姿。但是，應(yīng)該注意到這些三維傳感器也存在一些缺點(diǎn)，立體相機(jī)受限于相機(jī)基線距離對測量范圍和測量精度的影響，靈活性差、激光雷達(dá)價(jià)格昂貴、硬件復(fù)雜度高。當(dāng)采用體積小、功耗低、載荷資源有限的立方星等微小衛(wèi)星作為平臺衛(wèi)星時(shí)，這些三維傳感器往往難以得到應(yīng)用。相反，單目相機(jī)成本低、結(jié)構(gòu)簡單、靈活性強(qiáng)，適合于微小衛(wèi)星平臺，是微小衛(wèi)星具備自主在軌服務(wù)能力的關(guān)鍵[18]。然而，目前有關(guān)僅利用單目相機(jī)估計(jì)未知非合作目標(biāo)相對位姿的研究較少。文獻(xiàn)[19-20]提出了一種純單目視覺SLAM方法用于估計(jì)空間翻滾非合作目標(biāo)的相對位姿，并重建目標(biāo)的三維特征地圖，但該方法假定目標(biāo)初始位姿已知。文獻(xiàn)[21]針對此方法中的相對位姿估計(jì)部分進(jìn)行改進(jìn)，提出聯(lián)合無跡卡爾曼濾波(Unscented Kalman filter，UKF)和粒子濾波(Particle filter，PF)估計(jì)目標(biāo)的全部位姿參數(shù),但該方法依賴于UKF對稱分布采樣sigma點(diǎn)集合產(chǎn)生建議分布，計(jì)算量大。

本文針對微小衛(wèi)星自主抵近任務(wù)中模型未知非合作目標(biāo)相對位姿估計(jì)難題，采用單目相機(jī)獲取目標(biāo)圖像，在通過特征檢測、幀間特征匹配、三角測量等步驟計(jì)算相對位姿和特征點(diǎn)位置初始值后，考慮EKF的快速性和PF對非線性非高斯系統(tǒng)的處理能力，利用由EKF生成提議分布用于PF采樣的擴(kuò)展卡爾曼粒子濾波(Extended Kalman particle filter，EKPF)估計(jì)目標(biāo)相對位姿，利用擴(kuò)展卡爾曼濾波估計(jì)目標(biāo)特征點(diǎn)位置，實(shí)現(xiàn)無任何輔助信息的空間非合作目標(biāo)相對位姿的連續(xù)測量。

本文結(jié)構(gòu)安排如下：第1節(jié)給出任務(wù)星與非合作目標(biāo)之間的相對運(yùn)動(dòng)模型;第2節(jié)給出非合作目標(biāo)的測量方程;第3節(jié)詳細(xì)闡述特征初始化和幀間特征匹配方法，并給出具體的聯(lián)合EKF和EKPF的相對位姿和特征點(diǎn)位置估計(jì)算法;第4節(jié)對本文所提算法進(jìn)行仿真驗(yàn)證;第5節(jié)是結(jié)論。

1 相對運(yùn)動(dòng)模型構(gòu)建

1.1 坐標(biāo)系定義

圖1給出了各坐標(biāo)系的定義，包括任務(wù)星相機(jī)坐標(biāo)系OC-XCYCZC、像素坐標(biāo)系OI-uv、平面坐標(biāo)系OP-xy、目標(biāo)本體坐標(biāo)系OB-XBYBZB，其中相機(jī)焦距fc為相機(jī)光心OC到像平面主點(diǎn)OP的距離。目標(biāo)本體坐標(biāo)系固連在目標(biāo)上，坐標(biāo)系原點(diǎn)OB為目標(biāo)上某一特征點(diǎn)，坐標(biāo)軸指向在初始時(shí)刻與相機(jī)坐標(biāo)系重合。

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

1.2 相對姿態(tài)動(dòng)態(tài)模型

以歐拉角Ψ=[α,β,γ]T表示目標(biāo)本體坐標(biāo)系相對于相機(jī)坐標(biāo)系的相對姿態(tài)，規(guī)定轉(zhuǎn)動(dòng)順序?yàn)閆YX，旋轉(zhuǎn)方式為動(dòng)坐標(biāo)系旋轉(zhuǎn)，即相機(jī)坐標(biāo)系分別繞其Z軸、旋轉(zhuǎn)后的Y軸，再旋轉(zhuǎn)后的X軸旋轉(zhuǎn)α,β,γ角后，與目標(biāo)本體坐標(biāo)系重合。與歐拉角Ψ對應(yīng)的旋轉(zhuǎn)矩陣為：

(1)

(2)

假定目標(biāo)為剛體，根據(jù)剛體的轉(zhuǎn)動(dòng)定律，目標(biāo)的角加速度可表示為：

(3)

其中:J表示目標(biāo)的慣量矩陣，T表示目標(biāo)所受的合外力矩。

離散化式(2)和式(3)，相對姿態(tài)運(yùn)動(dòng)模型可表示為：

(4)

其中:下標(biāo)k，k-1表示離散時(shí)刻索引，Δt表示時(shí)間間隔。

假定未知非合作目標(biāo)處于失控下的翻滾狀態(tài)，J、T未知，由其引起的角速度變化可通過隨機(jī)高斯噪聲加以模擬，則式(4)可寫為：

(5)

其中：qω,k為零均值的高斯噪聲，即qω,k～N(0,Qω,k)，協(xié)方差Qω,k應(yīng)足夠大以充分考慮角速度變化的不確定性。

1.3 相對位置動(dòng)態(tài)模型

定義矢量ρ=[ρx,ρy,ρz]T表示目標(biāo)的相對位置，即目標(biāo)本體坐標(biāo)系的原點(diǎn)OB在相機(jī)坐標(biāo)系下的坐標(biāo)。定義矢量v為OB點(diǎn)速度，在相機(jī)坐標(biāo)系下表示為[vx,vy,vz]T。位置ρ和速度v之間是直接的時(shí)間微分關(guān)系，即:

(6)

目標(biāo)處于自由翻滾狀態(tài)，OB點(diǎn)速度v等于平動(dòng)速度vo和轉(zhuǎn)動(dòng)速度vr的矢量和，即:

v=vo+vr=vo+ω×rB

(7)

其中:上角標(biāo)“×”表示矢量的反對稱矩陣，rB表示OB點(diǎn)相對于目標(biāo)質(zhì)心的矢徑。

(8)

根據(jù)式(3)和牛頓第二定律，相對位置動(dòng)力學(xué)方程可表示為：

(9)

其中:F表示作用于目標(biāo)質(zhì)心的總外力，m表示目標(biāo)質(zhì)量。

離散化式(6)和式(9)，并采用隨機(jī)高斯噪聲模擬未知量F、T、rB引起的速度變化，構(gòu)建相對位置運(yùn)動(dòng)模型如下：

(10)

其中：qv,k為零均值的高斯噪聲，即qv,k～N(0,Qv,k)，協(xié)方差Qv,k應(yīng)足夠大以充分考慮各種可行線加速度。

2 測量方程構(gòu)建

(11)

(12)

在k時(shí)刻，相機(jī)的直接測量量為特征點(diǎn)j在像素坐標(biāo)系下的像素坐標(biāo)，根據(jù)相機(jī)內(nèi)參，可將其轉(zhuǎn)化為平面坐標(biāo)zj,k=[xj,k,yj,k]T，定義zk=[z1,k,z2,k, …,zM,k]T，函數(shù)g為測量函數(shù)，表示所有特征點(diǎn)透視投影函數(shù)gj(j=1,2,…,M)的組合，結(jié)合式(11)和式(12)構(gòu)建測量方程：

(13)

3 相對位姿估計(jì)

3.1 特征初始化和幀間特征匹配

當(dāng)單目相機(jī)獲取非合作目標(biāo)圖像后，檢測特征點(diǎn)得到的僅是特征點(diǎn)的像素坐標(biāo)，而無法獲取特征點(diǎn)的深度信息。根據(jù)多視圖幾何原理，從不同視角獲取目標(biāo)的多幀圖像，通過圖像預(yù)處理、特征檢測匹配和三角測量等操作可恢復(fù)特征點(diǎn)的深度信息。

考慮初始化速度，本文選取初始相鄰圖像進(jìn)行特征點(diǎn)初始化，算法流程如圖2所示。下面對其中的關(guān)鍵步驟處理做簡要介紹。

圖2 特征初始化和幀間特征匹配算法流程

1)特征檢測?？紤]空間光照環(huán)境復(fù)雜多變，非合作目標(biāo)旋轉(zhuǎn)平移的不確定性，本文選取SURF(Speeded up robust features，SURF)特征點(diǎn)作為目標(biāo)特征點(diǎn)，SURF特征具有良好的旋轉(zhuǎn)和尺度不變性，且在光照變化時(shí)仍能保持較好的檢測效果。

2)幀間特征匹配。在初始時(shí)刻完成SURF特征點(diǎn)檢測后，后續(xù)特征點(diǎn)匹配將采用KLT(Kanade-lucas-tomasi，KLT)光流法結(jié)合特征檢測來進(jìn)行跟蹤匹配。KLT光流法相對于利用特征描述子進(jìn)行特征匹配，計(jì)算效率高。但其基于相鄰兩幀圖像像素灰度不變的假設(shè)在空間光照變化以及相機(jī)存在自動(dòng)曝光導(dǎo)致像素過亮或過暗的情況下往往效果并不理想，因此，本文在KLT追蹤特征點(diǎn)的同時(shí)也進(jìn)行特征點(diǎn)的檢測，當(dāng)檢測的特征點(diǎn)位置和光流法預(yù)測的位置相接近時(shí)，則認(rèn)為這個(gè)特征點(diǎn)和第一幅圖中的對應(yīng)，并舍棄未匹配到的特征點(diǎn)。

3)三角測量。通過初始時(shí)刻的特征檢測和后續(xù)特征跟蹤獲取相鄰兩幀圖像二維特征點(diǎn)對應(yīng)后，進(jìn)行特征點(diǎn)初始化。在此，分別記相鄰兩幀為第一幀和第二幀。以旋轉(zhuǎn)矩陣R和平移向量t描述第二幀相機(jī)坐標(biāo)系相對于第一幀相機(jī)坐標(biāo)系的位姿關(guān)系，記目標(biāo)上一特征點(diǎn)在第一幀相機(jī)坐標(biāo)系下的坐標(biāo)為X，在第一幀和第二幀圖像上的投影像素坐標(biāo)分別為x1和x2，則存在:

(14)

其中:s1和s2分別為特征點(diǎn)在第一幀相機(jī)坐標(biāo)系和第二幀相機(jī)坐標(biāo)系下的深度，K為相機(jī)內(nèi)參矩陣。

根據(jù)對極幾何關(guān)系，由式(14)得到歸一化相機(jī)坐標(biāo)p1和p2，且p1和p2存在以下關(guān)系：

(15)

記本質(zhì)矩陣E=t×R。利用文獻(xiàn)[22]所述五點(diǎn)法計(jì)算本質(zhì)矩陣E，經(jīng)奇異值分解得到旋轉(zhuǎn)矩陣R和平移向量t。之后利用下式求解X在第一幀相機(jī)坐標(biāo)系下的坐標(biāo)：

(16)

其中:M1為單位矩陣，M2=[R,t]表示表示第一幀相機(jī)坐標(biāo)系到第二幀相機(jī)坐標(biāo)系的外參矩陣。

但是，需要注意的是：用于特征初始化的相鄰兩幀之間應(yīng)具有一定的平移，純旋轉(zhuǎn)變換無法提供額外的深度信息。此外，所求解的X的尺度是模糊的，需要更多的信息來確定實(shí)際的尺度。

3.2 聯(lián)合EKF和EKPF的相對位姿估計(jì)

(17)

本文基于FastSLAM算法的分解思想，將目標(biāo)狀態(tài)估計(jì)分解為相對位姿估計(jì)和以相對位姿為條件的特征點(diǎn)位置估計(jì)，利用EKPF估計(jì)目標(biāo)的相對位姿，利用EKF估計(jì)目標(biāo)的特征點(diǎn)位置。具體的算法流程如圖3所示。詳細(xì)步驟如下：

圖3 相對位姿和特征點(diǎn)位置估計(jì)算法流程

需要首先聲明的是，在下文中“∧”表示估計(jì)值，下標(biāo)“k/k-1”表示基于k-1時(shí)刻狀態(tài)估計(jì)進(jìn)行k時(shí)刻狀態(tài)預(yù)測值。

(18)

2)利用EKF生成PF的重要性概率密度函數(shù)，采樣新粒子集。具體流程如下：

(19)

(20)

(21)

(22)

其中:I12表示12維的單位矩陣。

3)基于EKF估計(jì)每個(gè)特征點(diǎn)的三維位置。假設(shè)目標(biāo)特征點(diǎn)服從基于相對位姿條件獨(dú)立的高斯分布，則在每個(gè)粒子所表示的相對位姿條件下，每個(gè)特征點(diǎn)位置都可以獨(dú)立于其他特征點(diǎn)位置進(jìn)行估計(jì)。本文利用M個(gè)EKF估計(jì)每個(gè)粒子的各個(gè)特征點(diǎn)位置，每個(gè)特征對應(yīng)一個(gè)EKF估計(jì)。具體流程如下:

(23)

根據(jù)式(12)預(yù)測特征點(diǎn)測量值：

(24)

(25)

(26)

(27)

當(dāng)有效粒子數(shù)Neff小于總粒子數(shù)一半時(shí)，進(jìn)行粒子重采樣。本文采用效率較高的系統(tǒng)重采樣算法[23]進(jìn)行重采樣。復(fù)制權(quán)值較大的粒子，拋棄權(quán)值較小的粒子，重采樣后平均粒子權(quán)值。

4 仿真校驗(yàn)

為驗(yàn)證本文所提算法的有效性，利用OpenGL(Open graphics library，OpenGL)建立目標(biāo)三維模型，并生成非合作目標(biāo)單目圖像序列。

假設(shè)目標(biāo)長、寬、高約為12 m、1 m、1 m，設(shè)置OpenGL光源為點(diǎn)光源，位置位于無窮遠(yuǎn)處，光照類型為散射光，以模擬空間太陽光照，目標(biāo)以角速度ω繞偏航軸旋轉(zhuǎn)，同時(shí)任務(wù)星以速度v接近目標(biāo)，其中

(28)

初始時(shí)刻目標(biāo)在相機(jī)坐標(biāo)系下的位置為ρ0=[-0.3, 0.3, 20]Tm。相機(jī)焦距fc=20 mm，視場角為36°。圖像分辨率為512 pixels×512 pixels，圖像采樣速率為1幀/秒，共采集120幀序列圖像，其中添加高斯噪聲。圖4給出了利用OpenGL所采集的部分圖像，由第1幀開始，從上到下，從左到右依次間隔12幀。

圖4 利用OpenGL生成的非合作目標(biāo)仿真圖像

選取圖像序列中的初始兩幀圖像，在經(jīng)過濾波、去噪等一系列圖像預(yù)處理操作后，進(jìn)行特征初始化。

考慮到仿真圖像中目標(biāo)太陽能帆板對光照變化的不穩(wěn)定性，本文選擇較為穩(wěn)定的目標(biāo)本體上8個(gè)SURF特征點(diǎn)作為觀測特征點(diǎn)?？紤]相鄰兩幀圖像之間目標(biāo)與任務(wù)星間的相對運(yùn)動(dòng)較為平緩，幀間特征匹配將采用KLT光流法結(jié)合特征檢測來進(jìn)行。圖5給出了第60幀圖像的特征跟蹤結(jié)果，其中白色實(shí)心圓表示所選擇的8個(gè)SURF特征點(diǎn)的初始位置，序號以白色文本箭頭標(biāo)出，相應(yīng)的特征點(diǎn)坐標(biāo)見表1，白色空心圓表示當(dāng)前幀圖像中跟蹤到的特征點(diǎn)位置，兩者之間的對應(yīng)匹配以白色實(shí)線表示。

圖5 第60幀圖像的特征跟蹤結(jié)果

表1 特征點(diǎn)位置的初始值

仿真中采樣粒子數(shù)為100，假定過程噪聲和測量噪聲均為零均值的高斯白噪聲。為充分驗(yàn)證算法，共進(jìn)行5次仿真實(shí)驗(yàn)。相對位姿估計(jì)結(jié)果如圖6～圖9所示，圖中實(shí)線表示真實(shí)值，虛線表示本文方法所得出的估計(jì)值。

圖6和圖7分別為相對姿態(tài)和相對角速度估計(jì)結(jié)果。從兩圖可以看出，估計(jì)值曲線和真實(shí)值曲線基本吻合，分別計(jì)算三軸姿態(tài)角平均誤差約為0.88°、 1°、0.5°，三軸相對角速度平均誤差約為0.11 (°)/s、0.16 (°)/s、0.08 (°)/s，表明本文方法具有較高的姿態(tài)估計(jì)精度。此外，在61幀目標(biāo)角速度發(fā)生突變時(shí)，該方法仍能快速的跟蹤目標(biāo)的姿態(tài)變化，表明該方法對相對姿態(tài)和相對角速度估計(jì)具有較好的魯棒性。

圖6 相對姿態(tài)估計(jì)

圖7 相對角速度估計(jì)

圖8和圖9分別為相對位置和相對速度估計(jì)結(jié)果。分析圖8和圖9可知，本文算法能夠?qū)崿F(xiàn)相對位置和相對速度的精確估計(jì)，相對位置的平均誤差分別約為0.07 m、0.09 m、0.26 m，相對速度的平均誤差約為0.001 m/s、0.001 m/s、0.011 m/s。當(dāng)任務(wù)星在61幀平移線速度突變?yōu)?，相對位置保持不變時(shí)，估計(jì)值仍能夠快速收斂于真實(shí)值附近，表明該方法對相對位置和相對速度估計(jì)具有較好的魯棒性。

圖8 相對位置估計(jì)

圖9 相對速度估計(jì)

定義特征點(diǎn)位置的估計(jì)誤差eXB為：

(29)

以誤差曲線的形式給出特征點(diǎn)位置的估計(jì)誤差，其結(jié)果如圖10所示。

分析圖10可知，目標(biāo)特征點(diǎn)誤差估計(jì)曲線在61幀以前收斂在0.03 m附近，隨著角速度和線速度在61幀發(fā)生突變，特征點(diǎn)誤差曲線發(fā)生波動(dòng)，隨后收斂在0.045 m附近。此結(jié)果表明本文所提算法能夠很好的估計(jì)目標(biāo)的特征點(diǎn)位置。

圖10 特征點(diǎn)位置估計(jì)誤差

此外，為評估算法的實(shí)時(shí)性，平均120幀圖像估計(jì)總耗時(shí)，可得估計(jì)每幀圖像算法耗時(shí)約為0.06 s，這說明了該算法滿足實(shí)時(shí)要求。

5 結(jié) 論

本文針對微小衛(wèi)星自主抵近模型未知非合作目標(biāo)任務(wù)中相對位姿估計(jì)難題，在建立相對位姿運(yùn)動(dòng)模型和測量方程的基礎(chǔ)上，提出一種聯(lián)合EKF和EKPF估計(jì)相對位姿、特征點(diǎn)位置的方法，基于OpenGL生成的非合作目標(biāo)圖像進(jìn)行仿真驗(yàn)證，結(jié)果表明本文所提算法具有較優(yōu)的估計(jì)精度，較好的魯棒性和實(shí)時(shí)性，能夠?yàn)樽灾鞯纸蝿?wù)提供必需的位姿信息。但是，應(yīng)該注意到，由于單目初始化的尺度模糊性，僅憑單目相機(jī)無法獲得相對位置和目標(biāo)特征點(diǎn)的實(shí)際坐標(biāo)，可以借助距離傳感器解決尺度問題。