天天看點

MPU6050姿态解算1-DMP方式MPU6050基本功能DMP簡介四元數轉歐拉角

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

MPU6050基本功能

  • 3軸陀螺儀

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

  • 3軸加速度計

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

    MPU6050姿态解算1-DMP方式MPU6050基本功能DMP簡介四元數轉歐拉角

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的表示形式,友善觀察姿态。

轉換公式為:

MPU6050姿态解算1-DMP方式MPU6050基本功能DMP簡介四元數轉歐拉角

程式表示為:

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上進行測試,視訊效果如下:

波形:

MPU6050姿态解算1-DMP方式MPU6050基本功能DMP簡介四元數轉歐拉角

姿态:

MPU6050姿态解算1-DMP方式MPU6050基本功能DMP簡介四元數轉歐拉角

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

繼續閱讀