点击联系发帖人
时间：2019-07-03 14:23
|
首先要明确MPU6050 是一款姿态传感器，使用它就是为了得到待测物体（如四轴、平衡小车） x、y、z 轴的倾角（俯仰角 Pitch、滚转角 Roll、偏航角 Yaw） 我们通过 I2C 读取到 MPU6050 的六个数据（三轴加速度 AD 值、三轴角速度 AD 值）经过姿态融合后就可以得到 Pitch、Roll、Yaw 角。
本帖主要介绍三种姿态融合算法：四元数法 、一阶互补算法和姿态角卡尔曼濾波波算法
关于四元数的一些概念和计算就不写上来了，我也不懂我能告诉你的是：通过下面的算法，可以把六个数据转化成四元数（q0、q1、q2、q3）然后四元数转化成欧拉角（P、R、Y 角）。
要注意的的是四元数算法输出的是三个量 Pitch、Roll 和 Yaw，运算量很大而像平衡小车这样的唎子只需要一个角（Pitch 或 Roll ）就可以满足工作要求，个人觉得做平衡小车最好不用四元数法
MPU6050 可以输出三轴的加速度和角速度。通过加速度和角速度都可以得到 Pitch 和 Roll 角（加速度不能得到 Yaw 角）就是说有两组 Pitch、Roll 角，到底应该选哪组呢别急，先分析一下MPU6050 的加速度计和陀螺仪各有优缺点，三轴的加速度值没有累积误差且通过算 tan() 可以得到倾角，但是它包含的噪声太多（因为待测物运动时会产生加速度电机运行时振動会产生加速度等），不能直接使用；陀螺仪对外界振动影响小精度高，通过对角速度积分可以得到倾角但是会产生累积误差。所以不能单独使用 MPU6050 的加速度计或陀螺仪来得到倾角，需要互补一阶互补算法的思想就是给加速度和陀螺仪不同的权值，把它们结合到一起进行修正。得到 Pitch 角的程序如下：
互补算法只能得到一个倾角这在平衡车项目中够用了，而在四轴飞行器设计中还需要 Roll 和 Yaw就需要两个 互补算法，我是这样写的注意变量不要搞混：
单靠 MPU6050 无法准确得到 Yaw 角，需要和地磁传感器结合使用
其实姿态角卡尔曼滤波波和一阶互补囿些相似，输入也是一样的卡尔曼原理以及什么5个公式等等的，我也不太懂就不写了，感兴趣的话可以上网查在此给出具体程序，囷一阶互补算法一样每次姿态角卡尔曼滤波波只能得到一个方向的角度。
//姿态角卡尔曼滤波波参数与函数
作个总结：三种融合算法都能夠输出姿态角（Pitch 和 Roll ）一次四元数法可以输出 P、R、Y 三个倾角，计算量比较大一阶互补和姿态角卡尔曼滤波波每次只能输出一个轴的姿态角。
注：使用“匿名地面站”上位机软件给调试过程带来了很大便捷。软件可在附件中下载
通过上位机可以看到，姿态融合成功！
MPU6050：加速度做一维姿态角卡尔曼滤波波角速度做一介低通滤波。
接着串级PID控制平衡外环角度环，内环角速度环
高度控制：高度和速度进行互补滤波（气压计得到的是高度，速度通过Z轴加速度积分获得而加速度通过陀螺仪的Z轴得到。）
第一行为载体坐标下X方向单位化向量第一列为x向量，第二列为y向量第三列为z向量。
第一行为载体坐标下Y方向单位化向量第一列为x向量，第二列为y向量第三列为z向量。
第一行为载体坐标下Z方向单位囮向量第一列为x向量，第二列为y向量第三列为z向量。
龙格库塔求解四元数。未完待续。
