10日で作る！ラズパイ倒立振子ロボット
この記事では、Raspberry Piを使って10日ほどでつくった倒立振子ロボットのご紹介をします。 とあるきっかけで、夏休みの宿題のノリで倒立振子を作ってみることにしました。
倒立振子ロボットを作るには、ジャイロセンサや加速度センサを駆使して、モータを制御し倒立させる必要があります。しかし、予想通りといいますか、これがなかなか大変でした。結論からいってしまうと、相補フィルタとPID制御で倒立振子の姿勢をコントロールさせました。
そんな私の倒立振子ロボット製作の葛藤の記録をご紹介します。これから倒立振子の製作に挑戦するみなさんのご参考になれば幸いです。
はじめに
倒立振子ロボットを製作するにあたって、マイコンボードは Raspberry Pi zero WH を使用しました。もちろん、他のラズパイやArduinoもお使いいただけます。
Raspberry Piの操作は、MacのターミナルからSSHでリモート操作します。SSHのセットアップは、 Raspberry Pi zero WHをモニター・キーボードなしでSSH接続するまで をご覧ください。また、これから紹介するプログラムはすべて、Python 2で動かしてます。Python 3をお使いの方は、各自で読み替えてください。m(_ _)m
千里の道もLチカから
gpiozeroライブラリのインストール
Raspberry PiのデジタルピンでLEDを制御するために、gpiozeroライブラリを次のようにインストールしました。
$ sudo apt install python-gpiozero
シンプルな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 \) としてます。
DCモータを動かす
それでは、いよいよ、DCモータを動かしていきます。
モータ選び
倒立振子に関するさまざまなブログを参考にして、 タミヤのダブルギアボックス を使うことにしました（後にロボサイトのギヤードモータに取り替えることになります）。ダブルギアボックスは、その名の通り、左右のタイヤを独立して動かすことができます。
モータドライバとは
Raspberry PiでDCモータを動かすには、モータドライバが必要です。「Lチカと同じようにライズパイにモータを直接繋いで、PWM信号で動かせないの？」と思いますが、それではモータは動かせません。なぜなら、モータを動かすのに必要な電流は、LEDのそれと桁違いに大きいからです。また、Raspberry Piのデジタルピンから出力できる電流は数十mA程度です。DCモータでは数百mA以上の電流が必要になります。そこでモータドライバを使って、モータへ大電流を流せるようにする必要があるのです。 PWM信号について詳しくは、 オペアンプ1個でつくる！PWMジェネレータ をご覧ください。
モータドライバの動かし方
さて、ダブルギアボックスを動かすためにBD6211Fというモータドライバを使用しました。こちらは秋葉原の千石電商で購入したものです。モータドライバの扱いはどれも似ているので、手に入らない場合は別のモータドライバでも代用可能です。
今回、はじめてモータドライバを使ってみたのですが、思いのほか簡単でした。モータドライバの使い方を簡単に説明しますと、2つの入力FINとRINに信号を入力し、その信号によって正転、逆転、ブレーキ、ストップを制御できます。また、速度はPWM信号の電力によって変えることができます。次の対応表をご覧ください。
| FIN | RIN | 動作（OPERATION） |
|---|---|---|
| L | L | 空転 |
| PWM | L | 正転 |
| L | PWM | 逆転 |
| H | H | ブレーキ |
ただし、BD6211Fのデータシートで確認すると、モータドライバへ送るPWMの周波数は20kHz〜100kHz程度でなければならないようです。
gpiozeroのPWMLEDを使ってモータドライバを動かすことができます。その際に、FIN_L = PWMLED(20, frequency=100000)のようにして周波数を設定します。しかし、gpiozeroにはもっと便利なMotorというライブラリがありますので、ここではそちらを使うことにしました。Raspberry Piでモータを動かす
実際にモータとモータドライバをRaspberry Piにつないで、次のようなプログラムでモータを動かしてみました。
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()加速度センサ
Raspberry Piでモータ制御することができたので、いよいよ倒立振子の姿勢制御に挑戦していきます。先にも述べましたが、ロボットを倒立させるためには、加速度センサとジャイロセンサで姿勢を維持する必要があるそうです。モータドライバ同様、これらの知識がありませんでしたので、まずは加速度センサを試してみます。
加速度センサMMA8452Q
加速度センサMMA8452QとRaspberry PiはI2Cという通信方法でデータ送受信を行います。I2Cの設定方法は、 【Raspberry Pi】はじめてのI2C設定 をご覧ください。 ただし、Raspberry PiではI2Cで使うピン番号が決まってますので注意してください。今回使用した Raspberry Pi zero WH では、GPIO 2にSDA、GPIO 3にSCLをつなぎます。
またI2C以外にも、SPIという通信方法があります。詳しくは 【Raspberry Pi】はじめてのI2C設定 をご覧ください。MMA8452Q の使い方はArduino版ではありますが、 加速度センサで角度の計算｜ArduinoとMMA8452Q をご覧ください。
ここでは、加速度センサで角度を計算する方法について触れておきます。
加速度センサで角度の計算
加速度センサが図のような静止状態の時（実際にはありえませんが）、重力加速度aのみが働いてますので、角度θはセンサにかかる加速度\(a_x\)と\(a_z\)を使って次式で表すことができます。
$$tanθ=\frac{a_x}{a_z}$$
よって、姿勢角度θは、逆三角関数のアークタンジェントを使って次のように計算します。
$$θ=tan^{-1}\frac{a_x}{a_z}$$
詳しくは、 加速度センサで角度の計算｜ArduinoとMMA8452Q をご覧ください。
加速度センサで角度をログする
実際に加速度センサで角度データをログして、グラフ化してみました。
次のグラフは、加速度センサを机の上に静止させた時のものです。机が多少傾いているせいか、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回傾け、取得したデータにさまざまな係数でローパスフィルタをかけてみます。その結果が次のグラフです。
ローパスフィルタを強くかければかけるほど、滑らかになり安定はしますが、速い動きに追従できず、位相も遅れてしまいます。そのようすは 加速度センサで角度の計算｜ArduinoとMMA8452Q で視覚化してみました。
加速度センサだけで倒立振子は作れる？
さて、「姿勢角度が分かれば、加速度センサだけで倒立振子は作れるんじゃないか？」そう思った方は多いはず。しかしそれは、あくまで加速度センサが静止している時またはゆっくりと動いている時のことです。速い運動状態の時、つまり、重力加速度に比べて運動加速度が無視できない場合、加速度センサだけでは、重力加速度なのか運動加速度なのか判断できず、正確な角度を計算できません。
このように、運動状態での角度検出には、ジャイロセンサが向いてます。しかし、ジャイロセンサもまた完璧ではありません。後にくわしく説明しますが、徐々に角度に誤差が出てきてしまうのです。しかし、誤差の補正は加速度センサで行うことができます。よって、倒立振子のような姿勢を制御するロボットでは、ジャイロセンサと加速度センサを組み合わせて制御するのがふつうです。
倒立振子のバージョンアップ
また、モータドライバもBD6211Fから TA8428K へ変更しました。TA8428Kの使い方は 【Raspberry Pi】モータドライバ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の設定方法と姿勢の計算方法は、後述するメインプログラムや、 【Raspberry Pi】BMX055でジャイロ・加速度の測定 をご覧ください。
ジャイロセンサと加速度で角度計算・ドリフト除去
BMX055を使う上で、ジャイロセンサと加速度センサで得られる角度の向きが逆であるので注意してください。たとえば、加速度で計算した値が20度だとしたらジャイロでは、-20度です。ですから、ジャイロの角度結果に-1を掛けて帳尻を合わせることになります。
ところで次のグラフは、90度傾ける作業を繰り返したときのジャイロセンサの出力をグラフにしたものです。元の位置にもどっても0度にならず、徐々にずれていきます。この誤差のことを、ドリフト成分と呼びます。
ご覧の通り、ジャイロセンサは瞬間的な角度、つまり相対的な角度検出は得意ですが、ドリフト成分が含まれてしまうため、絶対的な角度検出には得意ではありません。
そこで加速度センサの出番です。加速度センサが静止している状態ならば、地球の重力加速度のみですので、絶対的な角度を知ることができます。これを基準に、ジャイロセンサと組み合わせれば、ドリフト補正しながら角度を検出できそうです。
では具体的にどうやるのか。
ジャイロセンサと加速度センサを使った角度計算する方法を調べてみますと、「カルマンフィルタ」と「相補フィルタ」というものに辿りつきます。「カルマンフィルタ」は簡単に理解できるものではなさそうです。後に Pythonでカルマンフィルタを使ってみた で試してみましたが、いまだに理論はわかってません(^_^;) ちなみに「カルマンフィルタ」は経済学の分野でも使われてます。
一方で、「相補フィルタ」は私にも理解できるいたってシンプルなアルゴリズムでした。しかも「カルマンフィルタ」と対して結果に違いがないというじゃないですか。だったら簡単な「相補フィルタ」を採用しない手はありません。次のように、「相補フィルタ」はたったの一行で書けてしまう数式でありプログラミングです。
$$angle_{i} = k (angle_{i-1} + xGyro dt)\\ + (1 - k) * angleAccel$$
先に述べました、指数移動平均のローパスフィルタとよく似てます。倒立振子の前後のバランスのみ考えれば良いので、ジャイロセンサでは一軸方向のxGyroを入力します。また、angleAccelはアークタンジェントを使って加速度センサから計算された角度です。
▼ 「相補フィルタ」を使ってk=0.9として実際に角度を取得してみました。さきほどと同様に、90度傾ける作業をくりかえしました。次のグラフがその結果です。
いかがでしょうか？ドリフト成分がキレイに除去されてますね！
ここまでで角度の検知が可能になりましたので、「一定の角度を超えたら前進または後進させることで倒立振子ロボットの姿勢を維持できるのではないだろうか？」そう思って実験してみました。その姿は、まるで生まれたての子鹿のような立ち方でした（笑）▼ そのようすを撮影しましたのでぜひご覧ください。
#2 生まれたての小鹿!?倒立振子(とうりつしんし)【Raspberry Pi】 - YouTube
ここまで来れば、あともうひといきですね！
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制御のおかげで動きはだいぶマシになりました。ただ、どうしても片側に寄ってしまうようです。
#3 倒立振子にPID制御を導入 【Raspberry Pi】 - YouTube
PID制御のプログラミング自体はとても簡単だったのですが、問題はパラメーターの調整でした。数日の間、PIDパラメーターの調整に時間を費やしました。おかげで、微分・積分成分がどのようにロボットの動きへ影響するかを体感できてよかったともいえるのですが。
プログラムの変数をなんども書き直しながらPIDパラメータを調整するのには限界があります。▼ そこで、TCP通信でiPhoneからパラメータ調整できるようにSwiftでアプリを作ってみました。パラメータ調整が俄然ラクになり、なんとか合格点を出せる動作になりました。
#5 倒立振子のPIDパラメーターをTCPでプリセット【Raspberry Pi】 - YouTube
#4 倒立振子(とうりつしんし)ロボット作ってみた!【Raspberry Pi】 - YouTube
倒立振子のプログラム (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 現在では、倒立振子の情報がたくさん公開されているので、ロボット製作の敷居がとても低くなってます。そんな先人たちの知恵に感謝を申し上げるとともに、私のブログもまたどなたかに役立ってくれればこれほど嬉しいことはありません。