基于并行擴(kuò)展卡爾曼濾波的自主衛(wèi)星星座導(dǎo)航方法

張 原， 熊 凱， 邢 琰

北京控制工程研究所, 北京 100090

0 引 言

航天器導(dǎo)航技術(shù)主要用于確定航天器的位置、姿態(tài)和速度，這是當(dāng)前和未來(lái)許多航天任務(wù)的基本要素，其中，航天器自主導(dǎo)航技術(shù)可以降低對(duì)地面跟蹤觀測(cè)站的依賴性，增強(qiáng)空間系統(tǒng)的生存能力.

基于星對(duì)星跟蹤(SST)測(cè)量的自主衛(wèi)星星座導(dǎo)航方法在當(dāng)前技術(shù)條件下具有很高的精度.近些年來(lái)有學(xué)者給出了使用星間測(cè)距技術(shù)實(shí)現(xiàn)月球衛(wèi)星自主定軌的方案(LiAISON)，用于在地月三體系統(tǒng)中跟蹤衛(wèi)星[1-3].這種方法只需要進(jìn)行星間距離測(cè)量，而且可以利用星間鏈路(ISLs)方法實(shí)現(xiàn)高精度定位.將多顆地球衛(wèi)星之間的距離測(cè)量結(jié)果以及地球衛(wèi)星與月球衛(wèi)星之間的距離測(cè)量結(jié)果相結(jié)合，能夠提高導(dǎo)航系統(tǒng)的可觀度，適用于確定衛(wèi)星星座的絕對(duì)位置.

基于ISLs的星間距離測(cè)量容易受到外界干擾，從而導(dǎo)致噪聲統(tǒng)計(jì)特性的不確定性，通過(guò)設(shè)計(jì)自適應(yīng)濾波算法可以部分解決模型不確定性問(wèn)題[4-7].多模型自適應(yīng)估計(jì)(MMAE)作為目前研究最為廣泛的自適應(yīng)濾波方法之一，具有很高的應(yīng)用價(jià)值[8-11].

本文提出一種基于MMAE體系結(jié)構(gòu)的并行擴(kuò)展卡爾曼濾波器(PEKF).將不同的測(cè)量子集分配給各個(gè)并行濾波器，各個(gè)并行濾波器單獨(dú)運(yùn)行，整體狀態(tài)估計(jì)值通過(guò)并行濾波器估計(jì)值的加權(quán)和計(jì)算來(lái)實(shí)現(xiàn)，并行濾波器的權(quán)重根據(jù)殘差序列計(jì)算.該方法能夠自適應(yīng)地選擇合適的測(cè)量值以提高估計(jì)精度，同時(shí)削弱測(cè)量不確定性的不利影響.

1 系統(tǒng)模型

本文所研究的問(wèn)題是根據(jù)測(cè)量yj/i,k,(i=1,2,…,N-1,j=i+1,i+2,…N)估計(jì)未知狀態(tài)向量xi,k(i=1,2,…,N)，其中k表示離散的時(shí)間點(diǎn)，N表示網(wǎng)絡(luò)節(jié)點(diǎn)數(shù).對(duì)于所考慮的自主導(dǎo)航系統(tǒng)，N是星座中的衛(wèi)星數(shù).狀態(tài)向量的演化過(guò)程由離散非線性模型描述

xi,k=fi(xi,k-1)+wi,k

(1)

其中fi(·)是已知的狀態(tài)轉(zhuǎn)移函數(shù)，wi，k是均值為零的高斯白噪聲，其方差陣為

(2)

其中Qi，k(i=1,2，…，N)是半正定矩陣，實(shí)際工作中，考慮到數(shù)值計(jì)算時(shí)的截?cái)嗾`差，為保證濾波迭代順利進(jìn)行，一般設(shè)定Qi，k(i=1,2，…，N)為正定矩陣.

測(cè)量yj/i,k與狀態(tài)向量xi，k和xj，k有關(guān)

yj/i,k=hj/i(xi,k,xj,k)+vj/i,k

(3)

其中hj/i(·，·)是已知的非線性測(cè)量函數(shù)，vj/i，k是零均值高斯白噪聲，其方差陣為

(4)

其中Rj/i，k(i=1,2，…，N-1，j=i+1，i+2，…,N)是正定矩陣，假設(shè)函數(shù)fi(·)和hj/i(·，·)連續(xù)可微.如圖1所示為存在3個(gè)節(jié)點(diǎn)和3個(gè)傳感器的系統(tǒng)網(wǎng)絡(luò)中，狀態(tài)向量和測(cè)量之間的關(guān)系.

圖1 狀態(tài)向量和測(cè)量之間的關(guān)系Fig.1 Relation between the state vectors and the measurements

遞推形式動(dòng)力學(xué)方程和測(cè)量為

(5)

其中

(6)

(7)

(8)

wk和vk的方差陣分別為

Qk=diag{Q1,k,Q2,k,…,QN,k}

Rk=diag{R2/1,k，…，Rj/i,k，…，RN/N-1,k}

由于研究對(duì)象實(shí)際中并非強(qiáng)非線性系統(tǒng)，因此對(duì)于系統(tǒng)的非線性不作深入研究與探討.

測(cè)量調(diào)度的目的是選擇測(cè)量的子集，使得某些節(jié)點(diǎn)的狀態(tài)向量的估計(jì)誤差最小，這通過(guò)最小化適當(dāng)?shù)拇鷥r(jià)函數(shù)來(lái)實(shí)現(xiàn).對(duì)于非線性隨機(jī)系統(tǒng)，可以采用Cramer-Rao下界(CRLB)作為性能度量指標(biāo)[12-13].CRLB提供了均方估計(jì)誤差(MSE)的一個(gè)下界，可用于評(píng)估濾波器的性能.通常，較小的CRLB值意味著更精確的狀態(tài)估計(jì)，而且這種性能度量方式具有廣泛的適用性，并不只是針對(duì)某一種特定的濾波器.

作為狀態(tài)估計(jì)的誤差協(xié)方差矩陣下界的CRLB具有以下形式

(9)

(10)

如果忽略過(guò)程噪聲，則可以簡(jiǎn)化為

(11)

在計(jì)算CRLB時(shí)是不考慮模型不確定性的，而在測(cè)量噪聲統(tǒng)計(jì)特性不確定的情況下，對(duì)于某一個(gè)具體的濾波器而言，很難達(dá)到理論上的極限精度.

當(dāng)對(duì)噪聲統(tǒng)計(jì)特性有準(zhǔn)確的先驗(yàn)知識(shí)時(shí)，基于理想模型設(shè)計(jì)的濾波器性能將隨著測(cè)量的增多而提高.但是，如果噪聲統(tǒng)計(jì)信息未知或不準(zhǔn)確，則隨著測(cè)量的增多，濾波性能可能會(huì)下降.應(yīng)對(duì)這種情況，需要設(shè)計(jì)一種有效的濾波算法來(lái)自適應(yīng)地選擇合適的測(cè)量值以獲得良好的估計(jì)精度.

2 并行擴(kuò)展卡爾曼濾波器

為了使多源測(cè)量融合技術(shù)適用于具有潛在的噪聲統(tǒng)計(jì)不確定性的非線性系統(tǒng)，提出一種PEKF算法，通過(guò)殘差來(lái)計(jì)算相應(yīng)的權(quán)重，來(lái)降低受干擾測(cè)量在濾波器估計(jì)中的比重，提高其他測(cè)量所占的比重從而降低干擾帶來(lái)的影響.詳細(xì)步驟如下所述.

步驟1.初始化

對(duì)于存在N個(gè)節(jié)點(diǎn)的系統(tǒng)，設(shè)置N+1個(gè)并行濾波器.狀態(tài)向量初始化為

(12)

(13)

(14)

(15)

(16)

(17)

(18)

(19)

(20)

步驟2.并行濾波

(21)

(22)

(23)

(24)

從式(21)～(24)中不難看出，與第i個(gè)節(jié)點(diǎn)相關(guān)聯(lián)的測(cè)量不包括在第i個(gè)并行濾波器的測(cè)量子集中，而第(N+1)個(gè)濾波器利用網(wǎng)絡(luò)中所有可用的測(cè)量值.濾波器采用擴(kuò)展卡爾曼濾波(EKF)方法設(shè)計(jì)實(shí)現(xiàn)，濾波方程不再贅述.

步驟3.權(quán)重更新

(25)

并且

(26)

式(20)所示的濾波器初始權(quán)重表示每一個(gè)并行濾波器初始時(shí)具有相同的權(quán)重，后續(xù)濾波器工作中根據(jù)測(cè)量殘差進(jìn)行調(diào)節(jié)更新.但是如果存在與各并行濾波器重要性相關(guān)的先驗(yàn)知識(shí)，可以根據(jù)先驗(yàn)知識(shí)設(shè)置不同的濾波器權(quán)重初值.

如(25)和(26)所示的自適應(yīng)率的收斂性可以用類似于文獻(xiàn)[14]方法來(lái)證明.隨著濾波迭代計(jì)算過(guò)程的進(jìn)行，上述自適應(yīng)率為受不確定性影響相對(duì)較小性能更好的濾波器提供更高的權(quán)重，而為受不確定性影響相對(duì)較大性能最低的濾波器權(quán)重幾乎為零.

步驟4.狀態(tài)組合

PEKF算法的最后一步是根據(jù)權(quán)重來(lái)組合并行濾波器的估計(jì)值.完整的狀態(tài)估計(jì)僅用于輸出，在PEKF算法中不做遞歸使用.由于不同并聯(lián)濾波器的狀態(tài)向量維數(shù)不同，采用歸一法計(jì)算局部權(quán)值[15].第i個(gè)節(jié)點(diǎn)的局部權(quán)重計(jì)算如下:

(27)

(28)

3 自主衛(wèi)星星座導(dǎo)航

本文所研究的星座自主導(dǎo)航系統(tǒng)的任務(wù)是利用衛(wèi)星間距離測(cè)量的時(shí)間序列確定地球衛(wèi)星和月球衛(wèi)星的絕對(duì)位置.通過(guò)月球衛(wèi)星和多顆地球衛(wèi)星建立導(dǎo)航網(wǎng)絡(luò)，距離的測(cè)量基于星間鏈路(ISLs)方法.星座如果有足夠精確的測(cè)量，可以達(dá)到較高的定位精度. ISLs方法容易受到外界干擾，使得噪聲統(tǒng)計(jì)特性存在不確定性，這也是本文研究所要解決的問(wèn)題.圖2所示為一個(gè)典型的星間鏈路星座的結(jié)構(gòu)，其中包含了3個(gè)地球衛(wèi)星和1個(gè)月球衛(wèi)星.

圖2 星間鏈路星座示意圖Fig.2 Concept of satellite constellation with inter-satellite links

對(duì)于由N顆衛(wèi)星組成的星座，在(1)和(3)中描述了自主導(dǎo)航系統(tǒng)的模型結(jié)構(gòu).對(duì)于第i個(gè)衛(wèi)星，狀態(tài)向量xi,k由相應(yīng)的衛(wèi)星位置和速度矢量組成.

(29)

其中ri，k和vi，k是地心慣性系(ECI)中定義的位置和速度矢量.對(duì)于地球衛(wèi)星，狀態(tài)轉(zhuǎn)移函數(shù)建模如下[16]

(30)

其中T是軌道外推的步長(zhǎng)，μ是地球的引力常數(shù)，p(ri,k-1)表示衛(wèi)星的攝動(dòng)加速度.攝動(dòng)加速度包括了地球非球形質(zhì)量分布的引力加速度、大氣阻力、太陽(yáng)輻射壓力和月球太陽(yáng)引力攝動(dòng)等.月球衛(wèi)星的狀態(tài)轉(zhuǎn)移函數(shù)與地球衛(wèi)星的狀態(tài)轉(zhuǎn)移函數(shù)形式相似，這里省略.

利用地球衛(wèi)星與地球衛(wèi)星、地球衛(wèi)星與月球衛(wèi)星之間的距離測(cè)量值進(jìn)行融合導(dǎo)航.測(cè)量函數(shù)如下

(31)

式中‖·‖表示歐幾里德范數(shù).

4 仿真結(jié)果

通過(guò)數(shù)值仿真測(cè)試PEKF算法在自主衛(wèi)星星座導(dǎo)航系統(tǒng)中的性能，并與EKF和傳統(tǒng)MMAE算法比較來(lái)說(shuō)明結(jié)果.

以4顆衛(wèi)星組成的星座自主導(dǎo)航系統(tǒng)為例：3顆衛(wèi)星在地球軌道，另一顆衛(wèi)星在月球軌道.衛(wèi)星的初始軌道要素見(jiàn)表1.表中月球衛(wèi)星的軌道要素是相對(duì)于月球的.假設(shè)衛(wèi)星在小偏心率的近圓軌道飛行.

表1 星座衛(wèi)星的初始軌道要素Tab.1 Initial orbit elements of the satellites in constellation

通過(guò)初始軌道要素，利用高精度軌道仿真器產(chǎn)生衛(wèi)星的真實(shí)軌道數(shù)據(jù)，并根據(jù)真實(shí)軌道模擬產(chǎn)生星間距離測(cè)量結(jié)果.星間距離測(cè)量的更新周期為10 s，在正常情況下，測(cè)量噪聲為高斯噪聲，標(biāo)準(zhǔn)差為距離20 m，速度0.005 m/s.干擾條件下，對(duì)于任意一個(gè)地球衛(wèi)星的測(cè)量噪聲而言，增大與其相關(guān)的距離測(cè)量噪聲，初始導(dǎo)航誤差的標(biāo)準(zhǔn)差設(shè)置為三軸相等，均為位置50 m.模擬運(yùn)行時(shí)間為2 天.

考慮到地球附近的衛(wèi)星易受到干擾影響，在PEKF算法中設(shè)置了4個(gè)并行濾波器，其中一個(gè)濾波器設(shè)計(jì)為利用所有的測(cè)量，另外3個(gè)濾波器設(shè)計(jì)成與地球衛(wèi)星和月球衛(wèi)星的不同組合相關(guān)的測(cè)量子集.為了進(jìn)行公平的比較，對(duì)EKF，MMAE和PEKF設(shè)置相同的初始狀態(tài)誤差協(xié)方差和相同的過(guò)程噪聲協(xié)方差.

對(duì)于EKF和PEKF，測(cè)量噪聲協(xié)方差設(shè)置為標(biāo)稱值20 m.對(duì)于MMAE中的4個(gè)并行濾波器，測(cè)量噪聲的標(biāo)準(zhǔn)差分別設(shè)置為10 m、20 m、30 m和40 m.

無(wú)干擾條件下，EKF、MMAE和PEKF的位置估計(jì)誤差曲線如圖3～圖5所示，其中虛線表示3σ誤差界，實(shí)線表示估計(jì)誤差.

圖3 EKF的位置估計(jì)誤差Fig.3 Position estimation error curve of EKF

圖4 MMAE的位置估計(jì)誤差Fig.4 Position estimation error curve of MMAE

圖5 PEKF的位置估計(jì)誤差Fig.5 Position estimation error curve of PEKF

從圖中可以看出，在沒(méi)有噪聲干擾的情況下PEKF的估計(jì)結(jié)果優(yōu)于EKF.MMAE算法能夠在一定范圍內(nèi)適應(yīng)測(cè)量噪聲協(xié)方差的變化，所以MMAE算法的性能介于其他兩種算法之間.但是總體而言三者區(qū)別并不大.

為了評(píng)估所提出的濾波算法對(duì)噪聲統(tǒng)計(jì)不確定性的適應(yīng)性，引入干擾.對(duì)某一個(gè)地球衛(wèi)星，引入標(biāo)準(zhǔn)差為0.08 m/s的白噪聲作為干擾噪聲.EKF、MMAE和PEKF的位置估計(jì)誤差曲線如圖6～圖9所示.

圖6 EKF的位置估計(jì)誤差Fig.6 Position estimation error curve of EKF

圖7 MMAE的位置估計(jì)誤差Fig.7 Position estimation error curve of MMAE

圖8 PEKF的位置估計(jì)誤差Fig.8 Position estimation error curve of PEKF

仿真結(jié)果顯示存在干擾時(shí)EKF和MMAE的估計(jì)誤差顯著增加.EKF的估計(jì)誤差大于3σ，表明EKF在存在模型不確定性的情況下濾波器性能受到影響.由于隨機(jī)干擾不包括在預(yù)先設(shè)置的模型集中，MMAE的性能也不理想.事實(shí)上，很難在MMAE的模型集中準(zhǔn)確地涵蓋所有可能的模型，因?yàn)楹茈y獲得所有可能的不確定性的充分先驗(yàn)知識(shí)，而且不確定參數(shù)組合的數(shù)目受到應(yīng)用系統(tǒng)的限制.相比EKF和MMAE，PEKF的性能更好，因?yàn)榉抡嫜芯恐胁皇懿淮_定性影響的測(cè)量子集對(duì)應(yīng)的并行濾波器在算法中占主導(dǎo)地位，說(shuō)明了權(quán)重更新和狀態(tài)組合方法在干擾條件下的有效性.

圖9展示了PEKF算法中的權(quán)重更新過(guò)程，圖中可以看出，在經(jīng)過(guò)一定時(shí)間的迭代后，引入噪聲干擾的測(cè)量子集相關(guān)的權(quán)重降低，濾波所使用的測(cè)量信息的絕大部分來(lái)源來(lái)自無(wú)干擾的測(cè)量子集，表現(xiàn)了測(cè)量調(diào)度能力.

圖9 PEKF的權(quán)重更新Fig.9 Weight update of PEKF

兩種仿真條件下位置估計(jì)誤差的均方根誤差如圖10所示.

圖10 無(wú)干擾和有干擾條件下三種算法的均方根誤差對(duì)比Fig.10 Comparison of average RMS error for three filters in different conditions

圖中能明顯看出，無(wú)干擾條件下PEKF的優(yōu)勢(shì)并不大，但是在干擾條件下，PEKF表現(xiàn)出了明顯的性能優(yōu)勢(shì).

仿真算例參數(shù)見(jiàn)表2.

表2 仿真算例參數(shù)Tab.2 Parameters of simulation

5 結(jié) 論

本文提出一種基于MMAE體系結(jié)構(gòu)的并行擴(kuò)展卡爾曼濾波器(PEKF).重點(diǎn)解決當(dāng)測(cè)量受到干擾，導(dǎo)致噪聲統(tǒng)計(jì)特性產(chǎn)生不確定時(shí)的測(cè)量調(diào)度問(wèn)題.通過(guò)調(diào)節(jié)各測(cè)量的權(quán)重提高了導(dǎo)航系統(tǒng)的可靠性.仿真結(jié)果說(shuō)明了以下幾點(diǎn)：

1) 算法能夠有效地選擇測(cè)量的最佳子集，從而降低了濾波器對(duì)測(cè)量噪聲統(tǒng)計(jì)特性不確定性的敏感度.

2) 由于消除了噪聲統(tǒng)計(jì)不確定性的不利影響，相對(duì)于傳統(tǒng)EKF而言，采用PEKF明顯提高了導(dǎo)航系統(tǒng)的性能.

3) 當(dāng)MMAE算法中預(yù)先設(shè)置的模型集不能覆蓋所有可能的噪聲特性時(shí)， PEKF的性能優(yōu)于MMAE.