基于視慣融合的大型空間碎片質心位置辨識

姚金銘，李廣平，張慧博,*，田浩，游斌弟，戴士杰

1. 河北工業大學 省部共建電工裝備可靠性與智能化國家重點實驗室，天津 300401 2. 天津市微低重力環境模擬技術重點實驗室，天津 300301 3. 哈爾濱工業大學 航天學院，哈爾濱 150001 4. 哈爾濱工業大學(威海) 船舶與海洋工程學院，威海 264209

1 引言

隨著人類空間活動的發展，空間碎片的數量日益增多，與在軌航天器的碰撞概率逐漸增大[1-4]。其中，失效衛星和火箭末級等大型空間碎片在碰撞、解體后會產生碎片云，嚴重威脅在軌航天器安全，因此急需開展大型空間碎片移除任務[5-8]。大型空間碎片尺寸大都超過2 m，受地球引力、太陽光壓和殘余角動量的影響[9]，往往呈現出40 r/min的三軸翻滾運動[10]。若直接對空間碎片進行消旋、捕獲等接觸操作，會引起服務星失穩甚至失效，因此需要在接觸碎片前獲取其動力學參數。在碎片的動力學參數中，質心位置是最為重要的，它是建立服務星與目標星間坐標轉換關系、實現后續消旋和捕獲操作的關鍵。

雙目視覺是識別空間碎片質心位置的主流方法[5]。文獻[11-12]針對空間接近操作的第一階段[13]，利用雙目視覺獲取空間碎片特征點坐標，采用增量式平滑和建圖方法解決空間非合作目標的參數估計問題，并通過空間站試驗[14]證明該方法識別的質心位置三軸誤差小于5 mm。但是該方法將識別的特征點幾何中心視為目標質心，不符合實際。文獻[15-16]提出利用雙目視覺識別空間非合作目標的特征點位置和運動速度，進而利用剛體運動理論解算出空間碎片質心位置，并通過數值仿真證明該方法在不同旋轉矢量夾角下解算的質心位置三軸方差均小于0.061。但是該方法由于未考慮空間碎片章動，導致解算精度未能符合消旋捕獲要求。所以現有雙目視覺方法仍無法滿足空間碎片清除任務的需求。

文獻[17]提出基于慣性單元(IMU)的質心位置辨識方法，該方法利用微型衛星將慣性單元附著到空間碎片上，并基于慣性單元采樣頻率高、采樣精度高、不受空間雜光影響的特性，快速獲取碎片運動數據，并實時解算出空間碎片的歸一化慣性參數。但是該方法未能識別質心位置，沒有建立服務星與目標星間的坐標轉換關系，因此無法直接用于針對空間碎片的在軌服務任務。本文深入研究文獻[17]的方法，提出基于慣性單元與雙目視覺的空間碎片質心識別方法，將多個表面攜帶發光標記點的慣性單元通過太空魚叉附著到大型空間碎片表面[18]，通過開展慣性單元冗余測量數據優化，同時輔以雙目視覺對標記點的定位數據，實現對碎片質心位置的解算。

本方法基于無力矩歐拉方程建立空間碎片動力學模型，采用并矢坐標變換獲取慣性單元坐標系間的轉換關系。利用該轉換關系開展慣性單元冗余測量數據優化，再利用優化后的數據優化求解慣性單元到質心的距離。基于三角測量原理，獲取各時刻發光標記點的三維坐標。利用求得的標記點坐標與慣性單元到質心距離，基于三點定位原理求解各時刻帶噪聲的質心位置，形成質心點集。最后基于最小二乘原理解算質心坐標，實現對空間碎片質心位置的精確辨識，為后續消旋、捕獲操作提供準確、可靠的數據基準。

2 基于視慣融合的質心位置辨識原理

2.1 坐標系系統

本文參考文獻[19]，將空間碎片質心識別任務簡化為：針對在軌操作第3階段的追蹤衛星對目標碎片T的同軌檢測任務，則目標碎片質心在追蹤衛星坐標系下可視為常值。其中空間碎片T的中心主軸坐標系M位于碎片質心OT，其各軸均平行于與主慣量矩方向，且z軸指向空間碎片慣量最大方向，所以該坐標系與剛體的質量分布有關，為空間碎片的連體坐標系。

文中考慮了慣性單元k的體坐標系Sk(k=1，2，3)，Sk各軸由傳感器出場設置決定，且在發射后未知其空間指向。但是由于慣性單元與空間碎片固連，所以Sk為碎片的連體坐標系。Lk為發光標記點坐標系，其原點位于發光標記點，各軸平行于Sk。在發射前已知其與Sk的方向余弦矩陣及Sk與Lk原點間的向量，該坐標系同樣是空間碎片的連體坐標系。

本文將追蹤衛星上視覺坐標系C定義在雙目視覺的右相機光心OC，其z軸指向空間碎片，x軸與太陽翼平行，y軸與其他兩軸垂直且符合右手定則。同時定義其像素坐標系Op-ur-vr，其原點Op位于成像平面左上角，u和v分別表示橫縱像素坐標，下標r代表其為右相機的像素坐標。(cu,r,cv,r)為右相機光學中心的像素坐標。各坐標系的示意圖如1所示。

2.2 冗余慣性測量數據優化

首先需要建立慣性單元間的轉換關系。空間碎片位于微重力環境中，可以將其視為無力矩狀態下繞質心的剛體定點運動，即歐拉-潘索運動[20-21]。本文利用無力矩歐拉方程進行空間碎片動力學建模，

Jω+ω×(J·ω)=03×1

(1)

慣性張量的基本形式中包含6個獨立參數，包括相對三軸的慣量矩及坐標平面的慣量積，表示該坐標系下物體的質量分布。當坐標系中各軸指向慣量主軸時，即在M坐標系下，慣性張量矩陣則變為只包含3個主慣量矩的對角陣(也可歸一化為參數Ja和Jb)，此變換可由并矢坐標變換法求得：

建立數據矩陣：

其中l=1，2，3。利用R|SltoM與R|MtoSk建立目標函數：

(2)

利用剛體定點運動公式，導出第k個慣性單元到質心距離向量rk的計算公式：

(3)

2.3 質心位置辨識

為進行質心定位，本小節基于視覺三角測量進行標記點定位，再輔以第2.2小節求得的標記點到質心距離，基于三點定位原理求得各時刻的帶噪聲質心，形成質心點集，最后基于最小二乘原理求解質心位置。本節將基于對極約束，輔以不同顏色的發光標記點進行左右相機圖像匹配。坐標系及原理示意如圖2所示。

圖2 質心識別原理Fig.2 Reference frames and coordinate systems

i時刻視覺坐標系C上第k個慣性單元上發光標記點位置可以通過三角測量法求得，

式中：fr為右相機的焦距；b為基底；(iur,k,ivr,k)為i時刻右相機測得的第k個慣性單元上發光標記點的像素坐標。雙目相機在測量前已經進行了相應的標定和圖像校正，保證了兩個相機的焦距和光學中心像素坐標相等，即fr=fl=f，cu,r=cu,l，cv,r=cu,l。相機相關的坐標系如圖1所示。

圖1 坐標系系統Fig.1 Reference frames and coordinate systems

利用i時刻視覺測量的標記點的位置和rk|Lk，在每個采樣時刻建立3個球面，可知在視覺數據準確的情況下，質心應該位于3個球面的交點處。但由于視覺存在測量誤差，球面大概率不能在一點相交，所以質心應定位在空間中與球面距離最短的點上，進而轉化為一個無約束優化問題。所以可以建立目標函數：

iRk=(iOx|C-iXk|C)2+(iOy|C-iYk|C)2+

(iOz|C-iZk|C)2

其中，

iOT|C=[iOx|CiOy|CiOz|C]T

進行多個時刻的質心求解可以得到質心點集，利用最小二乘法，求得空間中到點集中坐標點的距離最小的一點，則該點為較為準確的質心位置：

ΔO=(iOTx|C-Ox|C)2+(iOTy|C-Oy|C)2+

(iOTz|C-Oz|C)2

圖3 算法流程Fig.3 Algorithm flow chart

3 質心位置辨識仿真

為了證明L-M算法對本文問題的有效性和可靠性，本節運用蒙特卡洛分析法對比L-M算法和擴展卡爾曼濾波算法對慣性單元1坐標系下J|S1的計算效果。為簡化計算，設置空間碎片為2 m×2 m×2 m的大型空間碎片，采用張貼質量塊的方式改變其主慣量矩(歸一化主慣量矩分別為2.6、2、1)，碎片質心位于視覺坐標系下的(-1 m,1 m,4 m)處；設定仿真次數為100次，每次的仿真初始值設置為從(0 m,0 m,0 m)到(-0.9 m,0.9 m,2.9 m)的隨機點；在該仿真中有3個慣性單元附著到了空間碎片上，各慣性單元連體基與碎片主軸間的夾角已知，且其與質心的距離分別為1.5 m、1.6 m、1.6 m，慣性單元上設置發光標記點，以提高較差光照條件下的視覺定位精度；參考文獻[9]，將所有噪聲設置為零均值高斯分布，考慮極端的光照環境，將視覺數據設為標準差為2.2像素的高斯分布，其數值大于文獻[16]的1像素，視覺采樣頻率設為20 Hz；參考傳感器實際的出廠設置，將慣性單元的采樣頻率設置為200 Hz，同時考慮實際測量值和空間環境影響，將其出廠標定的標準差放大3倍，其中角速度誤差分布的方差為0.008 7 rad/s，角加速度誤差分布的方差為0.017 44 rad /s2，線加速度的誤差方差為0.980 1 m/s2，碎片的初始角速度設置為[1 10 2]r/min；設置每采樣5個點進行迭代計算，最大迭代次數設置為10。對J|S1的仿真如表1所示。

表1 L-M算法與擴展卡爾曼濾波算法對比Table 1 Comparison of L-M algorithm and extended Kalman filter algorithm

由表1可知，在100次不同初始條件的仿真中， L-M算法實現了全部收斂，而擴展卡爾曼濾波只有87次收斂。經分析是由于有13次的迭代初始值與真實值相差較遠，使得觀測方程誤差較大，導致擴展卡爾曼濾波不收斂，而L-M算法受初始條件影響較小，因此實現了全收斂。在收斂的仿真中，兩種算法的最終誤差均能收斂到0.025左右，但L-M算法由于更好的局部收斂性，所以比擴展卡爾曼濾波更快收斂。因此針對文中無約束優化問題，選用穩定性更好、收斂速度更快的L-M算法。針對碎片質心位置辨識的仿真結果如圖4～7及表2所示。

表2 仿真結果：慣性單元冗余測量數據優化后誤差Table 2 Results of simulations: error of redundant inertial measurement unit’s data after optimization

針對慣量矩測量，6參數慣性張量與3參數主慣量矩矩陣的轉換是通過并矢坐標變換求得，同時慣性張量與慣性矩的誤差與慣性單元坐標系間方向余弦誤差是正相關的。因此為了使圖像表達更為清晰，本節采用三參數主慣量矩矩陣衡量慣性張量和方向余弦矩陣的辨識效果，并采用文獻[11-12]對觀測性的歸一化方法，將3參數主慣量矩矩陣進一步簡化為兩個歸一化主慣量矩(分別為Ja和Jb)。如圖4所示，可以看到當計算到2.09 s后，歸一化主慣量矩誤差收斂到0.03(無量綱)以下，該辨識精度符合后續計算要求，證明本文模型計算效果較好；同時由于本算法需要的數據量較小，計算時間較短，所以能夠降低隨機干擾的影響。

圖4 仿真結果：歸一化主慣量矩誤差Fig.4 Results of simulations: error of normalized principal moment of inertia

慣性單元冗余測量數據的實時優化結果如表2所示，可以看到，角速度的三軸平均相對誤差均小于或等于0.900 8%，角加速度的三軸平均相對誤差均小于或等于0.964 1%，而線加速度的三軸平均相對誤差均小于或等于0.920 9%。慣性單元測量數據優化后的相對誤差均降低到1%以下，實現了較好的實時優化效果，為后續的質心解算提供了良好的數據支持。

圖5顯示了慣性單元到質心距離的仿真誤差，可以看到在計算到2.55 s后，3個慣性單元到質心的距離均收斂到0.98 mm以下，符合質心求解的精度及實時性要求。

圖5 仿真結果：慣性單元到空間碎片質心的距離誤差Fig.5 Results of simulations: the error of the distance from the inertia sensors to the mass center of the space debris

圖6顯示了質心識別的過程。3種不同顏色小點代表不同傳感器上標記點的視覺定位坐標，而較大的紅色圓點表示質心點集，左上角的放大圖為質心點集的優化過程；圖7為質心識別的誤差曲線，可以看到當計算時間達到6.83 s時，質心的三軸誤差均收斂到0.47 mm以下，解算的數據精度符合消旋、捕獲的任務要求。

圖6 仿真結果：質心識別結果三維圖Fig.6 Results of simulations: three-dimensional diagram of the mass-center recognition result

圖7 仿真結果：質心位置識別誤差Fig.7 Results of simulations: error of mass-center location identification

4 質心位置辨識地面試驗

為了驗證本方法的有效性和可靠性，本節開展了地面模擬試驗。設計的地面模擬試驗系統如圖8所示，其中所用的3個慣性單元為WT-901，其采樣頻率為200 Hz，通過Wifi與上位機通信，各慣性單元與質心的距離分別為0.3 m、0.32 m、0.32 m，慣性單元參數如表3所示；慣性單元表面粘貼了不同顏色的標記點，以模擬太空環境中的發光標記點；所用雙目相機為MYNTAI-S2110，其采樣頻率為20 Hz，分辨率為720×480，雙目參數如表4所示；碎片模擬裝置為等比例縮小的仿真模型，尺寸為0.4 m×0.4 m×0.4 m，質心位于視覺坐標系(-0.2 m,0.2 m,0.8 m)處；選用球鉸作為回轉構件，以實現碎片的三軸旋轉；參考現有航天器質量特性調整機構，設計了一套十字滑臺與質量塊的組合裝置作為調心機構，質量塊為對稱設置的鋁合金塊和不銹鋼塊，以實現數值為2.6、2和1的歸一化主慣量矩；設置試驗裝置的初始角速度與仿真對應，均為[1，10，2]r/min，試驗數據如圖9所示。

圖8 試驗裝置Fig.8 Experimental device

表3 慣性單元性能指標Table 3 Performance index of IMU

表4 雙目相機性能指標Table 4 Performance index of binocular camera

圖9 試驗數據Fig.9 Experimental data

由于試驗中慣性單元冗余測量數據的優化效果較難恒定，但其辨識結果與慣性單元到空間碎片質心距離的計算結果正相關，因此只進行歸一化主慣量矩、慣性單元到空間碎片質心的距離誤差及碎片質心位置的求解，試驗結果如圖10～12所示。

圖10 試驗結果：歸一化主慣量矩誤差Fig.10 Results of experiments: normalized principal inertia moment's error of experiments

可以看到，當計算到5.42 s時，3個慣性單元的歸一化主慣量矩(無量綱)均收斂到0.027以下；當計算到6.79 s時，慣性單元到質心向量的三軸誤差均下降到0.86 mm以下，以該誤差衡量慣性單元冗余測量數據優化結果，可知優化后慣性單元測量數據平均相對誤差小于1.1%，證明了該方法可以實現較好的優化效果；在9.21 s時，解算的質心位置三軸誤差收斂到0.49 mm以下。相比于仿真，試驗中算法的收斂時間變長，其主要是受球較處摩擦力、空氣阻力以及調心誤差等系統誤差的影響，導致參數優化需要更多的數據點，從而使收斂時間增加。

表5為本方法的仿真、試驗結果與文獻[19]的雙目視覺方法仿真結果、文獻[11]的雙目視覺方法試驗結果的對比。可以看到，由于雙目視覺受空間復雜光照條件的影響，其測量精度低于慣性單元，因此本方法的收斂時間和誤差小于純視覺方法；Tweddle的雙目視覺方法識別對象為慣性對稱物體，因此其將碎片特征點的幾何中心辨識為碎片質心，但是其在用于非慣性對稱對象時存在一定的質心偏移，而本方法采用碎片的動力學特性識別空間碎片質心，所以本文方法更符合實際，同時本文方法的質心識別誤差小于文獻[11]的雙目視覺方法。上述對比證明本方法更符合工程實際需求。

圖11 試驗結果：慣性單元到空間碎片質心的距離誤差Fig.11 Results of experiments: the error of the distance from the inertia sensors to the mass center of the space debris

圖12 試驗結果：質心位置識別誤差Fig.12 Results of experiments: error of mass-center location identification

表5 本文算法與現有雙目視覺方法對比Table 5 Comparison of the algorithm with existing binocular vision methods

5 結論

本方法針對火箭末級、失效衛星等大型空間碎片的質心位置辨識問題，提出了基于慣性單元和雙目視覺數據的空間碎片質心位置識別方法，基于慣性單元的實時性和雙目視覺的定位優勢，提高質心辨識精度。基于無力矩歐拉方程，通過并矢坐標變換求得主軸坐標系對應的歸一化主慣量矩，同時獲取慣性單元間的方向余弦矩陣，建立慣性單元間的轉換關系，以加入高斯白噪聲的慣性單元與視覺測量數據進行數值仿真，仿真表明當計算到2.09 s時3個慣性單元的歸一化主慣量矩誤差(無量綱)均收斂到0.03以下。搭建了地面模擬試驗系統，試驗表明在計算到5.42 s時，3個慣性單元的歸一化主慣量矩誤差(無量綱)均收斂到0.024以下；基于慣性單元間轉換關系，進行慣性單元冗余測量數據的實時優化，優化后的角速度、角加速度、線加速度瞬時三軸誤差均小于1%；利用優化后的慣性單元測量數據，解算出慣性單元到質心點的向量，仿真結果表明在計算到2.55 s后的該向量的三軸誤差收斂到0.98 mm以下，試驗中在5.89 s后該向量的三軸誤差小于0.86 mm，以試驗中該三軸誤差衡量優化結果，可知優化后慣性單元測量數據平均相對誤差小于1.1%；基于三點定位原理，求解各時刻帶噪聲的質心位置，形成質心點集，再基于最小二乘原理進行了質心位置的優化求解，當仿真時間大于6.83 s、質心位置三軸誤差小于0.47 m、試驗時間大于9.21 s時，質心位置三軸誤差均小于0.49 m。仿真和試驗數據證明該方法精度高，實時性好，滿足空間碎片清除任務的需求，能夠大幅提高消旋、捕獲動力學參數的檢測精度，為針對空間碎片的在軌服務任務提供準確的數據基礎。