一種采用雙目視覺加慣性測量的航天器組合相對導航方法*

趙逸倫 喬 兵 靳永強 郁 豐

1.南京航空航天大學，南京210000 2.上海宇航系統(tǒng)工程研究所，上海201100

?

一種采用雙目視覺加慣性測量的航天器組合相對導航方法*

趙逸倫1喬 兵1靳永強2郁 豐1

1.南京航空航天大學，南京210000 2.上海宇航系統(tǒng)工程研究所，上海201100

以對偶四元數(shù)為數(shù)學工具，在機器視覺和慣性導航的基礎(chǔ)上提出了一種新的航天器相對導航算法。介紹了對偶四元數(shù)的空間位姿模型，并基于共線投影原理建立了雙目視覺的測量模型，為系統(tǒng)提供觀測數(shù)據(jù)。根據(jù)航天器運動學模型推導其狀態(tài)方程，并在此基礎(chǔ)上設(shè)計了擴展卡爾曼濾波器。通過計算機仿真驗證了該相對導航算法的有效性。仿真結(jié)果表明該方法對航天器的相對位置、姿態(tài)和速度均具有良好的估計精度，可以滿足交會對接、航天器捕獲等任務的精度需求。 關(guān)鍵詞 雙目視覺；組合相對導航；對偶四元數(shù)；卡爾曼濾波；航天器

兩航天器間的相對位姿問題本質(zhì)上可以歸結(jié)為兩不共點坐標系之間的螺旋變換問題[1]。目前用于描述兩坐標系位姿的方法有很多，常見的有歐拉角，方向余弦矩陣以及哈密頓四元數(shù)。方向余弦矩陣的幾何意義明確，但因其多達6個約束條件而影響其廣泛使用[1]。歐拉角的物理意義比較直觀，但運動學方程復雜不便于計算。四元數(shù)的引入，雖然彌補了旋轉(zhuǎn)矩陣在計算時的溢出缺陷，但是仍將坐標系的旋轉(zhuǎn)與平移(或者說是坐標系間的相對位置與姿態(tài))分開處理。這種辦法一方面會影響相對位置和姿態(tài)求解的精度，另一方面在位姿測控中，增加了系統(tǒng)的復雜性。

Chales定理表明[2]：任何剛體空間運動都可通過先繞某根軸旋轉(zhuǎn)再沿同一根軸平移的螺旋運動來實現(xiàn)。基于Chales定理把旋轉(zhuǎn)和平移統(tǒng)一考慮，產(chǎn)生了對偶四元數(shù)的概念。對偶四元數(shù)繼承了四元數(shù)的很多性質(zhì)和優(yōu)點，能避免在大角度旋轉(zhuǎn)時運算中的奇異性和姿態(tài)矩陣的正交性問題[3]。在計算機視覺[4]、機器人[5]及捷聯(lián)慣性導航[6]等領(lǐng)域應用中顯示出了其數(shù)學表達式直觀、明了和計算效率高等優(yōu)勢。可見，對偶四元數(shù)是一種適用于航天器相對導航任務模型建立的數(shù)學工具。

在航天器交會對接、空間目標跟蹤和在軌服務等多項空間任務中，通常采用光學敏感器作為觀測手段，來測量目標航天器與追蹤航天器間的相對位置及姿態(tài)[7]。一般的做法是在目標航天器上設(shè)置若干個光學特征點，由安裝在追蹤航天器上的CCD相機識別，并匹配這些光學特征點，根據(jù)成像信息解算相對位置及姿態(tài)。

但是對于傳統(tǒng)的單目視覺測量方法，其測量方程為非線性方程，需要迭代求解[8]，不僅需要較為精確的初始值，而且很可能會產(chǎn)生多值結(jié)果，對求解的速度精度都造成很大影響。此外，單一的光學導航方法亦存在一些缺點和不足：1)由于測量過程中需要對光學特征點進行檢測和匹配，導致測量結(jié)果輸出不夠密集，更新速度較慢；2)光學敏感器只能輸出單一的位置和姿態(tài)信息，對速度、角速度只能在位置和姿態(tài)測量結(jié)果的基礎(chǔ)上做差分得到粗略估計；3)在近距離相對導航中光學測量單元可能會由于特征點遮擋出現(xiàn)輸出中斷的情況。與光學導航相比，慣性導航存在以下優(yōu)勢[4]： 1)不依賴于任何外界信息，可全天候工作； 2)能連續(xù)輸出載體的加速度、角速度，輸出的導航信息有很好的連續(xù)性； 3)數(shù)據(jù)更新率高，短期精度和穩(wěn)定性較好。不過慣性元件由于存在初始誤差、漂移和噪聲等原因，長時間導航會產(chǎn)生較大累積誤差。鑒于單目視覺測量和慣性導航存在的固有缺陷，本文提出一種雙目攝像機加慣性原件的組合相對導航方法，可有效測量目標星和追蹤星間的相對姿態(tài)，高精度的估計出追蹤星的各類運動學參數(shù)，為多航天器協(xié)同任務的完成提供保障。

1 對偶四元數(shù)空間變換模型

1.1 對偶四元數(shù)

對偶數(shù)的概念由英國數(shù)學家Clifford于1873年首次提出[9]，對偶四元數(shù)是實部和對偶部均為四元數(shù)的對偶數(shù)。限于篇幅，詳情見文獻[9]。

如果把空間三維向量看作實部為0的四元數(shù)，則對偶四元數(shù)可用于表示空間直線：設(shè)空間直線L的方向向量為l，關(guān)于參考坐標系原點的矩為m，則L可表示為

(1)

1.2 基于對偶四元數(shù)的空間位姿變換

限于篇幅，下面不加證明的給出基于四元數(shù)和對偶四元數(shù)的位姿變換模型。

若坐標系A(chǔ)繞n軸旋轉(zhuǎn)θ角后與坐標系B重合，定義從A系至B系的轉(zhuǎn)換四元數(shù)：

(2)

設(shè)向量a在A系和B系下的坐標分別為aA和aB，有：

aB=q*·aA·q

(3)

若坐標系A(chǔ)繞n軸旋轉(zhuǎn)θ角，再平移t后與坐標系B重合，定義從A系至B系的轉(zhuǎn)換對偶四元數(shù)：

(4)

其中，q的定義同式(2)，tA，tB分別表示平移向量t在A系和B系下的坐標。

(5)

對比式(3)和(5)，可以看到四元數(shù)坐標變換與對偶四元數(shù)坐標變換有完全相同的形式。事實上，四元數(shù)的大部分性質(zhì)都被對偶四元數(shù)完全繼承了。

2 雙目視覺和慣性元件測量模型

2.1 視覺測量的坐標系定義

為便于分析建立如圖1所示的像平面坐標系、相機坐標系和目標航天器坐標系，并分別定義如下：

1)像平面坐標系：原點O1位于像平面中心，即攝像機光軸與成像平面的交點，x軸和y軸分別與圖像平面的兩鄰邊平行，記為O1系；

2)攝像機坐標系：原點為攝像機光心Oc，Xc軸和Yc軸分別與圖像坐標系的x軸和y軸平行，Zc軸為攝像機的光軸，與圖像平面垂直，記為Oc系；

3)目標航天器本體坐標系：原點Oa位于目標航天器質(zhì)心，x，y，z軸與航天器的3個慣量主軸重合，記為Oa系。

圖1 坐標系定義示意圖

2.2 雙目視覺測量模型

所謂雙目視覺，指的是同時用2臺攝像機對同一目標拍攝2幅圖像以確定物體空間方位的方法[10]。圖2描繪了平行雙目視覺成像系統(tǒng)的基本原理。

圖2 平行雙目視覺成像原理

左右攝像機的光心分別為Oc和Or，兩光軸平行且分別交成像平面于O1和O2點。兩攝像機的Z軸分別與各自的光軸重合，X軸則位于同一條直線上。左右攝像機光心的連線OcOr稱為基線，定義線段OcOr的長度為基線距B。設(shè)P點在左右攝像機成像平面上投影的坐標分別為p1(x1,y1)，p2(x2,y2)，P點在左右攝像機坐標下的位置分別是(Xc,Yc,Zc)，(Xr,Yr,Zr)，由平行雙目視覺系統(tǒng)的結(jié)構(gòu)易知：Xc=Xr+B，Yc=Yr，Zc=Zr，y1=y2。再由投影原理可得如下關(guān)系式：

(6)

由此可算出P點在左攝像機下的坐標：

(7)

2.3 相對位姿參數(shù)的解算

(8)

將式(8)展開，可得：

(9)

即：

(10)

對式(10)等號兩邊同時左乘q并整理得到：

(11)

根據(jù)四元數(shù)乘法法則，將式(11)中第1式寫為矩陣乘法的形式：

(12)

(13)

將式(13)帶入式(11)中第2式，并寫成矩陣乘法的形式，得：

(14)

解該非齊次線性方程組即得平移向量t。

2.4 慣性元件的測量模型

慣性測量元件中的加速度計和陀螺可分別測量航天器相對于慣性空間的加速度和角速度。對于捷聯(lián)式慣性系統(tǒng)，輸出的是載體相對于慣性系的非引力加速度和角速度在載體坐標系上的投影。在忽略安裝誤差和標定誤差的情況下，加速度計的測量輸出包括非引力加速度的真實值、加速度計漂移和量測噪聲3個部分[11]，即：

am=a+ba+ηa

(16)

與加速度計類似，陀螺的輸出包括真實角速度、陀螺漂移和測量白噪聲3個部分，即

ωm=ω+bω+ηω

(17)

式中，ηω和bω的定義同ηa和ba類似，不再贅述。

3 系統(tǒng)動態(tài)模型建立

(18)

其中，A(q)代表與q對應的方向余弦矩陣，ωa為目標航天器相對慣性系的角速度在Oa系下的坐標，M,N是基于C-W方程得到的修正項，分別定義為：

G(t)，w(t)分別為噪聲驅(qū)動矩陣和噪聲項。狀態(tài)轉(zhuǎn)移矩陣：

Φk/k-1=eFk/k-1·Δt

(19)

其中：

(20)

導航測量方程：

(21)

根據(jù)文獻[1]，雙目攝像機對位置的估計精度與距離為負相關(guān)的關(guān)系，即距離越近對距離的測量精度越高；而對姿態(tài)的測量精度與距離無關(guān)。根據(jù)雙目攝像機的測量特點，可對測量噪聲的方差做合理近似。

4 仿真驗證和分析

以兩航天器交會對接的逼近段為任務背景進行仿真驗證，下面以表格形式給出仿真所需的各類參數(shù)。

表1 仿真參數(shù)

圖3 相對位置估計誤差

圖4 四元數(shù)估計誤差

圖5 四元數(shù)測量誤差

圖6 速度估計誤差

圖7 角速度估計誤差

5 總結(jié)與展望

提出了一種雙目視覺加慣性元件的航天器組合相對導航方法，并在此基礎(chǔ)上設(shè)計了卡爾曼濾波器。首先介紹了基于對偶四元數(shù)的空間變換方法，然后以此為數(shù)學工具給出了雙目視覺條件下測量航天相對位姿的方法。最后以航天器交會對接逼近段為任務背景建立了系統(tǒng)的動態(tài)模型并進行了計算機仿真。仿真結(jié)果表明，該方法很好地發(fā)揮了視覺導航和慣性導航各自的優(yōu)點而相互彌補了各自的不足，能夠?qū)崟r準確地估計航天器相對運動中的各類參數(shù)。該方法可為航天器捕獲、航天器在軌維修等近地空間任務提供技術(shù)支持。

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

[2] 武元新.對偶四元數(shù)導航算法與非線性高斯濾波研究[D].長沙：國防科學技術(shù)大學，2005.(WuYuanxin.ResearehonDualQuaternionNavigationAlgorithmandNonlinearGaussianFiltering[D].Changsha:NationalUniversityofDeefnseTechnolog,2005.)

[3] 崔本杰.基于對偶四元數(shù)的航天器相對導航方法研究[D].哈爾濱：哈爾濱工業(yè)大學，2009.(CuiBenjie.StudyOnRelativeNavigationMethodForSpacecraftBasedOnDualQuaternion[D].Harbin:HarbinInstituteofTechnology. 2009.)

[4]LadislavK,StevenC,CarolO.DualQuaternionsforRigidTransformationBlending[R].Ireland:DublinUniversity, 2006.

[5]DaniilidisK.Hand-eyeCalibrationUsingDualQuaternions[J].InternationalJournalofRoboticsResearch, 1999(18): 286-298.

[6]WuYuanxin,HuXiaoping,HuDewen.StrapdownInertialNavigationSystemAlgorithmsBasedonDualQuaternions[J].IEEETransactionsonAerospaceandElectronicSystems, 2005, 41(1): 110-132.

[7] 賀江濤.航天器近距離交會自主光學相對導航算法研究[D].南京：南京航空航天大學，2012.(HeJiangtao.AutonomousOpticalRelativeNavigationAlgorithmsforSpacecraftClose-rangeRendezvous[D].Nanjing:NanjingUniversityofAeronauticsandAstronautics,2012.)

[8]CaoXibin,ZhangShijie.AnIterativeMethodforVision-basedRelativePoseParametersofRVDSpacecraft[J].JournalofHarbinInstituteofTechnology,2005,37(8):691-694.

[9]JoanL，AnthonyN，ChrisJL，etal．AUnifiedMathematicalLanguageforPhysicsandEngineeringinthe21stCentury[J]．PhilTrans:Mathematical，PhysicalandEngineeringSciences，2000，358(1765): 21-39．

[10] 李江游.雙目視覺位姿估計算法研究[D].哈爾濱：黑龍江大學，2012.(LiJiangyou.StudyonBinocularVisionPositionandAttitudeEstimationAlgorithm[D].Harbin:HeilongjiangUniversity,2012.)

[11] 李爽.著陸小行星的視線測量自主光學相對導航算法及其可觀性分析[D].哈爾濱：哈爾濱工業(yè)大學，2007.(LiShuang.StudyonOpticalMeasurementBasedRelativeNavigationandItsApplicationinPlanetaryLanding[D].Harbin:HarbinInstituteofTechnology. 2007. )

[12] 付夢印.Kalman濾波理論及其在導航系統(tǒng)中的應用[M]. 北京:科學出版社, 2010.

An Integrated Relative Navigation Algorithm for Spacecraft Based on Binocular Vision and Inertial Measurement

Zhao Yilun1，Qiao Bing1，Jin Yongqiang2，Yu Feng1

1. Nanjing University of Aeronautics and Astronautics，Nanjing 210000，China 2. Shanghai Institute of Aerospace System Engineering，Shanghai 201100，China

Bytakingthedualquaternionasamathematicaltool,anewrelativenavigationmethodforspacecraftbasedonmachinevisionandinertialnavigationispresented.Firstlythespacepositionandattitudemodelofdualquaternionisintroduced,andnextbinocularvisionmeasurementmodelisestablishedbasedoncollinearprojectionprincipletoprovideobservationdataforthesystem.Then,thestateequationofspacecraftisdeducedregardingthespacecraftkinematicsmodelandtheextendKalmanfilterisdesignedonward.Finally,thecomputersimulationisimplementedandtheresultsshowthattherelativeposition,attitudeandvelocityofthespacecraftarewellestimated.Therequirementsofspacemissionsuchasrendezvousanddockingandspacecraftcapturecanbemetbyusingthemethod.

Binocularvision;Integratedrelativenavigation;Dualquaternion; Kalmanfilter;Spacecraft

*上海航天創(chuàng)新基金資助項目(SAST201308)；國家自然科學基金資助項目(61203197)

2015-10-10

趙逸倫(1991-)，男，蘭州人，碩士研究生，主要研究方向為空間在軌服務；喬 兵(1967-)，男，江蘇鎮(zhèn)江人，副教授，主要研究方向為空間在軌服務；靳永強(1981-)，男，山西榆次人，高級工程師，主要研究方向為空間安全；郁 豐(1980-)，男，南京人，副研究員，主要研究方向為微小衛(wèi)星導航與控制技術(shù)。

V448

A

1006-3242(2016)04-0047-06