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

程楚楚 高林濤 魏承

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

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

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

1 ORB特征提取及匹配

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

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

1.1 FAST算法

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

(1)計(jì)算該點(diǎn)灰度值Ip，設(shè)定閾值t。

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

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

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

圖1 FAST特征點(diǎn)示意圖Fig.1 Schematic diagram of FAST feature points

1.2 BRIEF描述子

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

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

1.3 ORB特征提取

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

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

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

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

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

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

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

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

2.1 坐標(biāo)系

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

圖3 坐標(biāo)系Fig.3 Coordinate system

2.2 空間平面法向量

對n個(gè)時(shí)刻OCXCYCZC坐標(biāo)系原點(diǎn)進(jìn)行空間平面擬合，由于所有觀測點(diǎn)必在同一平面上，所以首先需對實(shí)測點(diǎn)進(jìn)行平面擬合。任意空間平面方程可以表示為

ax+by+cz-1=0

(1)

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

將n個(gè)觀測點(diǎn)的三維坐標(biāo)代入式(1)可得

A·X-l=0

(2)

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

X′=(ATA)-1ATl

(3)

2.3 空間圓圓心

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

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

(4)

可簡化為

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

(5)

由空間球體中垂面方程的相關(guān)性，n個(gè)觀測點(diǎn)坐標(biāo)可以列出n-1個(gè)線性無關(guān)的中垂面方程，可得誤差方程為

(6)

式(6)化簡為

V=B·Y-L

(7)

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

限制條件為

C·Y-Wx=0

(8)

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

(9)

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

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

得出最小二乘解為

(10)

2.4 章動初始條件設(shè)置

(11)

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

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

(12)

式中：It為引進(jìn)變量。

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

(13)

(14)

式(14)進(jìn)一步化簡得到

(15)

即

(16)

式中：t表示時(shí)間。

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

(17)

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

(18)

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

(19)

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

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

(20)

(21)

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

(22)

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

3 仿真分析驗(yàn)證

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]進(jìn)行動力學(xué)仿真，利用paraview軟件對二維和三維數(shù)據(jù)進(jìn)行分析和可視化。假設(shè)小天體做自旋運(yùn)動，且無章動，使用MBDyn軟件與paraview軟件進(jìn)行聯(lián)合仿真，設(shè)定原始轉(zhuǎn)軸方向，取時(shí)間步長為0.002 s，每10步取一幀由paraview保存為視頻，對視頻提取關(guān)鍵幀進(jìn)行ORB-SLAM得到位姿四元數(shù)并進(jìn)行計(jì)算繪圖。如圖6為旋轉(zhuǎn)擬合空間圓，如表1為計(jì)算得到旋轉(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關(guān)鍵幀姿態(tài)四元數(shù)得到相鄰幀轉(zhuǎn)角與幀數(shù)關(guān)系，其中以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 含章動小天體

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

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

圖8 擬合相機(jī)軌跡Fig.8 Fitted trajectory of camera

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

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

4 結(jié)束語

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