「カルマンフィルタ」タグアーカイブ

Seeed XIAO BLE nRF52840 Sense で1軸 姿勢制御モジュール 製作

先日はSeeed XIAO BLE nRF52840 Sense の味見をいたしました。

Seeed XIAO BLE nRF52840 Sense を使ってみた

[bc url=”https://www.seeedstudio.com/Seeed-XIAO-BLE-Sense-nRF52840-p-5253.html”]

ここではSeeed XIAO BLE nRF52840 Sense を用いて1軸 姿勢制御モジュールを製作いたしました。

 

 

姿勢制御モジュール製作

Seeed XIAO BLE nRF52840 Sense には6軸 IMUセンサが搭載されていますので1軸 姿勢制御モジュール SHISEIGYO-1  Jr.のコントローラにしてみました。

従来のATOM Matrix用基板に一部空中配線で調整しつつ実装し組み上げました。

動作

せっかくマイクも搭載されているので音声で起き上がるようにしました。

前回の傾斜計で確認したように姿勢検知には全く問題なさそうです。
モータ回転時にちょっと音がなっているのでブラシレスモータ駆動用のPWM信号の周波数の影響でしょうか?

Seeed XIAO BLE nRF52840 Senseのアナログ出力の周波数は何Hzなんでしょうね?まぁ測ればいいか。今度観ます。

 

音声検出

動画では「UP!」と発話して起動していますが、音声認識はしておらず大きい音が鳴ったら起き上がるだけですww

マイクの使用方法はPMD用サンプルコードを参考にしました。

以下のように大きい音を検出したらgetup()で起き上がています。

if (samplesRead) {
  for (int i = 0; i < samplesRead; i++) {
    Serial.println(sampleBuffer[i]);
    if(abs(sampleBuffer[i]) > 2000){
      samplesRead = 0;
      delay(500);
      getup();
      break;
    }
  }
  
  // Clear the read count
  samplesRead = 0;
}

 

学習モデルで音声認識もできるようなので今度試してみたいです。

 

IMU調整

6軸 IMUセンサ LSM6DS3TR-C の検知フルスケールレンジとサンプリングレートを以下のように変更しています。

 加速度:±4g、104Hz → ±2g、416Hz
 ジャイロ:±2000dps、104Hz → ±245dps、416Hz

変更はLSM6DS3TR-C ライブラリのLSM6DS3TR_C.cppを修正

 

Seeed XIAO BLE nRF52840 Sense を使ってみた

Seeed XIAO BLE nRF52840 Sense を購入し味見してみましたので報告いたします。

[bc url=”https://www.seeedstudio.com/Seeed-XIAO-BLE-Sense-nRF52840-p-5253.html”]

 

 

Seeed XIAO BLE nRF52840 Sense

Seeed XIAOと同じ形状で CPUにNordic nRF52840が採用された製品です。

[amazonjs asin=”B09HMY9X6D” locale=”JP” title=”SeeedStudio Seeeduino XIAO(ピンヘッダー実装済み) 102010388″]

 

nRF52840搭載ですのでBLE通信が可能です。
さらにSense版には 6軸IMUセンサ(LSM6DS3TR-C)と マイク(MSM261D3526H1CPM)が搭載されています。

12月にプレオーダーが開始されたのですぐに購入し、1/18にやっと到着いたしました。

プレオーダー版は技適マークないので技適未取得機器を用いた実験等の特例制度を申請いたしました
(ネット申請でマジで秒で承認されました)。

技適取得して国内販売が予定されているようです。

 

Lチカ

Seeed Wikiを頼りにArduino IDE用環境を整えてLチカしてみました。

[bc url=”https://wiki.seeedstudio.com/XIAO_BLE/”]

ブートモードにする際にXIAOと同様にリセットを2回GNDに落とす必要がありますがSeeed XIAO BLE nRF52840 Senseにはリセットボタンがついており便利です。
しかしボタンが小さすぎて少し押しにくいです。。

 

Seeed XIAO BLEにはRGB三色LEDが搭載されています。

 

Arduino IDEのスケッチ例→Basics→Blinkのピン名LED_BUILTINでは赤が点滅しました。

ピン名LEDR、LEDG、LEDBで赤、緑、青の点灯を確認できました。

BLEでLチカ

せっかくなのでBLE通信もしたいと思い、BLEでLチカしました。

Seeed Wikiの例にならってLightBlueというスマホアプリでBLEでLED遠隔操作しました。

 

いつも利用してるアプリBlynkにはまだ対応してないようなので、対応が待ち遠しい限りです。

 

IMUを堪能

6軸IMUセンサ LSM6DS3TR-C を楽しみます。
IMUの設定はSeeed Wiki の通りに実施しました。

軸は以下のようになっておりました。

 

加速度センサとジャイロセンサの値からカルマンフィルタを用いて傾斜計をこしらえました。
カルマンフィルタは以下のライブラリを使用しました。
 https://github.com/TKJElectronics/KalmanFilter

Arduino IDE コード

X軸の周りの傾きを出力します。

#include <Arduino_LSM6DS3TR_C.h>
#include <Kalman.h>

unsigned long oldTime = 0, loopTime, nowTime;
float dt;

float accX = 0, accY = 0, accZ = 0;
float gyroX = 0, gyroY = 0, gyroZ = 0;

float theta_acc = 0.0;
float theta_dot = 0.0;



Kalman kalmanX;
float kalAngleX, kalAngleDotX;

//加速度センサから傾きデータ取得 [deg]
float get_theta_acc() {
  IMU.readAcceleration(accX, accY, accZ);
  //傾斜角導出 単位はdeg
  theta_acc  = atan2(accY, accZ) * 57.29578f;
  return theta_acc;
}

//X軸 角速度取得
float get_gyro_data() {
  IMU.readGyroscope(gyroX, gyroY, gyroZ);
  theta_dot = gyroX;
  return theta_dot;
}


void setup() {
  Serial.begin(115200);

  if (!IMU.begin()) {
    Serial.println("Failed to initialize IMU!");

    while (1) yield();
  }

  kalmanX.setAngle(get_theta_acc());

  pinMode(LEDR, OUTPUT);
  pinMode(LEDG, OUTPUT);
  pinMode(LEDB, OUTPUT);

  digitalWrite(LEDR, HIGH);
  digitalWrite(LEDG, HIGH);
  digitalWrite(LEDB, HIGH);
}


void loop() {   
  nowTime = micros();
  loopTime = nowTime - oldTime;
  oldTime = nowTime;
  
  dt = (float)loopTime / 1000000.0; //sec


  //カルマンフィルタ 姿勢 傾き
  kalAngleX = kalmanX.getAngle(get_theta_acc(), get_gyro_data(), dt);
  
  //カルマンフィルタ 姿勢 角速度
  kalAngleDotX = kalmanX.getRate();

  Serial.println(kalAngleX);
  

  //LEDインジケータ
  if(fabs(kalAngleX) > 20.0){
    digitalWrite(LEDR, LOW);
    digitalWrite(LEDG, HIGH);
    digitalWrite(LEDB, HIGH);
  }else if(fabs(kalAngleX) < 1.0){
    digitalWrite(LEDR, HIGH);
    digitalWrite(LEDG, HIGH);
    digitalWrite(LEDB, LOW);
  }else{
    digitalWrite(LEDR, HIGH);
    digitalWrite(LEDG, LOW);
    digitalWrite(LEDB, HIGH);
  }

}

 

動作

LEDを角度のインジケータとして使用しています。

 

おわりに

Seeed XIAO BLE nRF52840 Sense をArduino IDEで楽しみました。

Lチカに加えてSeeed XIAO BLE nRF52840 Senseの醍醐味であるBLE通信やIMUセンサも堪能いたしました。

マイクも搭載されており、まだまだ色々楽しめそうなのでいじり倒したいと思います。

次の記事

Seeed XIAO BLE nRF52840 Sense で1軸 姿勢制御モジュール 製作

1軸 姿勢制御モジュール がやっと立った ーリアクションホイールへの道7ー

さて、前回の1軸 姿勢制御モジュール SHISEIGYO-1 (シセーギョーワン) 倒立に向けた下準備から随分日が経ちました。

実はずーーーーっと倒立させるべく試行錯誤しておりました。
この度無事になんとか倒立させることができましたのでご報告いたします。

 

 

倒立した!

まずは SHISEIGYO-1 が倒立した勇ましい姿をご覧ください。

マジで何度も心が折れそうになりましたが遂に倒立を確認することができました。
ちなみにモータ制御なしで倒立することはありません。

以下で今回実施した制御方法を記載いたします。

 

制御方法

姿勢制御モジュールは作用反作用の法則を用いて倒立を維持しています。
倒立地点 (傾き0°) から傾いた方向にフライホイールを回して反作用で起き上がります。

傾きを検出し、その状況に応じたモータ回転に制御して倒立を維持します。

 

[amazonjs asin=”B087H8TFX7″ locale=”JP” title=”トランジスタ技術 2020年 06 月号”]

2020年6月号のトラ技の3軸姿勢制御モジュールの記事にモータのトルク$T_m$を以下の式に基づいて制御していると記載がありました。

$$T_m = -K_{d1}・θ_b-K_{d2}・\dotθ_b-K_{d3}・\dotθ_w   (1)$$
$$θ_bはモジュールの傾き、\dotθ_wはホイールの角速度、K_{dx}はそれぞれの係数$$

こちらを参考にSHISEIGYO-1も制御することにいたしました。

モータ電流特性

SHISEIGYO-1はリアクションホイールとしてフライホイール付きのブラシレスモータ ID-549XWを使用しています。

[bc url = “https://ja.aliexpress.com/item/4000314159240.html?spm=a2g11.12057483.0.0.4cde2fd0t3ecdq”]

モータのトルクはモータに流れる電流に比例します。
SHISEIGYO-1のモータはPWM入力によって回転速度を制御しますので、PWMのOFFデューティーとモータに流れる電流の関係を測定しました。

モータに入力するPWM信号のOFFデューティーを上げていくとブラシレスモータ ID-549XWの回転速度も上昇し負荷電流も増えます。

上の測定結果のグラフからデューティーとモータ負荷電流Iはほぼリニアに比例していることがわかりました。

したがって上式(1)より

$$T_m ∝ I ∝Duty ∝-K_{d1}・θ_b-K_{d2}・\dotθ_b-K_{d3}・\dotθ_w$$

となり、モジュールの姿勢角($θ_b$) とその角速度($\dotθ_b$) とモータの回転速度($\dotθ_w$) からモータに入力するべき信号のデューティー比が導き出せると言えそうです。

また、SHISEIGYO-1がこの理論式だけで立つと感覚的に確証が持てなかったので更に以下のパラメータも追加して、手動で各パラメータを調整して倒立を目指しました。

  • 傾きオフセット補正
     SHISEIGYO-1の物理的偏りやセンサのオフセットを数値で補正します。
  • モータ初速
     モータの電気的抵抗や物理的摩擦による抵抗を打ち消すためのオフセット値
  • ブレーキ範囲
     倒立する姿勢角でモータがきっちり止まるためにブレーキをかけるデューティー値の範囲を指定します。

 

構成

前回と変更はありません。
PWM入力線とエンコーダ出力、ブレーキ端子をATOM Matrixに接続し制御します。

 

部品

  • M5Stack ATOM Matrix
    [amazonjs asin=”B0879JWK8F” locale=”JP” title=”M5Stack ATOM Matrix ESP32 Development Kit”]
     
  • フライホイール付きブラシレスモータ ID-549XW
    [bc url=”https://ja.aliexpress.com/item/4000314159240.html?spm=a2g11.12057483.0.0.4cde2fd0t3ecdq”]

 

Arduinoコード

#include "M5Atom.h"
#include <Kalman.h>

#define ENC_A 22
#define ENC_B 19
#define brake 23
#define rote_pin 32
#define PWM_pin 26
#define button 39

unsigned long oldTime = 0, loopTime, nowTime;
float dt;

volatile byte pos;
volatile int  enc_count = 0;

int DutyIni = 490, pwmDuty;
float Kp = 4.2;
float Kd = -0.19;
float Kw = 0.1;
float D0 = 0.31;
int brakeRange = 0;

float accX = 0, accY = 0, accZ = 0;
float gyroX = 0, gyroY = 0, gyroZ = 0;
float temp = 0;

float theta_acc = 0.0;
float theta_dot = 0.0;

//オフセット
float accXoffset = 0, accYoffset = 0, accZoffset = 0;
float gyroXoffset = 0, gyroYoffset = 0, gyroZoffset = 0;

Kalman kalmanY;
float kalAngleY, kalAngleDotY;

//センサオフセット算出
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.getAccelData(&accX,&accY,&accZ);
    M5.IMU.getGyroData(&gyroX,&gyroY,&gyroZ);
    delay(10);
    accXoffset += accX;
    accYoffset += accY;
    accZoffset += accZ;
    gyroXoffset += gyroX;
    gyroYoffset += gyroY;
    gyroZoffset += gyroZ;
  }

  if(accXoffset < 0){
    accXoffset = accXoffset / 10 + 1.0 / sqrt(2.0);
  }else{
    accXoffset = accXoffset / 10 - 1.0 / sqrt(2.0);
  }
  accYoffset /= 10;
  accZoffset = accZoffset / 10 + 1.0 / sqrt(2.0);
  gyroXoffset /= 10;
  gyroYoffset /= 10;
  gyroZoffset /= 10;
}

//加速度センサから傾きデータ取得 [deg]
float get_theta_acc() {
  M5.IMU.getAccelData(&accX,&accY,&accZ);
  //傾斜角導出 単位はdeg
  theta_acc  = atan(-1.0 * (accX - accXoffset) / (accZ - accZoffset)) * 57.29578f;
  return theta_acc;
}

//Y軸 角速度取得
float get_gyro_data() {
  M5.IMU.getGyroData(&gyroX,&gyroY,&gyroZ);
  theta_dot = gyroY - gyroYoffset;
  return theta_dot;
}

void setup() {
  M5.begin(true, false, false); //SerialEnable ,I2CEnable , LEDEnable

  pinMode(ENC_A, INPUT);
  pinMode(ENC_B, INPUT);
  pinMode(brake, OUTPUT);
  pinMode(button, INPUT);
  
  digitalWrite(brake, HIGH);
 
  attachInterrupt(ENC_A, ENC_READ, CHANGE);
  attachInterrupt(ENC_B, ENC_READ, CHANGE);

  M5.IMU.Init();

  //センサオフセット算出
  offset_cal();

  //フルスケールレンジ
  M5.IMU.SetGyroFsr(M5.IMU.GFS_500DPS);
  M5.IMU.SetAccelFsr(M5.IMU.AFS_4G);

  kalmanY.setAngle(get_theta_acc());

  ledcSetup(0, 20000, 9);
  ledcAttachPin(PWM_pin, 0);
  
  pinMode(rote_pin, OUTPUT);
}


void loop() {
  //オフセット再計算
  if (digitalRead(button) == 0){
    Serial.println("オフセット再計算");
    offset_cal();
  }
  
  nowTime = micros();
  loopTime = nowTime - oldTime;
  oldTime = nowTime;

  dt = (float)loopTime / 1000000.0; //sec

  //モータの角速度算出
  float theta_dotWheel = -1.0 * float(enc_count) * 3.6 / dt;
  enc_count = 0;

  //カルマンフィルタ 姿勢 傾き
  kalAngleY = kalmanY.getAngle(get_theta_acc(), get_gyro_data(), dt);

  //カルマンフィルタ 姿勢 角速度
  kalAngleDotY = kalmanY.getRate();

  /*
  Serial.print("kalAngleY: ");
  Serial.print(kalAngleY);
  Serial.print(", kalAngleDotY: ");
  Serial.print(kalAngleDotY);
  Serial.print(", theta_dotWheel: ");
  Serial.print(theta_dotWheel);
  */

  //ブレーキ
  if(abs(kalAngleY) > 20.0){
    digitalWrite(brake, LOW);
  }else{
    digitalWrite(brake, HIGH);
  }

  //シリアル入力
  char key;

  if ( Serial.available() ) {       
    key = Serial.read();  

    switch (key) {
      case 'q':
        Kp += 0.1;
        break;
      case 'a':
        Kp -= 0.1;
        break;
      case 'w':
        Kd += 0.01;
        break;
      case 's':
        Kd -= 0.01;
        break;
      case 'e':
        Kw += 0.01;
        break;
      case 'd':
        Kw -= 0.01;
        break;
      case 'r':
        D0 += 0.01;
        break;
      case 'f':
        D0 -= 0.01;
        break;
      case 't':
        DutyIni++;
        break;
      case 'g':
        DutyIni--;
        break;
      case 'y':
        brakeRange++;
        break;
      case 'h':
        brakeRange--;
        break;
    }
  }

  Serial.print("Kp: ");
  Serial.print(Kp);
  Serial.print(", Kd: ");
  Serial.print(Kd,3);
  Serial.print(", Kw: ");
  Serial.print(Kw, 3);
  Serial.print(", D0: ");
  Serial.print(D0, 3);
  Serial.print(", DutyIni: ");
  Serial.print(DutyIni);
  Serial.print(", brakeRange: ");
  Serial.print(brakeRange);
  Serial.print(", kalAngleY: ");
  Serial.print(kalAngleY);
  
  //モータ回転
  float M = Kp * (kalAngleY + D0) / 90.0 + Kd * kalAngleDotY / 500.0 + Kw * theta_dotWheel / 10000.0;
  M = max(-1.0f, min(1.0f, M));
  pwmDuty = DutyIni * (1.0 - fabs(M));
  /*
  Serial.print(", M: ");
  Serial.print(M);
  Serial.print(", pwmDuty: ");
  Serial.print(pwmDuty);
  */

  
  //回転方向
  if(pwmDuty > (DutyIni - brakeRange)){
    digitalWrite(brake, LOW);
    ledcWrite(0, 511);
  }else if(M > 0.0f){
    digitalWrite(rote_pin, LOW);
    ledcWrite(0, pwmDuty);
  }else{
    digitalWrite(rote_pin, HIGH);
    ledcWrite(0, pwmDuty);
  }
 
  /*
  if(M > 0){
    digitalWrite(rote_pin, LOW);
    ledcWrite(0, pwmDuty);
  }else{
    digitalWrite(rote_pin, HIGH);
    ledcWrite(0, pwmDuty);
  }
  */

  
  
  /*
  Serial.print("theta: ");
  Serial.print(kalAngleY);
  Serial.print(", PWM: ");
  Serial.print(pwmDuty);
  Serial.print(", loopTime: ");
  Serial.print((float)loopTime / 1000.0);
  Serial.print("msec, theta_dotWheel: ");
  Serial.println(theta_dotWheel);
  */


  delay(20);
  Serial.println("");
  //digitalWrite(brake, LOW);
}

void ENC_READ() {
  byte cur = (!digitalRead(ENC_B) << 1) + !digitalRead(ENC_A);
  byte old = pos & B00000011;
  byte dir = (pos & B00110000) >> 4;
 
  if (cur == 3) cur = 2;
  else if (cur == 2) cur = 3;
 
  if (cur != old)
  {
    if (dir == 0)
    {
      if (cur == 1 || cur == 3) dir = cur;
    } else {
      if (cur == 0)
      {
        if (dir == 1 && old == 3) enc_count--;
        else if (dir == 3 && old == 1) enc_count++;
        dir = 0;
      }
    }
 
    bool rote = 0;
    if (cur == 3 && old == 0) rote = 0;
    else if (cur == 0 && old == 3) rote = 1;
    else if (cur > old) rote = 1;
 
    pos = (dir << 4) + (old << 2) + cur;
  }
}

 

  • L. 135~139
    M5 ATOM Matrix内蔵の6軸慣性センサ MPU6886による加速度センサとジャイロからカルマン・フィルタライブラリを用いてモジュールの姿勢角 (kalAngleY )とその角速度 (kalAngleDotY )を算出します。
  • L. 132
    モータの回転速度 (theta_dotWheel ) はエンコーダのステップ数を割り込みでカウントして算出します。
  • L. 218~221
    モータに入力するPWM信号のデューティを算出します。
    PWMの分解能は9ビット(0~511)としました(L. 111)。

    モジュールの姿勢角と角速度、モータの回転速度に関する係数 (Kp, Kd, Kw) に加えて
    傾きオフセット補正値 (D0) とモータ初速 (DutyIni) も絡めて算出しています。

  • L. 230~240
    算出されたデューティ値の正負からモータ回転方向を判定しています。
    またブレーキ範囲 (brakeRange) を設けてバランスが取れた姿勢角付近でモータが止まるようにします。
  • L. 157~201
    M5 ATOM MatrixにUSBケーブルをつなぎながらシリアル入力で各パラメータを手動で調整しました。

 

動作

以下のパラメータ値で取りあえず倒立が実現できました。

  • 姿勢角係数 Kp = 4.2
  • 姿勢角速度係数 Kd = 0.19
  • モータ回転速度係数 Kw = 0.1
  • 姿勢角オフセット補正 D0 = 0.31
  • ブレーキ範囲 brakeRange = 0
  • モータ初速デューティ DutyIni = 490

上記パラメータの妥当性や物理的理解はまだできておらず、実際外乱には弱い状況です。

しかし、パラメータの大体のオーダーを把握することができ、なにより実際に倒立することが確認できたことは非常に有意義であったと言えます。

 

おわりに

やっと立ちました!

もうこの道を驀進するしかないですね。

次回はM5 ATOM MatrixにUSBケーブルを除去して無線でパラメータ調整できるようにする予定です。

またパラメータを物理的意味のあるものに絞って外乱に強い倒立を目指したいです!!

次の記事

姿勢制御装置とバーサライタの融合 ーリアクションホイールへの道50ー

1軸 姿勢制御モジュール の倒立に向けた準備 ーリアクションホイールへの道6ー

前回は1軸姿勢制御モジュールの筐体を製作し、簡単な動作確認を実施いたしました。

今回は姿勢制御モジュールの倒立実現に向けていくつか検討、改修を施したので
報告いたします。

 

 

エンコーダ導入

前回の動作確認段階でP制御では倒立しそうもないことは、体感的に理解できました。

 

そこで詳細は次回以降で報告しますが、制御のフィードバックにモータの角速度も追加するべく
モータのエンコーダ出力を利用することにしました。

構成

エンコーダ出力とブレーキ端子をATOM Matrixに接続しました。

 

部品

  • M5Stack ATOM Matrix
    [amazonjs asin=”B0879JWK8F” locale=”JP” title=”M5Stack ATOM Matrix ESP32 Development Kit”]
     
  • フライホイール付きブラシレスモータ ID-549XW
    [bc url=”https://ja.aliexpress.com/item/4000314159240.html?spm=a2g11.12057483.0.0.4cde2fd0t3ecdq”]

 

以前 試したようにエンコーダ出力で割り込みしてモータの回転位置と方向を検知します。

しかし、エンコーダ出力を割り込み検出するべくArduinoコードにattachInterruptを追加したとたんLEDフル点灯(全白)しだし、ATOMが アツアツなってしまいました。

原因はよくわからないのですがM5 ATOM Matrixで attachInterruptとM5ライブラリの共存がうまくいかないようです。

とりあえずは
M5.begin(true, false, false); //SerialEnable ,I2CEnable , LEDEnable
でLEDオフで対応しています。

 

フルスケールレンジ変更

傾き検出の精度を高めるべく、M5 ATOM Matrix内蔵の6軸慣性センサ MPU6886の検出レンジを以下のように変更しました。

  • 加速度:16bit ±8 g → ±4 g
  • ジャイロ(角速度):16bit ±2000 deg/sec → ±500 deg/sec

Arduinoコードでsetup()に以下を追加し測定レンジを指定ました。

M5.IMU.SetGyroFsr(M5.IMU.GFS_500DPS); 
M5.IMU.SetAccelFsr(M5.IMU.AFS_4G);

 

カルマン・フィルタ見直し

これまでカルマン・フィルタについて2019年7月号のトラ技のコードを参考に
使用してきました。

[amazonjs asin=”B07RS8ZTJ3″ locale=”JP” title=”トランジスタ技術 2019年 07 月号”]

 

 

しかし、なんだか応答がいまいち良くない気がしたので、
カルマン・フィルタによる姿勢角の推定値を改めて確認してみました。

すると角度を素早く変えるとオーバーシュートぎみで推定値が出力されていることが分かりました。

 

これはよくないので以下のカルマン・フィルタ ライブラリを使用することにいたしました。

[bc url = “https://github.com/TKJElectronics/KalmanFilter”]

以下の通り、応用性の改善を確認できました。

やはり、実績のある既存のライブラリは非常に有用でありがたいものですね。

参考

 

おわりに

現状はまだ倒立できておりませんが、準備は整ったかと思うので
制御の学習をすすめ倒立実現させたいと考えております。

 

次の記事

姿勢制御装置とバーサライタの融合 ーリアクションホイールへの道50ー

1軸 姿勢制御モジュール の筐体製作 ーリアクションホイールへの道5ー

前回M5Stack ATOM Matrixと内蔵の慣性センサMPU6886でモータ制御するところまで確認いたしました。

ここでは1軸姿勢制御モジュールの筐体を作りこんで
簡単に動作確認を実施いたしました。

 

筐体製作

1軸姿勢制御モジュールの筐体を製作します。
筐体は3Dプリンタで製作いたしました。

モータ寸法

モータの寸法はAliExpressのモータID-549XW 販売ページ と実機計測で把握しました。

Fusion360設計

Fusion360で筐体設計いたしました。

M5Stack ATOM Matrixもきっちり収まるように設計いたしました。

3Dプリント

ANYCUBIC MEGA-Sで出力いたしました。

[amazonjs asin=”B07J5P3SP9″ locale=”JP” title=”ANYCUBIC MEGA-S 3Dプリンター 金属製 高精度 Titan押出機 TPU/ABS/PLA等 操作簡易 家庭用 3DPrinter 初心者/学校等向け 日本語マニュアル付き (黒色)”]

 

モータとM5ATOMと一緒に組み立てます。

 

動作

前回 とほぼ同じコードと配線構成で動作確認いたしました。

とりあえずモータ電源なしで動作確認

 

モータ電源12Vを印可して動作確認。

やはり単純に傾きに比例して回転させる制御 (P制御)では立ちそうもありませんね。。。

しかし筐体はバッチリできたと言えそうです。
とてもかわいらしく製作できたので、この子をSHISEIGYO-1 (シセーギョー ワン)と名付けました。

 

おわりに

1軸姿勢制御モジュールの筐体が完成いたしました。
次回以降でしっかり制御を学んで倒立の実現を目指したいと思います。

それでは次の道でお会いしましょう!

次の記事

姿勢制御装置とバーサライタの融合 ーリアクションホイールへの道50ー

ATOM Matrix の慣性センサでモータ制御 ーリアクションホイールへの道4ー

前回はM5Stack ATOM Matrixでカルマン・フィルタを用いた傾斜計を製作しました。

ここでは検出した姿勢角でブラシレスモータを制御して1軸の姿勢制御モジュールの実現への足掛かりにしたいと思います。

 

 

構成

ATOM MatrixのG26ピンで20kHz PWMを出力してモータの回転速度を制御します。

G32ピンで回転方向切り替えます。

部品

  • M5Stack ATOM Matrix
    [amazonjs asin=”B0879JWK8F” locale=”JP” title=”M5Stack ATOM Matrix ESP32 Development Kit”]
     
  • フライホイール付きブラシレスモータ ID-549XW
    [bc url=”https://ja.aliexpress.com/item/4000314159240.html?spm=a2g11.12057483.0.0.4cde2fd0t3ecdq”]

動作テスト

M5Stack ATOM Matrix単体でのモータ回転制御動作を確認しました。
1秒おきに回転方向を切り替えています。

 

Arduinoコード

前回同様にM5Stack ATOM Matrixの慣性センサMPU6886でカルマン・フィルタを用いて角度を算出します。

算出した角度を元にモータを制御します。
姿勢角の正負で回転方向を切り替えます。
そして角度の大きさに比例して回転速度を上げます(P制御)。

#include "M5Atom.h"
#include <Ticker.h>

#define rote_pin 32
#define PWM_pin 26

int val, pwmDuty;

float accX = 0, accY = 0, accZ = 0;
float gyroX = 0, gyroY = 0, gyroZ = 0;
float temp = 0;
bool IMU6886Flag = false;

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

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

Ticker tickerUpdate;


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

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.getAccelData(&accX,&accY,&accZ);
    M5.IMU.getGyroData(&gyroX,&gyroY,&gyroZ);
    delay(meas_interval);
    accXoffset += accX;
    accYoffset += accY;
    accZoffset += accZ;
    gyroXoffset += gyroX;
    gyroYoffset += gyroY;
    gyroZoffset += gyroZ;
  }

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


//加速度センサから傾きデータ取得 [deg]
float get_acc_data() {
  M5.IMU.getAccelData(&accX,&accY,&accZ);
  //得られたセンサ値はオフセット引いて使用
  //傾斜角導出 単位はdeg
  theta_deg  = atan(-(accX - accXoffset) / (accZ - accZoffset)) * 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;
}


//Y軸 角速度取得
float get_gyro_data() {
  M5.IMU.getGyroData(&gyroX,&gyroY,&gyroZ);
  //得られたセンサ値はオフセット引いて使用
  theta_dot = gyroY - gyroYoffset;
  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
}


//カルマンフィルタの初期設定 初期姿勢は0°(直立)を想定
void ini_theta(){
  //初期設定開始 LED ON
  M5.dis.fillpix(0x00f000);

  //センサオフセット算出
  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;

  //初期設定終了 LED OFF
  M5.dis.clear(); 
}


void setup() {
  M5.begin(true, false, true);

  if (M5.IMU.Init() != 0)
    IMU6886Flag = false;
  else
    IMU6886Flag = true;

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

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

  ledcSetup(0, 20000, 8);
  ledcAttachPin(PWM_pin, 0);
  
  pinMode(rote_pin, OUTPUT);
}


void loop() {
  //初期化
  if (M5.Btn.wasPressed()){
    ini_theta();
  }

  /*
  //加速度センサによる傾斜角
  Serial.print(theta_deg);
  Serial.print(", ");
  //カルマン・フィルタによる角度の推定値
  Serial.println(theta_data[0][0]);
  */

  //LED表示
  M5.dis.clear(); 
  if(theta_data[0][0] > 30.0){
    for(int i = 0; i < 5; i++){
      M5.dis.drawpix(i * 5, 0x00f000);
    }
  }else if(theta_data[0][0] <= 30.0 && theta_data[0][0] > 20.0){
    for(int i = 0; i < 5; i++){
      M5.dis.drawpix(i * 5, 0xf00000);
    }
  }else if(theta_data[0][0] <= 20.0 && theta_data[0][0] > 10.0){
    for(int i = 0; i < 5; i++){
      M5.dis.drawpix(i * 5 + 1, 0xf00000);
    }
  }else if(theta_data[0][0] <= 10.0 && theta_data[0][0] > 5.0){
    for(int i = 0; i < 5; i++){
      M5.dis.drawpix(i * 5 + 2, 0xf00000);
    }
  }else if(abs(theta_data[0][0]) <= 5.0){
    for(int i = 0; i < 5; i++){
      M5.dis.drawpix(i * 5 + 2, 0x00f000);
    }
  }else if(theta_data[0][0] >= -10.0 && theta_data[0][0] < -5.0){
    for(int i = 0; i < 5; i++){
      M5.dis.drawpix(i * 5 + 2, 0xf00000);
    }
  }else if(theta_data[0][0] >= -20.0 && theta_data[0][0] < -10.0){
    for(int i = 0; i < 5; i++){
      M5.dis.drawpix(i * 5 + 3, 0xf00000);
    }
  }else if(theta_data[0][0] >= -30.0 && theta_data[0][0] < -20.0){
    for(int i = 0; i < 5; i++){
      M5.dis.drawpix(i * 5 + 4, 0xf00000);
    }
  }else if(theta_data[0][0] < -30.0){
    for(int i = 0; i < 5; i++){
      M5.dis.drawpix(i * 5 + 4, 0x00f000);
    }
  }
  
  if(theta_data[0][0] > 0){
    digitalWrite(rote_pin, HIGH);
  }else{
    digitalWrite(rote_pin, LOW);
  }

  pwmDuty = max((int)map(abs(theta_data[0][0]), 50, 0, 0, 255), 0);
  ledcWrite(0, pwmDuty);

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

  delay(50);
  M5.update();
}

 

動作

M5Stack ATOM Matrixの傾きを大きくすると回転速度が大きくなります(P制御)。

傾きに反発する方向にフライホイールが回転します。
現状は手持ちでテストしていますが手に確かな反動を感じますので
これは期待できるのではないでしょうか 🙄 

LEDマトリクスの角度インジケータも少し凝ってみました。

 

おわりに

M5Stack ATOM Matrixと内蔵の慣性センサMPU6886でモータ制御するところまで来ました。

これより、筐体を作りこんで1軸の姿勢制御モジュールの実現へ猛進したいと思います。

次の記事

姿勢制御装置とバーサライタの融合 ーリアクションホイールへの道50ー

カルマン・フィルタで ATOM Matrix 傾斜計 ーリアクションホイールへの道3ー

これまではフライホイール付きブラシレスモータの制御について学習してまいりました。

ここでは1軸の姿勢制御モジュールの実現に向けて、カルマン・フィルタを用いて
M5Stack ATOM Matrix に搭載された6軸慣性センサ MPU6886 で姿勢角を導出します。
 

 

6軸センサ MPU6886

データシート
 https://m5stack.oss-cn-shenzhen.aliyuncs.com/resource/docs/datasheet/core/MPU-6886-000193%2Bv1.1_GHIC_en.pdf

 

センサのデフォルトの分解能設定は以下の通りです。

  • 加速度:16bit ±8 g
  • ジャイロ(角速度):16bit ±2000 deg/sec

https://github.com/m5stack/M5Atom/blob/master/src/utility/MPU6886.h

 

XYZ軸方向は以下の通りです。

 

M5Stack ATOM Matrix のY軸の傾き$θ$は加速度から以下で算出できます。

$$θ = \tan^{-1}\left(\frac{-accX}{accZ}\right)$$

 

カルマン・フィルタ

カルマン・フィルタについては2019年7月号のトラ技のコードを参考にしました。

[amazonjs asin=”B07RS8ZTJ3″ locale=”JP” title=”トランジスタ技術 2019年 07 月号”]

 

以前M5StickCでもカルマン・フィルタによる傾斜計を製作していますので
詳細は以下を参照ください。

カルマン・フィルタで M5stickC 傾斜計 ー倒立振子への道 1ー

Arduinoコード

M5Atomライブラリはvar. 0.0.1を使用しました。
 https://github.com/m5stack/M5Atom

#include "M5Atom.h"
#include <Ticker.h>

float accX = 0, accY = 0, accZ = 0;
float gyroX = 0, gyroY = 0, gyroZ = 0;
float temp = 0;
bool IMU6886Flag = false;

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

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

Ticker tickerUpdate;


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

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.getAccelData(&accX,&accY,&accZ);
    M5.IMU.getGyroData(&gyroX,&gyroY,&gyroZ);
    delay(meas_interval);
    accXoffset += accX;
    accYoffset += accY;
    accZoffset += accZ;
    gyroXoffset += gyroX;
    gyroYoffset += gyroY;
    gyroZoffset += gyroZ;
  }

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


//加速度センサから傾きデータ取得 [deg]
float get_acc_data() {
  M5.IMU.getAccelData(&accX,&accY,&accZ);
  //得られたセンサ値はオフセット引いて使用
  //傾斜角導出 単位はdeg
  theta_deg  = atan(-(accX - accXoffset) / (accZ - accZoffset)) * 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;
}


//Y軸 角速度取得
float get_gyro_data() {
  M5.IMU.getGyroData(&gyroX,&gyroY,&gyroZ);
  //得られたセンサ値はオフセット引いて使用
  theta_dot = gyroY - gyroYoffset;
  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
}


//カルマンフィルタの初期設定 初期姿勢は0°(直立)を想定
void ini_theta(){
  //初期設定開始 LED ON
  M5.dis.fillpix(0x00f000);

  //センサオフセット算出
  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;

  //初期設定終了 LED OFF
  M5.dis.clear(); 
}


void setup() {
  M5.begin(true, false, true);

  if (M5.IMU.Init() != 0)
    IMU6886Flag = false;
  else
    IMU6886Flag = true;

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

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


void loop() {
  //初期化
  if (M5.Btn.wasPressed()){
    ini_theta();
  }

  //加速度センサによる傾斜角
  Serial.print(theta_deg);
  Serial.print(", ");
  //カルマン・フィルタによる角度の推定値
  Serial.println(theta_data[0][0]);

  //LED表示
  int line = map(theta_data[0][0], -30, 30, 4, 0);
  if(line >= 0 && line < 5){
    M5.dis.clear(); 
    for(int i = 0; i < 5; i++){
      M5.dis.drawpix(i * 5 + line, 0xf00000);
    }
  }

  delay(50);
  M5.update();
}

 

やってることの概要は以下の通りです。

  • センサのオフセット値を10回平均で導出し、以降測定値はオフセット引いて使用
  • センサばらつき算出:加速度とジャイロセンサを100回測って予め分散を導出
  • カルマン・フィルタの初期設定:使用する行列パラメータの初期値設定
  • タイマ割り込み(2.5msecごと)でカルマン・フィルタアルゴリズム発動させて傾斜角の推定値を算出
  • 50msecごとに加速度センサによる傾斜角とカルマン・フィルタによる角度の推定値をシリアル出力
  • M5AtomのLEDマトリクスに-30° ~ 30°の範囲でインジケータ表示

 

動作

青線:MPU6886の加速度センサから算出した傾斜角
赤線:カルマン・フィルタによる角度の推定値

加速度から導出した角度は外乱の影響を大きく受けていますが、
カルマン・フィルタによる角度は正確なセンサの姿勢角を示しています。

 

おわりに

カルマン・フィルタを用いてM5Stack ATOM Matrix の正確な姿勢角を得ることができましたので、
次回はモータと連動して1軸の姿勢制御モジュールの実現に向け進めていきたいと思います。

次の記事

姿勢制御装置とバーサライタの融合 ーリアクションホイールへの道50ー

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

前回、実現できたM5StickCによる倒立振子を前進/後進/旋回動作できるようにいたしました。

M5StickC で倒立振子 PID制御編 ー倒立振子への道 3ー

倒立振子自体の制御は前回と同様にPID制御です。タイヤの回転などを加味したより安定した制御方法は現在勉強中です。いつまでかかるかな。。。?

[amazonjs asin=”B07RS8ZTJ3″ locale=”JP” title=”トランジスタ技術 2019年 07 月号”]

[amazonjs asin=”B07R1QDVWF” locale=”JP” title=”2019 M5StickC ESP32 PICOミニIoT開発ボードフィンガーコンピューターカラーLCD付き (1セット)”]

 

動作

早速動作を見てください。

スマホアプリBlynkを使用してBluetoothでコントロールしています。なかなかかわいいです♪

Blynk設定

前回のアプリ設定にジョイスティックウィジットを追加しただけです。

 

ジョイスティックの設定はx軸とy軸の出力をMERGEしてヴァーチャルピンV4に出力させ、それぞれ値は-5~5としました。y軸の値で前進/後進をx軸で左右旋回動作します。値が大きくなるほど動作速度が上がります。

Arduino IDEプログラム

前回とほぼ同様のコードです。カルマン・フィルタによって傾斜角と角速度の推定値を導出してPID制御によってモータをフィードバック制御しております。

#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;

int mXL = 0, mXR = 0, mY = 0;


float powerL = 0.0, powerR = 0.0;

float thetaLP, thetaRP, thetaLI, thetaRI, 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;

  thetaLI = 0.0;
  thetaRI = 0.0;

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


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

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

void writeMotorResisterR(byte vset, byte data1){
  int vdata = vset << 2 | data1;
  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!!!!!!!!!!!!!!!!!!!!!!!!!!!!");
}

BLYNK_WRITE(V4) {
  int x = param[0].asInt();
  int y2 = param[1].asInt();

  if(y2 != 0){
    mY = y2;
    mXL = 0;
    mXR = 0;
  }else if(x != 0){
    mY = 0;
    mXL = x;
    mXR = -x;
  }else{
    mY = 0;
    mXL = 0;
    mXR = 0;
  }
}

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
  writeMotorResisterL(0x00, 0x00);
  writeMotorResisterR(0x00, 0x00);

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


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

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

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

  thetaLP = (theta_data[0][0] + float(mY) )/ 90.0;
  thetaRP = (theta_data[0][0] + float(mY)) / 90.0;
  thetaLI += thetaLP;
  thetaRI += thetaRP;
  thetaD = theta_dot2 / 250.0;

  powerL = (thetaLP + float(mXL)/90.0) * Kp + thetaLI * Ki + thetaD * Kd;
  powerR = (thetaRP + float(mXR)/90.0)* Kp + thetaRI * Ki + thetaD * Kd;
  powerL = max(-1.0, min(1.0, powerL));
  powerR = max(-1.0, min(1.0, powerR));

  int VdataL = ((float)(VMAX - VMIN) * fabs(powerL)) + VMIN;
  int VdataR = ((float)(VMAX - VMIN) * fabs(powerR)) + VMIN;

  if(fabs(theta_data[0][0]) > 60 || (VdataL == 0 && VdataR == 0)){
    writeMotorResisterL(0x00, 0x00);
    writeMotorResisterR(0x00, 0x00);
  }else{
    if(powerL > 0){
      writeMotorResisterL(byte(VdataL), 0x01);
    }else if(powerL < 0){
      writeMotorResisterL(byte(VdataL), 0x02);
    }
    
    if(powerR > 0){
      writeMotorResisterR(byte(VdataR), 0x01);
    }else if(powerR < 0){
      writeMotorResisterR(byte(VdataR), 0x02);
    }
  }

  
  
  //tickerUpdate.attach(theta_update_interval, update_theta);
  delay(WAIT);
}

前回コードとの主な差異

  • L421 – 435
    旋回動作を実現させるためにモータドライバの制御を左右のモータで関数を分離しました。これにより左右のモータを異なる動作の指示ができます。
     
  • L459-476
    ジョイスティックの値をヴァーチャルピンV4で受けます。
    y軸の値で前進/後進、x軸の値で旋回動作させます。
     
  • L520, 521
    推定された傾斜角(theta_data[0][0])にジョイスティックy軸の値を足すことで倒立振子の前進(+y)と後進(-y)を実現させています。
     
  • L526, 527
    PIDを係数かけてモータ出力を導出する前に比例項にジョイスティックx軸の値を足すことで倒立振子の旋回動作を実現します。左右それぞれx軸の値を反転させて(L469, 470)足して旋回動作させています。

 

おわりに

PID制御でも動作コントロールできることを確かめました。しかしまだ外乱には弱いですしもっと安定した振子にしたいです。でもロータリエンコーダは使用しないカジュアルな倒立振子にしたいものです。

折角M5StickCが小さくて半端ない集積でもってカワイイのですから。粛々と勉強を進めます。

追記

久々に動かしてみた (2020/3/19)

M5StickC で倒立振子 PID制御編 ー倒立振子への道 3ー

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

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

[amazonjs asin=”B07RS8ZTJ3″ locale=”JP” title=”トランジスタ技術 2019年 07 月号”]

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

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

 

 

まずは作ってみた

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

 

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

頑張って改良した

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

 

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

 

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

 

構成

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

部品

  • M5StickC
    [amazonjs asin=”B07R1QDVWF” locale=”JP” title=”2019 M5StickC ESP32 PICOミニIoT開発ボードフィンガーコンピューターカラーLCD付き”]
     
  • I2Cモータードライバ・モジュール DRV8830
    [bc url = “https://strawberry-linux.com/catalog/items?code=12030”]
     
  • ギアドモータ
    [amazonjs asin=”B01H6WZDO2″ locale=”JP” title=”ホビー用ギアドモーター(2個入)”]
     
  • スライドスイッチ
    [amazonjs asin=”B019Q620WO” locale=”JP” title=”uxcell SPDTパネル取付 ミニスライドスイッチ 半田ラグ SS12F48 On-On 2ポジション 10個”]
     
  • 電池ボックス 単3×1本
    [amazonjs asin=”B012VTXHM0″ locale=”JP” title=”uxcell 電池ホルダー 電池ケース ボックス 1.5V 単3電池1本 7本ホルダー入り”]
     
  • 電池ボックス 単3×2本
    [amazonjs asin=”B01H1P5PNS” locale=”JP” title=”uxcell 電池ケース バッテリーボックス スプリングクリップ 2 x 単3形 1.5V電池ボックスケース 収納ボックス 8個入り”]

マシン

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

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

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;

 

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);
}

 

参考

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

動作

調整中の様子。

 

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

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

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

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

次の記事

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

傾斜計 カルマン・フィルタ アルゴリズムの解きほぐし ー倒立振子への道 2ー

前回トラ技を参考に作ったM5stickCを用いた傾斜計ではカルマン・フィルタの効能を堪能しただけで、そのカルマン・フィルタが何をしているかはすっ飛ばして見てみぬフリをしておりました。

カルマン・フィルタで M5stickC 傾斜計 ー倒立振子への道 1ー

ここでは前回用いたコードのカルマン・フィルタ アルゴリズムを少し解きほぐし何をしているのかをみてみたいと思います。

しかし内容がかなり難しくここでも様々な点をすっ飛ばさなくてはなりません。カルマン・フィルタが制御工学や統計学など様々な基礎学問の上に成り立っているからです。

でも気にしてはいられません。この道の目的はM5stickCを用いたオリジナルの倒立振子の実現です。今から統計や制御理論を1から学ぶのは得策ではありません。挫折するに決まっています(お父ちゃんの場合)。

漫画家見習いが鉛筆の素材の木を植えるところから始めるわけにはいかないのですから。

 

 

傾斜計コード

前回のコードを改めて記載いたします。

トラ技のソースコードInclinometer.cppをほぼ丸々参考にしています(p. 49 – 52)。

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

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

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

//加速度センサ オフセット
int16_t accXoffset = 1020;
int16_t accYoffset = 450;
int16_t accZoffset = 958;

//ジャイロセンサ オフセット
int16_t gyroXoffset = -60;
int16_t gyroYoffset = -15;
int16_t gyroZoffset = 370;

Ticker tickerUpdate;


//センサ バラつき取得用変数
int sample_num = 100;
int meas_interval = 10;
float theta_deg;
float theta_dot_deg;
float theta_mean;
float theta_variance;
float theta_dot_mean;
float theta_dot_variance;


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

//カルマン・フィルタ処理 2.5msec(400Hz)毎に実施
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;
}

//加速度センサから傾きデータ取得 [deg]
float get_acc_data() {
  M5.IMU.getAccelData(&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.getGyroData(&gyroX,&gyroY,&gyroZ);
  //得られたセンサ値はオフセット引いて使用
  theta_dot_deg = ((float) (gyroX - gyroXoffset)) * M5.IMU.gRes;
  return theta_dot_deg;
}

//ジャイロセンサばらつき測定
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
}


void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  M5.begin();
  M5.Axp.ScreenBreath(8);
  M5.IMU.Init();
  M5.Lcd.setRotation(1);
  M5.Lcd.fillScreen(BLACK);

  //初期設定開始 LED ON
  pinMode(10, OUTPUT);
  digitalWrite(10, LOW);

  //センサばらつき取得 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;


  //Timer 2.5msec(400 Hz)ごとにカルマン・フィルタで角度導出
  tickerUpdate.attach(theta_update_interval, update_theta);

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


void loop() {
  /*
  //加速度センサによる傾斜角
  Serial.print(theta_deg);
  Serial.print(", ");
  //カルマン・フィルタによる角度の推定値
  Serial.println(theta_data[0][0]);
  */

  //ジャイロセンサによるx軸角速度
  Serial.print(theta_dot_deg);
  Serial.print(", ");
  //カルマン・フィルタによる角速度の推定値
  Serial.println(theta_dot_deg - theta_data[1][0]);
  delay(50);
}

ここではカルマン・フィルタ アルゴリズム処理 update_theta()を解きほぐします。

カルマン・フィルタの概要

カルマン・フィルタは”センサの測定値”と”モデルから算出した予測値”を元に真値を推定するフィルタです。下は傾斜計の場合のブロック図です。

実際どういった処理を施して傾斜角の推定値を得ているのかコードより解きほぐしてみましょう。

まずはフィルタに導入する測定値と予測値についてみていきましょう。

センサによる測定値

M5stickCに内蔵されている6軸の慣性センサ(加速度、ジャイロ) SH200Q を使用して傾斜角と角速度を測ります。

[amazonjs asin=”B07R1QDVWF” locale=”JP” title=”2019 M5StickC ESP32 PICOミニIoT開発ボードフィンガーコンピューターカラーLCD付き (1セット)”]

傾斜角θは加速度センサから重力加速度の傾きを検出して導出します。

角速度はジャイロセンサの値(x軸)をそのまま使用します。

数理モデルによる予測値

予測値を得るために物理現象を記述する式(数理モデル)を立てます。

傾斜角の関係式は以下の通りです。

$$θ_{k+1} = (ω_k – ω_{offset})・Δt + θ_k$$

$θ_k $は時刻kのときの傾斜角で$ω_k $は時刻kのときの角速度です。$w_{offset}$は角速度のオフセット誤差です。
時間kのときの角速度に微小時間$Δt $(コードでは2.5msec)をかけて$θ_k $を足すと$Δt $後(時間 k+1)の傾斜角$θ_{k+1}$が得られるということを示しています。

またここで簡単のため角速度(ジャイロセンサ)のオフセットは時刻によらず一定と仮定します。

$$ω_{offset}(k+1) = ω_{offset}(k)=ω_{offset}$$

ここですっ飛ばしブチかましますが、現代制御理論では数理モデルを行列で表現する習わしがあるそうです。

状態方程式

物理状態の移り変わりを示すのが状態方程式です。上記2式より以下で表せます。

$$\left( \begin{array}{c}  θ_{k+1}\\ ω_{offset} \end{array} \right)=\left( \begin{array}{cc}  1&-Δt\\0&1 \end{array} \right)\left( \begin{array}{c}  θ_{k}\\ ω_{offset} \end{array} \right)+\left( \begin{array}{c} Δt\\ 0 \end{array} \right)ω_k$$

一般式では以下のように書きます。

$$x_{k+1}=A・x_k+B・u_k$$

ここでの物理状態$x$ ($θ_k$と$ω_{offset}$)は行列Aをかけて2項目の入力$ω_k$(角速度)に関する値を足すと次の状態があられることを示しています。

出力方程式

物理状態で実際に観測できる状態(値)を示す行列式です。

$$θ_k=\left( \begin{array}{cc} 1&0\end{array} \right)\left( \begin{array}{c}  θ_{k}\\ ω_{offset} \end{array} \right)$$

$θ_{k}$だけがセンサから直接得られることを示しています。$ω_{offset}$だけを得ることはできませんのでね。

一般式は以下で書きます。

$$y_k=C・x_k$$

以上で数理モデルが用意できました。また行列A, B, Cは時間依存のない定数でした。

 

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

やっとコードのカルマン・フィルタ アルゴリズムupdate_theta()が何をやっているかをみてみます。かなりすっ飛ばします。とにかくここではコード上でどんな処理をしているかをみるのが目的です。よく理解できないし。。ここでは丸飲みします。

update_theta()は以下の5つの処理を実行しています。

  1. カルマン・ゲイン$G_k$の算出
  2. 現時点kの推定値$\hat x_k$の算出
  3. 現時点kの共分散行列$P_k$の算出
  4. 次時刻k+1の予測値$\tilde x_{k+1}$の算出
  5. 次時刻k+1の共分散行列$P’_{k+1} $の算出

1. カルマン・ゲイン算出

カルマン・ゲイン$G_k$とはモデルによる予測値$\tilde x_k$とセンサによる測定値$y_k$を混ぜ合わせて推定値$\hat x_k$を求める際の重みづけ配分比を意味します。

そして詳細は分かりませんが$G_k$は以下の式で得られます。

$$G_k=P’_k C^T(W + C P’_k C^T)^{-1}$$

   $P’_k$:前時刻に算出された共分散行列(5. で算出)
   $C^T$:出力方程式の係数行列C = $\left( \begin{array}{cc} 1&0\end{array} \right)$の転置。つまり$\left( \begin{array}{c} 1\\0\end{array} \right)$
   $W$:観測雑音の程度を表す量。つまり加速度センサによる角度の分散(theta_variance)

2. 推定値算出

前時刻に算出された予測値(4. で算出)$\tilde x_{k}$と測定値$y_k = θ_{k}$からカルマン・ゲイン$G_k$を絡めて推定値$\hat x_k$を求めます。

$$\hat x_k=\tilde x_k+G_k(y_k – C \tilde x_k)$$

3. 共分散行列算出

共分散行列とはデータをxとyとすると$\left( \begin{array}{cc}  {σ_x}^2 & {σ_{xy}}^2\\ {σ_{xy}}^2 & {σ_y}^2\end{array} \right)$で表されます。

要するにデータをxとyの変動の相関を表します。
データxとyに相関がない場合は分散$ {σ_{xy}}^2 = 0$となります。

物理状態$θ_k$の共分散行列$P_k$は以下で表されます。例によってなんでかは知りません。

$$P_k = (I_2 – G_k C) P’_k$$

   $I_2$:2行2列の単位行列$\left( \begin{array}{cc} 1&0\\0&1\end{array} \right)$
   $G_k$:カルマン・ゲイン
   $C$:出力方程式の係数行列C = $\left( \begin{array}{cc} 1&0\end{array} \right)$
   $P’_k$:前時刻に算出された共分散行列(5. で算出)

4. 次時刻の予測値算出

モデルを用いて次時刻k+1の傾斜角予測値$\tilde x_{k+1}$を算出します。

$$\tilde x_{k+1} = A \hat x_k + B \bar u_k$$

   $A$:状態方程式の係数行列$\left( \begin{array}{cc}  1&-Δt\\0&1 \end{array} \right)$
   $B$:状態方程式の係数行列$\left( \begin{array}{c}  Δt\\0\end{array} \right)$
   $ \hat x_k$:現時刻の傾斜角推定値(2. で算出)
   $ \bar u_k$:入力(角速度)の平均。ここでは現時刻の角速度測定値$ω_k$で近似。

5. 次時刻の共分散行列算出

これもよくわかりませんが次時刻k+1の共分散行列は以下で得られます。

$$P’_{k+1} = A P_k A^T + B U B^T$$

   $P_k$:現時刻に算出された共分散行列(3. で算出)
   $U$:入力雑音の程度を表す量。つまり角速度の分散(theta_dot_variance)

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

カルマン・フィルタの処理は前回算出した予測値と共分散行列を用いて推定を行います。常に前のデータを必要とするので最初に起動(k=0)する際には手動で初期値を指定する必要があります。

アルゴリズムの4. と5. で算出する次時刻の予測値が時刻0の時点ではないのであらかじめそれらしい値を決める必要があるということです。

コードでは初期の傾斜は直立であると条件付けして初期値を与えています。

状態予測値の初期設定

$\tilde x_0 = \left( \begin{array}{c}  θ\\ ω_{offset} \end{array} \right)  = \left( \begin{array}{c}  0\\ \bar ω \end{array} \right)$

直立を仮定しているため $θ = 0$、 動かしていないと仮定して$ω_{offset}$をジャイロの平均値としています。

共分散行列の初期設定

$P’_{0} =\left( \begin{array}{cc}  {σ_θ}^2&0\\0&{σ_{ω_{offset}}}^2 \end{array} \right) =\left( \begin{array}{cc} 1&0\\0&{σ_ω}^2 \end{array} \right)$

角度のばらつきは1°程度と想定し、$ω_{offset}$のばらつきはジャイロの測定値のばらつきとしています。傾斜角と$ω_{offset}$に相関はないので対角の値は0です。

初期値の値は多少ずれがあっても時刻が進み測定データが増えると推定値は真値付近に収束するようです。

 

おわりに

傾斜計コードで使用したカルマン・フィルタ アルゴリズムの処理内容を追いかけました。

数式などは分からないことが多々ありますが、確かにその処理をしていい感じの結果を得ているのです。

とにかくカルマン・フィルタ アルゴリズムを飲み込んで数理モデル方程式を立てて初期値の設定をそれらしく実施すればよいとのことなので応用が利くことでしょう。

さて次は倒立振子に向けて動きたいと思います。

次の記事

M5StickC で倒立振子 PID制御編 ー倒立振子への道 3ー

カルマン・フィルタで M5stickC 傾斜計 ー倒立振子への道 1ー

今月のトランジスタ技術が非常に興味深い内容でした。

[amazonjs asin=”B07RS8ZTJ3″ locale=”JP” title=”トランジスタ技術 2019年 07 月号”]

 

確率統計コンピューティング特集ということでカルマン・フィルタの歴史とカルマン倒立振子の作り方を非常に丁寧に紹介しています。

必要な数学や力学の基礎も記載されておりロボット制御の入門として最適なのではと思います。DVDでのセミナーも易しくありがたい限りです。

[bc url =”https://toragi.cqpub.co.jp/tabid/906/Default.aspx”]

はっきり言ってお父ちゃんはカルマン・フィルタとは何ぞやというレベルで全く知識のない状況です。そこで倒立振子への道と題してM5StickCを用いた倒立振子の実現を目指して一歩ずつお勉強していこうと思います。

ここではトラ技のコードを参考にカルマン・フィルタを用いた傾斜計を作ってみたいと思います。カルマン・フィルタの素晴らしさをまずは体験してみようと思います。

 

 

M5stickC

M5stickCには6軸の慣性センサ(加速度、ジャイロ) SH200Q が内蔵されていますので、このセンサ値を用いてM5stickC自身の傾きを算出してみます。

[amazonjs asin=”B07R1QDVWF” locale=”JP” title=”2019 M5StickC ESP32 PICOミニIoT開発ボードフィンガーコンピューターカラーLCD付き”]

6軸センサ SH200Q

SH200Q データシート
 https://github.com/m5stack/M5-Schematic/blob/master/Core/SH200Q.pdf

SH200QのArduino IDEサンプルコードは以下にございます。
 https://github.com/m5stack/M5StickC/blob/master/examples/Basics/SH200I/SH200I.ino 

M5StickCのXYZ軸方向は以下の通りです。

参考:144Labグループ開発者ブログ

センサのデフォルトの設定は以下の通りです。

  • 加速度:16bit ±8 g
  • ジャイロ(角速度):16bit ±2000 deg/sec

 

上の写真のようにM5stickCを置いてSH200Q のサンプルコードを動かすとSH200Qから出力されるデータは以下のようになるはずです。

  • 加速度:AccX = 0,  AccY = 0, AccZ = 4096(+1g)
  • 角速度 : GyroX = 0, GyroY = 0, GyroZ = 0

しかしセンサのオフセットのため実際に出力される値にはずれがありましたので、今回はあらかじめオフセットを測って引いて使用します。

カルマン・フィルタを用いた傾斜計に関してはセンサのデフォルトオフセットの影響はないかもしれませんが今回は生データとの比較も行いたいのでオフセット引いております。

カルマン・フィルタによる傾斜導出の概要

ここではカルマン・フィルタの詳細理解はすっ飛ばして とにかく使ってみてご利益を実感するのを目的としています。ですので詳細はトラ技や他に譲るとしてざっくりとした概要です。

カルマン・フィルタは測定値と数理モデルによる予測値をうまいこと利用してもっともらしい傾斜角を導出します。

加速度センサによる角度

重力加速度のY軸、Z軸の成分よりM5stickCの傾斜角θは以下の式で求められます。

$$θ = tan^{-1}\left(  \dfrac{g_z}{-g_y} \right)$$

しかし加速度センサは振動などの重力加速度以外の加速度も検出してしまう問題があります。

ジャイロセンサによる角度

ジャイロセンサのx軸の角速度$ω_x$を時間積分することでM5stickCの角度を求めることができます。

しかしジャイロは初期姿勢の角度は検出できません。さらにセンサ測定値にはオフセット誤差が含まれるので誤差も積分されて増幅していきます。

モデルによる予測

上記のそれぞれ短所をもった測定値とそのバラつき誤差を加味した数理モデルをつくって傾斜角度の予測値を算出します。

測定バラつきとして予めセンサを複数回測定(ここでは100回)して分散を求めます。

モデルの詳細はまだ理解できてないので割愛します。いつか理解できるその日まで。。。

カルマン・フィルタのアルゴリズム

上記の測定値とその分散そして数理モデルによる予測値をうまいこと用いて角度の推定値を得ます。

内容は全く分からないので後に回します。トラ技紙面に譲ります。。今は分からない。。。今は。。。

傾斜計コード

Arduino IDEをもちいでカルマン・フィルタM5stickC 傾斜計を作製しました。比較のために加速度センサによる傾斜角とカルマン・フィルタによる推定値を出力します。

トラ技のソースコードInclinometer.cppをほぼ丸々参考にしています(p. 49 – 52)。

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

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

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

//加速度センサ オフセット
int16_t accXoffset = 1020;
int16_t accYoffset = 450;
int16_t accZoffset = 958;

//ジャイロセンサ オフセット
int16_t gyroXoffset = -60;
int16_t gyroYoffset = -15;
int16_t gyroZoffset = 370;

Ticker tickerUpdate;


//センサ バラつき取得用変数
int sample_num = 100;
int meas_interval = 10;
float theta_deg;
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);
//State vector
//[[theta(degree)], [offset of theta_dot(degree/sec)]]
float theta_data_predict[2][1];
float theta_data[2][1];
//Covariance matrix
float P_theta_predict[2][2];
float P_theta[2][2];
//"A" of the state equation
float A_theta[2][2] = {{1, -theta_update_interval}, {0, 1}};
//"B" of the state equation
float B_theta[2][1] = {{theta_update_interval}, {0}};
//"C" of the state equation
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;
}

//加速度センサから傾きデータ取得 [deg]
float get_acc_data() {
  M5.IMU.getAccelData(&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.getGyroData(&gyroX,&gyroY,&gyroZ);
  //得られたセンサ値はオフセット引いて使用
  float x_data = ((float) (gyroX - gyroXoffset)) * M5.IMU.gRes;
  return x_data;
}

//ジャイロセンサばらつき測定
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()
{     
    //measurement data
    float y = get_acc_data(); //degree
    
    //input data
    float theta_dot_gyro = get_gyro_data(); //degree/sec
      
    //calculate Kalman gain: 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_data estimation: 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);
           
    //calculate covariance matrix: 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'
      
    //predict the next step data: 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 
    
    //predict covariance matrix: 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
}

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  M5.begin();
  M5.Axp.ScreenBreath(8);
  M5.IMU.Init();
  M5.Lcd.setRotation(1);
  M5.Lcd.fillScreen(BLACK);

  //初期設定開始 LED ON
  pinMode(10, OUTPUT);
  digitalWrite(10, LOW);

  //センサばらつき取得 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;


  //Timer
  //2.5msec(400 Hz)ごとにカルマン・フィルタで角度導出
  tickerUpdate.attach(theta_update_interval, update_theta);

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


void loop() {
  //加速度センサによる傾斜角
  Serial.print(theta_deg);
  Serial.print(", ");
  //カルマン・フィルタによる角度の推定値
  Serial.println(theta_data[0][0]);
  delay(50);
}

 

やってることの概要は以下の通りです。

  • センサばらつき算出:加速度とジャイロセンサを100回測って予め分散を導出
  • カルマン・フィルタの初期設定:使用する行列パラメータの初期値設定
  • タイマ割り込み(2.5msecごと)でカルマン・フィルタアルゴリズム発動させて傾斜角の推定値を算出
  • 50msecごとに加速度センサによる傾斜角とカルマン・フィルタによる角度の推定値をシリアル出力

カルマン・フィルタアルゴリズムについての詳細はしつこいですがトラ技紙面か以下の動画を参照ください!

 

 

動作

青線:M5stickCの加速度センサから算出した傾斜角で
赤線:カルマン・フィルタによる角度の推定値

 

最初の感想は”凄い!”でした。振ると当然 加速度の外乱が入るので、加速度センサによる角度(青線)は乱れますがカルマン・フィルタによる角度の推定値(赤線)に大きな乱れはありません。

しかも加算平均とかではなくあくまでリアルタイムのセンサ値と前回算出した予測値で算出されているようなのです。

なんかすごいね。。理解できないけど。

 

今回スタートした 倒立振子への道 。どうなるものか。たどりつけるのか。

全くの未知ですが 今回カルマン・フィルタのご利益を全身で堪能出来て興奮気味なので頑張りたいです!!

次の記事

M5StickC で倒立振子 PID制御編 ー倒立振子への道 3ー