空間失效慢旋衛星視覺特征跟蹤與位姿測量

2021-03-27 12:39:58劉宗明牟金震張碩杜宣曹姝清張宇

航空學報 2021年1期

劉宗明，牟金震，張碩，杜宣，曹姝清，張宇,*

1. 上海航天控制技術研究所, 上海 201109 2. 上海市空間智能控制技術重點實驗室, 上海 201109 3. 上海航天技術研究院, 上海 201109

針對諸如空間碎片、失效衛星和小行星等非合作目標的相對測量問題，其中一種解決思路是利用視覺傳感器直接對目標進行在線三維重建。隨著機器人技術的快速發展，同步定位與制圖(SLAM)[1]技術可以有益地借鑒到針對非合作目標的測量任務中來。SLAM技術已在機器人領域得到了廣泛的應用，它可以幫助機器人在一個未知的環境中計算出自己的運動軌跡，同時可以重建周圍環境的地圖。在大多數應用中，機器人運行的環境被認為是靜止不動的，但是如果動態場景中存在的是剛體目標的話，SLAM也是可以處理的。因而，完全可以將SLAM技術應用到空間非合作目標的測量中來。

Sonnenburg等[2]提出了一種基于特征和EKF(Extended Kalman Filter)濾波器的SLAM算法，該算法聚焦于相對導航濾波器結構設計，特征點集假設利用雙目相機已經完成了提取，并且作為輸入提供給濾波器；在濾波器中建立的平移運動方程是基于Hill相對運動模型的，該模型假設目標飛行器和追蹤飛行器之間的相對軌道是圓形的，但是濾波器的狀態量中不包含目標的慣量信息；考慮了兩種不同的絕對轉動動力學情況，在這兩種情況中都忽略了目標飛行器和追蹤飛行器在LVLH(Local Vertical Local Horizontal)坐標系下的指向差異；同時，提出了利用EKF進行單目相機特征提取和跟蹤的濾波器框架。

Augenstein和Rock[3]提出了一種基于單目視覺SLAM的非合作目標位姿跟蹤和外形重建算法，利用了Rao-Blackwellized粒子濾波器架構計算相對旋轉和平移動力學；假設目標以恒定角速度進行旋轉運動，并且為了給濾波器提供一個先驗估計，同時需要假設初始位姿已經獲得；由于算法只能得到目標的離散三維點云圖，所以無法對慣性矩進行估計；對算法進行了數學仿真，并且利用水下繩系目標進行了試驗驗證。

Schnitzer等[4]融合了基于EKF濾波的SLAM算法和基于RANSAC(RANdom SAmple Consensus)的表面重建算法對非合作目標進行建模，利用SURF(Speeded-Up Robust Features)算子[5]描述目標表面的特征點，基于立體視覺原理計算目標特征點的位置信息；利用小型接近操作模擬器在實驗室內對算法性能進行了驗證，試驗結果表明盡管存在噪聲干擾，同時目標三維點云的分布是稀疏的、不均勻的，該算法仍然能夠得到較好的表面重建結果，但完成一次計算需要耗時幾秒鐘，實時性難以保證。

Tweddle[6]在其博士畢業論文中在Kaess等[7]工作的基礎上提出了一種繞其慣性主軸旋轉的非合作目標SLAM測量方法，該算法采用的平滑濾波器相較于常規的卡爾曼濾波器而言具有更好的收斂性，但是帶來的問題同樣也是計算負擔較重、耗時較長；該算法假設追蹤飛行器靜止不動，模型方程中不考慮外部干擾力矩對相對運動動力學的影響；Tweddle[6]利用球形衛星模型對算法進行了試驗驗證，基于雙目視覺完成三維重建，結合濾波器可以實現對完整的相對狀態量的估計，包括目標線速度和角速度、質心位置和慣性主軸等。

Segal等[8]提出了一種基于多iEKF(iterated Extended Kalman Filter)濾波器和最大后驗概率的慣性張量辨識算法，采用雙目視覺的方式獲取目標特征點的三維位置信息，濾波器方程中基于希爾方程建立相對運動模型，同時考慮了旋轉和平移運動的動力學耦合[9]，通過數學仿真對算法性能進行了評估；在仿真中假設通過雙目視覺相機可以穩定地獲得均勻分布于目標表面的10個特征點，雙目相機的基線為1 m，最遠測量距離大于100 m，高斯噪聲為10-5rad，平均測量精度為厘米級。

Conway等[10]提出了一種微小型目標接近過程的導航策略，采用了RGB-D(RGB-Deepth)相機，采樣幀頻為30 fps，可以同時獲得圖像信息和深度信息；特征點提取和匹配采用的是ORB(Oriented fast and Rotated Brief)描述子，通過二維圖像特征點及其對應深度值的匹配可以得到特征點的三維信息；進行了數學仿真，并利用微軟公司的Kinect進行了測試試驗。

Pesce等[11]提出了一種相對運動估計和慣性參數重建的iEKF方法，文獻[11]中的濾波器結構與文獻[6]相似，但是文獻[11]采用的平滑方法提高了計算效率；由于文獻[11]并未考慮外部干擾力矩對相對旋轉動力學模型的影響，因為只能求得慣量比，但是可以通過一個離線的圖像處理過程獲得目標完整的三維重建模型；與文獻[8]類似，Pesce等[11]利用簡化的立體視模型，通過數學仿真對算法性能進行了驗證，并且對離線三維重建算法進行了初步的測試評估。

從現有文獻的分析來看，目前針對非合作旋轉目標的測量方案主要還是集中在基于特征的濾波器設計方面，利用濾波算法對目標狀態進行預測，常用的濾波方法在一個較小的運動尺度和較低的運動速度范圍內，通過持續地維護和升級攝像機狀態向量和特征點向量進行測量，可以表現出較好的跟蹤效果。但是其計算精度受限于所構建的狀態模型和測量模型的精度，模型的非線性和相機的測量噪聲等會導致測量誤差放大，甚至發散。計算速度也受到狀態向量維數的嚴重制約，即使是通過減少特征數來保持計算效率，也很難避免高維的狀態向量導致的“維數災難”問題[12]。

為了克服以上問題，本文構建了目標特征數據庫，將當前幀與特征數據庫進行比較，完成特征匹配，采用關鍵幀共視圖對特征點進行搜索跟蹤。基于位姿圖優化完成對目標位姿的局部和全局最優化求解。在李代數空間下建立位姿向量微分擾動方程，獲得測量值與估計值的殘差目標函數，基于貝葉斯法則最大后驗概率和李群與李代數的對指變換法則，求取位姿向量最優解，以期解決李群空間下由于位姿變換矩陣不可加性而無法優化的問題，提高連續測量過程中系統的測量精度。

1 失效慢旋衛星視覺特征跟蹤

1.1 構建目標特征點數據庫

空間的光照條件較為復雜，視覺系統采集到的目標航天器圖像會發生部分灰度退化或者幾何退化，如圖像模糊、圖像對比度變化和圖像灰度退化等。主要的干擾因素如下：

1) 目標表面反射不均勻。由于空間目標表面大都包覆有高反射系數的熱控多層保溫材料，太陽光、目標表面和相機在一定的角度范圍內會出現目標成像效果局部過亮或過暗的現象，導致特征點無法有效提取。

2) 雜散光干擾。相機鏡頭安裝在航天器艙外，所以會受到光的照射，陽光、地氣光、月光、恒星光和行星光等天體照射艙體外各種部件導致光反射，形成大量的雜散光干擾。

特征點提取的快速性、描述子的尺度不變性和旋轉不變性是實現非合作旋轉目標實時測量的基礎保證。為了保證特征的不變特性，需要在多個尺度空間下對圖像特征點進行提取，并且賦予旋轉不變特性的描述子，將高維的原始圖像轉化為低維的描述向量。目標特征子的提取速度和描述子的復雜程度將直接影響測量系統的計算效率和匹配效果，選取的圖像特征表達在提高計算速度的同時，也要保證在后續圖像匹配時的正確率，這兩點同時滿足才能保證特征檢測的精度與速度。為保證特征匹配的尺度不變性，使用尺度因子將當前幀圖像轉換到8個尺度空間上進行FAST(Features from Accelerated Segment Test)角點提取。為了確保在圖像核心目標區域角點的均勻分布，對每個尺度的圖像進行網格劃分，將其等分為若干份，在每個網格中至少提取5個角點。最后對提取到的FAST角點進行Brief(Binary robust independent elementary features)描述子和方向的計算，共同組成一個ORB特征點。

在針對非合作目標的測量過程中，目標模型的具體尺寸是無法預先得知的，目標表面的紋理特征就更無法提前獲取了。當前時刻位姿取決于前一時刻的位姿和前一時刻與當前時刻的位姿變換的乘積，該流程雖然計算簡單，運算量少，但是如果當前幀中特征點缺失，容易造成跟蹤失敗。并且當前一幀的位姿估計誤差較大時，還會出現顯著的漂移，造成誤差累積，直至發散。因而考慮構建目標的二維和三維特征數據庫，關注的重點變為當前幀與特征數據庫的準確跟蹤匹配和特征數據庫中特征點的優化問題。假設只考慮前后兩圖像幀之間運動關系，設前一幀為參考幀，用ref表示，當前幀用cur表示，以參考幀為基準，計算當前幀與其的運動關系。假設參考幀和當前幀相對世界坐標系的變換矩陣分別為Trw和Tcw，兩幀之間的變換矩陣為Tcr，則它們之間的關系可表示為

(1)

在圖1中，在t0～t1時刻，以t0為參考，求t1時刻的運動；然后在t1～t2時刻，又以t1時刻為參考幀，考慮t1～t2時刻間的運動關系，如此往復。如果當前幀或參考幀中紋理稀少、光照變化和遮擋等原因導致特征點缺失，容易造成跟蹤失敗。并且，當參考幀的位姿估計誤差較大時，還會出現顯著的漂移，造成誤差累積，直至發散。因此，考慮構建目標的二維和三維特征數據庫，將當前幀與特征數據庫進行比較，如圖2所示。問題轉變為相機在每一個t時刻采集到的圖像與之前時刻重建的特征數據庫進行準確匹配和特征數據庫中特征點的優化問題。在一個不斷更新的特征點數據庫中，只要特征點正確，即使過程幀出現了差錯，仍然有可能求出后續圖像幀的正確位置。特征數據庫又分為局部和全局兩種，局部特征數據庫描述了當前幀附近的特征信息，可以提高當前位姿的計算效率。全局特征數據庫則記錄了系統運行以來，根據一定規則選擇保留的特征點，主要用于閉環檢測。

目標數據庫的特征點是指可靠的目標三維點，每個數據庫特征點存包含的信息包括：

1) 其在世界坐標系下的三維坐標。

2) 平均視線方向(由該點指向所有共有這個點的關鍵幀光心的平均法向量)。

3) ORB特征算子[13]和描述子[14]。

4) 根據ORB尺度因子(1.2)確定的最大和最小可視范圍。

其中，關鍵幀是指從連續采集的圖像中選出來的幀，每個關鍵幀包括：

1) 相機姿態矩陣。

2) 相機的內參矩陣。

3) 本幀所有的ORB特征點。

為保證特征庫點云數量控制在一定范圍內，不至于過于膨脹，當有更合適點出現時，需要將其加入到特征庫中；當出現冗余或相似點云時，需要將其從特征庫中剔除。

1.2 利用特征數據庫進行特征點跟蹤

根據上一幀和勻速運動模型估計的當前幀位姿變換為

(2)

假設相機的內參為K，上一幀的三維特征點集為Piw，其在當前幀中的投影為

(3)

將估計的特征點與實際提取的ORB特征點進行匹配，如果當前幀與上一幀沒有足夠的匹配點，通過在上一幀預估的位置附近擴大一定的搜索范圍繼續搜索。當找到對應的匹配點后，利用它們對相機的預估位姿進行基于捆集調整優化處理，便可以得到當前幀較為準確的位姿變換。為了提高測量的可靠性和精度，在得到了一個相機的位姿估計和一個初始的特征匹配集的基礎上，便可以將特征數據庫中的三維點投影到當前幀中搜索更多的匹配點對。為了限制計算的復雜度，只投影局部三維特征數據庫，此局部特征數據庫包括一個關鍵幀集合K1，此集合與當前幀有公共的三維特征點；還包括一個關鍵幀集合K2，此集合與K1相鄰，具有共視圖；最后還包括一個參考關鍵幀Kref，其中，Kref∈K1，Kref與當前幀有最多的共視點[15]。它們之間的關系如圖3所示。其中，Fcur表示當前幀；K11、K12和K13構成集合K1；K13為參考關鍵幀Kref；K21、K22和K23構成集合K2。在測量過程中，關鍵幀和特征點數據集不應該一直無限增大，這樣不僅是對搜索的運算量還是存儲空間而言都是巨大的壓力。在局部建圖過程中有一個冗余關鍵幀和特征點剔除機制，因此，在跟蹤過程中要實時確定該點是否為可用特征點；在跟蹤的最后一步是確定當前幀是否為關鍵幀，這樣可以使得對相機運動的追蹤變得更加穩定。每一個在K1和K2集合中可以看到的三維點在當前幀中按照以下規則進行搜索：

1) 計算三維點在當前幀中的投影點，如果其超出了預先設置的圖像邊界，將該點剔除，不將其用于特征接力。

2) 計算當前視軸(當前幀法向量)和該三維點的平均視線(該三維點指向所屬的所有關鍵幀光心的平均法向量)的夾角，如果大于60°則將該點剔除，不將其用于特征接力。

3) 計算該三維點到當前幀所對應的光心距，如果超出其對應的尺度不變域范圍則將該點剔除，不將其用于特征接力。

4) 計算當前幀尺度。

在尺度范圍內，將估計的特征點與實際提取的ORB特征點進行匹配，如果當前幀與上一幀沒有足夠的匹配點，則通過在上一幀預估的位置附近擴大一定的搜索范圍繼續搜索。當找到對應的匹配點后，則將其與當前三維點關聯起來，據此，可以將當前幀與特征數據庫中的公共點對應起來，完成特征接力。利用當前幀中的特征點對相機的位姿進行基于捆集調整優化處理，便可以得到當前幀較為準確的位姿變換。

2 李代數空間下的位姿圖優化

2.1 圖優化概念

假設狀態變量x=[x1,x2,…,xn]T，其中xk表示在k時刻相機的位姿節點。兩節點之間的觀測方程為

zij=h(xi,xj)+vij

(4)

式中：h為測量函數；vij為觀測噪聲，由于各種誤差因素的存在，產生了測量殘差：

eij(xi,xj)=zij-h(xi,xj)

(5)

假設以xk為優化變量，對于一個具有n條邊的圖，其每條邊的優化目標函數為

(6)

式中：Ωij為信息矩陣，是協方差陣的逆，是一個對稱矩陣。所有邊的優化目標函數為

(7)

最終的優化目標誤差函數可以寫成：

(8)

(9)

(10)

(11)

如果考慮所有邊，則式(11)可改寫為

HΔx=-b

(12)

式中:H為信息矩陣;b為標量系數。

系統的解就是在初始值上疊加此增量：

(13)

GN迭代就是不斷重復式(13)直至收斂，LM迭代則引入了一個松弛因子λ控制迭代速度：

(H+λI)Δx=-b

(14)

由于GN迭代有可能出現雅可比矩陣乘積奇異或者病態的情況，導致增量的穩定性差，算法因此將會發散，LM迭代在一定程度上修正了這個問題，因而采用LM迭代。

2.2 位姿圖優化

在2.1節中定義的目標誤差函數可能會沒有意義，因為加法操作對位姿變換矩陣而言是不封閉的，即兩個變換矩陣的和并不是變換矩陣。而且，在構建優化問題時，旋轉矩陣作為待計算的優化變量，必須是一個行列式為1的正交矩陣，這樣會引入額外的約束條件，使優化問題變得更復雜，因而不便于在李群空間下直接對其進行優化操作。筆者先在李代數空間進行加法與求導操作，再將計算結果轉換為李群[19]，便可完成對測量誤差函數的優化求解。

在剛體變換過程中定義三維歐氏群為

SE(3)=

(15)

式中：g為位姿變換矩陣；SO(3)為三維旋轉群。

定義三維歐氏群對應的李代數為

se(3)=

(16)

式中：ξ為一個六維向量；上標∧代表求反對稱陣運算；ω為旋轉角速度；v為速度；so(3)為三維旋轉李代數。

李代數對應的六維向量為ξ=[v，ω]T∈6，可以得到三維歐氏李代數到三維歐氏李群的指數映射：

(17)

式中：eξ為李代數到李群的指數映射；eω∧為旋轉角速度的指數映射；V為變換系數,可表示為

(18)

式中：α為三維向量ω的方向；θ為三維向量ω的模。

由于ω是一個三維向量，由以上分析可知，相應的對數運算可以將李群映射到李代數：

(19)

式中：上標∨表示從矩陣到向量的運算。

在位姿圖優化中，節點表示相機的位姿，用ξ1,ξ2,…,ξn表述；邊表示兩個位姿節點之間相對運動狀態的估計。假設兩個位姿節點在三維歐氏李代數se(3)上的表示為ξi和ξj，它們之間的運動估計為ξij，則其對應關系為

(20)

如果在三維歐氏李群SE(3)上的表示為Ti、Tj和位姿i、j間的變換矩陣Tij，則其對應關系為

(21)

構建誤差函數：

(22)

利用李代數擾動模型將誤差函數eij分別對ξi和ξj各加一個左擾動δξi和δξj，式(20)變為

(23)

根據SE(3)上的伴隨性質[20]：

eξ∧T=Te(Ad(T-1)ξ)∧

(24)

式中：

(25)

(26)

根據李代數上的求導法則可以得到關于位姿Ti和Tj的雅可比矩陣，其中：

(27)

式中：Jr為雅可比矩陣。

在此對雅可比矩陣行近似，可以得到

(28)

3 測量實驗與結果

實驗環境為超近距離相對運動實驗室，如圖6所示；所用運動裝置為高精度三軸轉臺，如圖7所示。對目標星的模擬采用了1∶1的真實模型，表面貼有熱控多層反光材料，同時，采用高亮度LED光源模擬入射的太陽光。模型分別固定于轉臺上，以一定的角速度繞豎直軸勻速運動。由于目標的轉動，相機成像的亮度值發生了明顯變化，甚至出現曝光飽和現象。實驗中先后進行了基于前后圖像幀信息的EKF濾波算法、無位姿優化和有位姿優化的測量算法，從實驗結果來看，所提算法能較好地適應光照條件變化，具有較強的魯棒性。

3.1 基于前后圖像幀信息的EKF濾波算法驗證

目標以5 (°)/s的角速度繞垂直地面軸做自旋運動，每當視覺相機檢測到新的特征時，都將其添加到狀態方程中，組成全狀態模型[22]：

(29)

圖8為連續測量過程中保存的圖像幀，圖9為相機等效的運行軌跡。圖9可以反映出目標是在繞其固定軸做旋轉運動，但是軌跡圖并不完整，只繪制出了3/4的圓周運動。究其原因在于模型的非線性和相機的測量噪聲等因素影響，導致測量誤差放大，無法收斂，通過前后幀估計得到的特征點位置和位姿變換矩陣無法繼續完成運動狀態的跟蹤測量。由于測量結果無尺度因子，因而圖中未標出具體量綱。隨著特征點數量的增多，狀態向量的維度越來越大，數據更新的速率明顯降低，如圖10所示。

3.2 基于特征數據庫的無位姿優化測量算法驗證

目標以12 (°)/s的角速度繞垂直地面軸做自旋運動，從圖11可以看出，在目標衛星旋轉過程中，盡管光照強度發生了比較明顯的變化，但是仍然能夠對提取出的特征點進行比較好的跟蹤。從圖12可以看出，隨著時間的延長，累積誤差愈來愈大，三維點云逐步發散，無法較好地繪制出目標的三維形態。圖13和圖14分別為無位姿優化時針對以12 (°)/s旋轉的常規立方衛星大模型的位姿測試曲線。從圖13中可以看出自旋軸旋轉角在-180°～180°范圍內變化，與自旋軸垂直的兩軸角度隨著測量時間的延長并未趨于收斂。由于目標模型固定安裝在三軸轉臺，其三軸位置不隨測量時間的延長發生明顯變化，為了更直觀地反映相機測量效果，在相機對目標完成旋轉一周的測量后，其等效運動軌跡應該是個理想的圓形，因而采用繪制相機等效運動的方式說明非線性優化對位置測量的重要性。從圖15中也可以看出隨著時間的延長，位置曲線所構成的圓的半徑在逐漸擴大，呈發散狀態。

3.3 基于特征數據庫的有位姿優化測量算法驗證

圖16為包含位姿優化的測量算法三維重建效果圖，稀疏點云較好地重建了目標的三維形態，與實際試驗情況相符。圖17和圖18分別為有閉環檢測時針對以12 (°)/s旋轉的常規立方衛星大模型的位姿測試曲線。從圖中可以看出，測量曲線明顯連續光滑了很多，而且位置姿態是收斂狀態。圖17中自旋轉角在-180°～180°范圍內變化，之所以出現明顯的階躍現象，一方面是由于畫圖采用的是關鍵幀位姿，而關鍵幀相對來說比較稀疏；另一方面是由于過程中包含有位姿閉環優化算法，當目標旋轉回初始位置附近時，進行了位姿的閉環檢測和位姿優化，使原本趨于發散的位姿收斂了回來。