董振宇

(沈陽理工大學(xué)自動(dòng)化與電氣工程學(xué)院,沈陽 110159)

1 引言

近年來,在非合作目標(biāo)的研究中,由于未知目標(biāo)的結(jié)構(gòu)等可用信息,估計(jì)非合作目標(biāo)的運(yùn)動(dòng)參數(shù)一直是一個(gè)非常具有挑戰(zhàn)性的熱點(diǎn)問題,成為了非合作目標(biāo)抓捕中的關(guān)鍵任務(wù),廣泛應(yīng)用于在軌服務(wù)與空間碎片清除。

在軌服務(wù)(On-Orbit Servicing,OOS)是在空間通過人、機(jī)器人或服務(wù)衛(wèi)星完成延長(zhǎng)各種航天器壽命、提升執(zhí)行任務(wù)能力以及維護(hù)好空間環(huán)境的安全穩(wěn)定的空間操作。例如,輔助入軌、在軌維修、加注燃料、拖曳離軌。另一方面,近20年來空間碎片已經(jīng)成為了人類太空活動(dòng)的重要障礙,空間碎片和航天器碰撞可直接改變航天器的表面性能,造成表面器件損傷,甚至航天器故障?？臻g碎片還會(huì)對(duì)軌道資源構(gòu)成威脅,在同一軌道高度,當(dāng)空間碎片的密度達(dá)到臨界密度時(shí),碎片的鏈?zhǔn)脚蛎涍^程還會(huì)造成軌道資源的永久性破壞。

而上述兩種應(yīng)用背景下的捕獲階段中,如圖1所示,當(dāng)系統(tǒng)質(zhì)心位置發(fā)生偏移、慣性參數(shù)也會(huì)發(fā)生改變,可能會(huì)引起組合體姿態(tài)失穩(wěn),需采取協(xié)調(diào)控制實(shí)現(xiàn)捕獲后組合體的穩(wěn)定,否則會(huì)使得系統(tǒng)失控從而導(dǎo)致整個(gè)抓捕任務(wù)失敗。所以在抓捕前獲取目標(biāo)的運(yùn)動(dòng)參數(shù),以判斷能否抓捕目標(biāo)與制定合適的抓捕方案就尤為重要。

圖1 空間碎片與在軌服務(wù)示意圖Fig.1 Illustration of space debris and on-orbit servicing

綜上所述,非合作目標(biāo)運(yùn)動(dòng)參數(shù)估計(jì)對(duì)于抓捕任務(wù)甚至空間活動(dòng)中都有著不可替代的重要性。

非合作目標(biāo)的運(yùn)動(dòng)參數(shù)主要包括自旋運(yùn)動(dòng)參數(shù):自旋軸方向、自旋角速度;章動(dòng)運(yùn)動(dòng)參數(shù):翻滾軸方向、章動(dòng)角、章動(dòng)角速率。對(duì)于本文研究的角速度估計(jì),傳統(tǒng)方法是將角速度求解問題轉(zhuǎn)化為相對(duì)位姿關(guān)系的狀態(tài)參數(shù)求解,已經(jīng)相對(duì)成熟。很多學(xué)者在輸入特征方面做出改進(jìn),Peng等使用雙目立體匹配與激光雷達(dá)融合,在條件不滿足三維重建時(shí),直接使用激光雷達(dá)點(diǎn)云數(shù)據(jù),經(jīng)過點(diǎn)云融合、點(diǎn)云匹配最終使用擴(kuò)展卡爾曼濾波(Extended Kalman Filter,EKF)狀態(tài)估計(jì)得到目標(biāo)的位置姿態(tài)參數(shù)、角速度與線速度估計(jì)值。Li等提出了一種基于連續(xù)點(diǎn)云的估計(jì)方案,使用EKF估計(jì)李群矩陣陣列,完成對(duì)目標(biāo)結(jié)構(gòu)、慣性參數(shù)的估計(jì)。Ge等使用立體視覺提取目標(biāo)上三個(gè)不共線的特征點(diǎn)的位置矢量作為EKF輸入估計(jì),這樣避免了估計(jì)目標(biāo)觀測(cè)坐標(biāo)系與慣性主軸坐標(biāo)系之間的四元數(shù),可以直接估計(jì)目標(biāo)的慣性主軸,但這是一個(gè)雙線性問題,難以獲得全局收斂。

相對(duì)于提高輸入數(shù)據(jù)質(zhì)量,另一種思路是在估計(jì)方法或者濾波器方面做出了改進(jìn),例如Peng還融合了EKF與無跡卡爾曼濾波(Unscented Kalman Filter,UKF)形成新的混合卡爾曼濾波器(Hybrid Kalman Filter,HKF),相比EKF提高了濾波器對(duì)于非線性的近似誤差,相比UKF提高了迭代效率。Qiu等提出了角速度—慣性張量—運(yùn)動(dòng)參數(shù)的兩步法,即先從四元數(shù)的微分偽逆求解出粗略的角速度,經(jīng)過低通濾波與SVD降噪求解慣性張量,再利用EKF估計(jì)精確的運(yùn)動(dòng)參數(shù),但是兩步法在初始估計(jì)角速度就引入了相當(dāng)大的誤差,后續(xù)需要多次進(jìn)行降噪,有可能對(duì)輸入信息造成干擾。邢景儀等設(shè)計(jì)了基于方根高階體積卡爾曼濾波器的估計(jì)方法。其選擇五階體積卡爾曼濾波與平方根濾波方法相結(jié)合的方法,以減少舍入誤差對(duì)協(xié)方差矩陣正定性的影響,從而提高角速度估計(jì)的精度。邢還結(jié)合強(qiáng)跟蹤的思想,設(shè)計(jì)了一種基于方差修正的無蹤卡爾曼濾波的估計(jì)方法,實(shí)時(shí)修正濾波過程中的協(xié)方差矩陣,但其平滑性較差,計(jì)算復(fù)雜。

在上述以往提出的角速度估計(jì)方法都是使用濾波器的狀態(tài)估計(jì),精度很大程度上受到位姿估計(jì)結(jié)果誤差干擾,同時(shí)濾波器還受到初值的較大影響,導(dǎo)致在高轉(zhuǎn)速下需要較長(zhǎng)的收斂時(shí)間,甚至不收斂。Wang等提出了一種基于立體視覺的非合作目標(biāo)3D重建和狀態(tài)估計(jì)的6D-ICP方法。將光流法應(yīng)用于由金字塔KLT跟蹤的Harris特征點(diǎn),跨連續(xù)幀提取的每個(gè)單目相機(jī)中的目標(biāo)圖像特征跟蹤,再匹配兩個(gè)相機(jī)之間的同位特征,并生成相應(yīng)的3D結(jié)構(gòu)點(diǎn),最后基于6D-ICP算法估計(jì)相對(duì)3D變換并重建目標(biāo)3D點(diǎn)云,可以在10mm的精度跟蹤目標(biāo)的位移。范佳杰針對(duì)火箭箭體的幾何特征,將其抽象為一個(gè)細(xì)長(zhǎng)型圓柱體,使用激光雷達(dá)得到目標(biāo)火箭的表面點(diǎn)云,根據(jù)圓柱體在自由翻滾運(yùn)動(dòng)中其自旋軸、翻滾軸、章動(dòng)角之間的幾何關(guān)系與歐拉動(dòng)力學(xué)約束,依次求解得到目標(biāo)的自旋軸、翻滾軸、章動(dòng)角與章動(dòng)角速率,但是由于各項(xiàng)參數(shù)直接依賴于上一步的輸出,容易形成累計(jì)誤差,在章動(dòng)角7deg,章動(dòng)角速率30deg/s,章動(dòng)角速率誤差已經(jīng)達(dá)到4.2deg/s。Ivanov基于單目相機(jī),根據(jù)Clohessy-Wiltshire運(yùn)動(dòng)方程,建立估計(jì)參數(shù)向量函數(shù),再根據(jù)相鄰幀之間的函數(shù)變化求解運(yùn)動(dòng)。由于其相鄰幀之間只是簡(jiǎn)單的使用鄰域關(guān)系,并沒有用特征匹配,所以魯棒性并不樂觀。本文提出一種無需濾波器、基于目標(biāo)運(yùn)動(dòng)與投影的觀測(cè)關(guān)系的算法,其不依賴于位姿結(jié)果的微分關(guān)系。算法直接將非合作目標(biāo)的序列幀作為輸入,通過直線特征的全局跟蹤,得到的投影點(diǎn)序列在信賴域算法的回歸下,得到了高精度角速度。不同于Gu的基于投影角的方法,本文直接使用特征點(diǎn),不再受到圖像主點(diǎn)與三角函數(shù)奇異值的干擾,并且MSER為單特征,只要特征稍微有破壞,結(jié)果將失效。同時(shí)本文在原理上敘述更加完備,為后續(xù)研究提供了支撐。

2 投影幾何法估計(jì)角速度計(jì)算

2.1 相關(guān)坐標(biāo)系

如圖2所示,本文涉及的坐標(biāo)系主要有以下幾種:

圖2 空間坐標(biāo)系示意圖Fig.2 Illustration of the spatial coordinate systems

①慣性/空間參考系(Inertial Reference Frame,IRF):地心慣性坐標(biāo)系,;服務(wù)衛(wèi)星慣性坐標(biāo)系,;視線/相機(jī)坐標(biāo)系,,相機(jī)坐標(biāo)系與慣性坐標(biāo)系變換關(guān)系已知,故也可作為參考坐標(biāo)系。

②固定坐標(biāo)系/本體坐標(biāo)系(Body Flame,BF):空間非合作目標(biāo)本體坐標(biāo)系和服務(wù)衛(wèi)星本體坐標(biāo)系。坐標(biāo)原點(diǎn)選擇為目標(biāo)或是服務(wù)航天器的質(zhì)心,三個(gè)坐標(biāo)軸方向與各自的慣性主軸重合并且滿足右手坐標(biāo)系。

③其他坐標(biāo)系:圖像坐標(biāo)系,,與相機(jī)坐標(biāo)系關(guān)系已知。

2.2 投影幾何估計(jì)法

本文提出一種新的投影幾何法估計(jì)目標(biāo)的角速度,通過圖像坐標(biāo)系上的投影點(diǎn),直接獲取目標(biāo)角速度,如圖3所示,相比濾波器的狀態(tài)參數(shù)估計(jì)法基于位姿結(jié)果與運(yùn)動(dòng)關(guān)系求解角速度,投影幾何解法避開了位姿關(guān)系的求解,且所需要的數(shù)據(jù)信息量更少。

圖3 方法對(duì)比Fig.3 Method comparison

(1)三維空間中非合作目標(biāo)的角速度周期性

OX、OY為目標(biāo)本體坐標(biāo)系的軸向,根據(jù)簡(jiǎn)諧運(yùn)動(dòng)的定義,在慣性主軸、平面上,角速度的分量ω、ω滿足簡(jiǎn)諧運(yùn)動(dòng)規(guī)律。

(2)小孔模型下軸向角速度的周期性

對(duì)于單軸自旋穩(wěn)定目標(biāo),如圖4所示,p是三維點(diǎn)p(x,y,z)投影在圖像平面上的投影點(diǎn),坐標(biāo)記為(,)。

圖4 投影點(diǎn)示意圖Fig.4 Illustration of projection angle

根據(jù)相機(jī)的小孔模型有式(4)

式中,、p為目標(biāo)本體坐標(biāo)系上的特征點(diǎn)與其在圖像坐標(biāo)下投影點(diǎn)的齊次坐標(biāo),為相機(jī)內(nèi)參,

圖像投影點(diǎn)滿足

根據(jù)和差公式,d、d表示非含時(shí)項(xiàng),投影點(diǎn)的簡(jiǎn)諧運(yùn)動(dòng)關(guān)系可表示為

中的非合作目標(biāo)投影點(diǎn)(,)依然符合簡(jiǎn)諧運(yùn)動(dòng)的基本規(guī)律且周期與軸向自旋角相同。本文將基于這一性質(zhì),在非合作目標(biāo)單軸自旋狀態(tài)下估計(jì)軸向角速度。

3 投影幾何法中的圖像算法設(shè)計(jì)

以上的投影幾何估計(jì)法的關(guān)鍵就是對(duì)非合作目標(biāo)在圖像平面上的投影點(diǎn)進(jìn)行跟蹤。對(duì)于空間位姿求解,由于單目相機(jī)缺少深度信息,無法求解3D關(guān)系,但是對(duì)于角速度求解問題,單目相機(jī)足以滿足求解所需要的信息維度。本文提出一種基于圖片連續(xù)幀直線匹配的端點(diǎn)的跟蹤方法以獲取投影點(diǎn)。相比點(diǎn)特征其優(yōu)勢(shì)是:①直線特征更容易檢測(cè),尤其對(duì)于包括衛(wèi)星在內(nèi)的大多數(shù)人造物體,存在大量直線邊緣特征,使用直線特征提取可以更大程度上利用目標(biāo)的幾何特性;②線特征具有更好的魯棒性,受到光照、遮擋、視角變化的影響更小;③提供了更多信息(排序、拓?fù)浣Y(jié)構(gòu)等等),在特征數(shù)據(jù)維度上提高了上限。大量實(shí)驗(yàn)表明,由于衛(wèi)星大多配備太陽能帆板,直線特征作為高頻特征在衛(wèi)星圖像處理中的特征提取應(yīng)用中有良好的效果。

本文中將使用直線段檢測(cè)(Line Segment Detector,LSD)作為直線特征提取方法。相比于Hough變換直線檢測(cè),LSD直線特征擁有亞像素級(jí)檢測(cè)精度,且為無需算法內(nèi)參數(shù)調(diào)節(jié),只需要設(shè)定放大倍數(shù)與高斯核大小即可通過這兩個(gè)參數(shù),控制誤檢直線的數(shù)量,將誤檢概率降到最低。特別是空間這樣高噪聲的環(huán)境,LSD擁有絕對(duì)的時(shí)間復(fù)雜度優(yōu)勢(shì)。

3.1 幀間直線匹配流程

本文直線匹配采用了直線段的長(zhǎng)度、角度和中點(diǎn)位置作為匹配的標(biāo)準(zhǔn),在Xu的基礎(chǔ)上提出了一種根據(jù)鄰域邊進(jìn)行匹配檢驗(yàn)的連續(xù)幀直線段匹配算法,如圖5所示。

1998年，我在寫詩，并因此結(jié)識(shí)了幾個(gè)寫詩的朋友。那時(shí)候，互聯(lián)網(wǎng)還是個(gè)新東西。兩年之后，我所在的小城才開始出現(xiàn)第一個(gè)網(wǎng)吧。之后，那些瘋瘋癲癲的女孩子才開始用ICQ約會(huì)與聊天。

圖5 圖像連續(xù)幀匹配Fig.5 Image continuous frame matching

其中直線段的長(zhǎng)度、方向和中點(diǎn)位置很好地描述了直線段自身的屬性。直線段的長(zhǎng)度、角度、中點(diǎn)位置均與二維笛卡爾坐標(biāo)系下的直線幾何性質(zhì)定義一致。

直線段長(zhǎng)度相似度函數(shù)

直線段角度相似度函數(shù)

直線段中點(diǎn)相似度函數(shù)

最后得直線段相似匹配函數(shù)

sim表示前一幀第條直線與后一幀第條直線的相似度。取超過閾值的直線對(duì),將結(jié)果分為以下兩種情況:①sim＞ε且為列最大值也為行最大值,即前一幀關(guān)于第條直線在后一幀中的第條直線唯一匹配,直接輸出同名直線匹配對(duì)。②存在個(gè)sim＞ε,個(gè)sim＞ε,均為行(列)最大值非行(列)最大值。此時(shí)本文使用一致性檢測(cè)與領(lǐng)域邊檢驗(yàn),剔除誤匹配直線。

3.2 匹配檢驗(yàn)流程

圖6 鄰域邊檢驗(yàn)示意圖Fig.6 Illustration of neighborhood line test

通過調(diào)節(jié)LSD檢測(cè)放大倍數(shù)與高斯核參數(shù)控制誤檢直線的數(shù)量和檢測(cè)精度,通過調(diào)節(jié)閾值、控制匹配的準(zhǔn)確度,得到如圖7的匹配結(jié)果。將圖7得到的匹配端點(diǎn)數(shù)據(jù)均值

圖7 匹配結(jié)果示意圖Fig.7 Illustration of image matching

使用(,)·sin(·)對(duì)投影點(diǎn)進(jìn)行回歸,(,)為橫、縱坐標(biāo),參數(shù)與距非合作目標(biāo)距離、圖片像素相關(guān),參數(shù)與初始位姿相關(guān),參數(shù)用于平衡圖像主點(diǎn)與相機(jī)光心與非合作目標(biāo)的幾何中心,所得頻率,在單軸自旋運(yùn)動(dòng)中即為自旋頻率,即角度制角速度為

4 實(shí)驗(yàn)驗(yàn)證

本文在實(shí)際物理平臺(tái)測(cè)量了的空間單軸自旋穩(wěn)定狀態(tài)的角速度。如圖8所示,搭建了由計(jì)算機(jī)—單目相機(jī)、控制器—旋轉(zhuǎn)臺(tái)組成的運(yùn)動(dòng)估計(jì)實(shí)驗(yàn)平臺(tái)。其拍攝測(cè)量的數(shù)據(jù)集如圖9所示。圖9中使用的衛(wèi)星模型為NASA官網(wǎng)提供的SOHO衛(wèi)星。對(duì)序列幀進(jìn)行第二節(jié)的算法獲取轉(zhuǎn)動(dòng)平臺(tái)設(shè)置的角速度,使用計(jì)算機(jī)CPU為Intel(R)Core(TM)i5-1035G4,主頻1.10GHz,內(nèi)存8G?；貧w結(jié)果如圖10所示。

圖8 運(yùn)動(dòng)估計(jì)測(cè)量平臺(tái)Fig.8 Motion estimation platform

圖9 圖像序列幀F(xiàn)ig.9 Illustration of image frame sequence

圖10分別為相機(jī)采樣頻率1z下,使用信賴域算法在不同光照、不同初始位姿、角速度分別為2deg/s、5deg/s、13deg、17deg/s、21deg/s、30deg/s、45deg/s對(duì)投影點(diǎn)軌跡的單次回歸結(jié)果。圖中投影點(diǎn)(projection points)(,)為式結(jié)果,回歸曲線(regression curve)為(,)·sin(·)所得,描述了特征點(diǎn)在圖像坐標(biāo)系下投影的運(yùn)動(dòng)關(guān)系。根據(jù)式求解得到估計(jì)結(jié)果見表1。其中絕對(duì)誤差公式為

表1 誤差與回歸評(píng)價(jià)Table 1 Evaluation of error and regression

圖10 投影點(diǎn)軌跡回歸Fig.10 Projection point trajectory regression

相對(duì)誤差公式為

表示轉(zhuǎn)臺(tái)設(shè)定的角速度,表示算法輸出的角速度。使用殘差平方和(Sum of Squares due to Error,SSE)與均方根誤差(Root Mean Squared Error,RMSE)評(píng)價(jià)信賴域算法回歸的精度,用可決系數(shù)(R-square)與校正可決系數(shù)(Adjusted Rsquare)評(píng)價(jià)(,)·sin(·)關(guān)系的可靠性。

從實(shí)驗(yàn)結(jié)果中絕對(duì)誤差與相對(duì)誤差中可以發(fā)現(xiàn)誤差并不隨角度增加而增加,誤差水平的決定因素是直線檢測(cè)中的精度。而影響誤差的主要因素有:圖像獲取時(shí)的圖像質(zhì)量、與相機(jī)的相對(duì)位姿以及相關(guān)參數(shù)設(shè)置的精確性。如8deg/s與15deg/s由于檢測(cè)精度相對(duì)低,導(dǎo)致結(jié)果誤差水平高于其他組。通過回歸評(píng)價(jià)可以發(fā)現(xiàn)回歸關(guān)系可靠性高、精度水平高。在幀間自旋角差,即大于60deg時(shí)需要提高采樣頻率以確保直線匹配的旋轉(zhuǎn)不變性。

5 結(jié)論

對(duì)于空間非合作目標(biāo)在單軸自旋穩(wěn)定下的角速度求解問題,本文在歐拉運(yùn)動(dòng)學(xué)方程與小孔模型投影關(guān)系對(duì)圖像平面的投影與三維運(yùn)動(dòng)關(guān)系詳細(xì)的推導(dǎo)下,提出一種角速度估計(jì)方法,通過提取、跟蹤幀間直線特征,獲取與投影點(diǎn)周期,估計(jì)角速度,并且這種方法相比狀態(tài)參數(shù)估計(jì)方法更加簡(jiǎn)單、直接由圖像輸入、無需已知目標(biāo)的位姿信息,并且由于角速度估計(jì)結(jié)果方差與姿態(tài)結(jié)果獨(dú)立,可作為角速度狀態(tài)參數(shù)估計(jì)結(jié)果的參考修正值,本文由于實(shí)驗(yàn)條件限制,只在單軸自旋情況求取了角速度,后續(xù)研究將針對(duì)自由翻滾狀態(tài)下估計(jì)三軸角速度。文中通過實(shí)體實(shí)驗(yàn)說明了該方法在單軸自旋穩(wěn)定目標(biāo)角速度估計(jì)的有效性。