MPU6050姿态解算1-DMP方式

MPU6050的姿态解算方法有多種，包括硬體方式的DMP解算，軟體方式的歐拉角與旋轉矩陣解算，軟體方式的軸角法與四元數解算。本篇先介紹最易操作的DMP方式。

MPU6050基本功能

• 3軸陀螺儀

  陀螺儀，測量的是繞xyz軸轉動的角速度，對角速度積分可以得到角度。

• 3軸加速度計

  加速度計，測量的是xyz方向受到的加速度。在靜止時，測量到的是重力加速度，是以當物體傾斜時，根據重力的分力可以粗略的計算角度。在運動時，除了重力加速度，還疊加了由于運動産生的加速度。

  

DMP簡介

DMP就是MPU6050内部的運動引擎，全稱Digital Motion Processor，直接輸出四元數，可以減輕外圍微處理器的工作負擔且避免了繁瑣的濾波和資料融合。Motion Driver是Invensense針對其運動傳感器的軟體包，并非全部開源，核心的算法部分是針對ARM處理器和MSP430處理器編譯成了靜态連結庫，适用于MPU6050、MPU6500、MPU9150、MPU9250等傳感器。

四元數轉歐拉角

四元數可以友善的表示3維空間的旋轉，但其概念不太好了解，可以先類比複數，複數表示的其實是2維平面中的旋轉。

四元數的基本表示形式為：

q0+q1*i+q2*j+q3*k

，即1個實部3個虛部，具體細節本篇先不做展開介紹。

四元數雖然友善表示旋轉，但其形式不太直覺，旋轉轉換成pitch、roll、yaw的表示形式，友善觀察姿态。

轉換公式為：

程式表示為：

pitch = asin(-2 * q1 * q3 + 2 * q0* q2)
roll  = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)
yaw   = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3)
           

已在STM32F407以及FreeRTOS上進行測試，視訊效果如下：

波形：

姿态：

視訊位址：https://www.bilibili.com/video/BV18A411v7Du/