朱文超， 何 飛

（1．中國電子科技集團公司 第三十八研究所，安徽 合肥 230041；2．中國科學技術(shù)大學，安徽 合肥 230027；3．中國科學院 合肥智能機械研究所，安徽 合肥 230031）

0 引言

在已知系統(tǒng)和量測數(shù)學模型、噪聲統(tǒng)計特性、狀態(tài)初值的情況下，卡爾曼（Kalman）濾波器可利用輸出信號的量測數(shù)據(jù)獲取狀態(tài)變量的估計值，具有計算量小，可遞推、實時性高等特點［1］。然而，標準Kalman濾波器過分依賴于系統(tǒng)狀態(tài)空間模型的準確性，當狀態(tài)參數(shù)發(fā)生擾動時，往往無法精確跟蹤系統(tǒng)突變狀態(tài)，甚至會引起濾波發(fā)散現(xiàn)象［2］。針對這一問題，文獻［3-4］在預測協(xié)方差中引入單漸消因子，通過Sage-Husa估計法重新解算量測殘差，增強新息在狀態(tài)估計中的權(quán)重。文獻［5］通過強跟蹤中心查分濾波器實時估計汽車前后軸等效時變側(cè)偏剛度，對標稱模型的不精確與狀態(tài)擾動等因素具有強魯棒性。文獻［6-7］在強跟蹤濾波器中引入了限定記憶的思想，通過次優(yōu)漸消因子修正濾波增益，增強跟蹤突變狀態(tài)的能力。文獻［8-9］將強跟蹤濾波器和UKF濾波器相融合，引入時變漸消因子調(diào)節(jié)卡爾曼增益，強迫輸出殘差序列保持正交，提取殘差有效信息，提升突變狀態(tài)的跟蹤能力。文獻［10］在預測協(xié)方差中融合弱化因子與漸消因子，依據(jù)模糊控制理論，膨脹狀態(tài)突變下的量測新息，提升跟蹤精度，已成功應(yīng)用于GPS定位解算領(lǐng)域。

然而，上述研究均未對狀態(tài)突變程度進行等級劃分，致使收斂速度慢。針對這一問題，本文基于濾波發(fā)散判據(jù)，分析Sage窗量測新息與標準Kalman量測新息的關(guān)系，以儲備系數(shù)為等級，判斷狀態(tài)突變程度，利用雙漸消因子調(diào)整預測協(xié)方差，增權(quán)量測新息，提升突變狀態(tài)的估計精度。

1 Kalman濾波簡介

1.1 線性離散系統(tǒng)狀態(tài)空間模型

k歷元下，線性離散時間系統(tǒng)狀態(tài)方程和量測方程可表示為

式中，Xk為m 維狀態(tài)向量；Φk，k-1為m×m 維k-1歷元至k歷元的狀態(tài)轉(zhuǎn)移矩陣；Uk-1為p 維輸入控制量；Γk，k-1為m×p 維控制輸入系數(shù)矩陣；Wk-1為q維系統(tǒng)過程噪聲序列；Ψk，k-1為m×q維過程噪聲系數(shù)矩陣。Zk為j 維量測向量；Hk為j×m 維量測系數(shù)矩陣；Vk為j 維系統(tǒng)量測噪聲序列。

1.2 Kalman濾波魯棒性

Kalman濾波是一種線性、無偏且誤差方差最小的隨機系統(tǒng)最優(yōu)估計算法［11］，但魯棒性較差，無法精準跟蹤系統(tǒng)突變狀態(tài)。究其原因，主要有2點：①狀態(tài)擾動下的先驗預測偏差較大；②穩(wěn)態(tài)Kalman濾波器的增益為定量，狀態(tài)突變時，無法實時增權(quán)量測新息。假設(shè)狀態(tài)擾動（系統(tǒng)控制方式改變、噪聲特性變化等）于k歷元加載至穩(wěn)態(tài)系統(tǒng)，則突變信息將直接作用于狀態(tài)方程中的系數(shù)矩陣，如擾動方程（3）中ΔΦk，k-1、ΔΓk，k-1、ΔΨk，k-1所示。此時，若仍依靠舊狀態(tài)方程進行先驗預測，必然產(chǎn)生較大的偏差。然而，穩(wěn)態(tài)Kalman的濾波增益無法實時增權(quán)量測新息，則最優(yōu)估計將逐漸偏離真實值，最終致使濾波器發(fā)散。

精確跟蹤系統(tǒng)突變狀態(tài)，避免濾波器發(fā)散的方法有2種：①分析狀態(tài)擾動的統(tǒng)計特性，建立精確的數(shù)學模型，獲取準確先驗預測。②尋找激活穩(wěn)態(tài)濾波增益方法，當狀態(tài)擾動來臨時，實時增權(quán)測量新息。

然而，狀態(tài)擾動種類繁多，統(tǒng)計特性復雜，難以準確建立數(shù)學模型，故本文針對不同程度的突變狀態(tài)，利用雙漸消因子優(yōu)化預測協(xié)方差，激活穩(wěn)態(tài)濾波增益，采用實時調(diào)整量測新息在最優(yōu)估計中權(quán)重的策略，精確跟蹤系統(tǒng)突變狀態(tài)。

2 自適應(yīng)Kalman濾波

2.1 遞推規(guī)則設(shè)計

鑒于線性離散系統(tǒng)狀態(tài)空間模型，在Kalman遞推規(guī)則中，引入雙漸消因子，實時調(diào)節(jié)先驗預測協(xié)方差，獲取自適應(yīng)Kalman濾波迭代公式。

狀態(tài)預測：

狀態(tài)一步預測

一步預測協(xié)方差

觀測更新：

量測新息

濾波增益

最優(yōu)估計

后驗協(xié)方差

式中，Qk-1、Rk分別為過程噪聲Wk-1、量測噪聲Vk的協(xié)方差矩陣。

2.2 雙漸消因子的求解

最優(yōu)Kalman濾波具備量測新息處處正交的特性，新息序列Y（k）的正交方程可用自相關(guān)函數(shù)表示為［12］

式中，Ck為量測新息協(xié)方差矩陣，上標opt代表最優(yōu)矩陣。

依據(jù)Kalman濾波準則，化簡式（11）為

若系統(tǒng)狀態(tài)未發(fā)生突變，則標準Kalman濾波器的預測協(xié)方差Pbasek，k-1可滿足正交方程

式中，上標base代表標準Kalman濾波器產(chǎn)生的協(xié)方差矩陣。

然而，隨著時間的推移，Kalman濾波器逐漸進入穩(wěn)態(tài)，濾波增益及后驗誤差協(xié)方差的取值固定。倘若此時系統(tǒng)狀態(tài)發(fā)生突變，需激活濾波增益，利用優(yōu)化預測協(xié)方差滿足正交方程，則式（12）可化為

根據(jù)Sage開窗估計法［13］，由量測新息獲取Coptk在k 歷元下的估計值Ck，即

聯(lián)立式（13）、式（15）、式（16）進行矩陣的跡運算，獲取λk的函數(shù)解析式

需要說明的是，更新λk+1時，需將k歷元下的后驗方差作為Pkbase，代入式（13）與式（14），求解Ckba+se1，判斷其與的關(guān)系，構(gòu)造邊界條件，再進入k+1歷元的迭代估計階段。

綜合式（6）、式（14）、式（18）進行矩陣的跡運算，得到γk函數(shù)解析式

綜合式（17）～式（19），獲得因子λk與γk的終解，即

3 試驗驗證

本節(jié)以中科院智能機械研究所自行研制的雙E 型彈性體六維力傳感器為靜態(tài)標定對象，研究標準Kalman濾波器（SKF）、抗差Kalman濾波器［15］（RKF）、自適應(yīng)Kalman濾波器（AKF）的魯棒特性。

六維力傳感器標定實驗臺如圖1所示。依次從Fz方向標定數(shù)據(jù)庫中抽取3組量測值進行分析。其中，第一組為恒載輸出電壓（理想值25 m V）；第二組為卸載輸出電壓（理想值10 m V）；第三組為加載輸出電壓（理想值60 m V）。假設(shè)前47歷元，傳感器持續(xù)進行恒載輸出。第48歷元時，分別進行不同量級的卸載或加載操作，則系統(tǒng)狀態(tài)將發(fā)生不同程度突變，理想趨勢如圖2所示。狀態(tài)突變原因為傳感器的控制載荷發(fā)生變化，故k歷元下的擾動方程（3）可化為式（21），分別利用SKF、RKF、AKF對輸出信號的量測數(shù)據(jù)進行濾波處理，結(jié)果如圖3～圖10所示。

圖2 傳感器階躍輸出信號

圖3～圖5反映了傳感器輸出由恒載向卸載轉(zhuǎn)變時，3種算法的濾波效果。從圖4中可以看出，前47歷元，即系統(tǒng)狀態(tài)未突變前，量測新息序列滿足正交性要求，故AKF與RKF均退化為SKF。3種算法狀態(tài)收斂速度與精度均相同。

圖3 卸載前后3種算法跟蹤效果

圖4 卸載前3種算法跟蹤效果

然而，當系統(tǒng)狀態(tài)在第48歷元發(fā)生突變時，如圖3所示，SKF算法未更新濾波增益，逐漸偏離真實狀態(tài)，估計誤差無限增大。反觀AKF與RKF，兩者均能有效地跟蹤系統(tǒng)突變狀態(tài)。然而，由于狀態(tài)突變程度較淺，AKF退化為RKF，兩者濾波精度相同，如圖5所示。

圖6反映了狀態(tài)突變前后漸消因子的時變趨勢。不難發(fā)現(xiàn)，因子γk處于冷卻狀態(tài)，AKF僅用因子λk進行狀態(tài)調(diào)節(jié)。λk的時變趨勢分為5個階段。

（1）階段1（47歷元至48歷元）。狀態(tài)于此階段突變，突變信息作用于量測新息協(xié)方差，致使估計值驟然變大，λk由冷卻驟變?yōu)榧せ顟B(tài)。

（2）階段2（48歷元至49歷元）。該階段的先驗估計涵蓋了量測新息，其效果優(yōu)于階段1。然而，依據(jù)圖5所示，跟蹤初期的后驗誤差大于突變前的穩(wěn)態(tài)誤差。故減小，Ckbase增大，λk處于急速下降狀態(tài)。

（3）階段3（49歷元至71歷元）。該階段狀態(tài)后驗誤差開始減小，估計精度開始提升，則與Ckbase均開始減小，λk的下降速率減緩。

（4）階段4（71歷元至76歷元）。該階段狀態(tài)后驗誤差緩慢減小，估計精度緩慢提升，則與Ckbase均緩慢減小，λk的下降速率進一步減緩。

（5）階段5（76歷元至100歷元）。該階段后驗誤差穩(wěn)定，則λk僅在穩(wěn)態(tài)值范圍進行微調(diào)。

圖5 卸載后3種算法跟蹤效果

圖6 卸載后AKF漸消因子時變趨勢

圖7～圖8反映了傳感器輸出由恒載向加載轉(zhuǎn)變時，3種算法的濾波效果。當系統(tǒng)狀態(tài)在第48歷元發(fā)生突變時，SKF濾波發(fā)散。RKF僅利用因子λk調(diào)節(jié)濾波增益，增權(quán)量測新息，能夠有效跟蹤突變狀態(tài)。然而，AKF 的總體濾波效果優(yōu)于RKF。尤其在第72 歷元至第100 歷元階段，AKF 狀態(tài)收斂速度優(yōu)于RKF。

圖7 加載前后3種算法跟蹤效果

圖8 加載后3種算法跟蹤效果

從圖9～圖10可以看出，狀態(tài)突變后，AKF持續(xù)激活雙漸消因子λk與γk，能夠更深層次地利用量測新息糾正估計值。加載環(huán)境下，γk的時變趨勢分為4個階段；λk的時變趨勢分為6個階段。

圖9 加載后AKF因子λk 時變趨勢

圖10 加載后AKF因子γk 時變趨勢

γk的時變趨勢解析：

（1）階段1（47 歷元至48 歷元）。狀態(tài)于此階段突變，突變信息體現(xiàn)于量測數(shù)據(jù)，致使后驗協(xié)方差PLSW48驟然變大，γk由冷卻驟變?yōu)榧せ顟B(tài)。

（2）階段2（48歷元至49歷元）。該階段的先驗估計涵蓋了量測新息，先驗誤差Pbase48，48的精度回升，則相較于階段1，γk處于急速下降狀態(tài)。

（3）階段3（49歷元至72歷元）。該階段后驗誤差開始減小，先驗估計精度穩(wěn)步提升，則相較于階段2，γk的下降速率減緩。

（4）階段4（72歷元至100歷元）。該階段狀態(tài)后驗誤差繼續(xù)減小，第72歷元與Cbkase之間的關(guān)系到達臨界條件，γk由激活變?yōu)槔鋮s態(tài)。

λk的時變趨勢解析：

（1）階段1、階段2、階段3的趨勢分析同卸載環(huán)境λk。

（2）階段4（72歷元至81歷元）。該階段γk處于冷卻狀態(tài)，則Ckbase的衰減速率變慢。相較于階段3，λk的下降速率加快。

（3）階段5（81歷元至94歷元）。該階段后驗誤差緩慢減小，估計精度緩慢提升與Cbkase均緩慢減小，相較于階段4，λk的下降速率有所減緩。

（4）階段6（94歷元至100歷元）。該階段后驗誤差穩(wěn)定，則λk僅在穩(wěn)態(tài)值范圍進行微調(diào)。

通過式（22）解算72歷元至第100歷元3種算法估計結(jié)果的均方根誤差X~kRMSE，詳見表1。結(jié)果顯示，SKF誤差較大，處于發(fā)散狀態(tài)。相較于RKF，AKF穩(wěn)態(tài)精度提升了44．76%。

表1 濾波算法性能對比

4 結(jié)論

為解決標準Kalman濾波無法實時精確估計突變狀態(tài)的問題，設(shè)計了雙漸消因子自適應(yīng)Kalman濾波器。面對不同程度的狀態(tài)突變，采用合適的漸消因子組合，實時調(diào)節(jié)預測協(xié)方差，變權(quán)量測新息。實驗結(jié)果表明，自適應(yīng)Kalman濾波器，具有較強的魯棒性，穩(wěn)態(tài)精度優(yōu)于抗差Kalman濾波，可運用于六維力傳感器靜態(tài)標定領(lǐng)域。然而，本文僅研究標量漸消因子，對于多維度漸消因子的求解方法，還有待于進一步深入研究。