2024年2月26日月曜日

Raspberry Pi Pico で倒立振子を制御してみた

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 H1ピンヘッダ取り付け済の H が良いでしょう。無線機能は不要なので、W や WH を選ぶ必要はありません
加速度センサ関連BMX055使用9軸センサーモジュール1-
ICソケット ( 6P)1-
ロータリーエンコーダ関連岩通マニュファクチャリング EC202A100A ロータリーエンコーダ1買うのに覚悟が必要な価格です
岩通マニュファクチャリング A150 EC202用ハーネス1-
4Pカップリング ボリュームシャフト中継用ジョイント 4P1ロータリーエンコーダとシャフトの結合に用います。シャフト側にはM3ナットをかませておきます(参考)。
タイヤ関連タミヤ 72003 ハイパワーギヤーボックス HE2-
タミヤ 70111 スポーツタイヤセット1-
モータードライバTA7291P2この入手が問題ですね…
電池関連電池ボックス 単3×3本 リード線・スイッチ付1なんでも良いと思いますが、私が使ったのはこれです。多分、ペンチなどで側面のプラスチックを広げておかないと電池の取り出しが困難です
皿小ねじ(+) 皿ねじ M3×122本このタイプの頭が平らなねじでないと電池と干渉します。私が使ったのはこのねじではないのですが、多分大丈夫なはず(?)
充電池と対応充電器3本私はエネループプロを持っていたのでそれを使いましたが、雑誌ではプロではない通常のエネループを用いていますね
車体関連タミヤ 楽しい工作シリーズ No.172 ユニバーサルプレートL 210×160mm1上述したように、執筆時は入手しにくい状態です。これがない場合、代用として以下の3点を用います
はざいや ABS樹脂板 【住友ベークライト】白 厚さ 3mm サイズ 160×210 mm1「はざいや」さんでタミヤのユニバーサルプレートと同じ寸法を指定して購入します。私が購入したときは一枚339円でした。複数枚買うとお得になります
タミヤ 精密ピンバイスD (0.1~3.2mm)1ABS 樹脂板への穴あけ用の手動のドリルです
タミヤ ベーシックドリル刃セット (1,1.5,2,2.5,3mm)1ピンバイスとセットで用いるドリル刃です。3mmのもののみを使います
抵抗カーボン抵抗 220Ω4LED用に3つ、PWMのローパスフィルタ用に1つ。LED用の抵抗の大きさはこの値でなくても構いません。私はLED用の3つには330Ωを使いました。なお、秋月電子通商だと100本セットでの販売が多いです。千石電商だと10本から購入できますが、値段は上がります
カーボン抵抗 3.3kΩ2ロータリーエンコーダの4.8Vの出力を3.3Vに落とすためのもの
カーボン抵抗 2kΩ2ロータリーエンコーダの4.8Vの出力を3.3Vに落とすためのもの
コンデンサセラミックコンデンサー 0.1μF5モータ用2つ、モータードライバ用2つ、BMX055用1つ。10個パックでちょうど良いと思います
セラミックコンデンサー 2.2μF1PWMのローパスフィルタ用に1つ。10個パックへのリンクを張っていますが、単品売りのほうで良いかも
電解コンデンサー 220μF2モータードライバ用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
この中には3つのディレクトリがあります。
  • KalmanAngle: Inclinometor.cpp をビルドするためのディレクトリ
  • KalmanFinal: Inverted_Pendulum_Kalman.cpp をビルドするためのディレクトリ
  • Binaries: ビルド済ファイル Inclinometor.uf2 と Inverted_Pendulum_Kalman.uf2 を格納したディレクトリ
ここから先は、Inclinometor.cpp と Inverted_Pendulum_Kalman.cpp をRaspberry Pi Pico 向けにビルドする方法を記していきます。ソースコードを入手できない方は、Binarie フォルダに格納されたビルド済ファイルを試すこともできます。

さて、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 の両方に電源を接続するようにします。

ブレッドボード上で作成する回路は以下の通りです。
回路が組めたら、先ほどビルドして得られた pico-inverted-pendulum/KalmanAngle/build/Inclinometer.uf2 を Pico にコピーしましょう。ソースを入手できない場合は ~/pico-inverted-pendulum/Binaries ディレクトリにあるものも利用できます。 BOOTSELボタンを押しながら Pico を USB 経由で Raspberry Pi に接続し、ファイルマネージャーでファイルをコピーすれば良いのでしたね。コピーが終わると Pico が再起動し、プログラムが動き始めています。

そうすると、「カルマンフィルタを通した角度のデータ」と、「センサから直接計算した角度のデータ」が空白で区切られて 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 ではオリジナルの NUCLEO-F401RE に比べて動作が遅くなってしまいました。Raspberry Pi Pico のクロック周波数は 125MHz で NUCLEO-F401RE の 84MHz よりも速いのになぜだろうと一瞬考えてしまいます。これは、 NUCLEO-F401RE に搭載されている MPU STM32F401RE には浮動小数点ユニット (FPU) が搭載されているのに対し、Raspberry Pi Pico に搭載されている RP2040 にはそれがないからだと考えられます。

「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 言語におけるカルマンフィルタの計算のスケジュールを図示すると下図のようになります。
カルマンフィルタによる角度の計算が2.5msごと、すなわち 400Hz の周波数で行われるのは、プログラム中の下記の部分によります。実際にプログラム中で使われるのは周期にした theta_update_interval の方です。この値がタイマに渡され、2.5ms ごとの処理を実現しています。
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で表されたモーターの近くに配置されているコンデンサのことです。
少しわかりにくいですが、下図に水色のコンデンサが見えますね。
さて、モーターにコンデンサを取り付けたハイパワーギアーボックス HE は、タミヤのユニバーサルプレートにねじで固定しますが、ABS 樹脂板を購入した場合は自分で穴を空ける必要があります。雑誌でのユニバーサルプレートの利用方法に合わせるなら、下図のように 3mm の穴を4つピンバイスで開けることになるでしょう。
以上のようにして、ABS樹脂板にハイパワーギアーボックス HE を固定した様子が下図になります。この辺りはまだまだ楽しく作業ができる段階です。

ロータリーエンコーダ

ロータリーエンコーダの取り付けは、@Kosuke_Matsui(松井 耕介)さんによるqiitaの記事「トランジスタ技術7月号「倒立振子」の制作記録」を参考にしました。ギアボックスの製作過程も写真が豊富ですので参考になると思います。

私がカップリングとロータリーエンコーダを固定した様子を示したのが下図です。ロータリーエンコーダはABS樹脂板から浮いているので、タミヤのユニバーサルアームの切れ端と、1mm程度の厚さの両面テープを挟み、ABS樹脂板に穴をあけたうえで「ねじりっこ」で固定しています。雑な工作ですが、一応は機能しています。

回路の作成

回路の作成は、書籍 p.54 の図14とほぼ同じです。NUCLEO-F401RE を Raspberry Pi Pico に置き換えるわけですから、その違いの部分のみを図示すると下図のようになります。
回路をユニバーサル基板上に実現した様子が下図です。ピンヘッダやピンソケットを用いてパーツやケーブルを着脱可能にしています。なお、モータードライバの OUT1、OUT2 の信号ですが、OUT1 にはモーターの青のケーブル、OUT2 にはモーターの赤のケーブルを接続します。
はんだ付けに関しては根気よく行うしかありません。注意すべき点は、はんだを付けたいパーツを十分熱することでしょうか。

回路についてもう一点注意すべきなのは、モータードライバのピンの形状がやや特殊なので、ピンソケットと接触が悪いことがまれにある、ということです。
回路を完成させた後、片方のタイヤが全く回転せず、接続が悪いのか、パーツが悪いのか、パーツの配置が悪いのかなど、はんだをつけたり外したり何時間も試行錯誤しました。 結局、モータードライバとピンソケットの接触が悪かったことが原因とわかり、ピンソケットを取り外し、新たなピンソケットを切り出してまたはんだ付けし直して解決しました。 このあたりの試行錯誤では「はんだ吸い取り線」が必須でした。

車体の作成

最終的に全ての部品を車体に取り付けた様子が下図です。
ギアボックスを取り付ける際は、書籍に合わせようと慎重に行いましたが、このあたりになると力尽きていたので、「この辺かな?」と思った位置にサインペンで印をつけ、すぐに穴をあけて取り付けてしまいました。図中に記したように、書籍と比べると重心が上に来るように取り付けてしまったようです。

このように重さや重心位置が変わると、倒立振子の微分方程式が変わり、それに伴い状態方程式やフィードバックゲインも変わってきます(そもそもタミヤのユニバーサルプレートではなく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タイマ12つ目のCPUコアで while + sleep
カルマンフィルタによる角度の推定2.5 msタイマ2タイマ
モータへの制御10 mswhile文 + sleep1つ目のCPUコアで while + sleep

これを Pico に移植する場合、まずタイマが1つしかないのをどうするのかが問題でした。

上記のタスクのうち、「カルマンフィルタによる角度の推定」と「モーターへの制御」は時間に対して正確に実行しなければなりません。 なぜかというと、カルマンフィルタの計算式や状態方程式に時間間隔 Δt が入っているため、その Δt の通りに計算を実行しなければならないからです。
一方、ロータリーエンコーダの読み取りはそれほど時間に正確である必要はありません。「値の変化を取りこぼさない程度に速い」という条件が満たされれば良いのです。

以上を踏まえ、上記のように2つのCPUコアに計算を割り振ることにしました。

そうすると、1つ目のCPUコアでカルマンフィルタによる角度の推定とモーターへの制御の2つのタスクをうまくこなさねばなりません。

結論から言うと、制御周期 10ms は Pico にとっては速すぎました。オシロスコープの出力から作った下記のグラフをご覧ください。
カルマンフィルタによる角度の推定が 2.5ms で行われており、その合間に 4 倍の 10ms 周期で制御が行われるのですが、 角度の推定を行っていない時間に対して、制御にかかる時間がギリギリ収まる程度で長すぎるのです。このグラフでは問題なさそうに見えるのですが、モーターを実際に回すと、このグラフは大きく乱れてしまいます。

そのようなわけで、もう少し時間に余裕を持たせた制御の方が良いだろうと考え、制御の周期を 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. おわりに

いかがでしたか?

倒立振子の作成にも苦労しましたが、ブログを書くのも非常に大変でした。

「とりあえず動く」という状態ならば割と早いうちから実現できていました。実際、書籍のパラメータのままでも、なんとか倒立振子を安定させることができるほどです。

しかし、ブログを書くために細かく調べていくと、カルマンフィルタや制御が一定周期で動いていないなど問題がたくさん見つかり、それを一つ一つ潰していかなければならない、といった具合です。
問題をつぶしていくと、少しずつではありますが、制御の安定性が増していくのが可愛いところです。

そんな感じに苦労しただけあって、完成した倒立振子へはかなりの愛着がわいています。特に必要もないのに毎日ちょくちょく電源を入れて倒立させています。
皆さんも一家に一台、倒立振子を作成してみてはいかがでしょうか。

2021年2月18日木曜日

Raspberry Pi 4をSSDから起動しよう

0. はじめに

Raspberry Pi は microSDカード(旧タイプはSDメモリーカード)にOSをインストールして起動するのが一般的です。その場合、microSDカードへのアクセス速度の遅さから、OSが一瞬止まったような動作をすることが時々あります。

それを解消するためにはいくつかの方法があります。例えば、2020年10月に発表されたRaspberry Pi 4 Compute ModuleにはeMMCというストレージが搭載されているモデルがあります。eMMCにOSをインストールすることで、microSDカードを用いる場合よりも快適に動作することが期待されます。

しかし2021年2月現在、Raspberry Pi 4 Compute Moduleは日本ではまだ発売されておりませんし、発売されていたとしても、別途I/Oボードが必要になるなど、通常のRaspberry Pi 4よりも利用する際の敷居は高くなるでしょう。

一方、Raspberry Pi 4 では、microSDカードからではなくUSBデバイスからOSを起動することが容易になっており、USB接続のSSDにOSをインストールすることで快適にRaspberry Piを利用できるようになります。この方法は、以前は64ビット版のベータ版OSでの検証記事が多かったのですが、今回32ビット版の公式OSでも問題なく実現できたので、記事にまとめることにしました。

以下の2つの方法を紹介します。
  • [方法1] Buffalo のUSBスティックタイプのSSD (SSD-PUT250U3) を用いる方法
  • [方法2] M.2 SSDを利用可能にするケース Argon One M2 を用いる方法
下図は[方法1]に従い、Raspberry Pi 4 をBuffalo のUSBスティックタイプのSSD (SSD-PUT250U3) で利用している様子です。
以下で解説していきます。

1. Raspberry Pi 4 に対する準備

上の[方法1]、[方法2]のどちらの方法をとるにせよ、Raspberry Pi 4であらかじめ以下の2点の準備をしておく必要があります。
  • Raspberry Pi 4 のファームウェアを最新にしておく
  • Raspberry Pi 4 の起動の優先順序を変更しておく
これらの設定を行うためには、あらかじめ Raspberry Pi 4 を microSD カードにインストールしたOSで起動できるようにしておく必要があります。このページを見るような方には不要かもしれませんが、microSDカードへのOSのインストール方法を解説したページへのリンクを張っておきます。2020年3月より、Raspberry Pi Imagerというソフトウェアを用いてインストールする方法が主流となっておりますので、そちらに慣れておくことをお勧めします。

以下の内容は2021年1月版のOSで検証しました。

さて、microSDカードから起動したOSで、まずRaspberry Pi 4のファームウェアを最新にしましょう。 現在インストールされているファームウェアを確認するには、ターミナルで以下のコマンドを実行します。
sudo rpi-eeprom-update
すると、例えば以下のような表示が現れます。
CURRENT と表示されているのが現在インストールされているファームウェア、LATESTと表示されいてるのがアップデート可能な最新のファームウェアです。上図ではCURRENTとLATESTが一致していることがわかります。 本ページの内容を確認するには、少なくともCURRENTが2021年1月以降のファームウェアである必要があります。

Raspberry Pi 4のファームウェアをアップグレードしたい場合、以下の3コマンドを順に実行することで、OS自体を最新にする方法が公式で紹介されています。
sudo apt update
sudo apt full-upgrade
sudo reboot
もし、OS全体の更新ではなく、ファームウェアだけの更新を行いたい場合、以下の2つのコマンドを順に実行すると良いようです。
sudo rpi-eeprom-update -a
sudo reboot
以上の操作により、Raspberry Pi 4 のファームウェアが最新になりました。次に、Raspberry Pi 4 の起動の優先順序を変更します。

ターミナルで以下のコマンドを実行し、raspi-config を起動します。
sudo raspi-config
raspi-config は、初めて使う方にとっては操作に癖のあるツールですので、以下の手順に丁寧に従っていきましょう。

まず、raspi-configを起動すると以下のような画面になります。ここで、「↓」キーを5回押し、「6 Advanced Options」の項目にフォーカスを合わせます。
下図のようになりますので、ここでEnterキーを押し、Advanced Options の項目に入ります。
Advanced Optionsの項目で「↓」キーを5回押し、「A6 Boot Order」の項目にフォーカスを合わせ、Enterキーを押します。
すると、起動順序として下図の3つの選択肢が現れます。
ここで選んでよいのは、
  • B1 SD Card Boot(まずSDカードからの起動を試し、失敗したらUSBからの起動を試す)
  • B2 USB Boot(まずUSBからの起動を試し、失敗したらSDカードからの起動を試す)
のどちらかです。USB端子にSSDが接続されており、microSDカードが差さっていない状況なら、どちらを選んでもUSB接続のSSDからOSが起動するようになります。
(上記2つの選択肢で違いが現れるのは、USB接続のSSDとmicroSDカードの両方を同時に接続してRaspberry Pi 4を起動した場合です)

「B1 SD Card Boot」を選ぶ場合は何も押さず、「B2 USB Boot」を選ぶ場合はキーボードの「↓」キーを一回押してから、Enterキーを押して下さい。

なお、古いバージョンのraspi-configでは「SD Card Boot」に相当する項目がありませんので、必然的に「USB Boot」に相当する項目を選ぶことになります。

例えば「B1 SD Card Boot」を選んでEnterキーを押した場合は以下の表示が現れますので、Enterキーを押します。
すると、raspi-configの最初の画面に戻りますので、TABキーを二回押し、「Finish」に項目を合わせてからEnterキーを押します。
すると、下図のように再起動を促されますので、Enterキーを押して再起動しましょう。
以上で Raspberry Pi 4に対する準備は終了です。再起動が終わったら、Raspberry Pi 4の電源を一旦切りましょう。

2. Buffalo のUSBスティックタイプのSSD (SSD-PUT250U3) を用いて Raspberry Pi 4 を起動する

ここからは、Buffalo のUSBスティックタイプのSSD (SSD-PUT250U3) を用いて Raspberry Pi 4 を起動する方法を解説していきます。

といっても、こちらは非常に簡単です。microSDカードへのOSのインストール方法にて、Raspberry Pi Imagerを用いたOSのインストール方法を解説しています。その方法を用いて、Raspberry Pi Imager でUSBスティックタイプのSSD (SSD-PUT250U3)に対してOSを書き込むだけです。なお、OSは「Raspberry Pi OS (other)」の「Raspberry Pi OS Full (32-bit)」を選択しました。

OSのインストールが完了した SSD を接続する際、以下のことに注意します。
  • Raspberry Pi 4 のUSB端子のうち、青い色のUSB3対応の端子にSSDを接続する
  • Boot Order(起動順序)で「SD Card Boot」を選択した方は、microSDカードをRaspberry Pi 4から取り外す
  • Boot Order(起動順序)で「USB Boot」を選択した方は、microSDカードが接続されていても、SSDからRaspberry Pi 4が起動する
さて、以上の解説でUSB接続のSSDからOSは起動したでしょうか?
起動した後は通常のRaspberry Pi OSと変わらないので難しいことはありません。私が使った上での感想をいくつか述べます。

[長所]

microSDカードに比べるとアクセスが速いので快適に感じるはずです。ただし、「通常のPCと同程度の速度になるのでは?」などと期待しすぎると「大したことないじゃん」と感じるかもしれません。この辺りの感じ方には個人差があるでしょう。 なお、逆にSSD起動のOSに慣れてからmicroSDカード起動にOSに戻るとかなり動作の遅さを実感できます。特に、ディスクアクセスの多いOSの更新などでは大きな違いを実感します。まとめると、劇的というほどではないけれど堅実な速度向上、といったところでしょうか。

[短所]

これはBuffaloのSSD-PUT250U3特有の問題なのですが、小型とは言え通常のUSBメモリに比べると大きいので、USB端子部で他のUSBデバイスと物理的に干渉します。具体的には、マウスをUSB端子に差そうとするとSSDとぶつかるという問題が起こりました。ギリギリ差せるので大きな問題にはなりませんが、気になる人は気になるでしょう。

あと、これは私が所持しているマウスデバイスの問題かもしれないのですが、無線2.4GHzの無線マウスを使うとマウスポインタの動きが滑らかではなくなる、という問題が起こりました。

結局、マウスに関する上記2点の問題を解決するため、私は Bluetooth 3 のマウスとキーボードを購入しました。これ (M-BT15BRSBK)これ (TK-FBP102BK)です。Bluetooth 4 (BLE) のマウスやキーボードはデフォルトではRaspberry Pi に接続できないはずですので注意してください。 また、ペアリング時は有線または無線2.4GHzのマウスが必須となりますので、購入する場合はそれをわかった上で購入しましょう。

3. M.2 SSDを利用可能にするケース Argon One M2 を用いて Raspberry Pi 4 を起動する

次に、もう一つの Argon One M2 というケースを用いる方法を紹介します。これは、2020年末のITmediaの記事で紹介されていたもので、今回私がSSDを試してみたいと思ったのも、この記事がきっかけでした。

実際に使用している様子が下図です。GPIO部が開けられるようになっており、電子工作にも向いているのが特長です。他にも、microHDMI端子がHDMI端子に変換されている点もメリットといえるかもしれません。 デザインがもう少し洗練されていればよいのになあと個人的には思いますが、これは好みの問題でしょう。なお、このケースにはとんでもない欠点もあるのですが、それは後述します。
Argon One M2を利用するには、M.2 SATA SSD (Key-BまたはKey-B&M) を別途用意する必要があります。私が購入したのは下記の二つです。 組み立ては付属の説明書に従い、OSのインストールは上に記したITmediaの記事に従うと良いでしょう。

組み立て時の注意としては、下図のジャンパが挙げられます。 デフォルトでは図のように1-2ピンをショートするようにジャンパが接続されており、これは「電源接続後にパワーボタンを押す必要がある」ことを意味します。Raspberry Pi のデフォルトの挙動のように「電源を接続すると即電源が入る」状態が良い場合はショートするのを2-3ピンに変更しましょう。
OSインストール時のポイントは、「microSDカードにあらかじめRaspberry Pi OSをインストールして起動し、そのmicroSDカード上のOSをRaspberry Pi上でSD Card CopierというツールでSSDにコピーする」ということです。その際、microSDカードとSSDを同時に差した状態でOSを起動することがあり得ますので、raspi-configの起動順序(Boot Order)の設定は「B2 USB Boot」とするのがよいでしょう。

さて、無事 Argon One M2 を用いてSSDでRaspberry Pi 4 を起動できたでしょうか。起動後はターミナルを起動して以下のコマンドを実行しましょう。ケースファンを制御するプログラムがインストールされます。
curl https://download.argon40.com/argon1.sh | bash
そのプログラムの設定は下記のコマンドで行うようです。これらはこちらのWikiに書いてあります。
argonone-config  #configure driver
argonone-uninstall  #uninstall driver
このようにSSDをケース付きで利用できるようになる Argon One M2 なのですが、大きな欠点があります。

それは、事務机のように金属が埋め込まれた机の上に置いて利用すると、Wifiがほとんどつながらなくなる、ということです。ケースの上半分が金属であり、机も金属だとほぼシールドされるからだと思うのですが、 この仕様を知ったときはさすがに驚きました。大手メーカーの商品ならばあり得ないような設計ミスだと思います。

仕方ないので、私はArgon One M2を下図のように、手元にあったニンテンドーDSのソフトウェアの空きケースの上に置いて使っています。これだけでWifi接続がかなりマシになります。
というわけでこの Argon One M2、個人的にはあまりお勧めできないですね。木やプラスチック製の机の上で使うならば問題ないと思うのですけどね。もちろん、有線LANを使う場合も大丈夫でしょう。

4. おわりに

以下の2つの方法でRaspberry Pi 4をSSDから起動する方法を紹介しました。価格の面でも難易度の面でも、USBスティックタイプのSSDの方がお勧めです。興味のある方は試してみてはどうでしょうか。
  • [方法1] Buffalo のUSBスティックタイプのSSD (SSD-PUT250U3) を用いる方法
  • [方法2] M.2 SSDを利用可能にするケース Argon One M2 を用いる方法



「ラズパイ4対応 カラー図解 最新 Raspberry Piで学ぶ電子工作」、「実例で学ぶRaspberry Pi電子工作」、「Raspberry Piではじめる機械学習」を執筆しました。

2020年6月24日水曜日

Intel RealSenseでSkeleton Trackingを行うCubemosのライブラリをPythonで使ってみた

はじめに

(2022.11追記:調べたら、Skeleton Tracking SDK は既に利用できなくなっているようですね。Cubemos 社と Intel 社の契約が切れたのでしょうか。そのため、RealSense と骨格認識を組み合わせるには、mediapipe などを使うのが良いと思います)

距離を計測可能な深度カメラとして、Intel RealSense D415 および D435が良く知られています。

RealSense をロボットなどに搭載すれば、映像を取得できるだけではなく対象物までの距離も取得できるようになるというわけです。しかし、その「対象物」の位置自体はなんらかの方法で事前に見つけておく必要があります。例えば、人物までの距離を測定したい場合、人物の位置をあらかじめ知る必要があるのです。

RealSense を用いて人物の骨格の位置を得る方法として、Raspberry Pi と Coral USB Accelerator を用い、ディープラーニングの PoseNet を用いる方法を別サイトで紹介しました。
この方法により、RealSense を用いて人物の姿勢とその距離を得られるようになったのですが、この方法は Windows などの一般的な PC では速度が非常に遅いという難点がありました(そのため、上記ページではWindows 用プログラムは公開しませんでした)。

そこで、本ページでは、RealSense を用いて人物の骨格の位置を得る方法として、公式サイトで紹介されている Cubemos 社による Skeleton Tracking SDK を用いる方法を紹介します。これにより、Windows などの一般的な PC でも RealSense を用いて人物の骨格の位置を高速に得ることができます。

なお、Cubemos 社による Skeleton Tracking SDK は 75ドルの商品なのですが、本ページの内容は 30 日間の無料トライアルでも試すことができます。私自身は 30 日間の無料トライアルで本ページの内容を記しましたが、最終的このライブラリを購入する予定です。

実際に RealSense を用いて人物の骨格の位置を得ている様子が下図です。青~赤に色付けされているのは距離に相当し、骨格に相当する黄色の線分で表示されています。


本ページでは、これを Windows 10 + Python (Anaconda) で実現する方法を記します。RealSense は D415 と D435 が使えます。なお、公式によるシステム要件として以下が挙げられて言いますのでご注意ください。
  • CPUs: 6th to 10th generation Intel Core and Xeon Processors
  • GPUs: Intel Iris Pro, Inte HD Graphics 520, 530, 630

Skeleton Tracking SDK のインストール

ここでは、Skeleton Tracking SDK のインストールを行います。本ページではこの SDK を Windows 上の Python で利用することを意図しています。その場合、Visual Studio や RealSense SDK が PC にインストールされている必要はありません。

まず、Skeleton Tracking SDK のページにある「Try for free」ボタンをクリックしましょう。名前やメールアドレスを入力する欄が現れますので、英語で入力して Submit (送信) ボタンをクリックします。少し待つと、30日間の無料トライアル用のライセンスキーと、SDKのダウンロードリンクが書かれたメールが届きますので、まずは SDK をダウンロードしましょう。

ダウンロードした圧縮ファイル cubemos_SDK.zip を右クリックして「すべて展開」を選択することなどにより、展開してください。そして中に含まれている Windows 用のインストーラー(執筆時は cubemos-SkeletonTracking_2.3.1.6cffde4.exe )をダブルクリックし、インストールしましょう。なお、「Microsoft Visual C++ 2017 Redistributable (x64)」も(インストール済でなければ)合わせてインストールされます。

インストールが終わったら、ライセンスの設定を行います。このとき、職場や学校などのプロキシ環境下ではライセンスの設定ができません(Skeleton Tracking自体はプロキシ環境下でも実行できるのですが)。そのため、ライセンスの設定時のみは、PC をスマートフォンのテザリングなどによりインターネットに直接接続する必要があります。

さて、PCをインターネットに直接接続したら、エクスプローラーで「C:\Program Files\Cubemos\SkeletonTracking\scripts」に移動し、中にある「post_installation.bat」をダブルクリックして実行します。すると、以下のようなウインドウが現れますので、先ほど届いたライセンスキーをコピーして貼り付け、「OK」ボタンをクリックすると、ライセンスの登録が始まります。


途中で「Visual Studio 用のファイルを生成するか? (y/n)」というようなメッセージが現れますが(スクリーンショットを取り忘れました)、Python で Skeleton Tracking SDK を使う限り不要なので、「n」を入力して Enter して先に進めます。

最終的にこのプログラムは終了するのですが、ライセンスの登録に成功したかどうかは注意して確認する必要があります。 ライセンス登録のログは「C:\Users\ユーザー名\AppData\Local\Cubemos\SkeletonTracking\logs」フォルダに格納されています。AppData フォルダは隠しフォルダになっているのでエクスプローラーで「隠しフォルダを表示する」設定を行わないと見られませんので注意してください。そのフォルダにあるログファイルをテキストエディタで開いたとき、ライセンスの設定がうまくいっていれば、末尾が下記のようになっているはずです。
(中略)
[CUBEMOS_LOG_20][2020-Jun-19 18:21:11.519998][info]Loaded the cubemos plugin C:\Program Files\Cubemos\SkeletonTracking\bin\cubemos_intel_inference_engine_plugin.dll
[CUBEMOS_LOG_21][2020-Jun-19 18:21:11.520994][debug]Cubemos handle is now valid
しかし、ライセンスの登録に失敗している場合は、その問題を解消しない限り Skeleton Tracking SDK は使えません。 私が遭遇したエラーとしては、まず下記のものがあります。
(中略)
[CUBEMOS_LOG_16][2020-Jun-12 17:24:28.132243][info]The provided activation key has been accepted for creation of the cubemos handle.
[CUBEMOS_LOG_17][2020-Jun-12 17:24:28.203042][error]Exception caught in file C:\Users\sid\Documents\core_binaries\sources_and_scripts\core\modules\engine\src\engine.cpp and line 226 with error message: "boost::dll::shared_library::load() failed: 指定されたモジュールが見つかりません。"
このエラーの原因はよくわからなかったのですが、Skeleton Tracking SDK とともにインストールされる「Microsoft Visual C++ 2017 Redistributable (x64)」との連携がうまくいっていないのかもしれません。私の場合、一度 Skeleton Tracking SDK をアンインストールし、もう一度 Skeleton Tracking SDK をインストールしたらこの問題は解決しました。その際、再インストール時もスマホのテザリングによりPCを直接インターネットに接続したので、それが効いたのかもしれませんが未確認です。

もう一つ遭遇したのは、以下のエラーです。 CPU がサポート外だと言われています。
(中略)
[CUBEMOS_LOG_9][2020-Jun-19 18:06:42.372179][error]Exception thrown in file C:\Users\sid\Documents\core_binaries\sources_and_scripts\core\modules\engine\src\internal_api.cpp and line 79 with error message: "An attempt to activate the cubemos SDK was made on an incompatible CPU (Intel(R) Core(TM) i7-1065G7 CPU @ 1.30GHz) . In order to avoid consuming the license, the process will abort here. Please retry activation on a compatible hardware. . Return code: 6"
[CUBEMOS_LOG_10][2020-Jun-19 18:06:42.372179][error]Hardware information reading failed: An attempt to activate the cubemos SDK was made on an incompatible CPU (Intel(R) Core(TM) i7-1065G7 CPU @ 1.30GHz) . In order to avoid consuming the license, the process will abort here. Please retry activation on a compatible hardware. 
[CUBEMOS_LOG_11][2020-Jun-19 18:06:42.372179][error]Exception thrown in file C:\Users\sid\Documents\core_binaries\sources_and_scripts\core\modules\engine\src\internal_api.cpp and line 134 with error message: "An attempt to activate the cubemos SDK was made on an incompatible CPU (Intel(R) Core(TM) i7-1065G7 CPU @ 1.30GHz) . In order to avoid consuming the license, the process will abort here. Please retry activation on a compatible hardware. . Return code: 1"
古い PC がサポート外と言われるならわかるのですが、私の場合、2020年に購入した Surface Laptop 3 (Core i7-1065G7) でもこのエラーが出ました。サポートに問い合わせたらまだサポート外だそうです。古い PC だけでなく、新しいPC でもサポート外のエラーが出ることがあるのは注意が必要です。

最終的に、私の場合 2016年頃に購入した Surface Pro 4 (Core i7-6650U) でライセンス登録に成功しました。

なお、ライセンスの登録に成功すると、その情報が C:\Users\ユーザー名\AppData\Local\Cubemos フォルダに格納されます。同一マシンの別ユーザーで Skeleton Tracking SDK を使いたい場合(例えば、ライセンス登録するユーザーとプログラム開発をするユーザーが違う場合など)、このフォルダを別ユーザーにコピーすれば問題なく使えます。

Anacondaのインストールと設定

SDKをインストールしたら、それを使うための環境である Anaconda をインストールしましょう。Anaconda は、Python で機械学習を行うためのパッケージを提供するプラットフォームです。Python での機械学習の利用が容易になるので用います。 以下では、Windows 10 に Anaconda をインストールし、含まれている開発環境 Spyder で Python を利用する方針で記します。

その前に、いくつか設定が必要です。まず、下図のような Windows 10 の環境変数を設定するウインドウを表示します。このウインドウの表示方法は、Google などで検索すれば見つかるでしょう。

そして、上側にある「新規」ボタンを押し、 変数名に
CUBEMOS_SKEL_SDK
を、変数値に
C:\Program Files\Cubemos\SkeletonTracking
を入力してOK という変数を新規で作成し、以下の値を保存しましょう。下図の最上部に見えているように「CUBEMOS_SKEL_SDK」というユーザー環境変数が保存されます。


なお、環境変数 CUBEMOS_SKEL_SDK は、SDKのインストール時に「システム環境変数」(上図の下半分の領域)として自動的に登録されています。しかし、本ページで用いる Spyder では「システム環境変数」(下半分)ではなく「ユーザー環境変数」(上半分)しか読み込まないようなので、 「ユーザー環境変数」に新たに追加する、というわけです。

次に、Windows 10に Anaconda をインストールします。 「Anaconda のページ」をページ末尾までスクロールすると、下記のような内容が現れます。


64ビット版 Windows をご利用の場合、Python 3.8 用の「64-Bit Graphical Installer」をクリックします。なお、上図には macOS や Linux 用のリンクも見えます。ですから、それらの OS でも本ページの内容は実行可能なはずです。本ページの以下では Windows を例に実行方法の解説を行いますのでご了承ください。

ダウンロードが完了すると、「ダウンロード」フォルダなどに「Anaconda3-2020.07-Windows-x86_64.exe」のようなファイルが保存されています。ダブルクリックし、Anaconda のインストールを開始しましょう。

インストールが終わったら、スタートメニューの「A」の項目に下図のように「Anaconda3 (64bit)」という項目が増えています。 このうち「Anaconda Prompt (Anaconda3)」を選択して実行してください。


現れた下記の画面がプロンプトであり、ここでコマンド(命令)を実行することで、各種ツールのインストールを行います。「neura」の部分はユーザー名であるので人により異なります。


まず、プロンプト上で下記のコマンドを入力して実行して、「仮想環境」と呼ばれるものを作成します。「仮想環境」とは、Anacondaのデフォルトの環境とは別に、本ページ用の環境を作るために用います。ここでは本ページの演習を実行するために作成する仮想環境の名称を「cubemos」としました。
また、Anaconda は Python 3.8用のものをダウンロードしましたが、仮想環境は Python 3.7 用のものを作成しますのでご注意ください。
conda create -n cubemos python=3.7
なお、以後長いコマンドが続きます。ブラウザ上で上のコマンドをコピーし、下図のようにプロンプトの左上のアイコンをクリックして現れるメニューから「貼り付け」を選択すればコマンドをプロンプトに貼り付けられます。


すなわち、本ページからコピーしたコマンドを上図の方法でプロンプトに貼り付け、Enterキーを入力することでコマンドを実行するのです。この方法により、確実にコマンドを実行するようにしましょう。

なお、このとき「Continue creating environment (y/[n])?」や「Proceed (y/[n])?」と聞かれますので、どちらの場合もキーボードで「y」をタイプして「Enter」キーを押して作業を進めてください。 仮想環境の作成が完了したら、下記のコマンドを実行して作成した仮想環境「cubemos」に入ります。
conda activate cubemos
その結果、プロンプトの行頭が「(base)」から「(cubemos)」に変化しており、環境が「base」から「cubemos」に変わったことがわかります。

そのままの状態で、下記の3つのコマンドを一つずつ順に実行して、本ページの演習に必要なツールをインストールしましょう。これらコマンドは特に長いので、一つずつ注意してコピーして実行しましょう。先ほどと同様、 「Proceed (y/[n])?」などと聞かれたときはキーボードで「y」をタイプして「Enter」キーを押して作業を進めてください。
conda install py-opencv spyder console_shortcut toml

pip install pyrealsense2

pip install --find-links="%CUBEMOS_SKEL_SDK%\wrappers\python" cubemos-skeletontracking
なお、最後のコマンドは以前の Cubemosのバージョン(バージョン2系)では以下でした。
pip install --find-links="%CUBEMOS_SKEL_SDK%\wrappers\python" cubemos-core cubemos-skel
これらのコマンドの実行が終わると(正確には一つ目のコマンドの実行が終わると)、スタートメニューの「Anaconda3 (64bit)」の項目には「Anaconda Prompt (cubemos)」や「Spyder (cubemos)」が追加されています。どちらも、仮想環境 cubemos で必要なツールです。「Anaconda Prompt (cubemos)」は仮想環境 cubemos へツールをインストールしたいときに、「Spyder (cubemos)」は仮想環境 cubemos でPythonプログラムを実行するときに用います。

ここまでが終わったらプロンプトを閉じ、スタートメニューから「Anaconda3 (64bit)」の「Spyder (cubemos)」を実行してください。カッコ内の文字が cubemos であることが重要です。

起動した Spyder(cubemos) で、メニューから「ツール」→「現在のユーザーの環境変数」を選択してください。警告画面でOKすると、以下のように Spyder で読み込まれている環境変数のリストが現れます。先ほど自分で登録した「CUBEMOS_SKEL_SDK」が存在することがわかります。それを確認できたら、この画面を閉じて構いません。


なお、仮想環境 cubemos 用のSpyder が起動して、インターフェースが日本語ではなかった場合、メニューから「Tools」→「Preferences」を選択し、現れたウインドウで「General」→「Advanced settings」を選択してLanguageを日本語に設定してください。その際、再起動を促されますのでそれに従えばインターフェースが日本語になります。

Skeleton Tracking SDK を Windows の Anaconda 上の Python で使ってみる

以上が終わったら、サンプルファイルを実行してみましょう。Skeleton Tracking SDK をインストールした PC に RealSense D415 または D435 を接続しましょう。本ページのスクリーンショットは D435 で試した結果です。

neuralassembly/realsense-cubemos-skeletonのページに移動し、「Clone」→「Download ZIP」とたどることで、realsense-cubemos-skeleton-master.zip をダウンロードしましょう。realsense-cubemos-skeleton-master.zip は圧縮ファイルなので、右クリックして「すべて展開」を選択することなどにより、展開してください。中に含まれる realsense_skeleton.py がサンプルプログラムなので、先ほど起動した Spyder (cubemos) のメニューから「ファイル」→「開く」を選択して読み込みます。

実行する前に、Spyder (cubemos) のメニューから「実行」→「ファイルごとの設定」を選択します。現れたウインドウで、下図のように、「コンソール」→「外部システムターミナルで実行」にチェックを入れ、OKボタンをクリックします。


以上が終わったら、Spyder (cubemos) のメニューから「実行」→「実行」を選択します。以下のように3つのウインドウが開きます。順に、カラー画像(骨格表示)、深度画像、上記2枚の重ね書きです。

カラー画像(骨格表示)


深度画像


上記2枚の重ね書き

これらはプログラム中の132~134行目で表示されています。不要ならばコメントアウトなどしてください。
        cv2.imshow('color', color_image)
        cv2.imshow('depth', depth_image)
        cv2.imshow('overlay', added_image)

少しだけコメント

関数 render_result の内部にあるfor 文
for index, skeleton in enumerate(skeletons):
の内部では、
skeleton.joints[i][0] : 関節 i の x 座標
skeleton.joints[i][1] : 関節 i の y 座標
skeleton.confidences[i] : 関節 i の confidence 
を用いることができます。2つの関節の confidence がともに 0.5 以上のときにその間が線分で結ばれます。「関節 i」の数字 i (0~17) は、「C:\Program Files\Cubemos\SkeletonTracking\docs\doc_doxygen\html\group__skeleton.html で表示される図の数値 (1~18) から 1 を引いたもの」となっています。 

おわりに

以上、お疲れさまでした。




「高校数学からはじめるディープラーニング」、「Raspberry Piではじめる機械学習」を執筆しました。