對航姿參考系統(tǒng)的濾波方法研究

劉 禹

（船舶重工集團公司723研究所，揚州225001）

0 引 言

航姿參考系統(tǒng)（AHRS）是一種微型捷聯(lián)慣導(dǎo)系統(tǒng)，可以在3D空間中測量平臺的全角度方向，提供航向、俯仰和橫滾信息。航姿參考系統(tǒng)采用微機電系統(tǒng)（MEMS）技術(shù)和磁阻傳感器技術(shù)，成本低，體積小，集成度高，從而可以使得小型電子對抗設(shè)備這類對體積和成本敏感的系統(tǒng)具有航姿測量能力。由于成本和體積的約束，航姿參考系統(tǒng)輸出的航向角等數(shù)據(jù)由于噪聲的原因，誤差通常較大，必須經(jīng)過處理，才能達到使用要求。

1 卡爾曼濾波原理

卡爾曼濾波能夠利用目標(biāo)的動態(tài)信息，通過自回歸數(shù)據(jù)處理算法，得到一個關(guān)于目標(biāo)狀態(tài)的最優(yōu)估計。該估計可以是對過去狀態(tài)的估計（平滑），可以是對當(dāng)前狀態(tài)的估計（濾波），也可以是對將來狀態(tài)的估計（預(yù)測）。卡爾曼濾波算法以反饋控制的方法進行過程狀態(tài)的估計：濾波器估計出過程中某一時刻的狀態(tài)，然后以測量變量的方法獲得反饋。卡爾曼濾波的遞推最優(yōu)估計方法［1］，采用狀態(tài)空間描述法，在算法上采用遞推形式，只需要很少的幾個測量值和預(yù)測值就能進行最優(yōu)的狀態(tài)估計，因此在實時系統(tǒng)中能很好地對噪聲起到濾波作用，適合處理航姿參考系統(tǒng)的輸出數(shù)據(jù)。由于卡爾曼濾波器進行狀態(tài)估計時，其動態(tài)方程和測量方程均為線性方程，狀態(tài)可控，算法簡練，十分適合工程應(yīng)用［2］。下面對卡爾曼濾波方程進行分析。

在數(shù)學(xué)上，圖1中的信號流程圖至少可以表述為以下2個方程：

（1）過程方程：

式中：M×1向量v（n）為過程噪聲，按照高斯白噪聲過程進行建模，其相關(guān)矩陣可定義為：

該線性動態(tài)系統(tǒng)可表達為2個反饋連接的獨立單元，它們分別為：一個轉(zhuǎn)移矩陣，用A（n＋1，n）來表達；一個存儲單元，用z－1I來表達。其中，z－1為單位延遲，I為（M×M）單位陣。轉(zhuǎn)移矩陣A（n＋1，n）表示系統(tǒng)從n時刻到n＋1時刻的轉(zhuǎn)移過程。

（2）測量方程

觀測向量可由下面的方程來表示：

圖1 線性動態(tài)離散時間系統(tǒng)的信號流程圖

式中：C（n）為已知的N×M測量矩陣；N×1向量w（n）為測量噪聲，按照高斯白噪聲過程進行建模。

若i＝n，該問題稱為濾波；若i＞n，稱為預(yù)測；若1≤i＜n，則稱為平滑。此處只考慮濾波和預(yù)測，它們是密切相關(guān)的。

實際算法中迭代步驟如下：

（1）新息過程

計算n時刻觀測數(shù)據(jù)y（n）的最小均方估計，可以將y（n）對應(yīng)的新息過程定義為［3］：

式中：M×1向量α（n）為觀測數(shù)據(jù)y（n）中新的信息。

（2）新息過程的相關(guān)矩陣

為了求出新息過程α（n）的相關(guān)矩陣，通過遞歸法求解狀態(tài)方程（1），可以得到：

由式（5），可以將新息過程表示如下：

將測量方程式（3）代入式（6），得：

（3）卡爾曼增益

應(yīng)用新息過程進行狀態(tài)估計可得：

（4）濾波

首先假定過程噪聲v（n）和狀態(tài)向量x（n）相互獨立。由狀態(tài)方程（1）可以得到n＋1時刻狀態(tài)x（n＋1）的最小均方估計為：

2 仿真分析

航姿參考系統(tǒng)使用的三軸MEMS陀螺和三軸磁阻傳感器采用集成電路設(shè)計，成本低，可靠性高，環(huán)境適應(yīng)性和適裝性較采用模擬分立器件的傳統(tǒng)導(dǎo)航設(shè)備大為提高。但這些集成器件的采樣數(shù)據(jù)一致性差，包含隨機漂移等誤差。所以航姿參考系統(tǒng)輸出的航向等數(shù)據(jù)需要使用卡爾曼濾波進行處理，提高數(shù)據(jù)的準(zhǔn)確度，才能滿足精度要求。

航姿參考系統(tǒng)輸出的航向數(shù)據(jù)除了受器件噪聲影響以外，轉(zhuǎn)動到90°及270°附近時，會連續(xù)出現(xiàn)幾個相同的數(shù)值。這是因為，磁力計的方位角α可以表示為：

式中：Hey和Hex為地磁場在航姿參考系統(tǒng)的y軸與x軸方向上的分量。

由公式（14）可知，航姿參考系統(tǒng)轉(zhuǎn)到90°或270°時存在奇異點，導(dǎo)致航向數(shù)據(jù)在特定角度范圍內(nèi)難以分辨出現(xiàn)幾個相同的數(shù)值。所以卡爾曼濾波不但要解決噪聲干擾還要能夠解決上述問題。

仿真一：理論數(shù)據(jù)仿真。

該仿真主要評估卡爾曼濾波對噪聲的處理能力。通過對夾雜噪聲的正弦曲線的處理，可以評估出濾波效果。圖2中，線狀曲線是夾雜有高斯噪聲的原始數(shù)據(jù)，點狀曲線是通過卡爾曼濾波之后的數(shù)據(jù)。從圖2可以看出，經(jīng)過濾波后的數(shù)據(jù)明顯得到了很好的改善。

圖2 卡爾曼濾波前后的數(shù)據(jù)波形圖

仿真二：對實驗數(shù)據(jù)進行卡爾曼濾波處理的仿真。

該仿真主要評估卡爾曼濾波在實際工作環(huán)境下的濾波處理能力。實驗條件：將航姿參考系統(tǒng)固定在轉(zhuǎn)臺上，轉(zhuǎn)臺以30°／s的轉(zhuǎn)速勻速轉(zhuǎn)動，此時可得到航姿參考系統(tǒng)輸出的航向角數(shù)據(jù)。經(jīng)分析數(shù)據(jù)后發(fā)現(xiàn)，在90°及270°時，數(shù)據(jù)曲線會出現(xiàn)幾個相同的采樣值，數(shù)據(jù)的噪聲較大，線性度差，需要對數(shù)據(jù)進行平滑處理。

圖3及圖4中，線狀曲線代表航姿參考系統(tǒng)在90°及270°附近的真實采樣數(shù)據(jù)，點狀曲線代表經(jīng)過卡爾曼濾波后的數(shù)據(jù)；從圖中可以明顯地看到，經(jīng)濾波后的數(shù)據(jù)在90°和270°的數(shù)據(jù)點上得到更好的平滑，線性度明顯改善。

圖3 航姿參考系統(tǒng)在90°方位附近數(shù)據(jù)的濾波效果

圖4 航姿參考系統(tǒng)在270°方位附近數(shù)據(jù)的濾波效果

3 結(jié)束語

卡爾曼濾波算法具有以下明顯的優(yōu)點：

（1）在對隨機信號做估計時，卡爾曼濾波是常用的算法之一。由于采用了遞推算法，實時測量信息經(jīng)過提煉被濃縮在估計值中，不必存儲所有時刻的測量值，當(dāng)濾波步數(shù)增加時，所提取出的信息濃度也會逐漸增加。

（2）對估計量和測量量的知悉要求低，在已知驅(qū)動被估計量的驅(qū)動噪聲的統(tǒng)計特性以及描述這種驅(qū)動作用的系統(tǒng)狀態(tài)方程和測量噪聲的統(tǒng)計特性的情況下，即可進行卡爾曼濾波運算。由于系統(tǒng)的狀態(tài)方程是準(zhǔn)確已知的，測量噪聲及驅(qū)動噪聲為高斯白噪聲，且統(tǒng)計特性是不隨時間變化的平穩(wěn)過程，因而對非平穩(wěn)的被估計量，卡爾曼濾波是一種最優(yōu)的估計。

本文中的航姿參考系統(tǒng)輸出的航向數(shù)據(jù)經(jīng)卡爾曼濾波處理后已滿足了設(shè)計要求。經(jīng)過處理后的數(shù)據(jù)去除了奇異點，更加平滑，更接近真實值，輸入到設(shè)備伺服系統(tǒng)后，顯著提高了其閉環(huán)控制精度，取得了良好的應(yīng)用效果。隨著計算機信息技術(shù)的發(fā)展，卡爾曼濾波的處理速度和性能將得到進一步提高，應(yīng)用將更加廣泛。

［1］陸根源.信號檢測與參數(shù)估計［M］.北京：科學(xué)出版社，2004.

［2］孫璐璐，姜洋，王學(xué)敏.基于Kalman濾波的機動目標(biāo)跟蹤方法研究［J］.艦船電子對抗，2011，34（3）：32－35.

［3］王華，李有軍，劉存建.MATLAB電子仿真與應(yīng)用教程［M］.北京：國防工業(yè)出版社，2007.