六軸陀螺儀姿態(tài)解算研究

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

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

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

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

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

1 一階互補濾波

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

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

voidfilter（float angle_m， float gyro_m）

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

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

}

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

2 卡爾曼濾波

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

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

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

在該系統(tǒng)中，需利用該過程模型去預(yù)測下一狀態(tài)的系統(tǒng)，也即下一狀態(tài)結(jié)果=上一狀態(tài)結(jié)果+現(xiàn)在上狀態(tài)控制量（控制量可為0），公式為：

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

其中C表示協(xié)方差，A表示A的轉(zhuǎn)置矩陣，Q是系統(tǒng)過程的協(xié)方差。

隨著得到對系統(tǒng)的預(yù)測結(jié)果之后，我們便需要開始收集現(xiàn)在狀態(tài)的測量值。結(jié)合預(yù)測的結(jié)果及收集好的測量值，便可得到現(xiàn)今的最優(yōu)估算值，相應(yīng)公式為：

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

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

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

3 DMP（Digital Motion Processor）

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

在對四元數(shù)進行相應(yīng)的格式轉(zhuǎn)換后，可以采用如下公式計算歐拉角

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 解算后的四元數(shù)，q30格式，所以要除以一個2的30次方，其 q30是一個常量：1073741824，即2的30次方，然后帶入公式，計算得到歐拉角[5]。

4 對比解析

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

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

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

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

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

[5] 張允華.互補濾波器在四元數(shù)法姿態(tài)解算中的應(yīng)用[J].通訊世界，2015，35.