捷联惯导算法心得 (amoBBS 阿莫电子论坛)
我的图书馆
捷联惯导算法心得 (amoBBS 阿莫电子论坛)
&本帖最后由 seanwood 于
15:33 编辑
1、四个概念：“地理”坐标系、“机体”坐标系、他们之间换算公式、换算公式用的系数。
地理坐标系：东、北、天，以下简称地理。在这个坐标系里有重力永远是（0,0,1g），地磁永远是（0,1,x）（地磁的垂直不关心）两个三维向量。
机体坐标系：以下简称机体，上面有陀螺、加计、电子罗盘传感器，三个三维向量。
换算公式：以下简称公式，公式就是描述机体姿态的表达方法，一般都是用以地理为基准，从地理换算到机体的公式，有四元数、欧拉角、方向余弦矩阵。
换算公式的系数：以下简称系数，四元数的q0123、欧拉角的ROLL/PITCH/YAW、余弦矩阵的9个数。系数就是描述机体姿态的表达方法的具体数值。
姿态，其实就是公式+系数的组合，一般经常用人容易理解的公式“欧拉角”表示，系数就是横滚xx度俯仰xx度航向xx度。
2、五个数据源：重力、地磁、陀螺、加计、电子罗盘，前两个来自地理，后三个来自机体。
3、陀螺向量：基于机体，也在机体上积分，因为地理上无参考数据源，所以很独立，直接在公式的老系数上积分，得到新系数。
狭义上的捷联惯导算法，就是指这个陀螺积分公式，也分为欧拉角、方向余弦矩阵、四元数，他们的积分算法有增量法、数值积分法（X阶龙格-库塔）等等
4、加计向量、重力向量：加计基于机体，重力基于地理，重力向量（0,0,1g）用公式换算到机体，与机体的加计向量算出误差。理论上应该没有误差，这误差逆向思维一下，其实就是换算公式的系数误差。所以这误差可用于纠正公式的系数（横滚、俯仰），也就是姿态。
5、电子罗盘向量、地磁向量：同上，只不过要砍掉地理上的垂直向量，因为无用。只留下地理水平面上的向量。误差可以用来纠正公式的系数（航向）。
6、就这样，系数不停地被陀螺积分更新，也不停地被误差修正，它和公式所代表的姿态也在不断更新。
如果积分和修正用四元数算法（因为运算量较少、无奇点误差），最后用欧拉角输出控制PID（因为角度比较直观），那就需要有个四元数系数到欧拉角系数的转换。常用的三种公式，它们之间都有转换算法。
再搞个直白一点的例子：
机体好似一条船，地理就是那地图，姿态就是航向（船头在地图上的方位），重力和地磁是地图上的灯塔，陀螺/积分公式是舵手，加计和电子罗盘是瞭望手。
舵手负责估计和把稳航向，他相信自己，本来船向北开的，就一定会一直往北开，觉得转了90度弯，那就会往东开。
当然如果舵手很牛逼，也许能估计很准确，维持很长时间。不过只信任舵手，肯定会迷路，所以一般都有地图和瞭望手来观察误差。
瞭望手根据地图灯塔方位和船的当前航向，算出灯塔理论上应该在船的X方位。然而看到实际灯塔在船的Y方位，那肯定船的当前航向有偏差了，偏差就是ERR=X-Y。
舵手收到瞭望手给的ERR报告，觉得可靠，那就听个90%*ERR，觉得天气不好、地图误差大，那就听个10%*ERR，根据这个来纠正估算航向。。
------------------------------------------------------
来点干货，注意以下的欧拉角都是这样的顺序：先航向-再俯仰-然后横滚(就是zxy)
公式截图来自：袁信、郑锷的《捷联式惯性导航原理》，邓正隆的《惯性技术》。
--------------------------------------------------
根据加计计算初始欧拉角
这个无论欧拉角算法还是四元数算法还是方向余弦矩阵都需要，因为加计和电子罗盘给出欧拉角的描述方式比较方便。
imu.euler.x = atan2(imu.accel.y, imu.accel.z);
imu.euler.y = -asin(imu.accel.x / ACCEL_1G);
ACCEL_1G 为9.81米/秒^2，accel.xyz的都为这个单位，算出来的euler.xyz单位是弧度
航向imu.euler.z可以用电子罗盘计算
--------------------------------------------------
欧拉角微分方程
如果用欧拉角算法，那么这个公式就够了，不需要来回转换。
矩阵上到下三个角度（希腊字母）是roll pitch和yaw，公式最左边的上面带点的三个是本次更新后的角度，不带点的是上个更新周期算出来的角度。
Wx,y,z是roll pitch和yaw方向的三个陀螺在这个周期转动过的角度，单位为弧度，计算为间隔时间T*陀螺角速度,比如0.02秒*0.01弧度/秒=0.0002弧度.
--------------------------------------------------
以下是四元数
--------------------------------------------------
四元数初始化
q0-3为四元数四个值,用最上面公式根据加计计算出来的欧拉角来初始化
--------------------------------------------------
四元数微分方程
四元数更新算法，一阶龙库法，同样4个量（入、P1-3）也为四元数的四个值,即上面的q0-3。
Wx,y,z是三个陀螺的这个周期的角速度，比如欧拉角微分方程中的0.01弧度/秒，T为更新周期,比如上面的0.02秒。
再来一张，另外一本书上的，仔细看和上面是一样的delta角度，就是上面的角速度*周期，单位为弧度
--------------------------------------------------
四元数微分方程更新后的规范化
每个周期更新完四元数，需要对四元数做规范化处理。因为四元数本来就定义为四维单位向量。
求q0-3的平方和，再开根号算出的向量长度length。然后每个q0-3除这个length。
--------------------------------------------------
四元数转欧拉角公式
把四元数转成了方向余弦矩阵中的几个元素，再用这几个元素转成了欧拉角
先从四元数q0-3转成方向余弦矩阵：
再从方向余弦矩阵转成欧拉角
& & & & //更新方向余弦矩阵
& & & & t11=q.q0*q.q0+q.q1*q.q1-q.q2*q.q2-q.q3*q.q3;
& & & & t12=2.0*(q.q1*q.q2+q.q0*q.q3);
& & & & t13=2.0*(q.q1*q.q3-q.q0*q.q2);
& & & & t21=2.0*(q.q1*q.q2-q.q0*q.q3);
& & & & t22=q.q0*q.q0-q.q1*q.q1+q.q2*q.q2-q.q3*q.q3;
& & & & t23=2.0*(q.q2*q.q3+q.q0*q.q1);
& & & & t31=2.0*(q.q1*q.q3+q.q0*q.q2);
& & & & t32=2.0*(q.q2*q.q3-q.q0*q.q1);
& & & & t33=q.q0*q.q0-q.q1*q.q1-q.q2*q.q2+q.q3*q.q3;
& && &&&//求出欧拉角
& & & & imu.euler.roll = atan2(t23,t33);
& & & & imu.euler.pitch = -asin(t13);
& & & & imu.euler.yaw = atan2(t12,t11);
& & & & if (imu.euler.yaw & 0){
& & & & & & & & imu.euler.yaw += ToRad(360);
----------------------------------------------------
以下代码摘自网上，很巧妙，附上注释，有四元数微分，有加计耦合。
没电子罗盘，其实耦合原理也一样。//=====================================================================================================
// S.O.H. Madgwick
// 25th September 2010
//=====================================================================================================
// Description:
// Quaternion implementation of the 'DCM filter' [Mayhony et al].
// User must define 'halfT' as the (sample period / 2), and the filter gains 'Kp' and 'Ki'.
// Global variables 'q0', 'q1', 'q2', 'q3' are the quaternion elements representing the estimated
// orientation.&&See my report for an overview of the use of quaternions in this application.
// User must call 'IMUupdate()' every sample period and parse calibrated gyroscope ('gx', 'gy', 'gz')
// and accelerometer ('ax', 'ay', 'ay') data.&&Gyroscope units are radians/second, accelerometer
// units are irrelevant as the vector is normalised.
//=====================================================================================================
//----------------------------------------------------------------------------------------------------
// Header files
#include "IMU.h"
//----------------------------------------------------------------------------------------------------
// Definitions
#define Kp 2.0f& && && && && && && && &// proportional gain governs rate of convergence to accelerometer/magnetometer
#define Ki 0.005f& && && && && & // integral gain governs rate of convergence of gyroscope biases
#define halfT 0.5f& && && && && & // half the sample period
//---------------------------------------------------------------------------------------------------
// Variable definitions
float q0 = 1, q1 = 0, q2 = 0, q3 = 0;& && &&&// quaternion elements representing the estimated orientation
float exInt = 0, eyInt = 0, ezInt = 0;& && &&&// scaled integral error
//====================================================================================================
// Function
//====================================================================================================
void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az) {
& && &&&float vx, vy,
& && &&&float ex, ey,& && && &
& && &&&// normalise the measurements
& && &&&norm = sqrt(ax*ax + ay*ay + az*az);& && &
& && &&&ax = ax /
& && &&&ay = ay /
& && &&&az = az /& && &
把加计的三维向量转成单位向量。
& && &&&// estimated direction of gravity
& && &&&vx = 2*(q1*q3 - q0*q2);
& && &&&vy = 2*(q0*q1 + q2*q3);
& && &&&vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;
这是把四元数换算成《方向余弦矩阵》中的第三列的三个元素。
根据余弦矩阵和欧拉角的定义，地理坐标系的重力向量，转到机体坐标系，正好是这三个元素。
所以这里的vx\y\z，其实就是当前的欧拉角（即四元数）的机体坐标参照系上，换算出来的重力单位向量。
& && &&&// error is sum of cross product between reference direction of field and direction measured by sensor
& && &&&ex = (ay*vz - az*vy);
& && &&&ey = (az*vx - ax*vz);
& && &&&ez = (ax*vy - ay*vx);
axyz是机体坐标参照系上，加速度计测出来的重力向量，也就是实际测出来的重力向量。
axyz是测量得到的重力向量，vxyz是陀螺积分后的姿态来推算出的重力向量，它们都是机体坐标参照系上的重力向量。
那它们之间的误差向量，就是陀螺积分后的姿态和加计测出来的姿态之间的误差。
向量间的误差，可以用向量叉积（也叫向量外积、叉乘）来表示，exyz就是两个重力向量的叉积。
这个叉积向量仍旧是位于机体坐标系上的，而陀螺积分误差也是在机体坐标系，而且叉积的大小与陀螺积分误差成正比，正好拿来纠正陀螺。（你可以自己拿东西想象一下）由于陀螺是对机体直接积分，所以对陀螺的纠正量会直接体现在对机体坐标系的纠正。
& && &&&// integral error scaled integral gain
& && &&&exInt = exInt + ex*Ki;
& && &&&eyInt = eyInt + ey*Ki;
& && &&&ezInt = ezInt + ez*Ki;
& && &&&// adjusted gyroscope measurements
& && &&&gx = gx + Kp*ex + exI
& && &&&gy = gy + Kp*ey + eyI
& && &&&gz = gz + Kp*ez + ezI
用叉积误差来做PI修正陀螺零偏
& && &&&// integrate quaternion rate and normalise
& && &&&q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
& && &&&q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
& && &&&q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
& && &&&q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;&&
四元数微分方程
& && &&&// normalise quaternion
& && &&&norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
& && &&&q0 = q0 /
& && &&&q1 = q1 /
& && &&&q2 = q2 /
& && &&&q3 = q3 /
四元数规范化
//====================================================================================================
// END OF CODE
//====================================================================================================复制代码
TA的最新馆藏[转]&
喜欢该文的人也喜欢10417人阅读
谨以此纪念两天以来的对电子罗盘的认识与学习
&& mpu6050常用作提供飞控运行时的姿态测量和计算，在在姿态结算中有几个重要的概念，欧拉角、四元数等。
欧拉角：用来表征三维空间中运动物体绕着坐标轴旋转的情况。即物体的每时每秒的姿态可以由欧拉角表出。
四元数：超复数，q=(q0,q1,q2,q3)，q0位实数，q1,q2,q3为虚部的实数。简单的可以理解为四维空间，就是原有的三维空间加入一个旋转角。而四元数可以表征欧拉角，并且计算方便，故采用四元数来计算。在此还要提到加速度和磁力计补偿原理，可以参照http://blog.csdn.net/nemol1990/article/details/?utm_source=tuicool&utm_medium=referral 博客中提到的原理与基本概念。在此再啰嗦一下：补偿的目的是使两个坐标系世界坐标系和刚体坐标系能够完全重合，在此基础上，计算补偿值来修正旋转矩阵，即四元数矩阵。最终的结果是解算出四元数的姿态，就是四元数矩阵的各个元素的值。按照上述博客中的程序解算四元数的时候，用到了Kp和Ki两个参数，两个参数的作用是用来控制矫正刚体坐标系速度的。即调节加速度和磁力计补偿的速度（调节误差的生成速度，进而调节刚体坐标系和世界坐标系的重合度）
加速计补偿的理解：
由于重力加速度的原因，加速计只能补偿X-Y轴的偏差，即经过解算后的坐标中世界坐标XOY和刚体坐标XOY能够重合，但是无法补偿航向角。
在此基础上，利用磁力计进行补偿，因为当地磁场是一个椭圆，所以我们把世界坐标系的X-AIXS轴对准真实世界的北方，这样，磁场园只能在XOZ的平面圆里面，所以磁力计在世界坐标系中的分量是[I,0,k],即在Y轴没有分量，因为90度的夹角余弦为0.但此时i和k的值并不确定是多少。在刚体坐标系中磁力计的输出为[a,b,c,],旋转到和世界坐标系中，计算出i和k，然后，在经过旋转把其旋转到刚体坐标系中，然后计算误差修正旋转矩阵，此时得到精确的旋转矩阵。
&&相关文章推荐
* 以上用户言论只代表其个人观点，不代表CSDN网站的观点或立场
访问：10479次
排名：千里之外1692人阅读
嵌入式Linux（2）
程序执行过程
（1）读取数据，得到ax，ay，az，gx，gy，gz
（2）四元数解算，得到q1,q2,q3,q4
（3）计算欧拉角，得到pitch roll yaw
（4）串口输出
（5）误差处理:陀螺仪零值误差，上电后静止采样多次取平均值。
硬件接线图
PA9、PA10 usart1
这里采用IO模拟IIC
PA1：INT &MPU6050 dataready电平信号
程序下载地址：
&Tips:用MPU6050加速度计测振动，通常电机等运行过程中会产生振动，因此震动大小是电机控制效果好坏的一个重要的参考指标，通过测量电机各个方向的振动加速度，然后求方差，根据方差大小来衡量振动的大小。进一步，通过傅立叶变换对振动频率进行分析，可以测得振动的来源。
&&相关文章推荐
* 以上用户言论只代表其个人观点，不代表CSDN网站的观点或立场
访问：27394次
排名：千里之外
原创：25篇
转载：29篇
(1)(2)(4)(7)(12)(2)(1)(6)(6)(5)(2)(3)(3)查看: 81385|回复: 646
论坛首创MPU6050教程（编辑中，不小心点了发表）
本帖最后由 10xjzheng 于
14:43 编辑
如果你想玩四轴，想搞什么空中鼠标，平衡车等待，那么MPU6050真的是太强大了，能做很多东西。但是论坛上MPU6050整个教学过程基本上是没有的，资源共享，不应该只在自己需要资料的时候讲，我向往有一种开发，友好，互相学习的环境。我认识两种人，不分技术，区别在于对待别人问题的态度，一种乐于奉献，一种说多两句都不肯。我看到论坛上面一些帖子只是展示他的成果，下面一大堆评论要看下源程序，楼主都没有理睬，我就无语了，当然商业机密的除外。我知道，如果不是最近在阿莫论坛上面找到的这么多的网友的帮忙，我不可能这么快就搞好MPU6050。但是作为一个初学者，大家还是不要抱着偷懒的心态去学东西，如果你真的是不知道怎么去做，想过，那么你去打扰别人，这样才好，如果你根本不想努力，只想不劳而获，那么我很鄙视你，你也注定没有多大成就。我的QQ：，欢迎有限制的打扰，但是我不会手把手教你，我会给你指明努力的方向。
如果觉得我讲得好，对你有帮助，不要吝啬你的评论，这能让我倍受鼓舞。而且以后多发帖子就好了。
感谢一下各位对我帮助的网友。
玩MPU6050的步骤：
1.学习I2C，I2C就是MPU6050传送数据到单片机的一种协议，类似于USB，当然USB还是比较有难度的。
2.了解MPU6050相关寄存器，有中文版本的，一边学一边看例程就可以获取数据了。
3.然后大概就是姿态融合了吧，把获取的数据进行分析，现在进行到这步。
4.还要学下其他的很多东西？？
还是有一段路要走，慢慢来。I2C我首先从I2C开始讲起，其实I2C折腾得还挺快的，因为资料很多，那我就在这里告诉大家怎么去下手就好了。你可以去看一些I2C的书籍，例如野火的书，先学会从EEPROM里面写入和读取数据，因为EEPROM写入和读取数据也是根据I2C协议来的。I2C最好的教程我个人觉得是李想的视频，贴上地址：，百度一下，很多的。看完之后你绝对懂很多。I2C有分软件和硬件，软件就你通过对I2C的时钟线和数据线，可能你不知道时钟线和数据线，那还是先去学I2C的基础教程吧，然后再继续看。
软件模拟I2C就是根据下面的图然后再适当的时候给时钟线和数据线高低，具体可以看I2C的协议，这部分李想的视频讲得很好，我就不多讲了。
这是I2C协议的数据手册：
这部分学习的诀窍就是：先写下I2C通讯的一个环节就好了，其他的直接上网找例程就好了，想要用软件模拟出全部的时序当然也可以。还有一个方式可以用I2C读写数据，就是硬件I2C，硬件I2C就是单片机内部的电路，可以将I2C的时序用硬件电路搞出来，这样子你读写数据就方便很多了。STM32就有硬件I2C，52单片机部分型号应该也有，可以自己去找下，没有试过。STM32硬件I2C可以去看野火的教程。读取MPU6050原始数据我学MPU6050的过程受到了论坛上的生来孤独的帮助。他的两篇MPU6050的帖子也很有代表性，静下心来慢慢看吧。其中他的一个很重要的思路就是一步一步，确保每步都对之后就很容易读出正确的数据。MPU6050读取原始数据：DMP解析原始数据：我们对MPU6050进行读写传感器数据就是对MPU6050的寄存器用I2C进行读写。我们先来认识下MPU6050的硬件吧。这是MPU6050模块的图片，注意是模块，中间那个才是MPU6050，只有MPU6050是不够的，还要有一些外围电路才行，这就跟52芯片跟最小系统的区别一样。具体的电路我没有怎么了解。我只知道AD0决定MPU6050的地址，学过I2C的人都知道I2C通讯中从机是要有地址的，以区别多个从机。当AD0管脚接低电平的时候，从机地址是0x68。这里地址我当初还遇到了一个问题，可以看下我当初发的帖子看你会不会也错在这里：我们还要了解下MPU6050的寄存器，这个过程跟学习52单片机没有什么区别，就是配置寄存器，读取数据。这个是一个51单片机获取数据后在LCD上显示出来的程序： （提取码：5815）
记得要把液晶的那些管脚改下，这个是网上的例程，我也是拿过来，将管脚改下就可以用了。这是一个用野火STM32板子的程序，接线：IIC_SDA.PB11&&IIC_SCL.PB10： （提取码：1f0e）还要两个文档，一个英文，一个中文是关于MPU6050整体的介绍。
在写之前我们先要读懂上面两个工程部分内容。怎么阅读呢？举个例子。看下下面的函数，这个函数是初始化MPU6050。//**************************************
//初始化MPU6050
//**************************************
void InitMPU6050()
{
& && &&&Single_WriteI2C(PWR_MGMT_1, 0x00);& && &&&//解除休眠状态
& && &&&Single_WriteI2C(SMPLRT_DIV, 0x07);
& && &&&Single_WriteI2C(CONFIG, 0x06);
& && &&&Single_WriteI2C(GYRO_CONFIG, 0x18);
& && &&&Single_WriteI2C(ACCEL_CONFIG, 0x01);
}复制代码下面这句为什么可以解除休眠状态呢？至于为什么要接触休眠状态就要看MPU6050的datasheet，其实也可以看别人的程序怎么写就可以。Single_WriteI2C(PWR_MGMT_1, 0x00);& && &&&//解除休眠状态复制代码首先Single_WriteI2C();是给MPU6050寄存器写入数据的一个函数，需要有寄存器的地址，关于寄存器的描述，下面上次两个文档。一个是中文版，一个是英文版，中文版看得不是很明白，可能翻译得不好，英语可以的可以看下英文版本的。慢慢学着配合有道词典读下英文版的也好。
首先找到PWR_MGMT_1是某个寄存器的地址，其值如下#define& && &&&PWR_MGMT_1& && && && && & 0x6B& && &&&//电源管理，典型值：0x00(正常启用)复制代码我们在文档中找到地址为0x6B的寄存器，看下说明，可以知道为什么发送0x00给这个寄存器。慢慢地这样子去读，配合MPU6050的寄存器文档。获取数据也是差不多的。这个过程自己慢慢去看MPU6050的datasheet就好了，方法已经介绍。如果整个过程顺利的话，你可以用串口获得类似下面的数据。怎么检验这些数据是不是对的，我直接将手放在MPU6050上面，可以看到温度稍微会升高，而且我移动MPU6050的时候，数据会变化。初步证明数据是对的。
MPU6050原始数据分析我有看过几次在论坛上面有人获取原始数据后不知道这些代表什么，甚至不知道为什么要获取这些数据。为什么要获取这些数据呢？看完下一个小节你就会明白。下面我们来分析下这些数据代表什么。大家可以去看一个第七实验室的文档：我们配置MPU6050的函数：&font size=&2&&void MPU6050_Init(void)
{
& && &&&PMU6050_WriteReg(MPU6050_RA_PWR_MGMT_1, 0x00);& && && && & //解除休眠状态
& && &&&PMU6050_WriteReg(MPU6050_RA_SMPLRT_DIV , 0x07);& && && && &//陀螺仪采样率
& && &&&PMU6050_WriteReg(MPU6050_RA_CONFIG , 0x06);& && &&&
& && &&&PMU6050_WriteReg(MPU6050_RA_ACCEL_CONFIG , 0x00);& && && & //配置加速度传感器工作在16G模式
& && &&&PMU6050_WriteReg(MPU6050_RA_GYRO_CONFIG, 0x18);& &&&//陀螺仪自检及测量范围，典型值：0x18(不自检，2000deg/s)复制代码主要是看下面这两行，如果你的配置不同的话，要根据寄存器数据手册和第七实验室的文档结合分析数据才行。下面我根据我的配置分析给大家看下。PMU6050_WriteReg(MPU6050_RA_ACCEL_CONFIG , 0x00);& && && & //配置加速度传感器工作在16G模式
& && &&&PMU6050_WriteReg(MPU6050_RA_GYRO_CONFIG, 0x18);& &&&//陀螺仪自检及测量范围，典型值：0x18(不自检，2000deg/s)复制代码
查英文寄存器的手册可以知道我配置的陀螺仪的量程是±2000°/s，再查可知灵敏度是16.4LSB°/s。这里还要知道量程越大，测量精度越低。LSB的意思是最小有效位，为数字输出方式下使用；一般我们可以用mg/LSB来表示灵敏度，例如：mpu6050输出的位数为16位(2的16次方共65536个LSB)对应满量程，当量程为±8g时对应灵敏度就为16g/65536LSB=0.mv/g，取倒数为4096LSB/g，因为mpu6050只能16位输出，所以测量范围越大，对应精度就越低。根据第七实验室的描述一除可以知道我的陀螺仪绕3个轴的角速度最小是不够1°/s，最多是6°/s左右。当时我是保持静止测得的，不知道MPU6050这样子的结果大家是否满意，不知道大神们是否有办法让这几个值接近0呢？？
接下来是加速度的分析，同样查英文寄存器手册可以知道我设置的加速度例程是±2g，灵敏度是16384LSB/g，这样子下来加速度在y轴上面是1g，这个是重力加速度，说明我放的时候是y轴在竖直方向上的，其他的两周加速度是接近于0，说明我放得很垂直啊，你也可以换其他的轴在竖直方向上，看那个轴显示出来的值是否是16384左右，我试了，结果是。更加说明我们的结果很对啦。DMP移植趁现在有空，先上传个视频让大家一睹为快。你如果是初学者，只是获取了原始数据，那只能告诉你，其实原始数据没有处理没有多大的用处。我们还需要找将那些数据根据数学方面的东西转化为姿态有关的四元数和欧拉角，可以搜下融合算法。我顺便上传个文档，是关于这方面的，不过我还没有看。
接下来按照我的理解解释先怎么转化这个过程和转化后的四元数和欧拉角大概是什么，个人理解，不准确的话大神们帮忙指导下。这个过程有两种办法，一种你可以去学下数学，然后编程把我们上面获取的原始数据转化为四元数和欧拉角；另一种是直接用MPU6050内部的DMP。前者我还没有学，到时可能要学下。DMP是什么呢？我学的时候也是一头雾水，老是看见别人在说DMP，好像很厉害的样子。看下面的解释，是我找的。
DMP的概念：
DMP是什么意思？ DMP就是指 MPU6050内部集成的处理单元，可以直接运算出四元数和姿态，而不再需要另外进行数学运算。DMP的使用大大简化了四轴的代码设计。DMP是数字运动处理器的缩写，顾名思义mpu9150（mpu6050）并不单单是一款传感器，其内部还包含了可以独立完成姿态解算算法的处理单元。如在设计中使用DMP来实现传感器融合算法优势很明显。首先，invensense官方提供的姿态解算算法应该比像楼主这样的小白要可靠的多。其次，由DMP实现姿态解算算法将单片机从算法处理的压力中解放出来，单片机所要做的是等待DMP解算完成后产生的外部中断，在外部中断里去读取姿态解算的结果。这样单片机有大量的时间来处理诸如电机调速等其他任务，提高了系统的实时性。
直接上E文吧。标准一点。 DMP and DMP features: The DMP is aunique&&hardware feature of InvenSenseMPU&&devices&&which&&is&&capable&&of&&computing&&quaternion&&data&&from&&sensor&&readings,&&performing&&device&&calibration,and also includes application specific features such as pedometerstep-counting. The DMP image (firmware) is held in volatile memory on the MPUand needs to be updated to the DMP every time the chip powers up to leveragethis functionality.四元数和欧拉角是什么？我简单理解，简单解释。标准的自己去查下概念。四元数就是4个数，经过 DMP 或 数学+软件 你就可以得到四元数，四元数就是4个数，可以表征姿态，经过几个数学公式（下面编程会展示这几个公式）之后就可以的出姿态，姿态包括pitch，roll，yaw。这几个上传几张图片大家就知道是什么了。摘自：Z轴正方向为前进方向pitch()：俯仰，将物体绕X轴旋转（localRotationX）
yaw()：航向，将物体绕Y轴旋转（localRotationY）
roll()：横滚，将物体绕Z轴旋转（localRotationZ）下面一段又是别人的解释。“方向余弦，欧拉角，四元数东西太多，这里只简单说一下四元数，其他的自己找本惯性技术慢慢看吧。四元数是由一个实数单位和三个虚数单位i，j，k组成并具有实元的数，即q（p0，p1，p2，p3）=p0+p1i+p2j+p3k，其中三个虚实单位i，j，k可看做三维空间的单位向量在导航中，一般用的四元数均为特征四元数，形式如下，q=cosθ/2+nsinθ/2=cosθ/2+sin(θ/2)cosαi+sin(θ/2)cosβj+sin(θ/2)cosγk，n为转轴方向单位矢量，第二个等号后面的形式即为特征四元数，其中标量部分cosθ/2表示转角一半的余弦值，矢量部分表示了转角方向，即以原点为旋转中心，旋转的轴是(α, β, γ)，（ α^2 + β^2 + γ^2 = 1）， 转θ角的旋转，用四元数表示就是，Q = (cos(θ/2); α sin(θ/2), β sin(θ/2), γ sin(θ/2)) 。”MPU移植指的是要使用DMP功能官方已经给出移植的文档和移植的c文件、h文件等下面先上资料：。。。。。。。。。。。。。。。。。。匿名上位机显示姿态从上面的视频可以看到我用的上位机是匿名最新版本的，痛心的是匿名的前一版本的上位机不知道为什么在我的电脑上面老是出错，不能打开，搞了我半天多，后来下了新版本的就好了。错误如下图所示，如果有网友跟我一样的话，自己上百度找办法啊。
那又办法获取姿态之后怎么像我那样子在视频那样子玩呢？因为匿名上位机是在是有很多功能，所以要掌握他的上位机还是要看下他的教程。必须要先看下如果没有接触过，不然很多设置你都不知道怎么设置。包括匿名上位机，协议的下载，他的视频里面都有说到，不要问我，我也不想上传。
看完了之后，你就大概可以看得懂为什么在我的过程中有这么一个函数了。我就不讲解了。我自己也是自己根据他协议写的。
/*函数功能：根据匿名最新上位机协议写的显示姿态的程序
*具体原理看匿名的讲解视频
*/
void Data_Send_Status(float Pitch,float Roll,float Yaw,int16_t *gyro,int16_t *accel)
{
& && &&&unsigned char i=0;
& && &&&unsigned char _cnt=0,sum = 0;
& && &&&unsigned int _
& && &&&u8 data_to_send[50];
& && &&&data_to_send[_cnt++]=0xAA;
& && &&&data_to_send[_cnt++]=0xAA;
& && &&&data_to_send[_cnt++]=0x01;
& && &&&data_to_send[_cnt++]=0;
& && &&&
& && &&&_temp = (int)(Roll*100);
& && &&&data_to_send[_cnt++]=BYTE1(_temp);
& && &&&data_to_send[_cnt++]=BYTE0(_temp);
& && &&&_temp = 0-(int)(Pitch*100);
& && &&&data_to_send[_cnt++]=BYTE1(_temp);
& && &&&data_to_send[_cnt++]=BYTE0(_temp);
& && &&&_temp = (int)(Yaw*100);
& && &&&data_to_send[_cnt++]=BYTE1(_temp);
& && &&&data_to_send[_cnt++]=BYTE0(_temp);
& && &&&
& && &&&data_to_send[3] = _cnt-4;
& && &&&//和校验
& && &&&for(i=0;i&_i++)
& && && && && & sum+= data_to_send[i];
& && &&&data_to_send[_cnt++]=
& && &&&
& && &&&//串口发送数据
& && &&&for(i=0;i&_i++)
& && && && && & AnBT_Uart1_Send_Char(data_to_send[i]);
}复制代码姿态静止的波形：
扰动的姿态波形：
感谢各位的观看，我也在写教程的过程学到了很多，而且自己也把资料都整理好了。如果有什么解释得不好或者不对，大神们就感觉上啦，都是互相学习罢了。
本帖子中包含更多资源
才可以下载或查看，没有帐号？
圆点博士DMP库移植
本帖最后由 10xjzheng 于
21:39 编辑
24小时后就没有办法编辑了，在这里继续写。
圆点博士DMP库移植
刚才说到DMP移植，要上资料。
圆点博士的库：
我们移植的并不是官方的库，而是圆点博士的库，还包括他的I2C，两者是配套使用的。这个移植要比官方的简单。但是很想给官方和圆点博士点建议，多用点宏定义，不要老是retuen 0/1/2...绕晕了，还有那个发送用串口发送字符用起来太麻烦了，还要自己数多少个字符。添加圆点博士的文件到工程里面，该include的就include。
首先初始I2C、DMP、MPU6005如下：
& && &&&uart_init(115200);& && && &//串口初始化为115200
& && &&&//引用圆点博士的I2C程序，这里跟我们平常没有什么区别
& && &&&ANBT_I2C_Configuration();& && && && && & //IIC初始化
& && &&&AnBT_DMP_MPU6050_Init();& && && && && && && && &//6050DMP初始化复制代码
我稍微将他的程序改了下，一些英文的被我改为中文了，因为不习惯。^_^
接着定义一些变量，变量的意义如下：
& && &&&short gyro[3], accel[3],//陀螺仪存放数组，加速度存放数组，返回状态量
& && &&&
& && &&&long quat[4];//四元数存放数组
& && &&&float Yaw=0.00,Roll,P//欧拉角
& && &&&float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f;//计算姿态过程用到的变量复制代码
接着用库函数读出四元数
dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,&more);& && &&&复制代码计算欧拉角，里面包含了上面所说的将四元数转化为欧拉角的公式。
& && && && && &&&if(sensors&INV_WXYZ_QUAT)
& && && && && &&&{
& && && && && && && && & q0=quat[0] / q30;
& && && && && && && && & q1=quat[1] / q30;
& && && && && && && && & q2=quat[2] / q30;
& && && && && && && && & q3=quat[3] / q30;
& && && && && && && && & Pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch
& && && && && && && && & Roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
& && && && && && && && & Yaw =& && && &atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3;& && && && && &
& && &&&//& && && &printf(&pitch: %.2f& & roll:%.2f& && && && && & yaw:%.2f\r\n&,Pitch,Roll,Yaw);& && && && && & //普通串口输出
& && && && && && && && &Data_Send_Status(Pitch,Roll,Yaw,gyro,accel);
& && && && && && && && &delay_init(72);
& && && && && &&&}& && && &&&复制代码最后写一个符合你上位机通讯协议的发送程序，我的是匿名四轴最新版上位机，函数如下：
/*函数功能：根据匿名最新上位机协议写的显示姿态的程序
*具体原理看匿名的讲解视频
void Data_Send_Status(float Pitch,float Roll,float Yaw,int16_t *gyro,int16_t *accel)
{
& && &&&unsigned char i=0;
& && &&&unsigned char _cnt=0,sum = 0;
& && &&&unsigned int _
& && &&&u8 data_to_send[50];
& && &&&data_to_send[_cnt++]=0xAA;
& && &&&data_to_send[_cnt++]=0xAA;
& && &&&data_to_send[_cnt++]=0x01;
& && &&&data_to_send[_cnt++]=0;
& && &&&
& && &&&_temp = (int)(Roll*100);
& && &&&data_to_send[_cnt++]=BYTE1(_temp);
& && &&&data_to_send[_cnt++]=BYTE0(_temp);
& && &&&_temp = 0-(int)(Pitch*100);
& && &&&data_to_send[_cnt++]=BYTE1(_temp);
& && &&&data_to_send[_cnt++]=BYTE0(_temp);
& && &&&_temp = (int)(Yaw*100);
& && &&&data_to_send[_cnt++]=BYTE1(_temp);
& && &&&data_to_send[_cnt++]=BYTE0(_temp);
& && &&&
& && &&&data_to_send[3] = _cnt-4;
& && &&&//和校验
& && &&&for(i=0;i&_i++)
& && && && && & sum+= data_to_send[i];
& && &&&data_to_send[_cnt++]=
& && &&&
& && &&&//串口发送数据
& && &&&for(i=0;i&_i++)
& && && && && & AnBT_Uart1_Send_Char(data_to_send[i]);
}复制代码然后就可以了，就是这么简单！
最后把线接对，就可以了，还有把匿名的上位机视频真的好好看看，不然你有时都不知道你错在哪里！！！！！！！！！！
传上移植好的工程文件：
如果你想用官方的东西去移植也是可以的，我没有试过，但是我研究过别人的程序，我觉得跟圆点博士库的最大区别在于下面这个函数。实际上圆点博士已经讲初始化的步骤都包含在这个函数里面，这样子我们就不用一步一步去做，要了解究竟要做什么，去看这个函数就好。里面的内容一打开跟别人官方库移植的一对比你就会清楚的，官方的移植没有包含在这个函数里面而已。都差不多。AnBT_DMP_MPU6050_Init();& && && && && && && && &//6050DMP初始化复制代码上传官方文档：
阿莫上传文件太慢了，我直接上传到百度云盘给链接吧
DMP官方工程：
用库但是思路跟上面移植差不多的工程：（没有试验过，不能保证可以运行）
就先写到这里了，以后如果有什么补充再发多几个帖子。卡尔曼滤波等问题还没有学会，等学会了也发个帖子。
如果有大神发现什么错误，跟我说下！！互相学习
本帖子中包含更多资源
才可以下载或查看，没有帐号？
再附上一个英文讲解加速度计和陀螺仪的网站
本帖子中包含更多资源
才可以下载或查看，没有帐号？
如果数据读出来有问题，建议一步一步检查下程序。可能是从机地址设置不对，详情看这个帖子
或者是硬件问题，如果还有其他的问题欢迎大家补充！
顶到300楼，楼主会把卡尔曼滤波跟零点漂移，滑动滤波等解决这个运动传感器的知识也讲下，只是还差很久。接下来就看你们了。
能发一下51单片机读取数据的程序吗？链接不存在了！
在这里，重新上传
神啊，如果匿名软件打开不成功的看这个，搞了一个下午
这是一个用野火STM32板子的程序，接线：IIC_SDA.PB11&&IIC_SCL.PB10： （提 ...
哎，云盘怎么那么坑爹真是，上次一份野火板子MPU6050读取的，软件和硬件IIC都有，其他的板子也可以用吧，因为只是用到了片上的I2C外设
本帖子中包含更多资源
才可以下载或查看，没有帐号？
本帖最后由 10xjzheng 于
17:40 编辑
本人有点想法，集中点精力去攻克卡尔曼滤波还有其他的滤波等，最后能够讲得清楚，然后以一定的价格卖给学习者，虽然是付费的，但是毕竟自己会花更多的心思进去，不然本人也是作为一个学生也不大可能在这方面花费太多精力。如果不收费的话可能就花不了那么多的时间。包括原理讲解，matlab仿真，项目实战这些下来整理都要好久的。MPU6050这块我就花了一定的时间才写成的。不知道各位意下如何？
如果要下载所有的东西，包括教程，我已经整理放在
顶一下& &希望楼主大神能给讲讲滤波
如果是DSP方面，推荐看奥本海默的英文版。
#DSP学习推荐奥本海默的英文版本#
之前发过一个说说，吐槽奥本海默的DSP书籍很难看懂，后面发现不是奥本的问题，是翻译的问题。有一段大概是&..the first 300 points..&之类的，中文版翻译为“...第一个300点...”，我就无语了，英文的意思是序列最前的那300个点，而中文我瞬间就懵逼了。买了英文版后看了有300+页，虽然不能说是不用动脑就看得懂，但是稍微想下就看懂了，而且其中例子非常多，每个点讲完来个例子，画几个图。很多地方也是从DSP这门学科的发展进行撸顺，奥本好像在上个世纪60、70年代就开始写了SS和DSP的书籍了，想借鉴都没有得借鉴，唯有埋头认真写吧。我将学校图书馆近30多本外文的DSP书籍都基本翻了下，不乏有几章的描述的角度非常好，但是整体还是没有奥本好。有些同学担心自己英文水平跟不上，额，我背了考研单词感觉好了很多，另外感觉主要是搞清了几个专业术语的词汇，这是开始看的时候的需要克服的难点吧。其他日常单词经常用到的也就那几个吧，偶尔几个单词不会不大影响，反正我看的300+页没有查过词典。
赞一个，支持楼主 。
赞一个，支持楼主
赞一下&&希望继续完善
赞一个，支持楼主
支持楼主！
顶楼主 厉害咯&&嘿嘿
支持楼主。顶！
大赞一个，学习了
分享光荣，题目挺大，期待后面能有深挖！
分享光荣，题目挺大，期待后面能有深挖！
ding!!!!!!!!!!!!!!!
牛X!!!!!!!!!!!!!!!!!!!!!!!!!
mark& && && && && && && && && && && &
写的不错&&深夜收藏
期待后续更新......
支持楼主，好东西，
好贴，支持楼主
很详细赞一个
楼主不错。赞一个
赞一个，真不错。
MARK::MPU6560四元数
MARK::MPU6560四元数
讲得不错，资料整理到一块了，方便大家使用。
MARK::MPU6560四元数
好，收藏了
不错不错，好好学习一下！楼主精神值得敬佩！
精品，赞一个
赞！！！！！！
好有心，好好看看
赞一个！！！
DMP这段要仔细研究下，感谢楼主！
这个要顶，分享学习过程，比什么都好，支持！
非常详细，谢谢分享，学习了
期待LZ把姿态融合部分研究透彻啊
非常详细，谢谢分享，
赞一个！！！
强烈支持，楼组的分享精神令人佩服
lz~我用了你提供的野火STM32板子的程序，按照这个移植了下，结果我的输出结果加速度计和陀螺仪的值全是1，温度值是34，但是没有变化，这个是为什么？
lz~我用了你提供的野火STM32板子的程序，按照这个移植了下，结果我的输出结果加速度计和陀螺仪的值全是1， ...
首先，线有没有插对？
楼主讲究人！！！
mark 必须的
首先，线有没有插对？
这个肯定插对了啊~不然估计也不会有数据吧~~~
本帖最后由 10xjzheng 于
09:18 编辑
这个肯定插对了啊~不然估计也不会有数据吧~~~
一时也想不到为什么，可以按照上面提到的这个帖子里面的解决方法，一步一步去做。MPU6050读取原始数据：
解决后告诉我
一时也想不到为什么，可以按照上面提到的这个帖子里面的解决方法，一步一步去做。MPU6050读取原始数据：
是MPU6050_SLAVE_ADDRESS，原来是0x68，改成0xd0就好了~
有事就搞不明白，干嘛非得用模拟I2C，很多STM32的例程都是模拟的不用硬件I2C，io不够用？。其实我是来标记的，好贴
严重支持。
有事就搞不明白，干嘛非得用模拟I2C，很多STM32的例程都是模拟的不用硬件I2C，io不够用？。其实我是来标记 ...
模拟I2C还有一个好处，可以移植到任何的MCU，只要你的代码复用性好，而硬件不一样了，每个MCU的硬件I2C的寄存器都不一样。IO口都是一样的吧，其他好处可能还有吧？欢迎补充，不过硬件I2C也没有什么不好。
非常感谢楼主分享
是MPU6050_SLAVE_ADDRESS，原来是0x68，改成0xd0就好了~
看来地址这个问题坑了不少的人啊，不管是接线还是0x68、0x69或是0xD1、0xD2
支持，mark
看来地址这个问题坑了不少的人啊，不管是接线还是0x68、0x69或是0xD1、0xD2
嗯，谢谢楼主~~
楼主有心了，谢谢
多谢，楼主分享
赞一个，之前买了个BLE4.0的小板子，上面也有MPU6050，一直没折腾。有空按你的教程学学。
赞美楼主，来听课了
好贴& && && && && && && && && && && && && &
看楼主的贴就是一种享受，这种感觉真的很好！有一个疑问：
PMU6050_WriteReg(MPU6050_RA_ACCEL_CONFIG , 0x00);& && && & //配置加速度传感器工作在16G模式
这句代码的注释是不是搞错了？不是配置成2G模式吗？
要配置成16G的话，应该写入0x18啊
看楼主的贴就是一种享受，这种感觉真的很好！有一个疑问：
PMU6050_WriteReg(MPU6050_RA_ACCEL_CONFIG , 0x ...
你是对的，查了下，真的是这样子。发帖的时候没有注意。
大哥真会做人，问问题前先夸两句，让我们这种懒人不回答都不行。
本帖子中包含更多资源
才可以下载或查看，没有帐号？
楼上，你那图给我吓尿了，正在看鬼吹灯，今晚估计要做噩梦了
楼主很用心，谢谢楼主的分享！我之前做过自平衡小车，但是对陀螺仪还没有非常深入的了解，借助楼主的分享，正好回头补补。
楼主很用心，谢谢楼主的分享！我之前做过自平衡小车，但是对陀螺仪还没有非常深入的了解，借助楼主的分享， ...
好腻害，楼主有空教教我啊！
楼主总结对我很有价值，谢谢。
赞一个，支持楼主，觉得算法有用。
学习了。。。
太感谢楼主了&&总结的真好
好帖，支持楼主
好帖，学习了
好啊，学习了
还不错的说& && &&&
mpu6050教程。好好学习
还没有玩过,有这么好的教材,顶一下,有空一定得玩玩.楼主辛苦了,谢谢~~~
嗯，乐于分享，把东西藏藏掖掖的人我最讨厌了，但是也很可悲，我对人无保留，但是人却对我有戒心。
感谢分享！
楼主请问，现在是能处理航向角度了，那加速度和角速度的数据该如何处理呢？麻烦您了~
mark！MPU6050教程，这么好的教程必须顶
楼主请问，现在是能处理航向角度了，那加速度和角速度的数据该如何处理呢？麻烦您了~ ...
你把问题描述清楚点
好东西，感谢LZ无私的奉献
你把问题描述清楚点
& && && && && && && && &q0=quat[0] / q30;
& && && && && && && && & q1=quat[1] / q30;
& && && && && && && && & q2=quat[2] / q30;
& && && && && && && && & q3=quat[3] / q30;
上面不是把dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,&more);读取出来的四元数进行处理了吗，那对于加速度accel和角速度的数据该怎么处理呢？现在出来的都好大呀。应该除以一些数吗？
站一个& && && && && && &&&
q0=quat[0] / q30;
& && && && && && && && & q1=quat[1] / q30;
& && && && &&&...
我理解的是，你说的是原始数据不知道怎么处理？这个问题上面有说啊
楼主写得不错，支持一下
我理解的是，你说的是原始数据不知道怎么处理？这个问题上面有说啊
我看上面大都是PITCH和YAW的读取处理，我没找到加速度和角速度的处理方法~麻烦您了
我看上面大都是PITCH和YAW的读取处理，我没找到加速度和角速度的处理方法~麻烦您了 ...
有的，在原始数据处理那里
有的，在原始数据处理那里
好的，我现在研究研究，给您添麻烦了~
楼主 非常非常感谢你提供的资料
好贴，必须顶！
顶起来& && &&&
找了好久的好帖
赞一个，支持楼主
阿莫电子论坛, 原"中国电子开发网"}