Arduino连接两个MPU6050测算姿态角
单个MPU6050 连接
连线
采用卡尔曼滤波器,对加速度计进行修正。
//连线方法
//MPU-UNO
//VCC-VCC
//GND-GND
//SCL-A5
//SDA-A4
//INT-2 (Optional)
#include <Adafruit_MPU6050.h>
#include <Adafruit_sensor.h>
#include <Wire.h>
#include<Kalman.h>
Kalman.h 库 可以通过IDE自带的库搜索下载。或者git:https://github.com/TKJElectronics/KalmanFilter
Adafruit_MPU6050 IDE自带,如果没自带 用自带的库搜索下载。
完整代码
#include <Adafruit_MPU6050.h>
#include <Adafruit_sensor.h>
#include <Wire.h>
#include<Kalman.h>
const int RESET_PIN = 2; // 定义复位引脚为 D2
Adafruit_MPU6050 mpu;
float fRad2Deg = 57.295779513f; //将弧度转为角度的乘数
float AccX, AccY, AccZ;
float GyroX, GyroY, GyroZ;
float accAngleX, accAngleY, gyroAngleX, gyroAngleY, gyroAngleZ;
float AccErrorX, AccErrorY, AccErrorZ,GyroErrorX, GyroErrorY, GyroErrorZ = 0;
float AccSumX, AccSumY, AccSumZ,GyroSumX, GyroSumY, GyroSumZ = 0;
int times = 0;
float fLastRoll = 0.0f; //上一次滤波得到的Roll角
float fLastPitch = 0.0f; //上一次滤波得到的Pitch角
unsigned long nLastTime = 0; //上一次读数的时间
Kalman kalmanRoll; //Roll角滤波器
Kalman kalmanPitch; //Pitch角滤波器
Kalman kalmanYaw; //Yaw角滤波器
void setup() {
// put your setup code here, to run once:
Serial.begin(115200);
// initialize mpu
if (!mpu.begin()) {
Serial.println("Failed to start MPU6050");
while (1) {
delay(10);
}
} else {
Serial.println("Start MPU6050");
}
// display MPU6050 settings
mpu.setAccelerometerRange(MPU6050_RANGE_2_G);
mpu.setGyroRange(MPU6050_RANGE_500_DEG);
mpu.setFilterBandwidth(MPU6050_BAND_44_HZ);
calculate_IMU_error();
pinMode(2,OUTPUT);
}
void calculate_IMU_error()
{
sensors_event_t a, g, temp;
mpu.getEvent(&a, &g, &temp);
int c = 0;
int times_c =200;
while(c<times_c){
AccX = a.acceleration.x;
AccY = a.acceleration.y;
AccZ = a.acceleration.z;
AccErrorX+=AccX;AccErrorY+=AccY;AccErrorZ+=AccZ;
GyroX = g.gyro.x;
GyroY = g.gyro.y;
GyroZ = g.gyro.z;
GyroErrorX+=GyroX;GyroErrorY+=GyroY;GyroErrorZ+=GyroZ;
delay(0.001);c++;
}
AccErrorX = AccErrorX/times_c; AccErrorY = AccErrorY/times_c; AccErrorZ = AccErrorZ/times_c;
GyroErrorX = GyroErrorX/times_c; GyroErrorY = GyroErrorY/times_c; GyroErrorZ = GyroErrorZ/times_c;
}
//均值滤波
void avg()
{
int d = 0;
int times_d = 5;
AccSumX=0;AccSumY=0;AccSumZ=0;
GyroSumX=0;GyroSumY=0;GyroSumZ=0;
while(d<times_d)
{
sensors_event_t a, g, temp;
mpu.getEvent(&a, &g, &temp);
AccX = a.acceleration.x;
AccY = a.acceleration.y;
AccZ = a.acceleration.z;
GyroX = g.gyro.x;
GyroY = g.gyro.y;
GyroZ = g.gyro.z;
AccSumX+=AccX;AccSumY+=AccY;AccSumZ+=AccZ;
GyroSumX+=GyroX;GyroSumY+=GyroY;GyroSumZ+=GyroZ;
delay(0.001);d++;
}
AccX = AccSumX/times_d+AccErrorX;AccY = AccSumY/times_d+AccErrorY;AccZ = AccSumZ/times_d+AccErrorZ;
GyroX=GyroSumX/times_d+GyroErrorX;GyroY=GyroSumY/times_d+GyroErrorY;GyroZ=GyroSumZ/times_d+GyroErrorZ;
}
//算得Roll角
float GetRoll(float AccX,float AccZ, float fNorm) {
float fNormXZ = sqrt(AccX * AccX + AccZ * AccZ);
float fCos = fNormXZ / fNorm;
return acos(fCos) * fRad2Deg;
}
//算得Pitch角
float GetPitch(float AccY,float AccZ, float fNorm) {
float fNormYZ = sqrt(AccY * AccY + AccZ * AccZ);
float fCos = fNormYZ / fNorm;
return acos(fCos) * fRad2Deg;
}
//算得yaw角
float GetYaw(float AccX,float AccY, float fNorm) {
float fNormXY = sqrt(AccX * AccX + AccY * AccY);
float fCos = fNormXY / fNorm;
return acos(fCos) * fRad2Deg;
}
void loop()
{
//简单均值滤波并修正
avg();
//计算加速度向量的模长,均以g为单位
float fNorm = sqrt(AccX * AccX + AccY * AccY + AccZ * AccZ);
float fRoll = GetRoll(AccX,AccZ, fNorm); //计算Roll角
if (AccY > 0) {
fRoll = -fRoll;
}
float fPitch = GetPitch(AccY,AccZ, fNorm); //计算Pitch角
if (AccX < 0) {
fPitch = -fPitch;
}
float fYaw = GetYaw(AccX,AccY,fNorm);//计算Yaw角
if(AccZ<0){
fYaw = -fYaw;
}
//计算两次测量的时间间隔dt,以秒为单位
unsigned long nCurTime = micros();
float dt = (double)(nCurTime - nLastTime) / 1000000.0;
//对Roll角和Pitch角进行卡尔曼滤波
//rad2deg
float GyroX_deg =GyroX*fRad2Deg;
float GyroY_deg =GyroY*fRad2Deg;
float GyroZ_deg =GyroZ*fRad2Deg;
// 这个*2是在测试的时候发现测量数值是前者一半的时候补充的。理论推导是没有的。
// 后面在开发两个MPU6050时发现,是开头校准的时候出现的问题,不应该拿角度直接校准。
// yaw角 六轴陀螺仪测不准 , 用加速度计测量会不变(这里加入了角速度修正会移动然后回归到原先的值)如果采用角速度测量,加速度修正则会漂移。
float fNewRoll = kalmanRoll.getAngle(2*fRoll, GyroX_deg, dt);
float fNewPitch = kalmanPitch.getAngle(2*fPitch, GyroY_deg,dt);
float fNewYaw = kalmanYaw.getAngle(fYaw, GyroZ_deg,dt);
// //跟据滤波值计算角度速
// float fRollRate = (fNewRoll - fLastRoll) / dt;
// float fPitchRate = (fNewPitch - fLastPitch) / dt;
//更新Roll角和Pitch角
fLastRoll = fNewRoll;
fLastPitch = fNewPitch;
Serial.print(fNewRoll);
Serial.print(",");
Serial.print(fNewPitch);
Serial.print(",");
Serial.print(fNewYaw);
Serial.print("\n");
//更新本次测的时间
nLastTime = nCurTime;
// 检测复位信号
if (digitalRead(RESET_PIN) == HIGH) {
// 复位信号为低电平,说明 reset 按钮被按下
Serial.end(); // 关闭串口通讯
delay(10000); // 延时一段时间以确保串口通讯关闭
}
}
备注
yaw角 六轴陀螺仪测不准 , 用加速度计测量会不变(这里加入了角速度修正会移动然后回归到原先的值)如果采用角速度测量,加速度修正则会漂移。需要换成九轴(磁力计修正)才可以测准。
两个MPU
这里AD0 接ACC可以将iic地址变为0x69,接地是0x68 .这样可以实现连接两个陀螺仪的功能。
这里测试`
if (!mpu1.begin(0x68))
{
Serial.println("Failed to start MPU60501");
while (1) {
delay(10);
}
}
else
{
Serial.println("Start MPU60501");
}
// initialize mpu
if (!mpu2.begin(0x69))
{
Serial.println("Failed to start MPU60502");
while (1) {
delay(10);
}
}
else
{
Serial.println("Start MPU60502");
}
完整代码
#include <Adafruit_MPU6050.h>
#include <Adafruit_sensor.h>
#include <Wire.h>
#include<Kalman.h>
const int RESET_PIN = 2; // 定义复位引脚为 D2
Adafruit_MPU6050 mpu1;
Adafruit_MPU6050 mpu2;
float fRad2Deg = 57.295779513f; //将弧度转为角度的乘数
float Acc1[3];
float Gyro1[3];
float AccError1[3];
float GyroError1[3];
float AccSum1[3];
float GyroSum1[3];
float Acc2[3];
float Gyro2[3];
float AccError2[3];
float GyroError2[3];
float AccSum2[3];
float GyroSum2[3];
float deta1Roll;
float deta1Pitch;
// float deta1Yaw;
float deta2Roll;
float deta2Pitch;
// float deta2Yaw;
float fLastRoll = 0.0f; //上一次滤波得到的Roll角
float fLastPitch = 0.0f; //上一次滤波得到的Pitch角
unsigned long nLastTime = 0; //上一次读数的时间
unsigned long nLastTime2 = 0; //上一次读数的时间
Kalman kalmanRoll; //Roll角滤波器
Kalman kalmanPitch; //Pitch角滤波器
Kalman kalmanYaw; //Yaw角滤波器
Kalman kalmanRoll2; //Roll角滤波器
Kalman kalmanPitch2; //Pitch角滤波器
Kalman kalmanYaw2; //Yaw角滤波器
void setup(){
// put your setup code here, to run once:
Serial.begin(115200);
// initialize mpu
if (!mpu1.begin(0x68))
{
Serial.println("Failed to start MPU60501");
while (1) {
delay(10);
}
}
else
{
Serial.println("Start MPU60501");
}
// initialize mpu
if (!mpu2.begin(0x69))
{
Serial.println("Failed to start MPU60502");
while (1) {
delay(10);
}
}
else
{
Serial.println("Start MPU60502");
}
// display MPU6050 settings
mpu1.setAccelerometerRange(MPU6050_RANGE_2_G);
mpu1.setGyroRange(MPU6050_RANGE_500_DEG);
mpu1.setFilterBandwidth(MPU6050_BAND_21_HZ);
// display MPU6050 settings
mpu2.setAccelerometerRange(MPU6050_RANGE_2_G);
mpu2.setGyroRange(MPU6050_RANGE_500_DEG);
mpu2.setFilterBandwidth(MPU6050_BAND_21_HZ);
// calculate_IMU_error(1);
// calculate_IMU_error(2);
}
void calculate_IMU_error(int which){
if (which == 1){
sensors_event_t a, g, temp;
mpu1.getEvent(&a, &g, &temp);
int c = 0;
int times_c =200;
while(c<times_c)
{
Acc1[0] = a.acceleration.x;
Acc1[1]= a.acceleration.y;
Acc1[2] = a.acceleration.z;
AccError1[0]+=Acc1[0];AccError1[1]+=Acc1[1];AccError1[2]+=Acc1[2];
Gyro1[0] = g.gyro.x;
Gyro1[1] = g.gyro.y;
Gyro1[2] = g.gyro.z;
GyroError1[0]+=Gyro1[0];GyroError1[1]+=Gyro1[1];GyroError1[2]+=Gyro1[2];
delay(0.001);c++;
}
AccError1[0] = AccError1[0]/times_c; AccError1[1] = AccError1[1]/times_c; AccError1[2] = AccError1[2]/times_c;
// GyroError1[0] = GyroError1[0]/times_c; GyroError1[1] = GyroError1[1]/times_c; GyroError1[2] = GyroError1[2]/times_c;
deta1Roll = GetRoll(AccError1[0],AccError1[2],sqrt(AccError1[0] * AccError1[0] + AccError1[1] * AccError1[1] + AccError1[2] * AccError1[2]));
deta1Pitch = GetPitch(AccError1[1],AccError1[2],sqrt(AccError1[0] * AccError1[0] + AccError1[1] * AccError1[1] + AccError1[2] * AccError1[2]));
}
if (which == 2){
sensors_event_t a, g, temp;
mpu2.getEvent(&a, &g, &temp);
int c = 0;
int times_c =200;
while(c<times_c)
{
Acc2[0] = a.acceleration.x;
Acc2[1]= a.acceleration.y;
Acc2[2] = a.acceleration.z;
AccError2[0]+=Acc2[0];AccError2[1]+=Acc2[1];AccError2[2]+=Acc2[2];
Gyro2[0] = g.gyro.x;
Gyro2[1] = g.gyro.y;
Gyro2[2] = g.gyro.z;
GyroError2[0]+=Gyro2[0];GyroError2[1]+=Gyro2[1];GyroError2[2]+=Gyro2[2];
delay(0.001);c++;
}
AccError2[0] = AccError2[0]/times_c; AccError2[1] = AccError2[1]/times_c; AccError2[2] = AccError2[2]/times_c;
// GyroError2[0] = GyroError2[0]/times_c; GyroError2[1] = GyroError2[1]/times_c; GyroError2[2] = GyroError2[2]/times_c;
deta2Roll = GetRoll(AccError2[0],AccError2[2],sqrt(AccError2[0] * AccError2[0] + AccError2[1] * AccError2[1] + AccError2[2] * AccError2[2]));
deta2Pitch = GetPitch(AccError2[1],AccError2[2],sqrt(AccError2[0] * AccError2[0] + AccError2[1] * AccError2[1] + AccError2[2] * AccError2[2]));
}
}
void avg(int which){
int d = 0;
int times_d = 5;
if(which==1)
{
AccSum1[0]=0;AccSum1[1]=0;AccSum1[2]=0;
GyroSum1[0]=0;GyroSum1[1]=0;GyroSum1[2]=0;
while(d<times_d)
{
sensors_event_t a, g, temp;
mpu1.getEvent(&a, &g, &temp);
Acc1[0] = a.acceleration.x;
Acc1[1] = a.acceleration.y;
Acc1[2] = a.acceleration.z;
Gyro1[0] = g.gyro.x;
Gyro1[1] = g.gyro.y;
Gyro1[2] = g.gyro.z;
AccSum1[0]+=Acc1[0];AccSum1[1]+=Acc1[1];AccSum1[2]+=Acc1[2];
GyroSum1[0]+=Gyro1[0];GyroSum1[1]+=Gyro1[1];GyroSum1[2]+=Gyro1[2];
delay(0.001);d++;
}
// Acc1[0] = AccSum1[0]/times_d+AccError1[0];Acc1[1] = AccSum1[1]/times_d+AccError1[1];Acc1[2] = AccSum1[2]/times_d+AccError1[2];
// Gyro1[0]=GyroSum1[0]/times_d+GyroError1[0];Gyro1[1]=GyroSum1[1]/times_d+GyroError1[1];Gyro1[2]=GyroSum1[2]/times_d+GyroError1[2];
Acc1[0] = AccSum1[0]/times_d;Acc1[1] = AccSum1[1]/times_d;Acc1[2] = AccSum1[2]/times_d;
Gyro1[0]=GyroSum1[0]/times_d;Gyro1[1]=GyroSum1[1]/times_d;Gyro1[2]=GyroSum1[2]/times_d;
}
if(which==2)
{
AccSum2[0]=0;AccSum2[1]=0;AccSum2[2]=0;
GyroSum2[0]=0;GyroSum2[1]=0;GyroSum2[2]=0;
while(d<times_d)
{
sensors_event_t a, g, temp;
mpu2.getEvent(&a, &g, &temp);
Acc2[0] = a.acceleration.x;
Acc2[1] = a.acceleration.y;
Acc2[2] = a.acceleration.z;
Gyro2[0] = g.gyro.x;
Gyro2[1] = g.gyro.y;
Gyro2[2] = g.gyro.z;
AccSum2[0]+=Acc2[0];AccSum2[1]+=Acc2[1];AccSum2[2]+=Acc2[2];
GyroSum2[0]+=Gyro2[0];GyroSum2[1]+=Gyro2[1];GyroSum2[2]+=Gyro2[2];
delay(0.001);d++;
}
// Acc2[0] = AccSum2[0]/times_d+AccError2[0];Acc2[1] = AccSum2[1]/times_d+AccError2[1];Acc2[2] = AccSum2[2]/times_d+AccError2[2];
// Gyro2[0]=GyroSum2[0]/times_d+GyroError2[0];Gyro2[1]=GyroSum2[1]/times_d+GyroError2[1];Gyro2[2]=GyroSum2[2]/times_d+GyroError2[2];
Acc2[0] = AccSum2[0]/times_d;Acc2[1] = AccSum2[1]/times_d;Acc2[2] = AccSum2[2]/times_d;
Gyro2[0]=GyroSum2[0]/times_d;Gyro2[1]=GyroSum2[1]/times_d;Gyro2[2]=GyroSum2[2]/times_d;
}
}
//算得Roll角
float GetRoll(float AccX,float AccZ, float fNorm) {
float fNormXZ = sqrt(AccX * AccX + AccZ * AccZ);
float fCos = fNormXZ / fNorm;
return acos(fCos) * fRad2Deg;
}
//算得Pitch角
float GetPitch(float AccY,float AccZ, float fNorm) {
float fNormYZ = sqrt(AccY * AccY + AccZ * AccZ);
float fCos = fNormYZ / fNorm;
return acos(fCos) * fRad2Deg;
}
//算得yaw角
float GetYaw(float AccX,float AccY, float fNorm) {
float fNormXY = sqrt(AccX * AccX + AccY * AccY);
float fCos = fNormXY / fNorm;
return acos(fCos) * fRad2Deg;
}
void loop() {
avg(1);
//计算加速度向量的模长,均以g为单位
float fNorm = sqrt(Acc1[0] * Acc1[0] + Acc1[1] * Acc1[1] + Acc1[2] * Acc1[2]);
float fRoll = GetRoll(Acc1[0],Acc1[2], fNorm)+deta1Roll; //计算Roll角
if (Acc1[1] > 0) {
fRoll = -fRoll;
}
float fPitch = GetPitch(Acc1[1],Acc1[2], fNorm)+deta1Pitch; //计算Pitch角
if (Acc1[0] < 0) {
fPitch = -fPitch;
}
float fYaw = GetYaw(Acc1[0],Acc1[1],fNorm);//计算Yaw角
if(Acc1[2]<0){
fYaw = -fYaw;
}
//计算两次测量的时间间隔dt,以秒为单位
unsigned long nCurTime = micros();
float dt = (double)(nCurTime - nLastTime) / 1000000.0;
//对Roll角和Pitch角进行卡尔曼滤波
//rad2deg
float GyroX_deg =Gyro1[0]*fRad2Deg;
float GyroY_deg =Gyro1[1]*fRad2Deg;
float GyroZ_deg =Gyro1[2]*fRad2Deg;
/// 将单MPU的问题修正
float fNewRoll = kalmanRoll.getAngle(fRoll, GyroX_deg, dt);
float fNewPitch = kalmanPitch.getAngle(fPitch, GyroY_deg,dt);
float fNewYaw = kalmanYaw.getAngle(fYaw, GyroZ_deg,dt);
Serial.print(fNewRoll);
Serial.print(",");
Serial.print(fNewPitch);
Serial.print(",");
Serial.print(fNewYaw);
//更新本次测的时间
nLastTime = nCurTime;
//mpu2//下面没有定义直接调用
avg(2);
//计算加速度向量的模长,均以g为单位
float fNorm2 = sqrt(Acc2[0] * Acc2[0] + Acc2[1] * Acc2[1] + Acc2[2] * Acc2[2]);
float fRoll2 = GetRoll(Acc2[0],Acc2[2], fNorm2)+deta2Roll; //计算Roll角
if (Acc2[1] > 0) {
fRoll2 = -fRoll2;
}
float fPitch2 = GetPitch(Acc2[1],Acc2[2], fNorm2)+deta2Pitch; //计算Pitch角
if (Acc2[0] < 0) {
fPitch2 = -fPitch2;
}
float fYaw2 = GetYaw(Acc2[0],Acc2[1],fNorm2);//计算Yaw角
if(Acc2[2]<0){
fYaw2 = -fYaw2;
}
//计算两次测量的时间间隔dt,以秒为单位
float nCurTime2 = micros();
float dt2 = (double)(nCurTime2 - nLastTime2) / 1000000.0;
//对Roll角和Pitch角进行卡尔曼滤波
//rad2deg
float GyroX_deg2 =Gyro2[0]*fRad2Deg;
float GyroY_deg2 =Gyro2[1]*fRad2Deg;
float GyroZ_deg2 =Gyro2[2]*fRad2Deg;
float fNewRoll2 = kalmanRoll2.getAngle(fRoll2, GyroX_deg2, dt2);
float fNewPitch2 = kalmanPitch2.getAngle(fPitch2, GyroY_deg2,dt2);
float fNewYaw2 = kalmanYaw2.getAngle(fYaw2, GyroZ_deg2,dt2);
Serial.print(",");
Serial.print(fNewRoll2);
Serial.print(",");
Serial.print(fNewPitch2);
Serial.print(",");
Serial.print(fNewYaw2);
Serial.print("\n");
//更新本次测的时间
nLastTime2 = nCurTime2;
}
实验结果
卡尔曼公式推导
http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it/