从MPU6050了解姿态解算
前言
最近正在学习和陀螺仪有关的知识，要将陀螺仪用到期末大作业中，代码还处在调试阶段，目前先总结一下学到的理论知识，学习资料来源
三维转动的四元数表述 - 中国知网 (cnki.net)
MPU6050姿态解算2-欧拉角&旋转矩阵 - 知乎 (zhihu.com)
姿态解算说明（Mini AHRS）
MPU6050.pdf (szlcsc.com)
STM32F1开发指南(精英版)-HAL库版本_V1.2.pdf
MPU6050 获取角度理论推导(一)
原理介绍
加速度计
基本原理
我们想象在一个空间中有一个处于失重状态的球体，当箱子突然向左运动的时候，小球会受到一个反向的加速度
此时根据牛三定律可以测出施加的加速度
上面描述的只是一轴的情形，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); //复位MPU6050delay_ms(100);MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X00); //唤醒MPU6050MPU_Set_Gyro_Fsr(3); //陀螺仪传感器,±2000dpsMPU_Set_Accel_Fsr(0); //加速度传感器,±2gMPU_Set_Rate(50); //设置采样率50HzMPU_Write_Byte(MPU_INT_EN_REG,0X00); //关闭所有中断MPU_Write_Byte(MPU_USER_CTRL_REG,0X00); //I2C主模式关闭MPU_Write_Byte(MPU_FIFO_EN_REG,0X00); //关闭FIFOMPU_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.81u8 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;
}总结
其实写到这里对陀螺仪的学习还远远不够，只是这样粗糙的计算出来的结果会产生零点漂移现象，因此还需要加入滤波，这个就等我后续再来总结吧
从MPU6050了解姿态解算相关推荐
- 【ESP32Arduino+MPU6050 dmp姿态解算】学习笔记 PlatformIO 复制即可使用
0.引入 esp32,国产之光,拥有先进的网络功能和强大的双核处理器:mpu6050,经典的六轴陀螺仪,体积小价格低廉但是功能强大. 二者的优点相结合,可实现对机体高效的姿态测算以及以此进行远程控制. ...
- MPU6050姿态解算1-DMP方式
MPU6050的姿态解算方法有多种,包括硬件方式的DMP解算,软件方式的欧拉角与旋转矩阵解算,软件方式的轴角法与四元数解算.本篇先介绍最易操作的DMP方式. MPU6050基本功能 3轴陀螺仪 陀螺仪 ...
- 姿态解算基础知识（一）
目前,对于姿态解算已经有些认识,至少可以看懂别人的开源代码.感觉我现在知道的东西像一堆点连起来的线段,还有些地方是散的,没有联通.暂且在这记录下. 版权声明 原创文章,转载请说明出处:sheng-bl ...
- Arduino 与 MPU6050 姿态解算+ PROCESSING
2019独角兽企业重金招聘Python工程师标准>>> 买的MPU6050自带姿态解算大大减轻了上层处理器所做的工作. 通过熟悉了一下processing之后做了一个小例子更是感觉这 ...
- MPU6050介绍及姿态解算
1.介绍:MPU6050 是 InvenSense 公司推出的全球首款整合性 6 轴运动处理组件,相较于多组件方案,免除了组合陀螺仪与加速器时之轴间差的问题,减少了安装空间. (1)绕X轴旋转角度为r ...
- android 陀螺仪滤波_Arduino MPU6050陀螺仪运用卡尔曼滤波姿态解算实验
Arduino MPU6050陀螺仪运用卡尔曼滤波姿态解算实验 版权声明:本文为博主原创文章,未经博主允许不得转载. 2019年3月20日 发布 实例效果 输出效果: 首先看看本例程XYZ轴的输出效果 ...
- stm32 MPU6050 姿态解算 Mahony互补滤波算法
文章目录 0.介绍 1,理论分析 1.1 MPU6050 1.2 Mahony算法原理 2,代码实现 1.1 MPU6050初始化及数据读取 1.2 Mahony算法c语言实现 1.3 将代码移植到你 ...
- 基于STM32的四旋翼无人机项目（二）：MPU6050姿态解算（含上位机3D姿态显示教学）
前言:本文为手把手教学飞控核心知识点之一的姿态解算--MPU6050 姿态解算(飞控专栏第2篇).项目中飞行器使用 MPU6050 传感器对飞行器的姿态进行解算(四元数方法),搭配设计的卡尔曼滤波器与 ...
- STM32F103之实验6 采用MPU6050及DMP解算移动机器人姿态实验
写在最前面,这篇博客写到的姿态解算是由DMP硬件完成的,介绍了底层驱动,重点例举了MPU6050的初始化函数,加速度计,陀螺仪原始数据的读取函数,设置传感器的量程范围,采样频率函数.下一篇博客,我会不 ...
最新文章
- 聊聊 Kafka 那点破事！
- Adam又要“退休”了？耶鲁大学团队提出AdaBelief，NeurIPS 2020收录，却引来网友质疑...
- 一文带你读懂边缘计算是什么
- 2020-11-18（如何打开.jar工具）
- 使用代码设置Item级的权限（权限总结1）
- java水果超市mysql_Java基础 | 项目实战之水果超市
- 成立烘焙公司、买茶饮 瑞幸咖啡谋变破局
- centos ipv6 网卡_Linux_03-Centos的基本网络配置
- PDF阅读器哪个好用？看完这篇文章就可以不用再问了
- 记一次jstack线程诊断
- jQuery实现平年闰年判断
- 傻瓜式制作纯净版win10启动盘
- 2022-2028全球与中国夹层升降机市场现状及未来发展趋势
- Word2016查找和替换通配符(完全版)
- cmd 组合命令和管道命令的使用
- 文本 - CL_GUI_TEXTEDIT
- 【计算机组成原理】真值与机器值小数原码与整数原码定义及举例
- 常见电源品牌大揭密（转贴自pceva，作者royalk）
- 第四个把梳子卖给和尚的人
- 刻录光盘的结构及原理