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

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

夏休みの宿題のノリで倒立振子を作ってみた。Raspberry Piを使ってジャイロと加速度センサーからデータを読み取り、相補フィルターとPID制御で姿勢をコントロールしていく。

倒立振子(とうりつしんし)とは、二輪で自立するロボットだ。セグウェイなど、移動手段の実用的なロボットとして使われている。

今回、その倒立振子を作ってみたので、その過程を記録として残しておく。これから倒立振子の製作に挑戦する誰かの参考になれば幸いである。

倒立振子ロボット製作開始、千里の道もLチカから

倒立振子ロボットを製作するにあたって、メインコンピューターはRaspberry Pi zero WHを使った。

Raspberry Pi Zero W - ヘッダー ハンダ付け済み - ラズベリー・パイ ゼロ W ワイヤレス
Raspberry Pi Zero W - ヘッダー ハンダ付け済み - ラズベリー・パイ ゼロ W ワイヤレス

Amazon

ラズパイの操作は、MacのターミナルからSSHで行った。ロボットのプログラムはPython2.xで動かしている。またラズパイの基本的なセットアップは済んでいるものとし、ここでは説明を省略する。

さて、いきなりモーターを動かしてみたいところだが、それを実現するための予備知識が全然ないため、まずはとても小さな一歩であるLチカから始めてみる。

GPIOの制御には、gpiozeroライブラリを使う。Python2のgpiozeroをインストールするには次の通り。

$ sudo apt install python-gpiozero

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

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)

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

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 = np.sin(t-np.pi/2) / 2.0 + 0.5 
#    print(a)
    led.value = a
    sleep(0.002)

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

PWM制御をわかりやすく解説してみたので参考に

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

ここではモータードライバ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モジュールを使ってもよいだろう。

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()

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

タミヤ 楽しい工作シリーズ No.168 ダブルギヤボックス 左右独立4速タイプ (70168)
タミヤ 楽しい工作シリーズ No.168 ダブルギヤボックス 左右独立4速タイプ (70168)

2つのモーターのパワーを別々に取り出すことができる組み立て式のギヤボックスです。 低速から高速まで4種類のギヤ比が選べ、左右でギヤ比が変えられる独立構造を採用しました。 ケースやギヤはプラスチック製、シャフトは丈夫な金属製。はめ込みやネジ止めでやさしく組み立てられ、なめらかな動きを生み出します。

Amazon

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

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

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

センサーとの通信はI2Cで行う。I2Cの設定は以前の記事に書いたので、ここでは説明を省略する。

ただし注意点が1つある。ラズベリーパイゼロでI2C通信するときのピン番号に注意が必要だ。GPIO番号の2番にSDA、3番にSCLを接続する。

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

次のグラフは加速度センサーを3分間机の上に静止させた時のログである。机がすでに傾いているため0度中心にはなっていないが、-1.5度あたりを中心として誤差±0.5度くらいの精度で取得できている。また、角度はarcsinやarctan2で求められる。


画像の拡大

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


画像の拡大

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

加速度センサーで垂直かどうかを判定し、モーターの正転逆転だけで制御をやってみた。倒立振子にはほど遠いような反応の遅い動き。タイヤも細くて姿勢が安定しなさそうで心細い。

倒立振子のアップデート


画像の拡大

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

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

TOSHIBA(東芝) DC モータ用 フルブリッジドライバ 7V〜27V 1.5A TA8428K
TOSHIBA(東芝) DC モータ用 フルブリッジドライバ 7V〜27V 1.5A TA8428K

正転・逆転・ストップ・ブレーキの4モードがコントロール可能、 逆起電力吸収用ダイオード・熱遮断、過電流保護回路を内蔵、 出力電流:1.5A(AVE)、3.0A(PEAK)

Amazon
ロボカップジュニア ロボサイトギヤモータ30:1(RA250030-58Y91) ダイセン
ロボカップジュニア ロボサイトギヤモータ30:1(RA250030-58Y91) ダイセン

ギヤ比 30:1、使用電源 DC3V~6V、無負荷電流 0.15A、 モータ回転数 7000rpm、モータ起動トルク 174g・cm、 全長 73mm、最大径 Φ27.5mm、シャフト長 15mm、先端6mm M3、シャフト径 Φ4mm

Amazon

DCモータの制御方法はこちらの記事を参考に

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

さて、この頃には幾度となくプログラムが停止する問題に直面していた。下記のエラーが頻繁に出ている状況。どうもモーターの回転方向が変わるときに高確率でI2C通信エラーになるようだ。

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

ラズパイ電源部に100μのコンデンサをつけて電源の出力インピーダンスを低くしてみたが変化なし。そもそもモーターの電源とラズパイの電源は別にしてあるので関係はなさそう。


画像の拡大

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

try:
except IOError as e:

しかし、モーターの転回時にエラーは出ているので根本的な問題は解決はされていない。これはモーターの逆誘導起電力によるノイズかもしれないと思った。オシロスコープで調べてみるとやはり転回時にかなり波形が乱れる。これらのノイズがきっとラズパイに影響を与えてるに違いない。

モーターノイズ対策で調べるとやはり皆さん、パスコンを入れて対処しているようだ。さっそくモーターには0.1uFのバイパスコンデンサを入れる。写真のように3個のコンデンサをつけてみた。端子間に1つ、それぞれの端子とシャーシの間にコンデンサを入れた。ずばり、I2Cの通信エラーがなくなった。


画像の拡大

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

ジャイロセンサーと加速度センサーが一体になっているBMX055センサーを秋月電子で購入した。細かな設定や計算はメインプログラム、または下記の記事を参照してもらいたい。

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

さて、次のグラフはジャイロセンサーのみを使用して90度傾ける作業を繰り返したときのログ。元の位置に戻しても0度にはならず、ドリフト成分が含まれている。


画像の拡大

ジャイロセンサーは瞬間的な角度検出には向いているが、絶対値的な角度検出には向いていない。先ほどのドリフト成分が含まれるからだ。

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

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

調べていくと、カルマンフィルターと相補フィルターにたどり着いた。カルマンフィルターは簡単に理解できるものではなさそう。一方で倒立振子において、相補フィルターはカルマンフィルターと結果に大した違いがないにもかかわらず、たったの一行で書ける数式でありプログラミング。今回はこちらの簡単そうな相補フィルターを採用した。

$$angle = k * (angle + xGyro * dt)\\ + (1 - k) * angleAccel$$

k=0.9としてジャイロセンサーと加速度センサーを使って角度を検知したものが次のグラフだ。先ほどのグラフと同様、90度傾ける作業を繰り返したログだ。ジャイロセンサーで見られたドリフト成分が見事に除去されている。


画像の拡大

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

PID制御

先ほどの角度を基準に前後で制御するには限界があった。この頃、友人からの知らせでPID制御の存在を知った。PID制御は古典的手法のようだが、今でも世の中の制御のいろいろな場面で使われている。倒立振子でもPIDで制御が主流のようだ。

プログラミング的には全然難しいものではなく、簡単に書ける。制御工学の知識がなくても巷のプログラムをコピペすれば簡単に制御できてしまうだろう。しかしそれではつまらない。せっかくだからPID制御の理論を少しでも詳しく知りたいと思う。そこで次のマンガでわかるシリーズの制御工学をAmazonで購入してみた。

Pythonによる制御工学入門
Pythonによる制御工学入門

本書は、Pythonを使って制御工学を行うための入門書です。機械学習やデータマイニングで多用され、さらにその枠を越えて主流のプログラミング言語となりつつあるPythonを制御系設計に導入したい人向けに、Pythonプログラムを実行しながら「使ってみる、やってみる」を通して、制御工学を体感することができる書籍です。

KindleAmazon

書籍はだいぶ噛み砕いてわかりやすく説明してくれるのでありがたい。とはいえ制御工学自体が物理と数学をフルに使う分野だけにそれなりに理解するのには苦労する。すべて理解しようと思うとフーリエ変換を理解するより大変だ。

前半の伝達関数モデルまではたいへん面白かった。ラプラス変換そのものは理解できなくても、機械運動や電子回路の微分方程式を、シンプルな数式で置き換えるやり方に大変興奮した。

ラプラス変換の使い方を解説してみたのでよかったら参考に

しかしその後の状態空間モデルになると行列などでてきて「何これ死ぬの?」と言わざるを得ないくらい難解だ。

それでも制御工学の本を読んでおいてよかったのは、制御工学の全体像をつかめたからだ。

線形や非線形、システム、入力出力、uやy、直列結合、並列結合、フィードバック、ステップ応答、1次遅れ系、2次遅れ系など用語が独特でわかりづらかったが、本を読んだおかげで理解できた。

話は戻ってPIDは「Proportional-Integral-Differential Controller」の略である。

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

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

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

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

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

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

メインプログラム (Python)

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

#!/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)

とあるきっかけで夏休みの宿題感覚ではじめた倒立振子ロボットの製作。当初は一ヶ月くらいを見積もっていたが10日ほどで制作できてしまった。これもひとえに、諸先輩方たちが残したウェブログのおかげだと思っている。倒立振子を作る上で、今ではネット上でたくさんの情報が簡単に手に入るからだ。私のブログもまた誰かの参考になれば幸いである。

倒立振子のプログラムをGitHubへアップしたので、興味ある方は参考に。

この記事で使った関連製品はこちら

Raspberry Pi Zero W - ヘッダー ハンダ付け済み - ラズベリー・パイ ゼロ W ワイヤレス
Raspberry Pi Zero W - ヘッダー ハンダ付け済み - ラズベリー・パイ ゼロ W ワイヤレス

Amazon
BODYA CJMCU-055 BMX055 9DoF IMU高精度9軸姿勢センサーモジュール
BODYA CJMCU-055 BMX055 9DoF IMU高精度9軸姿勢センサーモジュール

Amazon
Pythonによる制御工学入門
Pythonによる制御工学入門

本書は、Pythonを使って制御工学を行うための入門書です。機械学習やデータマイニングで多用され、さらにその枠を越えて主流のプログラミング言語となりつつあるPythonを制御系設計に導入したい人向けに、Pythonプログラムを実行しながら「使ってみる、やってみる」を通して、制御工学を体感することができる書籍です。

KindleAmazon

Raspberry Piの参考書

Raspberry Pi クックブック 第2版 (Make:PROJECTS)
Raspberry Pi クックブック 第2版 (Make:PROJECTS)

本書は、全世界で多くのユーザーの支持を集めているマイコンボード「Raspberry Pi」を使いこなすためのレシピ集です。ハードウェアの基本、オペレーティングシステムの使い方、ネットワーク接続、Pythonプログラミングの基本を紹介した上デ、実際の作品製作に必要になる、高度なPythonプログラミング、GPIO(汎用入出力)、モーター、センサー、ディスプレイなどの使い方へと解説を進めていきます。

Amazon
写真や図解でよくわかる ラズパイZeroを使い倒す本 Raspberry Pi Zero/Zero W対応
写真や図解でよくわかる ラズパイZeroを使い倒す本 Raspberry Pi Zero/Zero W対応

本書ではRaspberry Pi Zero / Zero Wの概要から必要な周辺機器の説明、OSの導入やセットアップなどといった準備、そしてLinuxに初めて触れる人に向けてLinuxの基礎やシェルの操作などを解説しています。また、準備が整ったら実際に電子部品をRaspberry Pi Zero / Zero Wで制御する方法も解説しました。

KindleAmazon

参考サイト

Amazonでお得に購入するなら、Amazonギフト券がオススメ!

コンビニ・ATM・ネットバンキングで¥5,000以上チャージすると、プライム会員は最大2.5%ポイント、通常会員は最大2%ポイントがもらえます!
Amazonギフト券

あなたにおすすめ