六軸陀螺儀姿態解算研究

馬陟昶 王勤湧 郭張樂 范鵬宇 項暢清

摘要：六軸陀螺儀是三軸陀螺儀與三軸加速計的合稱，它是在三軸陀螺儀這個傳感器的基礎上增加上三軸加速器這個元器件。由于六軸陀螺儀收集的加速度、角速度等數據會受到噪聲信號的影響，所以要對這些收集到的數據進行濾波處理。在眾多的濾波方式中，選用較為普遍的一階互補濾波、卡爾曼濾波和DMP，在濾波后獲得更加精確的數據同時對這三種濾波方式進行對比。

關鍵詞：六軸陀螺儀；一階互補濾波；卡爾曼濾波；DMP（Digital Motion Processor）

中圖分類號：TP302 文獻標識碼：A 文章編號：1009-3044（2018）03-0243-02

陀螺儀是自動控制系統中的一個信號傳感器，因為它成本低、體積小、重量輕等優點被廣泛運用于飛行器行業，為飛行器提供準確的方位、水平、位置、速度和加速度等信號。本文通過對六軸陀螺儀姿態解算的方法分析，來闡述六軸陀螺儀如何向飛行器的單片機反饋信號，同時在匿名上位機該仿真軟件的幫助下，我們會分別使用三種算法對六軸陀螺儀的波形進行仿真測試并記錄，從而來分析各個濾波方式的優劣[1]。

1 一階互補濾波

因為MPU6050收集獲得的加速度和角速度數據會被傳感器噪聲信號的影響，從而使得我們不能直觀的通過加速度和角速度這兩個數據來獲得該軸上的角度。所以我們需要對加速度和角速度這兩個數據進行互補濾波來近似得到一個比較準確的角度。

互補濾波算法可以同時濾除低頻和高頻的干擾，能更好地實現傳感器的數據融合，以下為一階互補濾波的實函數

voidfilter（float angle_m， float gyro_m）

{angle = K1 * angle_m+ （1-K1） * （angle + gyro_m * dt）；

//本次濾波的輸出值 = 本次采樣值 + 上次濾波的輸出值

}

公式中angle_m和gyro_m分別是經過陀螺儀采集數據計算后得到的角度與角加速度；K1是對加速度計取值的權重；dt是濾波器的采樣時間。在獲得上一次濾波的輸出值以及本次陀螺儀采集得到數據的情況下，我們就可以通過該公式獲得本次濾波后的輸出值[2]。

2 卡爾曼濾波

在測量方差已知的情況時，卡爾曼濾波能夠從測量噪聲的數據中估計動態系統的狀態，所以卡爾曼濾波對于六軸陀螺儀收集的動態數據的處理有很大的幫助[3]。

首先，卡爾曼濾波需要一個離散控制過程的系統，這一過程可以使用一個線性隨機微分方程來描述：

上述方程中k表示一個實際的值，也就是第k時刻的真實量，比如X（k）為第k時刻系統狀態、Z（k）為第k時刻測量值，而U（k）則為第k時刻對系統的控制量。A和B為系統參數，是相對于多模型系統的矩陣；H為測量系統的參數，是相對于多測量系統的矩陣。W（k）和V（k）分別為過程中的噪音及測量中的噪聲。它們被假設為高斯白噪聲（White Gaussian Noise），他們的協方差（covariance）分別是Q和R。若以上條件滿足，那么卡爾曼濾波將比較理想。

在該系統中，需利用該過程模型去預測下一狀態的系統，也即下一狀態結果=上一狀態結果+現在上狀態控制量（控制量可為0），公式為：

接著在系統結果已經更新之后，我們需要對協方差（covariance）進行更新，至于如何更新協方差呢，這就要使用到X（k|k-1）、X（k-1|k-1）的協方差，也就是下一狀態和上一狀態的協方差，其公式為：

其中C表示協方差，A表示A的轉置矩陣，Q是系統過程的協方差。

隨著得到對系統的預測結果之后，我們便需要開始收集現在狀態的測量值。結合預測的結果及收集好的測量值，便可得到現今的最優估算值，相應公式為：

上式中Kg為卡爾曼增益（Kalman Gain），由協方差和H矩陣計算得到，相應的公式為：

有了第k時刻狀態下最優的估算值X（k|k），我們還需更新這第k時刻狀態下的協方差以便卡爾曼濾波能運行下去直至系統過程結束，該過程公式為：

其中I為1的矩陣，對于單模型單測量，I=1。C（k|k）是系統進入k+1狀態時上述公式中的C（k-1|k-1）[4]。

3 DMP（Digital Motion Processor）

DMP（Digital Motion Processor）為MPU6050 自帶的數字運動處理器硬件加速引擎，通過I2C接口可以輸出6軸姿態數據。同時，InvenSense 公司提供了相應的嵌入式運動驅動庫，結合 DMP可以將原始數據直接轉換成四元數輸出。而通過四元數可以計算出歐拉角，即航向角（yaw）、橫滾角（roll）和俯仰角（pitch）。使用內置的 DMP，不但可以讓6軸的代碼設計更加簡潔，而且省略了 MCU的姿態解算過程。可以技巧有效的降低 MCU負擔，進而提高系統實時性。

在對四元數進行相應的格式轉換后，可以采用如下公式計算歐拉角

pitch=asin（-2 * q1 * q3 + 2 * q0* q2）* 57.3； //俯仰角

roll=atan2（2 * q2 * q3 + 2 * q0 * q1， -2 * q1 * q1 - 2 * q2* q2 + 1）* 57.3； //橫滾角

yaw=atan2（2*（q1*q2 + q0*q3），q0*q0+q1*q1-q2*q2-q3*q3） * 57.3； //航向角

其中quat[0]～quat[3]是 MPU6050 的 DMP 解算后的四元數，q30格式，所以要除以一個2的30次方，其 q30是一個常量：1073741824，即2的30次方，然后帶入公式，計算得到歐拉角[5]。

4 對比解析

以上對比可知，一階互補濾波計算量小跟隨性好，卡爾曼計算量大，動態性能更優，四元數算法輸出的是三個量 Pitch、Roll 和 Yaw，數據較為平滑。 參考文獻：

[1] 郭秀中.陀螺儀理論及應用[M].航空工業出版社，1987.

[2] 郭曉鴻，楊忠，陳喆，等.EKF和互補濾波器在飛行姿態確定中的應用[J].傳感器與微系統，2011，30（11）.

[3] 宋文堯.卡爾曼濾波[M].科學出版社，1991.

[4] 孔令磊，湯潔.由TMS320C32芯片實現陀螺儀漂移卡爾曼濾波算法研究[J].計算機工程與設計， 2008，29（7）.

[5] 張允華.互補濾波器在四元數法姿態解算中的應用[J].通訊世界，2015，35.