基于跟瞄和加表PI濾波近程相對(duì)導(dǎo)航方法

王獻(xiàn)忠，張 肖，張麗敏，施常勇

(1.上海航天技術(shù)研究院，上海 201109； 2.上海航天控制技術(shù)研究所，上海 201109； 3.上海市空間智能控制技術(shù)重點(diǎn)實(shí)驗(yàn)室，上海 201109)

0 引言

近程相對(duì)導(dǎo)航是空間飛行器交會(huì)的關(guān)鍵技術(shù)。與非合作目標(biāo)進(jìn)行交會(huì)時(shí)，空間飛行器因不能與目標(biāo)進(jìn)行通信而無法獲取目標(biāo)精確的導(dǎo)航定位信息。因此，國內(nèi)外學(xué)者對(duì)近程相對(duì)導(dǎo)航進(jìn)行了廣泛研究。從公開文獻(xiàn)來看，大部分采用了卡爾曼濾波技術(shù)或其衍生算法。文獻(xiàn)[1-4]介紹了基于擴(kuò)展卡爾曼濾波技術(shù)的激光雷達(dá)和慣導(dǎo)的非合作目標(biāo)相對(duì)導(dǎo)航技術(shù)，其中，文獻(xiàn)[1-3]將導(dǎo)航算法建立在慣性坐標(biāo)系下，文獻(xiàn)[4]將導(dǎo)航算法建立在當(dāng)?shù)厮酱怪弊鴺?biāo)系下。文獻(xiàn)[5-6]提出了一種光學(xué)敏感器加慣導(dǎo)的航天器組合相對(duì)導(dǎo)航方法，并在此基礎(chǔ)上設(shè)計(jì)了卡爾曼濾波器。文獻(xiàn)[7-8]基于模糊迭代均方根容積卡爾曼濾波算法研究非合作目標(biāo)的相對(duì)導(dǎo)航算法，但其重點(diǎn)是將采用擴(kuò)展卡爾曼濾波、容積卡爾曼濾波，以及模糊迭代均方根容積卡爾曼濾波的算法進(jìn)行仿真對(duì)比，未與工程實(shí)際相結(jié)合，算法坐標(biāo)系被建立在站心赤道坐標(biāo)系下。上述文獻(xiàn)均采用了比較復(fù)雜的濾波算法，且都未關(guān)注對(duì)加表漂移的估計(jì)效果。對(duì)比上述文獻(xiàn)，本文提出的基于跟瞄和加表比例積分(PI)濾波的近程相對(duì)導(dǎo)航技術(shù)將坐標(biāo)系建立在軌道坐標(biāo)系下，有利于控制，且相對(duì)于卡爾曼濾波和更高階的濾波而言，算法更為簡(jiǎn)單。

本文研究了基于PI濾波估計(jì)加表漂移的跟瞄近程相對(duì)導(dǎo)航算法。首先推導(dǎo)了基于C-W(Clohessey-Whiltshire)方程的近程相對(duì)導(dǎo)航算法，并基于相對(duì)導(dǎo)航與跟瞄輸出計(jì)算的相對(duì)位置/相對(duì)速度誤差和PI濾波估計(jì)加表的加速度漂移；然后給出了基于相對(duì)位置/相對(duì)速度，以及僅基于相對(duì)位置信息的跟瞄與加表近程相對(duì)導(dǎo)航算法；最后進(jìn)行了工程應(yīng)用方式分析和仿真驗(yàn)證。

1 基于C-W方程的近程相對(duì)導(dǎo)航算法原理

近距離跟蹤時(shí)，兩星軌道坐標(biāo)系基本一致，軌道角速率也基本一致?？紤]到目標(biāo)星軌道一般未知，基于跟蹤星軌道系進(jìn)行近程相對(duì)導(dǎo)航，并用跟蹤星軌道角速率代替目標(biāo)星軌道角速率。

aao=Aoa·aaa

(1)

式中：aaa為加表測(cè)得的加表坐標(biāo)系下的非慣性加速度。

(2)

(3)

式中：n為跟蹤星軌道角速度；μ為地球引力常數(shù)；a為跟蹤星軌道半長(zhǎng)軸；ax，ay，az為跟蹤星在跟蹤星軌道系下的非慣性加速度。

ao為跟蹤星軌道系下兩星相對(duì)加速度，則

(4)

在跟蹤星軌道系下進(jìn)行近程相對(duì)導(dǎo)航解算，求得相對(duì)速度vo和相對(duì)位置ro為

(5)

(6)

式中：t為時(shí)間。

2 基于跟瞄和PI濾波近程相對(duì)導(dǎo)航算法

2.1 相對(duì)位置/相對(duì)速度誤差和加表漂移估計(jì)

近程相對(duì)導(dǎo)航一般基于C-W方程推算相對(duì)位置和相對(duì)速度。近距離跟蹤時(shí)，兩星慣性加速度基本一致。目標(biāo)星非慣性加速度一般不可獲得，跟蹤星配置的加表雖能連續(xù)輸出推力等非慣性力產(chǎn)生的加速度，但加表輸出加速度存在漂移。這會(huì)導(dǎo)致長(zhǎng)時(shí)間基于C-W方程遞推時(shí)，兩星相對(duì)位置和相對(duì)速度誤差逐步增大。因此，需要利用能測(cè)量相對(duì)信息的光電組合、微波雷達(dá)、激光雷達(dá)等跟瞄敏感器，對(duì)相對(duì)位置和相對(duì)速度進(jìn)行修正。

跟瞄可能受單粒子翻轉(zhuǎn)、姿態(tài)機(jī)動(dòng)或翻滾時(shí)失捕目標(biāo)、地面局部區(qū)域人為干擾等影響，特別是在跟瞄復(fù)位或重加載期間，跟瞄不能連續(xù)輸出兩星相對(duì)測(cè)量信息。因此，相對(duì)導(dǎo)航輸出仍為C-W方程遞推結(jié)果，跟瞄有效時(shí)，基于跟瞄信息對(duì)C-W方程遞推的相對(duì)位置和相對(duì)速度誤差進(jìn)行估計(jì)并修正。該方式既可確保相對(duì)位置和相對(duì)速度輸出的連續(xù)性，又可抑制跟瞄觀測(cè)噪聲對(duì)近程相對(duì)導(dǎo)航精度的影響。目前已有的卡爾曼濾波算法比較復(fù)雜，不利于在軌實(shí)現(xiàn)。本文提出的PI濾波估計(jì)方法不涉及多階矩陣求逆及其他計(jì)算，濾波參數(shù)由地面設(shè)計(jì)，算法簡(jiǎn)單可靠，適合于在軌應(yīng)用。

圖1為跟瞄測(cè)量示意圖。圖中：A為目標(biāo)星；T為跟蹤星；α為兩星相對(duì)位置矢量在跟蹤星軌道坐標(biāo)系XoYo面的投影與Xo的夾角，為跟瞄輸出的方位角；β為投影與兩星相對(duì)位置矢量的夾角，為跟瞄輸出的高低角。

圖1 跟瞄測(cè)量示意圖Fig.1 Schematic diagram of tracking and pointing measurement

跟蹤星相對(duì)目標(biāo)星的位置為

xTtoA=-ρ·cosβ·cosα

(7)

yTtoA=-ρ·cosβ·sinα

(8)

zTtoA=ρ·sinβ

(9)

式中：ρ為跟瞄輸出的視線距。

基于跟瞄視線距變化率和視線角變化率，求得相對(duì)速度為

(10)

(11)

(12)

設(shè)加表加速度漂移轉(zhuǎn)換到跟蹤星軌道系為δao，求得加表漂移引起的相對(duì)速度誤差δvo和相對(duì)位置誤差δro為

(13)

(14)

設(shè)基于跟瞄解算輸出的相對(duì)位置roT和相對(duì)速度voT為

(15)

(16)

基于近程相對(duì)導(dǎo)航第k步解算輸出的相對(duì)位置ro,k和相對(duì)速度vo,k，求得近程相對(duì)導(dǎo)航解算引起的相對(duì)位置誤差Δro,k和相對(duì)速度誤差Δvo,k為

Δro,k=ro,k-roT

(17)

Δvo,k=vo,k-voT

(18)

基于跟瞄求得近程相對(duì)導(dǎo)航相對(duì)位置誤差Δro,k和相對(duì)速度誤差Δvo,k。在近程相對(duì)導(dǎo)航積分過程中，逐步增加相對(duì)位置和相對(duì)速度誤差修正量，可確保誤差修正的平穩(wěn)性。第k步近程相對(duì)導(dǎo)航解算相對(duì)位置誤差修正量rdo,k和相對(duì)速度誤差修正量vdo,k為

rdo,k=kpr·Δro,k

(19)

vdo,k=kpv·Δvo,k

(20)

式中：kpr，kpv分別為三軸相對(duì)位置和相對(duì)速度誤差修正量估計(jì)比例系數(shù)。kpr，kpv為3×3對(duì)角陣，三軸可獨(dú)立估計(jì)相對(duì)位置和相對(duì)速度誤差修正量。

將跟瞄求得的近程相對(duì)導(dǎo)航相對(duì)速度誤差Δvo,k轉(zhuǎn)換到加表坐標(biāo)系，即

Δva,k=Aao·Δvo,k

(21)

式中：Aao為跟蹤星軌道系到加表坐標(biāo)系的姿態(tài)轉(zhuǎn)換矩陣；Δva,k為加表坐標(biāo)系下相對(duì)速度誤差。

基于PI濾波估計(jì)加表加速度漂移算法為

(22)

式中：δaa,k為第k步估計(jì)的加表漂移；kpa，kia分別為三軸加速度漂移估計(jì)比例系數(shù)和積分系數(shù)。kpa，kia為3×3對(duì)角陣，三軸可獨(dú)立估計(jì)加速度漂移。kpa影響濾波收斂速度，取值范圍一般為0.01～0.1；kia影響濾波收斂精度，取值通常比比例系數(shù)低一個(gè)數(shù)量級(jí)。

2.2 跟瞄與加表近程相對(duì)導(dǎo)航

設(shè)加表加速度漂移估計(jì)為δaa，求得跟蹤星軌道系下扣除加表漂移的跟蹤星非慣性加速度

aao=Aoa·(aaa-δaa)

(23)

式中：Aoa為加表坐標(biāo)系到跟蹤星軌道系的姿態(tài)轉(zhuǎn)換矩陣。

求得跟蹤星軌道系下，扣除加表漂移的兩星相對(duì)運(yùn)動(dòng)加速度

(24)

在跟蹤星軌道系下，扣除相對(duì)位置和相對(duì)速度誤差修正量，應(yīng)用簡(jiǎn)化積分算法進(jìn)行近程相對(duì)導(dǎo)航解算，即

vo,k=vo,k-1+[ao,k-1+

ao,k-ao,k-1/2]·T-vdo,k-1

(25)

ro,k=ro,k-1+[vo,k-1+

vo,k-vo,k-1/2]·T-rdo,k-1

(26)

式中：ao,k-1為第k-1步扣除加表漂移的加速度；ao,k為第k步扣除加表漂移的加速度；vdo,k-1為第k-1步估計(jì)的相對(duì)速度誤差修正量；vo,k-1為第k-1步扣除相對(duì)速度誤差修正量的相對(duì)速度；vo,k為第k步扣除相對(duì)速度誤差修正量的相對(duì)速度；rdo,k-1為第k-1步估計(jì)的相對(duì)位置誤差修正量；ro,k-1為第k-1步扣除相對(duì)位置誤差修正量的相對(duì)位置；ro,k為第k步扣除相對(duì)位置誤差修正量的相對(duì)位置；T為相對(duì)導(dǎo)航周期。

3 工程應(yīng)用

3.1 基于相對(duì)位置/相對(duì)速度估計(jì)

(27)

(28)

(29)

3.2 基于相對(duì)位置估計(jì)

跟瞄輸出的相對(duì)速度精度遠(yuǎn)低于相對(duì)位置精度，且觀測(cè)噪聲較大。為防止低精度的相對(duì)速度觀測(cè)量影響近程相對(duì)導(dǎo)航精度，實(shí)際工程應(yīng)用中，可僅基于跟瞄輸出的相對(duì)位置與加表進(jìn)行近程相對(duì)導(dǎo)航。

考慮到相對(duì)位置誤差也能反映相對(duì)速度誤差，基于相對(duì)位置誤差修正量rdo,k估計(jì)相對(duì)速度誤差修正量。

(30)

將相對(duì)速度誤差修正量vdo,k轉(zhuǎn)換到加表坐標(biāo)系，即

vda,k=Aao·vdo,k

(31)

4 仿真驗(yàn)證

加表三軸加速度常值漂移分別為-0.005，0.002，0.006 m/s2，隨機(jī)漂移為0.000 5 m/s2，跟瞄視線距常值誤差為15 m，視線角偏差為0.1°。仿真中比例和積分系數(shù)取值為0.03/0.001。

利用跟瞄輸出的視線距和視線角計(jì)算兩星相對(duì)位置，再與加表基于PI濾波進(jìn)行近程相對(duì)導(dǎo)航仿真。其中，加表加速度漂移估計(jì)仿真結(jié)果如圖2，3所示，跟瞄與加表近程相對(duì)導(dǎo)航收斂后相對(duì)位置和相對(duì)速度誤差仿真結(jié)果如圖4，5所示。

圖2 基于相對(duì)位置觀測(cè)信息加表加速度漂移估計(jì)曲線Fig.2 Estimation of acceleration drift based on relative position observation information

圖3 基于相對(duì)位置觀測(cè)信息加表加速度漂移估計(jì)放大Fig.3 Amplification curve of acceleration drift estimation based on relative position observation information

圖4 基于相對(duì)位置觀測(cè)信息組合導(dǎo)航位置誤差曲線Fig.4 Position error of integrated navigation based on relative position observation information

圖5 基于相對(duì)位置觀測(cè)信息組合導(dǎo)航速度誤差曲線Fig.5 Velocity error of integrated navigation based on relative position observation information

由圖可知：以僅基于跟瞄的相對(duì)位置作為觀測(cè)量時(shí)，加表的常值漂移也能被準(zhǔn)確估計(jì)，估計(jì)精度小于0.000 5 m/s2，相對(duì)位置誤差小于20 m，相對(duì)速度誤差小于0.05 m/s。

5 結(jié)束語

本文在PI濾波估計(jì)加表加速度漂移的基礎(chǔ)上，進(jìn)行了跟瞄與加表近程相對(duì)導(dǎo)航算法研究，通過仿真驗(yàn)證了近程相對(duì)導(dǎo)航算法的有效性?；赑I濾波估計(jì)加表漂移的跟瞄與加表近程相對(duì)導(dǎo)航方法計(jì)算簡(jiǎn)單，易于工程實(shí)現(xiàn)，且已通過在軌考核驗(yàn)證。本研究選用的目標(biāo)飛行器為一穩(wěn)定目標(biāo)，后續(xù)可針對(duì)旋轉(zhuǎn)或機(jī)動(dòng)目標(biāo)飛行器探測(cè)和近程相對(duì)導(dǎo)航算法適應(yīng)性做進(jìn)一步研究。