pitch&yaw&roll 相关知识(2)：
pitch&yaw&roll 相关知识(3)：
四元数与欧拉角知识：
&标签：&&&&&&&&&&&&&&&&&&&&&&&&
&&国之画&&&& &&&&chrome插件
版权所有 京ICP备号-2
迷上了代码！所有回答(2)
不会，但是帮你顶下
园豆：1370
你的 串口没有设置正确。
import toxi.processing.*;这个你 是哪里找到的？我 卡在这里。
&&&您需要以后才能回答，未注册用户请先。& 相关文章 &
MPU6050互补滤波法融合四元数姿态原理及代码
倾角，不过由于零漂等，在低频段信号不好。通过高通滤波可抑制低频噪声。将两者结合，就将陀螺和加表的优点融合起来，得到在高频和低频都较好的信号，互补滤波需要选择切换的频率点，即高通和低通的频率。
我个人觉得互补滤波在博士代码里的基本思想是以角速度积分得到角度为主进行姿态解算，但是由于周围环境或自身器件的原因，角速度计积分出来的角度可能会产生误差的累积，长时间可能引起严重的角度偏移，而加速度计虽然精度不高但没有积累误差，可以用来对角度进行纠正。通过上位机采集互补滤波法融合的四元数姿态数据
四轴、平衡小车等的互补滤波应用
就可以使用四元数或欧拉法求解姿态角，然后通过加速度计的三轴加速度值就可以对姿态角进行融合，这时的姿态角就基本可用了，然后就可以做你想做的事了。 互补滤波器就是根据传感器特性不同，通过不同的滤波器（高通或低通，互补的），然后再相加得到整个频带的信号。例如，加速度计测倾角，其动态响应较慢，在高频时信号不可用，所以可通过低通抑制高频；陀螺响应快，积分后可测倾角，不过由于零漂等，在低频段信号不好。通过高通滤波可抑制低频噪声。将两者结合，就将陀螺和加表的优点融合起来，得到在高频和低频都较好的信号。互补滤波需要选择切换的频率点，即高通和低通的频率。
的姿态。 以下给出笔者姿态融合的代码，该代码网上都有，笔者在这里做了些许注释，方便理解。 /*********************************************************************************
函数名：void IMUupdata(float gx, float gy, float gz, float ax, float ay, float az)
说明：IMU单元数据融合，更新姿态四元数
入口：float gx 陀螺仪x分量
四元数姿态解算中的地磁计融合解读
坐标系，参考坐标系），载体坐标系我们称之为b系，就是我们飞行器的坐标系。对于四元数法的姿态解算，我们求的就是四元数的值；方向余弦矩阵（用于表示n系和b系的相对关系）中的元素本来应该是三角函数，这里由于我们四元数法，所以矩阵中的元素就成了四元数。所以我们的任务就是求解由四元数构成的方向余弦矩阵nCb（nCb表示从b系到n的转换矩阵，同理，bCn表示从n系到b的转换矩阵，他们的关系是转置）。
显然，上述矩阵是有误差存在的。对于一个确定的向量n，用不同的坐标系表示时，他们所表示的大小和方向一
Ardunio mpu6050 dmp 数据通过串口实现与matlab(mac)实时绘图 学习过程 【】
DAY 1 设备： 0. 单片机 arduino mega 2560 1. 陀螺仪 mpu6050 2. 系统环境 macbook retina, os x 10.9.4
软件： 0. matlab r2012a (破解版)
//穷，求放过 1. Ardunio 参考资料： 0. 工程矩阵理论（第2版） 1. 卡尔曼滤波及其实时应用 2. MATLAB外部接口编程.pdf 3. 百度百科&&wiki：四元数 4. 百度百科&&wiki：欧拉角 //不得不说，对于一个高中生理解起来
请问得到四元数后怎么计算YAW，ROLL，PITCH？
作者 js 发表于
这段话很奇怪： 既然精度都是float，为什么用sqrt？用sqrtf啊。 除法一般比乘法慢很多，应该把除法移 ... 四元数和卡尔曼滤波是什么关系？一直没有搞懂～
我用的卡尔曼是这样的：以Y轴为例，先用加速度计计算出倾角（atan2（x，z）），然后用卡尔曼滤波融合陀螺仪得到最终姿态，但是在Y轴指向地面时，X和Z的分量都是0，加速度计计算出倾角在90度附近时出问题，小于30度精度还比较不错，是否利用4元数可以计算任意角度
擅长处理控制算法，PL擅长处理图像算法，本方案力图在PL中完成所有图像算法和大部分控制算法，Linux只需管理添加修改飞行任务。
飞控程序主要分为两部分，一是姿态解算，二是姿态控制。本方案飞行器不依赖遥控操作，需要完成自动飞行功能，因此姿态解算即成为飞控部分的核心算法。本方案采用四元数法进行姿态解算。姿态解算流程如下：读取陀螺仪、加速度计、磁力计等传感器数据，多传感器数据融合后得到角增量，计算出等效旋转因子，与上一时刻四元数迭代、单位正交化处理后得到当前姿态四元数，最后计算得坐标
MPU6050 卡尔曼滤波
(Gyro_y,8,1); //-------卡尔曼滤波融合----------------------- //Kalman_Filter(Angle_ax,Gyro_y);
//卡尔曼滤波计算倾角
Display10BitData(Kalman_Filter(Angle_ax,Gyro_y),2,1); /*//-------互补滤波----------------------- //补偿原理是取当前倾角和加速度获得倾角差值进行放大，然后与
//陀螺仪角速度叠加后再积分
的信息。 　　如何融合这些互补或冗余的传感器信息并得到更全面反映环境特征的信息方法尤为重要。在研究中最为关键的部分是信息融合算法的研究，人们已经提出了多种应用于不同系统的多传感器信息融合算法，这些算法可以分为2类：随机类方法和人工智能方法。 　　(1)随机类方法 　　这类方法研究对象是随机的，在多传感器信息融合中常采用随机类方法包括很多，如：加权平均法、统计决策理论、聚类分析法、小波变换法、Bayes推理方法、Dempster-Sharer的证据理论、Kalman滤波融合算法等。 　　(2
这篇论文的难点在于航天器的数学模型，控制算法倒很简单。 数学模型用到了姿态四元数、姿态误差四元数，仿真初始条件用的是欧拉角，这三者之间需要相互转换，用到了不少知识。 同时，为了节省篇幅，数学模型都是用向量、矩阵形式给出的，仿真的时候需要展开推导。 亮点还是在于将挠性振动作为了干扰项，设计的控制律能够很好地抑制了振动。 论文仿真过程见土豆：链接地址 论文中的一些矩阵：
四元数(Quaternions)
好吧，我必须承认到目前为止我还没有完全理解四元数，我一度把四元数理解为轴、角表示的4维向量，也就在下午我才从和同事的争辩中理解了四元数不完全是角、轴这么简单，为此写点心得给那些同我一样搞了2年3D游戏的还不清楚四元数的朋友。 为什么使用四元数 为了回答这个问题，先来看看一般关于旋转（面向）的描述方法－欧拉描述法。它使用最简单的x,y,z值来分别表示在x，y，z轴上的旋转角度，其取值为0-360(或者0-2pi），一般使用roll，pitch，yaw来表示这些分量的旋转值。需要注意的是，这里
四元数(Quaternions)
链接地址 好吧，我必须承认到目前为止我还没有完全理解四元数，我一度把四元数理解为轴、角表示的4维向量，也就在下午我才从和同事的争辩中理解了四元数不完全是角、轴这么简单，为此写点心得给那些同我一样搞了2年3D游戏的还不清楚四元数的朋友。
为什么使用四元数 为了回答这个问题，先来看看一般关于旋转（面向）的描述方法－欧拉描述法。它使用最简单的x,y,z值来分别表示在x，y，z轴上的旋转角度，其取值为0-360(或者0-2pi），一般使用roll，pitch，yaw来表示这些分量的旋转值。需要注意
【Unity技巧】四元数（Quaternion）和旋转
或者旋转身体，两种方法其实都可以，但一旦这些旋转不是以世界坐标轴为旋转轴，比如人物扭动脖子向下看等，那么四元数是一个更合适的选择。Unity还提供了transform.forward, transform.right and transform.up 这些非常有用的轴，这些轴可以和Quaternion.AngleAxis组合起来，来创建非常有用的旋转组合。例如，下面的代码让物体执行低头的动作： transform.rotation = Quaternion.AngleAxis(degrees, transform.right) * transform. 关于Quaternion的其他函数，后面再补充吧，原理类似~
或者都是整数或者都是分子为奇数分母为2的有理数。集合A是一个环，并且是一个格。该环中存在24个四元数，而它们是 施莱夫利符号为{3,4,3}的正二十四胞体的顶点。 4链接地址链接地址矩阵表示 有两种方法能以矩阵表示四元数，并以矩阵之加法、乘法应用于四元数之加法、乘法。 第一种是以二阶复数矩阵表示。若 h = a + bi + cj + dk 则它的复数形式为： &math&\begin a-di & -b+ci \\ b+ci & \;\; a+di \end&/math& 这种表示法有如下优点
。 aeroToolbox特征 ●
操作过程独立于STK ●
历元与日期函数—管理参考时间，历元日期转换 ●
坐标变换—STK坐标系间的变换 ●
旋转和四元数—四元数、旋转矩阵、欧拉角间的转换 ●
中心体（Central Body）操作和例程—确定中心体参数，包括惯性坐标、速度、表面法向量、弧长、重力参数，以及确定矢量是否在给定的中心体内相交 ●
航点、星历和姿态文件—读写STK数据文件 ●
位置和速度函数—动态反映运载工具的状态，计算视几何
迭代。 e. 重复a-e，直到收敛或达到既定的迭代次数。 --其中，计算旋转矩阵R时，需要矩阵方面的运算。 由新的点集，每个点到重心的距离关系，计算正定矩阵N，并计算N的最大特征值及其最大特征向量；其特征向量等价于旋转的四元数（且是残差和最小的旋转四元数），将四元数就可以转换为旋转矩阵。 数学概念： 四元数：/v113161.htm?ch=ch.bk.innerlink 正定矩阵：特征值都大于0的矩阵。 这些是矩阵理论，最优化原理方面的一些概念。 3.
新的点集，每个点到重心的距离关系，计算正定矩阵N，并计算N的最大特征值及其最大特征向量；其特征向量等价于旋转的四元数（且是残差和最小的旋转四元数），将四元数就可以转换为旋转矩阵。 数学概念： 四元数：/v113161.htm?ch=ch.bk.innerlink 正定矩阵：特征值都大于0的矩阵。 这些是矩阵理论，最优化原理方面的一些概念。 3. fast ICP解析： Fast ICP是对ICP的改进与扩展。论文Efficient Variants
新的点集，每个点到重心的距离关系，计算正定矩阵N，并计算N的最大特征值及其最大特征向量；其特征向量等价于旋转的四元数（且是残差和最小的旋转四元数），将四元数就可以转换为旋转矩阵。 数学概念： 四元数：/v113161.htm?ch=ch.bk.innerlink 正定矩阵：特征值都大于0的矩阵。 这些是矩阵理论，最优化原理方面的一些概念。 3. fast ICP解析： Fast ICP是对ICP的改进与扩展。论文Efficient Variants
11种经典软件滤波的原理和实现
11种经典软件滤波的原理和实现
1、限幅滤波法（又称程序判断滤波法）
根据经验判断，确定两次采样允许的最大偏差值（设为A）
每次检测到新值时判断：
如果本次值与上次值之差&=A,则本次值有效
如果本次值与上次值之差&A,则本次值无效,放弃本次值,用上次值代替本次值
能有效克服因偶然因素引起的脉冲干扰
10种简单的滤波算法
10种滤波算法 及 例子c代码(来自于互联网) 经常有朋友们提起传感器采样的时候数据会抖动,会跳动, 这时候需要一些滤波算法; 1、限幅滤波法（又称程序判断滤波法） 2、中位值滤波法 3、算术平均滤波法 4、递推平均滤波法（又称滑动平均滤波法） 5、中位值平均滤波法(又称防脉冲干扰平均滤波法) 6、限幅平均滤波法 7、一阶滞后滤波法 8、加权递推平均滤波法 9、消抖滤波法 10、限幅消抖滤波法 11、IIR滤波？？？ 假定从8位AD中读取数据（如果是更高位的AD可定义数据类型为int),子程序
& 2012 - 2016 &
&All Rights Reserved. &
/*爱悠闲图+*/
var cpro_id = "u1888441";mpu-6050 加速度、陀螺仪传感器的调试
单片机&嵌入式
单片机应用
嵌入式操作系统
学习工具&教程
学习和开发单片机的必备工具
(有问必答)
(带你轻松入门)
电子元件&电路模块
当前位置： >>
>> 浏览文章
mpu-6050 加速度、陀螺仪传感器的调试
&买了mpu-6050模块一段时间了，一直没有时间试，乘五一放假，总算是可以玩一下了。先找了一点简介：
&&&&&&&&&&
&MPU-6000为全球首例整合性6轴运动处理组件，相较于多组件方案，免除了组合陀螺仪与加速器时之轴间差的问题，减少了大量的包装空间。MPU-6000整合了3轴陀螺仪、3轴加速器，并含可藉由第二个I2C端口连接其他厂牌之加速器、磁力传感器、或其他传感器的数位运动处理(DMP: Digital Motion Processor)硬件加速引擎，由主要I2C端口以单一数据流的形式，向应用端输出完整的9轴融合演算技术 。
MPU-6000的角速度全格感测范围为&250、&500、&1000与&2000&/sec (dps)，可准确追緃快速与慢速动作，并且，用户可程式控制的加速器全格感测范围为&2g、&4g&8g与&16g。产品传输可透过最高至400kHz的I2C或最高达20MHz的SPI。&
MPU-6000可在不同电压下工作，VDD供电电压介为2.5V&5%、3.0V&5%或3.3V&5%，逻辑接口VVDIO供电为1.8V& 5%。MPU-6000的包装尺寸4x4x0.9mm(QFN)，在业界是革命性的尺寸。其他的特征包含内建的温度感测器、包含在运作环境中仅有&1%变动的振荡器。
&&&&&&&&&&&&&&
总的来说，6050算是现在比较便宜好用的加速度陀螺仪芯片了，但是直接读取时噪声大，需要进行滤波。因此开始还找了一下卡尔曼滤波方面的资料，并且在网上发现了一种做好的6050+卡尔曼滤波集成的模块，不贵，35-58元，从网站的图片看效果很好，而且输出改成了串口，连接比较简单。刚看到有人对此提出质疑：，觉得其实就是用了一块8位的stm8调用了6050自带的dmp功能。模块的采样率100hz，后面说的dmp也可以做到。呵呵，谁对谁错就不好说了，不过用这个模块省了自己琢磨dmp了，懒人福音吧。用卡尔曼动态滤波算法，或者简化版的滤波，这种方法用好了效果会非常好，有些参数可以根据实际应用进行调整，改进效果。另一种是利用MPU6050内部的姿态融合器DMP，直接输出姿态角，优点是CPU参与少，但融合参数不可调整，调试困难（主要是可参考的资料很少）。
连线：首先把6050和arduino连起来，6050模块有8个插口，不用都用到，只需要把SCL和SDL连在A5和A4端就可以用了，有时需要中断时，把INt和D2连接，如下图。
Jeff Jrowberg 的DMP库（库名 MPU6050 I2C device class, 6-axis MotionApps20.h；当然这个库不是AVR的）,这个库，是作者通过&反向工程&得到的，不是官方的！
如果你仔细读Jeff Jrowberg的代码，其实就是i2c操作，先设置6050寄存器，再在6050内存中写入firmware，然后是config，最后是update，然后就可以直接读出FIFO里的quarternion数据，当然还有gyro和accel数据！注意，陀螺仪数据是可以自动校正，只需要传感器8秒保持不动即可，四元数处理后得到的角度数据是非常稳定的！自动校正功能是在DMP模式下才有的.
MPU-6050内部的DMP运算，roll和pitch两个参数，用了片内的加计（重力）对陀螺仪数据进行了校正，而yaw轴就没有相应的校正参考，陀螺仪的输出会漂移是很正常的，只能采用磁感传感器校正（使用invensense的9轴DMP或自己校正）!很不莘，当你用5883时，请一定记得先将5883做8字校准，不然你的YAW轴不会得到能用的数据的！
9轴mpu-9150要贵一些，但是集成了地磁传感器。也可以用别的磁场传感器，后面可以试试。先说说直接读取的数据吧。导入I2Cdev.h和mpu6050.h两个库函数，运行示例程序，可以得到三个轴的角速度和加速度。数据的波动比较大，见图。ax-az是加速度，单位10E-3 g，gx-gz是角速度。未经滤波处理的数据是不能直接用的。而且如何从这6个参数得到模型的空间姿态，也是很麻烦的，后面摘抄了一段相关帖子，说的比较明白。
& & Serial.print(ax/16.384); Serial.print(&,&);
& & Serial.print(ay/16.384); Serial.print(&,&);
& & Serial.print(az/16.384); Serial.print(&,&);
& & Serial.print(gx/131); Serial.print(&,&);
& & Serial.print(gy/131); Serial.print(&,&);
& & Serial.println(gz/131);
未经处理的数据：
mpu-6050 加速度、陀螺仪传感器的调试（1）
dmp后得到的欧拉角：
mpu-6050 加速度、陀螺仪传感器的调试（1）
MPU6050模块是InvenSense公司推出的一款低成本的6轴传感器模块，包括三轴加速度，三轴角速度。其体积小巧，用途非常广。做平衡小车，四轴飞行器，飞行鼠标等等，都是必不可少而且是最优的传感器解决方案。本人根据自己的一些实际工作经验和使用体会来谈谈MPU6050的相关问题吧，抛砖引玉，如有不当之处，欢迎大家批评指正。
不论是做平衡还是四轴飞行器，关键的问题在于两方面，一是模块姿态的确定，通常需要用到积分运算与卡尔曼滤波算法，需要较强的数学功底与编程能力，二是稳定控制，方法比较单一，就是经典的PID控制算法，难点在于需要根据实际情况调整PID的参数，需要做实验确定，不难，只是费时间。因此以下主要分析姿态确定问题。
虽然6050模块能够输出三轴加速度和三轴角速度的数据，但实际应用的时候，直接使用的确不是这些量，而是需要根据这些数据解算出三轴的角度数据。比如平衡小车，需要算出&
模块的俯仰角，然后控制算法根据角度大小控制小车轮子的移动。四轴飞行器需要根据俯仰角度、滚转角度，和飞行指令来调节四个电机的转速。
从6轴的原始数据得到三轴的角度计算是一个比较复杂的运动学解算过程，有的童鞋可能会说，不就是三轴角速度积分不就行了吗？这就是没有实践，想当然的说法。有三点需要注意的问题：
1.三轴姿态的解算不能直接积分。因为三轴是有耦合的，只有在三轴角度为小角度的时候可以这么算，角度大了以后，比如60度了，这么算的误差就很大。标准的做法是用四元数的方法做姿态解算，积分的方法可以用4阶龙格-库塔法，或者4阶Gill法。详情请参考：航空航天器运动的建模&&飞行动力学的理论基础 肖业伦著 北京航空航天大学出版社。
2.积分运算的累积误差。角速度积分运算是有累积误差的，累积误差在短时间内表现不明显，只要零点漂移处理得好了，1分钟以内的漂移都不大，但时间长了，就会有累积误差，5分钟就漂到不知道哪里去了。
3.角速率零点漂移。所谓零点漂移就是模块静止的时候，我们认为正常的输出应该是0，或者均值为0的数据，但是实际上6050的输出不是，可能在2&/s或者其他，而且每次都不一样，如果不校准，别说1分钟了，10秒钟误差就有20度。
根据上面的分析，似乎要获得角度非常困难呀。又有些聪明的朋友会想，用角速率积分这么麻烦，我不怎么算好了，条条大路通罗马，为啥非得用这个方法。6050不是能输出加速度吗？我用重力在3轴的分量的反正切值，作为滚转角和俯仰角不久行了。
用加速度计算的确也是一种方法，但使用加速度也有三方面的问题：
1.无法在动态情况下使用，使用重力的来解算姿态的前提条件是模块本身没有加速度，因此模块输出的三轴加速度值，正好是重力在模块本体坐标系下的分量，从而能够求出俯仰和滚转的姿态角度。一旦模块运动起来，这种方法就傻了，因为模块无法分辨出哪些是重力的分量，那些是模块本身的加速度引起的。目前市面上很多倾角仪就是这种思路，但问题就是没法在动态情况下使用，最简单的测试方法就是把模块水平放置桌面上，缓慢运动，发现X，Y轴的角度基本不变化，都在0度左右，一旦快速运动起来，X，Y轴就显示有很大的角度了。而实际上模块一直水平，没有变化。
2.精度差。6050模块的加速度本身的精度不高，就算是在静态情况下，角度测量的精度也只能到1&左右。
3.三轴耦合问题。利用加速度求解姿态的时候，也会有三轴耦合的问题，因为姿态表示与坐标旋转顺序有关，这样只有一种一个轴能用反正切值计算，另一个轴不能用反正切值计算。
那么怎么才能得到高精度不漂移的三轴角度呢？陀螺仪精度高，但时间长了会有漂移，加速度动态精度差，但没有长期漂移。能否综合利用陀螺仪和加速度计的特点，优势互补获得准确的姿态角度呢？答案是肯定的，方法就是用卡尔曼滤波做数据融合。大致的思路是将模块的姿态用四元素表示，作为系统的状态量，模块的姿态运动学方程作为滤波的状态转移方程，加速度信息作为滤波的观察量信息，然后利用卡尔曼滤波的计算方法迭代计算更新，详细的过程可以参考惯性导航方面的书籍。不过卡尔曼滤波算法比较复杂，需要用到矩阵运算等等，数学功底和编程基础要求都较高，不是初学者能够快速掌握的。而且MPU6050模块是IIC接口输出的，也给初学者带来了不少障碍与困难。
&&&&&&&&&&&&&&&&&&&&
默认陀螺仪的量程是250度/s，可以设置到最高2000度/s。调用
& & MPU6050
& & accelgyro.setFullScaleGyroRange(MPU6050_GYRO_FS_2000);
& & accelgyro.setFullScaleAccelRange(MPU6050_ACCEL_FS_8);
#define MPU6050_GYRO_FS_250 & & & & 0x00
#define MPU6050_GYRO_FS_500 & & & & 0x01
#define MPU6050_GYRO_FS_1000 & & & &0x02
#define MPU6050_GYRO_FS_2000 & & & &0x03
#define MPU6050_ACCEL_FS_2 & & & & &0x00
#define MPU6050_ACCEL_FS_4 & & & & &0x01
#define MPU6050_ACCEL_FS_8 & & & & &0x02
#define MPU6050_ACCEL_FS_16 & & & & 0x03
这些定义可以在MPU6050.h和cpp文件中找到。
相应地，测量数据到角速度之间转换比例也要变，如：
& & Serial.print(ax/4.096); Serial.print(&,&);
& & Serial.print(ay/4.096); Serial.print(&,&);
& & Serial.print(az/4.096); Serial.print(&,&);
& & Serial.print(gx/16.4); Serial.print(&,&);
& & Serial.print(gy/16.4); Serial.print(&,&);
& & Serial.println(gz/16.4);
&&&&&&&&&&&&&&
&下载DMP的库文件：/jrowberg/i2cdevlib
添加 MPU6050_6Axis_MotionApps20.h
运行其自带的例子MPU6050_DMP6，发现不能正确得到结果，数据是乱的，有一定规律，隔几个数就出现一个FIFO溢出错误，说明问题出在FIFO读取上。不是很明白，于是上网找结果，最后找到一句话&在arduino loop代码最后mpu.resetFIFO();采样数据就稳定了&解决了问题。但是有点治标不治本，等有空再看看库函数学习学习。
DMP后的结果可以是欧拉角（ypr）yaw(偏航），pitch(俯仰），roll（滚转）角度，也可以是四元数等等。
对于欧拉角表示：
roll 是绕y轴旋转的角度
pitch 是绕x轴旋转的角度
yaw 是绕z轴旋转的角度
mpu-6050 &wbr&加速度、陀螺仪传感器的调试（2） &wbr&dmp
&&&&&&&&&&
数据的稳定性：
& 得到的数据的噪声很小，pitch和roll的变动幅度在0.1度以内，而yew一直在漂移。和上一篇中的转载一致。简单做了一下记录，可以看出漂移的量很大：
52.09,1.96,-2.01
52.09,1.97,-2.00
50.56,1.97,-1.96
50.56,1.97,-1.96
48.41,1.96,-1.96
48.41,1.96,-1.96
44.52,1.96,-1.92
44.52,1.96,-1.92
42.17,1.97,-1.95
42.17,1.97,-1.95
19.12,1.97,-1.92
19.12,1.97,-1.92
因此必须校准。同时可以看出角度也是相对量，即和初始位置相关。
****MPU6050库的安装方法*************************johnsonzzd
1. 下载arduino编译器，目前版本为1.0.5。
2. 下载MPU6050库。进入&/jrowberg/i2cdevlib&，点击&zip&即可。
3. 安装。将上一步下载的&I2Cdev&、&MPU6050&两个文件夹拷贝到&arduino-1.0.5\libraries&目录下。
运行arduino可以在例子中看到MPU6050。OK！
&&&&&&&&&&&&&&&&&&&&&&
MPU6050数据处理实验一
关于dmp，我的实验结果是数据非常稳定，应该没有其它方法比这个更稳定了，数据可以直接拿来用不用任何滤波。数据也应该是最准确的了。当然稳定和准确是要付出时间代价的。默认输出中断频率是100Hz,对头文件的数组最后一位改为0可输出200Hz(这是最快的了)，对于一般的应用应该是最好的选择，如果做随动方面的设备，延迟非常明显，不过可以用角速度积分来改善（我是昨天才想到的，这几天有时间来验证一下）。希望能成功吧。如果成功，那mpu6050的稳定精确和实时性就能达到随动的要求了。
---------------------SuperAnt&
平衡控制系统中最复杂的两部分内容，一部分是滤波，另一部分是PID稳定控制。
滤波是为了获得对真实状态的最佳估计，如姿态信息等，而PID控制是为了实现对状态偏差的矫正。其中滤波通常是最复杂也是最难的部分。
好的滤波算法可以获得非常准确地系统状态，提高精度，通常使用动态卡尔曼滤波算法，算法比较复杂，数学功底要求高，是技术活儿。PID控制看似复杂，其实非常简单，运算量不大，关键是三个控制参数的调整，一般需要进行很多次试验才能找到合适的参数，是个体力活儿。
【】【】【】【】
上一篇:下一篇:
CopyRight @
单片机教程网
, All Rights Reserved姿态角（Euler角）pitch yaw roll飞行器的姿态角并不是指哪个角度，是三个角度的统称。它们是：俯仰、滚转、偏航。你可以想象是飞机围绕XYZ三个轴分别转动形成的夹角。地面坐标系（earth-surface inertial reference frame）Sg--------OXgYgZg&ignore_js_op&&①在地面上选一点Og②使Xg轴在水平面内并指向某一方向③Zg轴垂直于地面并指向地心(重力方向)④Yg轴在水平面内垂直于Xg轴，其指向按右手定则确定机体坐标系(Aircraft-body coordinate frame)Sb-------OXYZ&ignore_js_op&&①原点O取在飞机质心处，坐标系与飞机固连②x轴在飞机对称平面内并平行于飞机的设计轴线指向机头③y轴垂直于飞机对称平面指向机身右方④z轴在飞机对称平面内，与x轴垂直并指向机身下方欧拉角/姿态角（Euler Angle）&ignore_js_op&&&ignore_js_op&&机体坐标系与地面坐标系的关系是三个Euler角，反应了飞机相对地面的姿态。俯仰角&（pitch）：机体坐标系X轴与水平面的夹角。当X轴的正半轴位于过坐标原点的水平面之上（抬头）时，俯仰角为正，否则为负。&ignore_js_op&&偏航角&（yaw）：机体坐标系xb轴在水平面上投影与地面坐标系xg轴（在水平面上，指向目标为正）之间的夹角，由xg轴逆时针转至机体xb的投影线时，偏航角为正，即机头右偏航为正，反之为负。&ignore_js_op&&滚转角&P（roll）：机体坐标系zb轴与通过机体xb轴的铅垂面间的夹角，机体向右滚为正，反之为负。&ignore_js_op&
首先要明确，MPU6050 是一款姿态传感器，使用它就是为了得到待测物体（如四轴、平衡小车） x、y、z 轴的倾角（俯仰角 Pitch、滚转角 Roll、偏航角 Yaw） 。我们通过 I2C 读取到 MPU6050 的六个数据（三轴加速度 AD 值、三轴角速度 AD 值）经过姿态融合后就可以得到 Pitch、Roll、Yaw 角。
本帖主要介绍三种姿态融合算法：四元数法 、一阶互补算法和卡尔曼滤波算法。
一、四元数法
关于四元数的一些概念和计算就不写上来了，我也不懂。我能告诉你的是：通过下面的算法，可以把六个数据转化成四元数（q0、q1、q2、q3），然后四元数转化成欧拉角（P、R、Y 角）。
& & & & 虽然 MPU6050 自带的 DMP库可以直接输出四元数，减轻 STM32 的运算负担， 这里在此没有使用，因为我是用 STM32 的硬件 I2C 读取 MPU6050 数据的（），DMP库需要对 I2C 函数进行修改，如 DMP 库中的 I2C 写：i2c_write(st.hw-&addr, st.reg-&pwr_mgmt_1, 1, &(data[0]))；有4个输入变量，而 STM32 硬件 I2C 的 I2C 写为：void MPU6050_I2C_ByteWrite(u8 slaveAddr, u8 pBuffer, u8 writeAddr)，只有 3 个输入量（这之间的差异好像是由于 MPU6050 的 DMP 库是针对 MSP430&写的），所以必须进行修改，但是改固件库是一件很痛苦的事，你们应该都懂。当然，如果你用模拟 I2C 的话，是容易实现的，网上的 DMP 移植几乎都是基于模拟 I2C 的。
#include&math.h&
#include "stm32f10x.h"
//---------------------------------------------------------------------------------------------------
// 变量定义
#define Kp 100.0f& && && && && && && && &// 比例增益支配率收敛到加速度计/磁强计
#define Ki 0.002f& && && && && & // 积分增益支配率的陀螺仪偏见的衔接
#define halfT 0.001f& && && && && & // 采样周期的一半
float q0 = 1, q1 = 0, q2 = 0, q3 = 0;& && && & // 四元数的元素，代表估计方向
float exInt = 0, eyInt = 0, ezInt = 0;& && &&&// 按比例缩小积分误差
float Yaw,Pitch,R&&//偏航角，俯仰角，翻滚角
void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az)
& && &&&float vx, vy,
& && &&&float ex, ey,&&
& && &&&// 测量正常化
& && &&&norm = sqrt(ax*ax + ay*ay + az*az);& && &
& && &&&ax = ax /& && && && && && & //单位化
& && &&&ay = ay /
& && &&&az = az /& && &
& && &&&// 估计方向的重力
& && &&&vx = 2*(q1*q3 - q0*q2);
& && &&&vy = 2*(q0*q1 + q2*q3);
& && &&&vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;
& && &&&// 错误的领域和方向传感器测量参考方向之间的交叉乘积的总和
& && &&&ex = (ay*vz - az*vy);
& && &&&ey = (az*vx - ax*vz);
& && &&&ez = (ax*vy - ay*vx);
& && &&&// 积分误差比例积分增益
& && &&&exInt = exInt + ex*Ki;
& && &&&eyInt = eyInt + ey*Ki;
& && &&&ezInt = ezInt + ez*Ki;
& && &&&// 调整后的陀螺仪测量
& && &&&gx = gx + Kp*ex + exI
& && &&&gy = gy + Kp*ey + eyI
& && &&&gz = gz + Kp*ez + ezI
& && &&&// 整合四元数率和正常化
& && &&&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;&&
& && &&&// 正常化四元
& && &&&norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
& && &&&q0 = q0 /
& && &&&q1 = q1 /
& && &&&q2 = q2 /
& && &&&q3 = q3 /
& && &&&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; // rollv
& && &&&//Yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3;& && && && && & //此处没有价值，注掉
& & & 要注意的的是，四元数算法输出的是三个量 Pitch、Roll 和 Yaw，运算量很大。而像平衡小车这样的例子只需要一个角（Pitch 或 Roll ）就可以满足工作要求，个人觉得做平衡小车最好不用四元数法。
二、一阶互补算法
& & & &MPU6050 可以输出三轴的加速度和角速度。通过加速度和角速度都可以得到 Pitch 和 Roll 角（加速度不能得到 Yaw 角），就是说有两组 Pitch、Roll 角，到底应该选哪组呢？别急，先分析一下。MPU6050 的加速度计和陀螺仪各有优缺点，三轴的加速度值没有累积误差，且通过算 tan()&&可以得到倾角，但是它包含的噪声太多（因为待测物运动时会产生加速度，电机运行时振动会产生加速度等），不能直接使用；陀螺仪对外界振动影响小，精度高，通过对角速度积分可以得到倾角，但是会产生累积误差。所以，不能单独使用 MPU6050 的加速度计或陀螺仪来得到倾角，需要互补。一阶互补算法的思想就是给加速度和陀螺仪不同的权值，把它们结合到一起，进行修正。得到 Pitch 角的程序如下：
//一阶互补滤波
float K1 =0.1; // 对加速度计取值的权重
float dt=0.001;//注意：dt的取值为滤波器采样时间
angle_ax=atan(ax/az)*57.3;& &&&//加速度得到的角度
gy=(float)gyo[1]/7510.0;& && & //陀螺仪得到的角速度
Pitch = yijiehubu(angle_ax,gy);
float yijiehubu(float angle_m, float gyro_m)//采集后计算的角度和角加速度
& &&&angle = K1 * angle_m + (1-K1) * (angle + gyro_m * dt);
& & 互补算法只能得到一个倾角，这在平衡车项目中够用了，而在四轴飞行器设计中还需要 Roll 和 Yaw，就需要两个 互补算法，我是这样写的，注意变量不要搞混：
//一阶互补滤波
float K1 =0.1; // 对加速度计取值的权重
float dt=0.001;//注意：dt的取值为滤波器采样时间
float angle_P,angle_R;
float yijiehubu_P(float angle_m, float gyro_m)//采集后计算的角度和角加速度
& &&&angle_P = K1 * angle_m + (1-K1) * (angle_P + gyro_m * dt);
& && && &return angle_P;
float yijiehubu_R(float angle_m, float gyro_m)//采集后计算的角度和角加速度
& &&&angle_R = K1 * angle_m + (1-K1) * (angle_R + gyro_m * dt);
& && && &return angle_R;
单靠 MPU6050 无法准确得到 Yaw 角，需要和地磁传感器结合使用。
三、卡尔曼滤波
& & & 其实卡尔曼滤波和一阶互补有些相似，输入也是一样的。卡尔曼原理以及什么5个公式等等的，我也不太懂，就不写了，感兴趣的话可以上网查。在此给出具体程序，和一阶互补算法一样，每次卡尔曼滤波只能得到一个方向的角度。
#include&math.h&
#include "stm32f10x.h"
#include "Kalman_Filter.h"
//卡尔曼滤波参数与函数
float dt=0.001;//注意：dt的取值为kalman滤波器采样时间
float angle, angle_//角度和角速度
float P[2][2] = {{ 1, 0 },
& && && && && &&&{ 0, 1 }};
float Pdot[4] ={ 0,0,0,0};
float Q_angle=0.001, Q_gyro=0.005; //角度数据置信度,角速度数据置信度
float R_angle=0.5 ,C_0 = 1;
float q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;
//卡尔曼滤波
float Kalman_Filter(float angle_m, float gyro_m)//angleAx 和 gyroGy
& && &&&angle+=(gyro_m-q_bias) *
& && &&&angle_err = angle_m -
& && &&&Pdot[0]=Q_angle - P[0][1] - P[1][0];
& && &&&Pdot[1]=- P[1][1];
& && &&&Pdot[2]=- P[1][1];
& && &&&Pdot[3]=Q_
& && &&&P[0][0] += Pdot[0] *
& && &&&P[0][1] += Pdot[1] *
& && &&&P[1][0] += Pdot[2] *
& && &&&P[1][1] += Pdot[3] *
& && &&&PCt_0 = C_0 * P[0][0];
& && &&&PCt_1 = C_0 * P[1][0];
& && &&&E = R_angle + C_0 * PCt_0;
& && &&&K_0 = PCt_0 / E;
& && &&&K_1 = PCt_1 / E;
& && &&&t_0 = PCt_0;
& && &&&t_1 = C_0 * P[0][1];
& && &&&P[0][0] -= K_0 * t_0;
& && &&&P[0][1] -= K_0 * t_1;
& && &&&P[1][0] -= K_1 * t_0;
& && &&&P[1][1] -= K_1 * t_1;
& && &&&angle += K_0 * angle_ //最优角度
& && &&&q_bias += K_1 * angle_
& && &&&angle_dot = gyro_m-q_//最优角速度
& && &&&return angle；
& & & 作个总结：三种融合算法都能够输出姿态角（Pitch 和 Roll ），一次四元数法可以输出 P、R、Y 三个倾角，计算量比较大。一阶互补和卡尔曼滤波每次只能输出一个轴的姿态角。
阅读(...) 评论()}