文章目录
- 0 前言
- 1 MPU6050概述
-
- 2 代码
- 3 姿态解算
-
- 3 校正
0 前言
作为惯性传感器中入门级别的器件,MPU6050凭借它出色的性价比成为一款非常常用的角度姿态传感器,在很多科创项目中被使用。我之前也接触过很多次这个器件,也收集了不少资料,趁此机会总结一下学习笔记。
1 MPU6050概述
1.1 基本概述
MPU6050包含3轴陀螺仪和3轴加速度计,其中陀螺仪的主要作用是测量物体绕芯片的三个坐标轴的角速度,其原理是高速旋转的转子指向的方向会保持不变,即所谓陀螺效应,详细介绍建议自行搜索“陀螺仪工作原理”;加速度计则是测量三个轴向的加速度,同样也是指芯片的三个坐标轴。其原理可以想象成一个立方体箱子里面有一个失重悬空的小球,当受到外界压力时,小球会朝着某个方向运动,箱子内壁就会受到对应大小的压力,从而可以计算出各个方向的加速度大小。
当整体受到向左的加速度时,小球会有一个相对箱子向右的加速度,从而右侧“箱壁”会受到对应加速度大小的力,这样就能计算出加速度的大小。
MPU6050是InvenSense公司推出的全球首款整合性6轴运动处理组件。目前InvenSense已被日本的TDK公司收购,在他官网(https://invensense.tdk.com/),可能是年代久远,MPU6050已经是该公司的边缘产品了,6轴芯片当中,6050和6500两款芯片被排在最后,还都是NOT RECOMMENDED的状态,而且资料支持也不是很完善,找遍了网站,也只找到了一个放数据手册的网页,开发相关的寄存器手册并未找到。因此,建议还是在网上去搜索资料吧,如下图所示,都是网上流传的经典资料。
MPU6050的核心就是它内部的寄存器。它内部有118个寄存器(编号从0到117)。其中需要注意,虽然寄存器手册上寄存器编号是从13开始,但实际上13之前的寄存器也是可以使用的(看后面的代码就知道了)。
这些寄存器有一些是用来设置参数的,可读可写;也有一些是存放一些数据供外部读取的,只可读。它是基于IIC进行通信,因此,在使用时,先找到传输器件地址,然后再传输寄存器地址,最后传输相应的数据或者指令,这也是IIC协议和器件寄存器交互的常用设定。
既然有这么多寄存器,那用起来岂不是很困难?并不是,虽然寄存器多,但实际使用时也不需要使用如此多的寄存器。而且即使要使用的话,也可以利用C语言中的宏定义,这样也不麻烦。
1.2 引脚和常用原理图
为了将这个芯片集成到我们需要的系统当中,就需要了解这款芯片的引脚和它常用的原理图,如下图所示,这个是市面上卖的MPU6050模块的原理图。
可以看到,这里引出了8个引脚,分别是电源引脚5V和GND,IIC通信引脚SCL和SDA,一般来说,大部分的应用只需要接这四个引脚即可。其中,XCL和XDA是额外的IIC通信引脚,主要用于连接外部的磁力传感器,并利用自带的运动处理器DMP硬件加速引擎,通过主IIC接口,向应用输出完整的9轴融合演算数据。
而AD0引脚是用来设置IIC通信中的从机地址,如果接地(不接),则从机地址为0x68, 如果接高电平,则从机地址为0x69。而INT引脚主要用于中断,如果要使用中断需要设置相关的寄存器。
2 代码
了解了MPU6050的基础知识,接下来就是写代码来使用了。如果还没确定使用的微处理器,我推荐先使用Arduino,因为它集成了很多的第三方库,这样在使用一些器件时不用自己再重复造轮子,只需要会调用即可。
在Arduino中也有MPU6050的库,如下图所示。
安装好库之后,接下来就找到给出的例子来学习它内部的代码了。
这里提供了6个例子,基本包含了大部分的使用。
当然,使用Arduino IDE也存在一个问题,那就是代码不能定位过去,查看库的源码不太方便,因此建议自己基于VS Code配一个Arduino的环境,或者直接下载插件Platform IO这个插件,具体的教程建议自行搜索。
具体可以看一下这个库的源码,主要是以下几个文件,各自的作用已标注清楚。
因此,如果不需要使用DMP时,只需要包含"MPU6050.h"
即可。
那如果是其他的微处理器呢?比如51或者STM32等。这个可以考虑在网上找一些现成的,也可以考虑自己根据这个库文件的源码自己写一个适配某个处理器的库。本质就是IIC通信和寄存器的读取。这里放一个基于51的网上流传甚广的代码。
#include <REG52.H>
#include <math.h>
#include <stdio.h>
#include <INTRINS.H>
typedef unsigned char uchar;
typedef unsigned short ushort;
typedef unsigned int uint;
sbit SCL = P1 ^ 5;
sbit SDA = P1 ^ 4;
#define SMPLRT_DIV 0x19
#define CONFIG 0x1A
#define GYRO_CONFIG 0x1B
#define ACCEL_CONFIG 0x1C
#define ACCEL_XOUT_H 0x3B
#define ACCEL_XOUT_L 0x3C
#define ACCEL_YOUT_H 0x3D
#define ACCEL_YOUT_L 0x3E
#define ACCEL_ZOUT_H 0x3F
#define ACCEL_ZOUT_L 0x40
#define TEMP_OUT_H 0x41
#define TEMP_OUT_L 0x42
#define GYRO_XOUT_H 0x43
#define GYRO_XOUT_L 0x44
#define GYRO_YOUT_H 0x45
#define GYRO_YOUT_L 0x46
#define GYRO_ZOUT_H 0x47
#define GYRO_ZOUT_L 0x48
#define PWR_MGMT_1 0x6B
#define WHO_AM_I 0x75
#define SlaveAddress 0xD0
uchar dis[6];
int dis_data;
void Delay5us();
void delay(unsigned int k);
void lcd_printf(uchar* s, int temp_data);
void InitMPU6050();
void I2C_Start();
void I2C_Stop();
void I2C_SendACK(bit ack);
bit I2C_RecvACK();
void I2C_SendByte(uchar dat);
uchar I2C_RecvByte();
void I2C_ReadPage();
void I2C_WritePage();
void display_ACCEL_x();
void display_ACCEL_y();
void display_ACCEL_z();
uchar Single_ReadI2C(uchar REG_Address);
void Single_WriteI2C(uchar REG_Address, uchar REG_data);
void lcd_printf(uchar* s, int temp_data)
{
if(temp_data < 0)
{
temp_data = -temp_data;
*s = '-';
}
else *s = ' ';
*++s = temp_data / 10000 + 0x30;
temp_data = temp_data % 10000;
*++s = temp_data / 1000 + 0x30;
temp_data = temp_data % 1000;
*++s = temp_data / 100 + 0x30;
temp_data = temp_data % 100;
*++s = temp_data / 10 + 0x30;
temp_data = temp_data % 10;
*++s = temp_data + 0x30;
}
void init_uart()
{
TMOD = 0x21;
TH1 = 0xfd;
TL1 = 0xfd;
SCON = 0x50;
PS = 1;
TR0 = 1;
TR1 = 1;
ET0 = 1;
ES = 1;
EA = 1;
}
void SeriPushSend(uchar send_data)
{
SBUF = send_data;
while(!TI);
TI = 0;
}
void delay(unsigned int k)
{
unsigned int i, j;
for(i = 0; i < k; i++)
{
for(j = 0; j < 121; j++);
}
}
void Delay5us()
{
_nop_();
_nop_();
_nop_();
_nop_();
_nop_();
_nop_();
_nop_();
_nop_();
_nop_();
_nop_();
_nop_();
_nop_();
_nop_();
_nop_();
_nop_();
_nop_();
_nop_();
_nop_();
_nop_();
_nop_();
_nop_();
_nop_();
_nop_();
_nop_();
}
void I2C_Start()
{
SDA = 1;
SCL = 1;
Delay5us();
SDA = 0;
Delay5us();
SCL = 0;
}
void I2C_Stop()
{
SDA = 0;
SCL = 1;
Delay5us();
SDA = 1;
Delay5us();
}
void I2C_SendACK(bit ack)
{
SDA = ack;
SCL = 1;
Delay5us();
SCL = 0;
Delay5us();
}
bit I2C_RecvACK()
{
SCL = 1;
Delay5us();
CY = SDA;
SCL = 0;
Delay5us();
return CY;
}
void I2C_SendByte(uchar dat)
{
uchar i;
for(i = 0; i < 8; i++)
{
dat <<= 1;
SDA = CY;
SCL = 1;
Delay5us();
SCL = 0;
Delay5us();
}
I2C_RecvACK();
}
uchar I2C_RecvByte()
{
uchar i;
uchar dat = 0;
SDA = 1;
for(i = 0; i < 8; i++)
{
dat <<= 1;
SCL = 1;
Delay5us();
dat |= SDA;
SCL = 0;
Delay5us();
}
return dat;
}
void Single_WriteI2C(uchar REG_Address, uchar REG_data)
{
I2C_Start();
I2C_SendByte(SlaveAddress);
I2C_SendByte(REG_Address);
I2C_SendByte(REG_data);
I2C_Stop();
}
uchar Single_ReadI2C(uchar REG_Address)
{
uchar REG_data;
I2C_Start();
I2C_SendByte(SlaveAddress);
I2C_SendByte(REG_Address);
I2C_Start();
I2C_SendByte(SlaveAddress + 1);
REG_data = I2C_RecvByte();
I2C_SendACK(1);
I2C_Stop();
return REG_data;
}
void InitMPU6050()
{
Single_WriteI2C(PWR_MGMT_1, 0x00);
Single_WriteI2C(SMPLRT_DIV, 0x07);
Single_WriteI2C(CONFIG, 0x06);
Single_WriteI2C(GYRO_CONFIG, 0x18);
Single_WriteI2C(ACCEL_CONFIG, 0x01);
}
int GetData(uchar REG_Address)
{
uchar H, L;
H = Single_ReadI2C(REG_Address);
L = Single_ReadI2C(REG_Address + 1);
return ((H << 8) + L);
}
void Display10BitData(int value)
{
uchar i;
lcd_printf(dis, value);
for(i = 0; i < 6; i++)
{
SeriPushSend(dis[i]);
}
}
void main()
{
delay(500);
init_uart();
InitMPU6050();
delay(150);
while(1)
{
Display10BitData(GetData(ACCEL_XOUT_H));
Display10BitData(GetData(ACCEL_YOUT_H));
Display10BitData(GetData(ACCEL_ZOUT_H));
Display10BitData(GetData(GYRO_XOUT_H));
Display10BitData(GetData(GYRO_YOUT_H));
Display10BitData(GetData(GYRO_ZOUT_H));
SeriPushSend(0x0d);
SeriPushSend(0x0a);
delay(2000);
}
}
3 姿态解算
前面提到MPU6050有很多的寄存器,但其实最核心的就是陀螺仪和加速度读取的数值,即姿态数据。但是,需要注意的是,前面也强调过,这个读取到的姿态数据是基于元器件坐标系的,而在实际应用中需要的更多是相对于大地坐标系的数据。这样得到的才是真正意义上的姿态。
首先要搞清楚芯片坐标系的样式,这直接关系到所测数据的正方向。如下图所示。
芯片水平放置,丝印朝正上方,此时,前方是+Y方向,右侧是+X方向,正上方是+Z方向,三轴符合右手坐标系。至于旋转的角速度的正方向,也是按照右手定则,大拇指指向轴的正方向,四指所指的方向为旋转的正方向。
关于MPU6050的姿态解算,主要有两种方式,分别是基于欧拉角和旋转矩阵推导 和直接调用芯片中的DMP模块,这里简要介绍一下。
3.1 欧拉角&旋转矩阵
关于欧拉角和旋转矩阵,如果不理解基础知识的建议翻阅我之前的一篇博客:
【学习笔记】空间坐标系旋转与四元数
了解基本的旋转矩阵的知识后,接下来就是利用旋转矩阵来计算芯片的姿态角了。这里建议参考这篇文章,写得比较详细。
有一点存在一点疑问,那就是文章中提到的是Z-Y-X欧拉角,但我认为应该叫Z-Y-X固定角更合理,这也符合教材上的矩阵相乘的顺序。
此外,这篇文章当中陀螺仪和加速度计解算是分开的,加速度主要负责静态的姿态角,陀螺仪反应动态的姿态角变化,然后设定不同的权值,加权求和得到。其实数据融合更加合理的应该是使用卡尔曼滤波。这个后续也会跟进补充。
3.2 DMP
DMP(Digital Motion Processor),即数字运动处理器,是MPU6050芯片内置的一个重要模块。它的作用就是根据测得的陀螺仪和加速度数据计算得到芯片的欧拉角yaw,pitch,roll的值。使用者可以不用关心它内部是怎么实现的。如果想了解的话可以去找找DMP相关的资料。
3 校正
IMU有一个重要的特性,那就是它的误差是会随时间累积的,最好是隔一段时间校正一次。而所谓校正,就一定要有一个参考对象。对于MPU6050来说,一般参考对象就是地面。
校正时,首先将MPU6050放置水平,保持静止。然后运行代码,读取陀螺仪和加速度数据。理论上来说,水平静止放置,陀螺仪数值应该为零,不为零则是误差,将误差写入到芯片内部“偏差寄存器”中,再次读取数值,计算误差,如此反复,直到误差值在允许范围之类,记下误差值,写入到偏差寄存器中。
至于加速度数值,要考虑重力的影响,因为是水平放置,所以重力只在Z轴有分量。即Z轴分量为g,而默认加速度的单位是2g,而加速度数值用16位寄存器表示,且第一位为符号位(有正负之分),因此实际数值为
(
2
15
−
1
)
2
=
16383.5
≈
16384
\frac{\left( 2^{15}-1 \right)}{2}=16383.5\approx 16384
2(215−1)=16383.5≈16384,但由于这个重力加速度与Z轴正方向相反,故值为负的,即-16384.
当然,有时候因为安装等原因,IMU校正时不能Z轴保持竖直,比如X轴保持竖直,则将实际数值代入到校正函数中,没啥差别。这里有一篇博客给出了一个测试代码,建议仔细阅读。
关于校正,实际上也可以看作是一个自动控制系统,因此也可以采用如PID或者是机器学习等控制方法来进行校正。
关于PID的例子,其实上面提到的Arduino的库中就有使用,即它内部的calibration()
函数,具体原理可以去找源码查看。
关于机器学习的例子,是我找到的一篇博客,用的是梯度下降的方式来取值,思路很清奇,但个人感觉本质上还是迭代,是否使用梯度下降差别不是很大。
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)