MPU6050姿态解算方式1-DMP

MPU6050的姿态解算方法有多种,包括硬件方式的DMP解算,软件方式的欧拉角与旋转矩阵解算,软件方式的轴角法与四元数解算。本篇先介绍最易操作的DMP方式。

1MPU6050基本功能

  • 3轴陀螺仪

陀螺仪,测量的是绕xyz轴转动的角速度,对角速度积分可以得到角度。

  • 3轴加速度计

加速度计,测量的是xyz方向受到的加速度。在静止时,测量到的是重力加速度,因此当物体倾斜时,根据重力的分力可以粗略的计算角度。在运动时,除了重力加速度,还叠加了由于运动产生的加速度。

  • IIC通信

通过IIC方式与单片机进行数据传输,输出的是数字信号。另外还有另一个IIC接口,可以连接地磁传感器,从而实现9轴数据的融合。

关于IIC的基础知识:从IIC实测波形入手,搞懂IIC通信

MPU6050姿态解算方式1-DMP

2DMP简介

DMP就是MPU6050内部的运动引擎,全称Digital Motion Processor,直接输出四元数,可以减轻外围微处理器的工作负担且避免了繁琐的滤波和数据融合。Motion Driver是Invensense针对其运动传感器的软件包,并非全部开源,核心的算法部分是针对ARM处理器和MSP430处理器编译成了静态链接库,适用于MPU6050、MPU6500、MPU9150、MPU9250等传感器。

3四元数转欧拉角

四元数可以方便的表示3维空间的旋转,但其概念不太好理解,可以先类比复数,复数表示的其实是2维平面中的旋转。

四元数的基本表示形式为:q0+q1i+q2j+q3*k,即1个实部3个虚部,具体细节本篇先不做展开介绍。

四元数虽然方便表示旋转,但其形式不太直观,需要转换成pitch、roll、yaw的表示形式,方便观察姿态。

转换公式为:

MPU6050姿态解算方式1-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)4

4测试效果

已在STM32F407以及FreeRTOS上进行测试,效果如下:

波形

MPU6050姿态解算方式1-DMP

姿态

MPU6050姿态解算方式1-DMP

点击下面的阅读原文可跳转到视频演示。

猜你喜欢

转载自blog.51cto.com/15060517/2641139

相关文章