一、Arduino與MPU-6050的接線
六軸傳感器子產品MPU6050連線在Arduino UNO闆子上,SDA接口對應的是A4引腳,SCL對應的是A5引腳。MPU6050需要5V的電源,可由UNO闆直接供電。
INT中斷線沒用到我就沒接
二、讀取資料并顯示
因為是使用到I/2通信,我們需要在初始化I2C和序列槽
void setup() {
Wire.begin(); //加入 I2C 總順序列
Serial.begin(9600); //開啟序列槽,設定波特率
delay(1000);
mpu.initialize(); //初始化MPU6050
}
然後就可以直接擷取值了
void loop() {
mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); //IIC擷取MPU6050六軸資料 ax ay az gx gy gz
Serial.print(ax);Serial.print(",");
Serial.print(ay);Serial.print(",");
Serial.print(az);Serial.println();
}
三、卡曼濾波
應用中傳感器采集的資料存在大量噪音的資料波動很大,卡爾曼濾波器的效果無疑是最好的資料濾波器
void Kalman_Filter(double angle_m, double gyro_m)
{
angle += (gyro_m - q_bias) * dt;
angle_err = angle_m - angle;
Pdot[0] = Q_angle - P[0][1] - P[1][0];
Pdot[1] = - P[1][1];
Pdot[2] = - P[1][1];
Pdot[3] = Q_gyro;
P[0][0] += Pdot[0] * dt;
P[0][1] += Pdot[1] * dt;
P[1][0] += Pdot[2] * dt;
P[1][1] += Pdot[3] * dt;
PCt_0 = C_0 * P[0][0];
PCt_1 = C_0 * P[1][0];
E = R_angle + C_0 * PCt_0;
K_0 = PCt_0 / E;
K_1 = PCt_1 / E;
t_0 = PCt_0;
t_1 = C_0 * P[0][1];
P[0][0] -= K_0 * t_0;
P[0][1] -= K_0 * t_1;
P[1][0] -= K_1 * t_0;
P[1][1] -= K_1 * t_1;
angle += K_0 * angle_err; //角度
q_bias += K_1 * angle_err;
angle_dot = gyro_m - q_bias; //角速度
}
四、整體運作
需要先下載下傳MPU6050的i2cdevlib
https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/MPU6050
https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/I2Cdev
把下載下傳的程式放入aduino 工程的 libraries目錄下
最後這是完整的測試代碼,可以直接用aduino的序列槽螢幕直接看到輸出的資料
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
MPU6050 mpu; //執行個體化一個 MPU6050 對象,對象名稱為 mpu
int16_t ax, ay, az, gx, gy, gz;
//********************angle data*********************//
float Gyro_y; //Y軸陀螺儀資料暫存
float Gyro_x;
float Gyro_z;
float angleAx;
float angle6;
float K1 = 0.05; // 對加速度計取值的權重
float Angle; //一階互補濾波計算出的小車最終傾斜角度
float accelz = 0;
//********************angle data*********************//
//***************Kalman_Filter*********************//
float P[2][2] = {{ 1, 0 },
{ 0, 1 }
};
float Pdot[4] = { 0, 0, 0, 0};
float Q_angle = 0.001, Q_gyro = 0.005; //角度資料置信度,角速度資料置信度
float R_angle = 0.5 , C_0 = 1;
float q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;
float timeChange = 5; //濾波法采樣時間間隔毫秒
float dt = timeChange * 0.001; //注意:dt的取值為濾波器采樣時間
//***************Kalman_Filter*********************//
void Angletest()
{
//平衡參數
Angle = atan2(ay , az) * 57.3; //角度計算公式
Gyro_x = (gx - 128.1) / 131; //角度轉換
Kalman_Filter(Angle, Gyro_x); //卡曼濾波
//旋轉角度Z軸參數
if (gz > 32768) gz -= 65536; //強制轉換2g 1g
Gyro_z = -gz / 131; //Z軸參數轉換
accelz = az / 16.4;
angleAx = atan2(ax, az) * 180 / PI; //計算與x軸夾角
Gyro_y = -gy / 131.00; //計算角速度
//一階互補濾波
angle6 = K1 * angleAx + (1 - K1) * (angle6 + Gyro_y * dt);
}
kalman/
float angle, angle_dot; //平衡角度值
void Kalman_Filter(double angle_m, double gyro_m)
{
angle += (gyro_m - q_bias) * dt;
angle_err = angle_m - angle;
Pdot[0] = Q_angle - P[0][1] - P[1][0];
Pdot[1] = - P[1][1];
Pdot[2] = - P[1][1];
Pdot[3] = Q_gyro;
P[0][0] += Pdot[0] * dt;
P[0][1] += Pdot[1] * dt;
P[1][0] += Pdot[2] * dt;
P[1][1] += Pdot[3] * dt;
PCt_0 = C_0 * P[0][0];
PCt_1 = C_0 * P[1][0];
E = R_angle + C_0 * PCt_0;
K_0 = PCt_0 / E;
K_1 = PCt_1 / E;
t_0 = PCt_0;
t_1 = C_0 * P[0][1];
P[0][0] -= K_0 * t_0;
P[0][1] -= K_0 * t_1;
P[1][0] -= K_1 * t_0;
P[1][1] -= K_1 * t_1;
angle += K_0 * angle_err; //角度
q_bias += K_1 * angle_err;
angle_dot = gyro_m - q_bias; //角速度
}
void setup() {
Wire.begin(); //加入 I2C 總順序列
Serial.begin(9600); //開啟序列槽,設定波特率
delay(1000);
mpu.initialize(); //初始化MPU6050
}
void loop() {
mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); //IIC擷取MPU6050六軸資料 ax ay az gx gy gz
Angletest(); //擷取angle 角度和卡曼濾波
Serial.print(ax);Serial.print(",");
Serial.print(ay);Serial.print(",");
Serial.print(az);Serial.print("---");
Serial.print(angle);Serial.print(",");
Serial.print(angle_dot);Serial.print(",");
Serial.println(angle6);
delay(5);
}