劉 濤 胡海霞 王 穎 王大軼

1.北京控制工程研究所,北京 100080 2. 空間智能控制技術(shù)國(guó)家級(jí)重點(diǎn)試驗(yàn)室,北京 100190

光學(xué)成像敏感器是一種高智能化的測(cè)量設(shè)備，作用距離從數(shù)米到數(shù)百米，在空間交會(huì)對(duì)接的平移靠攏段被廣泛應(yīng)用[1]。中國(guó)的載人二期交會(huì)對(duì)接任務(wù)中，就應(yīng)用國(guó)產(chǎn)CCD光學(xué)成像敏感器成功進(jìn)行了2次交會(huì)對(duì)接。

利用光學(xué)成像敏感器的測(cè)量信息進(jìn)行相對(duì)導(dǎo)航可采用2種方式：間接測(cè)量方式和直接測(cè)量方式。

間接測(cè)量方式首先利用測(cè)量信息通過(guò)數(shù)值算法求解相對(duì)位置和相對(duì)姿態(tài)，然后利用解算得到的相對(duì)位置和相對(duì)姿態(tài)分別設(shè)計(jì)濾波器，估計(jì)得到相對(duì)位置、相對(duì)速度和相對(duì)姿態(tài)等完備的相對(duì)狀態(tài)。間接測(cè)量方式的研究重點(diǎn)是采用有效的非線性方程組數(shù)值算法，提高算法收斂速度及剔除偽解等方面[2-3]。間接測(cè)量方式中濾波器設(shè)計(jì)比較簡(jiǎn)單，但該方法的不足是數(shù)值計(jì)算部分運(yùn)算量較大，且由于測(cè)量統(tǒng)計(jì)信息不明確，會(huì)影響估計(jì)效果。

直接測(cè)量方式將光學(xué)成像敏感器測(cè)量值轉(zhuǎn)換為相對(duì)每個(gè)目標(biāo)標(biāo)志器的視線測(cè)量值，利用多個(gè)視線測(cè)量值設(shè)計(jì)濾波器對(duì)狀態(tài)進(jìn)行估計(jì)。Sun等[4]對(duì)直接測(cè)量方式的可觀性進(jìn)行了分析，指出當(dāng)目標(biāo)標(biāo)志器不少于3個(gè)時(shí)，除了共線等特殊構(gòu)型外，利用視線測(cè)量信息，能夠唯一的確定相對(duì)位置和相對(duì)姿態(tài)六自由度的相對(duì)狀態(tài)；Kim等[5]以及Woffinden等[6]利用目標(biāo)標(biāo)志器視線角測(cè)量，通過(guò)設(shè)計(jì)EKF，直接對(duì)相對(duì)姿態(tài)和航天器絕對(duì)位置進(jìn)行估計(jì)。直接測(cè)量方式目前是理論研究的熱點(diǎn)，其研究重點(diǎn)是高維非線性系統(tǒng)的濾波器設(shè)計(jì)。直接測(cè)量方式需要對(duì)高維非線性系統(tǒng)設(shè)計(jì)濾波器，濾波器較復(fù)雜，但該方法避免了數(shù)值計(jì)算可能出現(xiàn)的數(shù)值偽解問(wèn)題，且直接測(cè)量信息的測(cè)量誤差統(tǒng)計(jì)特性比較明確，利于實(shí)現(xiàn)高精度濾波器設(shè)計(jì)。

本文提出一種利用光學(xué)成像敏感器直接測(cè)量信息，采用UKF濾波方法設(shè)計(jì)濾波器，直接對(duì)相對(duì)位置、相對(duì)速度和相對(duì)姿態(tài)進(jìn)行估計(jì)的相對(duì)導(dǎo)航方法。并以探月三期任務(wù)為背景，通過(guò)數(shù)學(xué)仿真對(duì)所提出的相對(duì)導(dǎo)航方法性能進(jìn)行分析。

1 光學(xué)測(cè)量敏感器測(cè)量原理

光學(xué)成像敏感器利用主動(dòng)光源或被動(dòng)光源，將安裝在目標(biāo)航天器上的多個(gè)標(biāo)志器成像到敏感器的成像面上，然后通過(guò)對(duì)目標(biāo)航天器上的標(biāo)志器在敏感器成像面上的像和有關(guān)坐標(biāo)系的轉(zhuǎn)換與數(shù)據(jù)處理，獲得目標(biāo)航天器相對(duì)于追蹤航天器的相對(duì)位置和相對(duì)姿態(tài)信息。光學(xué)成像敏感器的測(cè)量原理如圖1所示。

在光學(xué)成像敏感器的目標(biāo)標(biāo)志器上安裝n(n≥3)個(gè)特征光點(diǎn)A,B,C,…，其在像機(jī)成像面對(duì)應(yīng)的成像點(diǎn)為a,b,c,…；坐標(biāo)系OoXoYoZo為標(biāo)志坐標(biāo)系，其原點(diǎn)Oo為光點(diǎn)架中心，OoXo軸沿對(duì)接軸方向；坐標(biāo)系OcXcYcZc為像機(jī)坐標(biāo)系，其原點(diǎn)Oc為投影中心，OcXc軸與主光軸重合。

圖1 光學(xué)成像敏感器測(cè)量原理圖

(1)

(2)

利用式(1)和式(2)，通過(guò)聯(lián)立多個(gè)目標(biāo)標(biāo)志的測(cè)量結(jié)果，求解非線性方程可以得到相對(duì)位置T和相對(duì)姿態(tài)轉(zhuǎn)換矩陣Cco，可以此作為測(cè)量信息，設(shè)計(jì)Kalman濾波器，估計(jì)得到相對(duì)導(dǎo)航參數(shù)，本文稱之為基于間接測(cè)量信息的相對(duì)導(dǎo)航濾波器。

若不進(jìn)行非線性方程的求解，將式(2)直接作為測(cè)量量也可用于設(shè)計(jì)Kalman濾波器，本文稱之為基于直接測(cè)量信息的相對(duì)導(dǎo)航濾波器。實(shí)際應(yīng)用時(shí)需要對(duì)式(2)進(jìn)行適當(dāng)?shù)淖冃危?/p>

(3)

由式(3)容易得到：

(4)

將式(4)記做：

li=Ccori(i=1,…,n)

(5)

式(5)即為目標(biāo)標(biāo)志器視線方向與相對(duì)姿態(tài)和相對(duì)位置之間的關(guān)系，測(cè)量信息li可由相機(jī)原始測(cè)量信息經(jīng)簡(jiǎn)單轉(zhuǎn)換得到。

(6)

2 相對(duì)姿態(tài)運(yùn)動(dòng)學(xué)模型

如果交會(huì)中目標(biāo)航天器始終保持對(duì)月定向，則目標(biāo)器本體相對(duì)于月球慣性系的姿態(tài)角速度由歐拉角表示為

(7)

式中，ωo為目標(biāo)航天器的軌道角速度。

追蹤航天器相對(duì)于目標(biāo)航天器的轉(zhuǎn)動(dòng)角速度為ωr=ωc-ωt，其中，ωc和ωt分別為追蹤航天器本體和目標(biāo)航天器本體相對(duì)于慣性空間的角速度。

(8)

考慮到實(shí)際交會(huì)過(guò)程中，環(huán)月飛行時(shí)目標(biāo)器還可能處于對(duì)日定向或慣性定向等模式的情況，以下給出利用姿態(tài)四元數(shù)描述的相對(duì)姿態(tài)運(yùn)動(dòng)學(xué)模型。

(9)

由姿態(tài)運(yùn)動(dòng)學(xué)方程可以得到

(10)

其中，ωr=ωc-Cctωt，Cct為追蹤航天器本體相對(duì)目標(biāo)航天器本體的方向余弦陣。若設(shè)

則有[5]

(11)

上式推導(dǎo)中，用到以下關(guān)系式：

Cct=ΞT(qr)Ψ(qr),

Ψ(qr)ωt=Γ(ωt)qr。

利用四元數(shù)表示的相對(duì)姿態(tài)運(yùn)動(dòng)方程為

(12)

該方程為線性時(shí)變系統(tǒng)，為設(shè)計(jì)濾波器，需要對(duì)該方程進(jìn)行離散化處理，可以得到[5]

(13)

其中，

(14)

(15)

Δt為離散化周期，ωt,k由目標(biāo)航天器上安裝的陀螺測(cè)量得到，并利用太陽(yáng)敏感器等敏感器測(cè)量信息進(jìn)行陀螺常偏修正，再通過(guò)星間鏈路實(shí)時(shí)傳遞給追蹤航天器。

3 相對(duì)位置動(dòng)力學(xué)模型

由于目標(biāo)航天器通常處于近圓軌道，可用C-W方程描述相對(duì)位置的變化。以目標(biāo)航天器環(huán)月軌道系為基準(zhǔn)，C-W方程如下[7]：

(16)

Xk+1=Φk+1,kXk+Gk+1,kUk+Wk

(17)

其中

(18)

(19)

這里，φ=ωoΔt，Δt為濾波周期，Wk是零均值白噪聲序列，滿足E[Wk]=0,Cov(Wk,Wj)=Qkδk,j。

4 UKF濾波算法

考慮以下離散隨機(jī)系統(tǒng)

Xk+1=f(Xk)+Wk

(20)

Zk+1=h(Xk+1)+Vk+1

(21)

針對(duì)如式(20)～(21)所描述的系統(tǒng)，UKF濾波器設(shè)計(jì)步驟如下[8]：

1)產(chǎn)生采樣點(diǎn)

(22)

2)時(shí)間更新

χi,k+1|k=f(χi,k)

(23)

(24)

(25)

3)重新采樣

4)預(yù)測(cè)測(cè)量量均值，方差以及協(xié)方差

Zi,k+1|k=h(χi,k+1|k)

(26)

(27)

(28)

(29)

5)測(cè)量更新

(30)

(31)

(32)

5 相對(duì)導(dǎo)航濾波器設(shè)計(jì)

狀態(tài)方程：將C-W方程和四元數(shù)表示的相對(duì)姿態(tài)運(yùn)動(dòng)方程聯(lián)立作為狀態(tài)方程

(33)

測(cè)量方程

Zk+1=h(Xk+1)+Vk+1

(34)

其中，Zk+1=(α1β1,…,αnβn)T，h(Xk+1)的具體形式如式(6)所示，Vk+1為角度測(cè)量噪聲，設(shè)定其為零均值高斯白噪聲，方差為Rk+1。

式(33)和(34)所表示的系統(tǒng)狀態(tài)方程為線性，但測(cè)量方程為非線性，若利用EKF來(lái)設(shè)計(jì)濾波器，則雅可比矩陣的求解非常繁瑣，所以本文采用UKF設(shè)計(jì)相對(duì)導(dǎo)航濾波器。

姿態(tài)四元數(shù)必須滿足模值為1，由于UKF采用加權(quán)和的形式估計(jì)狀態(tài)均值(如式(24))，很難保證姿態(tài)四元數(shù)保持單位模值，所以必須采用間接方式估計(jì)相對(duì)姿態(tài)四元數(shù)。Crassidis等[9]提出采用Rodrigus參數(shù)替代誤差姿態(tài)四元數(shù)，由于Rodrigus參數(shù)取值無(wú)單位模值的限制，可按照UKF的計(jì)算方式進(jìn)行估計(jì)。根據(jù)該思想，將Rodrigus參數(shù)表示的姿態(tài)誤差與相對(duì)位置和相對(duì)速度一起作為UKF估計(jì)的直接狀態(tài)量，再利用估計(jì)得到的Rodrigus參數(shù)來(lái)修正相對(duì)姿態(tài)四元數(shù)，從而得到姿態(tài)四元數(shù)估計(jì)值。

(35)

式中，0

(36)

Step1：k+1時(shí)刻初始Sigma點(diǎn)選取。

(37)

Step2：預(yù)測(cè)更新。

按式(38)生成更新后的Rodrigus參數(shù)Sigma采樣點(diǎn)

(38)

Step3：測(cè)量更新。

按照式(39)得到測(cè)量更新后的相對(duì)姿態(tài)四元數(shù)估計(jì)值

(39)

6 數(shù)學(xué)仿真

將本文所提出的相對(duì)導(dǎo)航方法應(yīng)用于月球軌道交會(huì)對(duì)接任務(wù)的平移考慮段，本節(jié)給出相應(yīng)的仿真結(jié)果并進(jìn)行分析。

仿真中設(shè)定目標(biāo)航天器的初始軌道根數(shù)為：

at=1938km，et=0.0003，it=25°，

Ωt=180°，wt=240°，ft=0°

設(shè)定光學(xué)成像敏感器安裝在追蹤航天器質(zhì)心處，測(cè)量坐標(biāo)系與追蹤航天器本體系重合，測(cè)量噪聲為零均值高斯白噪聲，3σ=0.01°。

目標(biāo)航天器上共安裝4個(gè)目標(biāo)標(biāo)志器，采用四面錐安裝方式，各目標(biāo)標(biāo)志器在目標(biāo)航天器本體坐標(biāo)系下的位置分別為：

(-0.5 0 0)T，(0-1.5cos(30°)-1.5sin(30°))T。

濾波器初始狀態(tài)設(shè)置為：

飛行過(guò)程為：追蹤航天器從目標(biāo)航天器后方約150m處開(kāi)始以恒定速度直線接近目標(biāo)航天器，接近至4m仿真結(jié)束。

仿真結(jié)果如圖2～7所示，從圖2～7及表1可以看到，整個(gè)平移靠攏段相對(duì)位置估計(jì)精度達(dá)到0.1m量級(jí)，相對(duì)速度估計(jì)精度達(dá)到0.01m/s量級(jí)，相對(duì)姿態(tài)估計(jì)精度達(dá)到0.1度量級(jí)。并且隨著相對(duì)距離的接近相對(duì)導(dǎo)航精確也相應(yīng)提高，相對(duì)距離5m以內(nèi)，相對(duì)位置估計(jì)精度小于0.02m，相對(duì)速度估計(jì)精度小于0.01m/s,相對(duì)姿態(tài)估計(jì)精度小于0.1°，完全能夠滿足交會(huì)對(duì)接任務(wù)的需要。

圖2 相對(duì)位置真值與估計(jì)值

圖3 相對(duì)速度真值與估計(jì)值

圖4 相對(duì)姿態(tài)真值與估計(jì)值

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

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

7 結(jié)論

對(duì)基于光學(xué)成像敏感器測(cè)量的月球軌道交會(huì)對(duì)接相對(duì)導(dǎo)航問(wèn)題進(jìn)行了研究，提出了一種采用光學(xué)敏感器直接測(cè)量信息，利用UKF算法設(shè)計(jì)濾波器，直接對(duì)相對(duì)位置、相對(duì)速度和相對(duì)姿態(tài)進(jìn)行估計(jì)的相對(duì)導(dǎo)航方法。該方法直接利用光學(xué)成像敏感器提供的多視線角作為測(cè)量量，避免了非線性數(shù)值解算，同時(shí)使得濾波測(cè)量信息的統(tǒng)計(jì)信息比較明確。數(shù)學(xué)仿真證實(shí)了所提出方法可得到精確較高的相對(duì)導(dǎo)航結(jié)果，能夠滿足高精度交會(huì)對(duì)接任務(wù)的需求。本文的結(jié)果為提高基于光學(xué)成像敏感器測(cè)量的相對(duì)導(dǎo)航系統(tǒng)性能提供了理論參考。

表1 估計(jì)誤差統(tǒng)計(jì)值

[1] 張淑琴，等.空間交會(huì)對(duì)接測(cè)量技術(shù)及工程應(yīng)用[M].中國(guó)宇航出版社，2005：302～305.

[2] Mukundan R,Ramkrishnan K R.A Quaternion Solution to the Pose Determination Problem for Rendezvous and Docking Simulation[J].Mathematics and Computers in Simulation, 1995,39(1-2):143-153.

[3] 張昊,解永春,吳宏鑫.交會(huì)對(duì)接光學(xué)成像敏感器光點(diǎn)布局求解有效性研究[J].航天控制,2008， 26(3):44-48.(ZHANG Hao，XIE Yongchun，WU Hong xin．Research on the Target Pattern of Optical Sensor Used in RVD Based on Four Coplanar Points[J].Aerospace Control,2008，26 (3): 44-48.)

[4] Sun D,Crassidis J L .Observability Analysis of Six-Degree-of-Freedom Configuration Determination Using Vector Observations[J].Journal of Guidance Control and Dynamics,2005,25(6):1149-1156.

[5] Son-Goo K, Crassidis J L ,Cheng Y , et al.Kalman Filtering for Relative Spacecraft Attitude and Position Estimation[C].AIAA Guidance, Navigation, and Control Conference and Exhibit,San Francisco, California,15-18 August ,2005.

[6] Woffinden D C, Geller D K.Relative Angles-Only Navigation and Pose Estimation For Autonomous Orbital Rendezvous[C].AIAA/AAS Astrodynamics Specialist Conference and Exhibit,Keystone,Colorado,21-24 August, 2006.

[7] Wigbert F.Automated Rendezvous and Docking of Spacecraft[M].Cambridge University Press, 2003.

[8] Julier S J,Ulhmann J K, Whyte H F D B.A New Method for the Nonlinear Transformation of Means and Covariances in Filters and Estimators[J].IEEE Transaction on Automatic Control , 2000,45(3):472-482.

[9] Crassidis J L , F.Landis Markley.Unscented Filtering for Spacecraft Attitude Estimation[J].Journal of Guidance Control and Dynamics，2006,26(4):536-542.

[10] Rudolph van der Merwe.Sigma-Point Kalman Filters for Probabilistic Inference in Dynamic State-Space Models[D].Oregon Health and Science University,2004.

[11] Ienkaran A, Haykin S .Discrete-Time Nonlinear Filtering Algorithms Using Gauss-Hermite Quadrature.[J].Proceedings of the IEEE，2007,95(5):953-977.

[12] Chari R J V.Autonomous Orbital Rendezvous Using Angles-Only Navigation[D].Massachusetts Institute of Technology, 2001.