极客工坊

 找回密码
 注册

QQ登录

只需一步,快速开始

查看: 16293|回复: 7

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

[复制链接]
发表于 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融合?
回复 支持 反对

使用道具 举报

发表于 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);
回复 支持 反对

使用道具 举报

发表于 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
// 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 | 显示全部楼层
楼主贴一下程序呗~~~
回复 支持 反对

使用道具 举报

发表于 2014-6-24 18:25:55 | 显示全部楼层
,值得研究
回复 支持 反对

使用道具 举报

发表于 2014-10-15 16:50:34 | 显示全部楼层
回复 支持 反对

使用道具 举报

您需要登录后才可以回帖 登录 | 注册

本版积分规则 需要先绑定手机号

Archiver|联系我们|极客工坊

GMT+8, 2024-3-29 16:39 , Processed in 0.046794 second(s), 23 queries .

Powered by Discuz! X3.4 Licensed

Copyright © 2001-2021, Tencent Cloud.

快速回复 返回顶部 返回列表