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

基于STM32的MPU9255姿態(tài)解算算法的實現(xiàn)

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

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

STM32微控制器；MPU9255傳感器；四元數(shù)；姿態(tài)解算

0 引言

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

1 系統(tǒng)設(shè)計

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

圖1 姿態(tài)檢測系統(tǒng)設(shè)計框圖

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

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

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

并且四元數(shù)的旋轉(zhuǎn)矩陣為（b系到R系的坐標變換矩陣）：

四元數(shù)的解算姿態(tài)角的流程如圖2所示。

圖2 四元數(shù)解算過程

2.1 四元數(shù)初始化

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

2.2 修正陀螺儀數(shù)據(jù)誤差

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

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

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

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

2.3 數(shù)據(jù)融合

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

2.4 更新四元數(shù)

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

2.5 規(guī)范化處理

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

2.6 姿態(tài)角與歐拉角的轉(zhuǎn)換

最后，通過歐拉角對機體姿態(tài)進行描述，利用四元數(shù)與歐拉角的公式進行轉(zhuǎn)換，歐拉角包括俯仰角、滾轉(zhuǎn)角、偏航角：

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

由四元數(shù)計算出的歐拉角單位是弧度，需要乘以57.3將弧度轉(zhuǎn)為角度，完成歐拉角單位的轉(zhuǎn)換。至此，已將四元數(shù)轉(zhuǎn)換成精確的姿態(tài)信息。

3 結(jié)論與分析

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

通過數(shù)據(jù)的對比，發(fā)現(xiàn)四元數(shù)解算算法能準確的解算出MPU9255傳感器的姿態(tài)角，并能有效的抑制傳感器的漂移誤差，使解算出來的數(shù)據(jù)更接近真實值。

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

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

[2]鄧正?。畱T性技術(shù)[J]．哈爾濱工業(yè)大學出版社,2006．

[3]王?。畱T性技術(shù)研究現(xiàn)狀及發(fā)展趨勢[J]．自動化學報,2013,39(6)：723-729．

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

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