多傳感器互補濾波器數據融合算法設計

馬 龍，代超璠，周 航，裴 昕，黃 超，蘇志剛

(中國民航大學 中歐航空工程師學院，天津 300300)

0 引 言

多傳感器姿態測量系統是利用多個傳感器的信息測量載體姿態的設備，輸出載體相對于導航坐標系的角位置信息，包含橫滾角、俯仰角及航向角[1]。姿態角作為導航中不可或缺的重要信息，是保證載體運行安全的重要參數[2]。近年來，姿態測量系統多以多傳感器組合方式實現，組成多傳感器姿態測量系統，其姿態解算的關鍵技術為數據融合技術。

基于MEMS的傳感器具有可靠性高、小體積、低成本和高集成度等特點[6-10]，隨著MEMS技術的迅速發展，MEMS傳感器的精度不斷提高，其在多傳感器組合方面的應用也隨之不斷增加[11-13]。同時，FPGA以其高邏輯集成度和內嵌高性能硬件模塊的優勢彌補了單處理器系統在姿態測量系統中復雜度太高且計算能力不足的缺點，并且FPGA的強大的并行處理能力，使得多傳感器的數據融合得以實現[14]。利用慣性傳感器測量載體姿態存在諸多不足之處，如僅由陀螺儀構成的姿態測量系統雖然動態性能良好，但是其累計誤差不符合長時間導航要求[15]；而加速度計通過重力加速度來解算載體的姿態角要求載體處于靜止或者勻速直線運動狀態；此外，電子羅盤易受當地磁場的干擾，其輸出的精確性不能得到有效保證[16]。其次，國內外在互補濾波器的研究方面，對于兩種不同傳感器數據融合頗為注重[17]，致使多傳感器數據資源沒有得到充分的利用，此外，多用陀螺儀輔助磁羅盤完成故障檢測[18]，并且對陀螺儀的精度要求較高。例如，文獻[19]利用視覺的方法輔助慣導的方法獲得載體的姿態角，使其在局域的空間內作用顯著。文獻[20]對擴展卡爾曼濾波器(EKF)和互補濾波器進行了比較驗證，表明了互補濾波器相對于EKF具有較快的收斂速度，但是由于只限于兩種傳感器，其精度還是有限。文獻[21]利用三傳感器提出四元數擴展卡爾曼濾波器，但是未對地磁矢量加以考慮，磁場的干擾仍未得到有效的解決。

基于上述問題，本文以FPGA為核心，利用MEMS陀螺儀、MEMS加速度計和電子羅盤構建了一套基于互補濾波器的多傳感器姿態測量系統，針對各個傳感器的頻率特性分別設計了多傳感器下俯仰角和橫滾角互補濾波器以及航向角互補濾波器，實現多數據源的數據融合。實驗結果表明，互補濾波器能在保證輸出精確性前提下完成多傳感器的數據融合。

1 多傳感器姿態解算

基于互補濾波器的多傳感器姿態信息融合，首先在傳感器級別利用慣性傳感器進行姿態解算，得到各個傳感器解算的結果，隨后利用互補濾波器進行數據融合得到載體的姿態信息。其中，俯仰角和橫滾角互補濾波器融合的數據來源為陀螺儀，電子羅盤和加速度計解算得到的姿態信息，航向角互補濾波器融合的數據來源為陀螺儀和電子羅盤解算得到的姿態信息。

1.1 陀螺儀四元數姿態解算

四元數將復平面推廣到了四維空間，廣泛應用在剛體或者坐標系的旋轉運動描述中，四元數的標量部分和矢量部分分別表示了轉角一半的余弦值和其瞬時轉軸的方向余弦值，因此可以用于表示剛體運動過程中坐標系間的相對運動和姿態變化信息。

剛體定點轉動過程中四元數Q的表示如式(1)所示

(1)

式中：q0、q1、q2和q3值如式(2)所示

(2)

式中：四元數Q用于描述剛體繞定點的旋轉，可以認為載體坐標系b系經過無中間過程的一次性等效旋轉為導航坐標系n系。并且四元數Q中包含繞定點等效旋轉有關的信息為：θ為等效旋轉轉過的角度，un為等效旋轉的旋轉軸和轉動的方向。

(3)

(4)

故描述剛體轉動的四元數是規范四元數，式(3)可寫成式(5)

(5)

因此，可以建立四元數和姿態矩陣之間的關系，同時姿態矩陣可用于求解載體的姿態角，令

(6)

則由式(6)可得式(7)

(7)

式(7)和式(1)可推出式(8)

(8)

四元數通過四元數微分方程完成得到[20]，并根據式(5)可得到剛體轉動四元數和姿態矩陣之間的對應關系，進而利用姿態矩陣可以確定剛體轉動后的姿態角。

1.2 加速度計姿態解算

(9)

(10)

(11)

則將式(11)和式(9)代入式(10)可解算得到俯仰角θ和橫滾角γ，如式(12)所示

(12)

載體單獨的航向角變化并不能引起加速度計的輸出變化，因此利用加速度計并不能計算載體的航線角。

1.3 電子羅盤姿態解算

電子羅盤內部集成了磁強計和MEMS加速度計，利用磁強計可以根據旋轉過程中磁場變化計算得到載體航向角，并結合加速度計可測得載體的姿態角。

考慮初始載體坐標系n沿矢量[0 0ψ]T進行旋轉，得到坐標系為d，地磁場矢量m在d系和n系中分量的關系如式(13)所示

(13)

(14)

利用磁強計輸出的磁場強度mb和已經確定的橫滾角和俯仰角，可以得到md如式(14)所示。結合式(13)，并且查詢得到當地的磁場分量mn，則可解算出載體航向角ψ，如下式

(15)

2 互補濾波器數據融合方法

多傳感器姿態測量系統首先利用MEMS陀螺儀、MEMS加速度計和電子羅盤分別解算出姿態角，隨后利用各個傳感器的姿態信息進行姿態信息融合，得到最終的姿態角。在姿態信息融合時，首先需要對各個傳感器解算的姿態角噪聲進行頻譜分析。傳感器做旋轉運動時，MEMS陀螺儀通過四元數法解算出載體姿態角，并與姿態參考系統輸出值作差即可得陀螺儀姿態角計算偏差。同理，可以求得MEMS加速度計和電子羅盤的姿態角計算偏差，并且由于各個傳感器的三軸的相似性，本實驗以俯仰軸為例設計實驗。

利用快速傅里葉變換在頻率域上對傳感器的噪聲進行分析，可以更加直觀和準確的得到各個傳感器的噪聲特性，方便對噪聲頻率特性進行進一步的分析。結果如圖1～圖3所示，分別為MEMS陀螺儀、加速度計和電子羅盤姿態角輸出偏差的幅頻曲線，其能量截取比例均在95%以上。

圖1 陀螺儀姿態角計算偏差幅頻曲線

圖2 加速度計姿態角計算偏差幅頻曲線

圖3 電子羅盤姿態角計算偏差幅頻曲線

對比圖1～圖3可知陀螺儀和加速度計噪聲主要集中在低頻帶，而電子羅盤的噪聲主要集中在高頻帶。由此可知，電子羅盤得到的姿態角噪聲與MEMS陀螺儀和加速度計得到的姿態角的噪聲在頻率上具有一定的互補性。根據噪聲頻率互補性可設計互補濾波器，如圖4為俯仰角和橫滾角濾波器基本原理框架，其中Zprm(s)、Zprg(s)和Zpra(s)分別為利用電子羅盤、MEMS陀螺儀和加速度計解算的俯仰角和橫滾角，Gprm(s)、Gprg(s)和Gpra(s)為對應的濾波器，其表達如式(16)所示。根據圖4和式(16)可求出式(17)，并做出圖5俯仰角和橫滾角互補濾波器解算框架。

圖4 橫滾角和俯仰角解算原理框架

圖5 俯仰角和橫滾角互補濾波器解算框架

(16)

(17)

同理，圖6為航向角互補濾波器基本原理框架，圖中Zym(s)和Zyg(s)分別為電子羅盤和陀螺儀解算值，Gym(s)和Gyg(s)為兩傳感器對應的濾波器，其表達如式(18)所示，根據圖6和式(18)可求出式(19)，并做出圖7航向角互補濾波器解算框架

(18)

(19)

圖6 航向角互補濾波器基本原理框架

圖7 航向角互補濾波器解算框架

3 實驗結果與分析

圖8為多傳感器姿態測量系統硬件模塊。控制運算模塊以FPGA芯片作為系統處理核心，利用其內部的通用浮點計算資源和可配置IP核，構建以MicroBlaze為處理核心的小型流水線系統。利用ISE/XPS(EDK+SDK)開發環境配置AXI(Advanced eXtensible Interface)總線協議的各子模塊，并完成以MicroBlaze為核心的片上系統設計。其中AXI總線協議是一種面向高性能、高帶寬、低延遲的片內總線，可以完成處理核心與各個模塊接口之間的數據通信，以及傳輸FPGA芯片對其內部運算單元的控制指令。利用微處理器硬件規范文件(microprocessor hardware standardization，MHS)和微處理器外圍設備描述文件(microproces-sor periphery document，MPD)完成硬件系統的中斷處理和總線結構的配置；利用微處理器軟件規范文件(MSS)完成軟件系統中標準I/O設備和外圍設備連接方式以及各個模塊驅動的配置。

傳感器模塊包含MEMS慣性傳感器ADIS16385和電子羅盤HMR3500，可單獨計算載體的姿態角信息。其中，慣性傳感器內置三軸陀螺儀和三軸加速度計；電子羅盤內置三軸加速度計、三軸磁強計，可用于計算載體所處環境的磁場強度。系統利用256 KB高速Flash存儲器，彌補了FPGA內程序掉電易擦除的缺點，并完成系統上電自加載功能。電源模塊為系統提供穩定的5.0 V、3.3 V和1.2 V電壓，以保證系統的正常運行。圖9為姿態測量系統硬件平臺實物圖。

圖8 姿態測量系統硬件框架

圖9 多傳感器姿態測量系統實物

姿態測量實驗過程中，根據運動狀態和磁場狀況設定濾波器參數，通過姿態測量實驗將濾波器輸出結果與AHRS測量值作差比較，進行姿態測量實驗驗證算法。為了驗證互補濾波器在多傳感器姿態測量系統的有效性，利用三維轉臺設計姿態旋轉實驗。表1為姿態旋轉實驗運動的參數，三維轉臺圍繞3個軸旋轉軸旋轉角度均為±25°，俯仰角、橫滾角和航向角旋轉周期分別為47 s、35 s和15 s三維轉臺旋轉運動均為15個周期，并且轉動過程中靜止60 s，用于采集周圍的磁場強度和重力加速度，作為當地磁場強度和重力的參考值。

表2為俯仰角和橫滾角濾波器參數設定值，表3為航向角濾波器參數設定值。設計載體旋轉實驗，使系統分別繞三軸旋轉±25°，將多傳感器姿態測量系統和姿態參考系統輸出進行對比，結果如圖10、圖11所示，分別為多傳感器姿態測量系統姿態輸出偏差幅值-頻率曲線圖和多傳感器姿態測量系統互補濾波器輸出與AHRS偏差圖。由圖可知，多傳感器姿態測量系統噪聲水平明顯降低，并且橫滾角和俯仰角精度保持在±0.35°以內，航向角精度保持在±0.7°以內。

實驗結果表明，互補濾波器能有效降低多傳感器姿態測量系統的輸出噪聲，提高了系統精度，驗證了系統所設計的基于多傳感器的互補濾波器數據融合算法的有效性。

表2 俯仰角和橫滾角互補濾波參數設定

表3 航向角互補濾波參數設定

圖10 互補濾波器輸出偏差幅值-頻率曲線

圖11 姿態測量系統互補濾波器輸出與AHRS偏差值

4 結束語

本文以FPGA為處理核心，以MEMS陀螺儀、MEMS加速度計和電子羅盤建立多傳感器組合，根據傳感器噪聲互補性設計三傳感器組成的俯仰角和橫滾接互補濾波器以及雙傳感器組成的航向角互補濾波器，實現了數據融合。構建了一套基于FPGA的多傳感器姿態測量系統并進行互補濾波器的實驗驗證。實驗結果表明，相比于姿態航向參考系統(AHRS)輸出值，俯仰角和橫滾角精度在±0.37°以內，航向角精度在±0.8°以內，驗證了該系統具有較高精度的輸出。