趙逸倫 喬 兵 靳永強(qiáng) 郁 豐

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

?

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

趙逸倫1喬 兵1靳永強(qiáng)2郁 豐1

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

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

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

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

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

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

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

1.1 對(duì)偶四元數(shù)

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

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

(1)

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

限于篇幅，下面不加證明的給出基于四元數(shù)和對(duì)偶四元數(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)換對(duì)偶四元數(shù)：

(4)

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

(5)

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

2 雙目視覺和慣性元件測(cè)量模型

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

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

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

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

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

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

2.2 雙目視覺測(cè)量模型

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

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

左右攝像機(jī)的光心分別為Oc和Or，兩光軸平行且分別交成像平面于O1和O2點(diǎn)。兩攝像機(jī)的Z軸分別與各自的光軸重合，X軸則位于同一條直線上。左右攝像機(jī)光心的連線OcOr稱為基線，定義線段OcOr的長度為基線距B。設(shè)P點(diǎn)在左右攝像機(jī)成像平面上投影的坐標(biāo)分別為p1(x1,y1)，p2(x2,y2)，P點(diǎn)在左右攝像機(jī)坐標(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點(diǎn)在左攝像機(jī)下的坐標(biāo)：

(7)

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

(8)

將式(8)展開，可得：

(9)

即：

(10)

對(duì)式(10)等號(hào)兩邊同時(shí)左乘q并整理得到：

(11)

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

(12)

(13)

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

(14)

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

2.4 慣性元件的測(cè)量模型

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

am=a+ba+ηa

(16)

與加速度計(jì)類似，陀螺的輸出包括真實(shí)角速度、陀螺漂移和測(cè)量白噪聲3個(gè)部分，即

ωm=ω+bω+ηω

(17)

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

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

(18)

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

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

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

(19)

其中：

(20)

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

(21)

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

4 仿真驗(yàn)證和分析

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

表1 仿真參數(shù)

圖3 相對(duì)位置估計(jì)誤差

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

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

圖6 速度估計(jì)誤差

圖7 角速度估計(jì)誤差

5 總結(jié)與展望

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

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

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

[3] 崔本杰.基于對(duì)偶四元數(shù)的航天器相對(duì)導(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] 賀江濤.航天器近距離交會(huì)自主光學(xué)相對(duì)導(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] 李江游.雙目視覺位姿估計(jì)算法研究[D].哈爾濱：黑龍江大學(xué)，2012.(LiJiangyou.StudyonBinocularVisionPositionandAttitudeEstimationAlgorithm[D].Harbin:HeilongjiangUniversity,2012.)

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

[12] 付夢(mèng)印.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)新基金資助項(xiàng)目(SAST201308)；國家自然科學(xué)基金資助項(xiàng)目(61203197)

2015-10-10

趙逸倫(1991-)，男，蘭州人，碩士研究生，主要研究方向?yàn)榭臻g在軌服務(wù)；喬 兵(1967-)，男，江蘇鎮(zhèn)江人，副教授，主要研究方向?yàn)榭臻g在軌服務(wù)；靳永強(qiáng)(1981-)，男，山西榆次人，高級(jí)工程師，主要研究方向?yàn)榭臻g安全；郁 豐(1980-)，男，南京人，副研究員，主要研究方向?yàn)槲⑿⌒l(wèi)星導(dǎo)航與控制技術(shù)。

V448

A

1006-3242(2016)04-0047-06