一種基于投影幾何的空間非合作目標角速度估計方法

董振宇

(沈陽理工大學自動化與電氣工程學院,沈陽 110159)

1 引言

近年來,在非合作目標的研究中,由于未知目標的結構等可用信息,估計非合作目標的運動參數一直是一個非常具有挑戰性的熱點問題,成為了非合作目標抓捕中的關鍵任務,廣泛應用于在軌服務與空間碎片清除。

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

而上述兩種應用背景下的捕獲階段中,如圖1所示,當系統質心位置發生偏移、慣性參數也會發生改變,可能會引起組合體姿態失穩,需采取協調控制實現捕獲后組合體的穩定,否則會使得系統失控從而導致整個抓捕任務失敗。所以在抓捕前獲取目標的運動參數,以判斷能否抓捕目標與制定合適的抓捕方案就尤為重要。

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

綜上所述,非合作目標運動參數估計對于抓捕任務甚至空間活動中都有著不可替代的重要性。

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

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

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

2 投影幾何法估計角速度計算

2.1 相關坐標系

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

圖2 空間坐標系示意圖Fig.2 Illustration of the spatial coordinate systems

①慣性/空間參考系(Inertial Reference Frame,IRF):地心慣性坐標系,;服務衛星慣性坐標系,;視線/相機坐標系,,相機坐標系與慣性坐標系變換關系已知,故也可作為參考坐標系。

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

③其他坐標系:圖像坐標系,,與相機坐標系關系已知。

2.2 投影幾何估計法

本文提出一種新的投影幾何法估計目標的角速度,通過圖像坐標系上的投影點,直接獲取目標角速度,如圖3所示,相比濾波器的狀態參數估計法基于位姿結果與運動關系求解角速度,投影幾何解法避開了位姿關系的求解,且所需要的數據信息量更少。

圖3 方法對比Fig.3 Method comparison

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

OX、OY為目標本體坐標系的軸向,根據簡諧運動的定義,在慣性主軸、平面上,角速度的分量ω、ω滿足簡諧運動規律。

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

對于單軸自旋穩定目標,如圖4所示,p是三維點p(x,y,z)投影在圖像平面上的投影點,坐標記為(,)。

圖4 投影點示意圖Fig.4 Illustration of projection angle

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

式中,、p為目標本體坐標系上的特征點與其在圖像坐標下投影點的齊次坐標,為相機內參,

圖像投影點滿足

根據和差公式,d、d表示非含時項,投影點的簡諧運動關系可表示為

中的非合作目標投影點(,)依然符合簡諧運動的基本規律且周期與軸向自旋角相同。本文將基于這一性質,在非合作目標單軸自旋狀態下估計軸向角速度。

3 投影幾何法中的圖像算法設計

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

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

3.1 幀間直線匹配流程

本文直線匹配采用了直線段的長度、角度和中點位置作為匹配的標準,在Xu的基礎上提出了一種根據鄰域邊進行匹配檢驗的連續幀直線段匹配算法,如圖5所示。

1998年，我在寫詩，并因此結識了幾個寫詩的朋友。那時候，互聯網還是個新東西。兩年之后，我所在的小城才開始出現第一個網吧。之后，那些瘋瘋癲癲的女孩子才開始用ICQ約會與聊天。

圖5 圖像連續幀匹配Fig.5 Image continuous frame matching

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

直線段長度相似度函數

直線段角度相似度函數

直線段中點相似度函數

最后得直線段相似匹配函數

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

3.2 匹配檢驗流程

圖6 鄰域邊檢驗示意圖Fig.6 Illustration of neighborhood line test

通過調節LSD檢測放大倍數與高斯核參數控制誤檢直線的數量和檢測精度,通過調節閾值、控制匹配的準確度,得到如圖7的匹配結果。將圖7得到的匹配端點數據均值

圖7 匹配結果示意圖Fig.7 Illustration of image matching

使用(,)·sin(·)對投影點進行回歸,(,)為橫、縱坐標,參數與距非合作目標距離、圖片像素相關,參數與初始位姿相關,參數用于平衡圖像主點與相機光心與非合作目標的幾何中心,所得頻率,在單軸自旋運動中即為自旋頻率,即角度制角速度為

4 實驗驗證

本文在實際物理平臺測量了的空間單軸自旋穩定狀態的角速度。如圖8所示,搭建了由計算機—單目相機、控制器—旋轉臺組成的運動估計實驗平臺。其拍攝測量的數據集如圖9所示。圖9中使用的衛星模型為NASA官網提供的SOHO衛星。對序列幀進行第二節的算法獲取轉動平臺設置的角速度,使用計算機CPU為Intel(R)Core(TM)i5-1035G4,主頻1.10GHz,內存8G。回歸結果如圖10所示。

圖8 運動估計測量平臺Fig.8 Motion estimation platform

圖9 圖像序列幀Fig.9 Illustration of image frame sequence

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

表1 誤差與回歸評價Table 1 Evaluation of error and regression

圖10 投影點軌跡回歸Fig.10 Projection point trajectory regression

相對誤差公式為

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

從實驗結果中絕對誤差與相對誤差中可以發現誤差并不隨角度增加而增加,誤差水平的決定因素是直線檢測中的精度。而影響誤差的主要因素有:圖像獲取時的圖像質量、與相機的相對位姿以及相關參數設置的精確性。如8deg/s與15deg/s由于檢測精度相對低,導致結果誤差水平高于其他組。通過回歸評價可以發現回歸關系可靠性高、精度水平高。在幀間自旋角差,即大于60deg時需要提高采樣頻率以確保直線匹配的旋轉不變性。

5 結論

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