基于雙級互補濾波的姿態測量算法設計*

高 寧,李 杰,馮凱強,許廷金,高詩堯,李炳臻

(中北大學電子測試技術重點實驗室,太原 030051)

在廣泛的導航應用領域中,精確的姿態估計是必不可少的,如無人機導航、車輛導航、室內導航、人體姿態控制等[1-4]。由于體積小、成本低、功耗小等優點,MEMS器件被廣泛應用于姿態測量領域[5]。然而,由于MEMS陀螺儀存在漂移,長時間工作有累積誤差;利用加速度計和磁力計來估計姿態角會受載體線性加速度和磁場干擾的影響[6-7]。因此,單獨使用某一種傳感器不能獲得精確可靠的姿態信息,必須利用姿態融合算法對不同傳感器的數據進行融合,以提高系統的精度和抗干擾能力[8-10]。

至今,已有許多國內外學者相繼研究出了不同的數據融合算法用于姿態測量領域。Sabatini等[11]人提出了一種基于四元數的擴展卡爾曼濾波器(EKF),將旋轉四元數以及加速度計和磁力計誤差作為狀態矢量,同時引入了一種自適應測量噪聲協方差矩陣構造方法,以減小載體線性加速度和磁場干擾的影響。但是EKF存在觀測方程在線性化時會引入線性誤差等缺點。為此,Yun等[12]人提出了一種基于Gauss-Newton的外部觀測方法,有效解決了觀測方程的線性化問題。但其計算量大,低成本處理器難以實現。為了降低計算量,Mahony等[13]人提出了一種用于無人機姿態估計的顯式互補濾波器(ECF)。Madgwick等[14]人使用梯度下降算法優化提出了固定增益互補濾波器。這些互補濾波算法利用加速度計和磁力計的測量數據來補償陀螺儀的累積誤差,但是由于磁力計的測量值極易受周圍磁場干擾的影響,給航向角估計帶來誤差的同時也會給水平角帶來不確定的影響。

針對上述問題,本文提出了一種雙級互補濾波姿態融合算法。該算法利用加速度計和磁力計的測量值分步對MEMS陀螺儀估計的四元數進行補償,避免了磁場干擾情況下航向角誤差對水平角測量的影響,同時引入了載體非重力加速度誤差和磁干擾誤差自適應補償方案,提高了姿態測量精度。并且算法實現過程簡單,計算量小,降低了對處理器運算速度的要求。最后,基于現有的慣性測量系統對所提算法進行了實驗評估。

1 基于四元數的姿態確定分析

取載體的重心為坐標系原點,3個坐標軸分別與載體的縱軸、橫軸和豎軸相重合,定義為載體坐標系b,與之相對應的絕對坐標系通常稱為導航坐標系n。導航坐標系n向載體坐標系b的轉換可以通過四元數法或Euler角法實現,四元數法由于可以避免Euler角的奇異問題而得到了廣泛的應用[15]。

載體坐標系和導航坐標系之間的轉換關系可以表示為:

(1)

用四元數q=q0+q1i+q2j+q3k表示坐標系的轉換,則旋轉矩陣可表示為:

(2)

(3)

式中:Ω(ω)表示載體坐標系相對于導航坐標系角速度在載體坐標系上分量的四元數。矩陣形式可以表示為:

(4)

通常假設在時間間隔(t,t+ΔT)內,角速度ω是一個固定值,其中ΔT是旋轉四元數更新時間間隔。因此可以獲得式(3)對應的離散時間模型:

(5)

利用陀螺儀輸出的角速度數據,結合式(5)對四元數進行更新,從而獲得旋轉矩陣。

2 雙級互補濾波姿態融合算法

首先,利用上述更新的四元數獲得重力矢量和磁場矢量在載體坐標系下的計算值:

(6)

而加速度計和磁力計的實際測量矢量為:

(7)

2.1 一級互補濾波算法設計

第一級互補濾波利用加速度計的測量數據對陀螺儀估計的四元數進行補償修正,輸出準確的水平姿態。如圖1所示,通過將k時刻由式(5)計算的四元數qk繞向量na旋轉角度Δθa來補償水平角誤差。因此,相應的誤差四元數qae和修正四元數qa可以由下式獲得:

(8)

圖1 利用加速度計測量值補償估計的重力矢量

在靜止狀態下,加速度計通過測量由于重力引起的加速度,可以準確地計算出載體的水平姿態角。但是,當載體存在線性加速度時,加速度計測得的加速度矢量的大小和方向與重力矢量存在偏差,利用加速度計輸出值計算載體姿態角將會出現比較大的誤差。然而,陀螺儀的測量值不會受線性加速度的影響,因此在這種情況下應當把陀螺儀數據作為估計的主要來源,用來進行相對準確的姿態估計。為了解決這一問題,本文在式(8)中引入自適應增益系數μa來減少載體非重力加速度誤差對姿態測量精度的影響。

首先,定義載體非重力加速度誤差ea由下式獲得:

(9)

式中:‖gb‖為加速度計測得的加速度矢量模值,g為當地重力加速度。濾波增益系數μa與載體非重力加速度誤差ea之間的關系由下式表示:

μa=f(ea)

(10)

式中:f為分段連續函數,如圖2所示。當載體線性加速度很小且非重力加速度誤差不大于事先設定的閾值xa,濾波增益系數隨著非重力加速度誤差的增加而線性減小。如果載體存在較大的線性加速度,非重力加速度誤差大于閾值,那么濾波增益系數等于零。誤差閾值 的大小可通過衡量慣性器件各自的精度、載體的動態強度等條件,由試驗獲得最優值。

圖2 濾波增益系數與非重力加速度誤差的關系

2.2 二級互補濾波算法設計

第二級互補濾波利用磁力計的測量數據對上一級獲得的四元數進行補償,來修正航向角。在第一級工作的基礎上,利用磁力計測量值求得磁場在導航坐標系下的估計矢量:

(11)

忽略垂直分量,得

(12)

如圖3所示,通過將qa繞nm旋轉小角Δθm來補償偏航角誤差,因此,相應的誤差四元數qme和補償四元數qm可以由下式獲得:

(13)

式中:

mn=(0 1 0)為導航坐標系下忽略垂直分量的地磁參考矢量。濾波增益系數μm的獲取辦法與上述μa獲取辦法類似,在此不再贅述。

圖3 利用磁力計測量值補償估計的磁場矢量

由上述可以看出,航向角誤差四元數的旋轉軸nm與坐標軸z軸重合,也就是說第二級補償的四元數為上一級獲得的四元數僅繞z軸旋轉獲得,即使在磁場受到干擾的情況下也不會影響水平角的估計。

2.3 算法總體設計

根據上面的分析,設計的雙級互補濾波算法流程圖如圖4所示。

圖4 雙級互補濾波姿態測量算法流程圖

3 實驗驗證

利用實驗室現有的慣性測量系統對算法進行驗證。慣性測量單元包含三個單軸CRM100 MEMS陀螺儀、三個單軸MS9000 MEMS加速度計、一個三軸HMC1043L AMR磁力計和一個采集存儲單元。內部微處理器實時采集并存儲傳感器測量的原始數據。本文采用MATLAB設計所提濾波器,并對原始數據進行處理,來獲得載體的姿態信息。設計了2種實驗方式來驗證算法的有效性:磁干擾靜態試驗;跑車動態試驗。第一組試驗用來驗證算法能否有效避免磁干擾對水平姿態的影響;第二組試驗用來驗證是否能有效消除載體線性加速度的影響。數據處理時算法中的誤差閾值取為xa=xm=0.2。

圖5 磁干擾實驗姿態角對比圖

3.1 磁干擾靜態實驗

將測量單元水平靜止放置,拿一根鐵棒在不同時間段內靠近測量單元,以此來模擬磁場受到干擾的情況。將采集的數據用不同算法處理結果如圖5所示。

在圖5中可以看出,慣性測量單元分別在5 s～13 s 和22 s～29 s兩個時間段內受到了磁場的干擾。利用Mahony算法解算出來的航向角在磁干擾的情況下產生了15°左右的誤差,同時也給俯仰/滾動角帶來了不同程度的影響。相比于Mahony算法,雙級互補濾波姿態測量算法在磁場受到干擾的情況下,有效降低了磁干擾對航向角的影響,同時俯仰/滾動角完全不受航向角誤差的影響,性能明顯優于Mahony算法。在沒有磁干擾的情況下,所提算法可以輸出穩定可靠的三維姿態角信息,誤差均小于0.2°。

3.2 車載動態實驗

為驗證算法實時性以及能否有效消除載體線性加速度的影響,設計了跑車動態實驗。采用加拿大NovAtel公司高精度光纖組合導航系統span-lci為參考基準,考核算法的姿態精度。將系統與高精度母慣導捷聯安裝在與車體固聯的測試工裝上進行跑車實驗,兩套系統同時采集車體的運動信息。跑車現場圖如圖6所示。

采集到的加速度矢量的模值如圖7所示,從圖中可以看出,載體存在線性加速度。將融合濾波后輸出的三軸姿態角信息與母慣導輸出的姿態角相比較,對比圖如圖8所示。實驗系統與母慣導之間的姿態角誤差如圖9所示。

圖7 跑車試驗加速度矢量模值

從圖8中可以看出,本文所提雙級互補濾波融合算法得到的姿態角與高精度母慣導提供的姿態信息吻合度較高,并且具備對動態變化的實時跟蹤性能。對比Mahony算法,本文所提算法由于引入的自適應濾波增益而有效得減小了載體線性加速度對姿態測量精度的影響,精度得到了明顯的提升。從圖9姿態角誤差曲線可以看出,由于磁力計容易受到干擾,航向角誤差在三維姿態角中誤差最大,達到5°左右。俯仰角與滾動角誤差相對較小,俯仰角誤差小于2°,滾動角誤差小于3°,誤差曲線沒有發散趨勢。

圖8 三維姿態角對比圖

圖9 三維姿態角誤差曲線

4 結論

本文采用雙級互補濾波算法對MARG傳感器的數據進行融合,實現了三自由度的姿態測量。與傳統的Mahony互補濾波算法相比較,采用雙級互補濾波融合算法將水平與航向姿態分級融合修正,消除了磁干擾情況下航向角誤差對水平角測量的影響。同時,針對載體非重力加速度誤差和磁干擾誤差引入兩個獨立的濾波增益系數,可分別由加速度計與磁力計不同頻段的噪聲自適應調節,大大提高了姿態角測量精度。基于慣性測量單元的靜態、動態實驗結果表明:本文所提算法能夠有效的解決磁場干擾和載體線性加速度對姿態測量的影響,顯著提高了三維姿態角的測量精度。