陀螺仪积分出角度算法,请教

blackblue

L3D4200D陀螺,不管是在NXC还是Arduino下面,用以下算法,如果不加上加速度的数据融合,零飘太大了，静止放止3分钟左右，角度值可以从0飘到5度左右，请大家看看，以下算法有没有问题?

angle = angle + (gyro_read(vale,0)+gyro_offset) * dt

gyro_read(vale,0)   // 是库函数，读X轴数据；

gyro_offset  //是零飘抑制，我读了200百个数据后取的平均值；

dt //积分时间，我实际试的时候，试过100ms,150ms;

------------------

这么一个零飘，我瞎想想，怎么可能做自平衡呢？看到有用HT的陀螺单传感器做自平衡的，难道是HT的陀螺非常稳定？

糖伯虎

那个值应该是减吧，另外你说到了融合，可以有一种简单的融合法，加速度*0.05+陀螺*0.95，来长期纠正漂移。

糖伯虎

HT的也飘的，人家的算法里面有一段是这样的
gOffset = EMAOFFSET * gyroRaw + (1-EMAOFFSET) * gOffset;
gyroSpeed = gyroRaw - gOffset;
gAngleGlobal += gyroSpeed*tInterval;
gyroAngle = gAngleGlobal;

其中EMAOFFSET=0.0005
说明漂移也一直在修正。

blackblue

gOffset = EMAOFFSET * gyroRaw + (1-EMAOFFSET) * gOffset;
gyroSpeed = gyroRaw - gOffset;
-----------------------
哇,这二句我虽然不明白是什么算法，但明显要比我的取一个固定的OFFSET要好得多！这个是根据当前读数和前一次的OFFSET做一个比例均值！但不明白EMAOFFSET取值的原则是什么？
不过，今天回去就去试试,估计有用.......
-----------------------
我用加OFFSET是因为，我的大部份当前读数是负数，所以得到的OFFSET是个负数，我已经在前面计算OFFSET后，用0-OFFSET=OFFSET将OFFSET取绝对值了！

加速度传感我还没有买呢，听说那个AX3什么的，也不好，所以还没有确定买什么芯片呢！

blackblue

好强大的算法！！！
300秒后的部份数据：
angle_x: -0.082909;
angle_x: -0.094338;
angle_x: -0.099327;
angle_x: -0.092924;
angle_x: -0.078706;
angle_x: -0.081498;
angle_x: -0.094481;
angle_x: -0.104794;
angle_x: -0.108485;
angle_x: -0.109038;
angle_x: -0.107340;
angle_x: -0.118301;
angle_x: -0.091469;
angle_x: -0.082499;
angle_x: -0.090327;
angle_x: -0.069584;
angle_x: -0.054315;
angle_x: -0.040415;
angle_x: -0.032438;
angle_x: -0.039052;
angle_x: -0.039357;
angle_x: -0.048523;
angle_x: -0.052087;
angle_x: -0.052481;
angle_x: -0.040162;
angle_x: -0.025613;
angle_x: -0.040640;
angle_x: -0.040863;
angle_x: -0.050689;
angle_x: -0.033056;

---------------------------

#include <Wire.h>
#include <L3G4200D.h>
#define EMAoffset 0.05
#define Gyr_Gain 0.076335877862595  // 1/131,250dps
float angleG;
float G_offset;
float gyroSpeed;
unsigned long timer = 0;


L3G4200D gyro;

void setup() {
  Serial.begin(9600);
  Wire.begin();
  gyro.enableDefault();
  delay(500);
}

void loop() {
  long o_timer = timer;
  timer = millis();
  int dt = timer - o_timer;
  gyro.read();
  G_offset = EMAoffset *(gyro.g.x - 55) / 131 + (1-EMAoffset) * G_offset;
  gyroSpeed = (gyro.g.x - 55) / 131 - G_offset;
  angleG = angleG + gyroSpeed * dt / 1000;
  Serial.print("angle_x: ");
  Serial.print(angleG,6);
  Serial.println(";");   
  delay(100);
}

旭日东升

开始学习，，，受教，，

夜の歌ゞ

blackblue 发表于 2012-6-7 20:41
好强大的算法！！！
300秒后的部份数据：
angle_x: -0.082909;

请问一下 这段是利用陀螺仪测量角度变化的 算法么  我对陀螺仪工作原理不是很明白，但是最近又需要用到这个  很纠结，希望楼主能够指教一下  谢谢

DEV

gyro.g.x - 55    这里面的 -55  是做什么的

blackblue

55是零飘值!
所以要减去......
-----------------------
另外,这种算法有局限性的,滞后效应是明显能感觉到的！

云伤哲爱

糖伯虎 发表于 2012-6-7 14:09
HT的也飘的，人家的算法里面有一段是这样的
gOffset = EMAOFFSET * gyroRaw + (1-EMAOFFSET) * gOffset;
...

我之前也看到gOffset = EMAOFFSET * gyroRaw + (1-EMAOFFSET) * gOffset;
这个修正零漂的方法，不知道它的原理是什么

云伤哲爱

blackblue 发表于 2012-12-25 12:39
55是零飘值!
所以要减去......
-----------------------

确实，我现在就遇到了这个问题，请教一下，你是怎么解决的呢？

blackblue

我不知道你用什么陀螺，已前我用过L3G4200D，直接就是零飘纠正后积分出角度，是用NXC写的程序，用NXT-G不管怎么样，都只能短暂稳定！现在用MPU6050，目前这个陀螺开放了DMP功能库（目前不开放内部功能寄存器，还需要I2C写入一段代码后才能激活，所以目前无法在NXC下用）；在ARDUINO下我试过，不用融合直接可以得到角度（因为它内部集成了加速度传感器的），用在NXC时，我也是直接去零飘后用PD控制，能运行，但不稳定！

至于本贴中糖大提高的算法，我用过的二个数字陀螺，滞后明显，无法应于到倒立摆的！

没钱买乐高

挖坟学习，但是看不太懂啊

li001kai

没钱买乐高 发表于 2019-6-5 10:34
挖坟学习，但是看不太懂啊

一起挖。。

水煮鱼丶

挖个坑