帮忙看一下，MPU6050这样融合后的数据是否可以做平衡车了？
完全不懂卡尔曼，就用所谓的一阶互补，源程序先贴上：（其中的K值取多少，我也不懂原理和计算）#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"
MPU6050 accelgyro;
int16_t ax, ay, az;
int16_t gx, gy, gz;
#define Gry_offset 161//static deviation of the x-axis
#define Gyr_Gain 0.0763 // =1/131
#define ACC_Gain 0.000061 // =1/16384
#define pi 3.1415926
#define K 0.715//互补滤波权重取值
float angleG;
float A;
unsigned long timer = 0;
unsigned long preTime = 0;
void setup() {
Wire.begin();
Serial.begin(38400);
accelgyro.initialize();
delay(500);
}
void loop() {
unsigned long now = millis(); // 当前时间(ms)
float dt = (now - preTime) / 1000.0; // 微分时间(s)
preTime = now;
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
// these methods (and a few others) are also available
//accelgyro.getAcceleration(&ax, &ay, &az);
float Y_Accelerometer = ay * ACC_Gain; // 获取向前的加速度
float Z_Accelerometer = az * ACC_Gain; // 获取向下的加速度
float angleA = atan(Y_Accelerometer / Z_Accelerometer) * 180 / pi;
float omega =Gyr_Gain * ( gx +Gry_offset);
float A =K / (K + dt);
angleG = A * (angleG + omega * dt) + (1-A) * angleA;
Serial.print(angleA, 6);
Serial.print(",");
Serial.print(angleG,6);
Serial.print("\n");
delay(10);
}
静态时的数据如下：
1.962590,1.517662
1.562366,1.532969
1.593743,1.525755
1.491260,1.538712
1.611358,1.539084
1.591573,1.523348
1.499220,1.520789
1.542325,1.501054
1.651118,1.486202
1.848107,1.487913
1.793320,1.447107
1.780802,1.434657
1.591140,1.427784
2.093552,1.433617
1.780723,1.418282
2.170431,1.452794
2.056911,1.430457
1.991959,1.375236
1.915277,1.395300
2.013110,1.410308
2.108475,1.433622
2.062564,1.463360
动态下曲线感觉也不错！不会贴图，再给点数据吧（敲击桌子时的）：
4.597043,-1.081621
1.549426,-1.033189
-0.046595,-0.683347
0.426747,-0.818596
1.649786,-0.912630
3.280285,-0.741279
1.147905,-0.712550
-0.708813,-0.782673
-0.569814,-0.197119
-7.030243,-0.973573
0.032966,-1.350268
2.945956,-1.322242
-1.735231,-1.159333
0.861879,-1.134137
3.091095,-1.442734
1.323532,-1.244867
1.827124,-0.942808
10.706529,-2.613306
14.784671,-2.214338
-0.499738,-1.116414
-0.814508,-1.026801
6.542530,-1.810651
2.181357,-1.512274
本帖最后由 pww999 于 2012-7-24 21:26 编辑
学习了..... 卡尔曼 博大精深，学习了！ 互补滤波权权重取值应该也是动态的，我不知道原理，希望有路过的高手指点一下！！！
目前的情况是，剧烈晃动以后，angleG值(滤波后的角度值)还是有滞后，我已经将K值取0.715了。。。。。 本帖最后由 pww999 于 2012-7-25 19:43 编辑
将DT 那 /5000 发散的更快? 自己网上找找 Kalman的代码 能找到的都是一个人写的 我们自己在平衡车上用的参数 就是一下一组可以尝试看看滤波效果
0.001 0.0030.5 0.01 牛人玩到后来都是数学。。。。。
LZ怎么这么多号的？ 学习中…………………… lz angleA代表什么啊？是不是车的倾斜角度？如果是的话，我怎么感觉这个计算是错的啊， float angleA = atan(Y_Accelerometer / Z_Accelerometer) * 180 / pi;我觉得是float angleA = atan(Z_Accelerometer / Y_Accelerometer) * 180 / pi；不知道是不是哪里想错了。
页:
[1]