本專案中使用的是
mpu6050
模組，如圖 1所示。該模組整合了一片
mpu6050
晶元和一片
stm8s003f3p6
微控制器，具有串列埠和
i2c介面。
mpu6050中的dmp（數字運動處理器）對姿態進行融合，stm8微控制器通過iic讀取dmp的處理結果，再新增上包頭序列輸出三軸加速度、三軸角速度、三軸角度和溫度資料，輸出頻率為
圖 1mpu6050晶元內部由四部分組成：加速度計、陀螺儀、dmp(digital motion processing)、溫度感測器。dmp部分的datasheet原文介紹：the dmpacquires data from accelerometers, gyroscopes, and additional 3rdparty sensorssuch as magnetometers, and processes the data. the resulting data can be readfrom the dmp』s registers, or can be buffered in a fifo. the dmp has access toone of the mpu』s external pins, which can be used for generating interrupts.
二、解決方案分析
mpu6050
是invensense
公司推出的一款低成本的
6軸感測器晶元，包括三軸加速度，三軸角速度。其體積小巧，用途非常廣。做平衡小車，四軸飛行器，飛行滑鼠等等，都是必不可少而且是最優的感測器解決方案。不論是做平衡還是四軸飛行器，關鍵的問題在於模組姿態的確定，通常需要用到積分運算與卡爾曼濾波演算法，需要較強的數學功底與程式設計能力，因此以下主要分析姿態確定問題。 雖然
6050
晶元能夠輸出三軸加速度和三軸角速度的資料，但實際應用的時候，直接使用的確不是這些量，而是需要根據這些資料解算出三軸的角度資料。比如平衡小車，需要算出
模組的俯仰角，然後控制演算法根據角度大小控制小車輪子的移動。四軸飛行器需要根據俯仰角度、滾轉角度，和飛行指令來調節四個電機的轉速。 從
6軸的原始資料得到三軸的角度計算是乙個比較複雜的運動學解算過程，有三點需要注意的問題：
1.三軸姿態的解算不能直接積分。
因為三軸是有耦合的，只有在三軸角度為小角度的時候可以這麼算，角度大了以後，比如
60度了，這麼算的誤差就很大。標準的做法是用四元數的方法做姿態解算，積分的方法可以用
4階龍格
-庫塔法，或者4階
gill
法。詳情請參考：航空太空飛行器運動的建模
——飛行動力學的理論基礎
肖業倫著
北京航空航天大學出版社。
2.積分運算的累積誤差。
角速度積分運算是有累積誤差的，累積誤差在短時間內表現不明顯，只要零點漂移處理得好了，
1分鐘以內的漂移都不大，但時間長了，就會有累積誤差。穩像儀中使用的
mpu6050
模組零點漂移比較小，
5分鐘大概
0.02
度。3.
角速率零點漂移。
所謂零點漂移就是模組靜止的時候，我們認為正常的輸出應該是
0，或者均值為
0的資料，但是實際上
6050
的輸出不是，經測量，小於
0.15度/
秒。在紙上畫出四個角度，分別為0度、26.6度、45度、90度，將紙固定在桌子上，每次將陀螺儀模組旋轉到對應的位置，經過多次實驗（實驗結果如圖2所示
），證實所使用的
mpu6050
模組的角度輸出確實存在很大的誤差。但是從
datasheet
上看，mpu6050
晶元和其他精度高且**昂貴的晶元如
adis16
系列差別不是大，那麼極有可能是姿態融合演算法部分的問題。
mpu6050
模組的姿態融合是在
stm8
微控制器中實現的，而且沒有原始碼，精度難以保證。
三、總結
綜上所述，有兩種解決方法：
一、使用
mpu6050
模組輸出的角速度和加速度的原始資料，用演算法對其進行姿態融合，得到角度。綜合利用陀螺儀和加速度計的特點，優勢互補獲得準確的姿態角度，方法就是用卡爾曼濾波做資料融合。大致的思路是將模組的姿態用四元素表示，作為系統的狀態量，模組的姿態運動學方程作為濾波的狀態轉移方程，加速度資訊作為濾波的觀察量資訊，然後利用卡爾曼濾波的計算方法迭代計算更新，詳細的過程可以參考慣性導航方面的書籍。不過卡爾曼濾波演算法比較複雜，需要用到矩陣運算等等，而且
mpu6050
模組是iic
介面輸出的。
二、採用更高精度的陀螺儀模組，直接得到角度。
記錄下自己做的MPU6050陀螺儀模組
話不多說，上圖 硬體pcb 3d圖 焊接好實物圖 左 和維特智慧型jy61模組 右 對比 自己做練手的東西，沒有做半孔工藝 也叫郵票孔，半孔工藝費150塊 嘉立創5塊錢還包郵，沒有用dmp，濾波自己做用了一階互補濾波，kalman濾波可能是自己引數調的不是很好，沒有一階濾波響應快，穩態時波動跟一階濾...
姿態角解算（MPU6050 加速度計加陀螺儀）
本文持續更新 i2c通訊 ahrs是自動航向基準系統 automatic heading reference system 的簡稱。目前，使用四元數來進行ahrs姿態解算的演算法被廣泛採用於四軸飛行器上。imu部分 imu是慣性測量裝置 inertial measurement unit 的簡稱，通...
陀螺儀和加速度計MPU6050的單位換算方法
對於四軸的初學者，可能無法理解四軸源 裡面陀螺儀和加速度資料的那些數學轉換方法。下面我們來具體描述下這些轉換方法。我們首先來看陀螺儀資料。在mpu6050的手冊裡面，提供了乙個陀螺儀資料表如下 在 裡面，列出了當暫存器fs sel選擇不同數值時的陀螺儀量程範圍。以四軸常用的fs sel 3為例，這時...