陀螺仪(MPU6050)

MPU6050陀螺仪传感器具有许多强大的功能,采用单芯片封装。该芯片由一个MEMS加速度计、一个MEMS陀螺仪温度传感器组成。该模块在将模拟量转换为数字量时非常准确,因为每个通道都有一个16位的模数转换器硬件。

该模块能够同时捕获x、y和z通道。它有一个I2C接口与主控制器进行通信。这款MPU6050模块是一款兼备加速度计和陀螺仪的小型芯片。对于无人机、机器人、运动传感器等许多应用来说,这是一个非常有用的设备。它也被称为陀螺仪或三轴加速度计。

原理

MPU6050的数据写入和读出均通过其芯片内部的寄存器实现,寄存器的地址都是1字节,也就是8位寻址空间,其寄存器的详细列表说明书请点击下载:https://www.olimex.com/Products/Modules/Sensors/MOD-MPU6050/resources/RM-MPU-60xxA_rev_4.pdf

MPU6050芯片的座标系是这样定义的:令芯片表面朝向自己,将其表面文字转至正确角度,此时,以芯片内部中心为原点,水平向右的为X轴,竖直向上的为Y轴,指向自己的为Z轴。见下图:

加速度计

加速度计的三轴分量ACC_X、ACC_Y和ACC_Z,均为16位有符号整数,分别表示器件在三个轴向上的加速度,取负值时加速度沿座标轴负向,取正值时沿正向。

三个加速度分量均以重力加速度g的倍数为单位,能够表示的加速度范围,即倍率可以统一设定,有4个可选倍率:2g、4g、8g、16g。修改倍率需要通过MPU6050模块的1C位地址的寄存器实现,具体通过第4位和第5位(AFS_SEL)进行操作,如下图。

以ACC_X为例,若倍率设定为2g(默认),则意味着ACC_X取最小值-32768时,当前加速度为沿X轴正方向2倍的重力加速度;若设定为4g,取-32768时表示沿X轴正方向4倍的重力加速度,以此类推。显然,倍率越低精度越好,倍率越高表示的范围越大。

我们用f表示倍率,f=0为2g,f=3为16g,设定加速度倍率的代码如下:

Wire.beginTransmission(0x68); //开启MPU-6050的传输
Wire.write(0x1C); //加速度倍率寄存器的地址
Wire.requestFrom(0x1C, 1, true); //先读出原配置
unsigned char acc_conf = Wire.read();
acc_conf = ((acc_conf & 0xE7) | (1 << 3));//如过是2g,则0<<3;如果是4g,则1<<3;如果是8g,则2<<3;如果是16g,则3<<3。
Wire.write(acc_conf);
Wire.endTransmission(true); //结束传输,true表示释放总线

以ACC_X为例,若当前设定的加速度倍率为4g,那么将ACC_X读数换算为加速度的公式为:a = 4g × ACC_X ÷ 32768(g可取当地重力加速度)。

角速度计

绕X、Y和Z三个座标轴旋转的角速度分量GYR_X、GYR_Y和GYR_Z均为16位有符号整数。从原点向旋转轴方向看去,取正值时为顺时针旋转,取负值时为逆时针旋转。

三个角速度分量均以“度/秒”为单位,能够表示的角速度范围,即倍率可统一设定,有4个可选倍率:250度/秒、500度/秒、1000度/秒、2000度/秒。以GYR_X为例,若倍率设定为250度/秒,则意味着GYR取正最大值32768时,当前角速度为顺时针250度/秒;若设定为500度/秒,取32768时表示当前角速度为顺时针500度/秒。显然,倍率越低精度越好,倍率越高表示的范围越大。

我们用f表示倍率,f=0为250度/秒,f=3为2000度/秒,除角速度倍率寄存器的地址为0x1B之外,设定加速度倍率的代码与加速度计设置代码一致。

以GYR_X为例,若当前设定的角速度倍率为1000度/秒,那么将GRY_X读数换算为角速度(顺时针)的公式为:g_x=1000 × GRY_X ÷ 32768

求实际的偏转角


如图上所示,这个飞机模型翻转的角度(Roll、Yaw、Pitch)也是我们感兴趣的。
由于MPU6050可以获取三个轴向上的加速度,而地球重力则是长期存在且永远竖直向下,因此我们可以根据重力加速度相对于芯片的指向为参考算得当前姿态。

为方便起见,我们让芯片正面朝下固定在上图飞机上,且座标系与飞机的坐标系完全重合,以三个轴向上的加速度为分量,可构成加速度向量a(x,y,z)。假设当前芯片处于匀速直线运动状态,那么a应垂直于地面上向,即指向Z轴负方向,模长为:
与重力加速度大小相等,方向相反,见3.1节)。若芯片(座标系)发生旋转,由于加速度向量a仍然竖直向上,所以Z轴负方向将不再与a重合。见下图。

为了方便表示,上图坐标系的Z轴正方向(机腹以及芯片正面)向下,X轴正方向(飞机前进方向)向右。此时芯片的Roll角\phi(黄色)为加速度向量与其在XZ平面上投影(x,0,z)的夹角,Pitch角(绿色)与其在YZ平面上投影(0,y,z)的夹角。求两个向量的夹角可用点乘公式:

简单推到可得:

以及

注意,因为arccos函数只能返回正值角度,因此还需要根据不同情况来取角度的正负值。当y值为正时,Roll角要取负值,当x轴为负时,Pitch角要取负值。

因为没有参考量,所以无法求出当前的Yaw角的绝对角度,只能得到Yaw的变化量,也就是角速度GYR_Z。当然,我们可以通过对GYR_Z积分的方法来推算当前Yaw角(以初始值为准),但由于测量精度的问题,推算值会发生漂移,一段时间后就完全失去意义了。然而在大多数应用中,比如无人机,只需要获得GRY_Z就可以了。

如果必须要获得绝对的Yaw角,那么应当选用MPU9250这款九轴运动跟踪芯片,它可以提供额外的三轴罗盘数据,这样我们就可以根据地球磁场方向来计算Yaw角了,具体方法此处不再赘述。

数据如何得来的?


加速度计采用压电效应的工作原理。就像上面的图片一样,在一个立方体的盒子里面有一个小球,盒子的四壁是用压电晶体材料。

当盒子加速度发声变化时,由于惯性的作用,球就会向与加速度方向相反的地方倾斜,当小球碰到墙壁就会产生压电电流,通过这一电流就能确定该方向上加速度大小(实际测得的是压力的大小)。

实际上,Z轴方向上一直受到小球的挤压(即重力的作用),因而通过Z轴测得的压力大小,可以求解倾斜角度。

盒子中有上下、左右、前后三对相对的墙壁,每一对墙对应于三维空间中的一个轴:X轴、Y轴、Z轴。根据压电壁产生的电流,我们就可以确定倾角的方向和大小。

为了保证数据的物理意义,MPU6050的加速度计是以假想球在三轴上座标值的相反数作为三个轴的加速度值。当假想球的位置偏向一个轴的正向时,该轴的加速度读数为负值,当假想球的位置偏向一个轴的负向时,该轴的加速度读数为正值。

根据以上分析,当我们把MPU6050芯片水平放于地方,芯片表面朝向天空,此时由于受到地球重力的作用, 假想球的位置偏向Z轴的负向,因此Z轴的加速度读数应为正,且在理想情况下应为g。注意,此加速度的物理意义并不是重力加速度,而是自身运动的加速度,可以这样理解:正因为其自身运动的加速度与重力加速度大小相等方向相反,芯片才能保持静止。

数据处理

MPU6050芯片提供的数据夹杂有较严重的噪音,在芯片处理静止状态时数据摆动都可能超过2%。除了噪音,各项数据还会有偏移的现象,也就是说数据并不是围绕静止工作点摆动,因此要先对数据偏移进行校准 ,再通过滤波算法消除噪音。

校准

校准是比较简单的工作,我们只需要找出摆动的数据围绕的中心点即可。我们以GRY_X为例,在芯片处理静止状态时,这个读数理论上讲应当为0,但它往往会存在偏移量,比如我们以10ms的间隔读取了10个值如下:

-158.4, -172.9, -134.2, -155.1, -131.2, -146.8, -173.1, -188.6, -142.7, -179.5

这10个值的均值,也就是这个读数的偏移量为-158.25。在获取偏移量后,每次的读数都减去偏移量就可以得到校准后的读数了。当然这个偏移量只是估计值,比较准确的偏移量要对大量的数据进行统计才能获知,数据量越大越准,但统计的时间也就越慢。一般校准可以在每次启动系统时进行,那么你应当在准确度和启动时间之间做一个权衡。

三个角速度读数GYR_X、GYR_Y和GYR_Z均可通过统计求平均的方法来获得,但三个加速度分量就不能这样简单的完成了,因为芯片静止时的加速度并不为0。

加速度值的偏移来自两个方面,一是由于芯片的测量精度,导至它测得的加速度向量并不垂直于大地;二是芯片在整个系统(如无人机)上安装的精度是有限的,系统与芯片的座标系很难达到完美重合。前者我们称为读数偏移,后者我们称为角度偏移。因为读数和角度之间是非线性关系,所以要想以高精度进行校准必须先单独校准读数偏移,再把芯片固定在系统中后校准角度偏移。然而,由于校准角度偏移需要专业设备,且对于一般应用来说,两步校准带来的精度提升并不大,因此通常只进行读数校准即可。下面介绍读数校准的方法。我们还3.2节的飞机为例,分以下几个步骤:

  1. 首先要确定飞机的坐标系,对于多轴飞行器来说这非常重要。如果坐标系原点的位置或坐标轴的方向存在较大偏差,将会给后面的飞控造成不良影响。
  2. 在确定了飞机的坐标系后,为了尽量避免读数偏移带来的影响,首先将MPU6050牢牢地固定在飞机上,并使二者座标系尽可能的重合。当然把Z轴反过来装也是可以的,就是需要重新推算一套角度换算公式。
  3. 将飞机置于水平、坚固的平面上,并充分预热。对于多轴无人机而言,空中悬停时的XY平面应当平行于校准时的XY平面。此时,我们认为芯片的加速度方向应当与Z轴负方向重合,且加速度向量的模长为g,因此ACC_X和ACC_Y的理论值应为0,ACC_Z的理论值应为-16384(假设我们设定2g的倍率,1g的加速度的读数应为最大值-32768的一半)。
  4. 由于ACC_X和ACC_Y的理论值应为0,与角速度量的校准类似,这两个读数偏移量可用统计均值的方式校准。ACC_Z则需要多一步处理,即在统计偏移量的过程中,每次读数都要加上16384,再进行统计均值校准。

卡尔曼滤波

暂无

引脚说明

VCC 3.3-5V(内部有稳压芯片)
GND 地线
SCL MPU6050作为从机时IIC时钟线
SDA MPU6050作为从机时IIC数据线
XCL MPU6050作为主机时IIC时钟线
XDA MPU6050作为主机时IIC数据线
AD0 地址管脚,决定IIC地址的最低一位(LSB)
INT 中断引脚

引脚连接

MPU模块 Stduino UNO/Nano
VCC 5V
GND GND
SCL D9
SDA D10
XCL
XDA
AD0 GND
INT D2

程序

将数据写入MPU-6050

在每次向器件写入数据前要先打开Wire的传输模式,并指定器件的总线地址,MPU6050的总线地址是0x68(AD0引脚为高电平时地址为0x69)。然后写入一个字节的寄存器起始地址,再写入任意长度的数据。这些数据将被连续地写入到指定的起始地址中,超过当前寄存器长度的将写入到后面地址的寄存器中。写入完成后关闭Wire的传输模式。下面的示例代码是向MPU6050的0x6B寄存器写入一个字节0。

Wire.beginTransmission(0x68); //开启MPU6050的传输
Wire.write(0x6B); //指定寄存器地址
Wire.write(0); //写入一个字节的数据
Wire.endTransmission(true); //结束传输,true表示释放总线

从MPU-6050读出数据

读出和写入一样,要先打开Wire的传输模式,然后写一个字节的寄存器起始地址。接下来将指定地址的数据读到Wire库的缓存中,并关闭传输模式。最后从缓存中读取数据。下面的示例代码是从MPU6050的0x3B寄存器开始读取2个字节的数据:

Wire.beginTransmission(0x68); //开启MPU6050的传输
Wire.write(0x3B); //指定寄存器地址
Wire.requestFrom(0x68, 2, true); //将输据读出到缓存
Wire.endTransmission(true); //关闭传输模式
int val = Wire.read() << 8 | Wire.read(); //两个字节组成一个16位整数

具体实现

通常应当在setup函数中对Wire库进行初始化:

Wire.begin(); 

在对MPU6050进行各项操作前,必须启动该器件,向它的0x6B写入一个字节0即可启动。通常也是在setup函数完成,代码见1.1节。

MPU6050数据格式

我们感兴趣的数据位于0x3B到0x48这14个字节的寄存器中。这些数据会被动态更新,更新频率最高可达1000HZ。下面列出相关寄存器的地址,数据的名称。注意,每个数据都是2个字节。

  • 0x3B,加速度计的X轴分量ACC_X
  • 0x3D,加速度计的Y轴分量ACC_Y
  • 0x3F,加速度计的Z轴分量ACC_Z
  • 0x41,当前温度TEMP
  • 0x43,绕X轴旋转的角速度GYR_X
  • 0x45,绕Y轴旋转的角速度GYR_Y
  • 0x47,绕Z轴旋转的角速度GYR_Z
    注意需先按照下列步骤安装对应库,才可正常运行下列程序

程序示例

#include <Arduino.h>
#include <Wire.h>
#include <MPU6050.h>

MPU6050 mpu6050;

void setup(void)
{
    Serial.begin(115200);
    Wire.begin();
    mpu6050.begin();

    if(!mpu6050.available())
    {
        Serial.print("Unable to connect to MPU!");
        while(1);
    }

    // set the scaling registers
    mpu6050.setAccConfig(MPU6050_ACC_FULL_SCALE_2_G);
    mpu6050.setGyroConfig(MPU6050_GYRO_FULL_SCALE_500_DPS);

    // normalise attitude readings between -180 and 180 degrees
    mpu6050.setRange(RANGE_RELATIVE);

    // run the self calibration function, must not move the MPU during this function call!
    mpu6050.calibrate();
}


void loop(void)
{
    mpu6050.update();
    attitude_t attitude = mpu6050.getAttitude(UNITS_DEGREES);

    Serial.print("Roll: ");
    Serial.print(attitude.roll);
    Serial.print("\tPitch: ");
    Serial.print(attitude.pitch);
    Serial.print("\tYaw: ");
    Serial.println(attitude.yaw);
    Serial.flush();

    delay(500);
}
文档更新时间: 2021-06-19 18:48   作者:admin