采用雙目視覺的非合作空間目標相對導航與慣性參數辨識方法

胡啟陽，王大軼

(1.北京控制工程研究所，北京100190；2.北京空間飛行器總體設計部，北京100094)

0 引 言

在軌服務，即利用空間智能服務航天器對目標進行維護，加注，輔助離軌等操作，有助于延長航天器的使用壽命，降低運營成本，對于促進航天事業的可持續發展具有重要意義。許多國家已將在軌服務列為本國航天領域重要的發展方向，并開展對相關技術的攻關[1-3]。在軌服務包括交會，捕獲，操作，分離等一系列過程。其中，對目標進行感知測量以獲取目標狀態與性質的相關信息，是在軌服務過程的關鍵環節，也是服務航天器對目標接近并實施在軌操作的前提。在軌服務的對象通常屬于無應答，無標識，不配合的“非合作目標”。因此，作為在軌服務關鍵技術，對于非合作目標的測量感知技術具有重要的研究價值和迫切的現實需求。

對于非合作目標的測量感知包括獲取表征目標運動狀態的軌道姿態動力學參數以及目標固有屬性的質量與轉動慣量，二者分別屬于相對導航和慣性參數辨識問題。相對導航方面，目前的研究多基于視覺測量手段，即利用光學敏感器對目標進行連續觀測，并建立目標表面特征與目標自身運動規律的關系，進而設計相應的導航濾波器來實現[4-6]。文獻[7]利用激光雷達獲得的目標表面點云數據，結合迭代最近點和自適應濾波方法，實現了目標軌道姿態參數和慣量比的估計。文獻[8]利用位姿圖優化方法消除點云迭代過程中的累計誤差，并采用了基于李群的濾波算法。文獻[9]利用雙目相機觀測目標表面的特征點進行相對導航，推導了一般的耦合模型描述姿態運動對位置運動的影響。文獻[10]通過考慮相機安裝偏置實現利用單目相機對空間碎片的相對位姿估計。針對單一觀測精度受限的問題，文獻[11]提出了利用多平臺觀測的方法，并比較了不同的信息融合算法對導航精度的提升效果。以上研究多以非合作目標的慣性主軸描述姿態，由于主軸定義方式的不同，可能導致多解的情況，影響濾波穩定性。同時，對于相對軌道運動多以線性化的方程描述，制約了模型的精度。此外，現有文獻多采用軌道姿態同時估計的方法，增加了系統的維數，增加了計算負擔，不便于星上使用；慣性參數辨識方面，對于自由旋轉的目標，僅依靠視覺測量無法確定目標的質量與轉動慣量。因此，改變目標的運動狀態是進行慣性參數辨識的前提。目前對于非合作目標的質量和轉動慣量的辨識主要依靠服務航天器施加激勵來改變目標運動狀態，并利用線/角動量守恒或牛頓歐拉法原理實現[12]。文獻[13]利用機械臂改變航天器系統的運動狀態，同時測量速度與角速度，利用最小二乘法實現全部慣性參數的辨識。文獻[14]則將慣性參數辨識轉化為一個非線性優化問題，利用粒子群算法進行求解。針對最小二乘法計算量大的問題，文獻[15]利用細胞機器人交互來實現分布與異步式的參數辨識。文獻[16]建立了機器人末端關節捕獲目標的動力學模型，并著重分析了碰撞次數對于參數可辨識性的影響。文獻[17]利用柔性桿對目標觸碰施加激勵實現辨識，該方法需要精確測量桿件施加的力與力矩?？梢?，目前的辨識方法大多需要服務航天器直接與目標接觸，并且一般需要同時測量所施加的激勵以及運動狀態的改變，因此對于相應敏感器的配置具有一定的要求。同時，外部環境的干擾會影響動量守恒或牛頓歐拉方程的使用。對此，文獻[18]提出了一種黏附激勵下的慣性參數辨識方法，利用黏附衛星與非合作目標黏附前后速度的變化，結合動量守恒原理實現了目標質量的辨識。黏附衛星技術的成熟，為服務航天器在無需與目標直接接觸的情況下實現目標全部慣性參數辨識提供了可能性。

針對上述方法的不足，本文提出一種基于雙目視覺的非合作目標相對導航與慣性參數辨識方法。首先利用目標表面的特征點來定義目標的位姿，并分別設計了姿態測量和相對導航濾波器實現姿態軌道參數的估計，增強了算法的適用性，并減少了計算負擔。同時，相對運動采用軌道根數的方法進行描述，提高了建模精度。在此基礎上，僅利用雙目相機結合導航算法對與黏附衛星和目標形成的組合體進行觀測，而無需對于激勵的測量以及動量守恒的假設即可實現目標質量和轉動慣量的有效辨識。

1 基于雙目相機的相對導航方法

1.1 坐標系建立

利用雙目視覺實現相對導航的問題，涉及雙目相機觀測量以及相對運動模型的表征。為便于描述，定義如下坐標系：

地球慣性坐標系{I}:原點為地心，x軸指向春分點,z軸指向地球北極，y軸的確定滿足右手正交系。

服務航天器軌道坐標系{L}:原點為服務航天器質心，x軸方向沿服務航天器矢徑方向，z軸為軌道角動量方向,y軸的確定滿足右手正交系。

雙目相機坐標系{C}:原點為左相機的投影原點OC，x軸，y軸方向平行于成像平面并正交，z軸沿左相機的光軸垂直于成像平面。

非合作目標幾何坐標系{T}:根據相機觀測結果進行建立，并與非合作目標相固連，是描述非合作目標姿態運動的基準。其具體定義方法見下文。

1.2 基于雙目視覺的位姿測量

雙目相機對非合作目標進行連續觀測，并通過圖像處理技術識別和跟蹤非合作目標表面的N個特征點。

圖1 基于雙目視覺的位姿測量示意圖

對于任意特征點Pi，i=1,…,N，其在相機左右相平面上的投影像素坐標分別為(uli,vli)和(uri,vri)。根據雙目相機的三角測量原理，可以恢復相機坐標系{C}下特征點的坐標ρi，公式如下：

(1)

需要指出，一方面由于特征點直接利用圖像處理算法獲得，在分布上具有一定的任意性。另一方面，考慮到目標的非合作特性，特征點相對于目標的相對方位未知。因此有必要利用所獲得的特征點，建立相應的幾何坐標系，作為描述目標位置與姿態的基準[19]。假定已經獲得了三個特征點P1，P2，P3，其在相機坐標系下的坐標根據式(1)分別為記為ρ1,ρ2,ρ3，則可以根據式(2)得到一組滿足右手定則的單位正交向量c1,c2,c3。

(2)

以特征點P1為原點，單位正交向量c1,c2,c3為基矢量，可建立與目標相固連的幾何坐標系{T}。{T}與相機坐標系{C}之間的方向余弦矩陣RTC的各列由基矢量的坐標組成，即

(3)

1.3 姿態測量濾波器

利用以上幾何坐標系的建立方法，僅能獲得離散時刻的幾何坐標系的測量值，并且容易受單次測量噪聲的影響。通過姿態測量濾波器，不僅可以獲得受噪聲干擾較小的連續估計值，并且能夠進一步估計角速度和慣量等姿態運動參數。

假定目標為在空間中自由運動的剛體，當用四元數qTI描述其相對于慣性坐標系的姿態時，滿足如下的姿態運動學方程。

(4)

式中

(5)

同時，假定目標姿態不受主動力矩控制，處于自由翻滾狀態，則其角速度滿足如下的歐拉方程。

(6)

式中:ε是環境力矩引起的噪聲，可建模為零均值的高斯白噪聲。I矩陣是目標慣性張量在幾何坐標系下的展開形式。由于幾何坐標系的建立具有任意性，不失一般性，可以寫為如下包含慣量積的形式。

(7)

(8)

將式(8)代入(6)并進行整理，可得

(9)

l=[Iyy/Ixx,Izz/Ixx,Ixy/Ixx,Ixz/Ixx,Iyz/Ixx]T

(10)

作為表征慣量的參數，并滿足如下公式

(11)

根據上述分析，可以建立起姿態測量濾波器的結構：被估計狀態為幾何坐標系的姿態，角速度以及慣量比矢量，其動力學方程分別滿足式(4)(9)(11)。

(12)

濾波器的測量為幾何坐標系相對于慣性系的姿態四元數，可以按下式計算得到：

yr=qTI=qTC?qCI

(13)

式中:qTC為式(3)所求方向余弦陣對應的姿態四元數，是構建幾何坐標系{T}的直接結果。qCI為相機坐標系相對于慣性系的姿態四元數，可由服務航天器自身的姿態結合相機的安裝方位獲得。

1.4 相對導航濾波器

在相對導航的過程當中，為了確定目標的軌道，需要描述目標質心相對于服務航天器的運動規律?，F有文獻，對于目標進行相對運動的描述，多基于軌道坐標系下的相對位置，速度來描述，如CW方程，TH方程等等。上述方程在推導的過程中均進行了近似，并且對軌道的構型具有一定的要求。

由于慣性參數辨識需要對目標質心進行精確的估計，因從本文采用軌道根數的建模方法，一方面形式簡單精度較高，另一方面適用范圍更廣。考慮到近距離相對導航的過程中觀測時間較短，因此不考慮軌道攝動的影響。此時，二者的軌道根數具有以下簡單的演化形式。

(14)

式中:μ是引力常數。a是軌道半長軸，e是軌道偏心率，i是軌道傾角，Ω是升交點赤經，ω是近地點幅角，M是平近點角。

本文采用如下形式的相對軌道根數δx?表征目標與服務航天器的相對位置

(15)

式中:u=M+ω是平緯度幅度。下標t/s表示該軌道根數分別屬于服務航天器/目標。

考慮到服務航天器自身的軌道根數?s已知，相對軌道根數的演化過程可寫為如下形式的非線性方程。

(16)

由于幾何坐標系選取的隨機性，不失一般性，可將目標質心位置在幾何坐標系{T}下的坐標為r為待估計的參數，并滿足

(17)

在此基礎上，在相機坐標系{C}下幾何坐標系{T}的原點位置可表示為，

p=RCLρ+RCTr-d

(18)

式中:d是相機安裝位置相對于服務航天器質心的偏置，RCL是軌道坐標系{L}相對于{C}的方向余弦陣，二者均可視為已知。ρ是目標質心相對于服務航天器質心在軌道坐標系{L}下的表示，其與相對軌道根數的關系可利用軌道根數與慣性系下位置和速度的對應關系獲得，并記為如下非線性函數形式

ρ=g(δx?(t))

(19)

根據上述分析，可以建立起相對導航濾波器的結構：待估計狀態包括相對軌道根數和質心偏置，其動力學方程分別為式(16)(17)，

(20)

(21)

式中:符號*表示四元數求逆。R(q)為四元數q對應的方向余弦矩陣，按以下公式計算：

(22)

式中:q0和qv=[q1,q2,q3]T分別為四元數的標量與矢量部分，I3為單位矩陣。

整個相對導航算法流程如下：

圖2 相對導航流程圖

2 基于相對導航的慣性參數辨識方法

黏附衛星是隨著微重力環境技術發展而出現的具有黏附能力的新型飛行器，在與目標接觸后可黏附在其表面形成一個整體?；谙鄬Ш降膽T性參數辨識方法，需要已方的航天器去黏附非合作目標。利用相對導航對于質心和慣量比的可測性，可以實現非合作目標質量和轉動慣量的估計。

圖3 黏附后組合體質心變化示意圖

2.1 基于視覺導航的組合體質心估計方法

假定黏附后形成的組合體為剛體。因此，可以利用前面提出的相對導航方法，對組合體的姿態，角速度，慣量比以及質心進行估計。

2.2 基于質心變化的質量辨識方法

黏附之后，由于組合體的質量分布發生變化，因此其質心的位置也相應改變。由于黏附衛星為已知的合作目標，可以利用表面安裝的信標結合雙目視覺得到黏附衛星質心B，進而計算其在幾何坐標系下的分量rOB。同時，根據相對導航的估計結果，非合作目標的質心A與黏附后組合體的質心C在幾何坐標系下的坐標分別為rOA和rOC?；趧傮w假設，組合體的質心理論上應當在黏附衛星與非合作目標各自質心的連心線上，且相對于二者質心的距離和各自質量成反比。因此，非合作目標質量可按以下公式進行辨識

(23)

2.3 根據平行軸定理的轉動慣量辨識方法

如前文所述，當黏附之后，組合體質心位置發生了改變。根據平行軸定理，組合體關于質心的轉動慣量滿足如下等式，

(24)

式中：IA和IB分別為非合作目標和黏附衛星對于各自質心的轉動慣量矩陣。將與非合作目標質量分布無關的量定義為

(25)

則式(25)可以記為

(26)

(27)

進而可以得到如下Ixx辨識公式

(28)

3 仿真驗證

本節對所提出的相對導航和慣性參數辨識方法分別進行驗證，以相對導航的精度作為慣性參數辨識的測量精度。

1)相對導航方法驗證

相對導航算法相關參數設定如下：服務航天器的初始軌道根數a=7080 km,e=0.001,i=98.2°,Ω=189.9°,ω=0°,M=0°。姿態測量濾波器的初始方差，過程噪聲和觀測噪聲分別Pr=0.1×I12，Qr=10-10×I12，Rr=10-4×I4。相對導航濾波器的對應參數分別為Pr=0.1×I9，Qr=10-10×I9，Rr=10-4×I3。以上各式中In表示n階單位矩陣。各變量真實與濾波初始值如表1所示。仿真時間1000 s,測量頻率1 Hz。為提高數值穩定性，采用平方根形式的容積卡爾曼濾波算法。

表1 濾波初值設定

相對導航仿真結果如圖4所示。其中，四元數的估計誤差取誤差四元數的矢量部分。對稱分布的虛線為估計誤差的協方差值(3σ)。由于數據類似，四元數，角速度和質心位置只提供單軸估計結果，慣量比提供Iyy/Ixx的估計結果，相對軌道根數提供aδa的估計結果。

圖4 相對導航估計誤差

根據仿真結果可見，大概200 s左右，相對導航系統的被估計參數均可以收斂到真值左右。誤差基本在估計誤差協方差以內，說明濾波算法的一致性較好。為了進一步分析導航精度，進行100次蒙特卡洛仿真，得到的估計精度(1σ)分別為：四元數單軸精度0.002，角速度單軸精度0.03(°)·s-1，質心位置單軸精度0.002 m，慣量比精度0.002, 相對軌道根數精度0.02 m。以上結果表明，所提出的基于雙目相機的導航方法可以實現目標姿態，角速度，慣量比，軌道，質心位置等的有效估計。其中，質心位置和慣量比的高精度估計，是后續實現目標質量和轉動慣量辨識的前提。

2)慣性參數辨識方法仿真驗證

在實現相對導航的基礎上，本節驗證所提出的慣性參數辨識方法。為了簡化問題，假定黏附衛星的質量全部集中于質心?；谏鲜黾僭O，結合根據式(24)可知，當非合作目標的質量與慣量分布確定之后，相應的辨識結果主要取決于所使用的黏附衛星的質量以及形成組合體后黏附衛星與非合作目標質心的距離。因此，有必要研究以上因素對于辨識精度的影響。

令非合作目標質量為M=1000 kg，x軸的轉動慣量為1000 kg·m2。據上文中相對導航算法的仿真結果，設定慣量比和質心位置的測量精度分別為0.002和0.002 m/s。圖5和圖6分別為通過1000次仿真得到的不同的黏附衛星質量和質心間距下非合作目標質量與轉動慣量辨識精度。其中，黏附衛星的質量變化范圍從目標質量的0.1倍到10倍，質心間距的變化范圍從0.1 m到10 m。慣量比和質量的相對誤差精度分別按Δk/k和ΔM/M計算。

圖5 黏附衛星質量對于辨識精度的影響

圖6 質心間距對于辨識精度的影響

仿真結果表明，從變化趨勢上看：對于轉動慣量的辨識而言，當黏附衛星質量以及質心間距較小時，辨識誤差較大。而當上述兩個變量逐漸增加，辨識精度迅速提高。而隨著質量和間距的進一步增加，辨識精度又緩慢下降。這是因為本文所提出的對于慣性張量的辨識方法是建立在黏附前后的慣量比的變化之上。當黏附衛星的質量或黏附后的質心間距較小時，慣量比的變化較??；而隨著上述參數的進一步增加，根據式(24)，組合體的轉動慣量主要由質心變化后各自質量產生的慣量組成，目標自身的轉動慣量所占的比重減小，對于組合體慣量比的貢獻減小。而以上兩種情況均會降低目標的辨識精度。對于目標質量的辨識而言，隨著質心間距的增加，辨識精度逐漸增加。這是因為對于目標質量的辨識依賴于黏附前后質心位置的變化。而當質心間距越大，質心位置的變化越明顯，因此精度會提高。而對于黏附衛星質量的增加，質量辨識精度呈現先增加，后減小的趨勢，并且當二者質量相等時辨識精度最高。辨識精度方面：當黏附衛星的質量與目標質量的比值大于一定值以后，目標質量與慣量的辨識精度所受的影響較小。當質心間距1 m時，轉動慣量相對誤差2%以內，目標質量相對誤差可以達到1%以內。

仿真結果說明，本文提出的質量與慣性參數辨識方法精度較高，并且對于黏附衛星質量，以及黏附點位置的魯棒性較好，適合工程應用。

4 結 論

本文針對在軌服務任務中對于非合作目標的感知測量問題。

1)提出了基于雙目視覺的非合作目標相對導航方法。該方法利用目標表面的特征點構建幾何坐標系，并通過姿態測量濾波器與相對導航濾波其實現目標的姿態，角速度、慣量比、質心位置以及軌道參數的估計。所提出的算法通用性好，建模精度高。仿真結果表明，該算法對各參數估計精度較高。

2)進一步提出了基于相對導航的慣性參數辨識方法。該算法通過黏附衛星對非合作目標進行黏附，并利用黏附前后相對導航估計結果的變化實現對于目標質量和轉動慣量的辨識。仿真結果表明，所提出的辨識算法辨識精度較高，并且對黏附點以及黏附衛星質量的魯棒性較好。

上述研究成果對于空間在軌服務技術的研究具有一定的參考價值。