基于STM32的姿態測量系統設計

李世光 王文文 申夢茜 高正中 肖佳宜

摘 要： 以無人機航姿測量系統小型化、量輕化為背景，設計了一種基于微慣性單元MEMS的姿態測量系統。系統以STM32F103C8T6為主控制器，通過I2C總線分別采集慣性測量單元MPU6050和數字羅盤HMC5883L的測量數據，利用數據融合算法解算無人機當前姿態。對于MEMS溫漂和噪聲干擾的問題，提出了一種基于四元數的互補濾波算法，對測量的姿態數據進行補償修正。實驗結果表明，該姿態測量系統簡單可靠、性能穩定、精確度高，成功完成了姿態的最優控制。

關鍵詞： MEMS； STM32； 姿態測量系統； 互補濾波

中圖分類號： TN98?34； TP249 文獻標識碼： 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 引 言

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

系統由主控制器（STM32F103C8）、六自由度慣性測量單元（MPU6050）和數字羅盤（HMC5883L）組成。MEMS主要作用是實時監測機器人的原始姿態信息并將模擬測量信號轉化為數字信號輸出，主控制器及其外圍電路的主要任務是實時采集傳感器輸出的數字測量信號以及融合原始姿態數據，得出俯仰角、翻滾角和航向角的姿態信息[3]。姿態控制系統硬件結構如圖1所示。

1 主要模塊

1.1 微處理器模塊

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

1.2 姿態傳感器模塊

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

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

2 基于四元數的姿態解算

2.1 姿態的坐標描述

3 多傳感器的數據融合

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

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

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

4 實驗結果與分析

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

5 結 論

本文設計的基于STM32的姿態角測量系統，利用四元數法進行姿態解算，并通過互補濾波進行數據融合，有效綜合了陀螺儀、加速度計、磁力計在姿態測量中的優缺點，不僅提高了姿態測量的精確度，而且增強了系統的總體性能和容錯能力，為無人機完成各種飛行任務提供了根本保證。

參考文獻

[1] 余國林，陳繼平，余濤，等.無人機航空遙感平臺機載作業控制系統設計[J].現代電子技術，2012，35（4）：132?135.

[2] 萬曉鳳，康利平，余運俊，等.互補濾波算法在四旋翼飛行器姿態解算中的應用[J].測控技術，2015，34（2）：8?10.

[3] 萬良金.基于多傳感器信息融合的機器人姿態測量技術研究[D].北京：北京交通大學，2015.

[4] 賴義漢，王凱.基于MPU6050的雙輪平衡車控制系統設計[J].河南工程學院院報，2014，26（1）：53?57.

[5] 劉輝邦，褚金奎，支煒，等.基于STM32的無人機姿態測量系統設計[J].傳感器與微系統，2013，32（8）：108?110.

[6] 鄭健.基于9軸傳感器的姿態參考系統研究與實現[D].成都：電子科技大學，2013.

[7] 呂印新，肖前貴，胡壽松.基于四元數互補濾波的無人機姿態解算[J].燕山大學學報，2014，38（2）：175?180.