少年 发表于 2013-8-20 11:19:48

四轴姿态检测，陀螺仪 加速度计 电子罗盘数据融合

求姿态角的思路：求大神帮忙看一下我的思路可不可以
第一：从陀螺仪得到姿态角数据(Roll_Gyro, Pitch_Gyro, Yaw_Gyro)

第二：从加速度计得到roll_Acc,pitch_Acc再从电子罗盘得到yaw_Compass，然后把这三
个组成一个(Roll_Acc,Pitch_Acc,Yaw_Compass)

第三：把上面两个数据用卡尔曼滤波融合，最后得到最佳姿态角。

最后一个人问题就是陀螺仪有偏移我们应该定期的去修正陀螺仪的姿态角数据，请问一下我要怎去定期的修正啊？

学慧放弃 发表于 2013-8-20 16:32:41

可以DMP融合？

camion 发表于 2013-8-20 17:08:43

// obtaining and filtering Acc angles   
   accelgyro.getMotion6(&ax,&ay,&az,&gx,&gy,&gz);
   Ax=ax/16384.00;
   Ay=ay/16384.00;
   Az=az/16384.00;
   Angel_accX =atan(Ay/sqrt(Ax*Ax+Az*Az))*180/3.14 -Ax_offset; //offset
   Angel_accY =-atan(Ax/sqrt(Az*Az+Ay*Ay))*180/3.14 -Ay_offset; //offset
   estima_X = estima_cal(estima_X, Angel_accX, 0.2);
   estima_Y = estima_cal(estima_Y, Angel_accY, 0.2);   
//   Serial.print("estima_X=");
//   Serial.print(estima_X);
//   Serial.print("estima_Y=");
//   Serial.println(estima_Y);      
   
// obtaining Gyro angles   without filter simply because I'm lasy
   ggx=gx/131.00;
   ggy=gy/131.00;
   ggz=gz/131.00;
   Gx=Gx+(ggx-Gx_offset)*time_span;
   Gy=Gy+(ggy-Gy_offset)*time_span;
   autoCorrection();
//   Serial.print("Gx=");
//   Serial.print(Gx);
//   Serial.print(" Gy=");
//   Serial.println(Gy);


//fusion angles by combining 40% last observe + 30% Acc + 30% Gyro

fusion_angle_X = update_Estimat(fusion_angle_X, estima_X, Gx );
fusion_angle_Y = update_Estimat(fusion_angle_Y, estima_Y, Gy );
//   Serial.print("fusion_X=");
//   Serial.print(fusion_angle_X);
//   Serial.print("fusion_Y=");
//   Serial.println(fusion_angle_Y);   
   
//   if (fusion_angle_Y>=80){
//             digitalWrite(13, HIGH);   
//             delay(1);               
//             digitalWrite(13, LOW);   
//             delay(1);
//   }
   
//---------------------------------PID's----------------------------------------
uodate_error(orientX, orientY ,fusion_angle_X, fusion_angle_Y);

camion 发表于 2013-8-20 17:09:25

void uodate_error(double consigne_X, double consigne_Y, double mesure_X, double mesure_Y){
now_error_X = consigne_X - mesure_X;
now_error_Y = consigne_Y - mesure_Y;
error_diff_X = (now_error_X - last_error_X)/time_span;
error_diff_Y = (now_error_Y - last_error_Y)/time_span;
error_int_X = error_int_X + now_error_X*time_span;
error_int_Y = error_int_Y + now_error_Y*time_span;
last_error_X =now_error_X;
last_error_Y =now_error_Y;
}
double update_Estimat(double estimate, double metron_1, double metron_2){
double temp = 0.4* estimate + 0.3*metron_1 + 0.3*metron_2;
return temp;
}

学慧放弃 发表于 2013-11-8 21:31:17

camion 发表于 2013-8-20 17:08 static/image/common/back.gif
// obtaining and filtering Acc angles   
   accelgyro.getMotion6(&ax,&ay,&az,&gx,&gy,&gz);
   Ax=a ...

ggx=gx/131.00;那句里面为什么要除以131.00？？？

极客人 发表于 2013-11-21 12:41:22

楼主贴一下程序呗~~~:D

szxiaomili 发表于 2014-6-24 18:25:55

:o,值得研究

Anderw 发表于 2014-10-15 16:50:34

求文件[email protected]
页: [1]
查看完整版本: 四轴姿态检测，陀螺仪 加速度计 电子罗盘数据融合