聯合EKF和EKPF的空間非合作目標單目位姿估計

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

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

0 引 言

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

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

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

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

本文結構安排如下：第1節給出任務星與非合作目標之間的相對運動模型;第2節給出非合作目標的測量方程;第3節詳細闡述特征初始化和幀間特征匹配方法，并給出具體的聯合EKF和EKPF的相對位姿和特征點位置估計算法;第4節對本文所提算法進行仿真驗證;第5節是結論。

1 相對運動模型構建

1.1 坐標系定義

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

圖1 坐標系定義

1.2 相對姿態動態模型

以歐拉角Ψ=[α,β,γ]T表示目標本體坐標系相對于相機坐標系的相對姿態，規定轉動順序為ZYX，旋轉方式為動坐標系旋轉，即相機坐標系分別繞其Z軸、旋轉后的Y軸，再旋轉后的X軸旋轉α,β,γ角后，與目標本體坐標系重合。與歐拉角Ψ對應的旋轉矩陣為：

(1)

(2)

假定目標為剛體，根據剛體的轉動定律，目標的角加速度可表示為：

(3)

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

離散化式(2)和式(3)，相對姿態運動模型可表示為：

(4)

其中:下標k，k-1表示離散時刻索引，Δt表示時間間隔。

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

(5)

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

1.3 相對位置動態模型

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

(6)

目標處于自由翻滾狀態，OB點速度v等于平動速度vo和轉動速度vr的矢量和，即:

v=vo+vr=vo+ω×rB

(7)

其中:上角標“×”表示矢量的反對稱矩陣，rB表示OB點相對于目標質心的矢徑。

(8)

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

(9)

其中:F表示作用于目標質心的總外力，m表示目標質量。

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

(10)

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

2 測量方程構建

(11)

(12)

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

(13)

3 相對位姿估計

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

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

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

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

1)特征檢測。考慮空間光照環境復雜多變，非合作目標旋轉平移的不確定性，本文選取SURF(Speeded up robust features，SURF)特征點作為目標特征點，SURF特征具有良好的旋轉和尺度不變性，且在光照變化時仍能保持較好的檢測效果。

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

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

(14)

其中:s1和s2分別為特征點在第一幀相機坐標系和第二幀相機坐標系下的深度，K為相機內參矩陣。

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

(15)

記本質矩陣E=t×R。利用文獻[22]所述五點法計算本質矩陣E，經奇異值分解得到旋轉矩陣R和平移向量t。之后利用下式求解X在第一幀相機坐標系下的坐標：

(16)

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

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

3.2 聯合EKF和EKPF的相對位姿估計

(17)

本文基于FastSLAM算法的分解思想，將目標狀態估計分解為相對位姿估計和以相對位姿為條件的特征點位置估計，利用EKPF估計目標的相對位姿，利用EKF估計目標的特征點位置。具體的算法流程如圖3所示。詳細步驟如下：

圖3 相對位姿和特征點位置估計算法流程

需要首先聲明的是，在下文中“∧”表示估計值，下標“k/k-1”表示基于k-1時刻狀態估計進行k時刻狀態預測值。

(18)

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

(19)

(20)

(21)

(22)

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

3)基于EKF估計每個特征點的三維位置。假設目標特征點服從基于相對位姿條件獨立的高斯分布，則在每個粒子所表示的相對位姿條件下，每個特征點位置都可以獨立于其他特征點位置進行估計。本文利用M個EKF估計每個粒子的各個特征點位置，每個特征對應一個EKF估計。具體流程如下:

(23)

根據式(12)預測特征點測量值：

(24)

(25)

(26)

(27)

當有效粒子數Neff小于總粒子數一半時，進行粒子重采樣。本文采用效率較高的系統重采樣算法[23]進行重采樣。復制權值較大的粒子，拋棄權值較小的粒子，重采樣后平均粒子權值。

4 仿真校驗

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

假設目標長、寬、高約為12 m、1 m、1 m，設置OpenGL光源為點光源，位置位于無窮遠處，光照類型為散射光，以模擬空間太陽光照，目標以角速度ω繞偏航軸旋轉，同時任務星以速度v接近目標，其中

(28)

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

圖4 利用OpenGL生成的非合作目標仿真圖像

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

考慮到仿真圖像中目標太陽能帆板對光照變化的不穩定性，本文選擇較為穩定的目標本體上8個SURF特征點作為觀測特征點。考慮相鄰兩幀圖像之間目標與任務星間的相對運動較為平緩，幀間特征匹配將采用KLT光流法結合特征檢測來進行。圖5給出了第60幀圖像的特征跟蹤結果，其中白色實心圓表示所選擇的8個SURF特征點的初始位置，序號以白色文本箭頭標出，相應的特征點坐標見表1，白色空心圓表示當前幀圖像中跟蹤到的特征點位置，兩者之間的對應匹配以白色實線表示。

圖5 第60幀圖像的特征跟蹤結果

表1 特征點位置的初始值

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

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

圖6 相對姿態估計

圖7 相對角速度估計

圖8和圖9分別為相對位置和相對速度估計結果。分析圖8和圖9可知，本文算法能夠實現相對位置和相對速度的精確估計，相對位置的平均誤差分別約為0.07 m、0.09 m、0.26 m，相對速度的平均誤差約為0.001 m/s、0.001 m/s、0.011 m/s。當任務星在61幀平移線速度突變為0，相對位置保持不變時，估計值仍能夠快速收斂于真實值附近，表明該方法對相對位置和相對速度估計具有較好的魯棒性。

圖8 相對位置估計

圖9 相對速度估計

定義特征點位置的估計誤差eXB為：

(29)

以誤差曲線的形式給出特征點位置的估計誤差，其結果如圖10所示。

分析圖10可知，目標特征點誤差估計曲線在61幀以前收斂在0.03 m附近，隨著角速度和線速度在61幀發生突變，特征點誤差曲線發生波動，隨后收斂在0.045 m附近。此結果表明本文所提算法能夠很好的估計目標的特征點位置。

圖10 特征點位置估計誤差

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

5 結 論

本文針對微小衛星自主抵近模型未知非合作目標任務中相對位姿估計難題，在建立相對位姿運動模型和測量方程的基礎上，提出一種聯合EKF和EKPF估計相對位姿、特征點位置的方法，基于OpenGL生成的非合作目標圖像進行仿真驗證，結果表明本文所提算法具有較優的估計精度，較好的魯棒性和實時性，能夠為自主抵近任務提供必需的位姿信息。但是，應該注意到，由于單目初始化的尺度模糊性，僅憑單目相機無法獲得相對位置和目標特征點的實際坐標，可以借助距離傳感器解決尺度問題。