毕业设计之-教程：MPU6050姿态解算
⽂章⽬录
1 简介
Hi，⼤家好，这⾥是丹成学长，今天向⼤家介绍⼀个学长做的单⽚机项⽬
教程：MPU6050姿态解算
⼤家可⽤于 课程设计 或 毕业设计
2 MPU6050
MPU6050是⼀种⾮常流⾏的空间运动传感器芯⽚，可以获取器件当前的三个加速度分量和三个旋转⾓速度。由于其体积⼩巧，功能强⼤，精度较⾼，不仅被⼴泛应⽤于⼯业，同时也是航模爱好者的神器，被安装在各类飞⾏器上驰骋蓝天。
随着Arduino开发板的普及，许多朋友希望能够⾃⼰制作基于MPU6050的控制系统，但由于缺乏专业知识⽽难以上⼿。此外，MPU6050的数据是有较⼤噪⾳的，若不进⾏滤波会对整个控制系统的精准确带来严重影响。
MPU6050芯⽚内⾃带了⼀个数据处理⼦模块DMP，已经内置了滤波算法，在许多应⽤中使⽤DMP输出的数据已经能够很好的满⾜要求。关于如何获取DMP的输出数据，我将在以后的⽂章中介绍。本⽂将直接⾯对原始测量数据，从连线、芯⽚通信开始⼀步⼀步教你如何利⽤Arduino获取MPU6050的数据并进⾏卡尔曼滤波，最终获得稳定的系统运动状态。
3 ⼯作原理
加速度计采⽤压电效应的⼯作原理，就像上⾯的图⽚⼀样，在⼀个⽴⽅体的盒⼦⾥⾯有⼀个⼩球，盒⼦的四壁是⽤压电晶体材料，当盒⼦倾斜时，由于重⼒的作⽤，球就会向倾斜的⽅向移动，当⼩球碰到墙壁就会产⽣压电电流。盒⼦中有上下、左右、前后三对相对的墙壁，每⼀对墙对应于三维空间中的⼀个轴：X轴、Y轴、Z轴。根据压电壁产⽣的电流，我们就可以确定倾⾓的⽅向和⼤⼩。
4 单⽚机与MPU6050通信
这⾥以arduino单⽚机为例
为避免纠缠于电路细节，我们直接使⽤集成的MPU6050模块。MPU6050的数据接⼝⽤的是I2C总线协议，因此我们需要Wire程序库的帮助来实现Arduino与MPU6050之间的通信。请先确认你的Arduino编程环境中已安装Wire库。
Wire库的官⽅⽂档中指出：在UNO板⼦上，SDA接⼝对应的是A4引脚，SCL对应的是A5引脚。MPU6050需要5V的电源，可由UNO板直接供电。按照下图连线。
4.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
4.2 倾⾓计算⽅法
Roll-pitch-yaw模型与姿态计算
表⽰飞⾏器当前飞⾏姿态的⼀个通⽤模型就是建⽴下图所⽰坐标系，并⽤Roll表⽰绕X轴的旋转，Pitch表⽰绕Y轴的旋转，Yaw表⽰绕Z轴的旋转。
Yaw⾓的问题
因为没有参考量，所以⽆法求出当前的Yaw⾓的绝对⾓度，只能得到Yaw的变化量，也就是⾓速度GYR_Z。当然，我们可以通过对GYR_Z 积分的⽅法来推算当前Yaw⾓（以初始值为准），但由于测量精度的问题，推算值会发⽣漂移，⼀段时间后就完全失去意义了。然⽽在⼤多数应⽤中，⽐如⽆⼈机，只需要获得GRY_Z就可以了。
如果必须要获得绝对的Yaw⾓，那么应当选⽤MPU9250这款九轴运动跟踪芯⽚，它可以提供额外的三
轴罗盘数据，这样我们就可以根据地球磁场⽅向来计算Yaw⾓了，具体⽅法此处不再赘述。
5 实现代码汽车漂移教程
// 本代码版权归Devymex所有，以GNU GENERAL PUBLIC LICENSE V3.0发布
// /licenses/html
// 相关⽂档参见作者于知乎专栏发表的原创⽂章：
// zhuanlan.zhihu/devymex/20082486
//连线⽅法
//MPU-UNO
//VCC-VCC
//VCC-VCC
//GND-GND
/
/SCL-A5
//SDA-A4
//INT-2 (Optional)
#include<Kalman.h>
#include<Wire.h>
#include<Math.h>
float fRad2Deg =57.295779513f;//将弧度转为⾓度的乘数
const int MPU =0x68;//MPU-6050的I2C地址
const int nValCnt =7;//⼀次读取寄存器的数量
const int nCalibTimes =1000;//校准时读数的次数
int calibData[nValCnt];//校准数据
unsigned long nLastTime =0;//上⼀次读数的时间
float fLastRoll =0.0f;//上⼀次滤波得到的Roll⾓
float fLastPitch =0.0f;//上⼀次滤波得到的Pitch⾓
Kalman kalmanRoll;//Roll⾓滤波器
Kalman kalmanPitch;//Pitch⾓滤波器
void setup(){
Serial.begin(9600);//初始化串⼝，指定波特率
Wire.begin();//初始化Wire库
WriteMPUReg(0x6B,0);//启动MPU6050设备
Calibration();//执⾏校准
nLastTime =micros();//记录当前时间
}
void loop(){
int readouts[nValCnt];
ReadAccGyr(readouts);//读出测量值
float realVals[7];
Rectify(readouts, realVals);//根据校准的偏移量进⾏纠正
//计算加速度向量的模长，均以g为单位
float fNorm =sqrt(realVals[0]* realVals[0]+ realVals[1]* realVals[1]+ realVals[2]* realVals[2]); float fRoll =GetRoll(realVals, fNorm);//计算Roll⾓
if(realVals[1]>0){
fRoll =-fRoll;
}
float fPitch =GetPitch(realVals, fNorm);//计算Pitch⾓
if(realVals[0]<0){
fPitch =-fPitch;
}
//计算两次测量的时间间隔dt，以秒为单位
unsigned long nCurTime =micros();
float dt =(double)(nCurTime - nLastTime)/1000000.0;
//对Roll⾓和Pitch⾓进⾏卡尔曼滤波
float fNewRoll = Angle(fRoll, realVals[4], dt);
float fNewPitch = Angle(fPitch, realVals[5], dt);
//跟据滤波值计算⾓度速
float fRollRate =(fNewRoll - fLastRoll)/ dt;
float fPitchRate =(fNewPitch - fLastPitch)/ dt;
//更新Roll⾓和Pitch⾓
fLastRoll = fNewRoll;
fLastPitch = fNewPitch;
//更新本次测的时间
nLastTime = nCurTime;
//向串⼝打印输出Roll⾓和Pitch⾓，运⾏时在Arduino的串⼝监视器中查看
Serial.print("Roll:");
Serial.print(fNewRoll); Serial.print('(');
Serial.print(fRollRate); Serial.print("),\tPitch:");
Serial.print(fNewPitch); Serial.print('(');
Serial.print(fPitchRate); Serial.print(")\n");
delay(10);
}
//向MPU6050写⼊⼀个字节的数据
//指定寄存器地址与⼀个字节的值
void WriteMPUReg(int nReg,unsigned char nVal){
Wire.beginTransmission(MPU);
Wire.write(nReg);
Wire.write(nVal);
}
//从MPU6050读出⼀个字节的数据
//指定寄存器地址，返回读出的值
unsigned char ReadMPUReg(int nReg){
Wire.beginTransmission(MPU);
Wire.write(nReg);
ad();
}
//从MPU6050读出加速度计三个分量、温度和三个⾓速度计
/
/保存在指定的数组中
void ReadAccGyr(int*pVals){
Wire.beginTransmission(MPU);
Wire.write(0x3B);
for(long i =0; i < nValCnt;++i){
pVals[i]= ad()<<8| ad();
}
}
//对⼤量读数进⾏统计，校准平均偏移量
void Calibration()
{
float valSums[7]={0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0};
//先求和
for(int i =0; i < nCalibTimes;++i){
int mpuVals[nValCnt];
ReadAccGyr(mpuVals);
for(int j =0; j < nValCnt;++j){
valSums[j]+= mpuVals[j];
}
}
/
/再求平均
for(int i =0; i < nValCnt;++i){
calibData[i]=int(valSums[i]/ nCalibTimes);
}
calibData[2]+=16384;//设芯⽚Z轴竖直向下，设定静态⼯作点。
}
//算得Roll⾓。算法见⽂档。
float GetRoll(float*pRealVals,float fNorm){
float fNormXZ =sqrt(pRealVals[0]* pRealVals[0]+ pRealVals[2]* pRealVals[2]); float fCos = fNormXZ / fNorm;
return acos(fCos)* fRad2Deg;
}