基于ARM與MPU6050的測(cè)姿系統(tǒng)設(shè)計(jì)*

黃金鵬， 尚俊娜， 岳克強(qiáng)

(1.杭州電子科技大學(xué) 通信工程學(xué)院，浙江 杭州 310018；2.杭州電子科技大學(xué) 電子信息學(xué)院，浙江 杭州 310018)

0 引 言

姿態(tài)檢測(cè)在導(dǎo)航控制、智能設(shè)備、醫(yī)療器械、基站天線等范疇得到廣泛的應(yīng)用[1]。國(guó)內(nèi)外對(duì)姿態(tài)測(cè)量這一熱點(diǎn)進(jìn)行了大量探索和研究。浙江大學(xué)利用加速度計(jì)傳感器對(duì)人體實(shí)時(shí)監(jiān)測(cè),實(shí)現(xiàn)了人體跌倒監(jiān)測(cè)[2]。國(guó)防科技大學(xué)利用慣性器件傳感器，實(shí)現(xiàn)了對(duì)機(jī)器人水平姿態(tài)的檢測(cè)[3]。哈爾濱工業(yè)大學(xué)同樣利用慣性傳感器對(duì)機(jī)器人進(jìn)行姿態(tài)的檢測(cè)，并且取得了卓著的成果[4]。美國(guó)密歇根大學(xué)的機(jī)械工程系通過將姿態(tài)檢測(cè)模塊安裝到棒球及壘球的內(nèi)部，實(shí)現(xiàn)了實(shí)時(shí)監(jiān)測(cè)棒球或者壘球在運(yùn)動(dòng)過程中的運(yùn)動(dòng)軌跡[5]。

本文利用基于微機(jī)電系統(tǒng)(micro-electro-mechanical system,MEMS)技術(shù)的傳感器MPU6050進(jìn)行姿態(tài)解算，其方法可以分為兩種:利用卡爾曼濾波或者如互補(bǔ)濾波等其他的濾波法[6，7]，融合由內(nèi)置加速度計(jì)和陀螺儀分別計(jì)算得到的角度信息，以得到最終的姿態(tài)結(jié)果；通過MPU6050自帶的數(shù)字運(yùn)動(dòng)處理器，即數(shù)字運(yùn)動(dòng)處理 (digital motion processing，DMP)，將測(cè)量獲得的原始數(shù)據(jù)直接進(jìn)行轉(zhuǎn)換，轉(zhuǎn)換成四元數(shù)輸出，再通過四元數(shù)轉(zhuǎn)換為歐拉角，求解姿態(tài)信息。由于DMP僅需要三角函數(shù)運(yùn)算即可將四元數(shù)轉(zhuǎn)換為歐拉角，而卡爾曼濾波運(yùn)算復(fù)雜度較高，并且利用DMP不需要調(diào)節(jié)卡爾曼參數(shù)，數(shù)據(jù)會(huì)更加穩(wěn)定準(zhǔn)確。利用廣義延拓插值模型對(duì)解算的姿態(tài)信息進(jìn)行插值擬合處理，以消除測(cè)試數(shù)據(jù)的抖動(dòng)誤差，從而使得最終結(jié)果更加接近真實(shí)的待測(cè)角度信息。

1 測(cè)姿系統(tǒng)硬件平臺(tái)設(shè)計(jì)

測(cè)姿系統(tǒng)的硬件設(shè)計(jì)和實(shí)物如圖1所示。其中核心板LPC1788基于恩智浦ARM Cortex—M3的微控制器，專門用于處理要求高集成度和低功耗的嵌入式應(yīng)用[8,9]；LPC1788還增加了一個(gè)專用的Flash加速器，使Flash中代碼能執(zhí)行到最佳性能，該芯片處理速度快，其時(shí)鐘頻率最高可達(dá)120 MHz，并且還集成了大量通用輸入輸出接口，從而使其特別適于控制多個(gè)傳感器的應(yīng)用和數(shù)據(jù)處理。角度采集模塊采用InvenSence公司生產(chǎn)的型號(hào)為MPU6050的9軸傳感器芯片，其集成了三軸陀螺儀及三軸加速度傳感器，大幅減小了由于焊接和安裝所造成的軸間系統(tǒng)誤差，提供了更加準(zhǔn)確的實(shí)驗(yàn)測(cè)試環(huán)境。通信方式為內(nèi)部集成電路(inter-integrated circuit,I2C)總線，外接的SDRAM的存儲(chǔ)空間可達(dá)256 MB，工作頻率可達(dá)133 MHz，SDRAM存儲(chǔ)介質(zhì)更有利于進(jìn)行數(shù)據(jù)存儲(chǔ)和轉(zhuǎn)移分析，LCD面板可以實(shí)時(shí)觀察和控制系統(tǒng)的運(yùn)行狀態(tài)；通過硬件平臺(tái)上的串口與PC相連，可以通過上位機(jī)軟件直觀地對(duì)測(cè)試數(shù)據(jù)進(jìn)行實(shí)時(shí)分析，根據(jù)數(shù)據(jù)的波動(dòng)情況和特點(diǎn)來了解硬件系統(tǒng)的工作性能，可以有目的性地對(duì)ARM芯片內(nèi)部的算法進(jìn)行微調(diào)整，從而可以得到更加準(zhǔn)確和穩(wěn)定的測(cè)試結(jié)果。

圖1 測(cè)姿系統(tǒng)硬件設(shè)計(jì)方框圖及實(shí)物

I2C總線有2根信號(hào)線：SDA(數(shù)據(jù)線)和SCL(時(shí)鐘線)；I2C總線無信息發(fā)送時(shí)，SCL,SDA均為高電平。I2C總線數(shù)據(jù)傳輸時(shí)序如圖2所示，由SDA和SCL的電平拉低和拉高控制，其中包括信號(hào)的啟動(dòng)和結(jié)束，信號(hào)應(yīng)答及寫/讀時(shí)序。最關(guān)鍵的一點(diǎn)，在數(shù)據(jù)傳送過程中，當(dāng)時(shí)鐘信號(hào)保持在高電平時(shí)，數(shù)據(jù)線必須保持穩(wěn)定不變，只有當(dāng)時(shí)鐘信號(hào)跳變到并維持在低電平時(shí)，數(shù)據(jù)線上的數(shù)據(jù)才可以允許發(fā)生變化。

圖2 I2C總線數(shù)據(jù)發(fā)送時(shí)序示意

2 測(cè)姿系統(tǒng)軟件設(shè)計(jì)

在MDK環(huán)境下采用C語言編寫，可采用JTAG仿真器對(duì)程序進(jìn)行在線調(diào)試和燒寫，也可采用2種其他的燒錄方式，包括在線系統(tǒng)編程(in system programming,ISP)、在線應(yīng)用編程(in application programming,IAP)。初始化LPC1788_I2C接口，重點(diǎn)對(duì)MPU6050相關(guān)寄存器實(shí)施初始化，包括MPU6050的復(fù)位、使能加速度計(jì)和陀螺儀、加速計(jì)與陀螺儀的量程范圍選擇等其他參數(shù)；使能DMP，只有成功加載了DMP固件并使能后，才能將加速度計(jì)的分量與陀螺儀的分量融合并輸出四元數(shù)[10]；根據(jù)四元數(shù)轉(zhuǎn)化為歐拉角，最終輸出姿態(tài)解算的結(jié)果。

2.1 DMP功能移植

DMP是MPU6050內(nèi)部的運(yùn)動(dòng)引擎，由InvenSence公司自主提供，用于從內(nèi)部傳感器中直接解算出四元數(shù)，大幅降低運(yùn)算復(fù)雜度。由于DMP可直接輸出四元數(shù)，從而可以減輕外圍微處理器的工作負(fù)擔(dān)，且能避免繁瑣的濾波和數(shù)據(jù)融合處理，能降低系統(tǒng)運(yùn)算的復(fù)雜度[11]。此外DMP的運(yùn)動(dòng)引擎功能保存于主處理器的易失性內(nèi)存之中，斷電即會(huì)關(guān)閉其功能，所以每次上電啟動(dòng)DMP功能，均需要對(duì)其進(jìn)行初始化設(shè)置。加載并啟動(dòng)DMP運(yùn)動(dòng)引擎功能的具體步驟如下：

1)利用dmp_load_motion_driver_firmware()命令，將DMP功能加載到MPU的內(nèi)存；

2)通過dmp_set_orientation()命令以刷新定位矩陣，使得加速度計(jì)及陀螺儀的定位矩陣成功的寫入DMP之中；

3)通過dmp_set_fifo_rate()命令設(shè)置DMP輸出頻率，最大可設(shè)置頻率為200 Hz；

4)當(dāng)DMP檢測(cè)到運(yùn)動(dòng)或撞擊時(shí)，會(huì)觸發(fā)DMP回調(diào)自檢功能，所用命令為run_self_test()；

5)通過命令mpu_set_dmp_state()使能DMP的功能。

只有上述步驟全部完成，即成功加載了DMP驅(qū)動(dòng)后，方可輸出四元數(shù)數(shù)據(jù)，最后LPC1788根據(jù)四元數(shù)解算系統(tǒng)的姿態(tài)結(jié)果。

2.2 姿態(tài)角解算

當(dāng)讀取MPU6050各軸的角速度分量、加速度分量后，經(jīng)過DMP即可直接融合并輸出四元數(shù)，采用q30格式，放大到230[12]。此時(shí)，若轉(zhuǎn)換為歐拉角，操作如下：

q0=quat[0] / q30;

q1=quat[1] / q30;

q2=quat[2] / q30;

q3=quat[3] / q30;

Pitch=arcsin(-2×q1×q3+2×q0×q2)× 57.3; ∥俯仰角，繞Y軸轉(zhuǎn)動(dòng)

Roll=arctan 2(2×q2×q3+2×q0×q1, -2×q1×q1-2×q2×q2+1)× 57.3;∥橫滾角，繞X軸轉(zhuǎn)動(dòng)

Yaw=arctan 2(2×(q1×q2+q0×q3),q0×q0+q1×q1-q2×q2-q3×q3)×57.3;∥航向角，繞Z軸轉(zhuǎn)動(dòng)

其中 quat[0]～quat[3]為MPU6050的DMP解算后的四元數(shù)，q30格式；q30為常量，即230，為1 073 741 824；57.3為弧度轉(zhuǎn)換為角度，即180/π，最終轉(zhuǎn)換結(jié)果以度(°)為單位。

3 廣義延拓插值模型的應(yīng)用

將測(cè)姿系統(tǒng)固定于三維電動(dòng)轉(zhuǎn)臺(tái)上，分別沿著x，y，z軸各旋轉(zhuǎn)一定角度得到3個(gè)姿態(tài)角。實(shí)驗(yàn)隨機(jī)選取航向角、橫滾角、俯仰角分別為：68.50°，-86.22°和2.55°。在靜態(tài)測(cè)試環(huán)境下，將測(cè)姿系統(tǒng)通過廣義延拓插值模型進(jìn)行解算和外推，測(cè)試2 000組數(shù)據(jù)，共計(jì)2 000 s的時(shí)間。

廣義延拓插值模型，通過采用數(shù)據(jù)逼近的方法盡量反映出傳感器在實(shí)測(cè)環(huán)境下的角度真實(shí)值[13]。廣義延拓插值能夠做到較插值和擬合方法逼近精度更高，能更加精準(zhǔn)地逼近描述真實(shí)的函數(shù)圖像，通過對(duì)MPU6050輸出結(jié)果進(jìn)行廣義延拓外推的處理，使得最終的測(cè)量結(jié)果更加逼近真實(shí)的姿態(tài)值。廣義延拓插值模型應(yīng)用的具體操作步驟如下：

1)選用擬合函數(shù)為二次多項(xiàng)式Φ(x)=a1x2+a2x+a3;

2)將PU6050輸出的姿態(tài)測(cè)量結(jié)果作為廣義延拓的插值處理點(diǎn)；

3)通過最終的角姿態(tài)測(cè)量結(jié)果求出擬合的多項(xiàng)式系數(shù)a1～a3；

4)得到插值處理后的擬合姿態(tài)曲線，作為最新的姿態(tài)測(cè)量值，穩(wěn)定輸出。

廣義延拓模型的處理結(jié)果如圖3所示，分別為航向角、翻滾角、俯仰角姿態(tài)測(cè)量值曲線及其相應(yīng)的廣義延拓插值模型處理后的函數(shù)Φ(x)曲線。

圖3 姿態(tài)測(cè)量值曲線及廣義延拓插值處理后曲線

如圖3所示，測(cè)姿系統(tǒng)保持平穩(wěn)輸出角度。理論上，測(cè)姿系統(tǒng)輸出的3個(gè)角度數(shù)據(jù)應(yīng)繪制為一條直線，但本系統(tǒng)使用的低成本MEMS傳感器精度有限，使得測(cè)量結(jié)果與真實(shí)值存在偏差，并且航向角較俯仰角和橫滾角的偏差較大。廣義延拓插值法處理后的角度曲線能有效擬合實(shí)測(cè)數(shù)據(jù)，從而更加準(zhǔn)確逼近真實(shí)的角度值，可以消除傳感器抖動(dòng)誤差。3個(gè)角度的標(biāo)準(zhǔn)差如表1所示。

表1 3個(gè)角度的標(biāo)準(zhǔn)差 (°)

4 結(jié)果分析

通過角度測(cè)試儀進(jìn)行角度的標(biāo)準(zhǔn)測(cè)試，測(cè)姿系統(tǒng)與測(cè)試儀放置于同一水平臺(tái)面上，測(cè)試儀的精度可達(dá)0.01°。針對(duì)偏差最小的橫滾角進(jìn)行角度測(cè)試，測(cè)試儀和測(cè)姿系統(tǒng)固定于轉(zhuǎn)動(dòng)平臺(tái)上，平臺(tái)不添加額外的振動(dòng)源，進(jìn)行緩慢翻轉(zhuǎn)。如表2所示，通過對(duì)比發(fā)現(xiàn)，本系統(tǒng)測(cè)得角度值與測(cè)試儀角度值相吻合，偏差小于0.1°。

表2 實(shí)測(cè)角度偏差 (°)

5 結(jié)束語

采用MPU6050傳感器，通過對(duì)DMP的成功加載，以ARM板為平臺(tái)設(shè)計(jì)的測(cè)姿系統(tǒng)，能夠以小于0.1°的最小偏差進(jìn)行角度的測(cè)量和采集。通過廣義延拓插值模型進(jìn)行處理，獲得更加逼近真實(shí)值的角度值，消除了抖動(dòng)誤差。有效地消除了振動(dòng)干擾，抑制了漂移。

參考文獻(xiàn):

[1] 胡 煉,趙祚喜,吳曉鵬.ADIS16300四自由度IMU在姿態(tài)測(cè)量中的應(yīng)用[J].電子設(shè)計(jì)工程,2010,18(7):103-106.

[2] 文耀鋒.一種實(shí)時(shí)的跌倒姿態(tài)檢測(cè)和心率監(jiān)護(hù)系統(tǒng)的研究[D].杭州:浙江大學(xué),2008:2-3.

[3] 秦 勇,臧希喆,王曉宇,等.基于MEMS慣性傳感器的機(jī)器人姿態(tài)檢測(cè)系統(tǒng)的研究[J].傳感技術(shù)學(xué)報(bào),2007,20(2):298-301.

[4] 何 偉,陳 偉.基于MEMS慣性傳感器的機(jī)器人水平姿態(tài)檢測(cè)系統(tǒng)設(shè)計(jì)[J].機(jī)器人技術(shù)與應(yīng)用,2009(5):51-55.

[5] Mcginnis R S,Perkins N C.A highly miniaturized, wireless inertial measurement unit for characterizing the dynamics of pitched baseballs and softballs[J].Sensors,2012,12(9):11933-11945.

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

[7] 梁延德,程 敏,何福本,等.基于互補(bǔ)濾波器的四旋翼飛行器姿態(tài)解算[J].傳感器與微系統(tǒng),2011,30(11):56-58,61.

[8] 蔣祺煒.基于LPC1788芯片的OLED模組驅(qū)動(dòng)系統(tǒng)的設(shè)計(jì)與實(shí)現(xiàn)[D].成都：電子科技大學(xué),2014:1-5.

[9] 劉鐵勇,赫劉勤.基于LPC1788立體車庫(kù)控制器設(shè)計(jì)[J].電氣技術(shù),2016(10):77-80.

[10] 楊 磊.基于MEMS運(yùn)動(dòng)傳感器的姿態(tài)檢測(cè)系統(tǒng)設(shè)計(jì)[D].蘇州：蘇州大學(xué),2015:2-6.

[11] 王東云,李 珂,但永平.基于MPU6050的四旋翼飛行器的設(shè)計(jì)[J].自動(dòng)化應(yīng)用,2015(9):89-91，94.

[12] 林 宇,方 方,林澤萍,等.基于MPU6050三軸傾角測(cè)控設(shè)計(jì)[J].數(shù)字技術(shù)與應(yīng)用,2016(1):181.

[13] 施滸立,顏毅華,徐國(guó)華.工程科學(xué)中的廣義延拓逼近法[M].北京:科學(xué)出版社，2005.