本文關(guān)鍵詞： 基于 STM32 姿態(tài) 測量 系統(tǒng) 設(shè)計 .doc　　

基于 STM32 的姿態(tài)測量系統(tǒng)設(shè)計 摘 要： 以無人機航姿測量系統(tǒng)小型化、量輕化為背景，設(shè)計了一種基于微慣性單元 MEMS 的姿態(tài)測量系統(tǒng)。系統(tǒng)以 STM32F103C8T6 為主控制器，通過 I2C 總線分別采集慣性測量單元 MPU6050 和數(shù)字羅盤 HMC5883L的測量數(shù)據(jù)，利用數(shù)據(jù)融合算法解算無人機當(dāng)前姿態(tài)。對于 MEMS 溫漂和噪聲干擾的問題，提出了一種基于四元數(shù)的互補濾波算法，對測量的姿態(tài)數(shù)據(jù)進行補償修正。實驗結(jié)果表明，該姿態(tài)測量系統(tǒng)簡單可靠、性能穩(wěn)定、精確度高，成功完成了姿態(tài)的最優(yōu)控制。 關(guān)鍵詞： MEMS； STM32； 姿態(tài)測量系統(tǒng)； 互補濾波 中圖分類號： TN98?34； TP249 文獻(xiàn)標(biāo)識碼： A 文章編號： 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 引 言 小型無人機在國防建設(shè)和國民經(jīng)濟中具有廣闊的應(yīng)用前景，是當(dāng)前的研究熱點之一[1]。姿態(tài)的精確測量則是獲得良好飛行品質(zhì)的基礎(chǔ)。體積小、重量輕、集成度高的 MEMS 傳感器是完成微型姿態(tài)測量系統(tǒng)的最佳選擇，但同時這類傳感器有一定的缺陷。陀螺儀具有良好的高頻動態(tài)響應(yīng)特性，但受振動容易引起測量噪聲，且輸出存在溫漂和累計誤差，解算時姿態(tài)易發(fā)散，只適合做短時間的測量；加速度計與磁阻傳感器在低頻段動態(tài)響應(yīng)特性好，能得到不隨時間累計誤差的姿態(tài)角，但在高頻段動態(tài)響應(yīng)特性差。因此綜合陀螺儀、加速度和磁阻傳感器各自的頻率響應(yīng)優(yōu)勢，從頻域角度對 3 個傳感器數(shù)據(jù)進行融合，有利于使傳感器信息在時間和空間上相互補充，提高傳感信息的可靠性[2]。 系統(tǒng)由主控制器（STM32F103C8）、六自由度慣性測量單元（MPU6050）和數(shù)字羅盤（HMC5883L）組成。MEMS 主要作用是實時監(jiān)測機器人的原始姿態(tài)信息并將模擬測量信號轉(zhuǎn)化為數(shù)字信號輸出，主控制器及其外圍電路的主要任務(wù)是實時采集傳感器輸出的數(shù)字測量信號以及融合原始姿態(tài)數(shù)據(jù)，得出俯仰角、翻滾角和航向角的姿態(tài)信息[3]。姿態(tài)控制系統(tǒng)硬件結(jié)構(gòu)如 圖 1 所示。 1 主要模塊 1.1 微處理器模塊 處理器模塊是整個控制系統(tǒng)的核心，主要任務(wù)是提取各個傳感器的信息，利用數(shù)據(jù)融合算法解算出當(dāng)前姿態(tài)。姿態(tài)測量系統(tǒng)中數(shù)據(jù)?集與處理的主控制器采用 STM32F103C8T6，該芯片基于超低功耗的 CortexM3 內(nèi)核，32 位字寬，工作頻率高達(dá) 72 MHz，擁有 80 個快速 I/O 端口，9 個標(biāo)準(zhǔn)的通信接口（2 個 I2C 接口，3 個 USURT，2 個 SPI，一個 CAN 接口）提供了豐富的外設(shè)資源，便于系統(tǒng)的開發(fā)和擴展（STM32）。因此，該處理器模塊滿足對多傳感器數(shù)據(jù)同步?集以及數(shù)據(jù)融合算法運算速度的要求。 1.2 姿態(tài)傳感器模塊 MPU6050 六軸傳感器芯片集成了 3 軸 MEMS 陀螺儀和 3 軸 MEMS 加速度計，測量范圍達(dá)±16 g，其高分辨率（3.9 mg/LSB）能夠測量不到 1°的傾斜角度變化。芯片正常工作時，陀螺儀和加速度計分別采集[x]軸，[y]軸和[z]軸的電壓值，然后通過一個 16 位 A/D 轉(zhuǎn)換器，轉(zhuǎn)換成數(shù)字信號傳送到處理器，但此時得到的值并不是實際的角度和角速度值，還必須經(jīng)過一定的比例關(guān)系進行轉(zhuǎn)換，才能得到實際的角度和角速度值[4]。 MHC5883L 是由美國 Honeywell 公司生產(chǎn)的三軸磁場強度敏感器件，內(nèi)置 ASIC 放大器，工作頻率達(dá) 160 Hz，羅盤航向精度精確到 1°～2°，能在±8 高斯的磁場中實現(xiàn) 5 毫高斯分辨率，I2C 總線輸出。通過磁阻傳感器測量地磁信息，主要用于測量地磁場矢量，定位設(shè)備的方向。 2 基于四元數(shù)的姿態(tài)解算 2.1 姿態(tài)的坐標(biāo)描述 3 多傳感器的數(shù)據(jù)融合 數(shù)據(jù)融合算法設(shè)計是姿態(tài)測量系統(tǒng)中重要的環(huán)節(jié)。在本文的姿態(tài)測量系統(tǒng)中，互補濾波算法的主要思路是將加速度計和電子羅盤分別相對于重力加速度矢量與地磁場矢量的姿態(tài)角疊加到由陀螺儀高速積分得到的角增量中，利用翻滾角[?]補償[x]軸的角速度[wx，]俯仰角[θ]補償[y]軸的角速度[wy，]偏航角[φ]補償[z]軸的角速度[4][wz。]設(shè)置互補濾波器的傳遞函數(shù)為： 式中：[G（s）]表示姿態(tài)的方向余弦矩陣；互補濾波器計算輸出的姿態(tài)方向余弦矩陣為[R；][C0]表示由加速度計和數(shù)字羅盤觀測到的姿態(tài)方向 余弦 矩 陣 ；[WH] 表 示加 速 度 計和 數(shù) 字羅 盤 的 高頻 觀 測噪 聲；[C0=R+WH；][C1]表示由陀螺儀計算得到的姿態(tài)信息，[C1=R+WL，][WL]表示陀螺儀的低頻累積誤差。 因此，互補濾波算法在傳感器數(shù)據(jù)整合上，能同時濾除低頻和高頻干擾，在一定程度上提高了系統(tǒng)精確度[5]�；パa濾波器大多采用比例或比例積分 PI 補償方法，[G（s）]若取常數(shù)[k，]則設(shè)計的低通濾波器和高通濾波器的截止頻率為[f=k2π，]當(dāng)[k]較大時，截止頻率[f]較高，則濾除低頻的、幅值較大的運動加速度效果較差；[k]較小時，截止頻率[f]較低，易受陀螺儀的時間漂移造成誤差影響[6]。由于系統(tǒng)具有一定的可預(yù)見性，這里參考文獻(xiàn)[7]利用互補濾波器引入模糊機理實時推導(dǎo)參數(shù)[k]的方法。 4 實驗結(jié)果與分析 在搭建的硬件平臺上實現(xiàn)上述濾波算法。通過硬件電路采集各傳感器的信號，并在 STM32 內(nèi)將陀螺儀、加速度計、電子羅盤 3 個傳感器的測量 數(shù)據(jù)進行解算和濾波[2]。通過串口傳輸?shù)?PC 機上，實時顯示各姿態(tài)信息，最終運用 Matlab 實現(xiàn)數(shù)據(jù)的處理。觀察采用互補濾波前后姿態(tài)角的變化。圖 2 是俯仰角融合前后對比圖，，0～3 s 為陀螺儀振動情況，融合前后的角度信息非常接近，3～6 s 穩(wěn)定情況下，信號具有偏差，該圖有效地說明了加速度傳感器的數(shù)據(jù)可以糾正穩(wěn)態(tài)下的姿態(tài)信息。圖 3 說明陀螺儀自身存在隨機漂移誤差，并且誤差會隨時間不斷積累，相對于融合前，融合后波形稍有延遲，相位有一定的變化，數(shù)據(jù)穩(wěn)定性有了很大改善。上述實驗說明采用互補濾波進行數(shù)據(jù)融合，噪聲振幅小，抗干擾能力增強，數(shù)據(jù)更加穩(wěn)定和準(zhǔn)確。 5 結(jié) 論 本文設(shè)計的基于 STM32 的姿態(tài)角測量系統(tǒng)，利用四元數(shù)法進行姿態(tài)解算，并通過互補濾波進行數(shù)據(jù)融合，有效綜合了陀螺儀、加速度計、磁力計在姿態(tài)測量中的優(yōu)缺點，不僅提高了姿態(tài)測量的精確度，而且增強了系統(tǒng)的總體性能和容錯能力，為無人機完成各種飛行任務(wù)提供了根本保證。 參考文獻(xiàn) [1] 余國林，陳繼平，余濤，等.無人機航空遙感平臺機載作業(yè)控制系統(tǒng)設(shè)計[J].現(xiàn)代電子技術(shù)，2012，35（4）：132?135. [2] 萬曉鳳，康利平，余運俊，等.互補濾波算法在四旋翼飛行器姿態(tài)解算中的應(yīng)用[J].測控技術(shù)，2015，34（2）：8?10. [3] 萬良金.基于多傳感器信息融合的機器人姿態(tài)測量技術(shù)研究[D].北京：北京交通大學(xué)，2015. [4] 賴義漢，王凱.基于 MPU6050 的雙輪平衡車控制系統(tǒng)設(shè)計[J].河 南工程學(xué)院院報，2014，26（1）：53?57. [5] 劉輝邦，褚金奎，支煒，等.基于 STM32 的無人機姿態(tài)測量系統(tǒng)設(shè)計[J].傳感器與微系統(tǒng)，2013，32（8）：108?110. [6] 鄭健.基于 9 軸傳感器的姿態(tài)參考系統(tǒng)研究與實現(xiàn)[D].成都：電子科技大學(xué)，2013. [7] 呂印新，肖前貴，胡壽松.基于四元數(shù)互補濾波的無人機姿態(tài)解算[J].燕山大學(xué)學(xué)報，2014，38（2）：175?180.