基于STM32的姿態(tài)測(cè)量系統(tǒng)設(shè)計(jì)

李世光 王文文 申夢(mèng)茜 高正中 肖佳宜

摘 要： 以無(wú)人機(jī)航姿測(cè)量系統(tǒng)小型化、量輕化為背景，設(shè)計(jì)了一種基于微慣性單元MEMS的姿態(tài)測(cè)量系統(tǒng)。系統(tǒng)以STM32F103C8T6為主控制器，通過(guò)I2C總線分別采集慣性測(cè)量單元MPU6050和數(shù)字羅盤HMC5883L的測(cè)量數(shù)據(jù)，利用數(shù)據(jù)融合算法解算無(wú)人機(jī)當(dāng)前姿態(tài)。對(duì)于MEMS溫漂和噪聲干擾的問(wèn)題，提出了一種基于四元數(shù)的互補(bǔ)濾波算法，對(duì)測(cè)量的姿態(tài)數(shù)據(jù)進(jìn)行補(bǔ)償修正。實(shí)驗(yàn)結(jié)果表明，該姿態(tài)測(cè)量系統(tǒng)簡(jiǎn)單可靠、性能穩(wěn)定、精確度高，成功完成了姿態(tài)的最優(yōu)控制。

關(guān)鍵詞： MEMS； STM32； 姿態(tài)測(cè)量系統(tǒng)； 互補(bǔ)濾波

中圖分類號(hào)： TN98?34； TP249 文獻(xiàn)標(biāo)識(shí)碼： A 文章編號(hào)： 1004?373X（2016）09?0012?03

Abstract： An attitude measurement system based on micro inertial unit MEMS was designed by taking the miniaturization and light weight UAV attitude measurement system as the background. The STM32F103C8T6 is taken as the main controller in the system to acquire the measurement data of the inertial measurement unit MPU6050 and digital compass HMC5883L through I2C bus， and then the data fusion algorithm is used to solve the UVA current attitude. For the problems of MEMS temperature drift and noise interference， a complementary filtering algorithm based on quaternion is proposed to compensate and correct the measured attitude data. The experimental results show that the attitude measurement system is simple and reliable， and has stable property and high precision. The attitude optimal control was successfully completed.

Keywords： MEMS； STM32； attitude measurement system； complementary filtering

0 引 言

小型無(wú)人機(jī)在國(guó)防建設(shè)和國(guó)民經(jīng)濟(jì)中具有廣闊的應(yīng)用前景，是當(dāng)前的研究熱點(diǎn)之一[1]。姿態(tài)的精確測(cè)量則是獲得良好飛行品質(zhì)的基礎(chǔ)。體積小、重量輕、集成度高的MEMS傳感器是完成微型姿態(tài)測(cè)量系統(tǒng)的最佳選擇，但同時(shí)這類傳感器有一定的缺陷。陀螺儀具有良好的高頻動(dòng)態(tài)響應(yīng)特性，但受振動(dòng)容易引起測(cè)量噪聲，且輸出存在溫漂和累計(jì)誤差，解算時(shí)姿態(tài)易發(fā)散，只適合做短時(shí)間的測(cè)量；加速度計(jì)與磁阻傳感器在低頻段動(dòng)態(tài)響應(yīng)特性好，能得到不隨時(shí)間累計(jì)誤差的姿態(tài)角，但在高頻段動(dòng)態(tài)響應(yīng)特性差。因此綜合陀螺儀、加速度和磁阻傳感器各自的頻率響應(yīng)優(yōu)勢(shì)，從頻域角度對(duì)3個(gè)傳感器數(shù)據(jù)進(jìn)行融合，有利于使傳感器信息在時(shí)間和空間上相互補(bǔ)充，提高傳感信息的可靠性[2]。

系統(tǒng)由主控制器（STM32F103C8）、六自由度慣性測(cè)量單元（MPU6050）和數(shù)字羅盤（HMC5883L）組成。MEMS主要作用是實(shí)時(shí)監(jiān)測(cè)機(jī)器人的原始姿態(tài)信息并將模擬測(cè)量信號(hào)轉(zhuǎn)化為數(shù)字信號(hào)輸出，主控制器及其外圍電路的主要任務(wù)是實(shí)時(shí)采集傳感器輸出的數(shù)字測(cè)量信號(hào)以及融合原始姿態(tài)數(shù)據(jù)，得出俯仰角、翻滾角和航向角的姿態(tài)信息[3]。姿態(tài)控制系統(tǒng)硬件結(jié)構(gòu)如圖1所示。

1 主要模塊

1.1 微處理器模塊

處理器模塊是整個(gè)控制系統(tǒng)的核心，主要任務(wù)是提取各個(gè)傳感器的信息，利用數(shù)據(jù)融合算法解算出當(dāng)前姿態(tài)。姿態(tài)測(cè)量系統(tǒng)中數(shù)據(jù)釆集與處理的主控制器采用STM32F103C8T6，該芯片基于超低功耗的CortexM3內(nèi)核，32位字寬，工作頻率高達(dá)72 MHz，擁有80個(gè)快速I/O端口，9個(gè)標(biāo)準(zhǔn)的通信接口（2個(gè)I2C接口，3個(gè)USURT，2個(gè)SPI，一個(gè)CAN接口）提供了豐富的外設(shè)資源，便于系統(tǒng)的開發(fā)和擴(kuò)展（STM32）。因此，該處理器模塊滿足對(duì)多傳感器數(shù)據(jù)同步釆集以及數(shù)據(jù)融合算法運(yùn)算速度的要求。

1.2 姿態(tài)傳感器模塊

MPU6050六軸傳感器芯片集成了3軸MEMS陀螺儀和3軸MEMS加速度計(jì)，測(cè)量范圍達(dá)±16 g，其高分辨率（3.9 mg/LSB）能夠測(cè)量不到1°的傾斜角度變化。芯片正常工作時(shí)，陀螺儀和加速度計(jì)分別采集[x]軸，[y]軸和[z]軸的電壓值，然后通過(guò)一個(gè)16位A/D轉(zhuǎn)換器，轉(zhuǎn)換成數(shù)字信號(hào)傳送到處理器，但此時(shí)得到的值并不是實(shí)際的角度和角速度值，還必須經(jīng)過(guò)一定的比例關(guān)系進(jìn)行轉(zhuǎn)換，才能得到實(shí)際的角度和角速度值[4]。

MHC5883L是由美國(guó)Honeywell公司生產(chǎn)的三軸磁場(chǎng)強(qiáng)度敏感器件，內(nèi)置ASIC放大器，工作頻率達(dá)160 Hz，羅盤航向精度精確到1°～2°，能在±8高斯的磁場(chǎng)中實(shí)現(xiàn) 5毫高斯分辨率，I2C總線輸出。通過(guò)磁阻傳感器測(cè)量地磁信息，主要用于測(cè)量地磁場(chǎng)矢量，定位設(shè)備的方向。

2 基于四元數(shù)的姿態(tài)解算

2.1 姿態(tài)的坐標(biāo)描述

3 多傳感器的數(shù)據(jù)融合

數(shù)據(jù)融合算法設(shè)計(jì)是姿態(tài)測(cè)量系統(tǒng)中重要的環(huán)節(jié)。在本文的姿態(tài)測(cè)量系統(tǒng)中，互補(bǔ)濾波算法的主要思路是將加速度計(jì)和電子羅盤分別相對(duì)于重力加速度矢量與地磁場(chǎng)矢量的姿態(tài)角疊加到由陀螺儀高速積分得到的角增量中，利用翻滾角[?]補(bǔ)償[x]軸的角速度[wx，]俯仰角[θ]補(bǔ)償[y]軸的角速度[wy，]偏航角[φ]補(bǔ)償[z]軸的角速度[4][wz。]設(shè)置互補(bǔ)濾波器的傳遞函數(shù)為：

式中：[G（s）]表示姿態(tài)的方向余弦矩陣；互補(bǔ)濾波器計(jì)算輸出的姿態(tài)方向余弦矩陣為[R；][C0]表示由加速度計(jì)和數(shù)字羅盤觀測(cè)到的姿態(tài)方向余弦矩陣；[WH]表示加速度計(jì)和數(shù)字羅盤的高頻觀測(cè)噪聲；[C0=R+WH；][C1]表示由陀螺儀計(jì)算得到的姿態(tài)信息，[C1=R+WL，][WL]表示陀螺儀的低頻累積誤差。

因此，互補(bǔ)濾波算法在傳感器數(shù)據(jù)整合上，能同時(shí)濾除低頻和高頻干擾，在一定程度上提高了系統(tǒng)精確度[5]?；パa(bǔ)濾波器大多采用比例或比例積分PI補(bǔ)償方法，[G（s）]若取常數(shù)[k，]則設(shè)計(jì)的低通濾波器和高通濾波器的截止頻率為[f=k2π，]當(dāng)[k]較大時(shí)，截止頻率[f]較高，則濾除低頻的、幅值較大的運(yùn)動(dòng)加速度效果較差；[k]較小時(shí)，截止頻率[f]較低，易受陀螺儀的時(shí)間漂移造成誤差影響[6]。由于系統(tǒng)具有一定的可預(yù)見(jiàn)性，這里參考文獻(xiàn)[7]利用互補(bǔ)濾波器引入模糊機(jī)理實(shí)時(shí)推導(dǎo)參數(shù)[k]的方法。

4 實(shí)驗(yàn)結(jié)果與分析

在搭建的硬件平臺(tái)上實(shí)現(xiàn)上述濾波算法。通過(guò)硬件電路采集各傳感器的信號(hào)，并在STM32內(nèi)將陀螺儀、加速度計(jì)、電子羅盤3個(gè)傳感器的測(cè)量數(shù)據(jù)進(jìn)行解算和濾波[2]。通過(guò)串口傳輸?shù)絇C機(jī)上，實(shí)時(shí)顯示各姿態(tài)信息，最終運(yùn)用Matlab實(shí)現(xiàn)數(shù)據(jù)的處理。觀察采用互補(bǔ)濾波前后姿態(tài)角的變化。圖2是俯仰角融合前后對(duì)比圖，0～3 s為陀螺儀振動(dòng)情況，融合前后的角度信息非常接近，3～6 s穩(wěn)定情況下，信號(hào)具有偏差，該圖有效地說(shuō)明了加速度傳感器的數(shù)據(jù)可以糾正穩(wěn)態(tài)下的姿態(tài)信息。圖3說(shuō)明陀螺儀自身存在隨機(jī)漂移誤差，并且誤差會(huì)隨時(shí)間不斷積累，相對(duì)于融合前，融合后波形稍有延遲，相位有一定的變化，數(shù)據(jù)穩(wěn)定性有了很大改善。上述實(shí)驗(yàn)說(shuō)明采用互補(bǔ)濾波進(jìn)行數(shù)據(jù)融合，噪聲振幅小，抗干擾能力增強(qiáng)，數(shù)據(jù)更加穩(wěn)定和準(zhǔn)確。

5 結(jié) 論

本文設(shè)計(jì)的基于STM32的姿態(tài)角測(cè)量系統(tǒng)，利用四元數(shù)法進(jìn)行姿態(tài)解算，并通過(guò)互補(bǔ)濾波進(jìn)行數(shù)據(jù)融合，有效綜合了陀螺儀、加速度計(jì)、磁力計(jì)在姿態(tài)測(cè)量中的優(yōu)缺點(diǎn)，不僅提高了姿態(tài)測(cè)量的精確度，而且增強(qiáng)了系統(tǒng)的總體性能和容錯(cuò)能力，為無(wú)人機(jī)完成各種飛行任務(wù)提供了根本保證。

參考文獻(xiàn)

[1] 余國(guó)林，陳繼平，余濤，等.無(wú)人機(jī)航空遙感平臺(tái)機(jī)載作業(yè)控制系統(tǒng)設(shè)計(jì)[J].現(xiàn)代電子技術(shù)，2012，35（4）：132?135.

[2] 萬(wàn)曉鳳，康利平，余運(yùn)俊，等.互補(bǔ)濾波算法在四旋翼飛行器姿態(tài)解算中的應(yīng)用[J].測(cè)控技術(shù)，2015，34（2）：8?10.

[3] 萬(wàn)良金.基于多傳感器信息融合的機(jī)器人姿態(tài)測(cè)量技術(shù)研究[D].北京：北京交通大學(xué)，2015.

[4] 賴義漢，王凱.基于MPU6050的雙輪平衡車控制系統(tǒng)設(shè)計(jì)[J].河南工程學(xué)院院報(bào)，2014，26（1）：53?57.

[5] 劉輝邦，褚金奎，支煒，等.基于STM32的無(wú)人機(jī)姿態(tài)測(cè)量系統(tǒng)設(shè)計(jì)[J].傳感器與微系統(tǒng)，2013，32（8）：108?110.

[6] 鄭健.基于9軸傳感器的姿態(tài)參考系統(tǒng)研究與實(shí)現(xiàn)[D].成都：電子科技大學(xué)，2013.

[7] 呂印新，肖前貴，胡壽松.基于四元數(shù)互補(bǔ)濾波的無(wú)人機(jī)姿態(tài)解算[J].燕山大學(xué)學(xué)報(bào)，2014，38（2）：175?180.