基于馬達代數的交會對接相對位姿測量算法

陳偉 陳志明 王惠南

(南京航空航天大學航天學院，南京 210016)

?

基于馬達代數的交會對接相對位姿測量算法

陳偉 陳志明 王惠南

(南京航空航天大學航天學院，南京 210016)

精確測量追蹤航天器與目標航天器之間的相對位姿關系是成功完成航天器交會對接任務的關鍵。傳統的位姿測量算法將旋轉和平移分而視之，破壞了三維運動的統一性，同時增加了算法的復雜性和計算難度。針對這個問題，采用雙目視覺測量方法，在馬達代數框架內，以兩個特征光點確定的特征直線為變換基元，統一描述并設計算法測量追蹤與目標航天器之間的位姿關系，最終將兩航天器之間的相對位姿解算問題簡化為求解兩個線性方程組。該方法在計算形式上更為簡潔，且測量受特征光點的安裝位置限制較小。仿真結果表明，算法具有較高的精度和穩定性，可以滿足航天器交會對接任務的要求。

馬達代數；交會對接；相對位姿；雙目視覺；航天器

1 引言

在航天器交會對接的最終逼近段，追蹤航天器必須自主精確測定與目標航天器的相對姿態和相對位移，并通過控制使其相對狀態滿足最終逼近段準直線運動、逼近速度、安全性及軟對接等多項技術要求[1-2]。目前普遍的測量方法是在目標航天器上設置光學特征點，利用特征點在CCD相機視覺系統中的成像位置來確定相對位姿，該方法具有體積小、質量小、功耗低、可靠性高等優點。傳統單目視覺算法通過迭代法來求解二次非線性方程[3-4]，測量結果受收斂性和求解速度影響較大。而文獻[5]和文獻[6]分別采用雙目視覺和三目視覺算法，測量基本方程為線性方程組，存在解析解，大大提高了求解位姿的速度和精度以及系統的可靠性。另外，大多數位姿測量算法中均采用基于向量代數的描述方法，通常將剛體運動拆分為平移和旋轉兩部分，需要分別設計算法來計算兩部分操作結果，不僅破壞了運動學的完整性，也增加了算法的復雜度。而幾何代數中的馬達代數(Motor Algebra)可以很好地解決這個問題。馬達代數是對偶四元數的擴充，它將Grassmann擴張代數、Hamilton四元數和復數等統一起來，避免了計算時各種代數語言之間的轉換，可以直觀地表示幾何體的空間運動[7]。因此，馬達代數在涉及運動目標位姿處理的相關問題中得到了越來越廣泛的應用[7-9]。文獻[10]提出了一種基于普呂克直線的交會對接相對位姿確定算法，并通過奇異值分解的方法統一解算出相對位姿值，求解過程復雜，同時對于特征線的安裝也要求較高，而且普呂克直線方程的確定仍然需要識別該直線上兩個特征點，實際操作難度較大。文獻[11]利用對偶四元數描述坐標系間的相對位姿，并設計算法解算出位置和姿態信息，但計算中涉及復雜的矩陣乘法運算，較為繁瑣。

基于以上幾種方法的研究，本文采用馬達代數這一新型數學工具，以兩個特征光點確立一條線實體并作為變換基元，統一描述追蹤航天器和目標航天器之間的相對位姿，通過馬達代數實部和對偶的求解來完成位姿解算。該方法比傳統的歐拉角和四元數方法更為簡潔有效，且精度較高，具有一定的計算優勢和可研究性。

2 測量原理

2.1 坐標系

圖1 各坐標系的定義 Fig.1 Definition of all coordinate systems

本文通過在目標航天器上設置特征光點，并使用雙目視覺測量的方法進行位姿測量。為了便于分析計算，建立的坐標系如圖1所示。

1)目標航天器坐標系O′(x,y,z)，簡稱O′系。該坐標系根據特征光點的位置來定義。設置4個位于x-y平面內且構成正方形的特征光點A、B、C、D，坐標系原點可設置為正方形的中點，x軸沿O′C方向,y軸沿O′D方向，z軸按右手法則確定。

2)追蹤航天器坐標系O(X,Y,Z)，簡稱O系。該坐標系定義在追蹤航天器上，其原點定義為追蹤航天器的質心，X軸、Y軸、Z軸由追蹤器的形狀來規定。

4)攝像機坐標系C1(XC1,YC1,ZC1)和C2(XC2,YC2,ZC2)，簡稱C1系和C2系。攝像機C1和C2與追蹤航天器固連，因此兩坐標系相對于O系的位姿是已知的。原點C1、C2位于攝像機光心，XC軸和YC軸分別與像素坐標系中u軸和v軸平行，ZC軸沿主光軸方向并垂直于圖像平面，光心與圖像中心點之間的距離即為攝像機的焦距。

2.2 雙目視覺測量原理

雙目視覺測量原理如圖2所示[12]。追蹤坐標系與兩攝像機坐標系固連。測量目的是通過空間中一點P在兩攝像機圖像平面中成像點PC1和PC2的像素坐標，推算出該點在攝像機坐標系和追蹤坐標系中的坐標值。

圖2 雙目視覺測量原理 Fig.2 Principle of binocular vision measurement

P點在追蹤坐標系中的坐標為(X,Y,Z)，PC1和PC2的像素坐標分別為(u1,v1)和(u2,v2)。已知兩個攝像機的投影矩陣分別為M1和M2，其可由相機的內、外參數決定，不妨假設：

(X,Y,Z)的值可采用文獻[5-6]中的最小二乘法來獲得，該方法可以避免測量噪聲和匹配誤差的影響。具體計算形式為

3 位姿解算

若已知特征光點分別在追蹤坐標系和目標坐標系中的坐標值，便可以解算出兩坐標系間的相對位姿。在傳統的基于向量代數的位姿解算方法中，需要求出目標坐標系相對于追蹤坐標系的正交余弦矩陣(即旋轉矩陣)R3×3和位移向量t3×1。

假設空間一點在目標坐標系和追蹤坐標系中的坐標分別為(x,y,z)和(X,Y,Z)，且目標坐標系原點在追蹤坐標系中坐標為(tx,ty,tz)，則有

對于方程(2)中R3×3和t3×1的求解，目前有兩種方法——解析法和正交法。解析法的主要思想是，分別將4個特征點在目標和追蹤坐標系中的坐標代入公式(2)中，得到4組不同的R3×3和t3×1值，再求其平均值。這種方法不僅誤差較大，而且不能滿足所得旋轉矩陣R3×3的正交性。為了解決這個問題，文獻[5]提出了正交法，即在4個特征點中任選3個點(A、B、C)構造3個線性無關的向量，如向量dAB，dAC和它們的向量積dAB×dAC，并通過目標和追蹤坐標系中向量坐標的對應關系求解旋轉矩陣R3×3，最后再對4種情況求取均值。該方法顯然可以滿足旋轉矩陣的正交性，但是在相對位置的求解過程中必須假定4個特征點是對稱的，且目標坐標系原點位于它們的中心，要求相對嚴格。下面介紹一種新的基于馬達代數的求解相對位姿的算法。

馬達代數空間可由如下所示的基張成：

限于篇幅，作者對于馬達代數的具體運算法不再贅述，讀者可以參閱文獻[7]。

LAB=nAB+ImAB= (xB1-xA1)e2e3+(xB2-xA2)e3e1+(xB3-xA3)e1e2+

(5)

式中n和n′表示直線AB的方向向量；m和m′為點A和B的外積。

追蹤航天器相對于目標航天器的旋量R和平移量t可表示為：

則兩坐標系間的相對位姿可統一用如下馬達代數描述：

并且有

根據式(9)中直線的運動變化方程，可以列出

式中n和m已知，n′和m′由視覺測量計算得到。不妨假設：

將式(6)、(11)和(12)代入式(10)中第一式，根據馬達代數運算法則和等式兩邊實部、對偶部分別相等的原則，可以寫出一組關于未知數r0、r1、r2、r3的線性方程組，用矩陣乘法的形式可表示為

求得旋量R后便可以進一步得到旋轉運動所對應的旋轉軸和旋轉角度。旋量R中r0、r1、r2、r3與旋轉軸方向向量nR以及旋轉角度θR的對應關系為：

接下來求取目標航天器和追蹤航天器之間的相對位移t。將式(10)中第1式代入第2式中可得

式(15)右邊值可由計算得知，不妨假設

將式(7)、(12)和(16)代入式(15)，同理根據等式兩邊實部和對偶部分別相等的原則，可得到關于未知數t1、t2、t3的方程組：

同樣地，對于3條直線AB、BC、CD分別可以寫出矩陣Pi、Qi(i=1,2,3)，令P=[P1P2P3]T,Q=[Q1Q2Q3]T,則有

式中P為9×3的矩陣；Q為9×1的矩陣。采用最小二乘法求解該方程組，得：

至此，追蹤航天器與目標航天器之間的相對位姿M的實部和對偶部便求解完畢。

由上述算法可知，基于馬達代數的位姿解算算法將相對位姿中的旋量和平移量有效地統一起來，并將問題簡化為兩個線性方程組的求解。同時也可以看出，該算法以特征直線為變換基元，既可以滿足所得姿態矩陣的正交性，同時對特征點位置是否對稱等也并無要求。

4 數值仿真

本節使用Matlab進行數值仿真。已知攝像機鏡頭焦距為35mm，最高分辨率為4 256×2 832，傳感器尺寸為36.0mm×23.9mm，則根據上述參數可以得到攝像機的內部參數矩陣為

圖3顯示追蹤航天器相對于目標航天器的位姿測量誤差均隨像素坐標測量誤差的增大而增大，與實際情況相符。在像素誤差為1像素時，馬達代數算法中俯仰角、偏航角、滾動角以及相對位置的測量絕對誤差分別為0.09°、0.45°、0.07°和0.003 5m。

圖3 情況a仿真結果Fig.3 Simulation under scenario a

圖4 情況b仿真結果Fig.4 Simulation under scenario b

結合兩組仿真結果，可知基于馬達代數的位姿測量方法的測量誤差在航天器交會對接測量精度要求范圍之內，因此本文中所提的位姿測量算法是有效的。另外，在同一噪聲水平下，馬達代數算法相對于向量代數方法具有更高的測量精度，歐拉角測量誤差減少了50%左右,且該測量誤差受像素誤差的影響較小，結果更為穩定。

5 結束語

本文使用馬達代數這一新型數學工具，描述了交會對接任務中目標航天器與追蹤航天器之間的相對位姿，相對于傳統的向量代數描述方法，計算更為簡潔。另外，在目標航天器上設置特征光點，并采用雙目視覺測量方法，設計并驗證了基于馬達代數的位姿參數解算算法，最后與正交法的測量誤差進行比較。仿真結果表明該方法有效，3個方向上歐拉角的測量誤差均在0.5%以內，能夠滿足航天器對接任務的測量精度要求。同時，該馬達代數算法具有更高的精度和穩定性。

[1] 朱仁璋,尹艷. 論空間交會最終平移段制導設計[J]. 中國空間科學技術,2004,24(5):1-8.

ZHU RENZHANG,YIN YAN.Guidance Strategy design for the final translation of space rendezvous[J].Chinese Space Science and Technology,2014,24(5):1-8.

[2] 朱仁璋,湯溢,尹艷.空間交會最終平移軌跡安全模式設計[J]. 宇航學報,2004,25(4):443-448.

ZHU RENZHANG,YANG YI,YIN YAN.Safety mode design of final translation trajectories of space rendezvous[J].Journal of Astronautics,2004,25(4):443-448.

[3] JOHN L J, DECLAR C H, KARIM P W,et al. Vision-based navigation for rendezvous, docking and proximity operations[C]. 4th International Conference on Dynamics and Control of Space Structure，Cranfield University,May 24-28,1999.

[4] 林來興,李燦. 交會對接最后逼近階段 CCD 相機的測量方法[J]. 宇航學報, 1994,15(2):24-34.

LIN LAIXING,LI CAN.Method of measurement in the stage of final approaching for rendezvous and docking in space[J].Journal of Astronautics,1994,15(2):24-34.

[5] 張慶君,胡修林,葉斌,等.基于雙目視覺測量的航天器相對位置和姿態的測量方法[J]. 宇航學報, 2008,29(1):156-161 .

ZHANG QINGJUN,HU XIULIN,YE BIN,et al.Binocular vision-based relative position and attitude determination between spacecrafts[J].Journal of Astronautics,2008,29(1):156-161.

[6] 錢萍,王惠南. 基于三目視覺測量的航天器交會對接相對位姿確定算法[J]. 宇航學報, 2010,31(6):1575-1581.

QIAN PING,WANG HUINAN.A trinocular vision measurement based relative attitude and position determination algorithm for RVD between spacecrafts[J].Journal of Astronautics,2010,31(6):1575-1581.

[7] EDUARDO BAYRO-CORROCHANO, KOSTAS DANIILIDIS, GERALD SOMMER. Motor algebra for 3D kinematics: the case of the hand-eye calibration [J]. Journal of Mathematical Imaging and Vision,2000,13(2):79-100.

[8] 袁建平,李克昭,岳曉奎. 基于馬達代數的航天器視覺相對導航算法研究[OL].[2006-12-25].http:∥www.paper.edu.cn/release-paper/content/200612-371.

[9] 張廣軍.機器視覺[M].北京:科學出版社,2005:14-32.

ZHANG GUANGJUN.Machine vision[M].Beijing:Science Press of China,2005:14-32.

[10] 李靜,王惠南,劉海穎.基于普呂克直線的交會對接相對位姿確定算法[J]. 中國空間科學技術,2013,33(1):69-74.

LI JING,WANG HUINAN,LIU HAIYING.Plucker linear based relative position and attitude determination for RVD[J].Chinese Space Science and Technology ,2013,33(1):69-74.

[11] 錢萍,王惠南. 基于對偶四元數的航天器交會對接位姿雙目視覺測量算法[J].宇航學報, 2013,34(1):32-38.

QIAN PING,WANGHUINAN.A binocular vision algorithm based on dual quaternion for relative position and attitude determination of RVD spacecrafts[J].Journal of Astronautics,2013,34(1):32-38.

陳 偉 1992年生，2013年畢業于南京航空航天大學通信工程專業，現為南京航空航天大學導航、制導與控制專業碩士研究生。研究方向為交會對接視覺導航。

(編輯：高珍)

Measurement Algorithm Based on Motor Algebra for Relative Position and Attitude Determination of RVD Spacecraft

CHEN Wei CHEN Zhiming WANG Huinan

(College of Astronautics, Nanjing University of Aeronautics and Astronautics, Nanjing 210016)

Accurate measurement of the relative position and attitude between the active spacecraft and the target spacecraft is the key to the successful completion of rendezvous and docking (RVD) mission. In the traditional vector measurement methods, the position and attitude are usually treated separately, which led to the difficulty of algorithm design and computational complexity.A new binocular vision measurement algorithm based on motor algebra was proposed to uniformly describe the relative position and the attitude of the two RVD spacecraft. This new algorithm has a more concise form and some computational advantages compared to the traditional methods. The relative position and attitude determination problem was simplified to solving two linear equations. Simulation results show that the algorithm can meet the measurement accuracy requirements of RVD missions.

Motor algebra; Rendezvous and docking; Relative position and attitude; Binocular vision;Spacecraft

2014-12-31。收修改稿日期：2015-03-02

10.3780/j.issn.1000-758X.2015.04.011