針對在導航系統姿態解算中，陀螺儀和電子羅盤在解算姿態時分別存在積分誤差和磁場干擾的問題，提出了利用Kalman濾波和互補濾波相融合的算法進行定位。首先將電子羅盤和陀螺儀通過Kalman濾波得出最優估計四元數，然后利用互補濾波算法對陀螺儀的漂移進行補償得到校正后的四元數，將此次得到的四元數和Kalman濾波得出最優估計四元數再次通過Kalman濾波對四元數進行第二次最優估計，進而輸出姿態角。實驗中對比了本算法和互補濾波算法、無濾波算法的效果。實驗證明，該算法不僅可以有效解決方位角誤差發散問題，還有效解決了磁場干擾問題，實現了高精度的方位輸出。

隨著以MEMS(Micro-electromechanical Systems)傳感器為代表的微型化慣性器件的發展，基于捷聯式慣導原理和MEMS傳感器的慣性定位技術也日益受到重視，特別是在難以接收衛星信號的室內、地下、礦井、水下、戰場等場合[1]。針對上述問題往往采用利用電子羅盤對陀螺進行校正的方法，在室內、地下、礦井、水下等過程中磁強計更加容易受到干擾，造成方位更大的偏差。針對磁強計易受干擾和陀螺積分漂移的問題已經有眾多融合的算法出現，比如卡爾曼濾波、無跡卡爾曼濾波（UKF）、擴展卡爾曼濾波（EKF）等[2-4]，這些濾波方法需要建立準確的狀態方程和觀測方程。還有一種濾波算法是在互補濾波的基礎上進行擴展，比如經典互補濾波、基于梯度下降法的互補濾波算法等[3-6]，但是這種濾波算法適用的精度不高。面對這些問題，本文提出了一種Kalman濾波和互補濾波融合的慣性定位算法，該算法在設計Kalman濾波中，將加速度計和磁強計融合得出的四元數作為觀測值，利用陀螺儀得出的四元數作為狀態值，通過數據的融合進行濾波，完成四元數的第一次最優估計，針對陀螺漂移問題則利用所設計的互補濾波對陀螺漂移進行補償，得到校正后的角速度，進而求得校正后不斷更新的四元數，然后和第一次完成的最優估計四元數通過第二次Kalman濾波進行估計，進而輸出高精度的姿態角。

1 算法總體設計

Kalman濾波和互補濾波融合的慣性定位算法的總體思路如圖1所示。

首先，將磁強計和加速度計測量的磁場強度和加速度利用高斯牛頓迭代法解算出的四元數送入Kalman濾波中當作觀測值，將陀螺儀測量的角速度值利用四階-庫塔法解算出的四元數作為Kalman濾波的狀態值進而得到四元數的第一次最優估計值，在Kalman濾波過程中既可以去掉隨機噪聲，通過電子羅盤對陀螺儀進行校正，又可以避免由于磁場干擾帶來的干擾數據。

其次，利用載體坐標系和地理坐標系之間的轉換矩陣，將地理坐標系下的重力和地磁分量轉換到載體坐標系下，然后與加速度計和磁強計在載體坐標系下測量得到的加速度和磁場強度做向量積的運算，將二者向量積的和相加經過比例積分低通濾波，可以將電子羅盤測量姿態中的高頻抖動信號濾出，然后和陀螺儀測量的角速度進行融合，得到對陀螺儀補償后的角速度，利用四階龍格庫塔法得到校正后的四元數。

最后，將互補濾波算法得到的校正四元數作為狀態量，將第一次Kalman濾波得到的最優四元數作為觀測量進行第二次Kalman濾波，完成四元數的第二次優化，然后通過四元數建立姿態矩陣求得更加精確的姿態角。

2 卡爾曼濾波設計

卡爾曼濾波（Kalman filtering）是一種利用線性系統狀態方程，通過系統輸入輸出觀測數據，對系統狀態進行最優估計的算法。建立系統的狀態方程和測量方程是卡爾曼濾波的主要工作。本次卡爾曼濾波設計中用四元數的不斷更新量作為卡爾曼濾波中的狀態量。

下面建立測量方程，由重力加速度和三軸加速度計

式（4）中ax、ay、az表示重力加速度的測量值，重力加速度用g表示。

由地磁場和磁強計測量信息之間的關系可得：

由式（6）、式（7）并通過高斯牛頓迭代法求得四元數，作為觀測量。觀測量取為：

式（8）中觀測量方程Z（k）=H（k）X（k）+V（k），其中H（k）為觀測量的增益矩陣，V（k）為觀測噪聲，方差R（k）可以通過傳感器的測量數據獲得。由于第一次Kalman濾波和第二次Kalman濾波都是對四元數的最優估計，故上述濾波公式和建立的方程均保持不變。

3 互補濾波算法設計

由于陀螺儀通過積分得到的方位角存在長時間誤差積問題，但其動態響應好，不易受外界干擾，而與之對應的電子羅盤易受外界干擾，但它具有無誤差累積的優點。互補濾波就是利用陀螺儀的高頻特性和電子羅盤的低頻特性進行融合，通過比例環節進行頻率特性的調節，如圖2所示。

在圖2中，a、m表示載體坐標系下加速度計和磁強計測量得到的加速度和磁場強度；將地理坐標系下的重力分量和地磁分量通過四元數構成的姿態轉換矩陣變換為載體坐標系下的重力分量和地磁分量，分別用as和ms表示。在載體坐標系下分別對a、as和m、ms做向量積的運算，得到加速度計和磁強計對陀螺儀的校正誤差。其中俯仰角和橫滾角的誤差設為Δθ，γ，航偏角的誤差設為ck5-gs8-x1.gif即：

將最后得到的角速度通過四階龍哥庫塔法進行姿態解算，得到最優姿態角。

4 實驗驗證和結果分析

本文采用MPU9150模塊進行試驗驗證，通過硬件I2C總線的方式進行數據的讀取，傳感器按100 Hz/s進行數據更新，以外部中斷的方式進行數據采集，然后將采集的數據存放于TF中，通過MATLAB編程進行數據調用。為了驗證此慣性定位融合算法的效果，主要在行走和磁場干擾嚴重的兩種模式下進行測試，且兩種方式都是在室內完成。圖3為攜帶傳感器人員（將傳感器綁在腳上）在室內某一位置出發，轉一圈回到起始位置，時長約5 min；圖4為攜帶傳感器人員在下電梯過程中保持站立姿態的測試。在圖3和圖4中，單個陀螺儀解算（無濾波算法）用圖（a）代表，互補濾波算法用圖（b）代表，本算法用圖（c）表示。

通過圖3對比始末位置方位偏差的數據結果可知，互補濾波算法雖然比單個陀螺進行行人方位解算更加精確，但是利用互補濾波算法始末位置偏差在8°之內，而本算法在行走5 min左右始末位置偏差在 1.5°之內。由圖3（c）證明本算法可以準確提供在大動態下（轉圈）的方位。由圖4可知，在電梯內磁強計受到強烈干擾的情況下，僅利用互補濾波算法會比單個陀螺進行解算出現更嚴重的方位偏差問題，而利用Kalman濾波和互補濾波相融合的慣性定位算法避免了此問題，確保姿態精度在2°以內，有效抑制了磁場強度的干擾，確保方位的高精度輸出。

5 結束語

本文針對在導航解算中陀螺儀的積分偏差和磁強計容易受到干擾的問題，提出了一種利用Kalman濾波和互補濾波相融合的慣性定位算法，利用Kalman濾波求最優四元數，利用互補濾波對陀螺儀的角速度進行校正，通過校正的角速度進行四元數的更新，將兩次校正后的四元數通過Kalman濾波求最優四元數。通過實驗結果可以看出，此方法不僅可以很好地解決陀螺漂移問題，還能有效抑制磁強計受到干擾的問題，能夠準確地對方位進行校準，實現姿態角高精度的穩定輸出。