10日で作る!倒立振子ロボット



倒立振子は「とうりつしんし」と読む。二輪で自立するロボットのことだ。
Google翻訳では「inverted pendulum」と訳されるが、「Self Balancing Robot」と言ったほうが英語で表現する場合には良いだろう。

メインコンピューターはRaspberry Pi zeroを使うことにした。MacのターミナルからSCPでファイルを転送したり、SSHでログインしてロボットのプログラムを実行する。ラズパイの基本的なセットアップは済んでいるものとして進めていく。ロボットのプログラムはPython2.xで行うものとする。





01.千里の道もLチカから


モーターをプログラミングで制御する予備知識がないため、まずはとても小さな一歩であるLチカから始めよう。

GPIOの制御にはgpiozeroというPythonモジュールを使うことにした。
Python2のgpiozeroをインストールするには次の通り。
sudo apt install python-gpiozero

そして1秒おきに5回点滅を繰り返す簡単なプログラムを書いてみた。



点滅だけでは面白くないので、少し発展させて正弦波で滑らかな点灯になるように制御してみることにした。0-1の範囲に納めるためゲインを0.5倍に、そしてスタート時の値を0させるため、0.5*sin(t-π/2)+0.5として計算している。



gpiozeroのPWMLEDでLEDの明るさを制御している。PWMとはパルス幅変調(Pulse Width Modulation)のことで、周波数が一定のパルスのオンオフの比率(Duty比)を変えることで電力の大きさを変化させる技術である。パルス周波数変調ではないことに注意しておこう。実際にオシロスコープで観察すると尺取り虫のようにパルス幅が伸び縮みしていて面白い。モーターもPWMで制御することになるのでここら辺の仕組みの理解を覚えておいて損はない。


02.モータードライバーの動作テスト


モータードライバーとセンサーを買うために秋葉原まで出かけたが、秋月電子がお盆休みであることを忘れていた。千石電商は営業していたので、仕方なく予定とは違うモータードライバBD6211Fを購入することにした。

モータードライバーの使い方はとても簡単だった。FINとRINに送る信号を変えるだけで正転、逆転、ブレーキ、ストップの制御を行えるようになっている。データシートを読むとモーターへ送るPWMの周波数は20kHz〜100kHz程度にしなければならないようだ。

FIN RIN 動作(OPERATION)
L L 空転
PWM L 正転
L PWM 逆転
H H ブレーキ

gpiozeroのPWMLEDでパルス周波数を変更するにはFIN_L = PWMLED(20, frequency=100000)のように宣言すれば良い。またはもっと便利なMotorモジュールを使ってもよいだろう。




倒立振子に関する様々なブログを参考に、タミヤのダブルギアボックスで組み立てることにした。左右のタイヤを独立して動かすことができる。しかし倒立させるだけなら一つのモーターで動くギアボックスでもよかったかも知れない。

センサーは実装していないが早速組み立てて、バッテリーを積んで前後に動かしてみたが倒立振子には程遠かった。


03.加速度センサー動作テスト


千石電商で加速度センサーMMA8452Qを購入した。ジャイロセンサーも搭載されているものと思っていたら搭載されておらず。加速度センサーだけでも角度の抽出はできるようなので試してみた。

センサーとの通信はI2Cで行う。I2Cの設定は以前の記事に書いたので、ここでは説明を省略する。
ただし注意点が一つ。ラズベリーパイゼロでI2C通信するときのピン番号に注意が必要だ。GPIO番号の2番にSDA、3番にSCLを接続する。フィジカル番号ではなくGPIO番号であることに注意すること。

加速度センサーでも角度が検出できるなら、ジャイロセンサーはいらないのでは?しかし実際加速度センサーで角度を検出してみるとわかるが、センサー自体が動いている状態だと重力加速度なのか運動による加速度なのかが判断できず正確な角度は測ることができない。なので瞬間的な角度差はジャイロセンサーで取得し、ジャイロセンサーでのドリフト成分を加速度センサーで補正することになる。

次のグラフは加速度センサーを3分間机の上に静止させた時のログである。

机がすでに傾いているため0度中心にはなっていないが-1.5度あたりを中心として誤差±0.5度くらいの精度で取得できている。角度はarcsinやarctan2で求めることができる。

上図のように加速度センサーは高周波ノイズが含まれる。高周波ノイズはローパスフィルターでカットできる。加速度センサーを2回、90度傾けたときのログに様々なローパスフィルターをかけてみた。

ローパスフィルターのアルゴリズムは指数移動平均を使った。滑らかにはなったが位相が遅れる感じだ。ローパスフィルターを強くかけると滑らかにはなるが、速い動きに追従できない。

さて、加速度センサーで垂直かどうかを判定してモーターの正転逆転だけで制御を行なってみた。



倒立振子にはほど遠いような反応の遅い動き。タイヤも細くて姿勢が安定しなさそうで心細い。ここでモーターとギア比の改善が必要だと思ったので、倒立振子もう一度作りなおすことにした。


04.倒立振子のアップデート



木材でロボットの枠組みを作りなおすことにした。タイヤ幅を太くし、モーターをRS-385にして9V電源で動かすようにした。しかしモーターに直接タイヤを装着したのが失敗だったか、全然トルクが出ない。モーターの知識の乏しさを悔やむところだ。そこでモーターをギアードモーターに変えることにした。


アップデートしたギアードモーターはパワフルに動いてくれて好感触だったので、ようやく先が明るくなった。


05.I2C通信エラーのトラブルシューティング


さてこの頃には幾度となくプログラムが停止する問題に直面していた。
下記のようなエラーが頻繁に出ている始末だった。
bus.write_byte_data(0x69, 0x0F, 0x04)
IOError: [Errno 121] Remote I/O error

どうもモーターの回転方向が変わるときに高確率でI2C通信エラーになるようだ。
ラズパイ電源部に100μのコンデンサをつけて電源の出力インピーダンスを低くしてみたが変化なし。そもそもモーターの電源とラズパイの電源は別にしてあるので関係はなさそうだ。



Pythonプログラムで例外処理を書いたらプログラムの強制終了はなくなった。当たり前の話だった。
try:
except IOError as e:

それでもモーターの転回時にエラーは出ているので根本的な解決はされていない。モーターの逆誘導起電力によるノイズだと推定できる。オシロスコープで調べてみるとやはり転回時にかなり波形が乱れる。これらのノイズがきっとラズパイに影響を与えてるに違いない。モーターノイズ対策で調べるとやはり皆さん、パスコンを入れて対処しているようだ。さっそくモーターには0.1uFのバイパスコンデンサを入れることにした。

写真のように3個のコンデンサをつける。端子間に一つと、それぞれの端子とシャーシの間にコンデンサを入れる。ずばり、I2Cの通信エラーが無くなった!


06.BOSCH BMX055でジャイロセンサーと加速度センサーの導入


ジャイロセンサーと加速度センサーが一体になっているBMX055センサーを秋月電子で購入した。細かな設定や計算はメインプログラムを参照してもらいたい。
このセンサーで注意すべきところは、ジャイロと加速度で得られる角度の向きが逆だということだ。例えば加速度で計算した値が20度だとしたらジャイロでは-20度という具合になってしまっていた。だからジャイロの角度結果に-1を掛けて帳尻を合わせることにした。

さて、次のグラフはジャイロセンサーのみを使用して90度傾ける作業を繰り返したときのログだ。注目すべきところは、元の位置に戻しても0度にはならずドリフトしていることだ。

ジャイロセンサーは瞬間的な角度検出には向いているが、絶対値的な角度検出には向いていない。ドリフト成分が含まれるからだ。そこで加速度センサーの出番だ。加速度センサーは静止している状態であるならば地球の重力加速度のみ働いていると考えることができる。つまり地球を基準とした絶対値的な角度を測定できるのだ。だからジャイロセンサーと加速度センサーを使うことによって、互いの欠点を補い実用的な角度検出を行うことができることになる。

では具体的にどうやるのだろうか?
調べていくとカルマンフィルターと相補フィルターというものにたどり着いた。カルマンフィルターは簡単に理解できるものではなさそう。一方で倒立振子においては相補フィルターはカルマンフィルターと結果に大した違いが無いにも関わらず、たったの一行で書ける数式でありプログラミングのようだ。今回は相補フィルターを採用することにした。
$$angle = k * (angle + xGyro * dt)\\ + (1 - k) * angleAccel$$
k=0.9としてジャイロセンサーと加速度センサーを使って角度を検知したものが次のグラフだ。先ほどのグラフと同様、90度傾ける作業を繰り返したログだ。

ジャイロセンサーで見られたドリフト成分が見事に除去されている。

このようにして角度検出ができれば、一定角度をオーバーしたら前進または後進させることで姿勢を維持できるのではないだろうか。そう思って試したところ生まれたての子鹿のような立ち方をしていた。あともうひといきだ!!




07.PID制御


先のような角度を基準に前後で制御するには限界があるようだ。この頃、友人からの知らせでPID制御という存在を知った。PID制御というのは古典的手法のようだが、今でも世の中の制御のいろいろな場面で使われている。倒立振子でもPIDで制御が主流のようだ。プログラミング的には非常に簡潔に書けることが分かった。制御工学の知識がなくても巷のプログラムをコピペすれば簡単に制御できてしまうだろう。しかしそれではつまらない。せっかくだからPID制御の理論を少しでも詳しく知りたいと思う。そこで次のマンガでわかるシリーズの制御工学をAmazonで購入した。

書籍はだいぶ噛み砕いてわかりやすく説明してくれるのでありがたい。とはいえ制御工学自体が物理と数学をフルに使う分野だけにそれなりに理解するのには苦労する。真面目に理解しようと思うとフーリエ変換を理解するより大変そうだ。
前半の伝達関数モデルまではたいへん面白かった。ラプラス変換そのものは理解できなくても、機械運動や電子回路の微分方程式をシンプルな数式に置き換えられることに大変興奮した。しかしその後の状態空間モデルになると行列などでてきて「何これ死ぬの?」と言わざるを得ないくらい難解だ。
それでも制御工学の本を読んでおいてよかったと思うのは、制御工学の全体像を見ることができたからだ。線形や非線形、システム、入力出力、uやy、直列結合、並列結合、フィードバック、ステップ応答、1次遅れ系、2次遅れ系など用語が独特でわかりづらかったが、本を読んだおかげで理解することができた。

話は戻ってPID制御いうのはProportional-Integral-Differential Controllerの略だ。Proportionalは比例、Integralは積分、Differentialは微分のことである。今までは比例制御でなんとかしようとしてきた。しかしそれだと不安定な動作になってしまう。そこで積分制御を導入することで外力が加わった時の補正に強くなる。また、微分制御を導入すれば震えるような振動が抑えられ滑らかに移動することができるようになる。詳しいことは上記の書籍とこちらの動画を参考にしてほしい。



そんなこんなでPID制御を倒立振子の制御に導入して動かしてみた。PID制御で大分マシな動きになったが、どうしてもこっちに近寄ってきてしまう。



先にも述べたがPID制御のプログラミング自体はとても簡単である。問題はパラメーターの調整に苦労することだ。数日の間、このパラメーター調整に時間を費やした。この苦労かいあってか、微分、積分成分がロボットの動きにどのように影響するのかを実感できた。




プログラムをいちいち書き直してPIDのパラメーターを調整するのは大変なので、TCP通信で調整できるiPhoneアプリを作ってみた。劇的にPIDの調整がラクになり、合格点を出せる動きまでたどり着けた。



相補フィルターの係数とプログラムのwhileループのタイマー間隔とPIDのパラメーターは、相互に影響を受けやすいので最適化するには苦労した。PID制御の仕組みの理解とトライアンドエラーの根気強さが倒立振子ロボット製作には必要かも知れない。



08.メインプログラム (Python)


最後にPythonで書いた倒立振子のメインプログラムを載せておこう。



とあるきっかけで夏休みの宿題感覚ではじめた倒立振子ロボットの製作だった。当初は一ヶ月くらいを見積もっていたが10日ほどで制作することができた。これもひとえに諸先輩がたたちが残したウェブログのおかげだ。倒立振子のブームは数年前に過ぎ去り、今では安定した情報を簡単に得ることができるのだ。私のブログもまた、だれかの参考になれば幸いである。

他にも倒立振子のプログラムをGithubに公開してあるので興味ある方は参照してほしい。
https://github.com/araemon/BalancingRobot




09.参考サイト


Gpiozero Document
Data sheet BD6211F
Data sheet MMA8452Q
MMA8452Q を Raspberry Pi から使う
加速度センサから傾斜角度を求める
加速度センサーから軸廻り角度への変換計算
The Balance Filter
相補フィルタ
Androidの加速度センサー(Accelerometer)から重力の影響を省いて値を取得する(ローパスフィルタでノイズも除去)
ハイパスフィルタ(High Pass Filter)
Rasperry Pi 3で9軸センサ(BMX055)を使う
MicroPythonで「BMX055使用9軸センサーモジュール」を駆動するクラス
Data sheet TA8428K
9軸ジャイロセンサをJetson TK1で使う。
姿勢制御
Data sheet BMX055
M5StickC で倒立振子 PID制御編 ー倒立振子への道 3ー
ジャイロのドリフト補正と比較(カルマン、相補フィルター)
加速度センサーの値を強度と角度に変換
ジャイロのドリフト補正を改良
倒立振子の制作
システム同定による倒立振子のLQR制御
DCモータの基礎


10.関連記事


Raspberry Pi のセットアップ!モニター、キーボードなしでMacからSSH
I2C通信でADコンバータの値を読み込む (Raspberry Pi zero)
Decade Boxの制作
10日で作る!倒立振子ロボット
I made a Self Balancing Robot in 10 days!
はじめてのステッピングモーター (Raspberry Pi zero)



あなたにおすすめ