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

馬陟昶 王勤湧 郭張樂 范鵬宇 項(xiàng)暢清

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

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

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

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

1 一階互補(bǔ)濾波

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

互補(bǔ)濾波算法可以同時(shí)濾除低頻和高頻的干擾，能更好地實(shí)現(xiàn)傳感器的數(shù)據(jù)融合，以下為一階互補(bǔ)濾波的實(shí)函數(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ù)計(jì)算后得到的角度與角加速度；K1是對(duì)加速度計(jì)取值的權(quán)重；dt是濾波器的采樣時(shí)間。在獲得上一次濾波的輸出值以及本次陀螺儀采集得到數(shù)據(jù)的情況下，我們就可以通過該公式獲得本次濾波后的輸出值[2]。

2 卡爾曼濾波

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

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

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

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

接著在系統(tǒng)結(jié)果已經(jīng)更新之后，我們需要對(duì)協(xié)方差（covariance）進(jìn)行更新，至于如何更新協(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é)方差。

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

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

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

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

3 DMP（Digital Motion Processor）

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

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

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

4 對(duì)比解析

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

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

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

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

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

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