|
本帖最后由 ggko123 于 2012-10-2 08:57 编辑
我作为一直在论坛关注着6050的使用问题,几乎刨遍了所有有关的帖子,终于找到了一个合适的算法,就是所谓的一阶互补,
http://www.geek-workshop.com/forum.php?mod=viewthread&tid=1549
根据贴里的程序,我完成了x,y轴的程序,效果不错,能输出比较准确的角度 - #include "Wire.h"
- #include "i2cdev.h"
- #include "MPU6050.h"
- MPU6050 accelgyro;
- int16_t ax, ay, az;
- int16_t gx, gy, gz;
- #define Gry_offset_X 45//陀螺仪X轴的静态飘移。
- #define Gry_offset_Y 271
- #define Gyr_Gain -0.00763//由陀螺仪X轴读数转换为角速度值,=1/131,向前方向与X轴为反方向,故乘以-1
- #define ACC_Gain 0.000061//由读数转换为加速度值,=1/16384
- #define pi 3.1415926
- #define K_x 0.715//一阶互补滤波权重取值,不知由来,可更改。
- #define k_Y 1.3
- unsigned long preTime = 0;
- float angle_X = 0;
- float angle_Y = 0;
- void setup()
- {
- Wire.begin();
- Serial.begin(9600);
- accelgyro.initialize();//MPU6050初始化。
- }
- void loop()
- {
-
- unsigned long now = millis();//当前时间(ms)
- float dt = (now - preTime) / 500.0;//微分时间(s)
- preTime = now;//上一次采样时间(ms)
-
- accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);//读取六轴数值
-
- float Y_Accelerometer = ay * ACC_Gain;//转换为向前的加速度(g),为负值
- float Z_Accelerometer = az * ACC_Gain;//转换为向下的加速度(g)
- float X_Accelerometer = ax * ACC_Gain;
- float angleA_X= atan(Y_Accelerometer/ Z_Accelerometer)* (-180)/ pi;//加速度仪,反正切获得弧度值,乘以180度/pi
- //获得角度值,乘以-1得正
- float angleA_Y= atan(X_Accelerometer/Z_Accelerometer)*(-180)/pi;
-
- float gx_revised = gx + Gry_offset_X;//陀螺仪x轴静态时修正后的角速度读数,向前为负值
- float gy_revised = gy + Gry_offset_Y;
- float omega_X= Gyr_Gain* gx_revised;//陀螺仪,转换为向前的角速度(o/s),Gyr_Gain取负,故负负得正
- float omega_Y= Gyr_Gain* gy_revised;
- float angle_dt_X = omega_X * dt;//角度的单次积分值
- float angle_dt_Y = omega_Y * dt;
- angle_X+= (Gyr_Gain * (gx + Gry_offset_X)) * dt;
- angle_Y+= (Gyr_Gain * (gy + Gry_offset_Y)) * dt;
- float A_X= K_x/ (K_x+ dt);
- //陀螺仪的权值
- float A_Y= k_Y/(k_Y+dt);
- angle_X= A_X* (angle_X+ omega_X* dt)+ (1-A_X)* angleA_X;//一阶互补滤波后的输出角度(o)
- angle_Y= A_Y*(angle_Y+omega_Y*dt)+(1-A_Y)*angleA_Y;
- delay(10);
-
- Serial.println(angle_X);
- Serila.println(angle_Y);
- }
复制代码
但是到了z轴就卡住了,用同样的算法,z轴只能输出角度变化,不能输出准确的角度。 - #include "Wire.h"
- #include "i2cdev.h"
- #include "MPU6050.h"
- MPU6050 accelgyro;
- int16_t ax, ay, az;
- int16_t gx, gy, gz;
- #define Gry_offset 38//z轴偏移量
- #define Gyr_Gain -0.00763
- #define ACC_Gain 0.000061
- #define pi 3.1415926
- #define K 1.75//这个比例常数可以改的
- unsigned long preTime = 0;
- float angleG = 0;
- void setup()
- {
- Wire.begin();
- Serial.begin(9600);
- accelgyro.initialize();
- void loop()
- {
-
- unsigned long now = millis();
- float dt = (now - preTime) / 400.0;
- preTime = now;
-
- accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
-
- float Y_Accelerometer = ay * ACC_Gain;
- float X_Accelerometer = ax * ACC_Gain;
- float angleA= atan(Y_Accelerometer/ X_Accelerometer)* (-180)/ pi;
-
-
- float gz_revised = gz + Gry_offset;
- float omega= Gyr_Gain* gz_revised;
- float angle_dt = omega * dt;/
-
- angleG+= (Gyr_Gain * (gz + Gry_offset)) * dt;
- float A= K/ (K+ dt);
- angleG= A* (angleG+ omega* dt)+ (1-A)* angleA ;
-
- delay(10);
- Serial.println(angleG);
- }
复制代码
如图,有四次波动,代表四次旋转,波形上升是顺时针转动,下降是逆时针转动,但都会回到零点附近,不能停在具体偏转角度上。是算法问题,还是6050的设计问题?
我觉得离成功不远了,只有读出z轴角度,就能编写一个processing上的姿态识别程序,到时一定会写一篇详细过程,希望诸位大牛帮忙! |
本帖子中包含更多资源
您需要 登录 才可以下载或查看,没有帐号?注册
x
|