Arduino MPU6050陀螺儀運用卡爾曼濾波姿態解算實驗

版權聲明：本文為博主原創文章，未經博主允許不得轉載。

2019年3月20日

發布

實例效果

輸出效果：

首先看看本例程XYZ軸的輸出效果圖:

(時間曲線的體現是：靜止姿態→擺動→恢復原靜止姿態→拍動桌子→靜止姿態)

這圖片是通過Excel生成的

元件說明

作用于四軸無人機，平衡車，機器人，電子穩定器等等的電子實作當中，用于姿態判斷。掌握了可以發揮自己的想象完成更多更有趣的作品。

名稱：MPU-6050模塊(三軸陀螺儀 + 三軸加速度)

使用芯片：MPU-6050

供電電源：3-5v(內部低壓差穩壓)

通信方式：標準IIC通信協議

芯片內置16bit AD轉換器,16位數據輸出

陀螺儀范圍：±250 500 1000 2000 °/s

加速度范圍：±2±4±8±16g

采用沉金PCB,機器焊接工藝保證質量

引腳間距2.54mm

引腳說明

– VCC 3.3-5V(內部有穩壓芯片)

GND 地線

SCL MPU6050作為從機時IIC時鐘線

SDA MPU6050作為從機時IIC數據線

XCL MPU6050作為主機時IIC時鐘線

XDA MPU6050作為主機時IIC數據線

AD0 地址管腳，該管腳決定了IIC地址的最低一位

INT 中斷引腳

BOM表

Arduino UNO

x 1

MPU6050

x 1

面包板

x 1

USB數據線

x 1

跳線

若干

接線

程序代碼

程序的實現首先要更新I2C庫

在GITHUB找到的I2C庫

(庫下載地址: https://github.com/jrowberg/i2cdevlib)

打開Adruino IDE，把Arduino文件夾里的I2Cdev，MPU6050文件夾復制到Arduino IDE的庫文件夾里

(默認的路徑是這個 C:\Program Files (x86)\Arduino\libraries)

把程序上傳到板子上，打開串口監視器，就可以看到一堆堆的數據了

本實例程序和庫文件打包下載：

壓縮包文件夾說明：

I2Cdev — i2c庫(庫都是需要放置在Arduino的庫目錄里面)

MPU6050 — mpu6050陀螺儀庫

LS_MPU6050 — 主程序文件

#include Wire.h

#include I2Cdev.h

#include MPU6050.h

MPU6050 accelgyro;

unsigned long now, lastTime = 0;

float dt; //微分時間

int16_t ax, ay, az, gx, gy, gz; //加速度計陀螺儀原始數據

float aax=0, aay=0,aaz=0, agx=0, agy=0, agz=0; //角度變量

long axo = 0, ayo = 0, azo = 0; //加速度計偏移量

long gxo = 0, gyo = 0, gzo = 0; //陀螺儀偏移量

float pi = 3.1415926;

float AcceRatio = 16384.0; //加速度計比例系數

float GyroRatio = 131.0; //陀螺儀比例系數

uint8_t n_sample = 8; //加速度計濾波算法采樣個數

float aaxs[8] = {0}, aays[8] = {0}, aazs[8] = {0}; //x,y軸采樣隊列

long aax_sum, aay_sum,aaz_sum; //x,y軸采樣和

float a_x[10]={0}, a_y[10]={0},a_z[10]={0} ,g_x[10]={0} ,g_y[10]={0},g_z[10]={0}; //加速度計協方差計算隊列

float Px=1, Rx, Kx, Sx, Vx, Qx; //x軸卡爾曼變量

float Py=1, Ry, Ky, Sy, Vy, Qy; //y軸卡爾曼變量

float Pz=1, Rz, Kz, Sz, Vz, Qz; //z軸卡爾曼變量

void setup()

{

Wire.begin();

Serial.begin(115200);

accelgyro.initialize(); //初始化

unsigned short times = 200; //采樣次數

for(int i=0;i

{

accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); //讀取六軸原始數值

axo += ax; ayo += ay; azo += az; //采樣和

gxo += gx; gyo += gy; gzo += gz;

}

axo /= times; ayo /= times; azo /= times; //計算加速度計偏移

gxo /= times; gyo /= times; gzo /= times; //計算陀螺儀偏移

}

void loop()

{

unsigned long now = millis(); //當前時間(ms)

dt = (now - lastTime) / 1000.0; //微分時間(s)

lastTime = now; //上一次采樣時間(ms)

accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); //讀取六軸原始數值

float accx = ax / AcceRatio; //x軸加速度

float accy = ay / AcceRatio; //y軸加速度

float accz = az / AcceRatio; //z軸加速度

aax = atan(accy / accz) * (-180) / pi; //y軸對于z軸的夾角

aay = atan(accx / accz) * 180 / pi; //x軸對于z軸的夾角

aaz = atan(accz / accy) * 180 / pi; //z軸對于y軸的夾角

aax_sum = 0; // 對于加速度計原始數據的滑動加權濾波算法

aay_sum = 0;

aaz_sum = 0;

for(int i=1;i

{

aaxs[i-1] = aaxs[i];

aax_sum += aaxs[i] * i;

aays[i-1] = aays[i];

aay_sum += aays[i] * i;

aazs[i-1] = aazs[i];

aaz_sum += aazs[i] * i;

}

aaxs[n_sample-1] = aax;

aax_sum += aax * n_sample;

aax = (aax_sum / (11*n_sample/2.0)) * 9 / 7.0; //角度調幅至0-90°

aays[n_sample-1] = aay; //此處應用實驗法取得合適的系數

aay_sum += aay * n_sample; //本例系數為9/7

aay = (aay_sum / (11*n_sample/2.0)) * 9 / 7.0;

aazs[n_sample-1] = aaz;

aaz_sum += aaz * n_sample;

aaz = (aaz_sum / (11*n_sample/2.0)) * 9 / 7.0;

float gyrox = - (gx-gxo) / GyroRatio * dt; //x軸角速度

float gyroy = - (gy-gyo) / GyroRatio * dt; //y軸角速度

float gyroz = - (gz-gzo) / GyroRatio * dt; //z軸角速度

agx += gyrox; //x軸角速度積分

agy += gyroy; //x軸角速度積分

agz += gyroz;

/* kalman start */

Sx = 0; Rx = 0;

Sy = 0; Ry = 0;

Sz = 0; Rz = 0;

for(int i=1;i<10;i++)

{ //測量值平均值運算

a_x[i-1] = a_x[i]; //即加速度平均值

Sx += a_x[i];

a_y[i-1] = a_y[i];

Sy += a_y[i];

a_z[i-1] = a_z[i];

Sz += a_z[i];

}

a_x[9] = aax;

Sx += aax;

Sx /= 10; //x軸加速度平均值

a_y[9] = aay;

Sy += aay;

Sy /= 10; //y軸加速度平均值

a_z[9] = aaz;

Sz += aaz;

Sz /= 10;

for(int i=0;i<10;i++)

{

Rx += sq(a_x[i] - Sx);

Ry += sq(a_y[i] - Sy);

Rz += sq(a_z[i] - Sz);

}

Rx = Rx / 9; //得到方差

Ry = Ry / 9;

Rz = Rz / 9;

Px = Px + 0.0025; // 0.0025在下面有說明...

Kx = Px / (Px + Rx); //計算卡爾曼增益

agx = agx + Kx * (aax - agx); //陀螺儀角度與加速度計速度疊加

Px = (1 - Kx) * Px; //更新p值

Py = Py + 0.0025;

Ky = Py / (Py + Ry);

agy = agy + Ky * (aay - agy);

Py = (1 - Ky) * Py;

Pz = Pz + 0.0025;

Kz = Pz / (Pz + Rz);

agz = agz + Kz * (aaz - agz);

Pz = (1 - Kz) * Pz;

/* kalman end */

Serial.print(agx);Serial.print(,);

Serial.print(agy);Serial.print(,);

Serial.print(agz);Serial.println();

}

程序說明

這程序我還在研究，在這僅僅拋磚引玉。

總結

以上是生活随笔為你收集整理的android 陀螺仪滤波_Arduino MPU6050陀螺仪运用卡尔曼滤波姿态解算实验的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。