一種翻滾火箭箭體的運動參數估計方法

趙高鵬，范佳杰，梁維奎

(1. 南京理工大學自動化學院，南京 210094；2. 上海宇航系統工程研究所，上海 201108)

0 引 言

空間碎片主動清除的主要目標是安全地捕獲和處置太空垃圾，使低地球軌道區域碎片進入大氣層燒毀，或使地球靜止軌道區域碎片軌道抬高進入墳墓軌道，從而保護在軌航天器運行安全[1]。空間碎片大多屬于非合作目標，典型的包括完成任務的火箭箭體、失效衛星等。失效衛星和火箭箭體在空間攝動力作用下一般處于自由翻滾狀態，由于其尺寸大、包含未使用的燃料，存在碰撞及爆炸的潛在風險，是在軌主動清除任務中高優先級的目標，獲取其在翻滾運動狀態下的運動參數是近距離安全逼近和操控的重要前提[2]。

在空間非合作目標的運動參數估計的研究中，常用的測量傳感器有單目視覺、雙目視覺、ToF深度相機、掃描式激光雷達、非掃描式激光成像雷達等[3-4]。非掃描式激光成像雷達能夠實時獲取目標的三維量測點云數據，對背景雜散光抑制能力強，且不存在運動模糊，其在空間任務中的應用引起了研究人員的廣泛關注[5-6]。

近年來，面向非合作衛星的運動參數估計，國內外學者開展了大量研究[2-3,7-10]，文獻[7]提出了一種以雙目相機對目標特征點的投影作為測量值的解耦估計方法，實現了旋轉非合作目標相對狀態的估計；文獻[8]利用雙目相機觀測的目標表面特征點的運動規律與目標姿態運動的相關性，實現了對非合作目標角速度與自旋軸的估計；文獻[9]基于單目視覺識別并測量目標星固有特征的像素位置為觀測輸入，通過擴展卡爾曼濾波實現了目標星相對位姿、角速度、慣量比等狀態的估計；文獻[10] 基于單目視覺，通過提取衛星目標在成像平面的投影角，實現了快旋空間非合作目標自旋速率測量。區別于非合作衛星，非合作火箭箭體呈回轉體結構[11]，表面光滑，難以穩定地提取圖像特征點和計算特征描述，易造成特征誤匹配，導致運動參數估計困難。因此，已有面向衛星目標的運動參數估計方法難以直接應用于火箭箭體。

本文以激光成像雷達為測量傳感器，直接處理獲取的火箭箭體三維稠密點云數據，計算處于自由翻滾狀態下的火箭箭體的翻滾軸、章動角和空間章動角速率。

1 坐標系定義

本文中所涉及的坐標系進行定義，如圖1所示。目標慣性坐標系{O}：坐標系原點OO位于目標航天器的質心，坐標軸沿目標的慣量主軸方向。坐標軸OOzO為目標的翻滾軸m。

圖1 坐標系說明Fig.1 Coordinate system

目標本體坐標系{B}：坐標系原點OB位于目標航天器的質心，坐標軸OBzB位于火箭箭體目標的對稱軸上，OBzB與OBxB、OByB組成右手坐標系。火箭箭體的自旋軸l沿坐標軸zB的方向。

服務航天器本體坐標系{H}：坐標系原點OH位于服務航天器的質心。

激光成像雷達測量坐標系{C}：坐標系原點OC位于服務航天器上，OCxC軸沿激光成像雷達視線軸方向，OCxC與OCyC、OCzC組成右手坐標系。目標火箭箭體的運動參數翻滾軸m、章動角度θ、空間章動角速率Ω均在此坐標系中表示。

2 火箭箭體運動狀態分析

火箭箭體失去姿態調整能力時，受空間攝動力矩的作用，火箭箭體會表現出復雜的翻滾運動[12-13]。在分離的初始階段，火箭箭體處于繞最小慣量軸的自旋運動；當火箭箭體受到空間攝動力矩作用，一部分能量隨之消耗時，火箭箭體處于存在章動角的翻滾運動；一段時間過后隨著總動能趨向最小動能狀態，此時火箭箭體處于繞最大慣量軸的平旋運動。

火箭箭體呈回轉體結構，可將其看作為圓柱體結構。火箭箭體處于存在章動角的翻滾運動，如圖2所示[13]。圖中坐標系OBxByBzB為火箭箭體的本體坐標系，坐標系OOxOyOzO為慣性坐標系，l為火箭箭體的自旋軸，其與圓柱體的圓柱軸重合，OBH為翻滾軸及翻滾角動量矩，其中θ為章動角，自旋軸l繞翻滾角動量矩作圓錐運動，其角速率Ω為空間章動角速率。

圖2 目標帶章動角的翻滾運動說明Fig.2 Tumbling motion with nutation angle of target

3 火箭箭體運動參數估計方法

3.1 方法結構

以空間中處于帶章動的翻滾運動狀態的火箭箭體為研究對象，提出了一種翻滾火箭箭體運動參數估計方法，通過對激光成像雷達獲取火箭箭體的三維點云數據進行算法處理，求解得到翻滾軸、章動角和空間章動角速率，其總體框圖如圖3所示。

本文方法包含三部分，分別為火箭箭體的自旋軸估計、翻滾軸估計、章動角和空間章動角速率的解算。其中，火箭箭體的自旋軸估計的輸入為當前時刻激光成像雷達獲取的火箭箭體點云數據，通過點云表面法向量估計和隨機采樣一致性算法，求解得到當前時刻的火箭箭體自旋軸；結合序列幀各時刻得到的火箭箭體自旋軸估計結果，采用遞推最小二乘法求解得到翻滾軸；進而結合相鄰兩幀求得的自旋軸和翻滾軸參數計算出章動角和空間章動角速率。

圖3 方法框圖Fig.3 The framework of the proposed method

3.2 自旋軸估計

火箭箭體為一個回轉體對稱結構，可將其視為一個圓柱體，其自旋軸可用圓柱體的軸線表示。因此，火箭箭體的自旋軸估計可通過計算圓柱體的軸線得到。本文采用點云表面法向量估計和隨機采樣一致性算法對獲取的火箭箭體的點云量測數據進行分割，得到呈圓柱體結構的部分點云，同時求解圓柱體點云的自旋軸參數，具體為：

1)點云表面法向量估計

點云表面法向量估計是通過各點及其鄰域內的點計算出表面法向量，本文采用基于主成分分析的點云法向量估計，實現過程是通過為每個采樣點構建局部鄰域，擬合局部鄰域最小二乘平面，將擬合平面的法向量作為該采樣點的法向量，將曲面變分作為該點處的近似曲率。設Qi為點云數據中的一個采樣點，點Qi的3×3協方差矩陣M為

(1)

對協方差矩陣M進行特征值分解，協方差矩陣M的最小特征值對應的特征向量即為Qi點的法向量。

設激光成像雷達獲得的火箭箭體點云數據的點集為Q{Q1,Q2,…,QN}，逐點計算表面法向量，得到每個點對應的法向量集合n{n1,n2,…,nN}。

2)基于隨機一致性采樣算法的自旋軸估計

空間圓柱體模型的參數由軸線l(ldir,lcen)和半徑r表示，其中ldir為軸線的單位方向向量，lcen為軸線上一點，可通過點云集合中的任意兩點及其法向量確定，示意圖如圖4所示。求解過程可描述為：從點云集合中任意選取兩點Q1和Q2，分別計算得到其法向量n1和n2，將n1和n2進行叉乘并將結果單位化，則可以確定Q1和Q2所在圓柱體的軸線的單位方向向量ldir。取經過點Q1并垂直l的平面A，將參數直線Q1+tn1和Q2+tn2沿ldir方向投影到平面A，它們的交點即為軸線上的一點lcen。而lcen和Q1在該平面上的投影點之間的距離為圓柱體的半徑r[14]。

圖4 圓柱體模型示意圖Fig.4 Illustration of a cylinder model

隨機采樣一致性算法(Random sample consensus， RANSAC)是從一組觀測數據中，通過迭代的方式匹配已給出的數學模型，找出符合模型的內點并估計出模型的參數。

本文基于RANSAC算法的圓柱體自旋軸估計步驟如下：

步驟1:從點云Q{Q1,Q2,…,QN}中隨機選取其中的2個點，{Qi,Qj|i,j∈[1,N]且i≠j}及其法向量ni和nj；

步驟 2:通過點Qi和Qj以及其對應的法向量ni和nj可求解出點Qi和Qj所在的圓柱體模型參數，得到圓柱體的圓柱軸l以及其半徑r；

步驟 3:對輸入點云數據集中的每一個點{Qd|d∈[1,N]}，計算點Qd到估計的圓柱體模型的圓柱軸l的距離rd。設置一個符合條件的點的閾值δ，當滿足rd∈[r-δ,r+δ]時，點Qd被記作為內點，否則點Qd被記作為外點；

步驟4:統計所有內點的個數，判斷其是否大于設置的最小數目值，如果是，則記錄求得的圓柱體模型參數，并存儲所有內點作為分割結果，如果不是，則跳到步驟1進行下一輪計算；

步驟5:重復步驟1到步驟4共W次，從中選取內點個數最多的一次，該次求得的圓柱體模型參數即為最符合圓柱體模型的參數，存儲的內點為最終得到的分割數據。

通過RANSAC算法求解得到圓柱體的模型參數，其中圓柱軸l即為火箭箭體的自旋軸。

3.3 翻滾軸估計

火箭箭體處于帶章動的翻滾運動時，可看作火箭箭體的自旋軸繞翻滾軸旋轉，且自旋軸和翻滾軸之間存在一個傾角，該傾斜角即為章動角，如圖5所示。

圖5 自旋軸軌跡示意圖Fig.5 Illustration of spin axis trajectory

圖5中，空間直線m表示翻滾軸，li表示第i時刻火箭箭體的自旋軸，平面B為過點m0且垂直于翻滾軸m的平面。在較短的時間里，章動角度可看作為一個定值，不產生變化。除m0點外，火箭箭體的自旋軸任一點的運動軌跡為圓弧軌跡。

翻滾軸m在空間中的參數方程為：

(2)

式中：h為直線方程的參數，點m0(x0,y0,z0)為翻滾軸m上一點，mdir(mx,my,mz)為翻滾軸m的單位方向向量。

因此，翻滾軸的求解可分為兩個步驟，即計算翻滾軸上一點m0(x0,y0,z0)以及翻滾軸的單位方向向量mdir(mx,my,mz)。

1)公共點求解

相鄰幀之間的自旋軸相交于一個公共點，同時該公共點位于翻滾軸上。圖6為相鄰自旋軸的示意圖，li和li+1分別表示第i時刻和第i+1時刻的火箭箭體的自旋軸，線段pq與li,li+1垂直，點p和點q為相鄰兩幀自旋軸之間的最近點。

圖6 相鄰自旋軸示意圖Fig.6 Illustration of spin axis of consecutive frame

此時最近點可近似看作為公共點，具體的求解方法為：

已知li和li+1的參數方程：

(3)

式中：Ki和Ki+1分別為自旋軸li和li+1上一點，ldir,i和ldir,i+1分別為自旋軸li和li+1的單位方向向量。

因此，點p和點q可表示為：

(4)

式中：hp和hq為點p和點q的參數。

由于線段pq與li,li+1垂直，其點乘為0，如式(5)所示，將式(4)代入式(5)中，得到式(6)。

(5)

(6)

式中：a,b,c,d,e的表達式見式(7)。

(7)

將式(6)代入到式(4)中，可求得點p和點q的坐標軸值，選取點p作為翻滾軸上的公共點。

2)翻滾軸方向向量求解

由圖5可知，自旋軸li和翻滾軸m相交于點m0，由向量的夾角公式可得：

mxli,x+myli,y+mzli,z=cosθ

(8)

式中：ldir,i(li,x,li,y,li,z)和mdir(mx,my,mz)分別表示自旋軸li和翻滾軸m的單位方向向量，θ為章動角。

將式(8)兩邊同時除以cosθ，0≤θ<π/2得到：

(9)

令

(10)

將式(10)代入式(9)中，可得：

ξxli,x+ξyli,y+ξzli,z=1

(11)

自旋軸li的單位方向向量ldir,i(li,x,li,y,li,z)可由第3.2節得到，此時對翻滾軸m的單位方向向量mdir(mx,my,mz)和章動角θ的求解，可看作是對式(11)的參數ξx,ξy,ξz的求解。

選取連續三幀的數據，根據式(11)即可計算出參數ξx,ξy,ξz。考慮隨時間推移，觀測得到的自旋軸方向向量數據增多，為充分利用觀測得到的自旋軸數據，減小觀測數據誤差帶來的影響，采用帶有遺忘因子的最小二乘遞推算法來估計參數ξx,ξy,ξz，具體為：

步驟1:將式(11)用矩陣表示；

(12)

式中：ηi=[ξx,ξy,ξz]T，ψi=[li,x,li,y,li,z]T，yi=1。

步驟2:按照式(13)求解得到協方差矩陣Pi+1，增益矩陣Gi+1，其中ρ為遺忘因子，取經驗值0.9，I為3×3單位陣；

(13)

步驟3:根據Gi+1的值，按照式(14)，求解出i+1時刻的參數ηi+1；

(14)

步驟4:重復步驟2和步驟3，求得每一時刻的參數η。

得到當前時刻的參數η后，將參數單位化，此時得到的向量就是翻滾軸m的單位方向向量。由于至少需要三個時刻的自旋軸測量值，才能計算得到參數η，因此，在序列前兩幀不計算，從第三幀開始計算求解，逐幀得到參數計算結果。

3.4 章動角和空間章動角速率求解

1)章動角求解

由式(10)可知ηcosθ=mdir，且0≤θ<π/2，章動角的求解，如式(15)所示：

(15)

式中：|mdir|為mdir的模，|η|為η的模。

2)空間章動角速率求解

求得火箭箭體的翻滾軸和自旋軸后，可從前后兩幀中估算出自旋軸繞翻滾軸旋轉的空間章動角速率，如圖7所示。

圖7 自旋軸投影示意圖Fig.7 Illustration of spin axis projection

圖7中平面B為一垂直于翻滾軸m的平面，將自旋軸li和li+1分別投影到平面B上，得到兩條投影線l′i和l′i+1，投影線l′i和l′i+1的夾角υ(0≤υ<π/2)除以相鄰幀間的時間間隔即可得到繞翻滾軸旋轉的空間章動角速率Ω，求解如式(16)所示：

(16)

式中：mdir表示翻滾軸m的單位方向向量，ldir,i和ldir,i+1分別表示自旋軸li和li+1的單位方向向量，l′dir,i和l′dir,i+1分別表示投影線l′i和l′i+1的單位方向向量,υ表示相鄰幀間的空間章動角度，Δt表示相鄰幀間的時間間隔。

4 仿真校驗

為校驗本文中提出算法的有效性與精度，本文采用數字仿真實驗，模擬服務航天器在空間環境中對翻滾火箭箭體進行觀測，計算翻滾火箭箭體的運動參數，包括翻滾軸、章動角和空間章動角速率。

仿真使用的火箭箭體模型采用NASA官網的土星V型運載火箭的第二級模型，通過有限元分析軟件將其轉為目標模型點云，如圖8所示。

圖8 土星V型火箭第二級點云Fig.8 Point clouds of Saturn V rocket stage 2

假設火箭箭體在空間中處于帶章動的翻滾運動狀態，由歐拉動力學方程可知，為獲得帶章動的翻滾運動，本體坐標系上各個軸旋轉的旋轉角速率應滿足：

(17)

式中：wx0為繞自旋軸的自旋角速率，wy0和wz0為構成章動角θ的系數，Ωr為本體章動角速率。

對式(17)中的wx,wy和wz做積分：

(18)

式中：φ,φ,λ分別為各個時刻對應的滾轉角、俯仰角、偏航角，φ0,φ0,λ0分別為滾轉角、俯仰角、偏航角的初始值，初始值均設為0°。

依據式(18)的值，按文獻[15]方法，將目標模型點云數據變換到激光成像雷達觀測坐標系下，仿真生成激光成像雷達的點云量測數據。

本文實驗中設激光成像雷達的參數為：分辨率300×300，視場角20°×20°，傳感器測量誤差為50 mm。

4.1 測量精度驗證

為便于圖示，實驗設翻滾軸m的直線參數為(ma,mb,mc,md,me,mf)，其中(ma,mb,mc)為翻滾軸m的一個公共點，(md,me,mf)為翻滾軸m的單位方向向量。

實驗仿真設置：目標慣性坐標系的坐標原點OO在激光成像雷達測量坐標系OCxCyCzC下的坐標為(40,0,0)；翻滾軸m的直線參數為(40,0,0,0,1,0)；章動角為6.96°，空間章動角速率為10°/s，成像時刻按照0,1,2,…,99,100選取，間隔為1 s，共生成101幀點云量測數據。

本文方法求解翻滾軸方向向量參數，由于其至少需要三個時刻的自旋軸測量值才能計算，因此，本文方法從序列第三幀開始，逐幀計算得到翻滾軸參數、章動角、空間章動角速率。從圖9～圖14可以看出，前兩幀為空，沒有計算結果數據。誤差計算為本文方法計算的結果減去實驗設置的真實值；需特殊說明的是翻滾軸方向向量誤差采用本文方法計算的翻滾軸方向向量與方向向量真實值之間的夾角。

圖9和圖10給出了本文方法得到的翻滾軸參數曲線和誤差曲線。圖9(a)為翻滾軸公共點參數曲線圖，將逐幀計算得到的翻滾軸公共點參數減去其真實參數值(40,0,0)，得到翻滾軸公共點參數的誤差值，如圖9(b)。圖10(a)為翻滾軸方向向量參數曲線圖，逐幀計算得到的翻滾軸方向向量參數與其真實參數向量(0,1,0)之間的夾角，結果如圖10(b)。本文求解翻滾軸方向向量參數的方法采用的是帶有遺忘因子的最小二乘遞推算法，因此，曲線的變化有一收斂的過程。從圖10的曲線變化可知本文方法在第15幀時收斂，得到的翻滾軸公共點參數誤差優于1 m，翻滾軸向量參數誤差小于0.6°。

圖11給出了本文方法得到的章動角曲線及其誤差曲線。圖11(a)為求解得到的章動角曲線圖，

圖9 翻滾軸公共點參數曲線和誤差曲線Fig.9 Point curves and error curves of tumbling axis

圖10 翻滾軸方向向量參數曲線和誤差曲線Fig.10 Direction vector curves and error curve of tumbling axis

圖11 章動角曲線和誤差曲線Fig.11 The nutation angle curve and error curve

從圖11(a)的曲線變化可知，第15幀后，章動角收斂，收斂到7°；圖11(b)為章動角誤差曲線圖，由圖11(b)可知，章動角收斂后的測量誤差小于0.5°。

圖12給出了本文方法得到的空間章動角速率曲線及其誤差曲線。圖12(a)為空間章動角速率曲線，從圖12(a)的曲線變化可知，第15幀后，空間章動角速率收斂，收斂到10(°)/s；圖12(b)為空間章動角速率誤差曲線圖，由圖12(b)可知，空間章動角速率收斂后的測量誤差小于2(°)/s。

圖12 空間章動角速率曲線和誤差曲線Fig.12 The spatial nutation angular rate curve and error curve

4.2 測量范圍驗證

1)章動角測量范圍分析

分析了火箭箭體在不同的章動角度下本文方法的測量效果。假設在激光成像雷達相機坐標系OCxCyCzC下，火箭箭體的慣性坐標系的坐標原點OO為(40,0,0)，其翻滾軸m的直線參數為(40,0,0,0,1,0)，火箭箭體繞翻滾軸m做帶章動的翻滾運動。繞翻滾軸旋轉的空間章動角速率固定為10(°)/s，章動角從15°變化到45°，間隔為15°，成像時刻按照0,1,2,…,99,100選取，間隔為1 s，每次生成101幀點云量測數據。圖13給出了在不同章動角的翻滾運動中本文方法計算得到的章動角曲線。

圖13 章動角曲線Fig.13 Nutation angle curves

表1給出了當火箭箭體處于15°、30°、45°章動角的翻滾運動時，測量得到的章動角最大誤差值。

從圖13和表1可知，當火箭箭體處于章動角為15°、30°、45°的翻滾運動時，章動角測量的最大誤差隨章動角增大而增加，且最大誤差在1.3°以內。

表1 章動角測量誤差Table1 Nutation angle error

2)空間章動角速率范圍測量分析

分析火箭箭體在不同的空間章動角速率下本文方法的測量效果。假設在激光成像雷達測量坐標系OCxCyCzC下，火箭箭體的慣性坐標系的坐標原點OO為(40,0,0)，其翻滾軸m的直線參數為(40,0,0,0,1,0)，火箭箭體繞翻滾軸m做帶章動的翻滾運動。固定章動角為7°，空間章動角速率從10 (°)/s變化到30 (°)/s，間隔為10°，成像時刻按照0,1,2,…,99,100選取，間隔為1 s，每次生成101幀點云量測數據。圖14給出了在不同空間章動角速率的翻滾運動中本文方法計算得到的空間章動角速率曲線。

從圖14和表2可知，當火箭箭體處于空間章動角速率為10(°)/s、20(°)/s、30(°)/s的翻滾運動時，空間章動角速率測量的誤差隨空間章動角速率變大而增加。

圖14 不同空間章動角速率曲線Fig.14 Spatial nutation angular rate curves

表2 空間章動角速率測量誤差Table 2 Spatial nutation angle rate error

由實驗可知，本文提出的方法能夠有效地估計處于翻滾運動狀態的火箭箭體的運動參數，得到翻滾火箭箭體的翻滾軸、章動角及空間章動角速率。

5 結 論

針對空間處于自由翻滾運動狀態的火箭箭體的運動參數估計問題，提出了一種翻滾火箭箭體的翻滾軸、章動角、空間章動角速率的估計方法。以激光成像雷達獲取的點云數據為輸入，通過點云表面法向量估計和隨機采樣一致性算法計算出自旋軸，在此基礎上，利用序列幀數據進一步求解得到翻滾軸參數、章動角和空間章動角速率；通過仿真實驗驗證了方法的有效性，結果表明本文方法適用于翻滾火箭箭體的運動參數測量，具有較好的運動參數估計精度和適用范圍，為分析激光成像雷達參數與翻滾目標運動參數估計結果關系的研究奠定基礎。