角速度から傾斜角を計算する



Arduino UNO

Arduinoを使ってみる

最終更新日:2014/12/30

ここまでで角速度を求めることが出来ました。次に角速度から傾斜角を計算します。 計算の仕方は"角速度を積分する"です。なので、センサから得られる角加速度から最終的に2回積分をすることになります。
==================================================
#include <MsTimer2.h>

unsigned long   time, time_old, time3, time4, g1_init;
float                 deg_g1, g1, g1_lrn, i, deg_s_g1, deg_ss_g1;
boolean            xlearn;
unsigned int      count = 0;

void setup(){
    Serial.begin(38400) ; // シリアルモニターの初期化
    MsTimer2::set(500, learn); // 500ms period
    MsTimer2::start();
}

// 角速度センサ初期値学習
void learn() {
    i = i + 1;
    g1_lrn = g1_lrn + analogRead(1);
    if (i > 5){
        g1_init = g1_lrn / i;
        xlearn = true;
        MsTimer2::stop();
    }
}

void loop(){
// delay時間算出
    time       = micros();
    time3     = time - time_old - time4; // 前回の遅れ時間
    time_old = time;                            // 次回計算用として、今回までの演算時間を格納
    time4     = 10000 - time3;                  // 今回のdelay時間を決定

// 角速度算出
    g1 =0;
    for (i2 = 0 ; i2 < 10 ; i2++) { g1 = g1 + analogRead(1);}
    g1 = g1 / 10;

    if (abs(g1 - g1_init) <= 3) {
        g1 = g1_init;
        count = count + 1;
    } else {count = 0;}

        deg_g1 = (g1 - g1_init) / 1023 * 5 / 0.00067; // G1角速度

    if (xlearn == true){
        if (count > 20){ deg_s_g1 = 0;}
        else                { deg_s_g1 = deg_s_g1 + deg_g1 / 100;}
        deg_ss_g1 = deg_ss_g1 + deg_s_g1 / 100;
    }
// PCに送信
    Serial.print(time);
    Serial.print(" ");
    Serial.print(deg_g1) ; // G1角加速度
    Serial.print(" ") ;
    Serial.print(deg_s_g1) ; // G1角速度
    Serial.print(" ") ;
    Serial.print(deg_ss_g1) ; // G1傾斜角


    delayMicroseconds(time4);
}
==================================================

①作動結果
結果は以下のとおりになります。それらしくなってはいますが、傾斜角についても例の積算現象が出てしまっています。


②積算値をリセットする
倒立振子で使用するという前提として、角速度が出ていない状態は傾斜角は安定して保たれているという考えのもと、 角速度が0付近で且つ傾斜角も0付近にいたら、傾斜角を強制的に0にするという仕様を加えます。 ==================================================
#include <MsTimer2.h>

unsigned long   time, time_old, time3, time4, g1_init;
float                 deg_g1, g1, g1_lrn, i, deg_s_g1, deg_ss_g1;
boolean            xlearn;
unsigned int      count = 0;
unsigned int      count2 = 0;

void setup(){
    Serial.begin(38400) ; // シリアルモニターの初期化
    MsTimer2::set(500, learn); // 500ms period
    MsTimer2::start();
}

// 角速度センサ初期値学習
void learn() {
    i = i + 1;
    g1_lrn = g1_lrn + analogRead(1);
    if (i > 5){
        g1_init = g1_lrn / i;
        xlearn = true;
        MsTimer2::stop();
    }
}

void loop(){
// delay時間算出
    time       = micros();
    time3     = time - time_old - time4; // 前回の遅れ時間
    time_old = time;                            // 次回計算用として、今回までの演算時間を格納
    time4     = 10000 - time3;                  // 今回のdelay時間を決定

// 角加速度算出
    g1 =0;
    for (i2 = 0 ; i2 < 10 ; i2++) { g1 = g1 + analogRead(1);}
    g1 = g1 / 10;

    if (abs(g1 - g1_init) <= 3) {
        g1 = g1_init;
        count = count + 1;
    } else {count = 0;}

        deg_g1 = (g1 - g1_init) / 1023 * 5 / 0.00067; // G1角速度

// 角速度算出
    if (xlearn == true){
        if (count > 20){ deg_s_g1 = 0;}
        else                { deg_s_g1 = deg_s_g1 + deg_g1 / 100;}

// 傾斜角算出
    if (abs(deg_s_g1) <= 3) { count2 = count2 + 1;}
    else {count2 =0;}
    if ((count > 20 || count2 > 20) && abs(deg_ss_g1) < 12){ deg_ss_g1 = 0;}
    else {deg_ss_g1 = deg_ss_g1 + deg_s_g1 / 100;}

    }
// PCに送信
    Serial.print(time);
    Serial.print(" ");
    Serial.print(deg_g1) ; // G1角加速度
    Serial.print(" ") ;
    Serial.print(deg_s_g1) ; // G1角速度
    Serial.print(" ") ;
    Serial.print(deg_ss_g1) ; // G1傾斜角

    delayMicroseconds(time4);
}
==================================================

③作動結果
結果は以下のとおりになります。対処療法的ですが積算現象を一応は抑えることが出来ました










サブチャンネルあります。⇒ 何かのお役に立てればと

関連記事一覧



Arduino UNO

Arduinoを使ってみる