Robofun 機器人論壇

標題: L3G4200D 陀螺儀積分問題 [打印本頁]

作者: ga742112 時間: 2016-3-14 17:24
標題: L3G4200D 陀螺儀積分問題
各位前輩好
小弟因剛接觸四軸飛行器有些問題想請教,目前所使用的感測器為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);
}

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

作者: ga742112 時間: 2016-3-15 14:48
回復 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);
}

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

作者: 超新手 時間: 2016-3-15 15:37
本帖最後由超新手於 2016-3-15 15:47 編輯

把 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 時間: 2016-3-15 17:18
回復 4# 超新手

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

作者: ga742112 時間: 2016-3-16 00:31
因為出來的值從0開始會慢慢遞增...... 請問程式上出了什麼問題嗎?

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

作者: ga742112 時間: 2016-3-16 17:10
我lasttime初始值應該要怎麼設定??
出來的角速度值才能換成角度呢??

作者: 超新手 時間: 2016-3-17 08:21

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

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

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

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

作者: ga742112 時間: 2016-3-17 12:17
回復 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);

}

作者: 超新手 時間: 2016-3-17 14:49
不管怎麼做都很難避免, 只會減輕
所以我一開始才說, 必須使用FILTER 去解決積分問題

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

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

歡迎光臨 Robofun 機器人論壇 (https://robofun.net/forum/)