非合作慢旋目標即時狀態位姿確定

牟金震，溫凱瑞，劉宗明

(1.上海航天控制技術研究所，上海 201109； 2.上海市空間智能控制技術重點實驗室，上海 201109； 3.南京航空航天大學，南京 210016)

0 引言

慢旋運動是在軌失效衛星普遍具有的一種運動狀態，其繞慣性主軸以某一固定的角速度保持勻速旋轉運動。由于空間光照條件的復雜性，在空間開展在軌維修等操控任務時，需要對其旋轉信息進行快速、準確的測量[1-3]。對空間失效衛星等非合作目標進行在軌操控，采用的測量方案是視覺測量方法[4-5]，目前已經開展了相關地面驗證試驗，是近距離段主要的測量手段。張世杰等[6]假設被測目標幾何模型已知，設計了一種非合作光標位姿測量方法。文獻[7-8]采用的方法一致，首先獲得被測目標的3D模型，通過點云模板匹配的方法實現非合作目標的相對位姿測量。但空間大多數失效衛星以及垃圾碎片無法提供準確的幾何模型，因此文獻[7-8]提出的方法具有一定的局限性。Civera等[9]在擴展卡爾曼濾波(Extended Kalman Filter，EKF)-同步定位與地圖構建(Simultaneous Locali-zation and mapping，SLAM)的基礎上，利用逆深度參數處理方法估計特征的尺度。Augenstein等[10]提出了一種用于非合作目標姿態跟蹤和重建的單目視覺SLAM算法。Schnitzer等[11]將文獻[9]中EKF-SLAM與隨機抽樣一致(Random Sample Consensus，RANSAC)算法相結合，生成目標的模型，該模型可用于進一步細化估計位姿。Segal等[12]采用立體視覺系統跟蹤目標飛行器上的特征點，建立模型，采用EKF的方法估計特征點位置、目標旋轉角度、以四元數表示的目標旋轉狀態等。文獻[13]基于Hill方程的相對運動模型，考慮了非合作目標水平運動和旋轉運動之間的動力學耦合。Cho等[14]根據航天器相對距離的不同，采用了Shi-Tomas和加速穩健特征(Speeded Up Robust Features，SURF)兩種算法，并提出了角點檢測的魯棒性。文獻[15]在文獻[14]的SURF算法的基礎上，提出了對象請求代理(Object Request Broker，ORB)角點檢測算法，解決了特征檢測的旋轉不變性。文獻[16]在ORB-SLAM的基礎上，分析了非合作旋轉目標的閉環檢測性能。以上文獻[9-16]針對非合作目標相對位姿測量提出了多種解決方法,但關于復雜光照條件下非合作旋轉目標測量的研究較少，仍處于起步階段。

針對旋轉目標的測量按照工作流程來說可以分為3個階段，第1個階段為遠距離旋轉特性跟蹤測量階段，該階段的主要任務為懸停觀測，追蹤星在定點位置長時間連續觀測目標星運動特性，在此過程中相機準確估計目標的旋轉角速度，為后續的同步起旋提供測量輸入；第2個階段為起旋過程跟蹤測量階段，該階段的主要任務為起旋同步，追蹤星加速旋轉到與目標星自旋角速度穩定同步，在此過程中相機穩定跟蹤并提供相對角度和角速度信息；第3個階段為靠攏捕獲階段，該階段的主要任務為穩定測量，當追蹤星與目標星之間的旋轉相對穩定后，追蹤星逐漸逼近目標星，在此過程中相機準確提供相對位置和相對姿態信息。本文重點解決第1個階段——懸停觀測段，即相機如何在復雜的光照條件下快速、準確地跟蹤目標特征點，并且準確估計目標的自旋角速度信息。

1 非合作慢旋目標特位姿初始化

ORB[15]是一種快速特征點提取和描述的算法，分為特征點提取和特征點描述兩部分。ORB通過構造圖像金字塔，在每層金字塔采用Fast算法提取特征點。檢測候選特征點周圍一圈的像素值，如果候選點周圍領域內有足夠多的像素點與該候選點的灰度值差別夠大，則認為該候選點為一個特征點。ORB通過灰度質心法計算特征點半徑為r的圓形鄰域內灰度質心位置，并尋找特征點的主方向。為了解決旋轉的不變性，以特征點p為中心，取一個s×s大小的鄰域。在特征域內隨機選取2×N個點對(x,y)，然后對2×N點分別做高斯平滑，定義測試規則τ，比較N對像素點的灰度值

(1)

(2)

針對平面場景和非平面場景，本文分別采用單應矩陣和基礎矩陣求解位姿變換，根據對稱轉移誤差方程和距離閾值概率分布來統計內點的概率得分，如果選取的兩幀滿足約束條件，則初始化成功；否則，放棄這兩幀并重新進行。

根據對稱轉移誤差方程和距離閾值概率分布來統計內點的概率得分

(3)

根據RANSAC原則[16]，比較當前得分和歷史得分，保留最高得分并記錄相應的參數。用SH表示單應矩陣的得分，SF表示基礎矩陣的得分。如果SH/(SH+SF)>0.4，那么選擇單應矩陣求解位姿變換，反之選擇基礎矩陣。在完成得分概率統計后，針對所選用的模型求解陣R和平移向量t：1)根據單應矩陣分解R和t；2)根據基礎矩陣分解R和t。

圖1 2顆不同軸指向旋轉衛星模型的位姿初始化Fig.1 Pose initialization of two rotating satellite models with different axes

2 非合作慢旋目標位姿優化

2.1 特征點跟蹤

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

(4)

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

(5)

將估計的特征點與實際提取的ORB特征點進行匹配，如果當前幀與上一幀沒有足夠的匹配點，通過在上一幀預估的位置附近擴大一定的搜索范圍繼續搜索。當找到對應的匹配點后，利用它們對相機的預估位姿進行基于捆集調整的優化處理[17]，便可以得到當前幀準確的位姿變換。為了提高測量的可靠性和精度，在得到一個相機的位姿估計和一個初始的特征匹配集的基礎上，將特征數據庫中的三維點投影到當前幀中以搜索更多的匹配點對。

2.2 位姿優化

在本節中，引入李群和李代數，在李代數空間進行加法與求導操作，再將計算結果轉換為李群，便可完成對測量誤差函數的優化求解[18]。

對于旋轉矩陣R(t)，等式(6)成立

R(t)RT(t)=I

(6)

對式(6)兩邊求導可得

(7)

式(7)是一個反對稱矩陣，因此，存在一個三維向量ω(t)，滿足

(8)

其中，∧表示從一個向量到反對稱陣，式(8)兩邊同時右乘R(t)，可得

(9)

式(9)是一個微分方程，對旋轉矩陣求導只需要左乘一個反對稱矩陣ω(t)∧即可。設t0=0，且此時旋轉矩陣R(t0)=I，在t0附近ω(t0)保持為常數ω(t0)=ω0，則有

(10)

對其進行積分可得

(11)

=I+α∧sinθ+(α∧)2(1-cosθ)

(12)

(α∧)2

(13)

在剛體變換過程中，除了旋轉外還有平移變換，定義三維歐氏群SE(3)為

SE(3)=

(14)

其中，t為平移矩陣。對剛體變換矩陣g求導并乘以g-1可得

(15)

存在一個反對稱矩陣ω(t)∧和一個三維向量v(t)，滿足

(16)

(17)

因而，式(15)可以改寫為

(18)

在此拓展了∧ 表示意義，為從一個向量到矩陣，對于三維向量來說，其表示一個到反對稱陣的運算；對于高于三維的向量來說，其表示一個從向量到矩陣的運算，所以

(19)

(20)

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

(21)

(22)

其中

(23)

由以上分析可知，相應的對數運算可以將李群映射到李代數。

ω=ln(R)∨

(24)

v=V-1t

(25)

假設2個位姿節點在se(3)上的表示為ξi和ξj，它們之間的運動估計為ξij，則其對應關系如下所示

(26)

式中，∨表示從矩陣到向量的運算。如果在SE(3)上的表示為Ti、Tj和ΔTij，則其對應關系為

(27)

構建誤差函數

(28)

利用李代數擾動模型對誤差函數求導，分別給ξi和ξj一個左擾動δξi和δξj，式(28)變為

(29)

根據SE(3) 上的伴隨性質

eξ∧t=te(Ad(t-1)ξ)∧

(30)

其中

(31)

(32)

由式(32)可以得到，關于位姿Ti和Tj的雅克比矩陣，其中

(33)

(34)

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

(35)

根據圖優化，假設C為所有邊的集合，那么基于李代數空間位姿圖優化的目標函數可記為

(36)

其中，Ω是信息矩陣。

3 實驗分析

在進行地面仿真實驗時，對目標星的模擬采用了兩款模型，其中一款為常規衛星，表面貼有熱控多層反光材料；另一款為嫦娥衛星，帶有太陽能帆板。模型分別固定于轉臺上，其中常規衛星目標模型以 10(°)/s的角速度繞豎直軸勻速運動，嫦娥衛星模型以3(°)/s的角速度繞豎直軸勻速運動，相機光軸方向與目標旋轉軸方向垂直。同時，采用高亮度LED光源，模擬復雜光照條件。由于目標的轉動，相機成像的亮度值發生了明顯變化，甚至出現曝光飽和現象，但仍然能快速穩定地跟蹤旋轉目標，具有較好的魯棒性。

圖2和圖6所示分別為針對嫦娥衛星和常規衛星旋轉運動過程中采集的圖像幀，從中抽取關鍵幀為代表，圖中的綠色小方形對應的是提取的特征點。從圖2和圖6可以看出，當被測衛星出現局部過飽的情況下，依然能穩定提取特征點。圖3和圖8所示為相機的等效運動軌跡，其中藍色方框表示關鍵幀。圖7所示為在局部跟蹤丟失特征點后，根據已有的地圖估計的相機位姿。圖4和圖9所示分別為目標以3(°)/s和10(°)/s的角速度自旋運動時的測量曲線，從圖中可以看出，在目標自旋過程中能穩定跟蹤被測衛星。圖5和圖10中上半部分的點直線和實直線分別表示實際的旋轉角和估計的旋轉角；下半部分表示實際旋轉角與估計旋轉角之間的殘差，當目標以3(°)/s和10(°)/s自旋運動時，測量穩定后平均角速度誤差分別為0.02°和0.1°左右。

圖2 嫦娥衛星模型不同角度提取特征點(3(°)/s)Fig.2 Chang’e satellite model extracted feature from different perspectives

圖3 等效的相機位姿軌跡 Fig.3 Equivalent camera pose trajectory

圖4 實際運動曲線(3(°)/s)Fig.4 Actual motion curve(3(°)/s)

圖5 擬合曲線及測量誤差(3(°)/s) Fig.5 Fitting curve and measurement error(3(°)/s)

圖6 常規衛星不同角度特征提取(10(°)/s)Fig.6 General satellite model extracted feature from different perspectives(10(°)/s)

圖7 局部跟蹤丟失情況下姿態軌跡Fig.7 Pose trajectory in case of local tracking loss

圖8 優化后的等效運動姿態軌跡Fig.8 The optimized equivalent motion posture

圖9 實際曲線(10(°)/s)Fig.9 Actual motion curve(10(°)/s)

圖10 擬合曲線及殘差(10(°)/s) Fig.10 Fitting curve and measurement error(10(°)/s)

4 總結

針對復雜光照條件下非合作慢旋目標位姿測量問題，本文通過ORB對特征點賦予了尺度特性和旋轉不變特性，使目標點同時具備了提取快速性和匹配穩定性的特點。當相機前后兩幀成像視差較大時，如果特征點在同一個平面上，則通過計算單應性矩陣來估算位姿；如果特征點不在同一平面上，則通過計算基礎矩陣來估算位姿，基于概率統計模型求解初始化位姿。為了對位姿結果進行優化處理,通過計算李代數中測量值與估計值的殘差，利用李代數求導擾動模型進行求解，再將計算結果轉換為李群，實現了對測量誤差函數的優化求解。