基于并行模型自適應(yīng)濾波的空間目標(biāo)相對(duì)位姿估計(jì)*

熊 凱，魏春嶺，辛優(yōu)美

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

0 引 言

作為典型的非線性系統(tǒng)信息融合方法，擴(kuò)展卡爾曼濾波(EKF)及其改進(jìn)算法在航天器自主導(dǎo)航和姿態(tài)確定中得到廣泛應(yīng)用[1-2]，并已形成標(biāo)準(zhǔn)化算法.在EKF算法設(shè)計(jì)過程中，通常假設(shè)模型中的系統(tǒng)噪聲和測(cè)量噪聲方差是精確已知的.然而，在工程實(shí)踐活動(dòng)中，受應(yīng)用條件和環(huán)境因素的影響，這一假設(shè)往往得不到滿足.對(duì)于本文所研究的空間目標(biāo)相對(duì)位姿確定系統(tǒng)，通過觀察衛(wèi)星上的雙目立體視覺相對(duì)導(dǎo)航敏感器指向目標(biāo)衛(wèi)星進(jìn)行觀測(cè)，受觀察衛(wèi)星平臺(tái)姿態(tài)抖動(dòng)和空間光照條件等因素的影響，敏感器誤差特性往往會(huì)發(fā)生變化，此時(shí)，測(cè)量噪聲方差會(huì)偏離通過地面測(cè)試獲得的標(biāo)稱值.對(duì)于測(cè)量噪聲統(tǒng)計(jì)特性的變化，傳統(tǒng)EKF算法不具備自適應(yīng)能力.在實(shí)際敏感器測(cè)量噪聲方差不同于其標(biāo)稱值的情況下，會(huì)導(dǎo)致EKF性能下降.

針對(duì)噪聲統(tǒng)計(jì)特性不確定性的影響，以往研究中已給出多種策略，主要目的是增強(qiáng)濾波算法的自適應(yīng)能力.舉例來說，魯棒擴(kuò)展卡爾曼濾波(REKF)基于噪聲方差的上界進(jìn)行設(shè)計(jì)，但是，基于先驗(yàn)信息選定的噪聲方差上界往往具有某種保守性，不利于提升濾波器的估計(jì)精度.多模型濾波采用一種更為精巧的方式描述噪聲統(tǒng)計(jì)特性不確定性的影響[3]，具體方法是基于多個(gè)不同的噪聲方差陣建立多個(gè)模型，并基于各個(gè)模型分別設(shè)計(jì)并行濾波器，各個(gè)濾波器實(shí)施并行計(jì)算，最終的狀態(tài)估計(jì)結(jié)果是各個(gè)并行濾波器狀態(tài)估計(jì)值的加權(quán)和.該方法的局限性體現(xiàn)在，為了全面覆蓋噪聲方差的取值范圍，往往需要建立一個(gè)龐大的模型集構(gòu)成空間網(wǎng)格，相應(yīng)地，需要設(shè)計(jì)大量的并行濾波器并同時(shí)實(shí)施解算，較重的計(jì)算負(fù)擔(dān)限制了該方法的應(yīng)用范圍.

近些年來，具有噪聲統(tǒng)計(jì)特性在線估計(jì)能力的自適應(yīng)濾波算法得到廣泛重視[4-5].對(duì)于線性時(shí)不變系統(tǒng)、線性時(shí)變系統(tǒng)和非線性系統(tǒng)，分別給出了多種不同方法，包括自適應(yīng)卡爾曼濾波(AKF)[6]、自適應(yīng)擴(kuò)展卡爾曼濾波(AEKF)[7]、自適應(yīng)無跡卡爾曼濾波(AUKF)[8]，以及自適應(yīng)魯棒平方根容積卡爾曼濾波(ARSCKF)[9-10]等.對(duì)于噪聲統(tǒng)計(jì)特性未知的情況，應(yīng)用以AEKF為代表的自適應(yīng)濾波算法能夠取得較好的效果.但是，在噪聲統(tǒng)計(jì)特性已知的情況下，受噪聲方差在線估計(jì)誤差的影響，AEKF的性能往往不及傳統(tǒng)EKF.

空間目標(biāo)相對(duì)位姿確定系統(tǒng)的敏感器測(cè)量噪聲方差的標(biāo)稱值可通過地面測(cè)試得到，但對(duì)于部分應(yīng)用場(chǎng)景，實(shí)際敏感器測(cè)量噪聲方差會(huì)偏離其標(biāo)稱值.針對(duì)上述問題，本文基于特定的自適應(yīng)率將EKF和AEKF結(jié)合起來，設(shè)計(jì)了一種并行模型自適應(yīng)濾波(PMAF)算法，該方法在測(cè)量噪聲統(tǒng)計(jì)特性不確定性存在和不存在兩種情況下，均能取得較好的狀態(tài)估計(jì)結(jié)果.

本文內(nèi)容安排如下：首先，針對(duì)帶有統(tǒng)計(jì)特性不確定性的非線性狀態(tài)空間模型，設(shè)計(jì)了PMAF方法；其次，給出了空間目標(biāo)相對(duì)位姿確定系統(tǒng)的數(shù)學(xué)表達(dá)式；進(jìn)而，展示了對(duì)比研究和仿真分析的結(jié)果.

1 并行模型自適應(yīng)濾波

考慮如下所示的離散時(shí)間非線性系統(tǒng)

xk=f(xk-1)+wk(1)

yk=h(xk)+vk(2)

其中，xk和yk分別為l維狀態(tài)向量和m維觀測(cè)向量，f(·)和h(·)是連續(xù)可微狀態(tài)轉(zhuǎn)移函數(shù)和測(cè)量函數(shù)，wk和vk分別為l維過程噪聲和m維測(cè)量噪聲.假定wk和vk為不相關(guān)的高斯白噪聲，噪聲統(tǒng)計(jì)特性為

(3)

其中，Qk和Rk為正定對(duì)稱陣，δkj為克拉姆尼克-狄拉克算子，當(dāng)k=j時(shí)，其取值為1，其他情況下取值為0.假定系統(tǒng)噪聲方差Qk是精確已知的，而測(cè)量噪聲方差Rk在應(yīng)用過程中可能會(huì)發(fā)生變化，其取值范圍如下

(6)

以往研究表明，對(duì)于測(cè)量噪聲方差錯(cuò)誤的取值會(huì)顯著降低濾波性能.針對(duì)上述問題，提出一種基于EKF和AEKF并行計(jì)算的PMAF算法，兩個(gè)并行濾波器各分配一個(gè)權(quán)值，通過設(shè)計(jì)自適應(yīng)率調(diào)節(jié)權(quán)值的大小，將兩個(gè)并行濾波器估計(jì)結(jié)果的加權(quán)和作為全局狀態(tài)估計(jì).基于上述設(shè)計(jì)，在測(cè)量噪聲方差的真值等于標(biāo)稱值時(shí)，令EKF在狀態(tài)估計(jì)中起主導(dǎo)作用，相反，在測(cè)量噪聲方差的真值偏離標(biāo)稱值時(shí)，令A(yù)EKF起主導(dǎo)作用，從而揚(yáng)長(zhǎng)避短，充分發(fā)揮EKF和AEKF各自的性能優(yōu)勢(shì).

(7)

(8)

PMAF算法的迭代計(jì)算過程如下：

Step1.并行濾波

對(duì)于k∈{1,2,…,∞},兩個(gè)并行濾波器分別對(duì)狀態(tài)向量和估計(jì)誤差方差進(jìn)行預(yù)測(cè)和更新.狀態(tài)預(yù)測(cè)基于系統(tǒng)模型進(jìn)行

(9)

(10)

(11)

其中，

(12)

(14)

估計(jì)誤差方差根據(jù)下式進(jìn)行更新

兩個(gè)并行濾波器的主要區(qū)別體現(xiàn)在測(cè)量噪聲方差的取值.對(duì)于第1個(gè)并行濾波器，測(cè)量噪聲方差取標(biāo)稱值

(16)

對(duì)于第2個(gè)并行濾波器，通過滑動(dòng)平均法對(duì)測(cè)量噪聲方差進(jìn)行在線估計(jì)

(17)

其中，

(18)

Step2.權(quán)值更新

利用測(cè)量新息更新兩個(gè)并行濾波器的權(quán)值，并進(jìn)行歸一化.

(20)

其中，

(21)

Step3.加權(quán)融合

(22)

在權(quán)值收斂的情況下，PMAF的狀態(tài)估計(jì)值近似等于其中一個(gè)并行濾波器的狀態(tài)估計(jì)值.

所設(shè)計(jì)的PMAF算法結(jié)構(gòu)可歸納如圖1所示，其特色在于：能夠自適應(yīng)地在不同濾波算法之間進(jìn)行切換或折衷，這使其在應(yīng)對(duì)模型不確定性，或敏感器受環(huán)境影響測(cè)量特性發(fā)生變化時(shí)，能夠體現(xiàn)出優(yōu)于EKF或AEKF的性能水平.

圖1 并行模型自適應(yīng)濾波算法結(jié)構(gòu)示意圖Fig.1 Structure of parallel-model adaptive filter algorithm

2 空間目標(biāo)相對(duì)位姿確定

空間目標(biāo)相對(duì)位姿確定系統(tǒng)能夠提供目標(biāo)衛(wèi)星與觀察衛(wèi)星的相對(duì)姿態(tài)和相對(duì)位置信息，是空間操控任務(wù)成功的重要保障，基本方法是以敏感器獲得的目標(biāo)衛(wèi)星特征點(diǎn)位置矢量信息作為觀測(cè)量，以相對(duì)姿態(tài)、慣性角速度、相對(duì)位置和相對(duì)速度作為狀態(tài)向量，利用相對(duì)導(dǎo)航濾波算法，結(jié)合軌道姿態(tài)動(dòng)力學(xué)模型，對(duì)空間目標(biāo)的運(yùn)動(dòng)狀態(tài)進(jìn)行估計(jì).

系統(tǒng)的狀態(tài)向量定義為

受篇幅所限，在此僅給出線性化后的系統(tǒng)誤差方程[12]：

其中，

A(qsf)是根據(jù)四元數(shù)計(jì)算得到的從目標(biāo)衛(wèi)星體坐標(biāo)系到觀察衛(wèi)星體坐標(biāo)系的轉(zhuǎn)換矩陣，矩陣中元素為姿態(tài)角速率的各個(gè)分量ωo是目標(biāo)衛(wèi)星的軌道角速率，Ix、Iy和Iz是目標(biāo)衛(wèi)星的慣量參數(shù)，w表示未建模噪聲.

利用觀察衛(wèi)星上的雙目立體視覺相對(duì)導(dǎo)航敏感器對(duì)目標(biāo)衛(wèi)星上的位置已知的特征點(diǎn)進(jìn)行觀測(cè)，測(cè)量方程可寫為式(25)，相應(yīng)的觀測(cè)矩陣可參見文獻(xiàn)[12]

(25)

圖2 特征點(diǎn)位置矢量測(cè)量示意圖Fig.2 Position vector measurement of feature points

3 仿真結(jié)果

為了說明所提PMAF算法的有效性，通過仿真將其與EKF算法和AEKF算法進(jìn)行了對(duì)比研究.目標(biāo)衛(wèi)星和觀察衛(wèi)星的真實(shí)運(yùn)動(dòng)狀態(tài)通過高精度姿態(tài)軌道仿真器獲得，根據(jù)真實(shí)運(yùn)動(dòng)狀態(tài)數(shù)據(jù)和觀測(cè)模型產(chǎn)生測(cè)量數(shù)據(jù)，利用EKF、AEKF和PMAF這3種不同的濾波方法對(duì)測(cè)量數(shù)據(jù)進(jìn)行處理，產(chǎn)生狀態(tài)變量的估計(jì)值.將狀態(tài)變量的估計(jì)值與真實(shí)運(yùn)動(dòng)狀態(tài)數(shù)據(jù)進(jìn)行比對(duì)，得到狀態(tài)估計(jì)誤差.

假定目標(biāo)衛(wèi)星在半長(zhǎng)軸為6 878 km、軌道傾角為98°的近圓軌道飛行，其慣量參數(shù)為Ix=100 kg·m2，Iy=100 kg·m2，Iz=200 kg·m2,特征點(diǎn)位置矢量在星體系的坐標(biāo)為pin1=[1 0 0]Tm,pin2=[1 1 1]Tm,pin3=[0 1 0]Tm,pin4=[-1 1 -1]Tm.觀察衛(wèi)星在距離目標(biāo)衛(wèi)星100 m左右位置形成自然繞飛，雙目立體視覺相對(duì)導(dǎo)航敏感器的測(cè)量精度為1 cm，數(shù)據(jù)更新頻率為1 Hz.

為了公平起見，三種濾波算法中取同樣的初始誤差方差及系統(tǒng)噪聲方差.測(cè)量噪聲方差的標(biāo)稱值為pk=(0.01 m)2I12×12,AEKF中選擇參數(shù)LR=100,權(quán)值更新過程中，選擇矩陣Sk=Rk.

下面分兩種不同情況進(jìn)行仿真.

Case1：測(cè)量噪聲方差的真值等于標(biāo)稱值

基于不同的隨機(jī)噪聲進(jìn)行20次仿真，得到EKF、AEKF和PMAF的三軸相對(duì)姿態(tài)估計(jì)誤差均方根的范數(shù)，如圖3所示，相應(yīng)地，兩個(gè)并行濾波器的權(quán)值變化曲線如圖4所示.從圖中不難看出，不存在模型不確定性的情況下，PMAF的性能優(yōu)于AEKF，原因在于PMAF中的第1個(gè)并行濾波器不會(huì)受到測(cè)量噪聲方差估計(jì)誤差的影響.基于EKF設(shè)計(jì)的并行濾波器權(quán)值收斂于1，在總體狀態(tài)估計(jì)中起主導(dǎo)作用，結(jié)果符合預(yù)期.

圖3 相對(duì)姿態(tài)估計(jì)誤差(Case 1)Fig.3 Relative attitude estimation errors in normal case

圖4 并行濾波器權(quán)值變化曲線(Case 1)Fig.4 Weights for two parallel filters in normal case

Case2：測(cè)量噪聲方差的真值不等于標(biāo)稱值

在實(shí)際的測(cè)量噪聲方差增大為(0.1 m)2I12×12的情況下，通過20次仿真得到三種算法的姿態(tài)估計(jì)誤差，如圖5所示，相應(yīng)地，兩個(gè)并行濾波器的權(quán)值變化曲線如圖6所示.顯然，存在模型不確定性的情況下，PMAF的性能優(yōu)于EKF，原因在于PMAF中的第2個(gè)并行濾波器具備在線估計(jì)測(cè)量噪聲方差的能力.基于AEKF設(shè)計(jì)的并行濾波器權(quán)值收斂于1，表明所提算法可以自適應(yīng)地選取適當(dāng)算法進(jìn)行處理.

圖5 相對(duì)姿態(tài)估計(jì)誤差(Case 2)Fig.5 Relative attitude estimation errors indisturbance case

圖6 并行濾波器權(quán)值變化曲線(Case 2)Fig.6 Weights for two parallel filters in disturbance case

4 結(jié) 論

本文針對(duì)帶有噪聲統(tǒng)計(jì)特性不確定性的非線性系統(tǒng)，提出一種并行模型自適應(yīng)濾波方法，該方法充分利用了EKF和AEKF算法各自的性能優(yōu)勢(shì)，能夠根據(jù)應(yīng)用環(huán)境的變化選擇適當(dāng)?shù)臑V波算法進(jìn)行加權(quán)融合和自適應(yīng)處理，從而增強(qiáng)系統(tǒng)應(yīng)對(duì)各種復(fù)雜場(chǎng)景的能力.以空間目標(biāo)相對(duì)位姿確定為背景開展了仿真研究，結(jié)果表明，所提PMAF算法能夠在不同仿真條件下取得優(yōu)于傳統(tǒng)EKF或AEKF的濾波性能.

新的濾波算法層出不窮，每種方法都有其特定的優(yōu)勢(shì)和適用范圍，通過并行計(jì)算和智能融合，充分發(fā)揮先進(jìn)濾波方法在對(duì)抗環(huán)境下多源信息處理方面的效能，是未來研究中需要重點(diǎn)關(guān)注的內(nèi)容.