1．はじめに

前回の記事では、M5GoとRoller485 Liteを使って倒立振子を作成しました。車体を手で直立させた状態から制御を開始し、フローリング上でも倒立させることができました。

前回の記事はこちらです。

しかし、この方法では倒立させるたびに、人が車体を直立させる必要があります。

そこで今回は、倒れた状態から車体を自動的に起こし、そのまま倒立制御へ移行する機能を追加してみました。

操作方法は次のとおりです。

車体を倒立状態（直立位置）にして電源を入れる
そのまま動かさず、IMUのキャリブレーションが終了するまで待つ
キャリブレーション終了後、車体を傾けて床に置く
M5GoのAボタンを押す
車体が自動的に起き上がり、そのまま倒立制御へ移行する

車体が起立する動作は次のようになりました。

Aボタンを押した後は、車体を手で起こす必要はありません。モータで車体を起こし、直立付近で減速した後、自動的に通常の倒立制御へ切り替わります。

今回は、この自動起立機能について紹介します。

2．自動起立の考え方

前回のプログラムでは、基本的にPD制御だけで倒立させていました。

今回も倒立中の制御そのものは同じですが、倒れた状態からいきなりPD制御を行ってもうまく起き上がることはできません。

そこで、車体の状態を次の4つに分けました。

状態	内容
WAIT_START	Aボタンが押されるまで待機
STARTUP	車体を直立方向へ起こす
BRAKE	直立付近で回転を減速する
BALANCE	PD制御で倒立する

プログラムでは、次のように状態を定義しています。

enum RobotState {
  WAIT_START,     // Button A待ち
  STARTUP,        // 起立
  BRAKE,          // 直立付近で減速
  BALANCE         // 通常のPD倒立
};

RobotState state = WAIT_START;

つまり、待機 → 起立 → 減速 → 倒立 と制御を順番に切り替えるようにしました。

3．起立動作

キャリブレーションが終了したら、車体を傾けて床に置きます。

この状態ではモータは停止しており、Aボタンが押されるまでWAIT_START状態で待機します。

Aボタンを押すと、そのときの車体角度を確認します。

if (M5.BtnA.isPressed()) {

  angle = accelAngle;

  if (
    fabs(angle) > BALANCE_START_ANGLE &&
    fabs(angle) < STARTUP_MAX_ANGLE
  ) {

    state = STARTUP;
  }
}

今回のプログラムでは、起立開始可能な最大角度を45°としました。

const float STARTUP_MAX_ANGLE = 45.0f;

Aボタンを押したときの角度が起立可能な範囲であれば、STARTUP状態へ移行します。

STARTUPでは、車体が傾いている方向を角度の正負から判定し、直立方向へ起こすようにモータへ一定の電流を流します。

if (angle > 0.0f) {
  motorCurrent = -STANDUP_CURRENT;
}
else {
  motorCurrent = STANDUP_CURRENT;
}

起立時の電流は次の値としました。

const int32_t STANDUP_CURRENT = 20000;

これによって車輪が動き、車体を直立方向へ起こします。

4．そのままPD制御に切り替えるとうまくいかない

最初に考えたのは、ある程度まで起き上がったら、そのままPD制御に切り替える という方法です。

しかし、実際には車体が起き上がった時点で角速度を持っています。その状態で通常の倒立制御に切り替えると、勢いが大きすぎて直立位置を通過してしまいます。そこで今回は、STARTUPとBALANCEの間に**BRAKE（減速）**という状態を設けました。

車体の角度が±12°以内まで起き上がると、BRAKEへ移行します。

const float BRAKE_START_ANGLE = 12.0f;

if (fabs(angle) <= BRAKE_START_ANGLE) {
  state = BRAKE;
}

5．直立付近でブレーキをかける

BRAKEでは、ジャイロセンサから得られる角速度と逆方向にモータを動かします。

if (gx > 0.0f) {
  motorCurrent = -BRAKE_CURRENT;
}
else {
  motorCurrent = BRAKE_CURRENT;
}

今回のブレーキ電流は次の値としました。

const int32_t BRAKE_CURRENT = 20000;

考え方としては、PD制御のD項と似ています。

角速度が大きければ、車体が直立位置を勢いよく通過しようとしていることになります。

そこで、角速度と逆方向にモータを動かして減速させます。

6．倒立制御への切り替え

車体が直立付近まで来て、さらに角速度もある程度小さくなったところで、通常のPD制御へ切り替えます。

今回の条件は

車体角度：±8°以内
角速度：±100 deg/s以内

としました。

const float BALANCE_START_ANGLE = 8.0f;
const float BALANCE_START_GYRO  = 100.0f;

この2つの条件を満たすと、BALANCEへ移行します。

if (
  fabs(angle) <= BALANCE_START_ANGLE &&
  fabs(gx) <= BALANCE_START_GYRO
) {

  state = BALANCE;
}

BALANCEへ移行した後は、前回の記事で使用したPD制御を行います。

float pTerm = Kp * angle;
float dTerm = Kd * gx;

float control = pTerm + dTerm;

motorCurrent = (int32_t)(-control);

motorCurrent = constrain(
  motorCurrent,
  -MAX_CURRENT,
   MAX_CURRENT
);

今回もゲインは

float Kp = 4000.0f;
float Kd = 100.0f;

としています。

7．状態遷移で考えると分かりやすい

今回のプログラムを簡単に表すと、次のようになります。

  電源ON（直立状態）
          ↓
   IMUキャリブレーション
          ↓
    車体を傾ける
          ↓
     +------------+
     | WAIT_START |
     +------------+
          ↓
       Aボタン
          ↓
     +------------+
     |  STARTUP   |
     |   起立動作  |
     +------------+
          ↓
       ±12°以内
          ↓
     +------------+
     |   BRAKE    |
     |    減速     |
     +------------+
          ↓
  ±8°以内 ＆ 角速度低下
          ↓
     +------------+
     |  BALANCE   |
     |   PD制御    |
     +------------+

単純に「倒立するための制御式」を考えるのではなく、車体の状態によって制御方法を切り替えることが今回のポイントです。

起立時には大きく動かし、直立付近では減速し、条件が整ったところで通常の倒立制御に切り替えています。

8．プログラム

今回使用したプログラムでは、電源投入時に車体を直立させた状態でIMUのキャリブレーションを行います。

キャリブレーションでは、車体を静止させた状態で角度とジャイロセンサの値を取得し、角度のオフセットとジャイロのバイアスを求めています。

キャリブレーションが終了したら車体を傾けて床に置き、Aボタンを押します。起立可能な角度であれば、自動起立を開始します。

主な設定値は次のとおりです。

// 通常の倒立制御
float Kp = 4000.0f;
float Kd = 100.0f;

int32_t MAX_CURRENT = 40000;

float FALL_ANGLE = 35.0f;

// 自動起立
const int32_t STANDUP_CURRENT = 20000;

// この角度まで来たらBRAKEへ
const float BRAKE_START_ANGLE = 12.0f;

// BRAKE時の電流
const int32_t BRAKE_CURRENT = 20000;

// BALANCEへ切り替える条件
const float BALANCE_START_ANGLE = 8.0f;
const float BALANCE_START_GYRO  = 100.0f;

// 起立開始可能な最大角度
const float STARTUP_MAX_ANGLE = 45.0f;

これらの値は、今回作成した車体で実際に動かしながら調整したものです。

車体重量、重心位置、車輪径などが変われば、適切な値も変わると思います。

スケッチはこちらを参照ください

// 倒立振子
// 水平状態で電源オンしてキャリブレーション
// その後、倒した状態でAボタンを押すと倒立する
#include <M5Unified.h>
#include "unit_rolleri2c.hpp"
#include <math.h>

// ==========================================
// Roller485 Lite
// ==========================================
UnitRollerI2C rollerLeft;
UnitRollerI2C rollerRight;

#define SDA_PIN     21
#define SCL_PIN     22

#define LEFT_ADDR   0x64
#define RIGHT_ADDR  0x65

// ==========================================
// 状態
// ==========================================
enum RobotState {
  WAIT_START,     // Button A待ち
  STARTUP,        // 起立
  BRAKE,          // 直立付近で減速
  BALANCE         // 通常のPD倒立
};

RobotState state = WAIT_START;

// ==========================================
// 通常の倒立制御
// ==========================================
float Kp = 4000.0f;
float Kd = 100.0f;

int32_t MAX_CURRENT = 40000;

float FALL_ANGLE = 35.0f;

// ==========================================
// 自動起立
// ==========================================

// 起立時の電流
const int32_t STANDUP_CURRENT = 20000;

// この角度まで来たらBRAKEへ
const float BRAKE_START_ANGLE = 12.0f;

// BRAKE時の電流
const int32_t BRAKE_CURRENT = 20000;

// BALANCEへ切り替える条件
const float BALANCE_START_ANGLE = 8.0f;
const float BALANCE_START_GYRO  = 100.0f;

// 起立開始可能な最大角度
const float STARTUP_MAX_ANGLE = 45.0f;

// ==========================================
// 制御周期
// ==========================================
const uint32_t CONTROL_PERIOD_US = 5000; // 200 Hz
const uint32_t PRINT_PERIOD_MS   = 20;   // 50 Hz

// ==========================================
// IMU
// ==========================================
float angle = 0.0f;
float angleOffset = 0.0f;
float gxBias = 0.0f;

// ==========================================
// 時間
// ==========================================
uint32_t previousControlMicros = 0;
uint32_t startMillis = 0;
uint32_t lastPrintMillis = 0;

// ==========================================
// モータ
// ==========================================
void setMotorCurrent(int32_t current)
{
  rollerLeft.setCurrent(current);
  rollerRight.setCurrent(-current);
}

void stopMotor()
{
  rollerLeft.setCurrent(0);
  rollerRight.setCurrent(0);
}

// ==========================================
// 状態名
// ==========================================
const char* getStateName()
{
  switch (state) {

    case WAIT_START:
      return "WAIT";

    case STARTUP:
      return "STARTUP";

    case BRAKE:
      return "BRAKE";

    case BALANCE:
      return "BALANCE";

    default:
      return "UNKNOWN";
  }
}

// ==========================================
// 緊急停止
// ==========================================
void emergencyStop()
{
  stopMotor();

  rollerLeft.setOutput(0);
  rollerRight.setOutput(0);

  Serial.println();
  Serial.println("FALL DETECTED");
  Serial.println("MOTOR STOPPED");
  Serial.println("RESET M5GO TO RESTART");

  while (1) {
    delay(100);
  }
}

// ==========================================
// setup
// ==========================================
void setup()
{
  auto cfg = M5.config();
  M5.begin(cfg);

  Serial.begin(115200);
  delay(1000);

  // ==========================================
  // IMU
  // ==========================================
  if (!M5.Imu.begin()) {

    Serial.println("IMU initialization failed");

    while (1) {
      delay(1000);
    }
  }

  // ==========================================
  // Roller 左
  // ==========================================
  if (!rollerLeft.begin(
        &Wire,
        LEFT_ADDR,
        SDA_PIN,
        SCL_PIN,
        400000)) {

    Serial.println("Left Roller not found");

    while (1) {
      delay(1000);
    }
  }

  // ==========================================
  // Roller 右
  // ==========================================
  if (!rollerRight.begin(
        &Wire,
        RIGHT_ADDR,
        SDA_PIN,
        SCL_PIN,
        400000)) {

    Serial.println("Right Roller not found");

    while (1) {
      delay(1000);
    }
  }

  // ==========================================
  // Roller 電流制御
  // ==========================================
  rollerLeft.setOutput(0);
  rollerRight.setOutput(0);

  rollerLeft.setMode(ROLLER_MODE_CURRENT);
  rollerRight.setMode(ROLLER_MODE_CURRENT);

  rollerLeft.setCurrent(0);
  rollerRight.setCurrent(0);

  // ==========================================
  // IMUキャリブレーション
  //
  // ★ここではロボットを手で直立させる
  // ==========================================
  Serial.println();
  Serial.println("Keep robot UPRIGHT and stationary.");
  Serial.println("Calibrating IMU...");

  float angleSum = 0.0f;
  float gxSum = 0.0f;

  const int CALIBRATION_SAMPLES = 200;

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

    float ax, ay, az;
    float gx, gy, gz;

    M5.Imu.getAccel(&ax, &ay, &az);
    M5.Imu.getGyro(&gx, &gy, &gz);

    float accelAngle =
        atan2(ay, az) * 180.0f / PI;

    angleSum += accelAngle;
    gxSum += gx;

    delay(10);
  }

  angleOffset =
      angleSum / CALIBRATION_SAMPLES;

  gxBias =
      gxSum / CALIBRATION_SAMPLES;

  Serial.printf(
    "Angle offset = %.3f deg\n",
    angleOffset
  );

  Serial.printf(
    "Gyro X bias  = %.3f dps\n",
    gxBias
  );

  // ==========================================
  // モータ出力ON
  // ==========================================
  rollerLeft.setCurrent(0);
  rollerRight.setCurrent(0);

  rollerLeft.setOutput(1);
  rollerRight.setOutput(1);

  // ==========================================
  // 開始待ち
  // ==========================================
  Serial.println();
  Serial.println(
    "Place robot in resting position."
  );

  Serial.println(
    "Press Button A to start automatic stand-up."
  );

  // ==========================================
  // タイマー
  // ==========================================
  previousControlMicros = micros();

  startMillis = millis();
  lastPrintMillis = startMillis;

  // ==========================================
  // CSV
  // ==========================================
  Serial.println();

  Serial.println(
    "time_ms,state,angle_deg,gx_dps,current_cmd"
  );
}

// ==========================================
// loop
// ==========================================
void loop()
{
  M5.update();

  // ==========================================
  // 200 Hz
  // ==========================================
  uint32_t nowMicros = micros();

  if ((uint32_t)(nowMicros - previousControlMicros)
      < CONTROL_PERIOD_US) {

    return;
  }

  uint32_t elapsedMicros =
      nowMicros - previousControlMicros;

  previousControlMicros = nowMicros;

  float dt =
      elapsedMicros / 1000000.0f;

  if (dt <= 0.0f || dt > 0.05f) {
    dt = 0.005f;
  }

  // ==========================================
  // IMU
  // ==========================================
  float ax, ay, az;
  float gx, gy, gz;

  M5.Imu.getAccel(
    &ax,
    &ay,
    &az
  );

  M5.Imu.getGyro(
    &gx,
    &gy,
    &gz
  );

  gx -= gxBias;

  // ==========================================
  // 加速度角度
  // ==========================================
  float accelAngle =
      atan2(ay, az) * 180.0f / PI;

  accelAngle -= angleOffset;

  // ==========================================
  // 相補フィルタ
  // ==========================================
  const float FILTER_TAU = 0.5f;

  float alpha =
      FILTER_TAU / (FILTER_TAU + dt);

  // WAIT中はジャイロ積分誤差を溜めないため
  // 加速度角度をそのまま使用
  if (state == WAIT_START) {

    angle = accelAngle;
  }
  else {

    angle =
        alpha * (angle + gx * dt)
      + (1.0f - alpha) * accelAngle;
  }

  int32_t motorCurrent = 0;

  // ==========================================
  // WAIT_START
  // ==========================================
  if (state == WAIT_START) {

    motorCurrent = 0;

    // ------------------------------------------
    // Button Aで自動起立開始
    // ------------------------------------------
    if (M5.BtnA.isPressed()) {

      angle = accelAngle;

      if (
        fabs(angle) > BALANCE_START_ANGLE &&
        fabs(angle) < STARTUP_MAX_ANGLE
      ) {

        state = STARTUP;

        startMillis = millis();

        Serial.println();
        Serial.printf(
          "START angle = %.3f deg\n",
          angle
        );

        Serial.println(
          ">>> START AUTOMATIC STAND-UP <<<"
        );
      }
      else {

        Serial.println();
        Serial.printf(
          "Cannot start: angle = %.3f deg\n",
          angle
        );

        Serial.println(
          "Place robot in resting position."
        );

        delay(300);
      }
    }
  }

  // ==========================================
  // STARTUP
  // ==========================================
  else if (state == STARTUP) {

    // ------------------------------------------
    // 約30°から直立方向へ起こす
    // ------------------------------------------
    if (angle > 0.0f) {

      motorCurrent =
          -STANDUP_CURRENT;
    }
    else {

      motorCurrent =
          STANDUP_CURRENT;
    }

    // ------------------------------------------
    // ±12°以内に入ったらBRAKEへ
    // ------------------------------------------
    if (fabs(angle) <= BRAKE_START_ANGLE) {

      state = BRAKE;

      Serial.println();
      Serial.println(
        ">>> SWITCH TO BRAKE <<<"
      );
    }
  }

  // ==========================================
  // BRAKE
  // ==========================================
  else if (state == BRAKE) {

    // ------------------------------------------
    // 角速度と逆方向へ電流を与えて減速
    //
    // 通常のD制御と同じ考え方
    // motorCurrent = -Kd * gx
    // ------------------------------------------
    if (gx > 0.0f) {

      motorCurrent =
          -BRAKE_CURRENT;
    }
    else {

      motorCurrent =
          BRAKE_CURRENT;
    }

    // ------------------------------------------
    // 直立付近かつ角速度が低くなったら
    // BALANCEへ移行
    // ------------------------------------------
    if (
      fabs(angle) <= BALANCE_START_ANGLE &&
      fabs(gx) <= BALANCE_START_GYRO
    ) {

      state = BALANCE;

      Serial.println();
      Serial.println(
        ">>> SWITCH TO BALANCE <<<"
      );

      // 切替えた周期からPD制御
      float pTerm =
          Kp * angle;

      float dTerm =
          Kd * gx;

      float control =
          pTerm + dTerm;

      motorCurrent =
          (int32_t)(-control);

      motorCurrent =
          constrain(
            motorCurrent,
            -MAX_CURRENT,
             MAX_CURRENT
          );
    }
  }

  // ==========================================
  // BALANCE
  // ==========================================
  else if (state == BALANCE) {

    // ------------------------------------------
    // 転倒判定
    // ------------------------------------------
    if (fabs(angle) > FALL_ANGLE) {

      emergencyStop();
    }

    // ------------------------------------------
    // 元のPD制御
    // ------------------------------------------
    float pTerm =
        Kp * angle;

    float dTerm =
        Kd * gx;

    float control =
        pTerm + dTerm;

    motorCurrent =
        (int32_t)(-control);

    motorCurrent =
        constrain(
          motorCurrent,
          -MAX_CURRENT,
           MAX_CURRENT
        );
  }

  // ==========================================
  // モータ
  // ==========================================
  setMotorCurrent(motorCurrent);

  // ==========================================
  // CSV 50 Hz
  // ==========================================
  uint32_t nowMillis = millis();

  if ((uint32_t)(nowMillis - lastPrintMillis)
      >= PRINT_PERIOD_MS) {

    lastPrintMillis = nowMillis;

    uint32_t t =
        nowMillis - startMillis;

    Serial.printf(
      "%lu,%s,%.3f,%.3f,%ld\n",
      t,
      getStateName(),
      angle,
      gx,
      (long)motorCurrent
    );
  }
}

9．おわりに

今回は、前回作成した倒立振子に自動起立機能を追加しました。

操作としては、直立状態で電源を入れてIMUをキャリブレーションし、その後に車体を傾けてAボタンを押すだけです。Aボタンを押した後は、起立から倒立制御への切り替えまで自動で行います。

当初は、倒れた状態からモータで車体を起こし、そのままPD制御へ切り替えればよいと考えていました。

しかし、実際に動かしてみると、起立時の勢いをどのように抑えて通常の倒立制御へつなぐかがポイントになりました。

そこで、WAIT_START → STARTUP → BRAKE → BALANCE と状態を分け、それぞれで異なる制御を行うようにしました。

倒立振子というと、PID制御や制御ゲインの調整に目が行きがちですが、今回のように**「現在どのような状態にあるのか」を判断して制御を切り替えることも重要**だと分かりました。

比較的単純なプログラムですが、倒れた状態から自分で起き上がり、そのまま倒立するところまで実現できました。

マイコンを使った制御を試してみる題材として、倒立振子はなかなか面白いと思います。