前言
最近正在学习和陀螺仪有关的知识，要将陀螺仪用到期末大作业中，代码还处在调试阶段，目前先总结一下学到的理论知识，学习资料来源
MPU6050姿态解算2-欧拉角&旋转矩阵 - 知乎 (zhihu.com)
姿态解算说明（Mini AHRS）
STM32F1开发指南(精英版)-HAL库版本_V1.2.pdf
原理介绍
加速度计
基本原理
我们想象在一个空间中有一个处于失重状态的球体，当箱子突然向左运动的时候，小球会受到一个反向的加速度
此时根据牛三定律可以测出施加的加速度
上面描述的只是一轴的情形，MPU6050的强大之处在于其能一次测出三个轴的惯性力，我们将框倾斜45°
此时用平行四边形定则就可以得到三个方向的惯性加速度，用空间坐标表示就是一个三维的矢量场，三个分量分别对应三个不同的加速度
硬件实现
通过翻阅芯片手册我们得知MPU6050芯片内部为每个轴都配置了一个参考质点，沿着轴向产生的加速度会使对应的质点产生一定的位移，电容式传感器能够感受到位移的变化，通过测量位移的差异进而测出对应的加速度值
MPU6050是一款通过I2C接口读取数据的数字传感器，通过向寄存器写入对应的代码来配置加速度和角速度的量程范围，不同的量程范围对应不同的灵敏度(LSB)，加速度的数据寄存器会传递一个2字节的数据，这个数据并不是最终的加速度值，我们还需要除以不同量程范围下的灵敏度值
灵敏度越大，量程范围越小，反之亦反，例如我们选用2g量程，对应的灵敏度为16384LSB/G，此时真实的加速度值即为ADC / LSB * g= ADC / 16384 * g，(ADC为传感器输出的二字节数据)
陀螺仪
基本原理
利用角动量守恒原理，旋转物体的旋转轴所指的方向在不受外力影响时是不会发生改变的特性制成的元器件，通常由一个高速回转体和一个定子组成，用于测量影响角动量变化的参数的仪器，称其为陀螺仪
硬件实现
MPU6050就是一款基于MEMS技术设计的测量角速度的传感器，它由三个独立振动的MEMS速率陀螺仪组成，在之前学习的大学物理中曾出现过科氏加速度的概念，其大致解释如下
科里奥利效应是对旋转体系中进行直线运动的质点而言，由于惯性相对旋转体系产生的直线运动会发生偏移的现象，和惯性加速度类似，这是来自于物体运动所具有的惯性
通过科里奥利效应产生的偏移振动，这种振动会被传感器内部的电容拾取器所检测，将处理过的信号通过放大解调和滤波，就能产生和角速度成比例的电压值，再通过ADC将电压值处理成二字节的带符号数据，通过和上述类似的灵敏度处理得到最终的角速度值
算法介绍
很显然，如果我们只是读取传感器的数据，我们并不能很好地得到所有需要的数据，如果能将加速度和角速度传换成欧拉角那么对我们实际应用将会有很大的帮助，关于欧拉角这里就不展开赘述了，基本接触过机器人的都有所了解:Yaw，Pitch，Row三个角度分别对应Z，Y，X轴的转角
这里引入两种解决方案，一种是基于InvenSense公司推出的DMP解算器的四元数算法，另一种就是基于旋转变换矩阵的算法，下面将分别展开介绍
DMP算法
翻看正点原子的官方教程也能看到，他们推荐的也是DMP的解算方法，通过翻看数据手册可以了解到，DMP全称为数字运动处理器，其是一款嵌入式处理器，集成到了MPU6050模块上，DMP从加速度计、陀螺仪和其他第三方传感器(如AMR磁力计)中获取数据(如果对磁传感器感兴趣可以看看之前写过的磁编码器那一篇)，DMP将这些数据融合并处理成四元数数据，教程中的计算公式如下
这样就得到了欧拉角，在这里我想细说一下四元数的知识
四元数
我们知道复数可以用来表征二维空间的旋转，简单来说，四元数的作用就是用来表征三维空间的转动，虽然矩阵也可以进行旋转变换，但四元数的几何意义更加明确，同时计算更为简单，更重要的是，四元数代数涵盖了矢量代数、复数、实数以及矢量的数学性质，也就是说，后者们的计算法则对于四元数都适用
以 i1，i2，3代替 i，j，k 表示四元数的基元，四元数的一般形式为 A＝ a0＋ a1i1＋ a2i2＋ a3i3（ a0，a1，a2，a3为实数）
四元数表示空间中物体运动规律的原理是可以将矢量分解为分别平行和垂直于矢量En的分量，他们的计算推导如下
假设矢量α经过变换后得到的矢量为β
其中，α垂直分量的系数是由矢量的三角表达式推导而出
即可以得出结论：形如cosθ + en*sinθ的四元数可以表示一个矢量的旋转，转轴为en，进一步整合可以得出：
矢量α与转轴en不垂直时，α绕en旋转θ角变成矢量β可以认为是由α垂直于en的分量绕en旋转了θ角度后，此时的矢量与α平行，我们只需要将该矢量和α自身的平行矢量进行合成，就可以得到最终的β，简化成四元数的形式如下
至此可以证明一个矢量在空间中的转动可以由矢量左右各乘上一个四元数来实现，下面补充一些四元数的数学性质
参考资料中的数学推导可以得到两个四元数的乘积满足乘法结合律
而且从此推导中我们可以得到四元数的乘积包含了矢量的叉乘和点乘
不难看出两个四元数的乘积还是一个四元数，因为其满足矢量的诸多性质，我们假设有这样一个四元数
R = r0 + r1*i + r2*j + r3*k
它是Q和P这两个四元数的乘积
用矩阵的形式表示如下
最后将该矩阵和旋转矩阵做一个等式，即可求得最终的欧拉角（旋转矩阵放到后面提）
最后推导出的公式就和正点原子的教程一样
旋转矩阵
其实这个知识点本来是想等学习完机器人学导论再一并总结的，不过既然这里出现了那就先提前总结一下吧
在空间坐标系中通过对运动刚体的运动轨迹的微分就可以得到其速度与加速度，进而得出运动方程，在旋转矩阵中我们分为世界坐标系（A）和物体坐标系（B），通过追踪物体坐标系的移动状态就可以得到质心原点的移动状态，因此我们如果想让一个点相对世界坐标旋转，我们只需要将该点乘以物体坐标系相对世界坐标系的旋转矩阵即可
简单来说就是，将两个坐标系均转为单位向量，做点乘（即求投影），得出的矩阵即为旋转矩阵
旋转矩阵具有上述特性：一个旋转矩阵叉乘其转置后等与单位阵，也就是RT=R-1（正交矩阵均包含此特性）
这就揭示了另一个特性，由于旋转矩阵的坐标系采用的是单位向量，其长度为1，并且其均为正交阵，因此两两主轴必须垂直，进而导致原本由九个参数组成的旋转矩阵最后可控的参数只有剩下的三个
它的三角表达式如下
简单来说，就是将IMU上的坐标系的读数转换到大地空间坐标系中，接下来将从加速度求法和角速度求法分别介绍旋转矩阵到欧拉角的转换
加速度求法
将读取的数据连续进行三次空间坐标变换，因为正常放置时会受到自然的重力加速度g
最后就可以总结出下面的公式
角速度求法
首先我们知道，Ω = θ/t，因此若要通过角速度求角度值，我们需要引入积分的方法
对应的旋转矩阵如下
具体内容可以去原网址学习，这里用的也是别人的公式，如果侵权了马上删除QAQ
硬件原理
系统框图如下
最开始我们需要初始化I2C接口
然后配置电源管理寄存器一来复位IMU再唤醒IMU
然后再分别配置陀螺仪配置寄存器（0X1B）和加速度传感器配置寄存器（0X1C）设置角速度和加速度的量程
然后在中断使能寄存器中关闭所有中断，在用户寄存器中关闭I2C主模式
在FIFO寄存器中关闭FIFO
再配置系统使用的时钟为X轴（内部时钟源不稳定，建议使用轴上的时钟）
最后检测ID号是否对的上，最后开启IMU
关于寄存器的功能这里就不细说了，具体可以翻阅芯片手册
//初始化MPU6050
//返回值:0,成功
// 其他,错误代码
u8 MPU_Init(void)
{
u8 res;
MPU_IIC_Init();//初始化IIC总线
MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X80); //复位MPU6050
delay_ms(100);
MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X00); //唤醒MPU6050
MPU_Set_Gyro_Fsr(3); //陀螺仪传感器,±2000dps
MPU_Set_Accel_Fsr(0); //加速度传感器,±2g
MPU_Set_Rate(50); //设置采样率50Hz
MPU_Write_Byte(MPU_INT_EN_REG,0X00); //关闭所有中断
MPU_Write_Byte(MPU_USER_CTRL_REG,0X00); //I2C主模式关闭
MPU_Write_Byte(MPU_FIFO_EN_REG,0X00); //关闭FIFO
MPU_Write_Byte(MPU_INTBP_CFG_REG,0X80); //INT引脚低电平有效
res=MPU_Read_Byte(MPU_DEVICE_ID_REG);
if(res == 0) {
MPU_Write_Byte(MPU_PWR_MGMT1_REG, 0X01); //设置CLKSEL,PLL X轴为参考
MPU_Write_Byte(MPU_PWR_MGMT2_REG, 0X00); //加速度与陀螺仪都工作
MPU_Set_Rate(50); //设置采样率为50Hz
} else return 1;
return 0;
}算法实现
DMP方法
这个可以在正点原子的教程中找到整合资源的方法吗，将这个四个文件导入其中
具体的代码可以查找网上的例程，这里我就贴个.h文件方便和前面提到的寄存器相映射
#ifndef __MPU6050_H
#define __MPU6050_H
#include "Mpuiic.h"
#include "sys.h"
//#define MPU_ACCEL_OFFS_REG 0X06 //accel_offs寄存器,可读取版本号,寄存器手册未提到
//#define MPU_PROD_ID_REG 0X0C //prod id寄存器,在寄存器手册未提到
#define MPU_SELF_TESTX_REG 0X0D //自检寄存器X
#define MPU_SELF_TESTY_REG 0X0E //自检寄存器Y
#define MPU_SELF_TESTZ_REG 0X0F //自检寄存器Z
#define MPU_SELF_TESTA_REG 0X10 //自检寄存器A
#define MPU_SAMPLE_RATE_REG 0X19 //采样频率分频器
#define MPU_CFG_REG 0X1A //配置寄存器
#define MPU_GYRO_CFG_REG 0X1B //陀螺仪配置寄存器
#define MPU_ACCEL_CFG_REG 0X1C //加速度计配置寄存器
#define MPU_MOTION_DET_REG 0X1F //运动检测阀值设置寄存器
#define MPU_FIFO_EN_REG 0X23 //FIFO使能寄存器
#define MPU_I2CMST_CTRL_REG 0X24 //IIC主机控制寄存器
#define MPU_I2CSLV0_ADDR_REG 0X25 //IIC从机0器件地址寄存器
#define MPU_I2CSLV0_REG 0X26 //IIC从机0数据地址寄存器
#define MPU_I2CSLV0_CTRL_REG 0X27 //IIC从机0控制寄存器
#define MPU_I2CSLV1_ADDR_REG 0X28 //IIC从机1器件地址寄存器
#define MPU_I2CSLV1_REG 0X29 //IIC从机1数据地址寄存器
#define MPU_I2CSLV1_CTRL_REG 0X2A //IIC从机1控制寄存器
#define MPU_I2CSLV2_ADDR_REG 0X2B //IIC从机2器件地址寄存器
#define MPU_I2CSLV2_REG 0X2C //IIC从机2数据地址寄存器
#define MPU_I2CSLV2_CTRL_REG 0X2D //IIC从机2控制寄存器
#define MPU_I2CSLV3_ADDR_REG 0X2E //IIC从机3器件地址寄存器
#define MPU_I2CSLV3_REG 0X2F //IIC从机3数据地址寄存器
#define MPU_I2CSLV3_CTRL_REG 0X30 //IIC从机3控制寄存器
#define MPU_I2CSLV4_ADDR_REG 0X31 //IIC从机4器件地址寄存器
#define MPU_I2CSLV4_REG 0X32 //IIC从机4数据地址寄存器
#define MPU_I2CSLV4_DO_REG 0X33 //IIC从机4写数据寄存器
#define MPU_I2CSLV4_CTRL_REG 0X34 //IIC从机4控制寄存器
#define MPU_I2CSLV4_DI_REG 0X35 //IIC从机4读数据寄存器
#define MPU_I2CMST_STA_REG 0X36 //IIC主机状态寄存器
#define MPU_INTBP_CFG_REG 0X37 //中断/旁路设置寄存器
#define MPU_INT_EN_REG 0X38 //中断使能寄存器
#define MPU_INT_STA_REG 0X3A //中断状态寄存器
#define MPU_ACCEL_XOUTH_REG 0X3B //加速度值,X轴高8位寄存器
#define MPU_ACCEL_XOUTL_REG 0X3C //加速度值,X轴低8位寄存器
#define MPU_ACCEL_YOUTH_REG 0X3D //加速度值,Y轴高8位寄存器
#define MPU_ACCEL_YOUTL_REG 0X3E //加速度值,Y轴低8位寄存器
#define MPU_ACCEL_ZOUTH_REG 0X3F //加速度值,Z轴高8位寄存器
#define MPU_ACCEL_ZOUTL_REG 0X40 //加速度值,Z轴低8位寄存器
#define MPU_TEMP_OUTH_REG 0X41 //温度值高八位寄存器
#define MPU_TEMP_OUTL_REG 0X42 //温度值低8位寄存器
#define MPU_GYRO_XOUTH_REG 0X43 //陀螺仪值,X轴高8位寄存器
#define MPU_GYRO_XOUTL_REG 0X44 //陀螺仪值,X轴低8位寄存器
#define MPU_GYRO_YOUTH_REG 0X45 //陀螺仪值,Y轴高8位寄存器
#define MPU_GYRO_YOUTL_REG 0X46 //陀螺仪值,Y轴低8位寄存器
#define MPU_GYRO_ZOUTH_REG 0X47 //陀螺仪值,Z轴高8位寄存器
#define MPU_GYRO_ZOUTL_REG 0X48 //陀螺仪值,Z轴低8位寄存器
#define MPU_I2CSLV0_DO_REG 0X63 //IIC从机0数据寄存器
#define MPU_I2CSLV1_DO_REG 0X64 //IIC从机1数据寄存器
#define MPU_I2CSLV2_DO_REG 0X65 //IIC从机2数据寄存器
#define MPU_I2CSLV3_DO_REG 0X66 //IIC从机3数据寄存器
#define MPU_I2CMST_DELAY_REG 0X67 //IIC主机延时管理寄存器
#define MPU_SIGPATH_RST_REG 0X68 //信号通道复位寄存器
#define MPU_MDETECT_CTRL_REG 0X69 //运动检测控制寄存器
#define MPU_USER_CTRL_REG 0X6A //用户控制寄存器
#define MPU_PWR_MGMT1_REG 0X6B //电源管理寄存器1
#define MPU_PWR_MGMT2_REG 0X6C //电源管理寄存器2
#define MPU_FIFO_CNTH_REG 0X72 //FIFO计数寄存器高八位
#define MPU_FIFO_CNTL_REG 0X73 //FIFO计数寄存器低八位
#define MPU_FIFO_RW_REG 0X74 //FIFO读写寄存器
#define MPU_DEVICE_ID_REG 0X75 //器件ID寄存器
//如果AD0脚(9脚)接地,IIC地址为0X68(不包含最低位).
//如果接V3.3,则IIC地址为0X69(不包含最低位).
#define MPU_ADDR 0X68
因为模块AD0默认接GND,所以转为读写地址后,为0XD1和0XD0(如果接VCC,则为0XD3和0XD2)
//#define MPU_READ 0XD1
//#define MPU_WRITE 0XD0
#define ANGLE_SAMPLE_RATE 60
#define PI 3.1415926
#define G 9.81
u8 MPU_Init(void); //初始化MPU6050
u8 MPU_Write_Len(u8 addr,u8 reg,u8 len,u8 *buf);//IIC连续写
u8 MPU_Read_Len(u8 addr,u8 reg,u8 len,u8 *buf); //IIC连续读
u8 MPU_Write_Byte(u8 reg,u8 data); //IIC写一个字节
u8 MPU_Read_Byte(u8 reg); //IIC读一个字节
u8 MPU_Set_Gyro_Fsr(u8 fsr);
u8 MPU_Set_Accel_Fsr(u8 fsr);
u8 MPU_Set_LPF(u16 lpf);
u8 MPU_Set_Rate(u16 rate);
u8 MPU_Set_Fifo(u8 sens);
short MPU_Get_Temperature(void);
u8 MPU_Get_Gyroscope(short *gx,short *gy,short *gz);
u8 MPU_Get_Accelerometer(short *ax,short *ay,short *az);其实我是由尝试过DMP的方法的，不过我大作业中采用的芯片是C8T6....当我编译时提示我内存不足...QAQ
于是该方法只能舍弃了，本来可以省很多事的说...
公式求解
这里我用加速度求了pitch和row，用角速度求了yaw
double pitch,yaw,row;
u8 Compute_row_pitch_by_acc()
{
short aacx,aacy,aacz; //加速度传感器原始数据
double accx,accy,accz;
u8 res;
res = MPU_Get_Accelerometer(&aacx,&aacy,&aacz);
if (res == 0) { //得到加速度传感器数据
accx = (double) aacx / 16384;
accy = (double) aacy / 16384;
accz = (double) aacz / 16384;
row = atan2(accy,accz)*180/PI; //用加速度求row轴角度
pitch = -atan2(accx, sqrt(accy*accy + accz*accz))*180/PI; //用加速度求pitch轴角度
}else return 1;
return 0;
}
u8 Compute_yaw_by_gyro()
{
Compute_row_pitch_by_acc();
short gyrox,gyroy,gyroz; //加速度传感器原始数据
double gyrozz;
gyrozz = (double ) gyroz / 131; //除以对应模式下的分辨率
u8 res;
res = MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz);
if(res == 0){
yaw = yaw + (sin(row)/cos(pitch) + cos(row)/cos(pitch))
*gyrozz*ANGLE_SAMPLE_RATE*180*PI; //通过row和pitch求yaw轴角度
} else return 1;
return 0;
}总结
其实写到这里对陀螺仪的学习还远远不够，只是这样粗糙的计算出来的结果会产生零点漂移现象，因此还需要加入滤波，这个就等我后续再来总结吧
1786
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
