基于自適應(yīng)UKF微型航姿系統(tǒng)噪聲在線估計(jì)

劉　宇，劉　瓊，周　帆，李云梅，向高林

(重慶郵電大學(xué) 光電信息感測(cè)與傳輸技術(shù)重慶市重點(diǎn)實(shí)驗(yàn)室，重慶 400065)

?

基于自適應(yīng)UKF微型航姿系統(tǒng)噪聲在線估計(jì)

劉宇，劉瓊，周帆，李云梅，向高林

(重慶郵電大學(xué) 光電信息感測(cè)與傳輸技術(shù)重慶市重點(diǎn)實(shí)驗(yàn)室，重慶 400065)

摘要：針對(duì)先驗(yàn)噪聲與系統(tǒng)真實(shí)噪聲不符引起標(biāo)準(zhǔn)無跡卡爾曼(unscented Kalman filter，UKF)性能退化的情況，提出一種應(yīng)用于非線性時(shí)變狀態(tài)和參數(shù)聯(lián)合估計(jì)的自適應(yīng)UKF(adaptive unscented Kalman filter，AUKF)算法。根據(jù)新的協(xié)方差矩陣與相應(yīng)估計(jì)值之間存在的誤差，構(gòu)建成本函數(shù)。采用梯度下降法進(jìn)行在線預(yù)估，對(duì)噪聲的協(xié)方差進(jìn)行在線更新并反饋給標(biāo)準(zhǔn)的UKF。實(shí)驗(yàn)和仿真分析表明，與標(biāo)準(zhǔn)UKF相比，自適應(yīng)UKF算法在精度上有較大的提高。對(duì)于時(shí)變?cè)肼晠f(xié)方差不確定時(shí)，自適應(yīng)UKF噪聲在線估計(jì)的魯棒性得到明顯改善，驗(yàn)證了自適應(yīng)UKF噪聲在線估計(jì)模型的準(zhǔn)確性和可行性。

關(guān)鍵詞：無跡卡爾曼；自適應(yīng)UKF；聯(lián)合估計(jì)；成本函數(shù)；梯度下降算法；魯棒性

0引言

由于微型航姿系統(tǒng)具有時(shí)變性和復(fù)雜性的特點(diǎn)，系統(tǒng)為了得到精確的姿態(tài)角信息，準(zhǔn)確估計(jì)狀態(tài)和參數(shù)成為一個(gè)關(guān)鍵。因?yàn)閭鹘y(tǒng)的噪聲估計(jì)方法有諸如僅適應(yīng)線性系統(tǒng)、跟蹤突變的延時(shí)性等缺陷，無法滿足當(dāng)今的工程應(yīng)用，所以，對(duì)航姿系統(tǒng)的噪聲進(jìn)行實(shí)時(shí)估計(jì)成為提高微型姿態(tài)儀精度的關(guān)鍵技術(shù)。近些年來，人們?cè)谠诰€建模領(lǐng)域取得了驕人的成績(jī)，參考模型控制領(lǐng)域正在成為一個(gè)重要的研究方向[1]。在非線性系統(tǒng)，盡管擴(kuò)展卡爾曼濾波(extend Kalman filter，EKF)[2]是運(yùn)用比較普遍的估計(jì)器，但它在估計(jì)過程中仍然有一些缺陷，比如動(dòng)態(tài)狀態(tài)的可導(dǎo)性，對(duì)偏差和散度敏感。無跡卡爾曼(unscented Kalman filter，UKF)直接用非線性動(dòng)態(tài)方程替代線性方程。UKF和EKF具有相同的復(fù)雜度，都是O(L3)階[3]。由于UKF使用的非線性方程并沒有線性化，因此，不需要估計(jì)雅可比行列式或者Hessians矩陣就能實(shí)現(xiàn)二階精度(EKF的精度是一階)。 然而UKF也是卡爾曼濾波器的擴(kuò)充，它和卡爾曼濾波一樣，只有在知道一些假定信息作為先驗(yàn)信息的情況下[4-6]才能實(shí)現(xiàn)優(yōu)異的性能。這些先驗(yàn)信息包括：①精確的參考模型；②噪聲分布的完整信息；③正確的初始條件。但是，因?yàn)橄闰?yàn)信息會(huì)被時(shí)變和不確定的環(huán)境所影響，所以，不能保證很強(qiáng)的穩(wěn)定性。克服上述缺陷的一種有效方式是使用自適應(yīng)算法。國內(nèi)外在自適應(yīng)濾波領(lǐng)域已有許多研究[7-9]。Hu等[7]提出了限制卡爾曼濾波(Kalman filter，KF)記憶的方法，這種方式能根據(jù)最優(yōu)化條件來自適應(yīng)調(diào)整遺忘因子。Maybeck[8]設(shè)計(jì)一個(gè)自適應(yīng)KF，通過最大似然估計(jì)的方法估計(jì)錯(cuò)誤統(tǒng)計(jì)的協(xié)方差矩陣。Loebis等[9]使用了模糊邏輯技術(shù)來更新傳感器噪聲方差。但在實(shí)際中，每次要立即確定協(xié)方差的增量值是困難的。本文提出了自適應(yīng)無跡卡爾曼濾波，它通過新的協(xié)方差矩陣與它相應(yīng)的估計(jì)之間的誤差建立了成本函數(shù)，然后將更新好的協(xié)方差反饋到該系統(tǒng)，實(shí)現(xiàn)系統(tǒng)噪聲的在線估計(jì)。

1UKF濾波算法

離散系統(tǒng)動(dòng)態(tài)空間模型可以描述為

(1)

(2)

(1)—(2) 式中：φk|k-1為tk-1—tk時(shí)刻的一步轉(zhuǎn)移矩陣；Γk|k-1為系統(tǒng)噪聲驅(qū)動(dòng)陣；wk為系統(tǒng)噪聲；ek為協(xié)方差的高斯白噪聲；h(xk)為量測(cè)矩陣。(1)式和(2)式分別描述了系統(tǒng)的狀態(tài)模型和測(cè)量模型，狀態(tài)向量xk和測(cè)量向量yk所包含坐標(biāo)向量的數(shù)量，可根據(jù)實(shí)際的微型航姿系統(tǒng)三維姿態(tài)數(shù)據(jù)確定。由于噪聲對(duì)系統(tǒng)的影響，必須把噪聲項(xiàng)添加到狀態(tài)項(xiàng)。

增廣狀態(tài)表示為

(3)

增廣狀態(tài)的方差表示為

(4)

1)經(jīng)過無損變換(unscented transform，UT)，k時(shí)刻的sigma點(diǎn)和權(quán)值選取。

(5)

(6)

(7)

(8)

(9)

(10)

2)計(jì)算預(yù)測(cè)估計(jì)值。

(11)

3)計(jì)算預(yù)測(cè)協(xié)方差。

(12)

4)計(jì)算測(cè)量值。

(13)

5)計(jì)算預(yù)測(cè)測(cè)量值。

(14)

(15)

7)計(jì)算卡爾曼增益。

(16)

8)更新誤差協(xié)方差。

(17)

9)更新狀態(tài)。

(18)

UKF其實(shí)是卡爾曼濾波的擴(kuò)展，它以UT變換為核心，非線性傳遞均值和協(xié)方差。進(jìn)行UT變換時(shí)可以采用不同的采樣策略，包括確定相應(yīng)權(quán)值，sigma點(diǎn)的個(gè)數(shù)以及位置。

2自適應(yīng)UKF在線估計(jì)算法

2.1成本指標(biāo)函數(shù)

因?yàn)樾孪碓从谡鎸?shí)值的測(cè)量值，目前大多數(shù)算法都是基于新息的自適應(yīng)濾波算法，也就是以最小新息(測(cè)量值和估計(jì)值之差)為目標(biāo)，這只能得到最小新息，并不能反映系統(tǒng)的統(tǒng)計(jì)特性，本文主要是以最小化新息方差的真實(shí)值和估計(jì)值差作為自適應(yīng)濾波的指標(biāo)函數(shù)，用濾波更新中產(chǎn)生的真實(shí)值和估計(jì)值差值來更新濾波器中的參數(shù)。

(19)

(19)式中：N是估計(jì)窗口的尺度因子；vk是新息，也稱為殘差。vk可以描述為

(20)

(20)式中：yk是在k時(shí)刻的測(cè)量值；yk-1是UKF的估計(jì)值?？梢酝ㄟ^標(biāo)準(zhǔn)UKF更新公式知道，新息方差的UKF估計(jì)值為

(21)

自適應(yīng)參數(shù)R的調(diào)整標(biāo)準(zhǔn)是使指標(biāo)函數(shù)最小，因ΔSk對(duì)系統(tǒng)統(tǒng)計(jì)特性的變化更加敏感，自適應(yīng)的成本指標(biāo)函數(shù)表示為

(22)

2.2自適應(yīng)UKF

實(shí)際上，方差矩陣Q和R是決定精度和穩(wěn)定性的因素，因此，選擇協(xié)方差的對(duì)角元素去自適應(yīng)更新過程噪聲Q。不失一般性，R也可以通過該方法更新。Whittake準(zhǔn)則是一種比較常規(guī)的自適應(yīng)參數(shù)更新算法，方法簡(jiǎn)單，計(jì)算量少。此方法就是基于梯度下降算法，將成本函數(shù)下降最快的方向作為參數(shù)更新的方向。即

(23)

(24)

根據(jù)(23)式，可以得到遞歸機(jī)制

(25)

(25)式中，T為恒定時(shí)間間隔。(25)式需要結(jié)合常規(guī)的UKF來實(shí)現(xiàn)自適應(yīng)UKF。由(22)式可得

(26)

(27)

根據(jù)(21)式，可以推導(dǎo)出

(28)

1)初始化。

(29)

sigma點(diǎn)的導(dǎo)數(shù)為

(30)

(31)

2)測(cè)量更新。

(32)

(32)式中，h為轉(zhuǎn)化矩陣。

3)梯度預(yù)測(cè)。

(33)

(34)

2.3自適應(yīng)UKF算法復(fù)雜度和收斂性分析

對(duì)于一個(gè)n維系統(tǒng)，標(biāo)準(zhǔn)UKF與自適應(yīng)UKF的計(jì)算量在同一個(gè)數(shù)量階，它們的算法復(fù)雜度都是O(n3)，自適應(yīng)UKF算法收斂性可以根據(jù)濾波器收斂性判別依據(jù)進(jìn)行討論，如果濾波器發(fā)散，誤差協(xié)方差陣無界，這時(shí)實(shí)際估計(jì)誤差往往比理論預(yù)計(jì)的誤差大很多倍。這時(shí)理論預(yù)測(cè)誤差遠(yuǎn)遠(yuǎn)小于實(shí)際誤差，根據(jù)這樣的原理，可以利用信息序列構(gòu)建一個(gè)變量，來判斷濾波器是否發(fā)散。發(fā)散的判別依據(jù)為

(35)

(35)式的左邊是信息的平方和，反映的是實(shí)際估計(jì)誤差，它只能通過測(cè)試數(shù)據(jù)得到；右邊與信息序列的跡相關(guān)，反映的是理論預(yù)計(jì)誤差。當(dāng)不等式成立時(shí)，濾波器收斂，如果不等式不成立，就說明理論誤差遠(yuǎn)遠(yuǎn)小于實(shí)際誤差，說明該濾波器發(fā)散，這時(shí)要及時(shí)地調(diào)整新息協(xié)方差陣。

3系統(tǒng)試驗(yàn)及分析

3.1仿真平臺(tái)

本論文提出的自適應(yīng)UKF運(yùn)用在自主研制的微型航姿系統(tǒng)中，如圖1所示。微型航姿系統(tǒng)主要由三軸陀螺儀、三軸加速度和三軸磁強(qiáng)計(jì)組成，根據(jù)慣性導(dǎo)航的相關(guān)理論，采用東北天導(dǎo)航坐標(biāo)系描述載體姿態(tài)信息重要模型。

3.2微型航姿系統(tǒng)的測(cè)量的系統(tǒng)結(jié)構(gòu)

本文提出的是基于自適應(yīng)UKF噪聲的在線估計(jì)，系統(tǒng)利用加速度和磁力計(jì)計(jì)算出姿態(tài)，將陀螺的輸出反饋到動(dòng)態(tài)解算過程中，利用自適應(yīng)UKF實(shí)現(xiàn)對(duì)磁力計(jì)和加速度的噪聲估計(jì)，同時(shí)利用磁力計(jì)和加速度計(jì)對(duì)陀螺的累積誤差進(jìn)行在線預(yù)估。微型航姿系統(tǒng)的數(shù)學(xué)模型如圖2所示。

圖1　自主研制的微型姿態(tài)儀Fig.1　Self-developed mini-AHRS

圖2　微型航姿系統(tǒng)測(cè)量的系統(tǒng)結(jié)構(gòu)Fig.2　Measurement system structure of mini-AHRS

3.3微型姿態(tài)儀的狀態(tài)模型

在慣性坐標(biāo)系中，定義姿態(tài)角為(φ，θ，ψ)，其中，φ為航向角，θ為俯仰角，ψ是橫滾角。通過姿態(tài)角的導(dǎo)數(shù)，對(duì)磁力計(jì)和加速度動(dòng)態(tài)測(cè)量的誤差進(jìn)行補(bǔ)償，此時(shí)，在對(duì)自適應(yīng)UKF的狀態(tài)模型進(jìn)行設(shè)計(jì)時(shí)，采用陀螺儀的數(shù)據(jù)。在k+1時(shí)刻的姿態(tài)角可以通過前時(shí)刻角速度在采樣時(shí)間內(nèi)的積分得到，表示為

(36)

(37)

(36)式的左邊是通過磁力計(jì)和加速度進(jìn)行解算的姿態(tài)角，把這個(gè)解算出來的姿態(tài)角作為自適應(yīng)UKF算法的觀測(cè)值。觀測(cè)模型表示為

(38)

由于時(shí)間的積累，根據(jù)UKF的狀態(tài)方程，將前一個(gè)時(shí)刻計(jì)算的結(jié)果作為下一個(gè)時(shí)刻角速度，對(duì)其做一個(gè)簡(jiǎn)單的預(yù)估計(jì)，但是估計(jì)中會(huì)帶來積累誤差，表示為

(39)

(40)

4仿真驗(yàn)證

為了驗(yàn)證自適應(yīng)UKF算法的效果，實(shí)驗(yàn)對(duì)自主研制的微型航姿系統(tǒng)進(jìn)行數(shù)據(jù)采集，將算法運(yùn)用到該系統(tǒng)中。采樣時(shí)間為30 s，采樣頻率為50 Hz。為了驗(yàn)證該算法的優(yōu)越性，將自適應(yīng)UKF算法和普通的UKF算法進(jìn)行對(duì)比，分別做了靜態(tài)和動(dòng)態(tài)實(shí)驗(yàn)。由于橫滾角和俯仰角在算法的原理上一樣，為避免贅述，所以本實(shí)驗(yàn)主要對(duì)航向和俯仰進(jìn)行驗(yàn)證。期望誤差為0時(shí)，靜態(tài)和動(dòng)態(tài)俯仰角和航向角偏差如圖3所示。圖3中曲線表明：靜態(tài)實(shí)驗(yàn)中，標(biāo)準(zhǔn)UKF值呈現(xiàn)出在期望值附近振蕩，自適應(yīng)UKF值呈現(xiàn)出與期望值逼近的趨勢(shì)。采用標(biāo)準(zhǔn)UKF時(shí)，航向角偏差為±1°，靜態(tài)俯仰角偏差為±0.5；采用自適應(yīng)UKF時(shí)，靜態(tài)航向角偏差為0.3°，俯仰角偏差為0.2°。動(dòng)態(tài)實(shí)驗(yàn)中，采用標(biāo)準(zhǔn)UKF時(shí)，航向角偏差為±2°，俯仰角偏差為±0.75°；采用自適應(yīng)UKF時(shí)，航向角偏差在0.8°，俯仰角偏差在0.4°。實(shí)驗(yàn)驗(yàn)證表明，自適應(yīng)UKF算法在動(dòng)靜態(tài)實(shí)驗(yàn)中都比標(biāo)準(zhǔn)UKF算法對(duì)誤差估計(jì)的精度高。UKF不能得到較好的估計(jì)性能；自適應(yīng)UKF算法可以很好地在線估計(jì)噪聲。

圖3　期望誤差為0時(shí)動(dòng)靜態(tài)姿態(tài)角偏差Fig.3　Dynamic and static attitude angle deviation

本實(shí)驗(yàn)采用加速度計(jì)，磁強(qiáng)計(jì)和陀螺儀，靜態(tài)時(shí)，直接把微型航姿系統(tǒng)放在雙軸轉(zhuǎn)臺(tái)上。動(dòng)態(tài)采用的是搖擺實(shí)驗(yàn)，標(biāo)準(zhǔn)UKF和自適應(yīng)UKF算法對(duì)傳感器數(shù)據(jù)融合后，不同期望誤差下靜態(tài)動(dòng)態(tài)仿真結(jié)果如圖4所示。由圖4可知，靜態(tài)實(shí)驗(yàn)中，標(biāo)準(zhǔn)UKF值與自適應(yīng)UKF值都在期望值上下振蕩，但自適應(yīng)UKF值與期望值非常逼近。圖4表明，自適應(yīng)UKF較標(biāo)準(zhǔn)UKF對(duì)噪聲的在線估計(jì)精度高，在靜態(tài)的自適應(yīng)程度較動(dòng)態(tài)高。對(duì)噪聲的精確估計(jì)后，使微型航姿系統(tǒng)的靜態(tài)俯仰角精度為±0.2°，航向角精度為±0.3°，動(dòng)態(tài)俯仰角精度為±0.4°，動(dòng)態(tài)航向角精度為±0.8°，已經(jīng)滿足了工程應(yīng)用的需要。

5結(jié)束語

本論文針對(duì)傳統(tǒng)UKF由于假定的先驗(yàn)噪聲與實(shí)際系統(tǒng)噪聲不符導(dǎo)致精度下降，提出了一種基于自適應(yīng)UKF(adaptive unscented Kalman filter，AUKF)算法，構(gòu)建以新的協(xié)方差矩陣與相應(yīng)估計(jì)值誤差為成本函數(shù)，將更新的協(xié)方差反饋到實(shí)測(cè)系統(tǒng)，對(duì)自主研制的微型航姿系統(tǒng)的噪聲進(jìn)行在線估計(jì)。實(shí)驗(yàn)仿真表明，動(dòng)靜態(tài)精度都有較大的提高，驗(yàn)證了在線估計(jì)模型的正確性和可行性，具有很好的工程應(yīng)用價(jià)值。

由圖4a及圖4b可知，靜態(tài)實(shí)驗(yàn)中，標(biāo)準(zhǔn)UKF值及自適應(yīng)UKF值都在期望值上下振蕩，但自適應(yīng)UKF值與期望值非常逼近。

參考文獻(xiàn)：

[1]徐慧娟，吳美平，羅兵.EKF和UKF INS/GPS組合導(dǎo)航中的應(yīng)用分析[J].航天控制，2006，26(6)：7-10.

XU Huijuan, WU Meiping, LUO Bing. Analysis of Extended and Unscented Kalman Filtering for INS/GPS Integration[J]. Aerospace Control,2006,26(6)：7-10.

[2]張欣，白越，趙常均.多旋翼姿態(tài)解算中的改進(jìn)自適應(yīng)擴(kuò)展Kalman算法[J].光學(xué)精密工程，2014，22(12):3385-3390.

ZHANG Xin, BAI Yue, ZHAO Changjun. Improved adaptive extended Kalman algorithm for attitude estimation of multi-rotor UAV[J].Optics and Precision Engineering, 2014,22 (12)：3385-3390.

[3]JULIER S, UHLMANN J.Unscented filtering and nonlinear estimation[J].Proceedings of the IEEE, 2004，92(3):401- 421.

[4]周衛(wèi)東， 喬相偉， 吉宇人.基于新息和殘差的自適應(yīng)UKF 算法[J]. 宇航學(xué)報(bào)，2010,31(7):1798-1804.

ZHOU Weidong, QIAO Xiangwei,JI Yuren.An Innovation and Residual-Based Adaptive UKF Algorithm[J]. Journal of Astronautics，2010,31(7)：1798-1804.

[5]劉錫祥，徐曉蘇，馮愛國.有限記憶量測(cè)噪聲在線估計(jì)的Kalman改進(jìn)算法[J].東南大學(xué)學(xué)報(bào):自然科學(xué)版，2010,40(4)：766-770.

LIU Xixiang, XU Xiaosu, FENG Aiguo.Improved algorithm for limited memory on-line estimating Kalman filter [J].Journal of Southeast University:Natual Science Edition，2010, 40(4)：766-770.

[6]喬相偉，周衛(wèi)東，吉宇人.用四元數(shù)狀態(tài)切換無跡卡爾曼濾波器估計(jì)的飛行器姿態(tài)[J].控制理論與應(yīng)用，2012,29(1):97-103.

QIAO Xiangwei, ZHOU Weidong, JI Yuren. Aircraft attitude estimation based on quaternion stateswitching unscented Kalman filter[J].Control Theory & Applications，2012,29(1)：97-103.

[7]HU C W,CHEN W, CHEN Y Q.Adaptive Kalman filtering for vehicle navigation[J].Journal of Global Positioning Systems，2003,2(1):42- 47.

[8]PESONEN U,STECK J，ROKHSAZ K. Adaptive neural network inverse controller for general aviation safety[J].Journal of Guidance, Control, and Dynamics，2004,27(3):434- 443.

[9]LOEBIS D，SUTTON R, CHUDLEY J.Adaptive tuning of a Kalman filter via fuzzy logicfor an intelligent AUV navigation system[J].Control Engineering Practice，2004,12(12):1531-1539.

Online noise estimation of mini-AHRS based on adaptive UKF algorithm

LIU Yu, LIU Qiong, ZHOU Fan, LI Yunmei, XIANG Gaolin

(Chongqing Municipal Level Key Laboratory of Photoelectronic Information Sensing & Transmitting Technology,Chongqing University of Posts and Telecommunications, Chongqing 400065,P.R.China)

Abstract:Considering that the prior noise of a normal unscented Kalman filter does not agree with its real behavior，an adaptive unscented Kalman filter algorithm applied to nonlinear joint estimation of both time-varying states and parameters is proposed. Firstly, a cost function is built based on the error between the covariance matrices of innovation and their corresponding estimations. Then the gradient descent method for online forecast is used. Finally, the noise covariance is online updated, the updated covariance feedback to the standard UKF.Experimental and simulation analysis indicates that adaptive UKF provides higher estimation precision than the nomal UKF algorithm. For time-varying noise covariance is uncertain,adaptive UKF online noise estimation robustness is improved significantly, and the accuracy and feasibility of online adaptive UKF noise estimation model is verified.

Keywords：unscented Kalman filter; adaptive unscented Kalman filter; joint estimation;cost function;gradient descent algorithm;robustness

DOI：10.3979/j.issn.1673-825X.2016.03.001

收稿日期：2015-06-23

修訂日期：2016-02-29通訊作者：劉瓊739767483@qq.com

基金項(xiàng)目：國家自然科學(xué)基金(51175535)；MEMS振動(dòng)傳感與微姿態(tài)組合測(cè)井技術(shù)國際聯(lián)合研究中心科技平臺(tái)與基地建設(shè)(cstc2014gjhz0038)

Foundation Items：The National Natural Science Foundation of China(51175535);The Science and Technology Platform and Construction of International Joint Research Center(cstc2014gjhz0038)

中圖分類號(hào)：TN212

文獻(xiàn)標(biāo)志碼：A

文章編號(hào)：1673-825X(2016)03-0285-06

作者簡(jiǎn)介：

劉宇(1972-)，男，重慶人，教授，博士，碩士生導(dǎo)師，長(zhǎng)期從事微振動(dòng)慣性傳感器、光機(jī)電一體化科學(xué)技術(shù)研發(fā)和MEMS慣性傳感技術(shù)研究。E-mail：liuyu@cqupt.edu.cn。

劉瓊(1989-)，男，湖南衡陽人，碩士研究生，主要從事MEMS慣性傳感技術(shù)研究。E-mail：739767483@qq.com。

(編輯：王敏琦)