10日で作る!ラズパイ倒立振子ロボット

 

ラズパイ倒立振子ロボット
ラズパイ倒立振子ロボット

この記事では、Raspberry Piを使って10日ほどでつくった倒立振子ロボットのご紹介をします。とあるきっかけで、夏休みの宿題のノリで倒立振子を作ってみることにしました。

倒立振子ロボットを作るには、ジャイロセンサや加速度センサを駆使して、モータを制御し倒立させる必要があります。しかし、予想通りといいますか、これがなかなか大変でした。結論からいってしまうと、相補フィルタとPID制御で倒立振子の姿勢をコントロールさせました。

そんな私の、倒立振子ロボット製作の葛藤の記録を、すこしだけご紹介できればと思います。これから倒立振子の製作に挑戦するみなさんのご参考になれば幸いです。

倒立振子とは「とうりつしんし」と読みます。二輪で自立するロボットで、近年ではセグウェイやバランススクーターなど、移動手段のロボットにも応用されています。

はじめに

倒立振子ロボットを製作するにあたって、マイコンボードはRaspberry Pi zero WHを使用しました。もちろん、他のラズパイやArduinoもお使いいただけます。

ラズパイの操作は、MacのターミナルからSSHでリモート操作します。SSHのセットアップは、キーボードなしのラズパイをMacで初期設定、SSH環境構築をご覧ください。また、これから紹介するプログラムはすべて、Python 2で動かしています。Python 3をお使いの方は、各自で読み替えてください。m(_ _)m

千里の道もLチカから

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

さっそく、モータでも動かしてみたいところですが、それを実現するための予備知識がまったくないので、まずは、とても小さな一歩であるLチカ(LED点滅)から始めてみます。

gpiozeroライブラリのインストール

ラズパイのデジタルピンでLEDを制御するために、gpiozeroライブラリを次のようにインストールしました。

$ sudo apt install python-gpiozero
LEDとラズパイの配線は、ラズパイの11番ピンにLEDのアノードを、そして200Ω〜1kΩくらいの抵抗を介してカソード側をGNDへ接続しておきます。

シンプルなLチカプログラム

1秒おきに5回、LEDが点滅を繰り返すプログラムを書いてみました。

from gpiozero import LED
from time import sleep

led = LED(11)
for t in range(0, 5):
     led.on()
     sleep(1)
     led.off()
     sleep(1)

ホタルのようなLチカ

次にすこし発展させて、正弦波でなめらかな、ホタルのようなLEDの点滅をさせてみました。そのプログラムがこちら。

from gpiozero import PWMLED
from time import sleep
import numpy as np

led = PWMLED(11)

led.value = 0

for t in np.arange(0, 100*2*np.pi ,0.01):
    a = 0.5 * np.sin(t-np.pi/2) + 0.5 
#    print(a)
    led.value = a
    sleep(0.002)

▲ aの値を0〜1に正規化するため、また、プログラムスタート時の値を0からにするため、 \( 0.5 \times sin(t-\frac{π}{2}) + 0.5 \) としてます。

PWM制御

LEDの明るさはgpiozeroライブラリのPWMLEDを使って、PWM信号でLEDにかかる電力を変化させることで明るさの変化を実現しています。

PWMとは、パルス幅変調(Pulse Width Modulation)の略で、ある周波数をもったパルス波のオンオフの比率(Duty比)を変えることで、出力電力の大きさを擬似的に変える技術です。高速でスイッチをオンオフしてるようなイメージで理解してもらえれば、とりあえずは良いでしょう。

PWM信号のデューティ比
PWM信号のデューティ比

実はこれから制御していくDCモータも、LEDとおなじPWM信号で制御します。ですので、PWMの仕組みは理解しておくとよいです。PWM制御についてもうすこし詳しく知りたい方はラズパイとMOSFETとPWM制御をお読みください。

パルス幅変調と似たものに、パルス周波数変調がありますが、こちらは周波数そのものを変えるものになります。また、PWMをオシロスコープなどで観察しますと、尺取り虫のように波形が動いていておもしろいです。

DCモータを動かす

それでは、いよいよ、DCモータを動かしていきます。

モータ選び

倒立振子に関するさまざまなブログを参考にして、タミヤのダブルギアボックスを使うことにしました(後にロボサイトのギヤードモータに取り替えることになります)。ダブルギアボックスは、その名の通り、左右のタイヤを独立して動かすことができます。

モータドライバとは

ラズパイでDCモータを動かすには、モータドライバが必要です。「Lチカでやったみたいに、ライズパイにモータを接続して、PWM信号で動かせないの?」と思う方もいらっしゃるかもしれませんが、それではモータは動かせません。なぜなら、モータを動かすのに必要な電流は、LEDのそれと桁違いに大きいからです。また、ラズパイのデジタルピンから出力できる電流は数十mA程度です。DCモータでは数百mA以上の電流が必要になります。そこでモータドライバを使って、モータへ大電流を流せるようにする必要があるのです。

モータドライバの動かし方

さて、ダブルギアボックスを動かすためにBD6211Fというモータドライバを使用しました。こちらは秋葉原の千石電商で購入したものです。モータドライバの扱いはどれも似ているので、手に入らない場合は別のモータドライバでも代用可能です。

今回、はじめてモータドライバを使ってみたのですが、思いのほかカンタンでした。モータドライバの使い方をカンタンに説明しますと、2つの入力FINRINに信号を入力し、その信号によって正転、逆転、ブレーキ、ストップを制御できます。また、速度はPWM信号の電力によって変えることができます。次の対応表をご覧ください。

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

ただし、BD6211Fのデータシートで確認すると、モータドライバへ送るPWMの周波数は20kHz〜100kHz程度でなければならないようです。

gpiozeroPWMLEDを使ってモータドライバを動かすことができます。その際に、FIN_L = PWMLED(20, frequency=100000)のようにして周波数を設定します。しかし、gpiozeroにはもっと便利なMotorというライブラリがありますので、ここではそちらを使うことにしました。

ラズパイでモータを動かす

実際にモータとモータドライバをラズパイにつないで、次のようなプログラムでモータを動かしてみました。

from gpiozero import Motor
from time import sleep
import numpy as np

motor = Motor(forward=20, backward=21)

for t in range(0,5):
    motor.forward(1.0)
    sleep(2)

motor.stop()

加速度センサ

ラズパイでモータ制御することができたので、いよいよ倒立振子の姿勢制御に挑戦していきます。先にも述べましたが、ロボットを倒立させるためには、加速度センサとジャイロセンサで姿勢を維持する必要があるそうです。モータドライバ同様、これらの知識がありませんでしたので、まずは加速度センサを試してみます。

実は、秋葉原の千石電商でMMA8452Qを購入したのですが、こちらの商品は加速度センサのみで、ジャイロセンサが搭載されてませんでした。ただし、加速度センサだけでも角度を計算できるとのことですので、チャレンジしてみました。

加速度センサMMA8452Q

加速度センサMMA8452QとラズパイはI2Cという通信方法でデータ送受信を行います。I2Cの設定方法は、ラズパイではじめてのI2C通信のやり方をご覧ください。

ただし、ラズパイではI2Cで使うピン番号が決まってますので注意してください。今回使用したRaspberry Pi zero WHでは、GPIO 2にSDA、GPIO 3にSCLをつなぎます。

またI2C以外にも、SPIという通信方法があります。詳しくはラズパイではじめてのI2C通信のやり方をご覧ください。

MMA8452Q の使い方はArduino版ではありますが、Arduinoで加速度センサMMA8452Qの使い方をご参考になさってみてください。ここでは、加速度センサで角度を計算する方法について触れたいと思います。

加速度センサで角度の計算

加速度センサと姿勢角度
加速度センサと姿勢角度

加速度センサが図のような静止状態の時(実際にはありえませんが)、重力加速度aのみが働いていますので、角度θはセンサにかかる加速度\(a_x\)と\(a_z\)を使って次式で表すことができます。

$$tanθ=\frac{a_x}{a_z}$$

よって、姿勢角度θは、逆三角関数のアークタンジェントを使って次のように計算します。

$$θ=tan^{-1}\frac{a_x}{a_z}$$

詳しくは、加速度センサから角度を計算するをご覧ください。

加速度センサで角度をログする

実際に加速度センサで角度データをログして、グラフ化してみました。

次のグラフは、加速度センサを机の上に静止させた時のものです。机が多少傾いているせいか、0度中心のグラフではありませんが、-1.5度あたりを基準とすれば、誤差±0.5度くらいの精度で角度の計算ができています。

加速度センサが静止状態の時のグラフ
加速度センサが静止状態の時のグラフ

ただし、ノイズ成分が多いのでこのままでは都合が悪いです。これらのノイズはローパスフィルタを使って除去します。ローパスフィルタのアルゴリズムは指数移動平均(RCローパスフィルタ)を使います。

$$y_i=(1-k)x_i+ky_{i-1} \tag{1}$$

kは係数で、過去のデータ\(y_{i-1}\)と取得したデータ\(x_i\)を、どの程度重みづけして現在の結果\(y_i\)とするかを決める値になります。kの値を大きくするほどローパスフィルタが強くかかります。詳しくはデジタル信号におけるRCローパスフィルタをご覧ください。

このローパスフィルタを使って、実験してみました。加速度センサを0度から90度へ2回傾け、取得したデータにさまざまな係数でローパスフィルタをかけてみます。その結果が次のグラフです。

加速度センサのデータに、係数を変えながらローパスフィルタかけたグラフ
加速度センサのデータに、係数を変えながらローパスフィルタかけたグラフ

ローパスフィルタを強くかければかけるほど、滑らかになり安定はしますが、速い動きに追従できず、位相も遅れてしまいます。そのようすは加速度センサとローパスフィルタで視覚化してみましたので、ご参考になさってみてください。

Processingで加速度センサとローパスフィルタ
Processingで加速度センサとローパスフィルタ

こちらのアニメーションは YouTube動画 でご覧になれます。

加速度センサだけで倒立振子は作れる?

さて、「姿勢角度が分かれば、加速度センサだけで倒立振子は作れるんじゃないか?」そう思った方は多いと思います。しかしそれは、あくまで加速度センサが静止している時またはゆっくりと動いている時のことです。速い運動状態の時、つまり、重力加速度に比べて運動加速度が無視できない場合、加速度センサだけでは、重力加速度なのか運動加速度なのか判断できず、正確な角度を計算できません。

このように、運動状態での角度検出には、ジャイロセンサが向いています。しかしながら、ジャイロセンサもまた完璧ではありません。後にくわしく説明しますが、徐々に角度に誤差が出てきてしまうのです。しかし、誤差の補正は加速度センサで行うことができます。よって、倒立振子のような姿勢を制御するロボットでは、ジャイロセンサと加速度センサを組み合わせて制御するのがふつうです。

加速度センサと、モータだけで実験 してみました。倒立振子にはほどとおく、このとき、本当に倒立振子ができるかどうか不安になりました。そこで、倒立振子をつくり直すことになりました。

倒立振子のバージョンアップ

倒立振子のバージョンアップ
倒立振子のバージョンアップ

写真のように、倒立振子をつくりなおしました。ホームセンターで購入した木材を加工して、倒立振子の枠組みをつくりました。タイヤ幅も太くして、モータを9V電源で動かすよう改造しました。ただし、写真のモータはRS-385ですが、ギアなしでトルクを出せなかったため、ロボサイトのギアードモータへ変更しています。

また、モータドライバもBD6211FからTA8428Kへ変更しました。TA8428Kの使い方はラズパイでモータ制御・TA8428Kモータドライバに書きました。

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

ところで、モータを動かすとプログラムクラッシュする現象が頻繁に起こっていました。エラー出力は次の通りです。

bus.write_byte_data(0x69, 0x0F, 0x04)
IOError: [Errno 121] Remote I/O error

例外処理追記すれば、プログラムの強制終了は回避できました。ですが、エラーそのものはなくなりません。

try:
	...
except IOError as e:
	...

どうもモータの転回時にエラーが発生しているようです。そこでピンときたのですが、モータの逆誘導起電力によるノイズが悪影響しているのかもと思いました。オシロスコープで観察するとモータの転回時に大きなノイズが発生していることがわかりました。このノイズによって、I2C通信エラーが起こっているようです。

そこで「モータ ノイズ対策」で調べてみると、パスコンをモータに付けて対処する必要があることがわかりました。さっそく、写真のように0.1uF程度のセラミックコンデンサを3箇所にはんだ付けしました。

パスコンでモータノイズ対策
パスコンでモータノイズ対策

▼ モータ端子間に1つ、それぞれの端子とモータシャーシの間にはんだ付けします。その後は、I2Cの通信エラーがまったくなくなりました。

モータとパスコンの配線図
モータとパスコンの配線図

ジャイロセンサと加速度センサの導入

倒立振子のアップデートができたところで、いよいよ、ジャイロセンサと加速度センサを実装していきます。BOSCHのBMX055が搭載されている9軸姿勢センサモジュールを使用しました。秋葉原の秋月電子で購入しましたが、こちらの商品も同じBMX055が使われてますので動かし方は同じかと思われます。

BMX055の設定方法と姿勢の計算方法は、後述するメインプログラムや、ラズパイでBMX055の使い方をご覧ください。

ジャイロセンサと加速度で角度計算・ドリフト除去

BMX055を使う上で、ジャイロセンサと加速度センサで得られる角度の向きが逆であるので注意してください。たとえば、加速度で計算した値が20度だとしたらジャイロでは、-20度です。ですから、ジャイロの角度結果に-1を掛けて帳尻を合わせることになります。

ところで次のグラフは、90度傾ける作業を繰り返したときのジャイロセンサの出力をグラフにしたものです。元の位置にもどっても0度にならず、徐々にずれていきます。この誤差のことを、ドリフト成分と呼びます。

90度へ傾けたときのジャイロセンサのログ
90度へ傾けたときのジャイロセンサのログ

ご覧の通り、ジャイロセンサは瞬間的な角度、つまり相対的な角度検出は得意ですが、ドリフト成分が含まれてしまうため、絶対的な角度検出には得意ではありません。

そこで加速度センサの出番です。加速度センサが静止している状態ならば、地球の重力加速度のみですので、絶対的な角度を知ることができます。これを基準に、ジャイロセンサと組み合わせれば、ドリフト補正しながら角度を検出できそうです。

では具体的にどうやるのか。

ジャイロセンサと加速度センサを使った角度計算する方法を調べてみますと、「カルマンフィルタ」と「相補フィルタ」というものに辿りつきます。「カルマンフィルタ」は簡単に理解できるものではなさそうです。後にPythonでカルマンフィルタを使ってみたで試してみましたが、いまだに理論はわかってません(^_^;)

ちなみに「カルマンフィルタ」は経済学の分野でも使われています。

一方で、「相補フィルタ」は私にも理解できるいたってシンプルなアルゴリズムでした。しかも「カルマンフィルタ」と対して結果に違いがないというじゃないですか。だったらカンタンな「相補フィルタ」を採用しない手はありません。次のように、「相補フィルタ」はたったの一行で書けてしまう数式でありプログラミングです。

$$angle_{i} = k * (angle_{i-1} + xGyro * dt)\\ + (1 - k) * angleAccel$$

先に述べました、指数移動平均のローパスフィルタとよく似ています。倒立振子の前後のバランスのみ考えれば良いので、ジャイロセンサでは一軸方向のxGyroを入力します。また、angleAccelはアークタンジェントを使って加速度センサから計算された角度です。

▼ 「相補フィルタ」を使ってk=0.9として実際に角度を取得してみました。さきほどと同様に、90度傾ける作業をくりかえしました。次のグラフがその結果です。

相補フィルタによる角度計算
相補フィルタによる角度計算

いかがでしょうか?ドリフト成分がキレイに除去されてますね!

ここまでで角度の検知が可能になりましたので、「一定の角度を超えたら前進または後進させることで倒立振子ロボットの姿勢を維持できるのではないだろうか?」そう思って実験してみました。その姿は、まるで生まれたての子鹿のような立ち方でした(笑)▼ そのようすを撮影しましたのでぜひご覧ください。

ここまで来れば、あともうひといきですね!

PID制御

先ほどのように、ある角度を基準にして前後の動きで姿勢を維持するには無理がありませた。良い方法に悩んでいたところ、PID制御という方法を知ることになりました。しらべてみると、PID制御は古典的手法のようですが、今でも多くの場面で使われている制御方法のようです。また、倒立振子ロボットでも、PID制御で姿勢を維持させるのが一般的なようです。

PID制御を学ぶにあたって

実際に、どうやってPID制御をするのか調べたところ、プログラミング的にはまったく難しいものではありませんでした。実際のプログラムは、後に紹介する「倒立振子のプログラム」でご覧になってください。制御工学の知識がなくても、巷のプログラムをコピペすれば「PID制御できました!」とカンタンに名乗ることができます。

しかし、それでは面白くありません。せっかくですから、PID制御の理論をくわしく知りたいものです。そこで見つけたのが、マンガでわかるシリーズの本「Pythonによる制御工学入門」です。

この本は、制御工学に触れたことのない人でも、わかりやすく説明してくれるのでオススメです。しかも、Pythonで計算していくのでPythonスキルもアップします。ただし、制御工学自体が物理と数学をフルに使う分野だけに、この本だけで完璧に理解するのはなかなか難しいかもしれません。それでも、制御工学の全体像をつかめたので「Pythonによる制御工学入門」を読んでおいてよかったです。

いままでは、線形や非線形、システム、入力出力、uやy、直列結合、並列結合、フィードバック、ステップ応答、1次遅れ系、2次遅れ系など、制御工学の用語が独特でわかりづらかったのですが、この本のおかげでだいぶ理解がすすみました。

とくに、前半の伝達関数モデルまでは最高に面白かったです。ラプラス変換そのものは理解できなくても、機械運動や電子回路の微分方程式を、シンプルな数式で置き換えて計算できるのには目からうろこでした。あまりにラプラス変換にショックを受けたので、その後、自分なりに勉強してみました。はじめてのラプラス変換にまとめてみましたので、ご参考になさってみてください。

PID制御とは

話は戻って、PIDとは「Proportional-Integral-Differential Controller」のことです。比例、積分、微分を意味します。

  • Proportional: 比例
  • Integral: 積分
  • Differential: 微分

先ほどの倒立振子では、比例制御だけの姿勢維持でしたから、子鹿のような不安定な動きになってしまったのです。PID制御に置き換えることで、たとえば積分制御では突然の外力が加わった時の補正に強くなります。また、微分制御では震えるような振動が抑えられて滑らかな動きになります。PID制御の詳しい話は、先ほどのPythonによる制御工学入門ゼロからはじめるPID制御をご参考になさってみてください。

倒立振子にPID制御導入

さて、実際にPID制御を倒立振子ロボットに導入して動かしてみました。PID制御のおかげで動きはだいぶマシになりました。ただ、どうしても片側に寄ってしまうようです。

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

プログラムの変数をなんども書き直しながらPIDパラメータを調整するのには限界があります。▼ そこで、TCP通信でiPhoneからパラメータ調整できるようにSwiftでアプリを作ってみました。パラメータ調整が俄然ラクになり、なんとか合格点を出せる動作になりました。

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

倒立振子のプログラム (Python)

話が長くなりましたが、最後に今回つくった倒立振子の全体のプログラムをご紹介します。コードは洗練されておらずきたないままで恐縮ですm(_ _)m

また、前後に動かす記述が残ってますが、うまくいきませんでしたので改造してみてください。

#!/usr/bin/env python
# -*- coding: utf-8 -*-
# Created by Toshihiko Arai.
# https://101010.fun/iot/self-balancing-robot.html

import numpy as np
import time
from datetime import datetime
import smbus
from gpiozero import PWMLED
from gpiozero import Motor
from gpiozero import Robot
import sys
import threading
import termios

motorL = Motor(forward=18, backward=19)
motorR = Motor(forward=12, backward=13)
# robot = Robot(left=(18, 19), right=(12, 13))


GYRO_ADDR = 0x69
bus = smbus.SMBus(1)

# BMX055
# Data sheet -> https://www.mouser.jp/datasheet/2/783/BST-BMX055-DS000-1509552.pdf
# Acceleration address, 0x19
# Select PMU_Range register, 0x0F(15)
#       0x03(03)    Range = +/- 2g
bus.write_byte_data(0x19, 0x0F, 0x03)
# Select PMU_BW register, 0x10(16)
#       0x08(08)    Bandwidth = 7.81 Hz
bus.write_byte_data(0x19, 0x10, 0x08)
# Select PMU_LPW register, 0x11(17)
#       0x00(00)    Normal mode, Sleep duration = 0.5ms
bus.write_byte_data(0x19, 0x11, 0x00)

time.sleep(0.5)


# Gyro address, 0x69
# Select Range register, 0x0F(15)
#       0x04(04)    Full scale = +/- 125 degree/s
bus.write_byte_data(0x69, 0x0F, 0x04)
# Select Bandwidth register, 0x10(16)
#       0x07(07)    ODR = 100 Hz
bus.write_byte_data(0x69, 0x10, 0x07)
# Select LPM1 register, 0x11(17)
#       0x00(00)    Normal mode, Sleep duration = 2ms
bus.write_byte_data(0x69, 0x11, 0x00)
time.sleep(0.5)


def accl():
    xA = yA = zA = 0

    try:
        data = bus.read_i2c_block_data(0x19, 0x02, 6)
        # Convert the data to 12-bits
        xA = ((data[1] * 256) + (data[0] & 0xF0)) / 16
        if xA > 2047:
            xA -= 4096
        yA = ((data[3] * 256) + (data[2] & 0xF0)) / 16
        if yA > 2047:
            yA -= 4096
        zA = ((data[5] * 256) + (data[4] & 0xF0)) / 16
        if zA > 2047:
            zA -= 4096
    except IOError as e:
        print"I/O error({0}): {1}".format(e.errno, e.strerror)

    return xA, yA, zA


def gyro():
    xG = yG = zG = 0

    try:
        data = bus.read_i2c_block_data(GYRO_ADDR, 0x02, 6)
        # Convert the data
        xG = (data[1] * 256) + data[0]
        if xG > 32767:
            xG -= 65536

        yG = (data[3] * 256) + data[2]
        if yG > 32767:
            yG -= 65536

        zG = (data[5] * 256) + data[4]
        if zG > 32767:
            zG -= 65536

    except IOError as e:
        print "I/O error({0}): {1}".format(e.errno, e.strerror)

    return xG, yG, zG


class RobotJob(threading.Thread):
    def __init__(self):
        threading.Thread.__init__(self)
        self.move = 0

    def forward(self, value):
        motorL.forward(abs(value))
        motorR.forward(abs(value))

    def backward(self, value):
        motorL.backward(abs(value))
        motorR.backward(abs(value))

    def stop(self):
        motorL.brake()
        motorR.brake()

    def balance(self, value):
        if value < 0:
            self.forward(value)
        elif value > 0:
            self.backward(value)
        else:
            self.stop()

    time.sleep(0.5)

    def setup(self):
        _gyro = 0
        _angle = 0
        for i in range(0, 100):
            xGyro, yGyro, zGyro = gyro()
            _gyro += xGyro

            xAccl, yAccl, zAccl = accl()
            _angle = np.arctan2(
                zAccl, yAccl) * 180 / 3.141592

        _gyro = _gyro / 100
        _angle = _angle / 100

        return _gyro, _angle

    def run(self):
        degree = 0
        i = 0
        lastErr = 0
        errSum = 0
        Kp = 45
        Ki = 250
        Kd = 220
        preTime = time.time()

        offsetGyro, offsetAngle = self.setup()
        offsetGyro = 0
        offsetAngle = 1.5

        angle = 90 - offsetAngle
        angleGyro = angle

        while True:

            xAccl, yAccl, zAccl = accl()
            xGyro, yGyro, zGyro = gyro()

            now = time.time()

            dt = (now - preTime)
            preTime = now

            angleAccl = np.arctan2(
                zAccl, yAccl) * 180 / 3.141592 - offsetAngle

            K = 0.996

            # Full scale = +/- 125 degree/s
            # 125 / 32766 = 0.003815
            xGyro *= -1
            dGyro = (xGyro) * 0.003815 * dt
            angleGyro += dGyro
            angle = K * (angle + dGyro) + (1 - K) * angleAccl
            # print "angleGyro=%f angleAccl=%f angle=%f" % (angleGyro, angleAccl, angle)

            # PID制御
            # Proportional=比例、Integral=積分、Differential=微分
            error = angle / 90 - 1  # P成分:傾き0~180度 → -1~1
            errSum += error * dt  # I成分
            dErr = (error - lastErr) / dt / 125  # D成分:角速度±125dps → -1~1
            u = Kp * error + Ki * errSum + Kd * dErr + self.move

            lastErr = error

            if u < -1.0:
                u = -1.0
            elif u > 1.0:
                u = 1.0

            self.balance(u)

            if i % 1000 == 0:
                print(u)
            i += 1


if __name__ == "__main__":

    t = RobotJob()
    # スレッドをデーモンに設定し、メインスレッドの終了とともにデーモンスレッドも終了させる。
    t.setDaemon(True)
    t.start()

    # 標準入力のファイルディスクリプタを取得
    fd = sys.stdin.fileno()

    # fdの端末属性をゲットする
    # oldとnewには同じものが入る。
    # newに変更を加えて、適応する
    # oldは、後で元に戻すため
    old = termios.tcgetattr(fd)
    new = termios.tcgetattr(fd)

    # new[3]はlflags
    # ICANON(カノニカルモードのフラグ)を外す
    new[3] &= ~termios.ICANON
    # ECHO(入力された文字を表示するか否かのフラグ)を外す
    new[3] &= ~termios.ECHO

    while True:
        try:
            # 書き換えたnewをfdに適応する
            termios.tcsetattr(fd, termios.TCSANOW, new)
            # キーボードから入力を受ける。
            # lfalgsが書き換えられているので、エンターを押さなくても次に進む。echoもしない
            c = sys.stdin.read(1)
            if c == 'j':  # 前進
                t.move = 0.5
                print("前進")
                time.sleep(3)
                t.move = 0
            # elif c == 'k':  # 後進
            #     t.move = -0.3
            #     print("後進")
            elif c == 's':  # Stop
                t.move = 0
                print("s")
            elif c == 'q':
                t.kill_flag = True

        finally:
            # fdの属性を元に戻す
            # 具体的にはICANONとECHOが元に戻る
            termios.tcsetattr(fd, termios.TCSANOW, old)

本プログラムは GitHubで公開 してます。長いプログラムですので、そちらで閲覧すると見やすいかもしれません。

おわりに

とあるきっかけで、夏休みの宿題感覚ではじめてみた倒立振子ロボットの製作でした。当初は一ヶ月くらいを見積もっていましたが、わずか10日ほどでつくることができました。これもひとえに、諸先輩方たちが残したWebログのおかげです。m(_ _)m

現在では、倒立振子の情報がたくさん公開されているので、ロボット製作の敷居がとても低くなっています。そんな先人たちの知恵に感謝を申し上げるとともに、私のブログもまたどなたかに役立ってくれればこれほど嬉しいことはありません。

参考サイト

記事に関するご質問などがあればTwitterへお返事ください。
この記事で紹介した商品
人気のラズパイ
人気のラズパイ周辺機器
Raspberry Piのオススメ入門書
関連記事