陀螺仪mpu6050相关总结

声明：以下说法并不准确甚至不正确，只是为了让大家了解一下原理，涉及专业知识请查阅相关书籍

陀螺仪和加速计

这就是传说中可以引导洲际导弹的陀螺仪……很高科技的样子，相信有很多人小时候玩过

图片1.png

关于陀螺仪的定义：

敏感角运动的一种精密传感器，是惯性导航系统的中最重要、技术含量最高的仪器，是惯导系统中的核心器件。

陀螺仪的精度是惯导系统精度的主要决定因素，大家就不要想着用十几块钱的低性能地摊货去做战略导弹了，做战术导弹还是可以的

关于飘移：在干扰力矩的作用下陀螺仪产生的进动，使得自转轴在惯性空间逐渐偏离原来的方位，这种现象称之为漂移，零偏漂移就是零输入（静止）下的漂移。

关于加速计：加速度本身难以直接测量，因此通过测量敏感质量块上形成的惯性力，间接测量载体受到的加速度

在惯性空间，加速度计无法区分惯性力和万有引力。因此加速度计的输出反映的是单位检测质量所受的惯性空间的合力，即惯性力与万有引力的矢量和，
一般加速传感器厂商都会提供消去重力加速度的算法，

另外，加速计也有漂移，不过相对陀螺仪要稍微好一些

关于输出
MPU6050不只是简简单单整合了3轴陀螺仪、3轴加速器；其含可藉由第二个I2C端口连接其他厂牌之加速器、磁力传感器、或其他传感器的数位运动处理(DMP: Digital Motion Processor)硬件加速引擎，由主要I2C端口以单一数据流的形式，向应用端输出完整的9轴融合演算技术。

InvenSense 的运动处理资料库，可处理运动感测的复杂数据，降低了运动处理运算对操作系统的负荷，并为应用开发提供架构化的API。

不过在这里先不讨论这些高深的东西，还是先拿它当简单的3轴陀螺仪、3轴加速器用吧，

硬件的连接如图（这里给出一个参考）：

图片2.png

现在看一个简单的程序

ARDUINO 代码

#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"
MPU6050 accelgyro; //实例化传感器
int16_t ax, ay, az; //定义原始数据
int16_t gx, gy, gz;
char str[512];
void setup() {
Wire.begin();
Serial.begin(57600);
accelgyro.initialize();
}
void loop() {
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); //获得原始数据
sprintf(str, "%d,%d,%d,%d,%d,%d", ax, ay, az, gx, gy, gz);
Serial.print(str);
Serial.write(byte(10));
delay(30);
}

上面这段代码是基于arduino的mpu6050类库的（这个类库并非arduino的官方类库，需要另行下载），mpu6050类库下载https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/MPU6050这里只是简单的将六轴原始数据进行输出，并没有进行量程设置，标定等其他的工作，关于标定和量程的设置下面还会讲到

关于DMP和FIFO的使用，可以在上面的类库里找到源文件和相关例子，里面非常详细，不懂英文的话就……

就去到论坛里找弘毅写的“MPU6050.cpp核心代码注释”，寄存器的手册也有翻译版，相关资料都很详细，这里就不多讲了，感兴趣的可以去挖坟
这里再贴一段代码：

ARDUINO 代码

#include "Wire.h"
//I2Cdev 和 MPU6050 类库需要事先安装在 Arduino 类库文件夹下
//引入 I2Cdev.h 头文件
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
//实例化一个 MPU6050 对象，对象名称为 mpu
//声明MPU6050 控制和状态变量
bool dmpReady false; //set true if DMP init was successful
uint8_t mpuIntStatus; //此变量用于保存 MPU6050 中断状态
uint8_t devStatus; //[size=1em]返回设备状态，0为成功，不为0则发生错误
uint16_t packetSize; //expected DMP packet size (default is 42 bytes)
uint16_t fifoCount; //count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; //FIFO storage buffer
//声明方向和运动变量：
Quaternion q; //四元数变量 W,X,Y,Z
VectorFloat gravity; //重力矢量 X，Y, Z
float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
// =========================== 中断检测程序 =========================
volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
void dmpDataReady()
{
mpuInterrupt = true;
// =============== 初始设置凡 Arduino 代码都有 setup() ===============
void setup()
{
Serial.begin(115200); //开启串口，设置波特率为 115200，程序下载到 Arduino 之后注意打开串口观察
//加入 I2C 总线序列
Wire.begin();
//初始化MPU6050
Serial.println("Initializing I2C devices...");
mpu.initialize();
//验证连接
Serial.println("Testing device connections...");
Serial.println(mpu.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
delay(2); //延时2毫秒
//加载并配置 DMP 数字运动处理引擎
Serial.println("Initializing DMP...");
devStatus = mpu.dmpInitialize(); //返回 DMP 状态结果，0为成功，不为0则发生错误
//如果成功返回 0
if (devStatus == 0)英锐恩是一家新兴的单片机厂商，供给经营品牌位单片机、位单片机等产品方案规划，以及单片机相关资讯，如需了解更多内容，请拜访.。
{
//使能 DMP 数字运动处理引擎
Serial.println("Enabling DMP...");
mpu.setDMPEnabled(true);
//使能 Arduino 中断检测
Serial.println("Enabling interrupt detection (Arduino external interrupt 0)...");
attachInterrupt(0, dmpDataReady, RISING);
mpuIntStatus = mpu.getIntStatus();
// set our DMP Ready flag so the main loop() function knows it's okay to use it
Serial.println("DMP ready! Waiting for first interrupt...");
dmpReady = true;
// get expected DMP packet size for later comparison
packetSize = mpu.dmpGetFIFOPacketSize();
}
else {
// ERROR!
// 1 = initial memory load failed
// 2 = DMP configuration updates failed
// (if it's going to break, usually the code will be 1)
Serial.print("DMP Initialization failed (code ");
Serial.print(devStatus);
Serial.println(")");
}
}
// =========================== 主循环程序体 =========================
void loop()
{
float alpha, omiga; //@声明2个浮点数变量， alpha 和 omiga
[size=1em]//如果 MPU6050 的 DMP 状态为错误，程序停止工作
if (!dmpReady)
return;
// wait for MPU interrupt or extra packet(s) available
if (!mpuInterrupt && fifoCount < packetSize)
return;
// reset interrupt flag and get INT_STATUS byte
mpuInterrupt = false;
mpuIntStatus = mpu.getIntStatus();
// get current FIFO count
fifoCount = mpu.getFIFOCount();
// check for overflow (this should never happen unless our code is too inefficient)
if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
// reset so we can continue cleanly
mpu.resetFIFO();
Serial.println("FIFO overflow!");
// otherwise, check for DMP data ready interrupt (this should happen frequently)
}
else if (mpuIntStatus & 0x02) {
// wait for correct available data length, should be a VERY short wait
while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
// read a packet from FIFO
mpu.getFIFOBytes(fifoBuffer, packetSize);
// track FIFO count here in case there is > 1 packet available
// (this lets us immediately read more without waiting for an interrupt)
fifoCount -= packetSize;
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); //从DMP中取出Yaw, Pitch, Roll三个轴的角度，放入数组ypr单位：弧度
alpha=-ypr[2] * 180/M_PI;
omiga=mpu.getRotationX()/16.4; //配置是16位表示正负2000°/s, 65536/4000
Serial.print("Alpha ");
Serial.print(alpha);
Serial.print("\tOmiga ");
Serial.println(omiga);
}
}

例子下载地址：http://www.tonylabs.com/uploads/files/sourcecode/MPU6050_Sketch_by_TONYLABS_ino.zip

关于量程和测量精度以MPU6050加速度测量值为例：量程是±8g时，测量精度是4096LSB/g，LSB的意思是最小有效位，为数字输出方式下使用；

一般我们可以用mg/LSB来表示灵敏度，例如：mpu6050输出的位数为16位(2的16次方共65536个LSB)对应满量程，当量程为±8g时对应灵敏度就为16g/65536LSB=0.244140625mv/g，取倒数为4096LSB/g，因为mpu6050只能16位输出，所以测量范围越大，对应精度就越低

下面是手册上关于量程和精度的说明部分，详情请查阅官方手册

图片4.png

图片3.png

关于传感器标定

是仪器就会有误差，像很多仪器一样mpu6050也要在使用前校正，传感器的标定类似于天平使用前的调零，所以每次使用前进行传感器的标定工作是必要的，标定是计算修正偏移量的过程，得到偏移量后，在使用传感器时需要将传感器输出数据减去偏移量，这样才能得到修正后的数据
标定有很多方法，比较简单的一种方法，当然也是最不精确的一种方法是在静止时取多次测得数据的平均值，
标定后要将偏移量减去，标定的具体步骤不想多说，x轴角速度标定参考代码如下：

#define o_Gx=-2.11; //宏定义x轴角速度偏移量，-2.11即测得的偏移量
Gx=(gx-o_Gx); //其中gx是测量数据，Gx是标定后的数据
标定的方法还有很多，这里只是简单提供一个参考，在程序应用中可以在静止状态下由初始化代码自动完成传感器标定工作，上面图形中我只是在setup里做了采集1000次求均值然后以此作为偏量进行标定

不过，我相信任何一个专业人员都不会采用这种所谓的标定方法

图片5.png

这里附带一句，串口输出数据并绘波形图的方法有很多，可以先将数据保存下来事后用excel绘制，不过这样无法做到实时绘图，
串口实时绘图的软件网上也有很多例子，不想自己写的在这里推荐SerialChart，关于SeralChart的使用方法论坛里有相关帖子，
下载链接什么的自己搜吧

关于四元数

方向余弦，欧拉角，四元数东西太多，这里只简单说一下四元数，其他的自己找本惯性技术慢慢看吧

四元数是由一个实数单位和三个虚数单位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)) ，公式什么的输入很费劲，四元数就先写到这里。

> > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > >

初学者到底是应该先学C还是汇编?答：对于单片机的初学者来说，应该从汇编学起。因为汇编语言是最接近机器码的一种语言，可以加深初学者对单片机各个功能模块的了解，从而打好扎实的基础。