EKF的航姿參考系統(tǒng)自適應(yīng)濾波算法

中國船舶第七一五研究所 蔣弘威 綦俊峰 劉雨聰 趙 瑜

航姿參考系統(tǒng)（Attitude and Heading Reference System，AHRS）包括多個(gè)軸向傳感器，通過算法將傳感器的數(shù)據(jù)進(jìn)行融合解算，準(zhǔn)確地計(jì)算出載體的航向與姿態(tài)信息。常用的姿態(tài)解算方法有madgwick算法、mahony算法以及kalman濾波算法。相較于前者，Kalman濾波算法的優(yōu)點(diǎn)在于能夠辨識(shí)隨機(jī)噪聲，使得解算結(jié)果更加平穩(wěn)且精度更高。缺點(diǎn)在于計(jì)算量龐大，涉及到矩陣乘法及逆運(yùn)算。

隨著CPU運(yùn)算能力的提升，越來越多的主控芯片可以提供相關(guān)接口函數(shù)，支持矩陣的運(yùn)算，因此主流算法多以kalman濾波為基礎(chǔ)來對(duì)姿態(tài)角進(jìn)行解算。本文以MEMS傳感器為研究對(duì)象，在kalman濾波的基礎(chǔ)上，提出了一種改進(jìn)算法，減少因環(huán)境變化以及模型不準(zhǔn)確所帶來的影響，使解算出的姿態(tài)角度有著更高的精度和穩(wěn)定性。

1 擴(kuò)展卡爾曼濾波

由理論力學(xué)的知識(shí)可知，繞定點(diǎn)轉(zhuǎn)動(dòng)的剛體既可以通過一組歐拉角來描述，也可通過一組四元數(shù)來描述。由于四元數(shù)在旋轉(zhuǎn)矩陣的計(jì)算上，有非奇異表達(dá)、更緊湊、微分方程線性表達(dá)等優(yōu)點(diǎn)，常選取四元數(shù)作為狀態(tài)量來建立狀態(tài)空間模型。

對(duì)于量測(cè)方程，記歸一化后載體在初始狀態(tài)下的磁場(chǎng)向量為[0mt2mt3]T。mt2為地磁水平分量、mt3為垂直分量。由載體受到的重力場(chǎng)、地磁場(chǎng)與四元數(shù)關(guān)系可得，量測(cè)量的表達(dá)式為：

其中，h(k/k-1)表示加速度計(jì)和磁力計(jì)三個(gè)軸的測(cè)量值。從式(2)中可以看出，量測(cè)方程是一個(gè)非線性方程。對(duì)于一個(gè)非線性系統(tǒng)，在應(yīng)用卡爾曼濾波時(shí)，一種常見的解決思路是進(jìn)行泰勒展開，略去高階項(xiàng)，近似為線性系統(tǒng)。因此需要對(duì)量測(cè)方程進(jìn)行線性化處理。求式(2)的雅可比行列式，將其對(duì)應(yīng)的雅可比行列式視為量測(cè)矩陣Hk，帶入卡爾曼濾波公式中，即可得到以四元數(shù)為狀態(tài)量的擴(kuò)展卡爾曼濾波公式：

2 Sage-Husa自適應(yīng)濾波

除模型準(zhǔn)確之外，卡爾曼濾波獲得最優(yōu)估計(jì)的另一個(gè)條件是隨機(jī)動(dòng)態(tài)系統(tǒng)的結(jié)構(gòu)參數(shù)和噪聲統(tǒng)計(jì)特性確切已知。然而，在實(shí)際應(yīng)用中很難達(dá)到這個(gè)要求，一般是憑經(jīng)驗(yàn)來對(duì)噪聲參數(shù)進(jìn)行調(diào)整。自適應(yīng)濾波的目的就是在噪聲特性未知的情況下，面對(duì)各式各樣的不確定性，如何綜合處理相關(guān)信息，使得某一指定性能指標(biāo)達(dá)到最優(yōu)或近似最優(yōu)。

隨機(jī)系統(tǒng)的誤差往往會(huì)影響到其輸出值，量測(cè)輸出中隱含了關(guān)于系統(tǒng)模型的某些信息，可以根據(jù)量測(cè)輸出對(duì)部分參數(shù)進(jìn)行估計(jì)建模。對(duì)量測(cè)預(yù)測(cè)誤差（即新息）求方差，移項(xiàng)后可得量測(cè)噪聲方差陣的表達(dá)式為：

其中，初值β0=1，而0

然而，如果實(shí)際系統(tǒng)的量測(cè)噪聲比理論模型要小，或者狀態(tài)噪聲設(shè)置偏大，會(huì)使估計(jì)出的量測(cè)噪聲方差陣失去正定性，引起濾波異常。其中一種解決方法是對(duì)的每個(gè)元素的大小進(jìn)行限制。將估計(jì)的量測(cè)噪聲限制在一個(gè)區(qū)間之內(nèi)，使其具有較好的自適應(yīng)能力和可靠性。改進(jìn)后的公式如下：

需要注意的是，在實(shí)際應(yīng)用中，應(yīng)盡量減少自適應(yīng)參數(shù)的個(gè)數(shù)，有利于保證濾波的有效性。結(jié)合實(shí)際情況，取磁場(chǎng)相關(guān)的量測(cè)噪聲進(jìn)行估計(jì)。

3 試驗(yàn)結(jié)果與分析

本次實(shí)驗(yàn)所采用的MEMS慣性傳感器為MPU6050和RM3100，MPU6050提供三軸加速度計(jì)和三軸陀螺儀的數(shù)據(jù)，RM3100提供三軸地磁場(chǎng)數(shù)據(jù)。主控芯片采用STM32F407，用來進(jìn)行傳感器數(shù)據(jù)的讀取、姿態(tài)角度的解算、以及數(shù)據(jù)的輸出，芯片內(nèi)部角度解算采用一般的EKF算法。

將AHRS置于轉(zhuǎn)臺(tái)上，起始三個(gè)姿態(tài)角均為0°。將航向角每次轉(zhuǎn)5°停止20s采樣，連續(xù)采集6個(gè)角度后回歸初始位置。以同樣的方式轉(zhuǎn)動(dòng)俯仰角與橫滾角，分別采集兩者的數(shù)據(jù)。最終姿態(tài)角相應(yīng)的誤差曲線圖如圖1所示。

圖1 姿態(tài)角誤差曲線圖

從圖1中可以看出，經(jīng)過改良后的自適應(yīng)卡爾曼濾波在解算結(jié)果上比單純使用擴(kuò)展卡爾曼濾波要更加平穩(wěn)，方差值更小，證明了該算法的有效性。

結(jié)論：航姿參考系統(tǒng)（AHRS）在實(shí)際姿態(tài)解算的過程中，會(huì)因?yàn)橹茉猸h(huán)境的改變、建模不準(zhǔn)確等原因使得實(shí)際解算結(jié)果存在較大偏差。本文提出了一種改良算法，在擴(kuò)展卡爾曼濾波的基礎(chǔ)上，借鑒了遺忘濾波的思想，削弱舊信息的作用，從而減少因模型線性化所帶來的誤差影響。另外，為使算法更加適應(yīng)環(huán)境變化所帶來的影響，引入了Sage-Husa自適應(yīng)算法，將最容易受影響的地磁參數(shù)噪聲另作估計(jì)。從試驗(yàn)結(jié)果來看，改良后的算法更加平滑、穩(wěn)定，具有一定的工程應(yīng)用價(jià)值。