アドカレ 2025 の三部作です！ ついに完結！
配布するファイルはコチラです！
1. はじめに
本記事では，
- ホビー用のユニット付き BLDC モータ Unit Roller485 LITE
- ESP32 と 6 軸センサが内蔵されているバッテリー付きのマイコンモジュール M5Stack FIRE
- LEGO 部品
で，超簡単にセグウェイ風の車輪型倒立振子を作り，
- MATLAB / Simulink / Control System Toolbox
- Simulink Support Package for Arduino Hardware
- Simulink ライブラリ roller485lib
により実装する方法を，ざっくりと解説します！
2. これまでの動向
電子工作の界隈では，自力で様々なセグウェイ風の車輪型倒立振子を作っておられます．Qiita でざっと検索しただけでも，たとえば以下の記事がヒットします．
皆さん，お好きですねぇ！
- 投稿日 2024年12月24日
- 投稿日 2020年08月06日
- 投稿日 2020年06月21日
- 投稿日 2020年06月17日
- 投稿日 2020年03月02日
- 投稿日 2020年02月29日
- 投稿日 2020年02月17日
- 投稿日 2019年07月05日
- 投稿日 2017年09月30日
- 投稿日 2016年11月08日
わたしも 3D プリンタとか機械工作をせずに，LEGO 部品とか Arduino，ホビー用のエンコーダ付きギヤード DC モータを使った車輪型倒立振子を公開しました（指導学生の卒研が元ネタ）．LEGO なので比較的，簡単に作ることができます．
- 投稿日 2024年12月22日
ところがですね，わたしが LEGO で遊んでいたのと同時期に，猛者たちが衝撃的な車輪型倒立振子を公開をしたわけです！
なにが衝撃的かというと，高速に移動したり，ピタリと静止するんですわ，この車輪型倒立振子．
しかも，材料費も手が届かないほどお高いわけではない．たとえば，
- Unit Roller485 LITE x 2 個：6,743 円 x 2 個 = 13,486 円
- M5Stack AtomS3：3,014 円 …… ESP32-PICO-D4 + 6 軸センサ MPU6886
- M5Stack 用拡張ハブユニット：649 円
- M5Stack 用 GROVE 互換ケーブル
- タミヤ ナロータイヤセット（58mm 径）：528 円
- 適当なリポバッテリー …… ようわからん (T_T)
といった部品構成です．
そして，こうへい先生は車輪型倒立振子のレシピを簡単に紹介されています．
しかし，猛者たちの作品は
- 3D プリンタで部品の一部を作ってるやん（わたし，苦手）
- Arduino IDE とかでがっつりプログラミングしてるんやろな（Simulink がええなぁ，でも … そういえば AtomS3 は Simulink に未対応やん）
- リポ，自分で充電の管理するのはイヤや～（コワい～）
であり，わたしのように電子工作やプログラムが得意でない人には少し大変そう …
3. 車輪型倒立振子を反則級に楽に作る！
3.1 ハードウェア
それで，以下の部品を使って，電子工作や機械加工がいっさい不要な車輪型倒立振子を作りました！
- Unit Roller485 LITE x 2 個：6,743 円 x 2 個 = 13,486 円
- M5Stack FIRE：8,866 円 …… ESP32-D0WDQ6-V3 + 6 軸センサ MPU6886 + 専用リポバッテリー
- M5Stack 用拡張ハブユニット：649 円
- M5Stack 用 GROVE 互換ケーブル …… 上記のものに付属の GROVE 互換ケーブルでも良いけど，短ければ長いものを適当に購入
- LEGO 部品：合計 3,080 円
なお，LEGO の車輪の代わりに
- タミヤ ナロータイヤセット（58mm 径）：528 円
でも OK ですよ．LEGO の車輪を使った場合，
です．まぁ，ギヤ付 DC モータ（N20 モータ）を使用した市販の車輪型倒立振子でも
- BALA2 FIRE セルフバランスカーキット：14,586 円
くらいの価格なので，高性能であることを考えると，こんなもんかって思うことにしてください．
M5Stack FIRE には，以下のような利点があります．
M5Stack FIRE の利点
-
M5Stack FIRE には専用バッテリーが付属していて，別に用意する必要がない
（M5Stack Core には専用バッテリーが付属しているが，Basic には付属されていない）
Roller485 の推奨が 6V〜16V であるのに対して専用バッテリーは 3.7V ですが，今回の車輪型倒立振子くらいであれば問題なさそうです …… Roller485 の LCD には 4.3V と表示されます -
M5Stack FIRE は LEGO 部品を簡単に取りつけられるようになっている
（M5Stack Core とか Basic だとそれ自体には LEGO の取りつけ穴がない）
- M5Stack FIRE に内蔵されているマイコン ESP32-D0WDQ6-V3 は Simulink Support Package for Arduino Hardware に対応している（M5Stack Core や Basic でも OK）
-
M5Stack FIRE に 6 軸センサ MPU6886 が内蔵されており，Simulink Support Package for Arduino Hardware で用意されている「MPU6050 用の Simulink ブロック」に対応している
（M5Stack Core には 6 軸センサが内蔵されているが，Basic には内蔵されていない）
ちなみに，M5Stack FIRE の電源の ON/OFF は以下のように操作します．
なお，M5Stack FIRE と PC を USB ケーブルで接続すると，常に電源は ON の状態になります．
また，Roller485 の I2C アドレスは事前に，以下のように設定しておいてください．
Unit Roller485 LITE の I2C アドレスの変更
出荷時の Unit Roller485 LITE の I2C アドレスは 0x64 です．車輪型倒立振子は左右それぞれに Roller485 が取りつけられていますので，両者を区別するために，I2C アドレスを別々に設定しておく必要があります．今回は，以下のように設定します．
つまり，片側の Roller485 の I2C アドレスを 0x65 に変更します．以下に，その手順を示します．
- Roller485 と専用バッテリーを装着した M5Stack FIRE を GROVE ケーブルで接続します．
- Roller485 の A ボタンを押したまま M5Stack FIRE の電源を ON にします．
- A ボタンを離した後，手動でモータ軸を回転させ（ダイヤルの役割です），「I2C ADDR」に合わせます．
- A ボタンを押します．
- 手動でモータ軸を回転させ，I2C アドレスを「0x65」に設定します．
- A ボタンを押します．
- 手動でモータ軸を回転させ，「Quit」に合わせます．
- A ボタンを押します．
それでは，LEGO 部品を組み立ててみましょう．組立図の PDF ファイルはこちらです．
そして，以下に示すように
- 倒立振子のボディ
- 倒立振子の車輪
- Unit Roller485 LITE
- M5Stack FIRE
- 拡張ハブユニット
- GROVE ケーブル
を装着 / 接続します．
簡単だったでしょ！
3.2 ソフトウェア
わたしの勤務先は MATLAB 包括ライセンスを契約していますので，
- MATLAB / Simulink / Control System Toolbox
- Simulink Support Package for Arduino Hardware …… 無料！
- Simulink ライブラリ roller485lib …… 無料！（っていうか，わたしが作った！）
により実装しました！ インストール方法はこちら．ESP32 の設定追加も忘れずに！
Simulink ライブラリ roller485lib については，別記事を参照してください．
MATLAB を使用できない環境の方はゴメンナサイ．Arduino IDE とかで頑張ってください …
3.3 速度制御モード (speed mode) で倒立制御
配布する Simulink モデルは，M5Stack FIRE の COM 番号が 19 に設定されていますので，以下の手順で COM 番号を変更してください．
M5Stack FIRE と PC を USB ケーブルで接続し，デバイスマネージャで M5Stack FIRE の COM 番号を調べておきます．
配布するすべての Simulink モデルは「コンフィギュレーションパラメータ」（もしくは「ハードウェア設定」）で
のように COM 番号を手動で設定します．なお，M5Stack FIRE を使用しますので，Simulink 側でハードウェアボードとして
- ESP32-WROOM
を選択しています．なお，Application download の Baud rate は必要に応じて 115200 に落としてください．
MATLAB R2023a 以降のバージョンで，配布するファイル
- $\tt\mbox{mpu6050_datalog_external.slx}$
- $\tt\mbox{data_gyro.mat}$
- $\tt\mbox{data_accel.mat}$
- $\tt\mbox{set_segway_parameter.m}$
- $\tt\mbox{segway_speed_mode_external.slx}$
- $\tt\mbox{segway_speed_mode_external_initial.slx}$
- $\tt\mbox{segway_speed_mode_standalone.slx}$
により，簡単に立たせることができるはずです．たぶん …
ただし，Simulink モデルを実行しても，M5Stack FIRE の LCD には何も表示されませんことに注意してください．つまり，真っ黒です．LCD 表示については，後述の「4. おまけ： LCD に表示させたい！」を参照してください．
以下に実行手順を示します．
Step 1：6 軸センサの基準のデータ取得
倒立振子を倒立状態で静止させ，Simulinkモデル
- $\tt\mbox{mpu6050_datalog_external.slx}$
をエクスターナル（監視と調整）で実行します．
10 秒間で実行は終了しますので，実行終了後，コマンドウィンドウで
>> save data_accel.mat ax ay az
>> save data_gyro.mat wx wy wz
もしくは
>> save('data_accel.mat', 'ax', 'ay', 'az');
>> save('data_gyro.mat', 'wx', 'wy', 'wz');
と入力して，加速度センサのデータを
- $\tt\mbox{data_accel.mat}$
に，ジャイロセンサのデータを
- $\tt\mbox{data_gyro.mat}$
に保存します．
Step 2：6 軸センサのキャリブレーションとパラメータ設定
M ファイル
clear
format compact
load data_accel
mean_ax = mean(ax)
mean_ay = mean(ay)
mean_az = mean(az)
load data_gyro
mean_wx = mean(wx)
mean_wy = mean(wy)
mean_wz = mean(wz)
h = 0.015
set_h = 5
T = 0.5
Tf1 = 0.08
を実行します．
配布するデータファイル
- $\tt\mbox{data_accel.mat}$
- $\tt\mbox{data_gyro.mat}$
の場合だと，実行結果は以下のようになります．
>> set_segway_parameter
mean_ax =
0.1474
mean_ay =
-6.5808e-04
mean_az =
10.3983
mean_wx =
-0.1354
mean_wy =
-11.1813
mean_wz =
6.7883
h =
0.0150
set_h =
5
T =
0.5000
Tf1 =
0.0800
ここで，
- h = 0.015 [s]：サンプル時間（3 個の I2C デバイスを使用しているので，長めの時間に設定）
- set_h = 5 [s]：スタンドアロンで実行したときに，以下の時間を確保
- モータを ON にする時間（電源を入れてから 5 秒後にモータを ON にする）
- ユニット内のパラメータ（動作モード，PID ゲイン，電流最大値）を設定をする時間（電源を入れてから 5 秒間，設定したパラメータを書き込む）
- モータ角の初期値をリセット（電源を入れてから 5 秒のときの値を初期値にする）
- T = 0.5：M5Stack FIRE の 6 軸センサから斜体角を算出する際に用いる相補フィルタの時定数（相補フィルタを双 1 次変換で実装）
- Tf1 = 0.08：Roller485 のエンコーダから車輪角速度を算出する際に用いるローパスフィルタの時定数（不完全微分器を双 1 次変換で実装）
です．
Step 3：エクスターナル（監視と調整）で実行
手で倒立振子を倒立状態にし，Simulinkモデル
- $\tt\mbox{segway_speed_mode_external.slx}$
をエクスターナル（監視と調整）で実行します．実行開始後，
- $\tt\mbox{Manual Switch1}$ をマウスでダブルクリックし，OFF (0) から ON (1) に切り替える
- $\tt\mbox{Manual Switch2}$ をマウスでダブルクリックし，0 から状態フィードバック側に切り替える
ことにより，倒立することを確認します．
速度制御モード (speed mode) におけるユニット内の速度制御器の PID ゲインと最大電流値を
- P: 2, I: 0, D: 850, Imax: 1200 mA
のように変更しています．
ステップ 3 の様子を以下に示します． 「エクスターナル」を「エクスタール」ってタイプミスwww．
自分でコントローラのゲインを調整したい場合は，Simulinkモデル
- $\tt\mbox{segway_speed_mode_external_initial.slx}$
をエクスターナルで実行した後，
の「5.3.2 コントローラゲインを手動調整！」で説明する手順に従ってください．
Step 4：スタンドアロン（ビルド、展開起動）で実行
手で倒立振子を倒立状態にし，Simulinkモデル
- $\tt\mbox{segway_speed_mode_standalone.slx}$
をスタンドアロン（ビルド、展開起動）で実行してください．実行を開始してから 5 秒後に制御が開始されます．
いったん，M5Stack FIRE にプログラムが組み込まれたら，倒立振子をから USB ケーブルを外しても，M5Stack FIRE の電源が ON になるとプログラムが実行されます．
以下の動画では，傾けた状態からスタートさせています（個体によっては暴走するかも …）．
暴走を回避するために，左右のモータ角の大きさの平均が 1800°（5 回転）を超えると，自動的にモータを停止するようにしています．
3.4 電流制御モード (current mode)で倒立制御
動作モードを速度制御モード (speed mode) から電流制御モード (current mode) に変更して，倒立制御を実現することもできます．該当する Simulink モデルは
- $\tt\mbox{segway_current_mode_external.slx}$
- $\tt\mbox{segway_current_mode_external_initial.slx}$
- $\tt\mbox{segway_current_mode_standalone.slx}$
です．変更した点は，モータ駆動の部分と制御器のゲインです．
Simulink モデル
- $\tt\mbox{segway_current_mode_standalone.slx}$
をスタンドアロン（ビルド、展開起動）で実行した結果です．
3.5 ざっくりと比較
以上のように，ユニット内部の速度制御器の PID ゲインを調整した速度制御モード (speed mode) と電流制御モード (current mode) により車輪型倒立振子の倒立制御を実装してみました．動画ではわかりにくいですが（実際に製作して，車体をつついてみるとよくわかります），
- 電流制御モード (current mode) だと車体が大きく傾いたときのふるまいが振動的
- 速度制御モード (speed mode) だとなめらな動き
となりました．ただし，
- 速度制御モード (speed mode) はユニット内部の速度制御器の PID ゲインを適切な値に調整する必要がある
ことに注意してください．両者の特性の比較は，
に対して実証した以下が参考になります．
4. おまけ： LCD に表示させたい！
Takuya Otani (https://x.com/ta98otani) さんから OK をいただいたので，おまけで LCD 表示のファイルも提供します！ ちなみに，Takuya Otani さんが組み立てた車輪型倒立振子のレプリカはコチラです．
4.1 配布するファイル
3 章で用いた Simulink モデル
- $\tt\mbox{segway_speed_mode_external.slx}$
- $\tt\mbox{segway_speed_mode_external_initial.slx}$
- $\tt\mbox{segway_speed_mode_standalone.slx}$
- $\tt\mbox{segway_current_mode_external.slx}$
- $\tt\mbox{segway_current_mode_external_initial.slx}$
- $\tt\mbox{segway_current_mode_standalone.slx}$
に LCD 表示を追加した
- $\tt\mbox{LCD_segway_speed_mode_external.slx}$
- $\tt\mbox{LCD_segway_speed_mode_external_initial.slx}$
- $\tt\mbox{LCD_segway_speed_mode_standalone.slx}$
- $\tt\mbox{LCD_segway_current_mode_external.slx}$
- $\tt\mbox{LCD_segway_current_mode_external_initial.slx}$
- $\tt\mbox{LCD_segway_current_mode_standalone.slx}$
を配布します．
4.2 Simulink ライブラリ M5StackLCDlib のインストール
LCD 表示を行うために，Takuya Otani (https://x.com/ta98otani) さんが作成した Simulink ライブラリ M5StackLCDlib をインストールしてください．
4.3 使用方法
Simulink モデル
- $\tt\mbox{LCD_segway_speed_mode_standalone.slx}$
を例として，説明します．
まず，M ファイル
- $\tt\mbox{set_segway_speed_mode_parameter.m}$
を実行し，パラメータ設定を行います．つぎに，Simulink モデル
- $\tt\mbox{LCD_segway_speed_mode_standalone.slx}$
を開きます．
ここで，Simulink ブロック $\tt\mbox{M5Stack Table Display}$ をクリックすると，以下に示すように，$320 \times 240$ の LCD に表示する 6 行 2 列の表の色や文字色，文字サイズなどの設定を行うことができます．
ここで，色は 16 ビットカラー (RGB565) で指定します．16 ビットカラーの例は，
に掲載されています．また，1 列目の幅や文字サイズは single で指定します．
さいごに，Simulink モデル
- $\tt\mbox{LCD_segway_speed_mode_standalone.slx}$
をスタンドアロン（ビルド、展開起動）で実行します．その結果，以下のように動作します．LCD に
- motor：車輪の相対角（モータの相対角）$\theta_{\rm motor}\ {\rm [deg]}$
- body：車体角 $\theta_{\rm body}\ {\rm [deg]}$
- input：速度指令値 $\omega_{\rm m}^{\rm ref}\ {\rm [rad/s]}$
- time：時間
のデータが表示されていますね！
5. おわりに
ジョイスティックなどによるリモート操作については，現在，二人で検討中なので，別の機会で説明するかもしれません．
その前段階として，BLE による単純なリモート操作は以下で解説していますよ！
以上です．
付録
A.1 車輪型倒立振子の状態量と制御入力
本記事で，車輪型倒立振子の安定化制御に利用する角度は下図に示す車体角 $\theta_{\rm body}\ {\rm [rad]}$ と車輪の相対角（モータの相対角）$\theta_{\rm motor}\ {\rm [rad]}$ です．
また，速度制御モード (speed mode) で動作させる場合，制御入力は速度指令値 $\omega_{\rm m}^{\rm ref}\ {\rm [rad/s]}$です．したがって，状態フィードバック形式の制御器（コントローラ）
\begin{align*}
\color{#0CA274}{\omega_{\rm m}^{\rm ref}}
= {k}_{1}\color{#00B0F0}{\theta_{\rm body}}
+ {k}_{2}\color{#00B0F0}{\dot{\theta}_{\rm body}}
+ {k}_{3}\color{#FF6699}{\theta_{\rm motor}}
+ {k}_{4}\color{#FF6699}{\dot{\theta}_{\rm motor}}
\end{align*}
を使用して，車輪型倒立振子の安定化制御を実現しました．
一方，電流制御モード (current mode)で動作させる場合，制御入力は電流指令値 $i_{\rm q}^{\rm ref}\ {\rm [A]}$です．したがって，状態フィードバック形式の制御器（コントローラ）
\begin{align*}
\color{#E48312}{i_{\rm q}^{\rm ref}}
= {k}_{1}\color{#00B0F0}{\theta_{\rm body}}
+ {k}_{2}\color{#00B0F0}{\dot{\theta}_{\rm body}}
+ {k}_{3}\color{#FF6699}{\theta_{\rm motor}}
+ {k}_{4}\color{#FF6699}{\dot{\theta}_{\rm motor}}
\end{align*}
を使用して，車輪型倒立振子の安定化制御を実現しました．
ただし，いずれの制御モードの場合も制御器のゲイン $k_1,\ k_2,\ k_3,\ k_4$ は手動で調整しました（当然ですが，両者の制御器のゲインは異なる値です）．詳細は
の「5.3.2 コントローラゲインを手動調整！」を参照してください．
A.2 相補フィルタによる車体角の算出
6 軸センサからは下図のように，$\color{#E48312}{x},\ \color{#0070C0}{y},\ \color{#E84746}{z}$ 軸を基準とした角速度 $\color{#E48312}{\omega_x},\ \color{#0070C0}{\omega_y},\ \color{#E84746}{\omega_z}\ {\rm [rad/s]}$ がジャイロセンサから，加速度 $\color{#E84746}{a_x},\ \color{#0070C0}{a_y},\ \color{#E84746}{a_z}\ {\rm [m/s^2]}$ が加速度センサから検出されます．
車体角はジャイロセンサや加速度センサで検出された値から下図のように算出することができます．
よく知られているように，これらの算出方法は以下のような特徴があります．
-
加速度センサによる角度算出
- 低周波成分の信頼性が高い
- 高周波成分の信頼性が低い（高周波ノイズが問題）
-
ジャイロセンサによる角度算出
- 高周波成分の信頼性が高い
- 低周波成分の信頼性が低い（ゆっくりとズレるドリフト現象が問題）
そこで，下図のように，低周波の情報を得意とする加速度センサからは低周波成分を抽出し，高周波の情報を得意とするジャイロセンサからは高周波成分を抽出して，それぞれを加算して，車体角 $\theta_{\rm body}\ {\rm [rad]}$ を算出しました．このような算出法を，相補フィルタといいます．
詳細は，
の「3.5 6 軸センサにより 3 軸まわりの角度がわかるの？」を参照してください．
A.3 双 1 次変換による離散化
配布するファイルでは，6 軸センサにおける相補フィルタや積分器を，サンプル時間 $h$ で双 1 次変換することで，離散化しています．
\begin{align*}
{G}_{\rm L}(s)\bigl|_{s = \frac{2}{h}\frac{z - 1}{z + 1}}
&= \dfrac{1}{1 + Ts}\biggl|_{s = \frac{2}{h}\frac{z - 1}{z + 1}}
\\
&= \dfrac{h(z + 1)}
{(h+2T)z + h-2T}
\\
{G}_{\rm H}(s)\bigl|_{s = \frac{2}{h}\frac{z - 1}{z + 1}}
&= \dfrac{Ts}{1 + Ts}\biggl|_{s = \frac{2}{h}\frac{z - 1}{z + 1}}
\\
&= \dfrac{2T(z - 1)}
{(h+2T)z + h-2T}
\\
\dfrac{1}{s}\biggl|_{s = \frac{2}{h}\frac{z - 1}{z + 1}}
&= \dfrac{h}{2}
\dfrac{z + 1}{z - 1}
\end{align*}
また，車輪角速度の算出における不完全微分についても，サンプル時間 $h$ で双 1 次変換することで，離散化しています．
\begin{align*}
\dfrac{s}{1 + {T}_{\rm f1}s}\biggl|_{s = \frac{2}{h}\frac{z - 1}{z + 1}}
= \dfrac{2(z - 1)}
{(h + 2{T}_{\rm f1})z + h - 2{T}_{\rm f1}}
\end{align*}