倒立振子ロボット ESP8266 Kalmanフィルタ

2019年に製作した倒立振子ロボットについて記事にしました。MPU-6050 6軸センサをカルマンフィルタによるPID制御しました。

 動画

参考サイト

●セグウェイを自作してみた

 【https://qiita.com/sasaco/items/b72dec0edc16d19af6e4】

ブレッドボード

●ESP-WROOM-02開発ボード

スイッチサイエンス（技適製品）

●MPU-6050 ３軸ジャイロスコープ・３軸加速度センサー モジュール

●DRV8833 DC モータ ドライバ モジュール 

モータ・バッテリー

モータ・バッテリーホルダーのフレームは3Dプリンターで作成しました。

●DC 6V 0.45A 200RPM トルクギア付きギアボックス

●ゴムタイヤ　ホイール　N20 DCギアモーター

●18650充電式電池（１本未使用）

●18650電池バッテリーホルダーケース

回路図

シリアルモニター

シリアルプロッタ

青-degY　赤-gPowerP　緑-gPowerI　橙-gPowerD　紫-power

Arduino IDE設定

プログラム

同じフォルダにプログラムとカルマンフィルタを置く

KalmanFilter.cpp

// カルマンフィルタ。

// 2016/3/3

//

// 計算式の参考資料。

// http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it/

#include "KalmanFilter.h"

KalmanFilter::KalmanFilter() {

  angle = 0.0;

  bias = 0.0;

  P[0][0] = 0.0;

  P[0][1] = 0.0;

  P[1][0] = 0.0;

  P[1][1] = 0.0;

};

void KalmanFilter::setAngle(float newAngle) {

  angle = newAngle;

};

float KalmanFilter::calcAngle(float newAngle, float newRate, float dt) {

  // variances

  float Q_angle = 0.001;

  float Q_bias = 0.003;

  float R_measure = 0.03;

  // step 1

  float rate = newRate - bias;

  angle += dt * rate;

  // step 2

  P[0][0] += dt * (dt * P[1][1] - P[0][1] - P[1][0] + Q_angle);

  P[0][1] -= dt * P[1][1];

  P[1][0] -= dt * P[1][1];

  P[1][1] += Q_bias * dt;

  // step 3

  float y = newAngle - angle;

  // step 4

  float S = P[0][0] + R_measure;

  // step 5

  float K[2];

  K[0] = P[0][0] / S;

  K[1] = P[1][0] / S;

  // step 6

  angle += K[0] * y;

  bias += K[1] * y;

  // step 7

  float P00_temp = P[0][0];

  float P01_temp = P[0][1];

  P[0][0] -= K[0] * P00_temp;

  P[0][1] -= K[0] * P01_temp;

  P[1][0] -= K[1] * P00_temp;

  P[1][1] -= K[1] * P01_temp;

  return angle;

};

KalmanFilter.h

// カルマンフィルタ。

// 2016/3/3

class KalmanFilter {

private:

    float angle;

    float bias;

    float P[2][2];

public:

    KalmanFilter();

    void setAngle(float newAngle);

    float calcAngle(float newAngle, float newRate, float dt);

};

メインプログラム

#include <Wire.h>

#include "KalmanFilter.h"

#include "I2Cdev.h"

#include "MPU6050.h"

MPU6050 accelgyro;

int16_t ax, ay, az; // 加速度

int16_t gx, gy, gz; // 角速度

#define I2C_SDA  4

#define I2C_CLK  5

// モータードライバの制御定数。

#define AIN1 12

#define AIN2 13

#define AIN3 14

#define AIN4 15

#define PWN_FREQ 1000 // PWM frequency: 1000Hz(1kHz)

#define PWN_RANGE 100 // PWN range: 100

#define V_MIN 50// モーター駆動電圧(0～255)の最低値。これより低いと回らない。

#define V_MAX 100 // モーター駆動電圧(0～255)の最高値。

// 加速度/ジャイロセンサーの制御変数。

KalmanFilter gKfx, gKfy; // カルマンフィルタ。

float gCalibrateY; // 初期化時の角度。（＝静止角とみなす）

long gPrevMicros; // loop()間隔の計測用。

// 倒立振子の制御変数。

float gPowerP, gPowerI, gPowerD; // 現在出力値とPID成分。

void setup() {

  Wire.begin();

  accelgyro.initialize();

  Serial.begin(115200);

  pinMode(AIN1,OUTPUT);

  pinMode(AIN2,OUTPUT); 

  pinMode(AIN3,OUTPUT);

  pinMode(AIN4,OUTPUT); 

  delay(2000);

  accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

  float degRoll  = atan2(ay, az) * RAD_TO_DEG;

  float degPitch = atan(-ax / sqrt(ay * ay + az * az)) * RAD_TO_DEG;

  gKfx.setAngle(degRoll);

  gKfy.setAngle(degPitch);

  gCalibrateY = degPitch;

  gPrevMicros = micros();

  analogWriteFreq(PWN_FREQ);

  analogWriteRange(PWN_RANGE);

}

void loop() {

  accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

  float degRoll  = atan2(ay, az) * RAD_TO_DEG;

  float degPitch = atan(-ax / sqrt(ay * ay + az * az)) * RAD_TO_DEG;

  // 角速度値を分解能で割って角速度に変換する

  float dpsX = gx / 131.0; 

  float dpsY = gy / 131.0;

  float dpsZ = gz / 131.0;

  // カルマンフィルタで角度(x,y)を計算する。

  long curMicros = micros();

  float dt = (float)(curMicros - gPrevMicros) / 1000000; // μsec -> sec

  gPrevMicros = curMicros;

  float degX = gKfx.calcAngle(degRoll, dpsX, dt);

  float degY = gKfy.calcAngle(degPitch, dpsY, dt);

  degY -= gCalibrateY;

  // PID制御でモーター出力を計算。

  gPowerP = degY / 90;    // P成分：傾き-90～90度 → -1～1

  gPowerI += gPowerP;     // I成分：傾きの積算。

  gPowerD = dpsY / 250;   // D成分：角速度-250～250dps → -1～1

  float power = gPowerP * 17.0 + // 17この数字は試行錯誤で調整。

                gPowerI *  1.5 +

                gPowerD *  2.0;

  power = max((float) -1, min((float)1, power)); // → -1～1

  float powerL = power;

  float powerR = power;

  // 方向転換させる場合はここでpowerL, Rに差分を与える。

  // powerをモーター駆動電圧に変換。0～1 → V_MIN～V_MAX

  int voltL = (int)((V_MAX - V_MIN) * abs(powerL) + V_MIN);

  int voltR = (int)((V_MAX - V_MIN) * abs(powerR) + V_MIN);

    //シリアルモニター     

  //Serial.print("degY:"); Serial.print(degY);Serial.print("｜ ");

  //Serial.print("gPowerP:"); Serial.print(gPowerP);Serial.print("｜ ");

  //Serial.print("gPowerI:"); Serial.print(gPowerI);Serial.print("｜ ");

  //Serial.print("gPowerD:"); Serial.print(gPowerD);Serial.print("｜ "); 

  //Serial.print("power:"); Serial.print(power);Serial.print("｜ ");

  //Serial.println("");

   //シリアルプロッタ

  Serial.print(degY); Serial.print(","); 

  Serial.print(gPowerP); Serial.print(","); 

  Serial.print(gPowerI); Serial.print(","); 

  Serial.print(gPowerD); Serial.print(","); 

  Serial.println(power); 

  

  

  // モーター駆動。

  if(0 <= powerL){

    analogWrite(AIN3, voltL);

    digitalWrite(AIN4, LOW );

  }

  else{

    analogWrite(AIN4, voltL);

    digitalWrite(AIN3, LOW );

  }

 if(0 <= powerR){

    analogWrite(AIN1, voltL);

    digitalWrite(AIN2, LOW );

  }

  else{

    analogWrite(AIN2, voltL);

    digitalWrite(AIN1, LOW );

  } 

  

  if (30 < abs(degY)) {

    analogWrite(AIN1, 0);

    analogWrite(AIN2, 0);

    analogWrite(AIN3, 0);

    analogWrite(AIN4, 0);

    

    Serial.print("**** stop ");

    Serial.print(degY);

    while (1) delay(1000);

  }

  delay(1);

}