さて前回まででカルマン・フィルタを使用したM5StickC傾斜計を作りました。
いよいよM5StickCによる倒立振子を作っていきます。

この道シリーズではトランジスタ技術 2019年 7月号を参考に倒立振子の実現を目指しています。

トラ技の倒立振子はカルマンでの傾斜推測に加えてロータリエンコーダでタイヤの角度、角速度検出して倒立振子自体のモデルで更にカルマン・フィルタかまして安定度の高い倒立振子を実現しています。モータへの電圧供給フィードバックもなんか難しい関数で算出されています。

しかし、今回はM5SickCを用いたカジュアルな倒立振子の実現を目指します。ロータリエンコーダは使わず、さらにモータへの供給電圧は難しい関数は使用せずに馴染みのあるPID制御で算出しました。

まずは作ってみた

とりあえず手を動かさないことには始まらないということで、倒立振子を作ってみた。

傾斜角だけカルマン・フィルタかまして倒立振子してみた。
モータ供給電圧などは完全にマニュアルチューニングw 全然立たないww チューニングのせいなのか、I2Cモータドライバの応答のせいなのか、出力が0.48Vからしかでないからなのか、もっと根本なのか。道は次に続けよ#m5stickc #倒立振子への道 pic.twitter.com/5MeuNqCT7S

— HomeMadeGarbage (@H0meMadeGarbage) 2019年6月24日

やっぱ何も考えずに適当につくった倒立振子はどうあがいても全然倒立しませんでした。。。

頑張って改良した

いろいろ工夫してついに立ちました！！見てください！

立ったー！ #トラ技の難しい振子モデルは勉強中なので、とりあえずなじみ深いPID制御で供給電圧を算出しました。係数はBluetoothで送信しながら力業の手動チューニング。。。疲れた。。。
さてとりあえず立つ振子ができた！がぜん勉強する気になったぜ#m5stickc #机小汚い #Blynk #倒立振子への道 pic.twitter.com/feZDKGtBxR

— HomeMadeGarbage (@H0meMadeGarbage) 2019年7月1日

立った！立った！立ったー！やりぃー！キャッホー！！うれCY！！

さていかにして、この倒立振子が実現されたかを以下に記します。

構成

I2CモータドライバDRV8830をM5StickCで制御してモータを駆動します。

部品

M5StickC

　
I2Cモータードライバ・モジュール DRV8830

I2Cモータードライバ・モジュール DRV8830 - DRV8830 - ネット販売
strawberry-linux.com

　
ギアドモータ

　
スライドスイッチ

　
電池ボックス　単３×１本

　
電池ボックス　単３×２本

マシン

以下が自慢のマシン詳細です。

　
いろいろ試して気づいたのですがマシン自体のバランスが非常に重要です(あたりまえか)。底面に鉄板貼り付けてバランス調整しています。

PID制御

今回作成した倒立振子ではM5StickCの傾きθと角速度ω ($\dot{θ}$)をカルマン・フィルタをかまして推定値を算出して、その値を元にモータ供給電圧を決定して姿勢を床に対して垂直に保つようにします。

ここではPID制御を用いて供給電圧を決定します。

PはProportional(比例)、IはIntegral(積分)、DはDifferential(微分)です。

$$操作量 = Kp × 誤差 + Ki × 誤差の積算値 + Kd × (前回の誤差と今回の誤差の差)$$

上式でモータ供給電圧を決定します。誤差は目標値と現在の傾きの差で今回は垂直(0°)を目指しますので誤差は傾きθ(θ – 0°)となりますので

$$モータ供給電圧 = Kp × θ + Ki × \sum{θ} + Kd × ω$$

係数Kp, Ki, Kdを調整して、過度な供給や供給不足のない適切な電圧の供給を実現します。今回はBluetoothでM5StickCと通信してスマホで係数を送れるようして地道に手動で調整しました。

参考

6軸センサ SH200Q調整

比較的高速で繊細な観測が要求されますのでM5StickCに搭載されている6軸センサ SH200Qの設定値を調整しました。

SH200Q用のクラスを修正して以下を変更しました。

加速度センサ出力周期：256Hz → 1024Hz
ジャイロセンサ出力周期：500Hz → 1000Hz
ジャイロセンサLPF遮断周波数：50Hz → 250Hz
加速度センサレンジ：±8G → ±4G
ジャイロセンサレンジ：±2000°/sec → ±250°/sec

また出力レンジの変更に伴いヘッダファイルも修正しました。

//uint8_t Gscale = GFS_2000DPS;
//uint8_t Ascale = AFS_8G;

uint8_t Gscale = GFS_25DPS;
uint8_t Ascale = AFS_4G;

//uint8_t Gscale = GFS_2000DPS;

//uint8_t Ascale = AFS_8G;

uint8_t Gscale = GFS_25DPS;

uint8_t Ascale = AFS_4G;

Blynk設定

PID制御用の係数Kp, Ki, Kdとモータ供給電圧の最小値Vminをスマホから送れるようにします。

スマホとM5StickCはスマホアプリのBlynkを用いてBluetooth通信させます。Blynkアプリのバージョンは2.27.6です。

新規プロジェクトを作成します。HARDWRE MODELはESP32 Dev Boardを選択。CONNECTION TYPEはBluetoothを選択。AUTH TOKENはArduinoコード生成時に使用します(アカウント登録したメールに送信されます)。

ウィジェットとしてBluetoothとNumeric Inputウェジットを4つ配置します。Numeric Inputウェジットは±ボタンを押して値を増減させて送信できます。
　

Numeric Inputウェジットの設定は最小値と最大値、ステップ数を指定してヴァーチャルピンに出力させます。以下のように設定しました。

Kp：-1000~1000、0.1ステップ、出力ヴァーチャルピンV0
Ki：-1000~1000、0.001ステップ、出力ヴァーチャルピンV1
Kd：-1000~1000、0.01ステップ、出力ヴァーチャルピンV2
Vmin：0~255、1ステップ、出力ヴァーチャルピンV3

I2Cモータードライバ DRV8830

DRV8830はI2C入力によって、モータ供給電圧(スピード)と供給電圧方向(回転方向)を制御するものです。つまり正負の電圧を生成できるということです。

I2Cアドレス設定

本モジュールは基板上のジャンパA1, A0のステートによって以下の表のようにアドレスを指定することができます。

表のアドレス末尾xは読み込み時には1、書き込み時は0をしてします。ここでは書き込みのみ使用します。両方Openのアドレス：0x64としました。

書き込みI2Cデータ

アドレス0x00に8bitの情報を書き込みます。
各ビットの設定は以下の通り。上位6ビットで電圧を設定し、下位2ビットで正負を指定できます。

Arduino IDEプログラム

以下のBlynkのArduino用ライブラリを使用してプログラムしました。バージョンは0.6.1。
　https://github.com/blynkkk/blynk-library

ここではBluetoothを使用してM5StickCと通信します。ライブラリのコード例のESP32_BT.ino を参考にプログラムしました。

カルマン・フィルタによる傾斜角と角速度の誤差の推定値を元にPID制御によってモータに電圧を供給して姿勢を保ちます。

タイマ割り込み(2.5msec毎)でカルマン・フィルタ介して傾斜角と角速度の誤差を算出します。角速度に角速度の誤差を足して角速度の推定値としています。

10msec毎に供給電圧を計算してモータ駆動します。

初期設定時にセンサの測定値のオフセットを測って差し引くようにしてます。

#define BLYNK_PRINT Serial
#define BLYNK_USE_DIRECT_CONNECT

#include <BlynkSimpleEsp32_BT.h>
#include <M5StickC.h>
#include <Ticker.h>
#include <Wire.h>

char auth[] = "BlynkアプリのYourAuthTokenを入力";

int  VMIN = 1;
float Kp = 0.4;
float Ki = 0.1;
float Kd = 0.2;


float power = 0.0;

float thetaP, thetaI, thetaD;

#define WAIT 10
#define VMAX 63

int16_t accX = 0;
int16_t accY = 0;
int16_t accZ = 0;

int16_t gyroX = 0;
int16_t gyroY = 0;
int16_t gyroZ = 0;

//加速度センサ オフセット
int accXoffset = 0;
int accYoffset = 0;
int accZoffset = 0;

//ジャイロセンサ オフセット
int gyroXoffset = 0;
int gyroYoffset = 0;
int gyroZoffset = 0;

Ticker tickerUpdate;


//センサ バラつき取得用変数
int sample_num = 100;
int meas_interval = 10;
float theta_deg = 0.0;
float theta_dot = 0.0;
float theta_dot2;

float theta_mean;
float theta_variance;
float theta_dot_mean;
float theta_dot_variance;


//=========================================================
//カルマン・フィルタ用変数
//=========================================================

//カルマン・フィルタ処理 
float theta_update_freq = 400; //Hz
float theta_update_interval = 1.0/double(theta_update_freq); //2.5msec
//状態ベクトルx
//[[theta(degree)], [offset of theta_dot(degree/sec)]]
//状態ベクトルの予測値
float theta_data_predict[2][1];
float theta_data[2][1];
//共分散行列
float P_theta_predict[2][2];
float P_theta[2][2];
//状態方程式の"A"
float A_theta[2][2] = {{1, -theta_update_interval}, {0, 1}};
//状態方程式の"B"
float B_theta[2][1] = {{theta_update_interval}, {0}};
//出力方程式の"C"
float C_theta[1][2] = {{1, 0}};


//=========================================================
// 行列演算関数
//=========================================================

//行列の和
void mat_add(float *m1, float *m2, float *sol, int row, int column) {
    for(int i=0; i<row; i++) {
        for(int j=0; j<column; j++) {
            sol[i*column + j] = m1[i*column + j] + m2[i*column + j];    
        }    
    }
    return;
}

//行列の差
void mat_sub(float *m1, float *m2, float *sol, int row, int column){
    for(int i=0; i<row; i++) {
        for(int j=0; j<column; j++) {
            sol[i*column + j] = m1[i*column + j] - m2[i*column + j];    
        }    
    }
    return;
}

//行列の積
void mat_mul(float *m1, float *m2, float *sol, int row1, int column1, int row2, int column2){
    for(int i=0; i<row1; i++){
        for(int j=0; j<column2; j++){
            sol[i*column2 + j] = 0;
            for(int k=0; k<column1; k++) {
                sol[i*column2 + j] += m1[i*column1 + k]*m2[k*column2 + j];    
            }
        }    
    }
    return;
}

//転置行列算出
void mat_tran(float *m1, float *sol, int row_original, int column_original) {
    for(int i=0; i<row_original; i++) {
        for(int j=0; j<column_original; j++) {
            sol[j*row_original + i] = m1[i*column_original + j];    
        }    
    }
    return;
}

//行列の定数倍算出
void mat_mul_const(float *m1,float c, float *sol, int row, int column){
    for(int i=0; i<row; i++){
        for(int j=0; j<column; j++){
            sol[i*column + j] = c * m1[i*column + j];    
        }    
    }
    return;
}

//逆行列算出
void mat_inv(float *m, float *sol, int column, int row){
    //allocate memory for a temporary matrix
    float* temp = (float *)malloc( column*2*row*sizeof(float) );
    
    //make the augmented matrix
    for(int i=0; i<column; i++) {
        //copy original matrix
        for(int j=0; j<row; j++) {
            temp[i*(2*row) + j] = m[i*row + j];  
        }
        
        //make identity matrix
        for(int j=row; j<row*2; j++) {
            if(j-row == i) {
                temp[i*(2*row) + j] = 1;
            }    
            else {
                temp[i*(2*row) + j] = 0;    
            }
        }
    }

    //Sweep (down)
    for(int i=0; i<column; i++) {
        //pivot selection
        float pivot = temp[i*(2*row) + i];
        int pivot_index = i;
        float pivot_temp;
        for(int j=i; j<column;j++) {
            if( temp[j*(2*row)+i] > pivot ) {
                pivot = temp[j*(2*row) + i];
                pivot_index = j;
            }    
        }  
        if(pivot_index != i) {
            for(int j=0; j<2*row; j++) {
                pivot_temp = temp[ pivot_index * (2*row) + j ];
                temp[pivot_index * (2*row) + j] = temp[i*(2*row) + j];
                temp[i*(2*row) + j] = pivot_temp;    
            }    
        }
        
        //division
        for(int j=0; j<2*row; j++) {
            temp[i*(2*row) + j] /= pivot;    
        }
        
        //sweep
        for(int j=i+1; j<column; j++) {
            float temp2 = temp[j*(2*row) + i];
            
            //sweep each row
            for(int k=0; k<row*2; k++) {
                temp[j*(2*row) + k] -= temp2 * temp[ i*(2*row) + k ];    
            }    
        }
    }
        
    //Sweep (up)
    for(int i=0; i<column-1; i++) {
        for(int j=i+1; j<column; j++) {
            float pivot = temp[ (column-1-j)*(2*row) + (row-1-i)];   
            for(int k=0; k<2*row; k++) {
                temp[(column-1-j)*(2*row)+k] -= pivot * temp[(column-1-i)*(2*row)+k];    
            }
        }    
    }     
    
    //copy result
    for(int i=0; i<column; i++) {
        for(int j=0; j<row; j++) {
            sol[i*row + j] = temp[i*(2*row) + (j+row)];    
        }    
    }
    free(temp);
    return;
}


//センサオフセット算出
void offset_cal(){
  delay(1000);
  accXoffset = 0;
  accYoffset = 0;
  accZoffset = 0;
  gyroXoffset = 0;
  gyroYoffset = 0;
  gyroZoffset = 0;

  for(int i=0; i<10; i++) {
    M5.IMU.getAccelAdc(&accX,&accY,&accZ);
    M5.IMU.getGyroAdc(&gyroX,&gyroY,&gyroZ);
    delay(meas_interval);
    accXoffset += accX;
    accYoffset += accY;
    accZoffset += accZ;
    gyroXoffset += gyroX;
    gyroYoffset += gyroY;
    gyroZoffset += gyroZ;
  }

  accXoffset /= 10;
  accYoffset = accYoffset / 10 + 8192;
  accZoffset /= 10;
  gyroXoffset /= 10;
  gyroYoffset /= 10;
  gyroZoffset /= 10;
}


//加速度センサから傾きデータ取得 [deg]
float get_acc_data() {
  M5.IMU.getAccelAdc(&accX,&accY,&accZ);
  //得られたセンサ値はオフセット引いて使用
  //傾斜角導出 単位はdeg
  theta_deg  = atan( (float)(accZ - accZoffset) / (float)(-1 * accY - accYoffset) ) * 57.29578f;
  return theta_deg;
}


//加速度センサによる傾きのばらつき測定
void acc_init(){
  float theta_array[sample_num];
  for(int i=0; i<sample_num; i++) {
    theta_array[i] = get_acc_data();
    delay(meas_interval);
  }

  //平均値
  theta_mean = 0;
  for(int i=0; i<sample_num; i++) {
    theta_mean += theta_array[i];
  }
  theta_mean /= sample_num;
    
  //分散
  float temp;
  theta_variance = 0;
  for(int i=0; i<sample_num; i++) {
    temp = theta_array[i] - theta_mean;
    theta_variance += temp*temp;
  }
  theta_variance /= sample_num;
}


//x軸 角速度取得
float get_gyro_data() {
  M5.IMU.getGyroAdc(&gyroX,&gyroY,&gyroZ);
  //得られたセンサ値はオフセット引いて使用
  theta_dot = ((float) (gyroX - gyroXoffset)) * M5.IMU.gRes;
  return theta_dot;
}

//ジャイロセンサばらつき測定
void gyro_init() {
  float theta_dot_array[sample_num];
  for(int i=0;i<sample_num;i++) {
    theta_dot_array[i] =  get_gyro_data();
    delay(meas_interval);
  }
    
  //平均値
  theta_dot_mean = 0;
  for(int i=0;i<sample_num;i++) {
    theta_dot_mean += theta_dot_array[i];    
  }
  theta_dot_mean /= sample_num;
 
  //分散
  float temp;
  theta_dot_variance = 0;
  for(int i=0; i<sample_num; i++) {
    temp = theta_dot_array[i] - theta_dot_mean;
    theta_dot_variance += temp*temp;    
  }
  theta_dot_variance /= sample_num;
}


//=========================================================
//カルマン・フィルタアルゴリズム処理
//=========================================================
void update_theta()
{     
    //加速度センサによる角度測定
    float y = get_acc_data(); //degree
    
    //入力データ：角速度
    float theta_dot_gyro = get_gyro_data(); //degree/sec
      
    //カルマン・ゲイン算出: G = P'C^T(W+CP'C^T)^-1
    float P_CT[2][1] = {};
    float tran_C_theta[2][1] = {};
    mat_tran(C_theta[0], tran_C_theta[0], 1, 2);//C^T
    mat_mul(P_theta_predict[0], tran_C_theta[0], P_CT[0], 2, 2, 2, 1);//P'C^T
    float G_temp1[1][1] = {};
    mat_mul(C_theta[0], P_CT[0], G_temp1[0], 1,2, 2,1);//CP'C^T
    float G_temp2 = 1.0f / (G_temp1[0][0] + theta_variance);//(W+CP'C^T)^-1
    float G1[2][1] = {};
    mat_mul_const(P_CT[0], G_temp2, G1[0], 2, 1);//P'C^T(W+CP'C^T)^-1
    
    //傾斜角推定値算出: theta = theta'+G(y-Ctheta')
    float C_theta_theta[1][1] = {};
    mat_mul(C_theta[0], theta_data_predict[0], C_theta_theta[0], 1, 2, 2, 1);//Ctheta'
    float delta_y = y - C_theta_theta[0][0];//y-Ctheta'
    float delta_theta[2][1] = {};
    mat_mul_const(G1[0], delta_y, delta_theta[0], 2, 1);
    mat_add(theta_data_predict[0], delta_theta[0], theta_data[0], 2, 1);
           
    //共分散行列算出: P=(I-GC)P'
    float GC[2][2] = {};
    float I2[2][2] = {{1,0},{0,1}};
    mat_mul(G1[0], C_theta[0], GC[0], 2, 1, 1, 2);//GC
    float I2_GC[2][2] = {};
    mat_sub(I2[0], GC[0], I2_GC[0], 2, 2);//I-GC
    mat_mul(I2_GC[0], P_theta_predict[0], P_theta[0], 2, 2, 2, 2);//(I-GC)P'
      
    //次時刻の傾斜角の予測値算出: theta'=Atheta+Bu
    float A_theta_theta[2][1] = {};
    float B_theta_dot[2][1] = {};
    mat_mul(A_theta[0], theta_data[0], A_theta_theta[0], 2, 2, 2, 1);//Atheta
    mat_mul_const(B_theta[0], theta_dot_gyro, B_theta_dot[0], 2, 1);//Bu
    mat_add(A_theta_theta[0], B_theta_dot[0], theta_data_predict[0], 2, 1);//Atheta+Bu 
    
    //次時刻の共分散行列算出: P'=APA^T + BUB^T
    float AP[2][2] = {};   
    float APAT[2][2] = {};
    float tran_A_theta[2][2] = {};
    mat_tran(A_theta[0], tran_A_theta[0], 2, 2);//A^T 
    mat_mul(A_theta[0], P_theta[0], AP[0], 2, 2, 2, 2);//AP
    mat_mul(AP[0], tran_A_theta[0], APAT[0], 2, 2, 2, 2);//APA^T
    float BBT[2][2];
    float tran_B_theta[1][2] = {};
    mat_tran(B_theta[0], tran_B_theta[0], 2, 1);//B^T
    mat_mul(B_theta[0], tran_B_theta[0], BBT[0], 2, 1, 1, 2);//BB^T
    float BUBT[2][2] = {};
    mat_mul_const(BBT[0], theta_dot_variance, BUBT[0], 2, 2);//BUB^T
    mat_add(APAT[0], BUBT[0], P_theta_predict[0], 2, 2);//APA^T+BUB^T

    //角速度
    theta_dot2 = theta_dot_gyro - theta_data[1][0];
}


//カルマンフィルタの初期設定 初期姿勢は0°(直立)を想定
void ini_theta(){
  //初期設定開始 LED ON
  digitalWrite(10, LOW);

  //センサオフセット算出
  offset_cal();
  
  //センサばらつき取得 100回測って分散導出
  acc_init();
  gyro_init();

  //カルマンフィルタの初期設定 初期姿勢は0°(直立)を想定
  theta_data_predict[0][0] = 0;
  theta_data_predict[1][0] = theta_dot_mean;
  
  P_theta_predict[0][0] = 1;
  P_theta_predict[0][1] = 0;
  P_theta_predict[1][0] = 0;
  P_theta_predict[1][1] = theta_dot_variance;

  thetaI = 0.0;

  //初期設定終了 LED OFF
  digitalWrite(10, HIGH);  
}


const int motorL = 0x60;
const int motorR = 0x64;

long Speed;
long SpeedL, SpeedR;

//モータドライバ I2C制御 motor driver I2C
//Reference
//http://makers-with-myson.blog.so-net.ne.jp/2014-05-15
void writeMotorResister(byte vset, byte data1){
  int vdata = vset << 2 | data1;
  Wire.beginTransmission(motorL);
  Wire.write(0x00);
  Wire.write(vdata);
  Wire.endTransmission(true);
  
  Wire.beginTransmission(motorR);
  Wire.write(0x00);
  Wire.write(vdata);
  Wire.endTransmission(true);
}


//ヴァーチャルピンデータ受信
BLYNK_WRITE(V0) {
  Kp = param.asFloat();
  Serial.println("Kp!!!!!!!!!!!!!!!!!!!!!!!!!!!!");
}

BLYNK_WRITE(V1) {
  Ki = param.asFloat();
  Serial.println("Ki!!!!!!!!!!!!!!!!!!!!!!!!!!!!");
}

BLYNK_WRITE(V2) {
  Kd = param.asFloat();
  Serial.println("KD!!!!!!!!!!!!!!!!!!!!!!!!!!!!");
}

BLYNK_WRITE(V3) {
  VMIN = param.asInt();
  //Serial.println("OFFSET!!!!!!!!!!!!!!!!!!!!!!!!!!!!");
}

void setup() {
  pinMode(10, OUTPUT);
  pinMode(39, INPUT);

  Serial.begin(115200);
  M5.begin();
  M5.Axp.ScreenBreath(8);
  M5.Lcd.fillScreen(RED);
  M5.IMU.Init();

  //カルマンフィルタの初期設定
  ini_theta();

  Blynk.setDeviceName("Blynk");
  Blynk.begin(auth);

  Wire.begin(0, 26, 400000); //SDA, SCL
  writeMotorResister(0x00, 0x00);

  //Timer カルマン・フィルタで角度導出
  tickerUpdate.attach(theta_update_interval, update_theta); 
}


void loop() {
  Blynk.run();
  //tickerUpdate.detach();

  if(digitalRead(39) == LOW){
    //Blynk.disconnect();
    writeMotorResister(0x00, 0x00);
    ini_theta();
  }

  if(Blynk.connected()){
    M5.Lcd.fillScreen(GREEN);
  }else{
    M5.Lcd.fillScreen(RED);
  }

  thetaP = theta_data[0][0] / 90.0;
  thetaI += thetaP;
  thetaD = theta_dot2 / 250.0;

  power = thetaP * Kp + thetaI * Ki + thetaD * Kd;
  power = max(-1.0, min(1.0, power));

  int Vdata = ((float)(VMAX - VMIN) * fabs(power)) + VMIN;

  Serial.print(power);
  Serial.print(", ");
  Serial.print(Vdata);
  Serial.print(", ");
  Serial.print(theta_data[0][0]);
  Serial.print(", ");
  Serial.println(theta_dot2);

  /*
  Serial.print(accX - accXoffset);
  Serial.print(", ");
  Serial.print(accY - accYoffset);
  Serial.print(", ");
  Serial.print(accZ - accZoffset);
  Serial.print(", ");
  Serial.print(gyroX - gyroXoffset);
  Serial.print(", ");
  Serial.print(gyroY - gyroYoffset);
  Serial.print(", ");
  Serial.println(gyroZ - gyroZoffset);
  */

  if(fabs(theta_data[0][0]) > 60 || Vdata == 0){
    writeMotorResister(0x00, 0x00);
  }else if(power > 0){
    writeMotorResister(byte(Vdata), 0x01);
  }else if(power < 0){
    writeMotorResister(byte(Vdata), 0x02);
  }
  
  //tickerUpdate.attach(theta_update_interval, update_theta);
  delay(WAIT);
}

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

152

153

154

155

156

157

158

159

160

161

162

163

164

165

166

167

168

169

170

171

172

173

174

175

176

177

178

179

180

181

182

183

184

185

186

187

188

189

190

191

192

193

194

195

196

197

198

199

200

201

202

203

204

205

206

207

208

209

210

211

212

213

214

215

216

217

218

219

220

221

222

223

224

225

226

227

228

229

230

231

232

233

234

235

236

237

238

239

240

241

242

243

244

245

246

247

248

249

250

251

252

253

254

255

256

257

258

259

260

261

262

263

264

265

266

267

268

269

270

271

272

273

274

275

276

277

278

279

280

281

282

283

284

285

286

287

288

289

290

291

292

293

294

295

296

297

298

299

300

301

302

303

304

305

306

307

308

309

310

311

312

313

314

315

316

317

318

319

320

321

322

323

324

325

326

327

328

329

330

331

332

333

334

335

336

337

338

339

340

341

342

343

344

345

346

347

348

349

350

351

352

353

354

355

356

357

358

359

360

361

362

363

364

365

366

367

368

369

370

371

372

373

374

375

376

377

378

379

380

381

382

383

384

385

386

387

388

389

390

391

392

393

394

395

396

397

398

399

400

401

402

403

404

405

406

407

408

409

410

411

412

413

414

415

416

417

418

419

420

421

422

423

424

425

426

427

428

429

430

431

432

433

434

435

436

437

438

439

440

441

442

443

444

445

446

447

448

449

450

451

452

453

454

455

456

457

458

459

460

461

462

463

464

465

466

467

468

469

470

471

472

473

474

475

476

477

478

479

480

481

482

483

484

485

486

487

488

489

490

491

492

493

494

495

496

497

498

499

500

501

502

503

504

505

506

507

508

509

510

511

512

513

514

515

516

517

518

519

520

521

522

523

524

525

526

527

528

529

530

531

532

533

534

535

536

537

#define BLYNK_PRINT Serial

#define BLYNK_USE_DIRECT_CONNECT

#include <BlynkSimpleEsp32_BT.h>

#include <M5StickC.h>

#include <Ticker.h>

#include <Wire.h>

char auth[] = "BlynkアプリのYourAuthTokenを入力";

int VMIN = 1;

float Kp = 0.4;

float Ki = 0.1;

float Kd = 0.2;

float power = 0.0;

float thetaP, thetaI, thetaD;

#define WAIT 10

#define VMAX 63

int16_t accX = 0;

int16_t accY = 0;

int16_t accZ = 0;

int16_t gyroX = 0;

int16_t gyroY = 0;

int16_t gyroZ = 0;

//加速度センサオフセット

int accXoffset = 0;

int accYoffset = 0;

int accZoffset = 0;

//ジャイロセンサオフセット

int gyroXoffset = 0;

int gyroYoffset = 0;

int gyroZoffset = 0;

Ticker tickerUpdate;

//センサバラつき取得用変数

int sample_num = 100;

int meas_interval = 10;

float theta_deg = 0.0;

float theta_dot = 0.0;

float theta_dot2;

float theta_mean;

float theta_variance;

float theta_dot_mean;

float theta_dot_variance;

//=========================================================

//カルマン・フィルタ用変数

//=========================================================

//カルマン・フィルタ処理

float theta_update_freq = 400; //Hz

float theta_update_interval = 1.0/double(theta_update_freq); //2.5msec

//状態ベクトルx

//[[theta(degree)], [offset of theta_dot(degree/sec)]]

//状態ベクトルの予測値

float theta_data_predict[2][1];

float theta_data[2][1];

//共分散行列

float P_theta_predict[2][2];

float P_theta[2][2];

//状態方程式の"A"

float A_theta[2][2] = {{1, -theta_update_interval}, {0, 1}};

//状態方程式の"B"

float B_theta[2][1] = {{theta_update_interval}, {0}};

//出力方程式の"C"

float C_theta[1][2] = {{1, 0}};

//=========================================================

// 行列演算関数

//=========================================================

//行列の和

void mat_add(float *m1, float *m2, float *sol, int row, int column) {

for(int i=0; i<row; i++) {

for(int j=0; j<column; j++) {

sol[i*column + j] = m1[i*column + j] + m2[i*column + j];

}

return;

}

//行列の差

void mat_sub(float *m1, float *m2, float *sol, int row, int column){

for(int i=0; i<row; i++) {

for(int j=0; j<column; j++) {

sol[i*column + j] = m1[i*column + j] - m2[i*column + j];

}

return;

}

//行列の積

void mat_mul(float *m1, float *m2, float *sol, int row1, int column1, int row2, int column2){

for(int i=0; i<row1; i++){

for(int j=0; j<column2; j++){

sol[i*column2 + j] = 0;

for(int k=0; k<column1; k++) {

sol[i*column2 + j] += m1[i*column1 + k]*m2[k*column2 + j];

}

return;

}

//転置行列算出

void mat_tran(float *m1, float *sol, int row_original, int column_original) {

for(int i=0; i<row_original; i++) {

for(int j=0; j<column_original; j++) {

sol[j*row_original + i] = m1[i*column_original + j];

}

return;

}

//行列の定数倍算出

void mat_mul_const(float *m1,float c, float *sol, int row, int column){

for(int i=0; i<row; i++){

for(int j=0; j<column; j++){

sol[i*column + j] = c * m1[i*column + j];

}

return;

}

//逆行列算出

void mat_inv(float *m, float *sol, int column, int row){

//allocate memory for a temporary matrix

float* temp = (float *)malloc( column*2*row*sizeof(float) );

//make the augmented matrix

for(int i=0; i<column; i++) {

//copy original matrix

for(int j=0; j<row; j++) {

temp[i*(2*row) + j] = m[i*row + j];

}

//make identity matrix

for(int j=row; j<row*2; j++) {

if(j-row == i) {

temp[i*(2*row) + j] = 1;

}

else {

temp[i*(2*row) + j] = 0;

}

//Sweep (down)

for(int i=0; i<column; i++) {

//pivot selection

float pivot = temp[i*(2*row) + i];

int pivot_index = i;

float pivot_temp;

for(int j=i; j<column;j++) {

if( temp[j*(2*row)+i] > pivot ) {

pivot = temp[j*(2*row) + i];

pivot_index = j;

}

if(pivot_index != i) {

for(int j=0; j<2*row; j++) {

pivot_temp = temp[ pivot_index * (2*row) + j ];

temp[pivot_index * (2*row) + j] = temp[i*(2*row) + j];

temp[i*(2*row) + j] = pivot_temp;

}

//division

for(int j=0; j<2*row; j++) {

temp[i*(2*row) + j] /= pivot;

}

//sweep

for(int j=i+1; j<column; j++) {

float temp2 = temp[j*(2*row) + i];

//sweep each row

for(int k=0; k<row*2; k++) {

temp[j*(2*row) + k] -= temp2 * temp[ i*(2*row) + k ];

}

//Sweep (up)

for(int i=0; i<column-1; i++) {

for(int j=i+1; j<column; j++) {

float pivot = temp[ (column-1-j)*(2*row) + (row-1-i)];

for(int k=0; k<2*row; k++) {

temp[(column-1-j)*(2*row)+k] -= pivot * temp[(column-1-i)*(2*row)+k];

}

//copy result

for(int i=0; i<column; i++) {

for(int j=0; j<row; j++) {

sol[i*row + j] = temp[i*(2*row) + (j+row)];

}

free(temp);

return;

}

//センサオフセット算出

void offset_cal(){

delay(1000);

accXoffset = 0;

accYoffset = 0;

accZoffset = 0;

gyroXoffset = 0;

gyroYoffset = 0;

gyroZoffset = 0;

for(int i=0; i<10; i++) {

M5.IMU.getAccelAdc(&accX,&accY,&accZ);

M5.IMU.getGyroAdc(&gyroX,&gyroY,&gyroZ);

delay(meas_interval);

accXoffset += accX;

accYoffset += accY;

accZoffset += accZ;

gyroXoffset += gyroX;

gyroYoffset += gyroY;

gyroZoffset += gyroZ;

}

accXoffset /= 10;

accYoffset = accYoffset / 10 + 8192;

accZoffset /= 10;

gyroXoffset /= 10;

gyroYoffset /= 10;

gyroZoffset /= 10;

}

//加速度センサから傾きデータ取得 [deg]

float get_acc_data() {

M5.IMU.getAccelAdc(&accX,&accY,&accZ);

//得られたセンサ値はオフセット引いて使用

//傾斜角導出単位はdeg

theta_deg = atan( (float)(accZ - accZoffset) / (float)(-1 * accY - accYoffset) ) * 57.29578f;

return theta_deg;

}

//加速度センサによる傾きのばらつき測定

void acc_init(){

float theta_array[sample_num];

for(int i=0; i<sample_num; i++) {

theta_array[i] = get_acc_data();

delay(meas_interval);

}

//平均値

theta_mean = 0;

for(int i=0; i<sample_num; i++) {

theta_mean += theta_array[i];

}

theta_mean /= sample_num;

//分散

float temp;

theta_variance = 0;

for(int i=0; i<sample_num; i++) {

temp = theta_array[i] - theta_mean;

theta_variance += temp*temp;

}

theta_variance /= sample_num;

}

//x軸角速度取得

float get_gyro_data() {

M5.IMU.getGyroAdc(&gyroX,&gyroY,&gyroZ);

//得られたセンサ値はオフセット引いて使用

theta_dot = ((float) (gyroX - gyroXoffset)) * M5.IMU.gRes;

return theta_dot;

}

//ジャイロセンサばらつき測定

void gyro_init() {

float theta_dot_array[sample_num];

for(int i=0;i<sample_num;i++) {

theta_dot_array[i] = get_gyro_data();

delay(meas_interval);

}

//平均値

theta_dot_mean = 0;

for(int i=0;i<sample_num;i++) {

theta_dot_mean += theta_dot_array[i];

}

theta_dot_mean /= sample_num;

//分散

float temp;

theta_dot_variance = 0;

for(int i=0; i<sample_num; i++) {

temp = theta_dot_array[i] - theta_dot_mean;

theta_dot_variance += temp*temp;

}

theta_dot_variance /= sample_num;

}

//=========================================================

//カルマン・フィルタアルゴリズム処理

//=========================================================

void update_theta()

{

//加速度センサによる角度測定

float y = get_acc_data(); //degree

//入力データ：角速度

float theta_dot_gyro = get_gyro_data(); //degree/sec

//カルマン・ゲイン算出: G = P'C^T(W+CP'C^T)^-1

float P_CT[2][1] = {};

float tran_C_theta[2][1] = {};

mat_tran(C_theta[0], tran_C_theta[0], 1, 2);//C^T

mat_mul(P_theta_predict[0], tran_C_theta[0], P_CT[0], 2, 2, 2, 1);//P'C^T

float G_temp1[1][1] = {};

mat_mul(C_theta[0], P_CT[0], G_temp1[0], 1,2, 2,1);//CP'C^T

float G_temp2 = 1.0f / (G_temp1[0][0] + theta_variance);//(W+CP'C^T)^-1

float G1[2][1] = {};

mat_mul_const(P_CT[0], G_temp2, G1[0], 2, 1);//P'C^T(W+CP'C^T)^-1

//傾斜角推定値算出: theta = theta'+G(y-Ctheta')

float C_theta_theta[1][1] = {};

mat_mul(C_theta[0], theta_data_predict[0], C_theta_theta[0], 1, 2, 2, 1);//Ctheta'

float delta_y = y - C_theta_theta[0][0];//y-Ctheta'

float delta_theta[2][1] = {};

mat_mul_const(G1[0], delta_y, delta_theta[0], 2, 1);

mat_add(theta_data_predict[0], delta_theta[0], theta_data[0], 2, 1);

//共分散行列算出: P=(I-GC)P'

float GC[2][2] = {};

float I2[2][2] = {{1,0},{0,1}};

mat_mul(G1[0], C_theta[0], GC[0], 2, 1, 1, 2);//GC

float I2_GC[2][2] = {};

mat_sub(I2[0], GC[0], I2_GC[0], 2, 2);//I-GC

mat_mul(I2_GC[0], P_theta_predict[0], P_theta[0], 2, 2, 2, 2);//(I-GC)P'

//次時刻の傾斜角の予測値算出: theta'=Atheta+Bu

float A_theta_theta[2][1] = {};

float B_theta_dot[2][1] = {};

mat_mul(A_theta[0], theta_data[0], A_theta_theta[0], 2, 2, 2, 1);//Atheta

mat_mul_const(B_theta[0], theta_dot_gyro, B_theta_dot[0], 2, 1);//Bu

mat_add(A_theta_theta[0], B_theta_dot[0], theta_data_predict[0], 2, 1);//Atheta+Bu

//次時刻の共分散行列算出: P'=APA^T + BUB^T

float AP[2][2] = {};

float APAT[2][2] = {};

float tran_A_theta[2][2] = {};

mat_tran(A_theta[0], tran_A_theta[0], 2, 2);//A^T

mat_mul(A_theta[0], P_theta[0], AP[0], 2, 2, 2, 2);//AP

mat_mul(AP[0], tran_A_theta[0], APAT[0], 2, 2, 2, 2);//APA^T

float BBT[2][2];

float tran_B_theta[1][2] = {};

mat_tran(B_theta[0], tran_B_theta[0], 2, 1);//B^T

mat_mul(B_theta[0], tran_B_theta[0], BBT[0], 2, 1, 1, 2);//BB^T

float BUBT[2][2] = {};

mat_mul_const(BBT[0], theta_dot_variance, BUBT[0], 2, 2);//BUB^T

mat_add(APAT[0], BUBT[0], P_theta_predict[0], 2, 2);//APA^T+BUB^T

//角速度

theta_dot2 = theta_dot_gyro - theta_data[1][0];

}

//カルマンフィルタの初期設定初期姿勢は0°(直立)を想定

void ini_theta(){

//初期設定開始 LED ON

digitalWrite(10, LOW);

//センサオフセット算出

offset_cal();

//センサばらつき取得 100回測って分散導出

acc_init();

gyro_init();

//カルマンフィルタの初期設定初期姿勢は0°(直立)を想定

theta_data_predict[0][0] = 0;

theta_data_predict[1][0] = theta_dot_mean;

P_theta_predict[0][0] = 1;

P_theta_predict[0][1] = 0;

P_theta_predict[1][0] = 0;

P_theta_predict[1][1] = theta_dot_variance;

thetaI = 0.0;

//初期設定終了 LED OFF

digitalWrite(10, HIGH);

}

const int motorL = 0x60;

const int motorR = 0x64;

long Speed;

long SpeedL, SpeedR;

//モータドライバ I2C制御 motor driver I2C

//Reference

//http://makers-with-myson.blog.so-net.ne.jp/2014-05-15

void writeMotorResister(byte vset, byte data1){

int vdata = vset << 2 | data1;

Wire.beginTransmission(motorL);

Wire.write(0x00);

Wire.write(vdata);

Wire.endTransmission(true);

Wire.beginTransmission(motorR);

Wire.write(0x00);

Wire.write(vdata);

Wire.endTransmission(true);

}

//ヴァーチャルピンデータ受信

BLYNK_WRITE(V0) {

Kp = param.asFloat();

Serial.println("Kp!!!!!!!!!!!!!!!!!!!!!!!!!!!!");

}

BLYNK_WRITE(V1) {

Ki = param.asFloat();

Serial.println("Ki!!!!!!!!!!!!!!!!!!!!!!!!!!!!");

}

BLYNK_WRITE(V2) {

Kd = param.asFloat();

Serial.println("KD!!!!!!!!!!!!!!!!!!!!!!!!!!!!");

}

BLYNK_WRITE(V3) {

VMIN = param.asInt();

//Serial.println("OFFSET!!!!!!!!!!!!!!!!!!!!!!!!!!!!");

}

void setup() {

pinMode(10, OUTPUT);

pinMode(39, INPUT);

Serial.begin(115200);

M5.begin();

M5.Axp.ScreenBreath(8);

M5.Lcd.fillScreen(RED);

M5.IMU.Init();

//カルマンフィルタの初期設定

ini_theta();

Blynk.setDeviceName("Blynk");

Blynk.begin(auth);

Wire.begin(0, 26, 400000); //SDA, SCL

writeMotorResister(0x00, 0x00);

//Timer カルマン・フィルタで角度導出

tickerUpdate.attach(theta_update_interval, update_theta);

}

void loop() {

Blynk.run();

//tickerUpdate.detach();

if(digitalRead(39) == LOW){

//Blynk.disconnect();

writeMotorResister(0x00, 0x00);

ini_theta();

}

if(Blynk.connected()){

M5.Lcd.fillScreen(GREEN);

}else{

M5.Lcd.fillScreen(RED);

}

thetaP = theta_data[0][0] / 90.0;

thetaI += thetaP;

thetaD = theta_dot2 / 250.0;

power = thetaP * Kp + thetaI * Ki + thetaD * Kd;

power = max(-1.0, min(1.0, power));

int Vdata = ((float)(VMAX - VMIN) * fabs(power)) + VMIN;

Serial.print(power);

Serial.print(", ");

Serial.print(Vdata);

Serial.print(", ");

Serial.print(theta_data[0][0]);

Serial.print(", ");

Serial.println(theta_dot2);

Serial.print(accX - accXoffset);

Serial.print(", ");

Serial.print(accY - accYoffset);

Serial.print(", ");

Serial.print(accZ - accZoffset);

Serial.print(", ");

Serial.print(gyroX - gyroXoffset);

Serial.print(", ");

Serial.print(gyroY - gyroYoffset);

Serial.print(", ");

Serial.println(gyroZ - gyroZoffset);

if(fabs(theta_data[0][0]) > 60 || Vdata == 0){

writeMotorResister(0x00, 0x00);

}else if(power > 0){

writeMotorResister(byte(Vdata), 0x01);

}else if(power < 0){

writeMotorResister(byte(Vdata), 0x02);

}

//tickerUpdate.attach(theta_update_interval, update_theta);

delay(WAIT);

}

参考

倒立振子によるPID制御のコードは以下を参考にさせていただきました。

倒立振子を作ってみた。

動作

調整中の様子。

まだまだ不安定。Bluetooth接続でディスプレイ赤→青へ#M5StickC #Blynk #倒立振子への道 pic.twitter.com/vIsVWOlpdD

— HomeMadeGarbage (@H0meMadeGarbage) 2019年7月2日

起動すると初期設定（LED点灯中）がはじまり終了すると、倒立動作が開始します。BluetoothでBlynkとつながるとディスプレイが赤から緑にかわり各種設定値を送信できます。

動きをみながら地道に調整を進めます。。。

このマシンでPID制御だとこんなもんかな。
さてトラ技の誉れ高き制御方法勉強せねば#M5StickC #Blynk #倒立振子への道 #食洗器使用中 pic.twitter.com/ya3DkqwdW8

— HomeMadeGarbage (@H0meMadeGarbage) 2019年7月3日

上記コードでVMIN = 1、Kp = 0.4、Ki = 0.1、Kd = 0.2でだいたい安定しました。外乱には非常に弱いですが。。。

とりあえず倒立するマシンができたので次はカット＆エラーではなくトラ技の制御法を取り入れて
もっと安定性の高い倒立振子を目指します！

M5StickC で倒立振子 Blynk でコントロールー倒立振子への道 4ー

「M5StickC で倒立振子 PID制御編ー倒立振子への道 3ー」への5件のフィードバック

Vivid より:

2019年8月12日 10:54 PM

お見事な倒立振り子です！
私もこの一か月ほど。センサーの設定からモータードライバ、ＰＩＤと悪戦苦闘しています。
まずは、ある程度自立させないことには、ＰＩＤの最適化もできませんが、ＰＩＤを最適化しないことには自立しません。この矛盾。
鉄の重りで、重心を下にして安定させる裏技、なるほどです。（若干、反則技かとも思いますが）(笑)
当方、重心をかなり上にもってきておりますが、かなかな難しい状況です。
よろしければ、ブログを除いてやってください。励みになります(笑)
あと２－３日で、最新の記事をアップする予定です。

返信
お父ちゃん より:

2019年8月12日 11:09 PM

コメントありがとうございます。

私も色々形や定数を変えて試行錯誤してやっとやっと立ったという経緯です。
重心を上にすると比較的ゆっくりとした制御でいいのですが距離が大きくなるという
経験的印象があり重心を下にしてさらに機体自体のバランスを取りました。
おかげで微小な制御での安定が実現できましたが外乱には弱い倒立振子となっております。。。

かなり奥深いですよね。。。理論でガッツリ設計するべきなのでしょうが、まだまだ勉強中です。

ブログ楽しみにしております！　

返信
ピンバック: M5 Wheel Module ( moio ) | macsbug
Vivid より:

2019年8月31日 3:58 PM

こんにちは
お陰様で、とりあえず少しの間「立ち続ける」所までこぎつけました。
ジャイロセンサーのドリフトで、徐々に安定位置がずれていき、最後には「激突」となります(笑)
加速度センサーとジャイロセンサーの「相補フィルター」を試しているところですが、加速度からの角度と、角速度からの角度の整合をとることが難しく、はまっています。
お時間ありましたら、今の「倒立ロボット」の雄姿、ご笑覧ください。

返信
1. お父ちゃん より:
  
  2019年8月31日 6:45 PM
  
  ご案内ありがとうございます。
  重心が高いためか外乱にも強いようですね。
  
  ジャイロは素子の温度とかでもドリフトするようですね。。
  
  進捗を楽しみにしております！
  
  返信

コメントはこちらからコメントをキャンセル

このサイトはスパムを低減するために Akismet を使っています。コメントデータの処理方法の詳細はこちらをご覧ください。

まずは作ってみた

頑張って改良した

構成

部品

マシン

PID制御

参考

6軸センサ SH200Q調整

Blynk設定

I2Cモータードライバ DRV8830

I2Cアドレス設定

書き込みI2Cデータ

Arduino IDEプログラム

参考

動作

次の記事

関連記事

「M5StickC で倒立振子 PID制御編 ー倒立振子への道 3ー」への5件のフィードバック

コメントはこちらから コメントをキャンセル

「M5StickC で倒立振子 PID制御編ー倒立振子への道 3ー」への5件のフィードバック

コメントはこちらからコメントをキャンセル