続　Simulink と Arduino Mega で『制御工学』を学ぶ、はじめの一歩　— 倒立振子よ、今こそ立ち上がれ！

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/7e196858-ca11-26b0-bd9c-04f448084837.png)

　MATLAB 芸や Simulink 芸を披露する年末恒例のアドカレ [MATLAB/Simulink Advent Calendar 2024](https://qiita.com/advent-calendar/2024/matlab) に今年も参加してみました．制御工学の界隈だと年末年始は [MSCS（SICE 制御部門マルチシンポジウム）](https://mscs2025.sice-ctrl.jp/) の原稿締切に追われるはずなのですが，なんとアドカレの記事を優先するという暴挙！ （イヤぁ…… きっと来年度の授業か何かに使うはずだし……）
 



:::note alert
本記事で使用する Simulink モデルや mat ファイルは以下からダウンロードできます！
 * [R2024b](https://github.com/Prof-Carter/Advent_Calendar_2024/blob/main/R2024b.zip)
 * [R2024a](https://github.com/Prof-Carter/Advent_Calendar_2024/blob/main/R2024a.zip)
 * [R2023b](https://github.com/Prof-Carter/Advent_Calendar_2024/blob/main/R2023b.zip)
 * [R2023a](https://github.com/Prof-Carter/Advent_Calendar_2024/blob/main/R2023a.zip)
 * [R2022b](https://github.com/Prof-Carter/Advent_Calendar_2024/blob/main/R2022b.zip)
 * [R2022a](https://github.com/Prof-Carter/Advent_Calendar_2024/blob/main/R2022a.zip)
 * [R2021b](https://github.com/Prof-Carter/Advent_Calendar_2024/blob/main/R2021b.zip)
:::

# <font color="#33CC33">1. はじめに</font>

　2023 年のアドカレの記事

https://qiita.com/Carter/items/5526a72d05f90d073377

では，<b><font color="#0070C0">Simulink Support Package for Arduino Hardware</font></b>（以下，<b><font color="#0070C0">Simulink Support Package</font></b> と略記）を利用して <b><font color="#0070C0">Arduino Mega 2560 R3</font></b>（以下，<b><font color="#0070C0">Arduino Mega</font></b> と略記）により 
 
 * <b><font color="#E48312">DC モータ</font></b>の駆動
 * <b><font color="#E48312">ロータリエンコーダ</font></b>からの角度検出

を行う方法について説明しました．

　今年のアドカレの記事では，<b><font color="#00B0F0">6 軸センサ</font></b>の使用方法について説明します．そして，最終的には，<b><font color="#E84746">車輪型倒立振子の倒立を目指します！</font></b>

https://x.com/carter_control/status/1854856507403518421

元ネタは卒研生が製作したこちらです．

https://x.com/carter_control/status/1833782155866198024

これを，汎用で使いやすいように部品構成を見直しました．



# <font color="#33CC33">2. ソフトウェア環境</font>

 　必要なソフトウェアは
 
 * MATLAB
 * Simulink
 * Control System Toolbox

および無償で提供されている

 * <b><font color="#0070C0">Simulink Support Package for Arduino Hardware</font></b>

です．

　<b><font color="#0070C0">Simulink Support Package for Arduino Hardware</font></b> のインストール手順については，たとえば，Qiita の記事

https://qiita.com/Carter/items/cc7faa5084aa4a69f4e7

の 3 章を参照してください．


# <font color="#33CC33">3. 6 軸センサの使用方法</font>

## <font color="#33CC33">3.1　6 軸センサってなんなのさ</font>

　<b><font color="#00B0F0">6 軸センサ</font></b>とは，
 
* <font color="#E48312">$x$</font>, <font color="#0070C0">$y$</font>, <font color="#E84746">$z$</font> 軸まわりの角速度 <font color="#E48312">$\omega_x$</font>, <font color="#0070C0">$\omega_y$</font>, <font color="#E84746">$\omega_z$</font>
* <font color="#E48312">$x$</font>, <font color="#0070C0">$y$</font>, <font color="#E84746">$z$</font> 軸方向の加速度 <font color="#E48312">$a_x$</font>, <font color="#0070C0">$a_y$</font>, <font color="#E84746">$a_z$</font>
 
を検出するセンサであり，<b><font color="#0CA274">ジャイロセンサ</font></b>（角速度センサ）と<b><font color="#E32D91">加速度センサ</font></b>を組み合わせたものです．

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/f67cd2be-324d-5247-c14b-11552ad6edf7.png)

　そして，後述しますが，<b><font color="#00B0F0">6 軸センサ</font></b>によりセンシングされた角速度 <font color="#E48312">$\omega_x$</font>, <font color="#0070C0">$\omega_y$</font>, <font color="#E84746">$\omega_z$</font> と加速度 <font color="#E48312">$a_x$</font>, <font color="#0070C0">$a_y$</font>, <font color="#E84746">$a_z$</font> を利用すると，<b><font color="#E48312">$\pmb{x}$</font>, <font color="#0070C0">$\pmb{y}$</font>, <font color="#E84746">$\pmb{z}$</font> 軸まわりの角度 <font color="#E48312">$\pmb{\theta_x}$</font>, <font color="#0070C0">$\pmb{\theta_y}$</font>, <font color="#E84746">$\pmb{\theta_z}$</font> を算出することができるんです！</b>

　ちなみに，<b><font color="#0CA274">ジャイロセンサ</font></b>（角速度センサ）と<b><font color="#E32D91">加速度センサ</font></b>に加えて，
 
* <font color="#E48312">$x$</font>, <font color="#0070C0">$y$</font>, <font color="#E84746">$z$</font> 軸方向の地磁気
 
を検出する<b><font color="#00B0F0">コンパス（方位）センサ</font></b>を組み合わせたものを <b><font color="#00B0F0">9 軸センサ</font></b>といいます．また，<b><font color="#00B0F0">6 軸センサ</font></b>や <b><font color="#00B0F0">9 軸センサ</font></b>は<b><font color="#00B0F0"> IMU センサ（慣性センサ）</font></b>と呼ばれることもあります．

:::note warn
IMU は Inertial Measurement Unit（慣性測定ユニット）の略です．
:::




## <font color="#33CC33">3.2　電子部品リスト (2024/12/06)</font>

|   |型番|メーカー|購入先|価格|
|---|---|---|---|---|
|マイコンボード|Arduino Mega 2560 R3|Arduino|[RS](https://jp.rs-online.com/web/p/arduino/7154084)|5,940 円|
|6 軸センサ<br>（はんだ付けオプション：L 字ピン）|MPU6050 (GY-521)|-|[電子工作ステーション](https://electronicwork.shop/items/6262bf96be482f6d864f08c9)|360 円<br>（+500 円）|
|ミニブレッドボード|BB-601 (Black)|慈渓市万捷電子有限公司|[秋月電子通商](https://akizukidenshi.com/catalog/g/gP-05158/)|160 円|
|USB ケーブル（リールケーブル A-B タイプ）|-|COMON(カモン)|[amazon](www.amazon.co.jp/dp/B0041DL830)|584 円|

### <font color="#33CC33">(a)　6 軸センサ MPU6050</font>

　今回の主役は[電子工作ステーション](https://electronicwork.shop/items/6262bf96be482f6d864f08c9?srsltid=AfmBOoqapLVdJjyrk7KPRQkLvpz9PyhrCDuHLgnLx5ODrnBN6kQbSBdq)さんで購入した <b><font color="#00B0F0">6 軸センサ MPU6050</font></b>
 
 ![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/b27f96c9-b73a-8db6-efb6-31a2df392e44.png)

です．コレって 360 円なので，かなりお安いです！ なお，本製品はモジュールとピンがはんだ付けされていない状態で販売されていますが， [電子工作ステーション](https://electronicwork.shop/items/6262bf96be482f6d864f08c9?srsltid=AfmBOoqapLVdJjyrk7KPRQkLvpz9PyhrCDuHLgnLx5ODrnBN6kQbSBdq)さんに 500 円の追加料金を払えば，「[Pin はんだ付けサービス](https://electronicwork.shop/items/6254f327a102752afcb40d29)」によりはんだ付けされた状態で購入できます．

　付属するピンは「ストレートタイプ」と「L 字タイプ」の 2 種類がありますが，「<b>L 字タイプ</b>」としました．ブレッドボードに挿し込むと，以下のようになります．

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/60b6cbe1-bf8a-8057-a2ce-4a41b14f3445.png)


### <font color="#33CC33">(b)　Arduino Mega 2560 R3</font>

　今回は，純正の <b><font color="#0070C0">Arduino Mega 2560 R3</font></b> を利用しました．互換品でも動作することが多いですが，手動で COM ポート番号を指定する必要があったりして面倒なので，純正を利用しています．

　また，6 軸センサの動作を確認するだけであれば，<b><font color="#0070C0">Arduino Uno R3</font></b> を利用しても大丈夫ですが，<b><font color="#0070C0">Arduino Mega</font></b> と比べて接続できる DC モータやエンコーダの数が少ないです．なお，<b><font color="#0070C0">Simulink Support Package</font></b> は <b><font color="#E84746">R2024b</font></b> から <b><font color="#0070C0">Arduino Uno R4 WiFi</font></b> にも対応しています（私はまだ使用したことがないですが…）．





## <font color="#33CC33">3.3　Arduino Mega と MPU6050 との接続</font>

　<b><font color="#00B0F0">6 軸センサ MPU6050</font></b> は I²C（アイ・スクエア・シー，Inter-Integrated Circuit）接続で使用されます．各ピンの機能は以下のとおりです．

:::note info
<b>各ピンの機能</b>
* <font color="#E84746">${\tt VCC}$</font>：電源入力ピンです．通常，3.3 V または 5 V で動作します．
* ${\tt GND}$：システムの共通グラウンドに接続します．
* <font color="#E48312">${\tt SCL (Serial\ Clock\ Line)}$</font>：I²C 通信のクロック信号を提供します．マスター（マイコンなど）からのクロック信号を受け取ります．
* <font color="#0CA274">${\tt SDA (Serial\ Data\ Line)}$</font>：I²C 通信のデータラインです．マスターとスレーブ間でデータを送受信します．
* <font color="#7F7F7F">${\tt XDA (External\ Data\ Line)}$</font>：拡張用の I²C データラインです．他の I²C デバイスをチェーン接続する場合（他の I²C デバイスを増設する場合）に使用されます．
* <font color="#7F7F7F">${\tt XCL (External\ Clock\ Line)}$</font>：拡張用の I²C クロックラインです．他の I²C デバイスをチェーン接続する場合に使用されます．
他の I²C デバイスをチェーン接続する場合に使用されます。
* <font color="#7F7F7F">${\tt AD0 (Address\ Select)}$</font>：I²C アドレス選択ピンです。このピンを GND に接続すると I²C アドレスは 0x68，VCC に接続すると 0x69 になります．複数の MPU6050 を1つの I²C バスに接続する場合，このピンでアドレスを切り替えます．
* <font color="#7F7F7F">${\tt INT (Interrupt)}$</font>：割り込み信号を出力するピンです．
:::

これらのなかで今回使用するピンは

*  <font color="#E84746">${\tt VCC}$</font>
* ${\tt GND}$
* <font color="#E48312">${\tt SCL}$</font>
* <font color="#0CA274">${\tt SDA}$</font>

の４つであり，<b><font color="#00B0F0">MPU6050</font></b> と <b><font color="#0070C0">Arduino Mega</font></b> を下図のように接続します．

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/8e3c8c40-7654-3089-70e1-65bf70e58b59.png)

具体的には，下図のように，<b><font color="#00B0F0">MPU6050</font></b> をブレッドボードとジャンパ線で <b><font color="#0070C0">Arduino Mega</font></b> と接続するとよいでしょう．なお，下図では，ジャンパ線のピン付近をテープで巻いています．

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/f70509f1-df8e-39be-49a3-e78aed5f20a5.png)



## <font color="#33CC33">3.4　Simulink ブロック</font>

### <font color="#33CC33">3.4.1　ライブラリ</font>

　<b><font color="#0070C0">Simulink Support Package</font></b> はいくつかの <b><font color="#00B0F0">IMU センサ</font></b>に対応しています．執筆時点の最新バージョン <b><font color="#E84746">R2024b</font></b> では，Simulink のライブラリブラウザの
 
* Simulink Support Package for Arduino Hardware / Sensors

に以下の Simulink ブロックが格納されています．

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/fd6dca85-c4b8-abfc-bb18-fe634a453a25.png)


　「${\tt *** IMU Sensor}$」とい名前の Simulink ブロックが <b><font color="#00B0F0">IMU センサ</font></b>用です．これらのなかで，<b><font color="#00B0F0">6 軸センサ</font></b>の Simulink ブロックには，

* ${\tt Accel}$（あるいは ${\tt Acceleration}$）：加速度
* ${\tt Ang Rate}$（あるいは ${\tt Angular Rate}$）：角速度

という出力端子が，<b><font color="#00B0F0">9 軸センサ</font></b>の Simulink ブロックには，これらに加えて

* ${\tt Mag Field}$（あるいは ${\tt Magnetic Field}$）：方位

という出力端子があります．

 
### <font color="#33CC33">3.4.2　6 軸センサ MPU6050 用の Simulink ブロック</font>

　今回使用する <b><font color="#00B0F0">6 軸センサ MPU6050</font></b> 用の Simulink ブロックは

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/970f2847-db08-490d-37b0-a40ba62d588d.png)

です．この Simulink ブロック ${\tt MPU6050\ IMU\ Sensor}$ をダブルクリックすると，

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/31f758f1-5f24-a930-a500-f10880d726cb.png)


のように，いくつかのパラメータが設定できます．

* Simulink ブロック ${\tt MPU6050\ IMU\ Sensor}$ の出力端に表示したいものにチェックを入れます．
* <b>パラメータ ${\tt Accelerometer\ range}$ により<INS>加速度のレンジを 2G / 4G / 8G / 16G のいずれかに設定</INS></b>します（標準設定は 2G）．G は重力加速度 $g \simeq 9.8\ {\rm [m/s^2]}$ を意味しているので，2G と設定すればおおよそ $-19.6 \sim +19.6\ {\rm [m/s^2]}$ の範囲で，4G と設定すればおおよそ $-39.2 \sim +39.2\ {\rm [m/s^2]}$ の範囲で，<font color="#E48312">$x$</font>, <font color="#0070C0">$y$</font>, <font color="#E84746">$z$</font> 軸方向それぞれの加速度 <font color="#E48312">$a_x$</font>, <font color="#0070C0">$a_y$</font>, <font color="#E84746">$a_z$</font> を測定することができます．レンジを大きくするに従い広い範囲のデータを取得できますが，データは粗くなります（レンジを 2 倍にするとデータは 2 倍粗くなります）．
* <b>パラメータ ${\tt Gyroscope\ range}$ により<INS>角速度のレンジを 250 dps / 500 dps / 1000 dps / 2000 dps のいずれかに設定</INS></b>します（標準設定は 250 dps）．dps は角速度の単位 deg/s を意味しているので，250 dps と設定すればおおよそ $-250 \sim +250\ {\rm [deg/s]}$ の範囲で，<font color="#E48312">$x$</font>, <font color="#0070C0">$y$</font>, <font color="#E84746">$z$</font> 軸まわりのそれぞれの角速度 <font color="#E48312">$\omega_x$</font>, <font color="#0070C0">$\omega_y$</font>, <font color="#E84746">$\omega_z$</font> を測定することができます．レンジを大きくするに従い広い範囲のデータを取得できますが，データは粗くなります（レンジを 2 倍にするとデータは 2 倍粗くなります）．


### <font color="#33CC33">3.4.3　加速度と角速度を検出してみよう！</font>

　それでは，<b><font color="#00B0F0">6 軸センサ MPU6050</font></b>により加速度や角速度を検出するための Simulink モデルを作成してみましょう．

#### <font color="#33CC33">(a) Arduino Mega を動かすための Simulink モデルの基本設定</font>

:::note warn
<INS>ステップ 1</INS>

　空の Simulink モデルを開きます．
 
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/5760808c-618d-0759-e88f-9ad53e147865.png)

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/04cb8e50-1a77-990e-36c4-48a16c7267af.png)

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/43e90869-8d9a-6a74-bd25-bcdce50739e7.png)
:::

:::note warn
<INS>ステップ 2</INS>
Simulink モデルの空白部分を右クリックし，コンフィギュレーションパラメータを表示します．そして，以下のように設定します．

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/f7103fc2-1730-6ce6-b20d-22d1faf48b49.png)

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/28e0b27e-eccc-79f2-fab5-61707eb222b3.png)

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/7319240c-7f4e-8866-12f1-d4d67175e99f.png)

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/da0ce423-89d1-6c68-e7eb-ea913859a848.png)
:::

:::note alert
<b><font color="#0070C0">Arduino Mega の互換ボード</font></b>を使用しているとき，上記の設定（COM ポート番号を「Automatically（自動設定）」）により Simulink モデルを実行すると，エラーを生じることがあります．このような場合は，以下の手順で手動で COM ポート番号を手動で設定してみてください．

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/c778fced-b6a3-9ea4-43db-0812e472c6fe.png)
:::

#### <font color="#33CC33">(b) 角速度の検出</font>

　配布する角速度 <font color="#E48312">$\omega_x$</font>, <font color="#0070C0">$\omega_y$</font>, <font color="#E84746">$\omega_z$</font> $\rm{[deg/s]}$ を検出する Simulink モデル

* $\verb+sample_gyro.slx+$

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/7e7d2f26-9f1e-4154-96e0-896a27473b99.png)

を開いてください．また，<b><font color="#E84746">カレントディレクトリをこの Simulink モデルが保存されているフォルダに変更</font></b>してください（以下，他の Simulink モデルを実行する場合も同様です）．

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/5f0de316-ad50-3bcc-e501-1683e59f7e0e.png)


そして，この Simulink モデルの「監視と調整」をクリックして実行すると，以下のようになります（画像をクリックすると，YouTube 動画にジャンプします）．

[![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/d6526ef7-fb2c-544e-983d-ead1d457e7c9.png)](https://youtu.be/wOLiFRLCA6A)

https://youtu.be/wOLiFRLCA6A

　以下の点に注意してください．
 
:::note alert
角速度のレンジは標準の 250 dps ($250\ {\rm deg/s}$) としていますが，速く動かす場合は下図のように頭打ちとなるので，500 dps ($500\ {\rm deg/s}$) などの大きな値に変更する必要があります．

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/53025c6f-531f-6242-849d-3b108783cef2.png)

以下，Simulink ブロック ${\tt MPU6050\ IMU\ Sensor}$ のパラメータ（加速度レンジ）を 500 dps に変更して使用します．

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/f86f4537-e574-c4e1-baa0-68a8c5618f1a.png)

:::

:::note alert
Simulink ブロック ${\tt MPU6050\ IMU\ Sensor}$ の出力端 ${\tt Ang\ Rate}$ の信号は角速度 <font color="#E48312">$\omega_x$</font>, <font color="#0070C0">$\omega_y$</font>, <font color="#E84746">$\omega_z$</font> $\rm{[deg/s]}$ がベクトル化された  double 型の $3 \times 1 \times N$ の 3 次元配列なので，Simulink ブロック ${\tt Demux}$ によりこれらを分解し，3 つの $1 \times 1 \times N$ の 3 次元配列とします．さらに，Simulink ブロック ${\tt Reshape}$ により $N \times 1$ の 1 次元配列に変換します．

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/7fb84427-a5e4-a5aa-45bf-e5cca8fe2e37.png)
:::

　このことを確認するために，

* $\verb+sample_gyro_datalog.slx+$
    
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/a8fd2171-49cb-8616-dfa2-179c03dd6f3c.png)

を実行してみます．ただし，3 つの Simulink ブロック ${\tt To\ Workspace}$ の「保存形式」を「時系列」から「配列」に変更しています．また，Simulink モデルの終了時間を「${\tt inf}$」から「${\tt 1}$」に変更しています．この Simulink モデルを実行した後，MATLAB のコマンドウィンドウで ${\tt data}$, $\verb+wx_3d+$, ${\tt wx}$ のサイズを確認すると，以下のようになります．

```matlab:${\tt MPU6050\ IMU\ Sensor}$ の出力
>> size(data)
ans =
     3     1   201
>> data
data(:,:,1) =
    2.7008
   37.4298
   32.9056
data(:,:,2) =
    1.4496
   35.6369
   29.9301
~~~~~~~~~~~ < 略 > ~~~~~~~~~~~
data(:,:,201) =
  -11.4975
   -9.3231
  -30.2277
```
```matlab:${\tt Demux}$ の出力
>> size(wx_3d)
ans =
     1     1   201
>> wx_3d
wx_3d(:,:,1) =
    2.7008
wx_3d(:,:,2) =
    1.4496
~~~~~~~~~~~ < 略 > ~~~~~~~~~~~
wx_3d(:,:,201) =
  -11.4975
```

```matlab:${\tt Reshape}$ の出力
>> size(wx)
ans =
   201     1
>> wx
wx =
    2.7008
    1.4496
~~~~~~~~~~~ < 略 > ~~~~~~~~~~~
  -11.4975
```

#### <font color="#33CC33">(c) 加速度の検出</font>

　配布する加速度 <font color="#E48312">$a_x$</font>, <font color="#0070C0">$a_y$</font>, <font color="#E84746">$a_z$</font> $\rm{[m/s^2]}$ を検出する Simulink モデル

* $\verb+sample_accelerometer.slx+$

を実行すると，以下のようになります（画像をクリックすると，YouTube 動画にジャンプします）．

[![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/bcb9e965-039e-1364-5f7f-73a89830b8f9.png)](https://youtu.be/5j9NDNP2ExM)

https://youtu.be/5j9NDNP2ExM

:::note alert
加速度のレンジは標準の 2G としていますが，速く動かす場合は頭打ちとなるので，4G などの大きな値に変更する必要があります．
:::

:::note alert
Simulink ブロック ${\tt MPU6050\ IMU\ Sensor}$ の出力端 ${\tt Accel}$ の信号は加速度 <font color="#E48312">$a_x$</font>, <font color="#0070C0">$a_y$</font>, <font color="#E84746">$a_z$</font> $\rm{[m/s^2]}$ がベクトル化された  double 型の $3 \times 1 \times N$ の 3 次元配列なので，Simulink ブロック ${\tt Demux}$ によりこれらを分解し，3 つの $1 \times 1 \times N$ の 3 次元配列とします．さらに，Simulink ブロック ${\tt Reshape}$ により $N \times 1$ の 1 次元配列に変換します．
:::

　さて，実験したときに気づいたかもしれませんが，<b><font color="#00B0F0">6 軸センサ MPU6050</font></b> を動かさずに静止させた状態であっても，加速度 <font color="#E48312">$a_x$</font>, <font color="#0070C0">$a_y$</font>, <font color="#E84746">$a_z$</font> は 0 であるとは限りません．これは，重力加速度 $g$ の影響を受けているからです．

　試しに，Simulink モデル

* $\verb+sample_accelerometer.slx+$

を実行し，<b><font color="#00B0F0">6 軸センサ MPU6050</font></b> が搭載されたブレッドボードの 6 つの面のいずれかが床面でとなるように静止させると，以下のような結果が得られます．

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/b179042e-5937-1631-efab-43220408deb3.png)

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/d5a99343-e4cb-8772-152d-abee216f0c37.png)

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/e53297f3-eff3-779b-8caf-a7dab8e015aa.png)

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/de33cd35-2406-a799-65e3-c47b140136e3.png)

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/26e3695f-e537-5623-0d12-1bfea5e9071b.png)

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/c88baefa-2e6d-e05c-7d12-9392be94aed9.png)


これらの結果から，重力加速度 $g$ に抗っている垂直抗力（向きが重力加速度 $g$ と反対の加速度）を検出していることがわかります．そのため，実際の動作の加速度にオフセット分（DC 成分）が含まれたものを検出していることがわかります．


### <font color="#33CC33">3.4.4　キャリブレーションを行おう！</font>

　Simulink モデル

* $\verb+imu01_mpu6050.slx+$

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/834dea59-b9c0-b3ee-d4b3-502fdd5f1728.png)

を実行してみましょう．実行すると，

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/f38394a4-e447-98c4-d578-04d49885426d.png)

のようになり，理想的な値からは少々ずれていることがわかります．この誤差を補正しないで後述の角度算出を行うと，悪影響を及ぼしてしまいます．

　そこで，測定誤差を補正する「<b>キャリブレーション (calibration)</b>」という作業を行います．以下に，キャリブレーションの手順を示します．

:::note warn
<INS>ステップ 1</INS>
　<b><font color="#00B0F0">6 軸センサ MPU6050</font></b> が搭載されている<b>ブレッドボードを水平</b>に静止させた状態で，Simulink モデル

* $\verb+imu02_mpu6050_datalog.slx+$

を実行します．この Simulink モデルは 10 秒間の実行が行われ，

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/72d1dba5-6248-b153-0952-2e6772b299d9.png)

のように加速度データ <font color="#E48312">$a_x$</font>, <font color="#0070C0">$a_y$</font>, <font color="#E84746">$a_z$</font> $\rm{[m/s^2]}$ と角速度データ <font color="#E48312">$\omega_x$</font>, <font color="#0070C0">$\omega_y$</font>, <font color="#E84746">$\omega_z$</font> $\rm{[deg/s]}$ がワークスペースに保存されます．そして，これらをそれぞれ $\verb+data_accel.mat+$, $\verb+data_gyro.mat+$ という名前の mat ファイルで保存するため，以下のように入力します．

```matlab:加速度データの保存
>> save data_accel ax ay az  % data_accel.mat の保存
```
```matlab:角速度データの保存
>> save data_gyro wx wy wz  % data_gyro.mat の保存
```
:::

:::note warn
<INS>ステップ 2</INS>
　コマンドウィンドウで

```matlab:$\verb+data_accel.mat+$ の読み込みと平均の計算
>> load data_accel
>> mean_ax = mean(ax)
mean_ax =
   10.1848
>> mean_ay = mean(ay)
mean_ay =
    0.0922
>> mean_az = mean(az)
mean_az =
   -1.2272
```

と入力し，mat ファイル  $\verb+data_accel.mat+$ を読み込み，また，加速度 <font color="#E48312">$a_x$</font>, <font color="#0070C0">$a_y$</font>, <font color="#E84746">$a_z$</font> $\rm{[m/s^2]}$ の基準値 $\verb+mean_x_a+$, $\verb+mean_y_a+$, $\verb+mean_z_a+$ を計算します．

　同様に，コマンドウィンドウで

```matlab:$\verb+data_gyro.mat+$ の読み込みと平均の計算
>> load data_gyro
>> mean_wx = mean(wx)
mean_wx =
   -2.9691
>> mean_wy = mean(wy)
mean_wy =
    0.9323
>> mean_wz = mean(wz)
mean_wz =
    0.6750
```

と入力し，mat ファイル  $\verb+data_gyro.mat+$ を読み込み，また，加速度 <font color="#E48312">$\omega_x$</font>, <font color="#0070C0">$\omega_y$</font>, <font color="#E84746">$\omega_z$</font> $\rm{[deg/s]}$ の基準値 $\verb+mean_x_g+$, $\verb+mean_y_g+$, $\verb+mean_z_g+$ を計算します．
:::

:::note warn
<INS>ステップ 3</INS>
　<b><font color="#00B0F0">6 軸センサ MPU6050</font></b> が搭載されている<b>ブレッドボードを水平</b>に静止させた状態で，Simulink モデル
 
* $\verb+imu03_mpu6050_calibration.slx+$

を実行します．その結果，以下のように基準値が理想の値に近くなることが確認できます．

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/78348fa4-39f0-7adc-387b-6596ef571032.png)

Simulink モデル

* $\verb+imu04_mpu6050_calibration_datalog.slx+$

を実行して，キャリブレーションを行った後の加速度データ <font color="#E48312">$a_x$</font>, <font color="#0070C0">$a_y$</font>, <font color="#E84746">$a_z$</font> $\rm{[m/s^2]}$ と角速度データ <font color="#E48312">$\omega_x$</font>, <font color="#0070C0">$\omega_y$</font>, <font color="#E84746">$\omega_z$</font> $\rm{[deg/s]}$ を，10 秒間，検出しました．このときのデータを mat ファイル $\verb+data_cal_accel.mat+$, $\verb+data_cal_gyro.mat+$ に保存し，平均を求めると，以下のようになります．

```matlab:キャリブレーションを行った後の加速度データの保存と平均の計算
>> save data_cal_accel ax ay az
>> mean_cal_ax = mean(ax)
mean_cal_ax =
    9.8107
>> mean_cal_ay = mean(ay)
mean_cal_ay =
    0.0103
>> mean_cal_az = mean(az)
mean_cal_az =
  -1.6805e-04
```
```matlab:キャリブレーションを行った後の角速度データの保存と平均の計算
>> save data_cal_gyro wx wy wz
>> mean_cal_wx = mean(wx)
mean_cal_wx =
    0.0406
>> mean_cal_wy = mean(wy)
mean_cal_wy =
   -0.0086
>> mean_cal_wz = mean(wz)
mean_cal_wz =
    0.0019
```

また，Scope で観測した結果を以下に示します．

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/f4f252d7-bfa5-d5aa-e251-aa6c5eb68991.png)

:::



## <font color="#33CC33">3.5　6 軸センサにより 3 軸まわりの角度がわかるの？</font>

　ここでは，一例として，<b>$\pmb{z}$ 軸まわりの角度 $\pmb{\theta}$</b> を算出する方法について検討します．

### <font color="#33CC33">3.5.1　ジャイロセンサの値から角度を算出してみる</font>

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/c2c1ce5f-69cb-e494-0d8c-761ce02579ac.png)

　理想的には，<b><font color="#0CA274">ジャイロセンサ</font></b>で検出される角速度 $\omega_z(t) = \dot{\theta}_g(t)$ を
 
```math
\begin{align}
   \theta(t) = \theta_{\rm g}(t) := \int_0^t\omega_z(t) {\rm d}t
    \tag{1}
\end{align}
```

のように積分することで，<font color="#0CA274">$z$ 軸まわりの角度 $\theta_{\rm g}(t)$</font> を算出することができます．

　しかし，
 
* 初期姿勢が基準姿勢からズレている $(\theta_{\rm g}(0) \neq 0)$
* キャリブレーションが適切ではなく，角速度 $\omega_z(t)$ が $\delta$ ほどズレている（バイアスが $\delta$）

という状況下では，積分値は

 ```math
\begin{align}
    \int_0^t(\omega_z(t) + \delta){\rm d}t
    = \theta_{\rm g}(t) - \theta_{\rm g}(0) + {\delta}t
    \tag{2}
\end{align}
```

となります．したがって，

* $\theta_{\rm g}(0)$ ほどズレてしまう
* ドリフト ${\delta}t$ を生じてしまう

ということが問題となります．

　それでは，Simulink モデル
 
* $\verb+imu05_mpu6050_gyro_angle.slx+$

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/460a1bcd-5a04-b36e-229e-702f8950c7a3.png)

を以下に示す条件で動作させ，<b><font color="#0CA274">ジャイロセンサ</font></b>の特性を確認してみましょう．

:::note warn
<b><font color="#0CA274">ジャイロセンサ</font>による角度算出（キャリブレーションあり，初期姿勢が水平）</b>

　まず，先に行ったキャリブレーションによる補正を行います．

```matlab:$\verb+data_gyro.mat+$ の読み込みと平均の計算
>> load data_gyro
>> mean_wx = mean(wx);
>> mean_wy = mean(wy);
>> mean_wz = mean(wz);
```
そして，<b><font color="#00B0F0">6 軸センサ MPU6050</font></b> が搭載されている<b>ブレッドボードを水平</b>にし，Simulink モデル $\verb+imu05_mpu6050_gyro_angle.slx+$ を実行してください．

[![001_ジャイロ_通常 - frame at 0m6s.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/48721eb8-a63a-9b20-a376-c6cc8596cfbf.jpeg)](https://youtu.be/2bXTSpyQhmc)
https://youtu.be/2bXTSpyQhmc

イイ感じに角度が算出できていますね！

:::

:::note warn
<b><font color="#0CA274">ジャイロセンサ</font>による角度算出（キャリブレーションあり，初期姿勢が水平ではない）</b>

　つぎに，<b><font color="#00B0F0">6 軸センサ MPU6050</font></b> が搭載されている<b>ブレッドボードを少し傾けて</b>から Simulink モデル $\verb+imu05_mpu6050_gyro_angle.slx+$ を実行してください．

[![003_ジャイロ_初期値 - frame at 0m0s.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/aa14c318-ea2e-d180-58fb-6c41acc6f544.jpeg)](https://youtu.be/4ePPm2TmODk)
https://youtu.be/4ePPm2TmODk

　初期角度が $\theta_{\rm g}(0) \neq 0$ なので，<b><font color="#0CA274">ジャイロセンサ</font></b>では，ここを基準とした角度を算出してしまっていることがわかります．
:::

:::note warn
<b><font color="#0CA274">ジャイロセンサ</font>による角度算出（キャリブレーションなし，初期姿勢が水平）：ドリフト現象</b>

　<b><font color="#0CA274">ジャイロセンサ</font></b>のキャリブレーションを行わないと，どうなるでしょうか．まず，$z$ 軸まわりの角速度の補正値を

```matlab:ジャイロセンサの補正値のリセット
>> mean_wz = 0;
```

のように，リセットします．そして，<b><font color="#00B0F0">6 軸センサ MPU6050</font></b> が搭載されている<b>ブレッドボードを水平</b>にして，Simulink モデル $\verb+imu05_mpu6050_gyro_angle.slx+$ を実行すると，

[![002_ジャイロ_ドリフト - frame at 0m9s.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/0c6ecb5c-cb17-5101-af9d-58685624defc.jpeg)](https://youtu.be/47j26MhxJJI)
https://youtu.be/47j26MhxJJI

のように，<b>ドリフト項 $\pmb{{\delta}t}$ の影響が現れる</b>ことがわかります．
:::



### <font color="#33CC33">3.5.2　加速度センサの値から角度を算出してみる</font>

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/17e70d9c-a0f9-4fa0-825b-ceef8df85baa.png)

　<b><font color="#E32D91">加速度センサ</font></b>をゆっくりと動かしているとき，加速度はほぼ重力 $g$ のみです．このような状況下では，

```math
\begin{align}
    &
    \tan\theta_{\rm a}(t)
    = \dfrac{a_x}
            {a_y}
    \\
    &
    \quad \Longrightarrow \quad
    \theta(t) = \theta_{\rm a}(t) 
    := -\tan^{-1}\dfrac{a_x}{a_y}
    \tag{3}
\end{align}
```
により <font color="#E32D91">$z$ 軸まわりの角度 $\theta_{\rm a}(t)$</font> を算出することができます．

　Simulink モデル
 
* $\verb+imu06_mpu6050_accel_gyro_angle.slx+$

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/b4cef5bc-6a17-814f-ab41-c6f048022bbc.png)

を以下に示す条件で動作させ，<b><font color="#E32D91">加速度センサ</font></b>の特性を確認してみましょう．

:::note warn
<b><font color="#E32D91">加速度センサ</font>による角度算出（キャリブレーションあり，初期姿勢が水平）</b>

　まず，先に行ったキャリブレーションによる補正を行います．

```matlab:$\verb+data_gyro.mat+$, $\verb+data_accel.mat+$ の読み込みと平均の計算
>> load data_gyro
>> mean_wx = mean(wx);
>> mean_wy = mean(wy);
>> mean_wz = mean(wz);
>> load data_accel
>> mean_ax = mean(ax);
>> mean_ay = mean(ay);
>> mean_az = mean(az);
```

そして，<b><font color="#00B0F0">6 軸センサ MPU6050</font></b> が搭載されている<b>ブレッドボードを水平</b>にして，Simulink モデル $\verb+imu06_mpu6050_accel_gyro_angle.slx+$ を実行してください．

[![001_加速度_通常 - frame at 0m6s.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/2677ac0d-178f-7358-1d8d-ecd89b38ab66.jpeg)](https://youtu.be/J9UKAzjDdtw)
https://youtu.be/J9UKAzjDdtw

このように，ゆっくりと動かしたとき，<b><font color="#E32D91">加速度センサ</font></b>により算出された角度 <font color="#E32D91">$\theta_{\rm a}$</font> は，<b><font color="#0CA274">ジャイロセンサ</font></b>により算出された角度 <font color="#0CA274">$\theta_{\rm g}$</font> と似通った形状であることが確認できます．ただし，<b><font color="#E32D91">加速度センサ</font></b>を用いると，<b><font color="#E32D91">高周波ノイズの影響が大きい</font></b>ことが確認できます．
:::

:::note warn
<b><font color="#E32D91">加速度センサ</font>による角度算出（キャリブレーションあり，初期姿勢が水平）：振動を加える</b>

　高周波成分が問題となることを確認するために，<b><font color="#00B0F0">6 軸センサ MPU6050</font></b> が搭載されている<b>ブレッドボードを水平</b>で静止させている姿勢で，ブレッドボードを叩いて衝撃を与えてみます．

[![003_加速度_振動 - frame at 0m6s.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/13c064f0-57de-8acb-013b-c933ab4e99d9.jpeg)](https://youtu.be/JPymm0VGwdM)
https://youtu.be/JPymm0VGwdM

このように，<b><font color="#E32D91">加速度センサ</font></b>は<b><font color="#E32D91">振動に過敏に反応</font></b>して，インパルス状の信号を出していることがわかります．一方で，<b><font color="#0CA274">ジャイロセンサ</font></b>はこのような<b><font color="#0CA274">振動に強い</font></b>ことも確認できます．
:::

:::note warn
<b><font color="#E32D91">加速度センサ</font>による角度算出（キャリブレーションあり，初期姿勢が水平ではない）</b>

　つぎに，<b><font color="#00B0F0">6 軸センサ MPU6050</font></b> が搭載されている<b>ブレッドボードを少し傾けて</b>から Simulink モデル $\verb+imu06_mpu6050_accel_gyro_angle.slx+$ を実行してください．

[![002_加速度_初期値 - frame at 0m1s.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/966a241c-7131-1922-4319-728a8ab41804.jpeg)](https://youtu.be/HkeySzLwcEo)
https://youtu.be/HkeySzLwcEo

<b><font color="#0CA274">ジャイロセンサ</font></b>と異なり，<b><font color="#E32D91">加速度センサ</font></b>では，初期姿勢に依存せずに角度を算出できることが確認できます．
:::



:::note warn
<b><font color="#E32D91">加速度センサ</font>による角度算出（加速度センサのキャリブレーションなし，ジャイロセンサのキャリブレーションあり，初期姿勢が水平）</b>

　さいごに，<b><font color="#E32D91">加速度センサ</font></b>のキャリブレーションを行わないと，どうなるかを確認しましょう．まず，$x$，$y$ 軸方向の加速度の補正値を

```matlab:加速度センサの補正値のリセット
>> mean_ax = 0;
>> mean_ay = 0;
```

のように，リセットします．そして，Simulink モデル $\verb+imu06_mpu6050_accel_gyro_angle.slx+$ を実行すると，

[![004_加速度_キャリブレーションなし - frame at 0m9s.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/8ea59c0a-9f09-9187-257c-752b70c97258.jpeg)
](https://youtu.be/O2v0XLGMDRU)
https://youtu.be/O2v0XLGMDRU

のように，角度が正しく算出されません．これは，重力を $x$ 軸と $y$ 軸方向に分解したときの三角形が歪んでしまうからです．
:::

### <font color="#33CC33">3.5.3　相補フィルタによりイイ感じにセンサフュージョン</font>

　以上のことをまとめると，


* <b><font color="#E32D91">加速度センサ</font></b>による角度算出
    * <b><font color="#E32D91">低周波成分の信頼性が高い</font></b>
    * 高周波成分の信頼性が低い（高周波ノイズが問題）
* <b><font color="#0CA274">ジャイロセンサ</font></b>による角度算出
    * <b><font color="#0CA274">高周波成分の信頼性が高い</font></b>
    * 低周波成分の信頼性が低い（ゆっくりとズレるドリフト現象が問題）

という特徴があります．そこで，<b><font color="#E32D91">低周波</font></b>の情報を得意とする<b><font color="#E32D91">加速度センサ</font></b>からは<b><font color="#E32D91">低周波成分</font></b>を抽出し，<b><font color="#0CA274">高周波</font></b>の情報を得意とする<b><font color="#0CA274">ジャイロセンサ</font></b>からは<b><font color="#0CA274">高周波成分</font></b>を抽出して，それぞれを加算します．具体的には，
```math
\begin{align}
    G_{\rm L}(s) + G_{\rm H}(s) = 1
\end{align}
```
を満足する<b>ローパスフィルタ (LPF)</b>
```math
\begin{align}
    G_{\rm L}(s) = \dfrac{1}{1 + Ts}
\end{align}
```
と<b>ハイパスフィルタ (HPF)</b>
```math
\begin{align}
    G_{\rm H}(s) = \dfrac{Ts}{1 + Ts}
\end{align}
```
を用い，次式により角度算出を行います．

:::note info
<b><font color="#00B0F0">相補フィルタ (Complementary Filter)</font></b>
```math
\begin{align}
    \theta(t) = \theta_{\rm c}(t) 
    := G_{\rm L}(s)\theta_{\rm a}(t) 
    +  G_{\rm H}(s)\theta_{\rm g}(t) 
    \tag{4}
\end{align}
```

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/c2285a20-faac-733f-5594-3190d79dc76b.png)
:::

このように，複数のセンサから得られるデータを統合し，単一のセンサでは得られない正確で信頼性の高い情報を生成する技術を<b>センサフュージョン (Sensor Fusion)</b> といいます．

　相補フィルタは，カルマンフィルタのような高度なアルゴリズムと比べて，計算量が少ないにもかかわらず，そこそこの実用的な結果が得られるという利点があります．





　それでは，Simulink モデル
 
* $\verb+imu07_mpu6050_comp_filter_angle.slx+$

を以下に示す条件で動作させてみましょう．

:::note warn
<b><font color="#00B0F0">相補フィルタ</font>による角度算出（キャリブレーションあり，初期姿勢が水平）</b>

　まず，先に行ったキャリブレーションによる補正を行います．

```matlab:$\verb+data_gyro.mat+$, $\verb+data_accel.mat+$ の読み込みと平均の計算
>> load data_gyro
>> mean_wx = mean(wx);
>> mean_wy = mean(wy);
>> mean_wz = mean(wz);
>> load data_accel
>> mean_ax = mean(ax);
>> mean_ay = mean(ay);
>> mean_az = mean(az);
```

そして，Simulink モデル $\verb+imu07_mpu6050_comp_filter_angle.slx+$ を実行してください．

[![001_相補フィルタ_通常 - frame at 0m15s.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/56534224-64e4-3e89-fbc3-f1295d4236c8.jpeg)](https://youtu.be/R2dm82gO0vs)
https://youtu.be/R2dm82gO0vs

<b><font color="#00B0F0">相補フィルタ</font></b>はイイ感じですね！
:::

:::note warn
<b><font color="#00B0F0">相補フィルタ</font>による角度算出（キャリブレーションあり，初期姿勢が水平）</b>

　つぎに，<b><font color="#00B0F0">6 軸センサ MPU6050</font></b> が搭載されている<b>ブレッドボードを少し傾けて</b>から Simulink モデル $\verb+imu07_mpu6050_comp_filter_angle.slx+$ を実行してください．

[![002_相補フィルタ_初期角度のズレ - frame at 0m2s.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/1dfeeb34-7f10-0f3a-6771-5c2522e63ac5.jpeg)](https://youtu.be/R2dm82gO0vs)
https://youtu.be/R2dm82gO0vs

<b><font color="#00B0F0">相補フィルタ</font></b>は初期姿勢の影響を受けず，イイ感じですね！
:::

:::note warn
<b><font color="#00B0F0">相補フィルタ</font>による角度算出（ジャイロセンサのキャリブレーションなし，加速度センサのキャリブレーションあり，初期姿勢が水平）</b>

　最後に，<b><font color="#0CA274">ジャイロセンサ</font></b>のキャリブレーションが適切でない場合，どうなるかを確認しましょう．まず，$z$ 軸まわりの角速度の補正値を

```matlab:ジャイロセンサの補正値のリセット
>> mean_wz = 0;
```

のように，リセットします．そして，<b><font color="#00B0F0">6 軸センサ MPU6050</font></b> が搭載されている<b>ブレッドボードを水平</b>にして， Simulink モデル $\verb+imu07_mpu6050_comp_filter_angle.slx+$ を実行すると，

[![003_相補フィルタ_キャリブレーションなし - frame at 0m17s.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/c07671ec-3960-5f2e-a625-5887cee0e75c.jpeg)](https://youtu.be/2zwizFUzfPQ)
https://youtu.be/2zwizFUzfPQ

となります．<b><font color="#00B0F0">相補フィルタ</font></b>はドリフトの影響を受けず，イイ感じですね！
:::



# <font color="#33CC33">5. 倒立振子よ、今こそ立ち上がれ！</font>

　さて，<b><font color="#E84746">いよいよ，倒立振子です！</font></b>

　今回は，セグウェイタイプの<b><font color="#E84746">車輪型倒立振子</font></b>を LEGO で製作し，コントローラを手動で調整して倒立させます．

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/b8796e4e-7ef6-a1f7-c125-667903c15f78.png)



## <font color="#33CC33">5.1　車輪型 LEGO 倒立振子の組み立て</font>

### <font color="#33CC33">5.1.1　電子部品リスト (2024/12/06)</font>
　以下の電子部品を使用して<b><font color="#E84746">車輪型倒立振子</font></b>を製作しました．

|   |型番|メーカー|購入先|価格|
|---|---|---|---|---|
|<font color="#E84746">【再掲】</font><br>マイコンボード|Arduino Mega 2560 R3|Arduino|[RS](https://jp.rs-online.com/web/p/arduino/7154084)|5,940 円|
|<font color="#E84746">【再掲】</font><br>6 軸センサ<br>（はんだ付けオプション：L 字ピン）|MPU6050 (GY-521)|-|[電子工作ステーション](https://electronicwork.shop/items/6262bf96be482f6d864f08c9)|360 円<br>（+500 円）|
|<font color="#E84746">【再掲】</font><br>ミニブレッドボード|BB-601|-|[秋月電子通商](https://akizukidenshi.com/catalog/g/gP-05158/)|160 円|
|<font color="#E84746">【再掲】</font><br>USB ケーブル（リールケーブル A-B タイプ）|-|COMON(カモン)|[amazon](www.amazon.co.jp/dp/B0041DL830)|584 円|
|<font color="#E84746">【再掲】</font><br>プロトタイプ開発用シールド|DFR0131|[DFRobot](https://www.dfrobot.com/)|[digikey](https://www.digikey.jp/ja/products/detail/dfrobot/DFR0131/6588456)<br><br>[mouser](https://www.mouser.jp/ProductDetail/DFRobot/DFR0131?qs=Zcin8yvlhnOKshIBlIpE3A%3D%3D)|2,116 円<br><br>2,137 円|
|基板用端子台<br>（2.54mm ピッチ 1 列 3 極）<br>【2 個必要】|790-1092|RS PRO|[RS](https://jp.rs-online.com/web/p/pcb-terminal-blocks/7901092)|174 円|
|モータドライバ<br>（はんだ付けオプション）|DRV8833|-|[電子工作ステーション](https://electronicwork.shop/items/633fb0a5f3de5c361ebadc45)|150 円<br>（+500 円）|
|エンコーダ付ギヤード DC モータ<br>（減速比 50:1，エンコーダ分解能 7PPR）<br>【2 個必要】|FIT0482|[DFRobot](https://www.dfrobot.com/)|[digikey](https://www.digikey.jp/ja/products/detail/dfrobot/FIT0482/7087159)|1,944 円|
|モータブラケット（2 個入）|FIT0160|[DFRobot](https://www.dfrobot.com/)|[digikey](https://www.digikey.jp/ja/products/detail/dfrobot/FIT0160/7087133)|572 円|
|単 3 x 6 本用電池ボックス||Sitrda|[amazon](https://www.amazon.co.jp/dp/B07KF2NKS2)|450 円|
|LEGO 用軸アダプタ付車輪<br>Black Multi-Hub Wheel for TT / Lego or N20 Motor - 65mm Diameter<br>【2 個必要】|4205|[Adafruit](https://www.adafruit.com/product/4205)|[digikey](https://www.digikey.jp/ja/products/detail/adafruit-industries-llc/4205/10187734)<br><br>[mouser](https://www.mouser.jp/ProductDetail/Adafruit/4205?qs=PzGy0jfpSMu2zfaLKkrDLQ%3D%3D)|319 円<br><br>322 円|

これ以外に，モータを固定するための

* [M2 x 16mm バインド小ねじ](https://www.monotaro.com/p/2902/1537/)：4 本
* [M2 ワッシャー](https://www.monotaro.com/p/5052/6997/)：2 個 

や，ケーブルの延長のための

* ケーブル
* 熱圧縮チューブ

を使用しています．

### <font color="#33CC33">5.1.2　車輪型倒立振子の LEGO 部品リストと組立図</font>

　さっそく，LEGO 部品を利用して，<b><font color="#E84746">車輪型倒立振子</font></b>のボディを製作してみましょう．

* LEGO 部品のリストは[こちら（PDF ファイル）](https://github.com/Prof-Carter/Advent_Calendar_2024/blob/main/LEGO_parts_segway.pdf)です．

* 組立図は[こちら（PDF ファイル）](https://github.com/Prof-Carter/Advent_Calendar_2024/blob/main/segway_compressed.pdf)です．
 
 ![アニメ.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/dd9afd9c-fbcc-42ae-02dc-04fb33a42273.gif)

この組立図をもとに組み立てた本体は，このようになります．

 ![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/9fb73b5d-c14a-e26f-aff3-f7134238d6f5.png)


### <font color="#33CC33">5.1.2　モータの LEGO 部品への取り付け</font>

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/0d93af1b-949a-77a0-0cdd-c0b5a636119f.png)

　DFRobot のエンコーダ付ギヤード DC モータ [FIT0482](https://www.digikey.jp/ja/products/detail/dfrobot/FIT0482/7087159) とモータブラケット [FIT0160](https://www.digikey.jp/ja/products/detail/dfrobot/FIT0160/7087133) を，M2 のねじにより [LEGO 部品 [40490]](https://brickers.jp/search?type=product&q=40490) に取り付けます．モータブラケット [FIT0160](https://www.digikey.jp/ja/products/detail/dfrobot/FIT0160/7087133) にもねじが付属しているのですが，短いので，別途購入しました．

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/598779f5-aab0-e6bd-5c32-e65794979a19.png)

　モータブラケットのねじ穴と LEGO 部品の穴の位置は少しだけずれているのですが，ねじを締めると固定できます（ねじが少し「ハの字」にはなります）．ねじは左右を少しずつ締めるようにしてください．

　そして，下図に示すように，[LEGO 部品 [32054]](https://brickers.jp/search?type=product&q=32054) をモータブラケット [FIT0160](https://www.digikey.jp/ja/products/detail/dfrobot/FIT0160/7087133) の上部に押し付けてください．

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/4d284555-581e-9632-c913-d278bbdacf71.png)

また，[LEGO 用軸アダプタ付車輪](https://www.adafruit.com/product/4205)から軸アダプタを取り外し，モータ軸に取り付けてください．

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/6becf6f3-dd3a-42b9-cedd-ceca741db0dd.png)


　最後に，モータケーブルの延長を行います．モータケーブルは 6 本の線で構成されていますが，これらのうち，

* <b>緑色 [$\tt{C1 (A)}$] の線</b>（USB コネクタの逆側のモータ）
* <b>黄色 [$\tt{C2 (B)}$] の線</b>（USB コネクタの逆側のモータ）
* <b>白色 [$\tt{M1 (M+)}$] の線</b>（USB コネクタ側のモータ）
* <b>赤色 [$\tt{M2 (M\verb+-+)}$] の線</b>（USB コネクタ側のモータ）

の長さが不足しています．そのため，

* <b>緑色 [$\tt{C1 (A)}$] の線</b>と<b>黄色 [$\tt{C2 (B)}$] の線</b>は，手持ちの緑色，黄色のジャンパワイヤと「はんだ接続」をして延長（熱圧縮チューブで補強）
* <b>白色 [$\tt{M1 (M+)}$] の線</b>と<b>赤色 [$\tt{M2 (M\verb+-+)}$] の線</b>は，手持ちの白色，赤色の線と「はんだ接続」をして延長（熱圧縮チューブで補強）

し，モータのコネクタに接続しました．
 
### <font color="#33CC33">5.1.3　マイコンの配線と LEGO 部品への取り付け</font>

　<b><font color="#0070C0">Arduino Mega</font></b> に[プロトタイプ開発用シールド](https://www.digikey.jp/ja/products/detail/dfrobot/DFR0131/6588456)を装着し，その上に，[ミニブレッドボード](https://akizukidenshi.com/catalog/g/gP-05158/)を両面テープで取り付けます．

　つぎに，ジャンパワイヤなどを併用して，下図のように接続します．

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/9316524a-39f2-7c39-e45e-dbafa4e0a422.png)

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/c6d30971-ab8d-7b7d-0e28-188cfba8c1d2.png)

　最後に，下図のように，両面テープでマイコンをボディ上部の LEGO 部品に固定します．

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/dcf840e8-3687-fe82-c834-ab252418f7ba.png)



### <font color="#33CC33">5.1.4　電池ボックスの LEGO 部品への取り付け</font>

　下図のように，両面テープで[電池ボックス](https://www.amazon.co.jp/dp/B07KF2NKS2)をボディに固定します．固定の際，<b><font color="#E84746">電池の位置が中心線となるように気をつけてください．</font></b>
 
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/50c1b3ca-ea46-e713-c8bc-01e91687815c.png)

また，[電池ボックス](https://www.amazon.co.jp/dp/B07KF2NKS2)の電源プラグを <b><font color="#0070C0">Arduino Mega</font></b> の DC ジャックに挿し込んでください．

### <font color="#33CC33">5.1.5　完成図</font>

　以上で，<b><font color="#E84746">車輪型倒立振子</font></b>は完成です！

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/f50a9a50-0964-207b-35a9-d69f2eed5fd7.png)

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/caf87104-1a2b-515d-b7ef-2524e9255501.png)


## <font color="#33CC33">5.2　相補フィルタの動作を再確認</font>

### <font color="#33CC33">5.2.1　土台の LEGO 部品リストと組立図</font>

　<b><font color="#E84746">車輪型倒立振子</font></b>に搭載された <b><font color="#00B0F0">6 軸センサ MPU6050</font></b> の動作確認をするために，LEGO 部品で土台を製作してみましょう．

* LEGO 部品のリストは[こちら（PDF ファイル）](https://github.com/Prof-Carter/Advent_Calendar_2024/blob/main/LEGO_parts_base_wide.pdf)です．

* 組立図は[こちら（PDF ファイル）](https://github.com/Prof-Carter/Advent_Calendar_2024/blob/main/base_wide_compressed.pdf)です．

![アニメ_base_wide.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/d252a832-9f85-7330-8c8d-04151f4e6553.gif)

この組立図をもとに組み立てた本体は，このようになります．

![base_wide.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/07cabd9c-29a3-030e-2e7c-63f18d901876.png)

### <font color="#33CC33">5.2.2　相補フィルタの動作</font>

　<b><font color="#00B0F0">相補フィルタ</font></b>の動作を確認するために，下図に示すように，車輪を取り外した<b><font color="#E84746">倒立振子</font></b>のモータ軸を，土台に取り付けます．

![取り付け.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/720a0d7e-4b2d-72cc-6f54-8d4f01fd7179.gif)

このようにすると，モータ後部に取り付けられている<b><font color="#E48312">ロータリエンコーダ</font></b>により得られる角度がボディ角と等しいため，<b><font color="#00B0F0">相補フィルタ</font></b>により算出されるボディ角の妥当性を検証することができます．

　なお，<b><font color="#E48312">ロータリエンコーダ</font></b>の Simulink での使用方法については，2023 年のアドカレの記事

https://qiita.com/Carter/items/5526a72d05f90d073377

を参考にしてください．

:::note warn
<b><font color="#E84746">車輪型倒立振子</font>に搭載した<font color="#00B0F0">相補フィルタ</font>による角度算出（キャリブレーションあり，初期姿勢が真上で倒立した状態）</b>

　まず，先に行ったキャリブレーションによる補正を行います．

```matlab:$\verb+data_gyro.mat+$, $\verb+data_accel.mat+$ の読み込みと平均の計算
>> load data_gyro
>> mean_wx = mean(wx);
>> mean_wy = mean(wy);
>> mean_wz = mean(wz);
>> load data_accel
>> mean_ax = mean(ax);
>> mean_ay = mean(ay);
>> mean_az = mean(az);
```

そして，<b><font color="#E84746">車輪型倒立振子</font></b>を真上で倒立した状態で静止させてから，Simulink モデル $\verb+segway01_sensor.slx+$ を実行してください．

[![001_segway01_sensor - frame at 0m19s.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/4637af7e-f273-2938-207d-3888d8a51b12.jpeg)](https://youtu.be/btPc2tR-ylE)
https://youtu.be/btPc2tR-ylE

<b><font color="#00B0F0">相補フィルタ</font></b>による角度算出の結果は<b><font color="#E48312">ロータリエンコーダ</font></b>による角度算出の結果とほぼ同じであり，イイ感じですね！
:::

:::note warn
<b><font color="#E84746">車輪型倒立振子</font>に搭載した<font color="#00B0F0">相補フィルタ</font>による角度算出（キャリブレーションあり，初期姿勢が倒れている状態）</b>

　つぎに，<b><font color="#E84746">車輪型倒立振子</font></b>を倒した状態で静止させてから，Simulink モデル $\verb+segway01_sensor.slx+$ を実行してください．

[![002_segway01_sensor_初期値 - frame at 0m3s.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/5266854a-55bc-3fd1-6f72-3e8df37008f7.jpeg)](https://youtu.be/QeoA138hmmA)
https://youtu.be/QeoA138hmmA

<b><font color="#E48312">ロータリエンコーダ</font></b>により算出される角度は初期姿勢からの相対角です．それに対し，<b><font color="#00B0F0">相補フィルタ</font></b>により算出される角度は真上での倒立状態を基準とした絶対角です．そのため，初期姿勢の影響を受けず，イイ感じですね！
:::





## <font color="#33CC33">5.3　いよいよ、車輪型倒立振子の安定化</font>

### <font color="#33CC33">5.3.1　安定化コントローラの形式</font>

　<b><font color="#E84746">車輪型倒立振子</font></b>の安定化を行うために

:::note info
<b><font color="#E84746">状態フィードバック形式のコントローラ</font></b>
```math
\begin{align}
    u = {k}_{1}\phi 
      + {k}_{2}\dot{\phi} 
      + {k}_{3}\theta
      + {k}_{4}\dot{\theta}
    \tag{5}
\end{align}
```
:::

を用います．ここで，各状態は以下のとおりです．

* <font color="#E48312">$\pmb{\phi}$</font>：左右の<b><font color="#E48312">車輪角</font></b>の平均であり，左右のモータに取り付けられている<b><font color="#E48312">ロータリエンコーダ</font></b>により算出します．

* <font color="#E48312">$\pmb{\dot{\phi} = {\rm d}\phi/{\rm d}t}$</font>：左右の<b><font color="#E48312">車輪角速度</font></b>の平均であり，<font color="#E48312"><b>車輪角</b> $\pmb{\phi}$</font> を<b>不完全微分（積分器＋ローパスフィルタ $\pmb{s{\hskip1pt}G_{\rm f1}(s) = s/(1 + T_{\rm f1}s)}$ に通過させたもの）</b>を使用します．

* <font color="#E32D91">$\pmb{\theta}$</font>：<b><font color="#E32D91">ボディ角</font></b>であり，<b><font color="#00B0F0">相補フィルタ</font></b>により算出します．

* <font color="#E32D91">$\pmb{\dot{\theta} = {\rm d}\theta/{\rm d}t}$</font>：<b><font color="#E32D91">ボディ角速度</font></b>であり，<b><font color="#0CA274">ジャイロセンサ</font></b>により得られる <b><font color="#E84746">$\pmb{\omega_z}$</font> をローパスフィルタ $\pmb{G_{\rm f2}(s) = 1/(1 + T_{\rm f2}s)}$ に通過させたもの</b>を使用します．

また，$\pmb{k_1,\ k_2,\ k_3,\ k_4}$ は設計者が値を決定する<b>コントローラゲイン</b>です．

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/727e25e6-ece8-3c5b-e7c0-6c1dd614e7b6.png)

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/af77740f-fc20-e694-bcdd-fa6bb13a449c.png)

　制御屋さんであれば，
 
 * ラグランジュ法などにより<b><font color="#E84746">車輪型倒立振子</font></b>の数理モデルを導出
 * 数理モデルに含まれるパラメータを実験データから決定
 * 数理モデルを線形化して，状態方程式を導出
 * 最適レギュレータや極配置法などによりコントローラゲインを設計

というアプローチをとるでしょうが，ここでは，手動調整によりコントローラゲインを決定します．


### <font color="#33CC33">5.3.2　コントローラゲインを手動調整！</font>

　使用する Simulink モデル

* $\verb+segway02_sfbk.slx+$

を下図に示します．

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/79bc0a7e-e4f8-478f-13ac-2a8af01a4022.png)

:::note info
$\verb+Subsystem (Inverted Pendulum)+$ のなかに記述されている $\verb+Subsystem (body)+$ では，下図のように，<b><font color="#00B0F0">相補フィルタ</font></b>によるボディ角 $\theta$ を，加えて，<b><font color="#0CA274">ジャイロセンサ</font></b>とローパスフィルタを利用してボディ角速度 $\dot{\theta} = {\rm d}\theta/{\rm d}t$ を算出しています．

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/d559cec8-60c4-a164-1030-fe972bd895bc.png)
:::

:::note info
$\verb+Subsystem (Inverted Pendulum)+$ のなかに記述されている $\verb+Subsystem (motor)+$ では，下図のように，<b><font color="#E48312">エンコーダ</font></b>用の Simulink ブロック ${\tt Encoder}$ を利用して車輪角 $\phi$ を，さらに，不完全微分により車輪角速度 $\dot{\phi} = {\rm d}\phi/{\rm d}t$ を算出しています．また，Simulink ブロック ${\tt PWM}$ を利用して「正転 / 逆転 / ブレーキ」によりモータを駆動しています．

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/6fbcc7b2-944c-0286-64e2-20cbf1dc05bb.png)

なお，<b><font color="#E48312">DC モータ / モータドライバ</font></b>の Simulink での取り扱い方法については，2023 年のアドカレの記事

* https://qiita.com/Carter/items/5526a72d05f90d073377
    * 5.4.3　正負の指令電圧により回転方向と速さを切り替える！
    * 5.4.4　モータ角の比例制御を行ってみる！

を参考にしてください．
:::

　それでは，以下の手順で，<b><font color="#E84746">車輪型倒立振子</font></b>を安定化するコントローラゲインを手動で調整してみてください．
 
:::note warn
<INS>ステップ 1</INS>

![step01.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/7f61e0e1-109e-3169-0bdb-d8a7796d3688.jpeg)

　コマンドウィンドウで

```matlab:初期設定
>> 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.005;    % サンプリング周期
>> T = 0.5;      % 相補フィルタの時定数
>> Tf1 = 0.08;   % 車輪角速度に対するローパスフィルタの時定数
>> Tf2 = 0.02;   % ボディ角速度に対するローパスフィルタの時定数
```

と入力し，初期設定を行います．そして，Simulink モデル

* $\verb+segway02_sfbk.slx+$

を実行します．
:::



:::note warn
<INS>ステップ 2</INS>

[![step02.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/8380a73d-a88d-36e3-e541-80e0f56a467e.jpeg)](https://youtu.be/4q5CNcu85D8)
https://youtu.be/4q5CNcu85D8
:::

:::note warn
<INS>ステップ 3</INS>

[![step03.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/bbbc5256-793a-e561-d702-30e306b0a4dc.jpeg)](https://youtu.be/RI0kXGk5J2c)
https://youtu.be/RI0kXGk5J2c
:::

:::note warn
<INS>ステップ 4</INS>

[![step04.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/8ada983a-f5df-8f81-9f8c-6b09d5d2fbe7.jpeg)](https://youtu.be/JYOiIGqtvrk)
https://youtu.be/JYOiIGqtvrk
:::

:::note warn
<INS>ステップ 5</INS>

[![step05.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/e83d58bf-add2-1281-4b39-cc771a91d69f.jpeg)](https://youtu.be/dBIo_7C1lsA)
https://youtu.be/dBIo_7C1lsA
:::

:::note warn
<INS>ステップ 6</INS>

[![step06.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/e7d305dc-039e-60f5-4bc9-8de23c45198d.jpeg)](https://youtu.be/6s0kVAvxf0k)
https://youtu.be/6s0kVAvxf0k
:::

:::note warn
<INS>ステップ 7</INS>

[![step07.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/0b1cd71c-f5cb-5b9c-79cc-7c5c5dae8fb7.jpeg)](https://youtu.be/9htEy7AgdD0)
https://youtu.be/9htEy7AgdD0
:::

:::note warn
<INS>ステップ 8</INS>

[![step08.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/28ad7ba3-277f-4eb6-4f60-443740583721.jpeg)](https://youtu.be/Uf1Ny52QRCc)
https://youtu.be/Uf1Ny52QRCc
:::

　通し動画はこちらからどうぞ！

https://x.com/carter_control/status/1870690322264768994

https://youtu.be/_FgkfwUo22I

　なお，最終的に調整されたコントローラゲインは
 
```math
\begin{align}
    k_1 &= 2.2 \\
    k_2 &= 0.77 \\
    k_3 &= 53.2 \\
    k_4 &= 0.96
\end{align}
```

です．


### <font color="#33CC33">5.3.3　スタンドアロンで動作させる！</font>

　ここまでは，Simulink ブロック $\tt Scope$ による動作確認やパラメータ調整を行うため，エクスターナルモードで Simulink を動かしていました．ここでは，最終的に得られたコントローラゲインを使用して，スタンドアロンで<b><font color="#E84746">車輪型倒立振子</font></b>を単独で動かします．


|   |エクスターナルモード  |スタンドアロンモード  |
|---|---|---|
|動作環境  |Simulink と USB 通信  |Arduino 単独で動作  |
|リアルタイム通信 |USB 経由で可能  |不可能  |
|用途 |モニタリング，デバッグ |実運用 |
|パラメータ調整 |Simulink 上でリアルタイム調整可能 |実行前に設定する必要あり |
|効率性 |USB 通信のオーバーヘッドあり |高い |

　スタンドアロンで動かすための Simulink モデルは

 * $\verb+segway03_sfbk_standalone.slx+$

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/0f4e3d15-c1d8-74e4-4951-600bd6d10120.png)

です．モニタリングは不要ですので，Simulink ブロックは削除しています．また，車輪角が $|\phi| > 720\ {\rm [deg]}$ となると，暴走しているとして，制御入力 $u$ 強制的に 0 にします．

　以下に，実行の手順を示します．

:::note warn
<INS>ステップ 1</INS>

　コマンドウィンドウで

```matlab:初期設定
>> 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.005;    % サンプリング周期
>> T = 0.5;      % 相補フィルタの時定数
>> Tf1 = 0.08;   % 車輪角速度に対するローパスフィルタの時定数
>> Tf2 = 0.02;   % ボディ角速度に対するローパスフィルタの時定数
>> k1 = 2.2;
>> k2 = 0.77;
>> k3 = 38;
>> k4 = 1.7;
```

と入力し，初期設定を行います．そして，

* <b>電源ボックスのスイッチを OFF にする</b>
* <b>車輪を床面から離している</b>

であることを確認して Simulink モデル

* $\verb+segway03_sfbk_standalone.slx+$

を実行します．コンパイルが終了して，<b><font color="#0070C0">Arduino Mega</font></b> への転送が終わると，USB からの電源供給により車輪が 2 回転して静止します．
:::


:::note warn
<INS>ステップ 2</INS>

　<b>電源ボックスのスイッチを ON</b> にし，<b><font color="#E84746">車輪型倒立振子</font></b>を倒立点付近で軽く支えておきます．そして，<b><font color="#0070C0">Arduino Mega</font></b> のリセットボタンを押して離すと，安定化制御が開始されます．

[![スタンドアロン横 - frame at 0m14s.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/fb311828-6a75-ac63-ee4c-ec1e662f0515.jpeg)](https://youtu.be/QeckLmwDs7g)
https://youtu.be/QeckLmwDs7g
:::




### <font color="#33CC33">5.4　注意したいこと</font>

　ネット界隈を見ると，[イチケンさんの動画](https://www.youtube.com/watch?v=FXNtDEfyKpI)も含め，ボディ角 $\theta$ の制御を目指して，PID コントローラ

```math
\begin{align}
    u = {k}_{\rm P}\theta
      + {k}_{\rm I}\int_0^t \theta\,{\rm d}t
      + {k}_{\rm D}\dot{\theta}
\end{align}
```

を使用している例が多く見られます．しかし，以下のような理由で，個人的にはあまりオススメしません．

* そもそも車輪角 $\phi$ を制御していないので，どの位置で静止するかはわかりません．エンコーダが付属していないモータを利用している場合にこの PID コントローラを利用している傾向が見られます．

* 倒立姿勢で静止している状況を考えると，理想的にはボディ角 $\theta$ は 0 ですが，実際には（わずかかもしれませんが）0 からズレています．そうすると，積分項が増加してしまいます．このことに対処するために，ゴネゴネと補正をする必要があったりして…

https://x.com/carter_control/status/1831321552828452952

https://x.com/carter_control/status/1831707931710251153

# <font color="#33CC33">6. おわりに</font>

　ということで，今年もなんとか投稿を終えました．
 
　私は決してモノづくりが得意なわけではないのですが，そんな私でも，LEGO 部品と汎用のマイコン（Arduino），センサ，モータを利用すれば，いろいろな実験装置が製作できる！ しかも Simulink で簡単に実験ができる！ 「制御工学」の分野を学ぶのであれば，仮想世界に留まらず，実世界に足を踏み入れてみませんか？

　この記事が少しでも皆さんのお役に立てば幸いです．

 【追伸】
 　しげきさんが本記事を参考にして，3D プリンタを利用して自作されたようです．
 
https://x.com/shi78ge/status/1915050368180134088

# <font color="#33CC33">参考文献</font>

* 熊谷：第4 章加速度センサ，角速度センサの活用事例「車輪移動型倒立振子の開発にみるセンサの使い方」，[Design Wave Magazine](https://www.amazon.co.jp/dp/B000RW6QKY) (CQ 出版社)，No. 117, pp. 94-105 (2007)

* 熊谷：[倒立振子のパラメータ調整](https://www.mech.tohoku-gakuin.ac.jp/rde/contents/tech/BallIPMini/techinfo.html)

* @qoopen0815：[MATLAB を使って車輪型倒立振子の運動方程式を導出する](https://qiita.com/qoopen0815/items/ebf1b0c5b4bd67c2ce8d)

* @fumiya_sato：[システム同定による倒立振子のLQR制御](https://qiita.com/fumiya_sato/items/f33ff2152686a648f2bb)