基于STM32的MPU9255姿態解算算法的實現

東華理工大學機械與電子工程學院 王孚貴 朱兆優 李 星 賴云波

基于STM32的MPU9255姿態解算算法的實現

東華理工大學機械與電子工程學院 王孚貴 朱兆優 李 星 賴云波

本文采用STM32微控制器與MPU9255多軸姿態傳感器結合，設計完成了水下機器人姿態檢測和信息解算。通過MPU9255傳感器內置的陀螺儀、加速度計和磁力計，對系統的三軸角速度、三軸加速度、三軸磁感應強度進行信息采集，利用四元數姿態解算方法，對所測量的數據進行分析與計算，進而解算成水下機器人的姿態信息，并采用PID算法輸出PWM信號，實現對水下機器人系統的有效控制。文中給出信息采集、解算方法和程序算法思路，具有較好的實用價值。

STM32微控制器；MPU9255傳感器；四元數；姿態解算

0 引言

MPU9255是內部集成陀螺儀、加速度計、磁力計的9軸姿態傳感器。通過陀螺儀檢測物體處于運動狀態時的角速度，再利用算法解算成物體當前處于的角度，加速度計和磁力計分別測量加速度和磁感應強度，在數據算法中，四元數解算算法能夠利用加速度數據對陀螺儀測量的存在誤差的角度數據進行補償，有效的減少誤差，使得測量物體的姿態信息更加準確。通過微控制器實時解算姿態角。

1 系統設計

對于物體姿態信息檢測是以STM32微控制器構成的硬件系統，通過其內部集成的I2C接口與MPU9255進行通信，通過接口讀取MPU9255姿態傳感器各軸的數據，讀取到的數據是未進行算法處理的基本姿態信息，進而進行算法解算。解算完成后通過串口發送給上位機顯示，并輸出脈沖寬度調制信號用以執行機構調整當前物體的運動狀態。姿態信息檢測系統設計框圖如圖1所示。

圖1 姿態檢測系統設計框圖

MPU9255是集成三軸陀螺儀、三軸加速度計和三軸磁力計的九軸物體姿態檢測傳感器，檢測數據全數字輸出，其高度的集成性避免了單體傳感器組合時數據間產生的誤差問題，并且減少了單體傳感器帶來的體積和系統功耗問題。

2 基于四元數的姿態解算

四元數是一種常見的表示旋轉的方式。四元數中通過繞一個定義在參考坐標系的向量μ的單次轉動來實現一個坐標系到另一個坐標系的變換，其中向量μ既表示旋轉軸的長度，也表示旋轉角，四元數Q通常表示為：

并且四元數的旋轉矩陣為（b系到R系的坐標變換矩陣）：

四元數的解算姿態角的流程如圖2所示。

圖2 四元數解算過程

2.1 四元數初始化

通過STM2微控制器在任意個連續的采樣周期內測量MPU9255傳感器的輸出數據，求得其輸出數據的平均值，作為四元數解算姿態信息的基本數據。

2.2 修正陀螺儀數據誤差

三軸重力加速度計在運動過程中容易受機體的振動而產生較大的誤差，產生較大的噪聲，短時間內的可靠性不高。陀螺儀在運動過程中比較穩定，且噪聲波動小，但是對陀螺儀數據長時間積分會導致數據誤差，產生漂移。所以需要通過加速度計和電子磁力計測量的數據來修正由陀螺儀數據快速解算得到的存在誤差的姿態，最后得到準確的物體的姿態角度。

根據四元數矩陣以及用來定義物體姿態信息的歐拉角，轉換物體的地理坐標系的重力向量，設所測姿態物體的從地理坐標系旋轉到機體坐標系上的重力加速度為：

通過陀螺儀的積分來獲得旋轉角度，然后通過加速度和磁力計的比例和積分運算來修正陀螺儀的積分結果。

在一個三維的空間內，因為重力加速度的存在，加速度計提供了一個水平位置的絕對參考，但是它無法給提供一個方向的參考。而磁力計提供了一個正北方向的絕對參考。如果磁力傳感器各軸的數均是0，那么忽略該磁力計磁數據。否則在磁力計數據歸一化處理的時候，會導致除以0的錯誤。

2.3 數據融合

為了快速修正因積分而產生誤差的陀螺儀數據，需要通過PI系數進行修正，經過大量實際實驗的驗證，當Ki=2.0f，Kp=0.005f的時候，對陀螺儀積分誤差的補償效果最好。

2.4 更新四元數

更新四元數實質即為對四元數的微分方程進行求解。對于四元數的微分方程，使用修正后的陀螺儀數據對時間積分，得到用四元數表示的姿態，然后求解四元數。因為需要滿足解算結果的精確，所以采用一階的龍格庫塔法對四元數微分方程進行求解。

2.5 規范化處理

因為微處理器在實時更新并計算四元數的原因，所以存在浮點型數據計算不完整的情況而存在誤差，誤差使得在計算中的數據逐漸沒有了規范化特性，所以在每一次對四元數進行計算更新之后，需要重新對四元數的規范化進行處理。

2.6 姿態角與歐拉角的轉換

最后，通過歐拉角對機體姿態進行描述，利用四元數與歐拉角的公式進行轉換，歐拉角包括俯仰角、滾轉角、偏航角：

由于arctan和arcsin的結果是，這并不能覆蓋所朝向（對于俯仰角的取值范圍已經滿足），因此需要用atan2來代arctan。

由四元數計算出的歐拉角單位是弧度，需要乘以57.3將弧度轉為角度，完成歐拉角單位的轉換。至此，已將四元數轉換成精確的姿態信息。

3 結論與分析

實驗部分是通過MPU9255實測的數據與四元數算法解算出的數據進行對比，以達到對四元數解算算法的驗證。在MPU9255的實測數據的采集過程中，將MPU9255傳感器安裝在三軸轉臺上，轉臺由光柵碼盤計算轉過的角度，將采集到的轉臺的三軸角度與微控制器經過四元數算法采集到的MPU9255傳感器的數據進行對比。

通過數據的對比，發現四元數解算算法能準確的解算出MPU9255傳感器的姿態角，并能有效的抑制傳感器的漂移誤差，使解算出來的數據更接近真實值。

基于四元數對MPU9255姿態解算的方法中，充分的利用三軸陀螺儀、三軸加速度計以及三軸磁力計的數據對機體姿態角度進行修正，使測量的姿態角度更加的精確，經過程序代碼的驗證，證明了四元數解算算法的有效性和準確性，有著廣泛的應用前景。

[1]張榮輝,賈宏光,陳濤．基于四元數的捷聯式慣性導航系統的姿態解算[J]．光學精密工程,2008(10)：1963-1968．

[2]鄧正隆．慣性技術[J]．哈爾濱工業大學出版社,2006．

[3]王巍．慣性技術研究現狀及發展趨勢[J]．自動化學報,2013,39(6)：723-729．

[4]夏圣,許勇．基于MEMS組合模塊的姿態檢測系統設計[J]．單片機與嵌入式系統應用,2014(4)：52-55．

[5]馬艷紅,胡軍．姿態四元數相關問題[J]．空間控制技術與應用,2008(03)：55-60．