1. はじめに
最近、古典制御理論を集中的に勉強する機会があったのですが、ラプラス変換などの理論をいくら勉強しても、制御の実際がわかった気にはあまりならないのですよね。手を動かして体験できるような制御の教材が欲しいと思っていました。そんななか、「いつか作ろう」と思って昔買っていた「トランジスタ技術 2019年7月号」で特集されている「カルマン倒立振子」を思い出し、それを作ってみることにしました。
下図のようなものです。
上述のトランジスタ技術の作例では、カルマンフィルタの技術により振り子の角度 θ と 支点の位置 x の読み取りを安定させ、現代制御理論を使って倒立振子を直立させ続ける模型の工作方法が解説されています。
作例ではマイコンとして「STM32 Nucleo Board STM32F401」が使われていたのですが、全く同じでは面白くないかなと思い、Raspberry Pi Pico を使ってみることにしました。
Raspberry Pi Pico に愛着があり、使用経験を増やしたかったという理由もあります。推しマイコンボードってやつですね。何も下調べせずに決めたために色々と苦労することになりましたが（浮動小数点ユニット(FPU)がない、タイマーが一つしかない、など）、最終的に動いたので結果オーライです（そうか？）。
実際に動作している様子を示した動画はこちらです。
ちなみに、この作例はバリバリに現代制御理論を使っているので、古典制御理論の教材が欲しいという本来の目的は達成できていないのですが、まあその点はそのうちなんとかしましょう。
そんなわけで、このトランジスタ技術 2019年7月のカルマン倒立振子の Raspberry Pi Pico への移植版を作るうえでのメモを本ページに残します。
2. 注意
本ページを読むうえでいくつか注意がありますので、順に述べていきます。「トランジスタ技術 2019年7月号」が必須
カルマン倒立振子は「トランジスタ技術 2019年7月号」の特集「月着陸船アポロに学ぶ確率統計コンピュータ」で特集されているのですが、これは全体で 150 ページにも及ぶ大特集です。その内容全てを解説することはできないので、同じような倒立振子を自分でも作ってみたいと思った場合この書籍は必須です。「トランジスタ技術 2019年7月号 (電子版)」の PDF は今でも入手可能ですので、こちらはそれほど問題にはならないでしょう。「トランジスタ技術 2019年7月号」の付録 DVD に含まれるプログラムのソースコードが必須
ここが一番ネックになると思うのですが、上述の電子版にはプログラムのソースコードがごくごく一部しか含まれていません。全てのソースコードを入手するには付録 DVD が必須なのですが、古本などでは DVD が付属しないことが多いですよね。私が勝手に公開するわけにはいかないので、なんとかして入手する必要があります。図書館などで付録 DVD も一緒に貸し出しているところを探すのが良いでしょうか。ちなみに、Raspberry Pi Pico 用のソースコードは元のソースコードへのパッチという形で提供します。ビルド済のバイナリファイルも提供しますのでそれを試すことはできますが、パラメータを変更する等のためにはソースが必要となります。タミヤのユニバーサルプレートL を入手しにくい
これは時期によると思うのですが、執筆時は「タミヤ 楽しい工作シリーズ No.172 ユニバーサルプレートL 210×160mm」を入手しにくい状態が続いています。タミヤに問い合わせたところ、生産終了確定ではないが次回生産時期は未定だそうです。これについては、同じサイズの ABS 樹脂板を購入し、必要な個所に自分でピンバイスで 3mm の穴をあけることにしました。購入した ABS 樹脂板の写真が以下です。左が、通常サイズのユニバーサルプレートのサイズ（160×60mm）、右が今回用いる 210×160mm サイズです。
モータードライバ TA7291P を入手しにくい
長らく電子工作で愛用されてきた TA7291P は既に生産終了となり、現在入手がしにくいです（amazon では足が短い、足にはんだが残っているなど、いかにも中古という見た目の製品が売られていますね）。別の入手しやすいものを使おうかとも考えたのですが、そうするとモデル化の手間が増えるので、手元に複数あった TA7291P をそのまま使うことにしました。ロータリーエンコーダが高価
倒立振子の位置 x を計測するために「ロータリーエンコーダ EC202A100A」を用いるのですが、6500 円以上となかなかに高価ですよね。この価格を見たときが、この倒立振子の作成に最もくじけそうになった瞬間でした。ランクを下げたもう少し安価なものは使えないかとも考えたのですが、トラブルを避けるためにマイコン以外はなるべく同じものを用いることにしました。Raspberry Pi Pico のプログラムを C 言語で書く
Raspberry Pi Pico を使うと決めたときは「当然プログラムは Python で書くでしょ」と思っており、途中まではそうしていたのですが、「動作速度が足りない」、「安定性も足りない」など問題が多発したため、やむなく C 言語を用いることにしました。Raspberry Pi Pico を C 言語で開発するためには、開発環境として Raspberry Pi 上で cmake を使うのが一番簡単だと思います。他の Linux や Windows でもできるかもしれませんが未検証です。はんだ付けが超大変
ユニバーサル基板を用いた回路の作成は超久しぶりだったのですが、恐ろしく大変で泣きそうでした。はんだ付けをした面はとても人には見せられません。3. 必要なもの
必要なものをリストアップすると以下のようになります。| カテゴリ | 物品 | 個数 | 備考 |
| マイコン | Raspberry Pi Pico H | 1 | ピンヘッダ取り付け済の H が良いでしょう。無線機能は不要なので、W や WH を選ぶ必要はありません |
| 加速度センサ関連 | BMX055使用9軸センサーモジュール | 1 | - |
| ICソケット (6P) | 1 | - | |
| ロータリーエンコーダ関連 | 岩通マニュファクチャリング EC202A100A ロータリーエンコーダ | 1 | 買うのに覚悟が必要な価格です |
| 岩通マニュファクチャリング A150 EC202用ハーネス | 1 | - | |
| 4Pカップリング ボリュームシャフト中継用ジョイント 4P | 1 | ロータリーエンコーダとシャフトの結合に用います。シャフト側にはM3ナットをかませておきます（参考）。 | |
| タイヤ関連 | タミヤ 72003 ハイパワーギヤーボックス HE | 2 | - |
| タミヤ 70111 スポーツタイヤセット | 1 | - | |
| モータードライバ | TA7291P | 2 | この入手が問題ですね… |
| 電池関連 | 電池ボックス 単3×3本 リード線・スイッチ付 | 1 | なんでも良いと思いますが、私が使ったのはこれです。多分、ペンチなどで側面のプラスチックを広げておかないと電池の取り出しが困難です |
| 皿小ねじ(+) 皿ねじ M3×12 | 2本 | このタイプの頭が平らなねじでないと電池と干渉します。私が使ったのはこのねじではないのですが、多分大丈夫なはず（？） | |
| 充電池と対応充電器 | 3本 | 私はエネループプロを持っていたのでそれを使いましたが、雑誌ではプロではない通常のエネループを用いていますね | |
| 車体関連 | タミヤ 楽しい工作シリーズ No.172 ユニバーサルプレートL 210×160mm | 1 | 上述したように、執筆時は入手しにくい状態です。これがない場合、代用として以下の３点を用います |
| はざいや ABS樹脂板 【住友ベークライト】白 厚さ 3mm サイズ 160×210 mm | 1 | 「はざいや」さんでタミヤのユニバーサルプレートと同じ寸法を指定して購入します。私が購入したときは一枚339円でした。複数枚買うとお得になります | |
| タミヤ 精密ピンバイスD （0.1～3.2mm） | 1 | ABS 樹脂板への穴あけ用の手動のドリルです | |
| タミヤ ベーシックドリル刃セット （1，1.5，2，2.5，3mm） | 1 | ピンバイスとセットで用いるドリル刃です。3mmのもののみを使います。 なお、本ページの内容を実行するだけならばドリル刃は3mmのもので良いのですが、ユニバーサルプレートの穴の径になるべく近づけたいなら、3.2mmのドリル刃を用いた方が良いです。その場合、千石電商の「NACHI 3.2 鉄工用ドリル刃(2本組)」や「ホーザン K-5 ドリルセット」があります | |
| 抵抗 | カーボン抵抗 220Ω | 4 | LED用に3つ、PWMのローパスフィルタ用に1つ。LED用の抵抗の大きさはこの値でなくても構いません。私はLED用の3つには330Ωを使いました。なお、秋月電子通商だと100本セットでの販売が多いです。千石電商だと10本から購入できますが、値段は上がります |
| カーボン抵抗 3.3kΩ | 2 | ロータリーエンコーダの4.8Vの出力を3.3Vに落とすためのもの | |
| カーボン抵抗 2kΩ | 2 | ロータリーエンコーダの4.8Vの出力を3.3Vに落とすためのもの | |
| コンデンサ | セラミックコンデンサー 0.1μF | 5 | モータ用2つ、モータードライバ用2つ、BMX055用1つ。10個パックでちょうど良いと思います |
| セラミックコンデンサー 2.2μF | 1 | PWMのローパスフィルタ用に1つ。10個パックへのリンクを張っていますが、単品売りのほうで良いかも | |
| 電解コンデンサー 220μF | 2 | モータードライバ用2つ。極性（+/-）があるので使用時は注意 | |
| LED | 赤、緑、黄 | 各1 | なんでも良いのですが、例えば左記のものでしょうか |
| その他 | ユニバーサル基板 | 1 | 手元にあったこれを使いましたが、なんでも良いと思います |
| スズメッキ線(0.6mm 10m) | - | 回路の配線用。0.6mm が手元にあったのでそれを使いましたが、やや固いので 0.5mmの方が使いやすいかも？ | |
| 被覆付きの配線 | - | 回路の配線が交差することもあるので被覆付きの配線もあると良いでしょう。個人的には単芯のものをよく使うのですが（どこで買ったのか覚えていない）、秋月電子通商では撚線のものしかなかったのでそれにリンクしました | |
| 耐熱電子ワイヤー | - | なんでも良いのですが、モーターやロータリーエンコーダの配線を延長するために必要になります | |
| 熱収縮チューブ | - | ワイヤー同士の結合部や、ワイヤーとピンコネクタとの結合部の保護に用います | |
| ピンヘッダ | - | ニッパでカットして使います。カット時に隣接部が割れることがあるので、多めに買っておくのが安全です。私はモーター結合用に2ピン×2、ロータリーエンコーダ結合用に4ピン×1、電池結合用に2ピン×1、シリアル通信用に3ピン×1だけ使いました。 | |
| ピンソケット | - | ニッパでカットして使います。やはりカット時に隣接部が割れることがあるので、多めに買っておくのが安全です。Raspberry Pi Pico の差し込み用に20ピン×2、モータードライバの差し込み用に10ピン×2、モーター結合用に2ピン×2、ロータリーエンコーダ結合用に4ピン×1、電池結合用に2ピン×1だけ使いました。なお、モータードライバとピンソケットの接触が良くないことがあるので、その点は注意が必要です | |
| はんだ吸い取り線 | - | 一度はんだ付けしたパーツを取り外すときに用います。トラブルが一切なければ不要ですが、トラブル時にないと詰みます | |
| スペーサー M3 10mm TP-10 | - | 基板を車体に固定する際に最低4本必要になります。私は、さらにスペーサー3つをつなげたものを保護用として車体上に2本立て、倒立振子が倒れたときに地面に回路が激突しないようにしています（上の動画ではそれがわかるはず） | |
| 3mmプラネジ(8mm) | - | M3のボルトとナットはタミヤのキットに付属するのでそれで済むことが多いのですが、このようなプラネジがあると便利です | |
| 3mm六角ナット M3 | - | このようなプラナットもあると便利です | |
| 工具類 | - | リンクは張りませんが、ニッパ、ラジオペンチ、はんだごて、はんだなどは必要です。ピンセットやワイヤストリッパもあった方が良いでしょう |
4. Raspberrry Pi Pico の開発環境の設定
さて、ここから先は倒立振子の作成に入っていくわけですが、いきなり車体の組み立てに入るわけではありません。まずは、ブレッドボードを用いた「9軸センサーBMX055とカルマンフィルタを用いた角度の推定」から話を進めていきます。 書籍でいうと、p.41 から始まる「第3節 傾斜計のソフトウェア開発」および p.49 の Inclinometer.cpp の動作の部分です。そのために、まずは Raspberrry Pi Pico の開発環境の設定から始めていきましょう。 C 言語で開発を行うための基本的な情報は「The C/C++ SDK」に書かれています。
まず、通常の Raspberry Pi のデスクトップ環境を用意し、Raspberrry Pi Pico の開発に必要なツールのインストールから始めましょう。下記の2つのコマンドを順に行います。最後の「minicom」はターミナル上でシリアル通信を行うためのものです。
sudo apt update sudo apt install cmake gcc-arm-none-eabi libnewlib-arm-none-eabi libstdc++-arm-none-eabi-newlib minicomインストールが終わったら、お使いのユーザーのホームディレクトリに、Raspberry Pi Pico 用の SDK をダウンロードしましょう。一つ目のコマンドはホームディレクトリに移動するためのものです。
cd git clone https://github.com/raspberrypi/pico-sdk.git次に、ダウンロードを終えた pico-sdk の場所を環境変数 PICO_SDK_PATH にセットしましょう。そのためには、ファイル .bashrc の末尾に設定を追加する必要があります。 まずは以下のコマンドで .bashrc を編集用に開きましょう。
mousepad .bashrcそして、開いたファイルの末尾に移動し、以下の3行を追記してファイルを保存して閉じます。一つ目で 環境変数 PICO_SDK_PATH にホームディレクトリにある pico-sdk を指定しています。 二つ目は「minicom -b 115200 -o -D /dev/ttyACM0」という長いコマンドを「miniacm」という短いコマンド（エイリアス）で実行するためのものです。 二つ目は倒立振子の完成前に用いるエイリアスで、三つ目は倒立振子の完成後に（人によっては）用いるデバッグ用のエイリアスです。
export PICO_SDK_PATH=/home/$USER/pico-sdk alias miniacm="minicom -b 115200 -o -D /dev/ttyACM0" alias miniusb="minicom -b 115200 -o -D /dev/ttyUSB0"追記が終わったら、そのターミナルで下記コマンドを実行すれば設定が反映されます。
source .bashrcなお、新たに起動したターミナルでは追記した設定は自動的に読み込まれるので再度読み込む必要はありません。
以上で Raspberry Pi Pico 用のライブラリの設定が終わりました。チュートリアルサイトを参考に簡単な例を試してみましょう。Pico で「"Hello, world!」と出力する printf 命令を実行し、 それを Rapsberry Pi のターミナルで受け取って表示する、というものです。
まず、hello ディレクトリを作成し、そこに SDK から pico_sdk_import.cmake というファイルをコピーしてきましょう。
mkdir hello cd hello cp ~/pico-sdk/external/pico_sdk_import.cmake .次に、ビルド用の設定ファイル CMakeLists.txt ファイルを作成しましょう。hello ディレクトリにいるターミナルでそのまま以下のコマンドを実行します。
mousepad CMakeLists.txt空の mousepad が開いたら、以下の内容を記述しましょう。
cmake_minimum_required(VERSION 3.13)
# initialize the SDK based on PICO_SDK_PATH
# note: this must happen before project()
include(pico_sdk_import.cmake)
project(my_project)
# initialize the Raspberry Pi Pico SDK
pico_sdk_init()
# rest of your project
add_executable(hello_world
hello_world.c
)
# Add pico_stdlib library which aggregates commonly used features
target_link_libraries(hello_world pico_stdlib)
pico_enable_stdio_usb(hello_world 1)
pico_enable_stdio_uart(hello_world 0)
# create map/bin/hex/uf2 file in addition to ELF.
pico_add_extra_outputs(hello_world)
記述が終わったら、保存してそのファイルを閉じます。このファイルは、書いたプログラムをビルドするための設定ファイルとなります。
次に、C言語プログラム hello_world.c を記述しましょう。hello ディレクトリにいるターミナルでそのまま以下のコマンドを実行して 空の hello_world.c ファイルを開きます。
mousepad hello_world.c開いたら、下記の内容を記述しましょう。このファイルは、1秒おきに "Hello, world!" という文字を表示するというものです。正確には、1秒おきに"Hello, world!" という文字列をシリアル通信で送信する、という内容で、送信先は USB の接続先という設定になっています。
#include <stdio.h>
#include "pico/stdlib.h"
int main() {
stdio_init_all();
while(1){
printf("Hello, world!\n");
sleep_ms(1000);
}
return 0;
}
記述が終わったら保存してファイルを閉じます。
さて、C言語プログラムが書け、それをビルドするための設定ファイルも用意できたので次にビルドを行います。hello ディレクトリにいるターミナルで以下のコマンドを実行してビルドを行いましょう。 ビルド用のディレクトリ build を作成してから、そのディレクトリ内で「cmake ..」、「make」という二つのコマンドを実行しています。
mkdir build cd build cmake .. make実行が終わると、下記のような表示になっているのではないでしょうか。
(中略) [100%] Built target pioasm [ 98%] No install step for 'PioasmBuild' [100%] Completed 'PioasmBuild' [100%] Built target PioasmBuildそして、その build ディレクトリ内に hello_world.uf2 というファイルができているのではないかと思います。このファイルが、Pico にコピーして実行すべきファイルとなります。
Pico に hello_world.uf2 をコピーするため、Pico の BOOTSEL ボタンを押しながら Raspberry Pi に USB 接続しましょう。Pico がファイルマネージャで開きますのでその中（RPI-RP2）に hello/build ディレクトリにある hello_world.uf2 をコピーしましょう。ファイルマネージャによるドラッグアンドドロップで構いません。
コピーが終わると Pico が再起動され、先ほどの C 言語プログラムが自動的に動作を開始します。
Raspberry Pi のターミナル上でエイリアス miniacm を実行しましょう。
miniacmこのエイリアスは「minicom -b 115200 -o -D /dev/ttyACM0」というコマンドを実行したのと同じ効果があるのでした。 これは、「/dev/ttyACM0 として認識されている Pico とシリアル通信をする」という意味になります。
さて、miniacm を実行したターミナルでは、Pico が出力した「 Hello, world! 」という文字列が1秒おきに表示されているのではないでしょうか？すなわち、シリアル通信により、Pico からの文字列の送信を Raspberry Pi で受信できたことになります。
以上で動作確認終了です。まず、miniacm で実行した minicom を終了しましょう。「 Hello, world! 」という文字列が表示されているターミナル上で、キーボードで「Ctrl-A」→「Q」→「Enter」と順に入力しましょう。minicom が終了します。そして、Pico との USB 接続を切り離すことで Pico の電源を切りましょう。
5. 倒立振子用プログラムの準備
それでは、Pico 用のプログラムの準備に入りましょう。Raspberry Pi のホームディレクトリに移動し、下記のコマンドで必要なファイルをダウンロードしましょう。一つ目のコマンドがホームディレクトリへの移動を表します。cd git clone https://github.com/neuralassembly/pico-inverted-pendulumダウンロードが終わったら、pico-inverted-pendulum ディレクトリに移動します。
cd pico-inverted-pendulumこの中には３つのディレクトリがあります。
- KalmanAngle: Inclinometor.cpp をビルドするためのディレクトリ
- KalmanFinal: Inverted_Pendulum_Kalman.cpp をビルドするためのディレクトリ
- Binaries: ビルド済ファイル Inclinometor.uf2 と Inverted_Pendulum_Kalman.uf2 を格納したディレクトリ
さて、KalmanAngle と KalmanFinal の二つのディレクトリに、pico_sdk_import.cmake ファイルを KalmanAngle ディレクトリと KalmanFinal ディレクトリにコピーします。先ほどの Hello, world! プログラムでも同等の作業を行いましたね。
cp ~/pico-sdk/external/pico_sdk_import.cmake KalmanAngle cp ~/pico-sdk/external/pico_sdk_import.cmake KalmanFinal二つのディレクトリのうち、 KalmanAngle は書籍の p.41 から始まる「第3節 傾斜計のソフトウェア開発」および p.49 の Inclinometer.cpp を実行するためのものです。
また、KanlanFinal は車体が完成したあとに動作させるプログラムが格納されるディレクトリです。
以上を踏まえ、トランジスタ技術2019年7月号 付録DVD に含まれるオリジナルのプログラムのうち、3つのファイルを下記の場所にコピーします。
Inclinometer.cpp を pico-inverted-pendulum/KalmanAngle ディレクトリにコピー SolveRiccatiEquation.py と Inverted_Pendulum_Kalman.cpp を pico-inverted-pendulum/KalmanFinal ディレクトリにコピーそれが済んだら、pico-inverted-pendulum ディレクトリにいるターミナルで以下のコマンドを実行し、Raspberry Pi Pico 用のファイルに更新します。
patch -p0 -i pico-ip.patchこのコマンドにより、Inclinometer.cpp 、SolveRiccatiEquation.py 、Inverted_Pendulum_Kalman.cpp が Raspberry Pi Pico 用のプログラムに更新されました。 もちろん、これら3ファイルがあらかじめ適切な場所にないと更新は失敗します。 失敗したら、各ディレクトリにある三ファイルを一旦消し、もう一度コピーからやり直すとよいでしょう。
さて、プログラムの更新が済んだらビルドしてみましょう。下記のコマンドを順に実行します。まずは KalmanAngle ディレクトリです。
cd ~/pico-inverted-pendulum/KalmanAngle mkdir build cd build cmake .. makeそれが終わったら KalmanFinal ディレクトリです。
cd ~/pico-inverted-pendulum/KalmanFinal mkdir build cd build cmake .. makeどちらもエラーなくビルドできたら、それぞれの build ディレクトリに uf2 ができているはずです。先に進みましょう。
6. 9軸センサーBMX055とカルマンフィルタを用いた角度の推定
では、まずはブレッドボードを用いた「9軸センサーBMX055とカルマンフィルタを用いた角度の推定」を行ってみましょう。 書籍でいうと、p.41 から始まる「第3節 傾斜計のソフトウェア開発」および p.49 の Inclinometer.cpp の動作の部分なのでした。まず、9軸センサーBMX055 をはんだ付けしなければなりません。電源と信号レベルがともに 3.3V なので、説明書にあるように JP7 の部分にはんだを盛り、VCC と 3.3V の両方に電源を接続するようにします。
ブレッドボード上で作成する回路は以下の通りです。
そうすると、「カルマンフィルタを通した角度のデータ」と、「センサから直接計算した角度のデータ」が空白で区切られて 0.1 秒ごとに送られてきます。 Raspberry Pi のターミナルで miniacm エイリアスで minicom を実行してみましょう。
miniacmすると、ターミナル上に角度が表示されるはずです。なお、回路図に記したように、Pico への電源投入時は、USB接続端子が真上を向いた状態（角度 θ=0）で接続する前提となっています。 一度 Pico への USB 接続を切り、向きを合わせた状態で USB 接続するようにしてみましょう。角度 0 付近から表示が始まり、傾けた向きによって正または負の角度が得られるはずです。 倒立振子はこのように角度 0 度付近で動作します。
また、角度をなるべく一定に保ったまま、ブレッドボードを少し激しめに動かしてみましょう（例えばテーブル上でブレッドボードの角度を保ったままブレッドボードをテーブル上で前後に滑らせる、など）。「カルマンフィルタを通した角度のデータ」はあまり変化しないのに対し、「センサから直接計算した角度のデータ」は大きく変動するのがわかるはずです。例えば以下のような出力が得られます。
(中略) 60.476841 60.588188 60.568378 60.581692 59.464199 46.408962 57.363106 60.888397 61.548885 82.362831 64.96154 78.891701 63.586735 44.846802 59.855469 51.911228 59.990036 61.171368 60.115009 60.16819これは、角度60度をなるべく保ちつつ、センサを動かしたときの様子です。カルマンフィルタの出力である左側の数字は 60 度付近の値に保たれていますが、センサから直接求めた角度である右側の数値は、大きく変動しています。これを下図のようにグラフにするとよりはっきりします。5秒間で3回センサを激しく動かしたときの様子です。
さて、このカルマンフィルタによる角度の推定ですが、計算にどれくらいの時間がかかるのでしょうか。カルマンフィルタの計算は浮動小数演算を用いた行列計算を多用するので、計算コストは高いはずです。
オリジナルの Inclinometer.cpp を見ると、関数「void update_theta()」の冒頭に「//It takes 650 usec. (NUCLEO-F401RE 84MHz, BMX055)」とコメントが書いてあります。
一方、私の Raspbery Pi Pico バージョンで計測を行ってみたところ、およそ 800 μs でした。
この計測方法ですが、以下の二つの方法を思いつきます。
- オシロスコープを持っている場合：update_theta の冒頭でどこかの GPIO を 1 にし、update_theta 終了時にその GPIO を 0 にする。その信号をオシロスコープで観測する。プログラムの修正が最低限で済むのがメリット
- オシロスコープを持っていない場合：update_theta を多数回繰り返すプログラムを書き、開始時と終了時の時刻の差を printf する。800μs だと10,000 回で 8 秒くらいになる。計測専用のプログラムを書かなければならないのがやや面倒
「Raspberry Pi Pico FPU」で検索すると、「Interface 2021年8月号」の宮田賢一さんによる記事「MicroPython×Picoの実力検証」がヒットします。無料で読める1ページ目に「FPUを持つCortex-M4系ボードとPicoでは約1.5倍の差が出ました」と書いてあり、おおむねその記事を反映した結果と言えると思います。
カルマンフィルタを仕事などで本格的に使う方ならば、FPU 搭載のボードを選ぶべきなのでしょう。それはそれとして受け入れた上で、本ページでは Raspberry Pi Pico での倒立振子の実現を目指します。
余談ですが、C言語で 800μsかかったカルマンフィルタの計算を MicroPython で実装した場合、 4.2ms と 5 倍くらいの時間がかかりました。それに加え、I2C 経由でのセンサの値の取得が時折ランダムにエラーを返す問題があり、Python の利用を断念しました。
ちなみに、C 言語におけるカルマンフィルタの計算のスケジュールを図示すると下図のようになります。
const float theta_update_freq = 400; //Hz const float theta_update_interval = 1.0/theta_update_freq;2.5ms から 0.8ms を引いた 1.7ms の時間で残りの制御などを行わなければいけないわけですが、若干スケジュールが窮屈な気がしますね。その部分がどうなるか気にしつつ、先に進みましょう。
7. 車体の組み立て
さて、カルマンフィルタを通した角度の取得に成功したら、いよいよ車体の作成です。ここから先は Pico の使い方が大きく変わりますので、車体作成前に注意しておきましょう。ここまでは、Pico への電源を USB 端子により供給してきました。しかし、ここからは Pico 上の USB 端子を用いません。Pico への電力は、VSYS 端子へ単三充電池3本の出力を加えることで供給します。
また、倒立振子が動作しているときは USB 端子を用いませんから、USB 端子を通して Raspberry Pi とシリアル通信することはできません。上の動画のように倒立振子が正常動作しているときはシリアル通信は不要ですからそれで大きな問題はないのですが、デバッグ時などに Raspberry Pi 上で Pico からの出力を読みたいときがあるかもしれません。そのような場合は、Pico の UART0 TX と UART0 RX のピンを用いてシリアル通信をすることになります。通信相手としては「FTDI USBシリアル変換アダプター Rev.2」を 3.3V モードで用いるのが簡単でしょう。「RX←TX」、「TX→RX」、「GND-GND」の組み合わせからなる3本で Pico とシリアル変換アダプタを接続し、シリアル変換アダプタと USB 接続した Raspberry Pi で miniusb エイリアスにより minicom を起動し、/dev/ttyUSB0 と通信すれば良いのです。この辺りはオプションであり必須ではありません。
以上の注意を踏まえて車体を作成していきます。
足回り
まず、書籍に記されている通りハイパワーギアーボックス HE は、ギア比 64.8:1 で2つ作成することになります。いもねじによるシャフトの固定位置は、車体への取り付けや、ロータリーエンコーダの取り付けの際に変わる可能性がありますので、位置が確定するまではいもねじは軽めに締めておきましょう。ギアーボックスが完成した後、モーターの端子を橋渡しするように 0.1μF のコンデンサをはんだ付けする必要があります。これは。書籍図14の回路図において、Mで表されたモーターの近くに配置されているコンデンサのことです。
少しわかりにくいですが、下図に水色のコンデンサが見えますね。
ロータリーエンコーダ
ロータリーエンコーダの取り付けは、@Kosuke_Matsui(松井 耕介)さんによるqiitaの記事「トランジスタ技術７月号「倒立振子」の制作記録」を参考にしました。ギアボックスの製作過程も写真が豊富ですので参考になると思います。私がカップリングとロータリーエンコーダを固定した様子を示したのが下図です。ロータリーエンコーダはABS樹脂板から浮いているので、タミヤのユニバーサルアームの切れ端と、1mm程度の厚さの両面テープを挟み、ABS樹脂板に穴をあけたうえで「ねじりっこ」で固定しています。雑な工作ですが、一応は機能しています。
回路の作成
回路の作成は、書籍 p.54 の図14とほぼ同じです。NUCLEO-F401RE を Raspberry Pi Pico に置き換えるわけですから、その違いの部分のみを図示すると下図のようになります。
回路についてもう一点注意すべきなのは、モータードライバのピンの形状がやや特殊なので、ピンソケットと接触が悪いことがまれにある、ということです。
回路を完成させた後、片方のタイヤが全く回転せず、接続が悪いのか、パーツが悪いのか、パーツの配置が悪いのかなど、はんだをつけたり外したり何時間も試行錯誤しました。 結局、モータードライバとピンソケットの接触が悪かったことが原因とわかり、ピンソケットを取り外し、新たなピンソケットを切り出してまたはんだ付けし直して解決しました。 このあたりの試行錯誤では「はんだ吸い取り線」が必須でした。
車体の作成
最終的に全ての部品を車体に取り付けた様子が下図です。
このように重さや重心位置が変わると、倒立振子の微分方程式が変わり、それに伴い状態方程式やフィードバックゲインも変わってきます（そもそもタミヤのユニバーサルプレートではなくABS樹脂板を使っている時点で大きな変化です）。その変化をプログラムに反映させるためのツールも書籍で用意されており、それが SolveRiccaciEquation.py です。その使い方は後述します。
さらに、重さや重心位置が多少違ってもプログラムは動作することが多いと思います。
ですので、回路や電池ボックスの取り付け位置についてはそれほど神経質になる必要はありません。
以下は回路が完成してあと一息、といった状況の様子です。まだモーターの配線を行っていない段階ですね。動くかどうか、ドキドキです。
8. 倒立振子の起動
さて、倒立振子が完成したら、Pico にプログラムを書きこんで動作させてみましょう。「5. 倒立振子用プログラムの準備」で行ったビルドにより ~/pico-inverted-pendulum/KalmanFinal/build ディレクトリに Inverted_Pendulum_Kalman.uf2 ができていますので、これを書き込みます。ソースを入手できない場合は ~/pico-inverted-pendulum/Binaries ディレクトリにあるものも利用できます。 その際に注意がいくつかあります。まず、Pico を USB 接続するまえに、充電池による電源を切っておきましょう。電源のソケットをピンから抜いてしまうのが確実です。
なお、これは環境によるのかもしれませんが、私の場合 Pico を基板にとりつけたままだと、BOOTSEL を押しながら Raspberry Pi に差しても認識しないという問題がありました。これは USB に流れる電流量が不足しているからかもしれません。
そのような問題があったので、私は Pico を基板から抜いて Raspberry Pi に取り付けるようにしました。なお、Pico を基板に深く差していると抜くのはなかなか難しいです。ピンソケットと Pico の隙間にマイナスドライバを差し込み、隙間を広げながら少しずつ抜くようにしました。力任せに抜こうとするとせっかく作った基板を破損する恐れがありますので注意しましょう。ですから、Pico を何度も抜き差しする可能性があるときは、基板にあまり深く差し込まない、という注意をするとよいでしょう。
また、Raspberry Pi に対してではなく、Windows マシンに接続すると、Pico を基板に差したままでも認識されることがありました。その場合、ビルドによりできた uf2 ファイルをあらかじめ Windows に移動しておいて、Windows から Pico にコピーする、というのも手です（それはそれで面倒ですが）。
いずれにせよ、Pico に uf2 ファイルをコピーすると再起動がかかり、倒立振子の回路が動作し始めます。しかし、USB から電源が供給されている状況ではモーターに電力が供給されませんので倒立振子は動きません。USB から Pico を切り離し、改めて充電池を接続して動作を開始させましょう。
ブレッドボードで行ったように、電源投入時は倒立振子を直立させた状態、すなわち θ=0 付近の状態を維持します。黄色のLED が点灯している間は、様々な初期化が行われていますので、可能な限り θ=0 を維持するようにしましょう。黄色のLED が消えると倒立振子は動作を開始します。タイヤの回転の向きによって緑と赤のLEDのどちらかが点灯し、倒立振子の安定性が維持されます。
電源投入時が最も安定性が崩れやすいタイミングであり、操作にはある程度の慣れが必要です。何度も練習して慣れてみましょう。
うまく動作しないという場合は。回路などの見直しをすることになります。
9. 倒立振子のパラメータの変更
さて、私と似たパーツを使って倒立振子を作る限り、プログラムを変更する必要はあまりないはずです。基板や電池ボックスの取り付け位置が多少違っても、プログラムはおおむね動作するのではないかと思います。 ですが、参考のためにパラメータ（重さや重心位置など）を変更する方法を記しておきます。pico-inverted-pendulum/KalmanFinal ディレクトリに SolveRiccatiEquation.py という Python プログラムがあります。 これは、倒立振子のパラメータから倒立振子の状態方程式やフィードバッゲインを計算してくれるものです。
テキストエディタ mousepad や Python 開発環境 Thonny などでこのファイルを開いてみてみると、オリジナルのファイルから私が変更した部分がいくつかみつかります。 以下のような部分です。
# タミヤユニバーサルプレートから ABS 樹脂板に変えたので重さが変わった部分 #mass (kg) #m_plate = 0.080 /2 m_plate = 0.108 /2 # シャフトからバッテリーの重心位置が 6.5cm から 8.5cm に変わった部分 #The length between the center of gravity and the axis (m) #d_battery = 0.065 d_battery = 0.085 # 基板の重さ、大きさ、シャフトからの距離が変わった部分 #mass (kg) #m_circuit = 0.100 /2 m_circuit = 0.0406 /2 #length (m) x_circuit = 0.010 #y_circuit = 0.095 y_circuit = 0.070 #The length between the center of gravity and the axis (m) #d_circuit = 0.140 d_circuit = 0.165それ以外では下記の部分も重要です。
ここはもともとのプログラムでは T=0.01 と記されており、モーターへの制御が 10ms ごとに行われることを決めている部分です。
しかし、後述するように Pico では速度がぎりぎり追いつかないので、モーターへの制御を14ms ごとに変更しています。
この変更により、状態方程式も影響を受けます。
#sampling rate of the discrete time system T = 0.014 #sec以上の中身を確認したうえで、SolveRiccatiEquation.py を実行してみましょう。その前に必要なライブラリを以下のコマンドでインストールしておく必要があります。
sudo apt update sudo apt install python3-numpy python3-matplotlib sudo pip3 install control --break-system-packagesインストールを終えたら実行してみましょう。pico-inverted-pendulum/KalmanFinal ディレクトリで以下を実行します。
python3 SolveRiccatiEquation.py書籍 p.68 図27 に類似したシミュレーション結果のグラフが現れますが、ここで重要なのはグラフではなく、コンソールに表示された下記の部分です。
（中略） sampling rate = 0.014 sec matrix Ax (discrete time) [[ 1.00447563e+00 1.40208911e-02 0.00000000e+00 9.73784303e-05] [ 6.39181504e-01 1.00447563e+00 0.00000000e+00 1.37291716e-02] [ -2.00253350e-03 -9.40752945e-06 1.00000000e+00 1.34265323e-02] [ -2.82332812e-01 -2.00253350e-03 0.00000000e+00 9.19205414e-01]] matrix Bx (discrete time) [[-0.00062615] [-0.08827914] [ 0.00368742] [ 0.51951251]] （中略） Gain (calculated) [[ 28.38403323 4.29034848 0.09009136 0.36309152]]これらの数値を、制御プログラム Inverted_Pendulum_Kalman.cpp に反映させねばなりません。なお、上で見た重さや重心位置などの数値を変えていない場合は、 出力された数値は既に私が反映済です。 Inverted_Pendulum_Kalman.cpp を開けば、下記のような部分がみつかるはずです。
（中略）
float A_x[4][4] = {
{1.00447563e+00, 1.40208911e-02, 0.00000000e+00, 9.73784303e-05},
{6.39181504e-01, 1.00447563e+00, 0.00000000e+00, 1.37291716e-02},
{-2.00253350e-03, -9.40752945e-06, 1.00000000e+00, 1.34265323e-02},
{-2.82332812e-01, -2.00253350e-03, 0.00000000e+00, 9.19205414e-01}
};
（中略）
float B_x[4][1] = {
{-0.00062615},
{-0.08827914},
{0.00368742},
{0.51951251}
};
（中略）
float Gain[4] = {28.38403323, 4.29034848, 0.09009136, 0.36309152};
皆さんも必要に応じてここを書き換えればよいわけです。なお、書き換えたら、pico-inverted-pendulum/KalmanFinal/build ディレクトリに移動して make コマンドを実行すれば、uf2 ファイルが更新されます。
10. 倒立振子の制御のタイムスケジュール
さて、倒立振子の制御のためには、以下の3つが定期的に実行される必要があり、書籍では以下の周期と実現方法になっていました。| タスク | 周期 | 書籍での実現方法 | Pico での実現方法 |
| ロータリーエンコーダの読み取り | 25 μs | タイマ1 | 2つ目のCPUコアで while + sleep |
| カルマンフィルタによる角度の推定 | 2.5 ms | タイマ2 | タイマ |
| モータへの制御 | 10 ms | while文 + sleep | 1つ目のCPUコアで while + sleep |
これを Pico に移植する場合、まずタイマが1つしかないのをどうするのかが問題でした。
上記のタスクのうち、「カルマンフィルタによる角度の推定」と「モーターへの制御」は時間に対して正確に実行しなければなりません。 なぜかというと、カルマンフィルタの計算式や状態方程式に時間間隔 Δt が入っているため、その Δt の通りに計算を実行しなければならないからです。
一方、ロータリーエンコーダの読み取りはそれほど時間に正確である必要はありません。「値の変化を取りこぼさない程度に速い」という条件が満たされれば良いのです。
以上を踏まえ、上記のように2つのCPUコアに計算を割り振ることにしました。
そうすると、1つ目のCPUコアでカルマンフィルタによる角度の推定とモーターへの制御の2つのタスクをうまくこなさねばなりません。
結論から言うと、制御周期 10ms は Pico にとっては速すぎました。オシロスコープの出力から作った下記のグラフをご覧ください。
そのようなわけで、もう少し時間に余裕を持たせた制御の方が良いだろうと考え、制御の周期を 14ms、カルマンフィルタによる角度の推定はその 1/4 の周期 3.5ms にしてみました。実際に制御時にオシロスコープの出力から作成したグラフが下図です。
実際には、10ms 周期でも 14ms 周期でも倒立振子は安定させられるのですが、心持ち 14ms 周期の方が安定してるかも？という気がします。
以上の内容は、プログラム中の下記の部分の変更により実現しています。
//const float theta_update_freq = 400; //Hz (Not Used) const float theta_update_interval = 0.0035; (中略） float feedback_interval_sec = 0.014; //sec float feedback_interval_sec_wait = 0.0122; //sec最後の feedback_interval_sec_wait の 0.0122 という数値は、制御を行う while ループの sleep に使われるのですが、適切な値はオシロスコープで上のような波形を見ながら調節するしかないのが厄介でした。
11. おわりに
いかがでしたか？倒立振子の作成にも苦労しましたが、ブログを書くのも非常に大変でした。
「とりあえず動く」という状態ならば割と早いうちから実現できていました。実際、書籍のパラメータのままでも、なんとか倒立振子を安定させることができるほどです。
しかし、ブログを書くために細かく調べていくと、カルマンフィルタや制御が一定周期で動いていないなど問題がたくさん見つかり、それを一つ一つ潰していかなければならない、といった具合です。
問題をつぶしていくと、少しずつではありますが、制御の安定性が増していくのが可愛いところです。
そんな感じに苦労しただけあって、完成した倒立振子へはかなりの愛着がわいています。特に必要もないのに毎日ちょくちょく電源を入れて倒立させています。
皆さんも一家に一台、倒立振子を作成してみてはいかがでしょうか。
