基于視覺的含章動小天體運動狀態(tài)估算方法

程楚楚 高林濤 魏承

(哈爾濱工業(yè)大學，哈爾濱 150001)

隨著人類空間活動日益頻繁，在軌航天器不斷增多，空間失效航天器以及空間碎片帶來的問題引起了各國的廣泛重視，它們不但占用了許多空間資源，還威脅著其他航天器的安全。然而，無論是對故障航天器進行維修，還是對太空垃圾進行清理，首先要解決的關鍵問題即空間目標的位姿測量問題。近些年來，利用視覺進行位姿測量的方法得到了廣泛關注和實踐。文獻[1]提出了使用計算機視覺從二維圖像獲取物體三維信息的可能性，由此基于計算機視覺的發(fā)展涌現(xiàn)出了許多新方法。文獻[2]提出了一種新的匹配方法——半稠密匹配法，這種方法解決了稀疏重建信息少和稠密重建點云信息多等問題。文獻[3]在開放式圖形庫(OpenGL)環(huán)境下，基于雙目視覺系統(tǒng)建立非合作目標位姿測量仿真系統(tǒng)。文獻[4]提出了一種基于尺度不變特征變換(Scale-Invariant Feature Transform，SIFT)算法的改進立體匹配方法，該方法較大地提高了匹配準確率和效率。文獻[5]提出了基于特征光流信息的非合作目標位姿估計方法，利用光流法構(gòu)建了光流信息與相對位姿之間的數(shù)學模型，并進行了實驗仿真。文獻[6]提出了一種基于單目圖像序列目標重建結(jié)果的非合作目標相對位姿測量方法，利用觀測到的圖像序列計算目標三維坐標，再基于點云數(shù)據(jù)建立遞推深度模型，最后利用卡爾曼濾波算法實現(xiàn)了目標位姿估計，得到了較為精確的位姿結(jié)果。文獻[7]利用立體序列觀測影像實現(xiàn)了非合作目標位姿估計，并通過仿真實驗驗證該方法的可行性。文獻[8]提出了基于單目懸停相機實現(xiàn)空間非合作目標三維重建，將SIFT算法與k-維二叉樹(k-d)相結(jié)合獲取目標位姿信息。目前還可以通過多種傳感器實現(xiàn)非合作目標的位姿測量，例如基于掃描式激光雷達測量、無掃描三維激光成像測量[9]、基于多傳感器融合的位姿測量方法[10]等。視覺測量技術可同時對目標進行成像監(jiān)視和位姿測量，并且具有精度高、設備簡易、成本低等特點。但目前使用的視覺測量方法具有一定的局限性，為了更好地解決針對小天體的位姿估計問題，由此引入在移動機器人領域中得到廣泛應用的同步定位與建圖(SLAM)方法，它是指運動物體根據(jù)傳感器的信息，一邊計算自身位置，一邊構(gòu)建環(huán)境地圖的過程。本文重點研究基于視覺SLAM的相對位姿測量，提出一種旋轉(zhuǎn)小天體相對位姿確定方法。

本文對特征提取后的小天體進行視覺SLAM，對得到的相機軌跡進行空間圓擬合，可確定小天體的相對位置及轉(zhuǎn)速。針對小天體可能出現(xiàn)的章動問題，本文也給出了含章動小天體的轉(zhuǎn)速測量方法并進行了仿真驗證。由于提取的小天體特征具有一般性，該方法可以為衛(wèi)星姿態(tài)的觀測和估計以及其他在軌航天器的位姿測量提供參考。

1 ORB特征提取及匹配

圖像的特征是圖像上最具代表性的一些點，這些點包含了圖像表述的大部分信息。即使旋轉(zhuǎn)、縮放，甚至調(diào)整圖像的亮度，這些點仍然穩(wěn)定地存在，不會丟失。找出這些點，就相當于確定了這張圖像，它們可以用來做匹配、識別等等有意義的工作。特征點由關鍵點和描述子兩部分組成。特征點有很多類，如SIFT、基于加速分割測試的特征(Features from Accelerated Segment Test，F(xiàn)AST)、加速穩(wěn)健特征(Speeded Up Robust Features，SURF)、快速特征點提取和描述算法(Oriented FAST and Rotated BRIEF，ORB)，考慮到實時性和性能，本文選用ORB特征。

ORB特征由Ethan Rublee等人[11]在2011年提出，關鍵點提取采用FAST算法，特征描述采用二進制穩(wěn)健獨立基本特征(Binary Robust Independent Elementary Features，BRIEF)描述子。ORB算法運行速度是SURF的10倍，是SIFT的100倍。

1.1 FAST算法

FAST算法檢測關鍵點的核心思想是找出與周圍像素不同的點作為關鍵點。FAST算法認為：若給定閾值t，當兩個像素點的灰度值之差的絕對值大于t，則這兩點不同，反之這兩個點比較相似。判斷一個點是否為FAST特征點，具體算法如下。

(1)計算該點灰度值Ip，設定閾值t。

(2)考慮該像素周圍16個點，如圖1所示。

(3)如果這16個點中有連續(xù)的n個點都和P點不同，那么它就是一個特征點，一般設定n為12。

針對第(3)步可以提出一個優(yōu)化，僅僅檢查位置1、9、5和13這4個位置的像素。如果是一個角點，那么上述4個像素點中至少有3個應該和中心點相同。如果都不滿足，那么不可能是一個角點。這樣可以快速排除大量非特征點的像素點。

圖1 FAST特征點示意圖Fig.1 Schematic diagram of FAST feature points

1.2 BRIEF描述子

BRIEF描述子計算出來的是一個二進制串的特征描述符，它是在一個特征點的鄰域內(nèi)，選擇n對像素點pi、qi(i=1,2,…,n)。然后比較每個點對的灰度值的大小。如果I(pi)>I(qi)，則生成二進制串中的1，否則為0。所有的點對都進行比較，則生成長度為n的二進制串。一般n取128、256或512，默認為256。為了增加特征描述符的抗噪性，算法首先需要對圖像進行高斯平滑處理。

特征點S×S的區(qū)域內(nèi)選取點對的方法，采用p和q符合(0,S2/25)的高斯分布的策略。

1.3 ORB特征提取

ORB特征采用具有方向性的FAST特征，并采用具有旋轉(zhuǎn)不變性的BRIEF特征描述子。FAST和BRIEF都是非?？焖俚奶卣饔嬎惴椒?，因此ORB具有非同一般的性能優(yōu)勢。OpenCV是一個基于BSD許可發(fā)行的跨平臺計算機視覺和機器學習軟件庫，可以運行在Linux、Windows、Android和Mac OS操作系統(tǒng)上。它由一系列C函數(shù)和少量C++類構(gòu)成，同時提供了Python、Ruby等語言的接口，實現(xiàn)了圖像處理和計算機視覺方面的很多通用算法。提取ORB特征點可直接用OpenCV直接提取，匹配ORB特征點的過程如下(見圖2)。

從提取的兩張相鄰圖片中所有的描述子中找到兩兩距離最近的配對，可用歐氏距離進行量度。但實際上有些特征點可能并沒有在兩張圖中同時出現(xiàn)，因此誤匹配的情況還是很多的，所以，匹配好后還要進一步篩選。篩選策略是當描述子之間的距離大于所有配對的描述子的最小距離的二倍時，認為匹配有誤。

圖2 ORB特征提取Fig.2 ORB feature extraction

2 小天體旋轉(zhuǎn)軸確定

SLAM中環(huán)境是靜止的，相機是相對運動的，而小天體相對位姿測量假定相機是靜止的，目標是相對運動的，因此二者可以等效。

(1)對采集圖像進行SIFT特征提取，建立圖像集之間的特征點匹配關系，并基于視覺SLAM對目標天體運動狀態(tài)進行測量。

(2)在遠距離視覺觀測中，完成對小天體軌道位置的視角測量，并估計小天體的位置運動狀態(tài)。

(3)在中近距離視覺觀測中，完成小天體姿態(tài)運動狀態(tài)的測量與辨識。

2.1 坐標系

OCXCYCZC坐標系定義為相機坐標系，OC0XC0YC0ZC0坐標系為固連在小天體上的，第一幀的時候與相機坐標系重合，OBXBYBZB坐標系為本體坐標系，且過目標質(zhì)心。根據(jù)相對運動原理，假定小天體靜止，相機繞目標轉(zhuǎn)動，即OC0XC0YC0ZC0坐標系與OBXBYBZB坐標系靜止，OCXCYCZC坐標系運動。理想情況下OCXCYCZC坐標系原點的運動軌跡為圓弧。該空間圓弧所在平面的法向量即為小天體自旋軸的平行向量，且自旋軸必經(jīng)過此圓弧的圓心(見圖3)。

圖3 坐標系Fig.3 Coordinate system

2.2 空間平面法向量

對n個時刻OCXCYCZC坐標系原點進行空間平面擬合，由于所有觀測點必在同一平面上，所以首先需對實測點進行平面擬合。任意空間平面方程可以表示為

ax+by+cz-1=0

(1)

式中：a、b、c為不全為0的常數(shù)，x、y、z分別為空間直角坐標系3個軸上的投影。

將n個觀測點的三維坐標代入式(1)可得

A·X-l=0

(2)

根據(jù)最小二乘法則VTPV=min可知(權(quán)陣P為單位矩陣，V為目標矩陣)，擬合平面的法向量的方向系數(shù)為

X′=(ATA)-1ATl

(3)

2.3 空間圓圓心

文獻[12]提出一種實用的空間圓形擬合檢測新方法，根據(jù)空間圓中任意兩條弦所對應的中垂面與空間圓所處的平面必然相交且交點即為圓心這一空間圓特性，利用空間向量按照最小二乘法推導出圓心計算方程,按照附有條件的間接平差求解圓心坐標，進而反算出空間圓半徑。

(x2-x1,y2-y1,z2-z1)·

(4)

可簡化為

Δx12·x0+Δy12·y0+Δz12·z0-l1=0

(5)

由空間球體中垂面方程的相關性，n個觀測點坐標可以列出n-1個線性無關的中垂面方程，可得誤差方程為

(6)

式(6)化簡為

V=B·Y-L

(7)

認定圓心必在擬合的空間平面上，依此作為限制條件，按照附有條件的間接平差進行計算，限制條件為式(8)，聯(lián)立得式(9)，推導可得圓心的最小二乘解。

限制條件為

C·Y-Wx=0

(8)

聯(lián)立得到方程為

(9)

此時權(quán)陣P為單位陣。

式中：Ks為限制條件的聯(lián)系數(shù)向量。

得出最小二乘解為

(10)

2.4 章動初始條件設置

(11)

式中：Ix、Iy、Iz為相對于直角坐標系三軸的轉(zhuǎn)動慣量，ωx、ωy、ωz為三軸下的角速度，Tx、Ty、Tz為三軸方向上的力矩。

假設本體系坐標軸為主慣量軸，對于關于YB軸對稱剛體，轉(zhuǎn)動慣量存在如下關系

(12)

式中：It為引進變量。

代入式(11)，考慮空間自由旋轉(zhuǎn)剛體，化簡得到

(13)

(14)

式(14)進一步化簡得到

(15)

即

(16)

式中：t表示時間。

根據(jù)式(16)得出：對稱剛體自由轉(zhuǎn)動狀態(tài)下，存在繞對稱軸恒定的自轉(zhuǎn)角速度以及垂直對稱軸平面內(nèi)的大小恒定，方向周期性變化的平面角速度分量。據(jù)此，剛體自由旋轉(zhuǎn)角動量矢量在本體系下可表示為

(17)

自轉(zhuǎn)軸與角動量矢量夾角即為章動角，設為α，則

(18)

如圖4所示，初始條件下四元數(shù)可由章動角表示為

(19)

圖4 初始姿態(tài)示意圖Fig.4 Schematic diagram of the initial pose

由本體系下角速度與歐拉四元數(shù)之間關系，可知

(20)

(21)

根據(jù)圖4，得到初始條件

(22)

代入式(21)，可得四元數(shù)導數(shù)初始條件設置方法。

3 仿真分析驗證

3.1 無章動小天體

選取小天體模型(見圖5)，分別選取Y和與XYZ三軸呈相同角度的w軸為旋轉(zhuǎn)軸，根據(jù)四元數(shù)乘法公式

(23)

式中：q0、q1、q2、q3，r0、r1、r2、r3為歐拉四元數(shù)。

圖5 小天體模型Fig.5 Model of small celestial body

使用MBDyn軟件[14]進行動力學仿真，利用paraview軟件對二維和三維數(shù)據(jù)進行分析和可視化。假設小天體做自旋運動，且無章動，使用MBDyn軟件與paraview軟件進行聯(lián)合仿真，設定原始轉(zhuǎn)軸方向，取時間步長為0.002 s，每10步取一幀由paraview保存為視頻，對視頻提取關鍵幀進行ORB-SLAM得到位姿四元數(shù)并進行計算繪圖。如圖6為旋轉(zhuǎn)擬合空間圓，如表1為計算得到旋轉(zhuǎn)擬合空間圓圓心和旋轉(zhuǎn)擬合空間圓平面法向量。

圖6 擬合空間圓Fig.6 Fitted spatial circle

表1 擬合空間圓圓心和平面法向量Table 1 Center of the fitted space circle and normal vector of the plane

由ORB-SLAM2關鍵幀姿態(tài)四元數(shù)得到相鄰幀轉(zhuǎn)角與幀數(shù)關系，其中以Y軸為旋轉(zhuǎn)軸每隔0.5 s取一幀，由其直線斜率擬合可得轉(zhuǎn)速為7.065 (°)/s，如圖7(a)所示；以w軸為旋轉(zhuǎn)軸每隔1 s取一幀，由其圖像斜率擬合可得轉(zhuǎn)速為6.781 (°)/s，如圖7(b)所示。

圖7 小天體轉(zhuǎn)速Fig.7 Rotational speed of small celestial body

3.2 含章動小天體

設置小天體對稱主軸慣量IYB=150，IXB=IZB=100，章動角設置為α=π/12，繞自轉(zhuǎn)軸自轉(zhuǎn)速度為ω=π/12，使用MBDyn軟件與paraview軟件進行聯(lián)合仿真，設定原始轉(zhuǎn)軸方向，取時間步長為0.01 s，每10步取一幀由paraview保存為視頻，對視頻提取關鍵幀進行ORB-SLAM得到位姿四元數(shù)并進行計算繪圖，如圖8為擬合相機軌跡。

由ORB-SLAM2關鍵幀姿態(tài)四元數(shù)得到相鄰幀轉(zhuǎn)角與幀數(shù)關系，其中以Y軸為旋轉(zhuǎn)軸每隔1 s取一幀，由其圖像斜率可得轉(zhuǎn)速為14.972(°)/s，誤差為0.028 (°)/s，如圖9所示。

圖8 擬合相機軌跡Fig.8 Fitted trajectory of camera

圖9 小天體轉(zhuǎn)速Fig.9 Rotation speed of small celestial body

驗證結(jié)果表明：基于視覺SLAM對目標天體進行三維重構(gòu)與分析，對得到的相機軌跡進行空間圓擬合，可確定小天體的相對位置及轉(zhuǎn)速，對于章動空間目標也具有適用性。

4 結(jié)束語

本文設計了一種小天體目標在軌位姿測量方法，實現(xiàn)了基于視覺SLAM的目標天體觀測處理，獲得了在軌小天體的位姿信息。針對空間小天體目標的章動特性，設計了一種章動小天體的觀測測速方法，誤差控制在0.1(°)/s以內(nèi)。本文為航天器在軌運行以及空間垃圾處理等任務提供了一種有效的基于視覺的目標位姿測量方法，通過視覺同步定位與建圖，獲得了空間目標的旋轉(zhuǎn)軸和旋轉(zhuǎn)角速度，對章動目標也具有適用性，可為后續(xù)的捕獲和抓取任務提供可靠依據(jù)。