萤火

有一份热，发一分光

0%

An Introduction for IMU 1 - IMU原理与MPU6050数据采集

发表于 更新于 分类于 阅读次数： Valine：

本系列博客将从IMU的基础概念和基本原理出发，基于Arduino单片机和MPU6050传感器，介绍数据采集、数据融合、姿态解算，进一步设计无线IMU模块，开发人体姿态测量系统，并同步到生物力学分析软件OpenSim中进行逆运动学分析，等。

IMU的测量原理

IMU全称Inertial Measurement Unit，惯性测量单元，主要用来检测和测量加速度与旋转运动的传感器。根据测量原理不同，可分为微机电传感器(MEMS)和光纤陀螺仪，其中光纤陀螺主要用于航空航天与军事领域，民用IMU大都是基于惯性原理的MEMS传感器。IMU根据所能感知状态量的个数(DOF)进行区分，其中6DOF的IMU最为常见，也就是可以测量三轴加速度和三轴角速度。9DOF的IMU会多一个三轴磁力计，10DOF的IMU再多一个气压计。

加速度计的物理实现是利用牛顿第二定律，如上左图所示，中间红色物体为一个质量块，两头通过具有弹簧性质的长条结构与基底相连，红色的短栅与绿色的短栅分别为电容的极板。当基底在双箭头方向有加速度a时，由$f=ma=kx$，质量块会沿加速度相反的方向移动，红色极板与绿色极板之间的距离会发生变化，通过测量极板电容$C$的变化就可以得到加速度的大小。在三轴加速度计中，这样的结构在三个方向各有一个，且做到了微米的尺寸，并配合相应的测量电路集成在一个芯片中，构成一个微机电系统。

3

角速度测量的原理比加速度要复杂一些，利用了在旋转坐标系下运动的物体会受到科里奥里力(Coriolis Force)的特性。科里奥里力是由坐标系的转动与物体在动坐标系中的相对运动引起的，其本质是物体的惯性。如果图示模块置于绝对静止的坐标系中，当在x方向施加一个驱动力使质量块运动时，根据牛顿第二定律，质量块只会在x方向上运动，而在y方向上不会运动。但如果将图示模块置于一个旋转坐标系下，由于坐标系的旋转，使得当质量块沿x方向运动时，在y方向上会受到一个力，即科里奥里力F=-2mvω，从而使质量块沿y方向运动。地球上的很多自然现象，如热带气旋、季风带、河道两侧冲刷程度不同，都源于科里奥里力。

陀螺仪的物理实现如上右图所示，外侧的蓝色与黄色部分为驱动电极，它们在模块的驱动方向施加交变电压，使内部的质量块以及红色的测量电极沿着驱动方向运动。当整个平台发生旋转时，质量块受科里奥里力影响会在垂直方向上发生运动，且周期运动的幅值与平台的角速度成正比，通过测量质量块上的红色电极和固定在底座上蓝色电极之间的电容，便可以得到角速度的大小。

MPU6050

MPU6050是InvenSense公司推出的一款高性能三轴加速度+三轴陀螺仪的六轴传感器芯片， 该芯片内部整合了 3 轴陀螺仪和 3 轴加速度传感器，并可利用自带的数字运动处理器（DMP: Digital Motion Processor）硬件加速引擎，体积较小，减少了大量的封装空间。主要特性如下：

  • 6位ADC和信号调理的三轴MEMS陀螺仪，具有 131 LSBs/° /sec 敏感度与全格感测范围为±250、±500、±1000 与±2000°/sec 的 3 轴角速度感测器(陀螺仪)；
  • 16位ADC和信号调理的三轴MEMS加速度计，范围为±2g、±4g、±8g 和±16g 的 3 轴加速度传感器；
  • 自带数字运动处理(DMP: Digital Motion Processing)引擎，内建运作时间偏差与磁力感测器校正演算技术，以数字形式输出 6 轴或 9 轴（需外接磁传感器）的旋转矩阵、四元数、欧拉角格式的融合演算数据，可减少 MCU 复杂的融合演算数据、感测器同步化、姿势感应等的负荷；
  • VDD 供电电压为 2.5V±5%、 3.0V±5%、 3.3V±5%； VLOGIC 可低至 1.8V± 5%；
  • 陀螺仪工作电流： 5mA，陀螺仪待机电流： 5uA；加速度计工作电流： 500uA，加速度计省电模式电流： 40uA@10Hz；
  • 支持最高 400Khz 的 IIC 通信，IIC地址为 0X68（AD0接GND）或0x69（AD0接VDD）；

MPU6050数据读取

本篇博客通过Arduino Nano读取MPU6050的数据，相同的程序也适用于其他的Arduino开发版。程序主要通过调用现有的MPU6050库，同时将数据通过匿名上位机以波形的方式显示出来，相关的库函数、上位机软件、上位机的Arduino通信函数可以到CSDN资源下载，也可以在GITHUB上下载，本部分读取IMU数据的程序请在此处下载。库函数的安装方法，以及匿名上位机的使用方法请参考其他论坛和博客，若使用存在问题烦请给我留言。

7

Arduino通过I2C于MPU6050进行通信，因此除了供电引脚外，还需要连接I2C引脚。根据上面的Arduino Nano引脚图，需要将MPU6050的SCL连接到Arduino Nano的A5引脚，SDA连接到Arduino的A4引脚。具体连线图如下所示：

5

程序主要调用了MPU6050的库，同时为了能够通过上位机显示数据波形，还调用了项目文件下的ANO.h里的函数来向上位机发送数。调用部分包括如下内容：

1
2
3
4
5
6
#include "I2Cdev.h"
#include "MPU6050.h"
#include "ANO.h"
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
	#include "Wire.h"
#endif

在setup之前需要先定义一些MPU6050的相关变量，包括MPU6050对象的定义，以及三轴的加速度与角速度：

1
2
3
MPU6050 IMU;
int16_t ax, ay, az;
int16_t gx, gy, gz;

在setup函数中，分别对串口、I2C总线以及MPU6050进行初始化：

1
2
3
4
5
6
7
8
9
10
11
12
13
void setup() 
{
  // Initialize Serial
  Serial.begin(115200);
  // Initialize I2C & IMU
  // By default, the measurement range of acc is 2g, and the gyro's range is 250 deg/s
  #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
      Wire.begin();
  #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
      Fastwire::setup(400, true);
  #endif
  IMU.initialize();
}

在这里有两点需要注意：

  • I2C初始化的时候会和硬件存在一定关系，大部分开发版通过软件的方式进行I2C通信，也就是Wire.h库，但有些开发版具有硬件I2C，可以更快的速度通信（400kHz）。
  • MPU6050的initialize函数默认将MPU6050的测量范围设置为加速度2g，角速度250deg/s。因此初始化之后还需合理的设置传感器的测量范围，以及offset。

初始化完成后便可以通过getMotion6函数读取MPU6050的数据，这里进一步将读取的数据用ANO_DT_Send_Senser函数通过串口发送给上位机：

1
2
3
4
5
6
7
8
void loop() 
{
  // Get IMU Raw Data
  IMU.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  // Send Data to PC
  ANO_DT_Send_Senser(ax,ay,az,gx,gy,gz,0,0,0);
  delay(10);
}

下图为上下移动传感器时上位机的波形曲线，黄色为竖直方向Z轴的加速度，完整的程序请在此处下载。

4

MPU6050的DMP数据读取

MPU6050得到的原始数据仅有加速度与角速度，但很多时候我们最需要用的是三轴的姿态角信息。通过加速度和角速度估计姿态角叫做IMU的姿态解算，下一篇博客将介绍姿态解算的原理与两种姿态计算的方法-互补滤波与卡尔曼滤波。其实MPU6050内部自带了姿态解算算法，也就是DMP。DMP是MPU6050内部的运动引擎，全称Digital Motion Processor，直接输出姿态解算后的四元数，可以减轻外围微处理器的工作负担且避免了繁琐的滤波和数据融合。Invensense提供了DMP的数据接口，但并非完全开源，Arduino平台下也有DMP读取的库，本篇博客也直接调用库函数来实现DMP数据的读取，相关的库和依赖还是在上文的链接中下载。

DMP的数据需要采用中断的方式进行读取，当DMP数据准备完成后，MPU6050的INT引脚会输出一个高电平脉冲，因此除了I2C引脚外，还需要将MPU6050的INT引脚接到Arduino Nano的0号中断通道，也就是数字引脚D2（中断通道序号和物理引脚序号的映射关系参考Arduino论坛），连线图如下：

5

读取DMP数据需要加载DMP的支持，因此用MPU6050_6Axis_MotionApps20.h代替MPU6050.h。同时为了对读取DMP数据的频率进行评估，以及向上位机定时发送数据，还另外调用了定时中断库，所有的调用如下：

1
2
3
4
5
6
7
8
#include "MsTimer2.h"
#include "I2Cdev.h"
//#include "MPU6050.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include "ANO.h"
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
    #include "Wire.h"
#endif

程序首先需要定义一些MPU6050相关的对象和变量，包括MPU6050对象、DMP相关的状态变量、传感器的数据变量、以及用于计算DMP返回频率的变量等。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
// MPU6050 Object
MPU6050 IMU;
// DMP state variables
bool dmpReady = false;  // set true if DMP init was successful
bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
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
// Measurement Data 
int16_t acc[3];
int16_t gyro[3];
float euler[3];
Quaternion q;
// Data Receive Frequence Count
int dataCounter = 0;
int dataFreq = 0;

传感器的初始化代码如下，包括初始化I2C总线、初始化MPU6050芯片、初始化DMP模块；当DMP模块被成功初始化之后，使能DMP模块，并设置Arduino的0号中断通道（数字引脚D2）为外部中断上升沿触发，然后从MPU6050获取DMP返回正确数据包的长度。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
void IMU_Init()
{
  // Initialize I2C & IMU
  // By default, the measurement range of acc is 2g, and the gyro's range is 250 deg/s
  #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
      Wire.begin();
  #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
      Fastwire::setup(400, true);
  #endif
  IMU.initialize();
  delay(200);
  
  // Initialize DMP
  devStatus = IMU.dmpInitialize();

  if (devStatus == 0) 
  {
    // turn on the DMP
    IMU.setDMPEnabled(true);
    // enable Arduino interrupt detection
    attachInterrupt(0, dmpDataReady, RISING);
    mpuIntStatus = IMU.getIntStatus();
    // set our DMP Ready flag
    dmpReady = true;
    // get expected DMP packet size for later comparison
    packetSize = IMU.dmpGetFIFOPacketSize();
  }
  else
    Serial.println("IMU Init Failed!");
}

// INTERRUPT DETECTION ROUTINE
void dmpDataReady() 
{
    mpuInterrupt = true;
}

以上初始化完成后，当MPU6050的中断引脚产生高电平信号，且FIFO缓存区数据的长度大于正确数据包的长度后，就可以读取DMP数据了。数据读取的代码如下，首先对初始化状态和外部中断状态进行判断；当外部中断触发后，首先读取MPU6050当前的中断状态，当状态为0x02表示解算完成（0x10表示缓存溢出中断，需要清空FIFO），再循环读取FIFO寄存器的数据长度，当长度大于一个正确数据包的长度后，从FIFO中读取数据getFIFOBytes，然后将数据解算为四元数dmpGetQuaternion，再变化到欧拉角dmpGetEuler。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
void IMU_DMP_read()
{
  // return if IMU is not initialized
  if (!dmpReady) return;
  // wait for MPU interrupt or extra packet(s) available
  if(!mpuInterrupt) return;

  // reset interrupt flag and get INT_STATUS byte
  mpuInterrupt = false;
  mpuIntStatus = IMU.getIntStatus();

  // get current FIFO count
  fifoCount = IMU.getFIFOCount();

  // check for overflow
  if ((mpuIntStatus & 0x10) || fifoCount == 1024) 
  {
    // reset so we can continue cleanly
    IMU.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 = IMU.getFIFOCount();

    // read a packet from FIFO
    IMU.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;

    IMU.dmpGetQuaternion(&q, fifoBuffer);
    IMU.dmpGetEuler(euler, &q);
    
    dataCounter++;

  }  
}

主程序中初始化串口和IMU，同时开启5ms定时中断向上位机发送欧拉角数据，同时对DMP接受到的数据频率进行统计；主循环中不断执行DMP的数据读取，代码如下：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
void setup() 
{
  Serial.begin(115200);
  IMU_Init();
  MsTimer2::set(5,sendPC);
  MsTimer2::start();
}

void loop() 
{
  IMU_DMP_read(); 
}

void sendPC()
{
  static int timeCounter = 0;
  
  timeCounter += 5;
  if(timeCounter > 1000)
  {
    dataFreq = dataCounter;
    dataCounter = 0;
    timeCounter = 0;
  }
  
  // Send Data to PC
  ANO_DT_Send_Senser(euler[1]*180/M_PI,euler[2]*180/M_PI, euler[0]*180/M_PI, dataFreq,0,0,0,0,0);
}

下图为旋转传感器时上位机的波形曲线，前三条曲线为欧拉角；第四条蓝色曲线为DMP读取的频率，实测结果约为66Hz，可能和软件I2C有关，其他一些博客在STM32 F407平台上测试结果为100Hz，官方手册中最高为200Hz。本部分的完整程序请在此处下载。

8