非合作慢旋目標(biāo)即時狀態(tài)位姿確定

牟金震，溫凱瑞，劉宗明

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

0 引言

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

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

1 非合作慢旋目標(biāo)特位姿初始化

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

(1)

(2)

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

根據(jù)對稱轉(zhuǎn)移誤差方程和距離閾值概率分布來統(tǒng)計內(nèi)點的概率得分

(3)

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

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

2 非合作慢旋目標(biāo)位姿優(yōu)化

2.1 特征點跟蹤

根據(jù)上一幀和勻速運動模型估計的當(dāng)前幀位姿變換為

(4)

假設(shè)相機(jī)的內(nèi)參為K，上一幀的三維特征點集為Piw，其在當(dāng)前幀中的投影為

(5)

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

2.2 位姿優(yōu)化

在本節(jié)中，引入李群和李代數(shù)，在李代數(shù)空間進(jìn)行加法與求導(dǎo)操作，再將計算結(jié)果轉(zhuǎn)換為李群，便可完成對測量誤差函數(shù)的優(yōu)化求解[18]。

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

R(t)RT(t)=I

(6)

對式(6)兩邊求導(dǎo)可得

(7)

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

(8)

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

(9)

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

(10)

對其進(jìn)行積分可得

(11)

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

(12)

(α∧)2

(13)

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

SE(3)=

(14)

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

(15)

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

(16)

(17)

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

(18)

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

(19)

(20)

定義三維歐氏群對應(yīng)的李代數(shù)為

(21)

(22)

其中

(23)

由以上分析可知，相應(yīng)的對數(shù)運算可以將李群映射到李代數(shù)。

ω=ln(R)∨

(24)

v=V-1t

(25)

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

(26)

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

(27)

構(gòu)建誤差函數(shù)

(28)

利用李代數(shù)擾動模型對誤差函數(shù)求導(dǎo)，分別給ξi和ξj一個左擾動δξi和δξj，式(28)變?yōu)?/p>

(29)

根據(jù)SE(3) 上的伴隨性質(zhì)

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

(30)

其中

(31)

(32)

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

(33)

(34)

在此，對雅克比矩陣進(jìn)行近似，可以得到

(35)

根據(jù)圖優(yōu)化，假設(shè)C為所有邊的集合，那么基于李代數(shù)空間位姿圖優(yōu)化的目標(biāo)函數(shù)可記為

(36)

其中，Ω是信息矩陣。

3 實驗分析

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

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

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

圖3 等效的相機(jī)位姿軌跡 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 常規(guī)衛(wèi)星不同角度特征提取(10(°)/s)Fig.6 General satellite model extracted feature from different perspectives(10(°)/s)

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

圖8 優(yōu)化后的等效運動姿態(tài)軌跡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 總結(jié)

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