L3G4200D 陀螺儀積分問題

ga742112

各位前輩好  
小弟因剛接觸四軸飛行器有些問題想請教,目前所使用的感測器為adxl345 l3g4200d

目前已經可以從陀螺儀得到角速度 , 想請教的是接下來該怎麼去積分才能得我要的角度值呢??


因剛接觸,所以程式碼沒有什麼概念 ,望前輩多指教!!下面是我目前的程式碼





#include <Wire.h>
#include <L3G.h>

L3G gyro;

void setup() {
  Serial.begin(9600);
  Wire.begin();

  if (!gyro.init())
  {
    Serial.println("Failed to autodetect gyro type!");
    while (1);
  }

  gyro.enableDefault();
}

void loop() {
  gyro.read();


  float dpsgyrox;
  float dpsgyroy;
  float dpsgyroz;

dpsgyrox  = ( gyro.g.x * 8.75 ) / 1000;
dpsgyroy  = ( gyro.g.y * 8.75 ) / 1000;
dpsgyroz  = ( gyro.g.z * 8.75 ) / 1000;



  Serial.print("G ");
  Serial.print("X: ");
  Serial.print(dpsgyrox);
  Serial.print(" Y: ");
  Serial.print(dpsgyroy);
  Serial.print(" Z: ");
  Serial.println(dpsgyroz);

  delay(100);
}

超新手

dpsgyrox  的單位是 度/秒
也就是 一秒轉了多少度
把量測時間乘以角速度, 就是轉了多少角度...
所謂積分, 就是加法而已
不過, 一般在計算角度時
會使用加速度計和陀螺儀. 使用 KALMAN FILTER 或 complementary FILTER去計算
這些範例應該很好找

ga742112

回復 2# 超新手


#include <Wire.h>
#include <L3G.h>


unsigned long nowtime, lasttime, dt;
L3G gyro;


void setup() {
  Serial.begin(9600);
  Wire.begin();


  if (!gyro.init())
  {
    Serial.println("Failed to autodetect gyro type!");
    while (1);
  }
  gyro.enableDefault();
  }


void loop() {


  nowtime=millis();
  dt = nowtime - lasttime;
  lasttime = nowtime;


  gyro.read();
  float dpsgyrox;
  float dpsgyroy;
  float dpsgyroz;
  float anglex;
  float angley;
  float anglez;


dpsgyrox  = ( gyro.g.x * 8.75 ) / 1000;
dpsgyroy  = ( gyro.g.y * 8.75 ) / 1000;
dpsgyroz  = ( gyro.g.z * 8.75 ) / 1000;


anglex = anglex + dpsgyrox * dt / 1000;
angley = angley + dpsgyroy * dt / 1000;
anglez = anglez + dpsgyroz * dt / 1000;



  Serial.print("G ");
  Serial.print("X: ");
  Serial.print(anglex);
  Serial.print(" Y: ");
  Serial.print(angley);
  Serial.print(" Z: ");
  Serial.println(anglez);


  delay(100);
}


加入積分後  出來的值似乎還是角速度   請問這是程式上哪邊出錯了呢??

超新手

把 loop() 中的下面三行宣告
float anglex;
  float angley;
  float anglez;
改移到 setup 上面去宣告, 如下

unsigned long nowtime, lasttime, dt;
L3G gyro;

float anglex;
  float angley;
  float anglez;
void setup() {
.................................

nowtime=millis();
另外, lasttime 的初始值沒處理好

ga742112

回復 4# 超新手

   關於lasttime 這部分是哪邊沒處理好呢??

ga742112

因為出來的值 從0開始會慢慢遞增...... 請問程式上出了什麼問題嗎?

超新手

1. lasttime 因為沒給初始值, 所以第一次計算出來的dt就會有一點誤差,
雖然不大
2. 因為是用面積法求積分, 所以dt 要越小越好,
但你為了顯示, 加了一個 0.1 秒delay, 誤差自然會大,
可以把 delay 拿掉, 然後循環個幾百次再顯示一次,
3. 就算如此, 但是還是不可避免誤差, 雜訊, 未歸零...等問題造成積分會爆掉的問題,
所以就會用一些 filter 來解決這些問題

ga742112

我lasttime初始值應該要怎麼設定??
出來的角速度值才能換成角度呢??

超新手

我lasttime初始值應該要怎麼設定??

比較簡單的做法, 就是在 SETUP 最後一行加上
lasttime = millis();

出來的角速度值才能換成角度呢??

現在印出來的, 不是角度嗎?

ga742112

回復 9# 超新手


可以出來是角度了
但照您的方法後,靜止時一樣會慢慢遞增

#include <Wire.h>
#include <L3G.h>

float anglex;
float angley;
float anglez;
float dpsgyrox;
float dpsgyroy;
float dpsgyroz;
unsigned long nowtime, lasttime, dt;

L3G gyro;

void setup() {
  Serial.begin(9600);
  Wire.begin();

  if (!gyro.init())
  {
    Serial.println("Failed to autodetect gyro type!");
    while (1);
  }
  gyro.enableDefault();
  lasttime = millis();
  }

void loop() {

  nowtime=millis();
  dt = nowtime - lasttime;
  lasttime = nowtime;

  gyro.read();

dpsgyrox  = ((( (gyro.g.x) * 8.75 ) / 1000) );
dpsgyroy  = ((( (gyro.g.y) * 8.75 ) / 1000) );
dpsgyroz  = ((( (gyro.g.z) * 8.75 ) / 1000) );


anglex = anglex + dpsgyrox * dt / 1000;
angley = angley + dpsgyroy * dt / 1000;
anglez = anglez + dpsgyroz * dt / 1000;

  Serial.print("G ");
  Serial.print("X: ");
  Serial.print(anglex);
  Serial.print(" Y: ");
  Serial.print(angley);
  Serial.print(" Z: ");
  Serial.println(anglez);

}

超新手

不管怎麼做都很難避免, 只會減輕
所以我一開始才說, 必須使用FILTER 去解決積分問題

另外, 以上的程式還是有改善空間(但不能完全解決)
如把 dpsgyrox , anglex 宣告成 unsigned long
......
anglex = anglex + dpsgyrox * dt;
....
處理時用整數, 然後顯示時才乘以 8.75 /1000/1000
然後不要一直顯示, 要循環幾百次再顯示
把 dt 變小, 才能減輕

如果你要做四旋翼, 使用加速度計和陀螺儀配合 filter
去算角度, 才是常見做法