王　騫

(中國運(yùn)載火箭技術(shù)研究院 研究發(fā)展中心，北京　100076)

?

基于指數(shù)加權(quán)卡爾曼濾波的組合定姿算法

王騫

(中國運(yùn)載火箭技術(shù)研究院 研究發(fā)展中心，北京100076)

星敏感器/陀螺組合定姿系統(tǒng)利用擴(kuò)展卡爾曼濾波進(jìn)行數(shù)據(jù)融合，但在軌運(yùn)行時(shí)星敏感器的量測(cè)噪聲模型不斷變化，由于擴(kuò)展卡爾曼濾波不能自適應(yīng)調(diào)整，導(dǎo)致濾波器無法正常工作；基于模糊邏輯提出了一種指數(shù)加權(quán)卡爾曼濾波算法，實(shí)時(shí)監(jiān)測(cè)系統(tǒng)濾波殘差，利用模糊邏輯計(jì)算指數(shù)因子，自適應(yīng)更新濾波器的量測(cè)噪聲模型，從而有效地抑制了濾波器發(fā)散，提高了濾波精度;通過以TMS320C6713為處理器的DSP系統(tǒng)進(jìn)行的半物理仿真實(shí)驗(yàn)，驗(yàn)證了指數(shù)加權(quán)卡爾曼濾波算法的有效性。

卡爾曼濾波; 指數(shù)加權(quán);模糊邏輯; 量測(cè)噪聲;半物理仿真

0　引言

陀螺能以較高的頻率提供角速度和姿態(tài)角信息，并具有非常好的短期精度和穩(wěn)定性，但是由于陀螺漂移的存在，長(zhǎng)期工作由角速度積分得到的姿態(tài)角會(huì)有較大的累積誤差[1]。星敏感器利用對(duì)星體的觀測(cè)，通過與導(dǎo)航星庫進(jìn)行比對(duì)來確定姿態(tài)，精度高且無累積誤差[2]，但是輸出頻率較低。星敏感器/陀螺組合定姿系統(tǒng)利用擴(kuò)展卡爾曼濾波通過星敏感器提供的高精度姿態(tài)信息實(shí)時(shí)對(duì)陀螺漂移進(jìn)行校正并補(bǔ)償，使二者優(yōu)勢(shì)互補(bǔ)，從而實(shí)現(xiàn)高精度、高頻率、長(zhǎng)期穩(wěn)定工作的定姿系統(tǒng)[3]。

擴(kuò)展卡爾曼濾波通過系統(tǒng)模型和噪聲模型建立狀態(tài)方程和量測(cè)方程。系統(tǒng)模型可以通過運(yùn)動(dòng)學(xué)方程推導(dǎo)，而噪聲模型只能通過實(shí)驗(yàn)數(shù)據(jù)估計(jì)。在軌運(yùn)行時(shí)星敏感器的量測(cè)噪聲模型不斷變化，擴(kuò)展卡爾曼濾波由于不能進(jìn)行自適應(yīng)調(diào)整，導(dǎo)致濾波精度下降甚至濾波發(fā)散。眾多學(xué)者也提出過很多自適應(yīng)改進(jìn)算法。文獻(xiàn)[4]通過衰減因子調(diào)整協(xié)方差陣，增加了新息的權(quán)重，克服了數(shù)據(jù)飽和的現(xiàn)象，但只能對(duì)動(dòng)力學(xué)模型進(jìn)行修正。文獻(xiàn)[5]利用目標(biāo)的機(jī)動(dòng)信息計(jì)算縮放因子，修正過程噪聲模型，但會(huì)造成決策滯后，且對(duì)量測(cè)噪聲模型無法進(jìn)行調(diào)整。文獻(xiàn)[6]針對(duì)目標(biāo)動(dòng)態(tài)定位時(shí)系統(tǒng)噪聲統(tǒng)計(jì)特性不明確的問題，應(yīng)用Sage自適應(yīng)濾波方法(又稱極大后驗(yàn)估計(jì))，根據(jù)每次測(cè)量值所獲得的新信息，推算出當(dāng)前時(shí)刻的系統(tǒng)噪聲統(tǒng)計(jì)特性的估算值，并求出狀態(tài)估值。文獻(xiàn)[7]在進(jìn)行狀態(tài)濾波的同時(shí)對(duì)系統(tǒng)噪聲的統(tǒng)計(jì)特性進(jìn)行在線估計(jì)，確保算法收斂于期望用戶，提高了檢測(cè)器在動(dòng)態(tài)環(huán)境下的跟蹤性能。

擴(kuò)展卡爾曼濾波包括預(yù)測(cè)和修正兩個(gè)過程，在軌運(yùn)行量測(cè)噪聲的變化將直接導(dǎo)致濾波器修正過程出現(xiàn)錯(cuò)誤，而以上文獻(xiàn)對(duì)動(dòng)力學(xué)模型和過程噪聲的調(diào)整僅能糾正預(yù)測(cè)過程出現(xiàn)的錯(cuò)誤，對(duì)修正過程出現(xiàn)的錯(cuò)誤無能為力。針對(duì)此問題，本文提出一種基于模糊邏輯的指數(shù)加權(quán)卡爾曼濾波算法，通過模糊邏輯系統(tǒng)計(jì)算指數(shù)因子，從而實(shí)時(shí)更新量測(cè)噪聲模型，使之不斷與實(shí)際模型進(jìn)行匹配，有效調(diào)整了濾波器的修正過程，使其能夠穩(wěn)定工作。

1　指數(shù)加權(quán)卡爾曼濾波

該算法基本框圖如圖1所示。

圖1　基于模糊理論的指數(shù)加權(quán)卡爾曼濾波算法框圖

定義指數(shù)因子eα，量測(cè)噪聲陣更新方程為Rk=eα·Rk-1，通過模糊邏輯系統(tǒng)計(jì)算出α來調(diào)整Rk。

由此可得指數(shù)加權(quán)卡爾曼濾波的遞推方程為(1)式～(6)式：

狀態(tài)變量的一步預(yù)測(cè)：

(1)

協(xié)方差陣的一步預(yù)測(cè)：

(2)

量測(cè)噪聲陣的更新：

(3)

濾波增益的更新：

(4)

狀態(tài)變量的更新：

(5)

協(xié)方差陣的更新：

(6)

指數(shù)因子α與估計(jì)量測(cè)噪聲偏離實(shí)際量測(cè)噪聲的程度有關(guān)，而比較濾波殘差的實(shí)際值與理論值可以描述這種偏離程度，濾波殘差rk的定義為(7)式：

(7)

殘差方差陣的理論值為(8)式：

(8)

2　模糊邏輯系統(tǒng)

模糊邏輯系統(tǒng)是一種基于知識(shí)并運(yùn)用語言變量的系統(tǒng)，與其它傳統(tǒng)的自適應(yīng)方法相比，模糊邏輯系統(tǒng)的優(yōu)點(diǎn)在于它的設(shè)計(jì)簡(jiǎn)單并充分運(yùn)用了被控制系統(tǒng)的相關(guān)知識(shí)[8]。

當(dāng)實(shí)際量測(cè)噪聲大于估計(jì)量測(cè)噪聲時(shí)，Trace(Sr)>Trace(Pr)，模糊邏輯系統(tǒng)的輸入input>0，應(yīng)使輸出α>0，適當(dāng)增大估計(jì)量測(cè)噪聲；當(dāng)實(shí)際量測(cè)噪聲小于估計(jì)量測(cè)噪聲時(shí)，Trace(Sr)

模糊邏輯系統(tǒng)的作用為保持估計(jì)量測(cè)噪聲與實(shí)際量測(cè)噪聲的匹配，即應(yīng)使模糊邏輯系統(tǒng)的輸入input保持在0附近，以維持量測(cè)噪聲模型的準(zhǔn)確性。

首先對(duì)模糊邏輯系統(tǒng)的輸入進(jìn)行模糊化，其隸屬度函數(shù)如圖2所示，即三角形隸屬度函數(shù)，其中橫坐標(biāo)為input(無量綱變量，單位為1)，縱坐標(biāo)為隸屬度membership(無量綱變量，單位為1)，分別定義為模糊集合less，表示基本小于0；模糊集合equ，表示在0附近；模糊集合more，表示基本大于0。

圖2　輸入隸屬度函數(shù)

解模糊化的方法這里采用重心法，即模糊控制系統(tǒng)的輸出α取為從輸入隸屬度函數(shù)上得到的隸屬度值在輸出隸屬度函數(shù)與相應(yīng)集合曲線及橫軸圍成的三角形重心的橫坐標(biāo)。根據(jù)這一原則，通過不斷仿真實(shí)驗(yàn)，確定了輸出隸屬度函數(shù)如圖3所示，其中橫坐標(biāo)為output(無量綱變量，單位為1)，縱坐標(biāo)為隸屬度membership(無量綱變量，單位為1)：

圖3　輸出隸屬度函數(shù)

模糊控制系統(tǒng)的輸入與3個(gè)模糊集合的位置關(guān)系共有5種情況如式(9)所示：

(9)

所以通過重心法解模糊化來計(jì)算模糊控制系統(tǒng)的輸出α也對(duì)應(yīng)有5條解算規(guī)則如式(10)所示：

(10)

例如符號(hào)XCG(Δless∪Δequ)表示與輸出隸屬度函數(shù)的less曲線和equ曲線分別圍成的三角形并集的重心橫坐標(biāo)。

3　半物理仿真

為了評(píng)價(jià)本文提出的指數(shù)加權(quán)卡爾曼濾波算法的性能，我們利用TI公司型號(hào)為TMS320C6713的DSP實(shí)現(xiàn)了該算法，陀螺和星敏感器分別通過SPI和HPI接口與DSP通信，對(duì)該算法進(jìn)行了半物理仿真。

組合定姿系統(tǒng)的狀態(tài)向量取為(11)式：

(11)

其中:Δq1,Δq2,Δq3為估計(jì)四元數(shù)與真實(shí)四元數(shù)之間旋轉(zhuǎn)偏差四元數(shù)的矢量部分，Δb1,Δb2,Δb3為陀螺三軸漂移的估計(jì)值與真實(shí)值之間的差值。

量測(cè)向量取為(12)式：

(12)

星敏感器的量測(cè)噪聲來源主要有暗電流噪聲、讀出噪聲、光子散粒噪聲和非均勻性噪聲，而影響這些噪聲最主要的外部因素是溫度[9]。星敏感器在軌運(yùn)行時(shí)由于受到太陽輻射、地球紅外輻射和地球陽光反照等因素的影響，工作環(huán)境的溫度有可能產(chǎn)生漸變或者突變，所以可能導(dǎo)致量測(cè)噪聲有漸變或突變的情況[10]。為了符合實(shí)際工作環(huán)境的需求，本文對(duì)星敏感器量測(cè)噪聲漸變和突變兩種情況都進(jìn)行了仿真測(cè)試。

3.1星敏感器量測(cè)噪聲從10″漸變至60″

整個(gè)過程的姿態(tài)角誤差曲線和角速度誤差曲線如圖4和圖5所示。

圖4　量測(cè)噪聲漸變時(shí)姿態(tài)角誤差曲線

圖5　量測(cè)噪聲漸變時(shí)角速度誤差曲線

圖中實(shí)線指數(shù)加權(quán)卡爾曼濾波結(jié)果，虛線為擴(kuò)展卡爾曼濾波結(jié)果。對(duì)比可知隨著量測(cè)噪聲的漸變擴(kuò)展卡爾曼濾波在整個(gè)過程中誤差逐漸增大，有發(fā)散的趨勢(shì)，而指數(shù)加權(quán)卡爾曼濾波在整個(gè)過程中誤差很小且保持穩(wěn)定。

圖6為指數(shù)加權(quán)卡爾曼濾波中指數(shù)因子在整個(gè)量測(cè)噪聲漸變過程中的變化曲線。

圖6　指數(shù)加權(quán)因子α曲線

由圖可知整個(gè)過程中α始終穩(wěn)定在0附近，證明噪聲模型匹配準(zhǔn)確。

3.2星敏感器量測(cè)噪聲在750秒時(shí)從10″突變至60″

整個(gè)過程的姿態(tài)角誤差曲線和角速度誤差曲線如圖7和圖8所示：

圖7　量測(cè)噪聲突變時(shí)姿態(tài)角誤差曲線

圖8　量測(cè)噪聲突變時(shí)角速度誤差曲線

由圖可知在750 s以前兩種方法濾波精度相差不大，在750 s以后由于實(shí)際量測(cè)噪聲突然變大，擴(kuò)展卡爾曼濾波的誤差隨之增大且有發(fā)散趨勢(shì)，指數(shù)加權(quán)卡爾曼濾波的誤差則沒有太大影響，能夠全程保持穩(wěn)定。

圖9為指數(shù)加權(quán)卡爾曼濾波中指數(shù)因子在整個(gè)過程中的變化曲線。

圖9　指數(shù)加權(quán)因子α曲線

由圖可知在750 s實(shí)際量測(cè)噪聲發(fā)生突變的同時(shí)α也有一個(gè)突變，隨后又逐漸回到0，并重新穩(wěn)定在0附近，可見指數(shù)加權(quán)卡爾曼濾波有很強(qiáng)的自適應(yīng)調(diào)整能力。

表1為兩種方法的誤差統(tǒng)計(jì)數(shù)據(jù)。

表1　兩種方法的誤差統(tǒng)計(jì)數(shù)據(jù)

對(duì)比兩組數(shù)據(jù)可知指數(shù)加權(quán)卡爾曼濾波的精度要高于擴(kuò)展卡爾曼濾波，且穩(wěn)定性要明顯優(yōu)于擴(kuò)展卡爾曼濾波。

4　結(jié)論

本文提出了指數(shù)加權(quán)卡爾曼濾波算法，在每個(gè)濾波周期實(shí)時(shí)監(jiān)控系統(tǒng)殘差的變化，利用模糊邏輯系統(tǒng)輸出指數(shù)調(diào)節(jié)因子α，在線調(diào)整濾波器的量測(cè)噪聲模型，自適應(yīng)地逼近實(shí)際噪聲模型，改善了濾波器的精度和穩(wěn)定性。

對(duì)于星敏感器/慣性陀螺組合定姿系統(tǒng)利用星敏感器在線校正陀螺漂移，其實(shí)際工作環(huán)境量測(cè)噪聲模型未知且不斷變化，本文提出的指數(shù)加權(quán)卡爾曼濾波算法很好地解決了這一問題，通過監(jiān)控星敏感器量測(cè)值與一步預(yù)測(cè)值之間的微小旋轉(zhuǎn)四元數(shù)，利用模糊邏輯系統(tǒng)調(diào)節(jié)濾波器量測(cè)噪聲模型，增強(qiáng)了濾波器的性能，有效地防止了濾波器發(fā)散。本文的研究對(duì)提高星敏感器/陀螺組合定姿系統(tǒng)的精度具有重要意義。

[1] Georgy J,Noureldin A,Korenberg M J,et al. Modeling the stochastic drift of a MEMS-based gyroscope in gyro/odometer/GPS integrated navigation[J]. IEEE Transactions on Intelligent Transportation Systems,2010,11(4): 856-872.

[2] Sigel D A,Wettergreen D. Star tracker celestial localization system for a Lunar rover[A]. Proceedings of the 2007 IEEE/RSJ International Conference on Intelligent Robots and Systems[C]. CA. 2007:2851-2856.

[3] 邢廣華，劉建業(yè)，孫永榮,等.多信息融合組合導(dǎo)航半物理仿真系統(tǒng)設(shè)計(jì)與實(shí)現(xiàn)[J].航天控制，2005，23(2)：64-68.

[4] 馬忠孝，劉宗玉，陳明.基于模糊邏輯的自適應(yīng)卡爾曼濾波在GPS/INS組合導(dǎo)航中的應(yīng)用[J].信息與控制，2006，35(4)：457-461.

[5] 顧潮琪，周德云，曲藝海.一種精確跟蹤機(jī)動(dòng)目標(biāo)的濾波算法的研究[J].電光與控制，2007，14(2)：8-11.

[6] 宋悅銘,劉晶紅,沈宏海，等. 航空光電平臺(tái)目標(biāo)定位自適應(yīng)卡爾曼濾波方法的應(yīng)用研究[J]. 航空精密制造技術(shù),2011,47(1): 47-54.

[7] 高維廷,李輝,翟海天. 基于自適應(yīng)卡爾曼濾波的盲多用戶檢測(cè)算法[J]. 計(jì)算機(jī)工程,2011,37(12): 104-106.

[8] 童正環(huán),江穎,張佩,等. 自適應(yīng)模糊控制器的實(shí)現(xiàn)[J]. 儀器儀表學(xué)報(bào),2006,27(6): 1939-1940.

[9] 劉海波,譚吉春,郝云彩,等. 環(huán)境溫度對(duì)星敏感器測(cè)量精度的影響[J]. 光電工程,2008,35(12): 40-44.

[10] 李學(xué)夔,譚海曙,于昕梅,等. 星敏感器定位精度噪聲影響因素研究[J]. 光電子技術(shù),2010,30(2): 97-101.

Integrated Attitude Determination Algorithm Based on Kalman Filter with Weighted Index

Wang Qian

(Research and Development Center，China Academy of Launch Vehicle Technology,Beijing100076，China)

Integrated attitude determination system of star sensor/gyroscope completes data fusion with Extended Kalman filter. Measurement noise model on-orbit of star sensor is variable. Extended Kalman filter is unadaptable so that the filter is out of order. This paper presents Kalman filter with weighted index based on fuzzy logic. The algorithm monitors residuals real time,exports index factor by fuzzy logic and then adjusts measurement noise model in order to restrain diffusion and improve precision. The semi-physical simulation results based on DSP system with TMS320C6713 processor validate availability of the algorithm.

Kalman filter;weighted index; fuzzy logic;measurement noise; semi-physical simulation

1671-4598(2016)04-0133-04DOI：10.16526/j.cnki.11-4762/tp.2016.04.040

V249.32

A

2015-08-08；

2015-11-04。

王騫(1986-)，男，黑龍江人，碩士，工程師，主要從事航天器電氣總體設(shè)計(jì)、航天器測(cè)試等方向的研究。