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

趙逸倫 喬 兵 靳永強 郁 豐

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

?

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

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

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

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

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

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)慣性導(dǎo)航[6]等領(lǐng)域應(yīng)用中顯示出了其數(shù)學(xué)表達式直觀、明了和計算效率高等優(yōu)勢。可見，對偶四元數(shù)是一種適用于航天器相對導(dǎo)航任務(wù)模型建立的數(shù)學(xué)工具。

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

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

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

1.1 對偶四元數(shù)

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

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

(1)

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

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

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

(2)

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

aB=q*·aA·q

(3)

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

(4)

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

(5)

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

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

2.1 視覺測量的坐標(biāo)系定義

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

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

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

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

圖1 坐標(biāo)系定義示意圖

2.2 雙目視覺測量模型

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

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

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

(6)

由此可算出P點在左攝像機下的坐標(biāo)：

(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)，輸出的是載體相對于慣性系的非引力加速度和角速度在載體坐標(biāo)系上的投影。在忽略安裝誤差和標(biāo)定誤差的情況下，加速度計的測量輸出包括非引力加速度的真實值、加速度計漂移和量測噪聲3個部分[11]，即：

am=a+ba+ηa

(16)

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

ωm=ω+bω+ηω

(17)

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

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

(18)

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

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

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

(19)

其中：

(20)

導(dǎo)航測量方程：

(21)

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

4 仿真驗證和分析

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

表1 仿真參數(shù)

圖3 相對位置估計誤差

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

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

圖6 速度估計誤差

圖7 角速度估計誤差

5 總結(jié)與展望

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

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

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

[3] 崔本杰.基于對偶四元數(shù)的航天器相對導(dǎo)航方法研究[D].哈爾濱：哈爾濱工業(yè)大學(xué)，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] 賀江濤.航天器近距離交會自主光學(xué)相對導(dǎo)航算法研究[D].南京：南京航空航天大學(xué)，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].哈爾濱：黑龍江大學(xué)，2012.(LiJiangyou.StudyonBinocularVisionPositionandAttitudeEstimationAlgorithm[D].Harbin:HeilongjiangUniversity,2012.)

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

[12] 付夢印.Kalman濾波理論及其在導(dǎo)航系統(tǒng)中的應(yīng)用[M]. 北京:科學(xué)出版社, 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)；國家自然科學(xué)基金資助項目(61203197)

2015-10-10

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

V448

A

1006-3242(2016)04-0047-06