【电子陀螺仪模块】 应用介绍

        MPU-60X0 是全球首例9 轴运动处理传感器。它集成了3 轴MEMS 陀螺仪，3 轴MEMS加速度计，以及一个可扩展的数字运动处理器DMP（Digital Motion Processor），可用I2C接口连接一个第三方的数字传感器，比如磁力计。扩展之后就可以通过其I2C 或SPI 接口输出一个9 轴的信号（SPI 接口仅在MPU-6000 可用）。MPU-60X0 也可以通过其I2C 接口连接非惯性的数字传感器，比如压力传感器。
         MPU-60X0 对陀螺仪和加速度计分别用了三个16 位的ADC，将其测量的模拟量转化为可输出的数字量。为了精确跟踪快速和慢速的运动，传感器的测量范围都是用户可控的，陀螺仪可测范围为±250，±500，±1000，±2000°/秒（dps），加速度计可测范围为±2，±4，±8，±16g。
         MPU6050的用途非常广泛，手机、可穿戴式设备、VR/AR游戏设备等设备都需要用到MPU6050的传感器来读取移动的角速度和角度。

         笔者以小R科技的WiFi机器人小车为例，通过MPU6050的传感器来获得小车的转动角度。          如图所示，三位空间的坐标如图所示。由图可知，将陀螺仪装在小车上，我们需要通过陀螺仪获得的数据是Yaw的旋转变量，即小车在一个平面上向左或者向右旋转的角度。我们需要通过读取陀螺仪的旋转角速度，进而计算出小车的旋转角度，以此来控制小车每次角度。
         首先是定义陀螺仪的数据变量：
#include "I2Cdev.h"
#include "MPU6050.h"
 
// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
// is used in I2Cdev.h
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif
 
// class default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for InvenSense evaluation board)
// AD0 high = 0x69
MPU6050 accelgyro;
//MPU6050 accelgyro(0x69); // <-- use for AD0 high
int16_t ax, ay, az;
int16_t gx, gy, gz;
int axoffs,ayoffs,azoffs;
int gxoffs,gyoffs,gzoffs;
double rgx,rgy,rgz;
double rax,ray,raz;
double ax0,ay0,az0;
double ax1,ay1,az1;
double wx0,wy0,wz0;
double wx1,wy1,wz1;
double dwx,dwy,dwz;
double q0,q1,q2,q3;
double p0,p1,p2,p3;
double qc0,qc1,qc2,qc3;
double roll,pitch,yaw;
double t1;
double t0;
double t;
double dw;
// uncomment "OUTPUT_READABLE_ACCELGYRO" if you want to see a tab-separated
// list of the accel X/Y/Z and then gyro X/Y/Z values in decimal. Easy to read,
// not so easy to parse, and slow(er) over UART.
#define OUTPUT_READABLE_ACCELGYRO
 
// uncomment "OUTPUT_BINARY_ACCELGYRO" to send all 6 axes of data as 16-bit
// binary, one right after the other. This is very fast (as fast as possible
// without compression or data loss), and easy to parse, but impossible to read
// for a human.
//#define OUTPUT_BINARY_ACCELGYRO
 
#define LED_PIN 13
bool blinkState = false;
 
// 然后是打开串口准备接收和发送数据：
void setup() {
// join I2C bus (I2Cdev library doesn*t do this automatically)
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
Wire.begin();
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::setup(400, true);
#endif
 
// initialize serial communication
// (38400 chosen because it works as well at 8MHz as it does at 16MHz, but
// it*s really up to you depending on your project)
Serial.begin(9600);
 
// initialize device
Serial.println("Initializing I2C devices...");
accelgyro.initialize();
accelgyro.setFullScaleGyroRange(MPU6050_GYRO_FS_500);
accelgyro.setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
 
// verify connection
Serial.println("Testing device connections...");
Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
 
// use the code below to change accel/gyro offset values
 
q0=1;
q1=0;
q2=0;
q3=0;
 
getoffs();
 
// configure Arduino LED for
pinMode(LED_PIN, OUTPUT);
}
 
// 然后写个函数获取小车角速度和转动角度的数据：
void Readangle() {
// read raw accel/gyro measurements from device
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
 
t0=t1;
t1=micros();
t=(t1-t0)/1000000;
 
 
// these methods (and a few others) are also available
//accelgyro.getAcceleration(&ax, &ay, &az);
//accelgyro.getRotation(&gx, &gy, &gz);
ax=ax+axoffs;
ay=ay+ayoffs;
az=az+azoffs;
gx=gx+gxoffs;
gy=gy+gyoffs;
gz=gz+gzoffs;
//calibration error
 
rgx=gx;
rgy=gy;
rgz=gz;
rax=ax;
ray=ay;
raz=az;
 
wx0=wx1;
wy0=wy1;
wz0=wz1;
//
 
p0=q0;
p1=q1;
p2=q2;
p3=q3;
//
 
wx1=rgx/65.5;
wy1=rgy/65.5;
wz1=rgz/65.5;
ax1=(rax/16384)*9.80;
ay1=(ray/16384)*9.80;
az1=(raz/16384)*9.80;
//
 
dwx=((wx1+wx0)/2)*t*0.01745;
dwy=((wy1+wy0)/2)*t*0.01745;
dwz=((wz1+wz0)/2)*t*0.01745;
//
 
dw=sqrt(dwx*dwx+dwy*dwy+dwz*dwz);
qc0=cos(dw/2);
qc1=(dwx/dw)*sin(dw/2);
qc2=(dwy/dw)*sin(dw/2);
qc3=(dwz/dw)*sin(dw/2);
 
 
q0=qc0*p0-qc1*p1-qc2*p2-qc3*p3;
q1=qc1*p0+qc0*p1+qc3*p2-qc2*p3;
q2=qc2*p0-qc3*p1+qc0*p2+qc1*p3;
q3=qc3*p0+qc2*p1-qc1*p2+qc0*p3;
 
 
//
 
roll=atan2(2*(q0*q1+q2*q3),1-2*(q1*q1+q2*q2))*57.25977;
pitch=-asin(2*(q0*q2-q3*q1))*57.25977;
yaw=-atan2(2*(q0*q3+q1*q2),1-2*(q2*q2+q3*q3))*57.25977;
//
 
if(yaw<0)
{
yaw=360+yaw;
}
// 在串口输出陀螺仪的转动角度数据：
Serial.print(roll);Serial.print(",");
Serial.print(pitch);Serial.print(",");
Serial.print(yaw);Serial.print("///");
Serial.print(ax1);Serial.print(",");
Serial.print(ay1);Serial.print(",");
Serial.println(az1);
}
 
// 在程序中为了减少陀螺仪的数据误差，我们还需要在程序中加上陀螺仪的校准算法：
void getoffs()
{
int16_t ax, ay, az;
int16_t gx, gy, gz;
long int axsum=0;
long int aysum=0;
long int azsum=0;
long int gxsum=0;
long int gysum=0;
long int gzsum=0;
int i;
for(i=1;i<=2000;i++)
{
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
axsum=ax+axsum;
aysum=ay+aysum;
azsum=az+azsum-16384;
gxsum=gx+gxsum;
gysum=gy+gysum;
gzsum=gz+gzsum;
}
axoffs=-axsum/2000;
ayoffs=-aysum/2000;
azoffs=-azsum/2000;
gxoffs=-gxsum/2000;
gyoffs=-gysum/2000;
gzoffs=-gzsum/2000;
}
 
         将程序写好以后，在Arduino 的IDE里编译并上传到Arduino的开发板上面，将陀螺仪连接到wifi机器人的小车上面（笔者用的是DS版），连通电源后开始测试。          如图所示，图中红圈圈住的地方就是笔者在小R科技的wifi机器人DS版小车上安装陀螺仪的地方，笔者用两枚M3的螺丝将陀螺仪固定住，并将陀螺仪上的引脚接口通过杜邦线接到电机驱动板的I2C接口上面（也就是通过杜邦线将两头的VCC、GND、SCL、SDA四个引脚分别对应的接起来）。
         打开串口监视器，看看陀螺仪有没有正常运行，获得的角速度和计算出来的角度数据是否正确：

         从图中可以看出，小车没有转动时，陀螺仪的角度数据基本上没有什么变化，说明陀螺仪的数据还是比较准确的。
         大家可以根据这个程序来加以改进，获取更精确的数据，来更精确的控制小车的转动和运行。