1
1

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?

@_Moony

M5GoとRoller485 Liteで倒立振子を作ってみる ― IMUとPD制御による電流制御 ―

1
Last updated at Posted at 2026-09-23

1. はじめに

倒立振子は制御工学の代表的な題材の一つです。機体が倒れないように傾きを検出し、その傾きを打ち消すように車輪を動かすことで姿勢を維持します。

以前から倒立振子には興味があり、これまでにも何台か試作してきました。しかし、柔らかい床では倒立できても、フローリングのような滑りやすい床ではなかなか安定しませんでした。

そこで今回はM5StackのM5GOと、モータ制御ユニットのRoller485 Liteを2台使用して、二輪型の倒立振子を試作しました。

制御にはM5GOに内蔵されているIMUを使用します。IMUから取得した加速度と角速度から機体の傾きを求め、簡単なPD制御によってモータを動かします。

今回は速度制御と電流制御の2種類を試しました。その結果、今回試作した装置と制御条件では、電流制御の方が細かな前後運動が少なく、安定して倒立しました。

この記事では、最終的に使用した電流制御によるプログラムを中心に紹介します。

2. 使用した機器

今回使用した主な機器等は次のとおりです。

機器等 内容
マイコン M5GO
モータ Roller485 Lite × 2
姿勢センサ M5GO内蔵IMU
車輪 直径80 mm
機体質量 約330 g
開発環境 Arduino IDE

Roller485 Liteは左右に1台ずつ配置しました。通信方式はI2Cとし、アドレスは左側を0x64、右側を0x65としています。二つのモータはGROVE HUBを中継してM5GOに接続しています。

M5GOのGrove Port Aを使用し、SDAをGPIO21、SCLをGPIO22として接続しました。

PXL_20260922_114838192 (1).jpg

PXL_20260921_081420045.jpg

PXL_20260921_081405239.jpg

PXL_20260921_081427079.jpg

図1　試作した倒立振子の外観

※機体は以下の記事を参考に作成しました。

左右のモータは向かい合わせに取り付けています。このため、機体を同じ方向へ走らせるには、左右のモータへ符号を反転した指令値を与える必要があります。

例えば、電流制御では次のようにしています。

rollerLeft.setCurrent(current);
rollerRight.setCurrent(-current);

3. 倒立制御の考え方

今回の制御の流れを図2に示します。

image.png

図2　倒立制御の流れ

制御は　IMUによる加速度・角速度の取得 → 傾斜角度の算出 → PD制御 → モータへの指令 → 機体姿勢の変化　という流れになります。これを短い周期で繰り返すことで、機体が傾いたときに車輪を動かし、倒れないように姿勢を戻します。

加速度と角速度はM5GOのIMUから取得し、前後方向の傾斜角度を次のように求めました。

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

ここに、ayとazは前後方向と上下方向の加速度です。accelAngleは、機体をほぼ垂直にした状態を0°とすると、今回の座標系では前方へ傾けると負、後方へ傾けると正となります。

ただし、加速度だけから求めた角度は振動などの影響を受けます。一方、ジャイロから得られる角速度を積分すると、時間とともに誤差が蓄積します。そこで、両者を組み合わせる相補フィルタを使用しました。

float alpha = FILTER_TAU / (FILTER_TAU + dt);
angle = alpha * (angle + gx * dt) + (1.0f - alpha) * accelAngle;

ジャイロによる短時間の姿勢変化と、加速度から求めた傾斜角度を組み合わせることで、制御に使用する機体の傾斜角度を求めています。

4. PD制御

倒立制御には、比較的簡単なPD制御を使用しました。

今回のプログラムでは

float control = Kp * angle + Kd * gx;

として制御量を計算しています。

angleは機体の傾斜角度、gxはIMUから取得した角速度です。

つまり

P制御：現在どのくらい傾いているか
D制御：どのくらいの速さで傾いているか

の2つを使ってモータを動かしています。

一般的なPD制御では偏差を微分してD項を求める方法がありますが、今回はIMUから角速度を直接取得できるため、gxをD項として使用しています。

実際のモータ指令では、機体の座標系とモータの回転方向に合わせて符号を反転します。

int32_t motorCurrent = (int32_t)(-control);

制御周期は約5 ms、すなわち約200 Hzとしました。

5. 最初は速度制御を試行

最初はRoller485 Liteを速度制御モードで使用しました。考え方は電流制御とほぼ同じで、PD制御によって求めた値をモータの速度指令値として与えます。

float control = Kp * angle + Kd * gx;
int32_t motorSpeed = (int32_t)(-control);

rollerLeft.setSpeed(motorSpeed);
rollerRight.setSpeed(-motorSpeed);

この方法でも倒立させることはできました。

しかし、フローリング上で動作させると、倒立中に機体が細かく前後運動する現象が見られました。
Kp、Kdを変更して調整しましたが、今回の装置では、この細かな前後運動を十分に抑えることができませんでした。そこで、次にモータへの指令方法を速度から電流へ変更してみました。

6. 電流制御に変更

Roller485 Liteを電流制御で動作させ、PD制御で求めた値を電流指令値として与えました。基本的な考え方は速度制御の場合と同じです。

float control = Kp * angle + Kd * gx;
int32_t motorCurrent = (int32_t)(-control);

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

rollerLeft.setCurrent(motorCurrent);
rollerRight.setCurrent(-motorCurrent);

今回使用した主な制御パラメータは次のとおりです。

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

int32_t MAX_CURRENT = 40000;
float FALL_ANGLE = 35.0f;

機体が±35°以上傾いた場合は、転倒したと判断してモータを停止させます。

電流制御へ変更したところ、速度制御で見られた細かな前後運動が大幅に減少しました。

フローリング上でも比較的静かに倒立し、機体を指で軽く押した場合にも、車輪が移動して姿勢を戻す動作が確認できました。

7. 速度制御と電流制御との比較

動作の違いを確認するため、シリアルモニタへ傾斜角度と角速度を出力しました。

傾斜角度の比較を図3に示します。

image.png

図3　速度制御と電流制御における傾斜角度

速度制御では、傾斜角度が周期的に変動しています。一方、電流制御では変動が小さくなっています。

次に、角速度の比較を図4に示します。

image.png

図4　速度制御と電流制御における角速度

こちらも速度制御では大きな変動が見られますが、電流制御では0°/s付近の比較的小さな変動となりました。

これらの結果から、今回試作した機体と制御条件では、電流制御に変更することで倒立中の細かな前後運動を抑えることができました。

ただし、この結果だけから「倒立振子には速度制御より電流制御が適している」と一般化することはできません。

速度制御でも、制御方法や使用する状態量、ゲイン、モータ側の制御パラメータなどによって安定した倒立は可能と考えられます。今回の結果は、あくまで今回試作した装置とプログラムにおける比較です。

8. 電流制御版のプログラム

ここでは、今回最終的に倒立できた電流制御版のArduinoスケッチを掲載します。

ボードマネージャでM5Stack by M5Stackをインストール後、ボードのM5Stackの中のM5Coreを使用しました。

スケッチはこちらを参照ください 
#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

// ==========================================
// 制御パラメータ
// ==========================================
float Kp = 4000.0f;
float Kd = 100.0f;

int32_t MAX_CURRENT = 40000;

// この角度を超えたら転倒と判定
float FALL_ANGLE = 35.0f;

// ==========================================
// 制御周期
// ==========================================
// 5 ms = 200 Hz
const uint32_t CONTROL_PERIOD_US = 5000;

// CSV出力周期
// 20 ms = 50 Hz
const uint32_t PRINT_PERIOD_MS = 20;

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

// 加速度センサから求めた直立角度のオフセット
float angleOffset = 0.0f;

// ジャイロX軸のゼロ点バイアス
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);
}

// ==========================================
// 転倒時停止
// ==========================================
void emergencyStop()
{
  // まず電流を0にする
  stopMotor();

  // Roller出力もOFF
  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キャリブレーション
  //
  // ここでは
  //  1. 直立角度
  //  2. ジャイロX軸ゼロ点
  // の両方を測定する
  // ==========================================
  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
  );

  // ==========================================
  // 初期角度
  // ==========================================
  angle = 0.0f;

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

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

  // ==========================================
  // タイマー初期化
  // ==========================================
  previousControlMicros = micros();

  startMillis = millis();
  lastPrintMillis = startMillis;

  // ==========================================
  // CSVヘッダ
  // ==========================================
  Serial.println();
  Serial.println(
    "time_ms,angle_deg,gx_dps,current_cmd"
  );
}

// ==========================================
// loop
// ==========================================
void loop()
{
  // ==========================================
  // 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;

  // ==========================================
  // 相補フィルタ
  //
  // 時定数 0.5秒程度として
  // dtに応じて係数を計算
  // ==========================================
  const float FILTER_TAU = 0.5f;

  float alpha =
      FILTER_TAU / (FILTER_TAU + dt);

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

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

    emergencyStop();
  }

  // ==========================================
  // PD制御
  // ==========================================
  float pTerm =
      Kp * angle;

  float dTerm =
      Kd * gx;

  float control =
      pTerm + dTerm;

  // 実機で確認済みの方向
  int32_t 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,%.3f,%.3f,%ld\n",
      t,
      angle,
      gx,
      (long)motorCurrent
    );
  }
}

プログラムの処理は、大きく分けると次のようになります。

  1. M5GOとRoller485 Liteの初期化
  2. IMUの初期化
  3. 静止状態で傾斜角度とジャイロのオフセットを取得
  4. 5 msごとにIMUの読み取り
  5. 相補フィルタで傾斜角度を算出
  6. PD制御で電流指令値を計算
  7. 左右のRoller485 Liteへ指令
  8. 傾斜角度が±35°を超えた場合は停止

なお、Kp、Kdなどの値は、機体質量、重心位置、車輪径などによって変わります。そのため、この値をそのまま別の機体へ使用しても、同じように倒立するとは限りません。

最初はモータの電流上限を小さく設定し、機体を手で支えながら動作方向を確認する方が安全です。

9. 生成AIを使ってプログラムを作成

今回のプログラム作成には生成AIを利用しました。

最初から完成したプログラムが得られたわけではありません。

例えば、

  • IMUのどの軸が前後方向の傾きに対応するか
  • 機体を傾けたときに角度の符号がどう変化するか
  • 左右のモータをどちらの方向へ回すか
  • 速度制御と電流制御のどちらを使うか
  • Kp、Kdをどの程度にするか

といった点は、実際の機体を動かしながら確認しました。

生成AIにプログラム案を作成させ、実機で動作を確認し、その結果を再び生成AIに伝えて修正する、という作業を繰り返しました。

今回の試作では、このような方法で作業を進め、2～3時間で安定して倒立するところまで到達しました。

生成AIを使うことで、制御プログラムを作成するハードルはかなり下がったように感じます。一方、生成されたプログラムが実機で正しく動作するかどうかは、実際に確認する必要があります。

10. おわりに

今回は、M5GOとRoller485 Liteを使用して、二輪型の倒立振子を試作しました。

M5GOに内蔵されたIMUから傾斜角度と角速度を取得し、比較的簡単なPD制御によって倒立させることができました。

また、速度制御と電流制御を試したところ、今回の装置では、電流制御に変更することで倒立中の細かな前後運動が大幅に減少しました。

倒立振子というと、状態方程式やLQRなどを使った本格的な制御を思い浮かべるかもしれません。しかし今回の試作では、傾斜角度と角速度を使った比較的簡単なPD制御でも、実際に倒立させることができました。

M5Stackのようなマイコンと、IMUや制御機能を内蔵したモータユニットを組み合わせることで、以前よりも手軽に制御技術を試せるようになっています。

まずは実際に作って動かしてみるという意味でも、倒立振子は制御を体験する題材として面白いと思います。

参考

車体の部品一覧です。
PXL_20260928_134040404.jpg

追記（2026年10月3日）
本記事の倒立振子に、転倒した状態から自動的に起き上がり、そのまま倒立制御へ移行する機能を追加しました。
自己起立機能については別の記事で紹介しています。
→「M5GoとRoller485 Liteで倒立振子を自力で起き上がらせてみる」

1
1
3

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
1
1

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?