一種參數化非合作目標相對位姿和慣量估計方法

馮 乾，潘 泉，侯曉磊，楊家男

(1. 西北工業大學自動化學院，西安 710072；2. 西北工業大學信息融合教育部重點實驗室，西安 710072)

0 引 言

近些年來，隨著人類空間活動日益增加，在外太空滯留大量的失效航天器、廢棄衛星及火箭末級等空間碎片，這些空間碎片對其它航天器的正常運行構成巨大的潛在威脅[1-2]。據美國空間監測網絡(Space surveillance network, SSN)報告，截止2020年，環繞在地球周圍的航天器(包括主動/被動衛星、衛星碎片、火箭外殼等)總數大約為20000個，其中包含約11000個解體碎片，2000個任務相關碎片[3]。為了維護空間安全，降低空間碎片與正常航天器碰撞的概率，亟需采用主動清除手段來減少這些碎片[4]。為了實現對空間碎片的有效抓捕/清除，對空間碎片的相對狀態(包括相對位置、相對線速度、相對姿態和角速度)和慣量參數進行有效估計具有重要的現實意義[5-6]。

與傳統交會對接任務中的合作目標相比，大部分空間碎片屬于非合作目標，即目標的幾何參數和慣量參數未知，沒有可以直接測量角速度的傳感器，缺乏航天器間的相互通信，這些特征給非合作目標的相對導航帶來了很大挑戰[7]。CCD/CMOS等視覺傳感器具有非接觸、體積小、功耗低等優勢，廣泛應用于非合作目標的相對位姿(位置和姿態)測量。文獻[8]提出一種基于單目視覺的迭代算法來求解非合作目標相對位姿參數，該方法假設目標的形狀及幾何尺寸已知，通過迭代方法求解目標的相對位姿。文獻[9]通過前后不同時刻非合作目標旋轉軸的中線中點來近似非合作目標質心位置，對于高速旋轉且低速平移的非合作目標能夠較好地近似質心位置。文獻[10]假設先驗已知目標的CAD結構，采用迭代最近點(Iterative closest point,ICP)方法來解算目標的相對位姿信息，同時還引入狀態預測反饋來提高ICP算法的魯棒性。文獻[11]提出一種基于雙目立體視覺的相對位姿測量方法，該方法以航天器上太陽帆板支架作為識別對象，借助支架上的三個頂點來解算非合作目標的相對位姿參數，但在第一幀識別過程中需要借助地面遙測通過人機交互方式來選擇支架的一組內外點。文獻[12]提出一種基于幾何特征綜合匹配的雙目視覺相對位姿測量方法，該方法以非合作目標的太陽帆板為幾何特征，經過特征提取及特征匹配獲取太陽帆板上的若干特征點信息，進而解算出非合作目標的相對位姿參數。

為了實現對非合作翻滾目標的精確近場操作，需要估計出非合作目標的慣量參數，進而預測出非合作目標的旋轉狀態[13]。文獻[14]采用立體視覺追蹤目標上若干特征點，預先給定目標慣性張量的若干模態，在每種模態下采用迭代擴展卡爾曼濾波(Iterated extended Kalman filter, IEKF)估計出相對位姿參數和特征點位置，然后基于各模態下的位姿參數和立體視覺量測采用最大后驗概率估計獲得目標的最優慣性張量和位姿參數，但該方法得到的慣性張量只能是預先給出各模態中的某一模態。文獻[15]由立體視覺獲得的非合作目標上三個特征點建立非合作目標幾何坐標系，然后基于該幾何坐標系量測先后設計了姿態測量和相對導航濾波器，從而估計出非合作目標的相對狀態和慣量比率，同時借助粘附衛星實現對非合作目標質量和轉動慣量的辨識，但在慣量比率的估計過程中沒有考慮各慣量比率之間的不等式約束。文獻[16]通過引入目標的慣量比率來傳播目標的角速度，設計了基于擴展卡爾曼濾波(Extended Kalman filter, EKF)的單目視覺相對導航方法，但所引入的三個慣量比率彼此不獨立且存在區間約束。文獻[17]提出了一種基于即時定位與地圖構建(Simultaneous localization and mapping,SLAM)技術的非合作目標相對位姿估計方法，該方法通過反復迭代來求解目標的相對位姿、質心位置、主慣性軸及慣量比率等參數，但該方法計算量大，很難滿足實時性要求。文獻[18]建立了一種非合作目標相對特征的運動模型和投影模型，然后對非合作目標的旋轉運動和平移運動進行解耦，最終設計EKF算法對非合作目標的相對狀態進行估計，其中慣量比率采用冪指數形式來參數化，雖然取值范圍沒有限制，但彼此之間需滿足三角約束，即任意兩個軸主轉動慣量之和大于第三軸主轉動慣量。文獻[19]以非合作目標固有特征的像素位置為觀測輸入，提出一種基于單目視覺的非合作目標相對狀態估計方法，其中慣量參數采用文獻[18]的參數化方法，也存在相應的不等式約束。

到目前為止，由立體視覺系統同時估計非合作目標的相對狀態和慣量參數仍然是一個研究難點，現有的參數化方法所描述的慣量參數都存在等式約束或不等式約束，不能作為獨立的自由變量進行狀態估計。針對非合作目標缺乏先驗模型，無法直接獲得其角速度以及慣量參數的約束問題，本文采用立體視覺測量系統跟蹤目標上的若干特征點，用反雙曲正切函數來描述非合作目標的慣量參數，結合自由航天器姿態動力學模型來傳播非合作目標的角速度，進而設計EKF算法以同時估計非合作目標的相對狀態和慣量參數。其中，本文所提的慣量參數化方法僅含兩個獨立變量，且充分考慮了慣量參數之間的相互約束。

本文其余部分安排如下：第1節給出本文用到的各類坐標系，并推導出非合作目標的相對平移動力學方程、相對姿態運動學方程、姿態動力學方程以及立體視覺量測方程；第2節設計了估計非合作目標相對狀態和慣量參數的EKF算法；第3節給出了數值仿真驗證；第4節總結了本文所提的算法。

1 系統模型

本節主要介紹濾波器設計所需的各類系統模型，包括非合作目標的相對平移動力學方程、相對姿態運動學方程、非合作目標姿態動力學方程以及立體視覺量測方程。尤其在非合作目標姿態動力學模型中，創造性地引入了反雙曲正切函數來描述慣量參數。

本文用到的坐標系包括地心慣性坐標系、本體坐標系、軌道坐標系和相機坐標系，各坐標系的示意圖如圖1所示。具體定義如下：

圖1 各坐標系示意圖Fig.1 Schematic diagram for the coordinate systems

1.1 相對平移動力學方程

(1)

(2)

1.2 相對姿態運動學方程

R(qtc)=ΞT(qtc)Ψ(qtc)

(3)

其中:

式中:In表示n×n的單位矩陣;a×是向量a=[a1,a2,a3]T對應的叉乘矩陣，可表示為:

(4)

用四元數描述的相對姿態運動學方程可表示為:

(5)

式(5)的一種離散化描述為:

(6)

其中

(7)

(8)

1.3 非合作目標航天器姿態動力學方程

在不考慮主動力矩控制和噪聲情況下，非合作目標航天器的姿態動力學模型可表示為:

(9)

式中:Jt=diag(Jtxx,Jtyy,Jtzz)為非合作目標航天器的主慣性張量矩陣，Jtxx、Jtyy和Jtzz分別對應各軸主轉動慣量，展開式(9)可得:

(10)

由式(10)可以看出，當非合作目標航天器的主轉動慣量等比擴大或縮小相同比例時，其姿態動力學方程保持不變，因此在沒有對非合作目標航天器施加主動力矩的情況下，基于視覺的測量方法只能解算出轉動慣量的相對值。定義以下慣量比率:

(11)

由定義(11)可得各慣量比率之間存在以下約束:

px+py+pz+pxpypz=0

(12)

顯然，式(11)定義的三個慣量比率并不獨立，由等式約束(12)可知慣量比率只有兩個獨立變量，即任一慣量比率可由其它兩個慣量比率來表示，如:

(13)

此外，由主轉動慣量的物理意義可得以下約束:

(14)

對應的慣量比率滿足約束:

(15)

如果直接將式(11)定義的慣量比率作為狀態量進行估計，存在以下問題：1)其取值范圍應為(-1,1)，不是取值任意的高斯隨機變量；2)采用三個非獨立的變量描述慣量比率，最終估計出的慣量比率不再滿足等式約束式(12)；3)在某些時刻，所估計的慣量比率因不滿足不等式約束式(15)而失去物理意義。為此，本文提出一種用反雙曲正切函數來描述慣量參數的方法，定義慣量參數kω=[k1,k2]T，使得:

(16)

此時慣性張量可表示為:

(17)

由定義(16)可知參數kω各分量的取值范圍為(-∞， +∞)，由式(17)可知kω也滿足慣量參數之間不等式約束式(14)。如果將式(17)代入式(11)，可以得到:

(18)

顯然，此時各慣量比率可表示為參數kω的雙曲正切函數形式，由雙曲正切函數的值域為(-1,1)可知慣量比率也滿足式(15)的約束。因此，用反雙曲正切函數定義的含兩個獨立變量的慣量參數kω能很好地滿足等式約束式(12)和不等式約束式(14)，慣量參數都符合物理意義，且kω各分量的取值范圍為(-∞，+∞)，可直接作為無約束的狀態變量進行估計。通過這種潛在包含約束的參數化方法，其實質是引入了新信息(約束信息)，從而提高狀態估計的性能。

考慮到重力梯度、太陽光壓及空間氣壓等對非合作目標航天器產生的干擾力矩t=[tx,ty,tz]T該干擾力矩服從均值為零,協方差為的正態分布，則式(10)可重新寫為:

(19)

將參數k1和k2代入式(19)，得:

(20)

其中:

1.4 立體視覺測量方程

圖2 立體視覺測量系統Fig.2 Stereo vision measurement system

(21)

式中:[uiL,viL]T和[uiR,viR]T分別是左右相機的圖像坐標，f是相機的焦距，b是相機的基線。

(22)

(23)

(24)

式中:N為特征點數目。

圖3 特征點坐標幾何關系圖Fig.3 Geometric relationship of some feature point’s coordinates

(25)

2 濾波器設計

基于以上各運動模型和立體視覺量測模型，本節主要對各狀態方程和量測方程進行線性化處理，并設計EKF算法以估計各狀態量。

2.1 線性化狀態方程

根據誤差四元數的定義得:

(26)

對式(26)兩端求導得:

(27)

四元數估計量的姿態運動學方程為:

(28)

分別將式(5)、(26)和(28)代入式(27)得:

(29)

(30)

(31)

將式(31)代入式(29)并忽略二階項，得:

(32)

(33)

其中:

對于剛體目標，其慣性張量是常量，對應的慣量參數也是常量，即慣量參數kω的誤差方程滿足:

(34)

由式(2)可得相對平移誤差運動學方程為:

(35)

式中:

綜上，線性化的狀態誤差運動學方程可表示為:

(36)

式中:

對應的系統噪聲序列方差陣為:

(37)

2.2 線性化量測方程

結合式(21)，對量測方程(22)進行線性化可得:

1≤i≤N

(38)

式中:

(39)

基于EKF的相對狀態和慣量參數估計算法總結見表1。

表1 基于EKF的相對狀態和慣量參數估計算法Table 1 Relative state and inertia parameters estimation algorithm based on EKF

其中，上標“-”和“+”分別對應各分量的預測值和更新值。

3 仿真校驗

表2 特征點位置坐標Table 2 Location of the feature points

表3 仿真參數Table 3 Simulations parameters

在仿真中，相對姿態的濾波初值是在真值上加[1°, -1°, 1°]T的姿態誤差；非合作目標航天器角速度、相對位置和線速度的濾波初值均在真值上加1σ誤差；慣量參數的濾波初值設為零。濾波器的初始協方差矩陣為：

非合作目標的真實相對位置軌跡如圖4所示，200次蒙特卡洛仿真的誤差和3σ邊界結果如圖5～9所示，其中，3σ是3倍的200次蒙特卡洛仿真樣本標準差。

圖4 非合作目標真實相對位置Fig.4 The true relative position of the non-cooperative target

圖5 相對姿態估計誤差和3σ邊界Fig.5 Relative attitude errors and 3σ bounds

圖6 相對角速度估計誤差和3σ邊界Fig.6 Relative angular velocity errors and 3σ bounds

圖7 慣量參數估計誤差和3σ邊界Fig.7 Inertia parameter errors and 3σ bounds

圖8 相對位置估計誤差和3σ邊界Fig.8 Relative position errors and 3σ bounds

圖9 相對線速度估計誤差和3σ邊界Fig.9 Relative linear velocity errors and 3σ bounds

為了進一步量化仿真誤差參數，定義時間平均均方根誤差(Root time-average mean square error, RTMSE)為:

(40)

eθ=2arccos(δqtc4)

(41)

在200次蒙特卡洛仿真中，由最后50 min仿真結果求得各狀態量的eRTSME值見表4。

表4 各狀態量時間平均均方根誤差Table 4 RTSME for each variable

考慮特征點數目、目標尺寸(即特征點分布)以及噪聲水平對算法性能的影響，仿真對比以下三種場景：1)特征點數目分別為3、4、6、9和12，目標尺寸為1.0 m，噪聲水平為10-5rad； 2)特征點數目為4，目標尺寸分別為0.2 m、0.6 m、1.0 m、2.0 m和3.0 m，噪聲水平為10-5rad；3)特征點數目為4，目標尺寸為0.6 m，噪聲水平分別為10-5rad、 2×10-5rad、 4×10-5rad、 6×10-5rad和8×10-5rad。對應200次蒙特卡洛結果的eRTSME見表5～7。

表5 特征點個數對算法性能的影響Table 5 The influence of the number of the feature points on the algorithm performance

由圖5～9的仿真結果可以看出，所設計的算法能夠有效估計出非合作目標的相對位姿和慣量參數，且結果都收斂于各自的3σ邊界。在中間階段誤差和3σ邊界略有增大，對比圖3的y軸可知，這是由于這一階段非合作目標深度增加而導致立體視覺測量誤差增大造成的。結合表4的平均時間均方根誤差的量化結果看，各估計量都能夠達到較高精度，相對姿態誤差小于0.1°，相對角速度誤差小于6×10-5rad/s，相對位置誤差不超過10 mm，相對線速度誤差低于10 μm，同時慣量參數也收斂到不超過0.02的誤差。

對比分析表5可知，隨著特征點數目的增加，各狀態量的估計精度逐漸提高，但計算量也隨之加重，權衡精度和計算量，選取4個特征點較為合適。對比分析表6可知，隨著目標尺寸的增大，各狀態量的估計精度也逐漸提高，這是因為特征點的分布更加分散，立體視覺量測信息差異性更大，量測信息也更豐富。對比分析表7可以看出，噪聲水平直接影響各狀態量的估計，在實際應用中盡可能選取噪聲水平低的測量系統，但從表7也可以看出，即使在8×10-5rad的噪聲水平下，所提算法仍能達到較高的精度。此外，表5～7的慣量參數估計結果表明，其估計精度在各場景中變化不大，這是因為作為常量的慣量參數狀態信息不豐富，同時本文采用非線性的反雙曲正切函數來描述慣量參數，在所設計的EKF算法中，非線性是影響其估計精度的主要原因。

表6 目標尺寸對算法性能的影響Table 6 The influence of the size of the targe on the algorithm performance

表7 噪聲水平對算法性能的影響Table 7 The influence of the measurement noise level on the algorithm performance

4 結 論

本文針對非合作目標缺乏可直接量測的角速度信息和慣量參數約束問題，通過引入反雙曲正切函數來描述慣量參數，借助姿態動力學模型來傳播非合作目標的角速度，并結合立體視覺測量系統跟蹤測量非合作目標上三個特征點的位置信息，設計了一種EKF算法來估計非合作目標的相對位姿和慣量參數。該算法所引入的慣量參數只包含兩個獨立變量，且沒有區間約束，能夠直接作為濾波器的狀態隨機變量進行估計。通過蒙特卡洛數值仿真結果表明，所設計算法在不同量測噪聲水平下都能夠有效估計出非合作目標相對位姿和慣量參數，且隨特征點數目和目標尺寸的增加，估計精度逐漸提高。對于引入反雙曲正切函數所增加的系統非線性，下一階段將考慮采用基于梯度或隨機優化等方法來提高處理系統非線性的能力。