倒立振子ロボット製作記～ソフトウェア編～

ハードウェア編はこちら

まずは動作させているところの動画です。

pic.twitter.com/uPIDVM1jyy June 15, 2020

まだPID制御のパラメータの追い込みや角度の補正が甘そうですが、一応立ちました。
10日で作る！倒立振子ロボット
↑こちらのコードをほぼそのまま使わせてもらっている感じですが、以下にソースコードを載せます。
（動けばいいという感じで書いてまして、ツギハギの変なコードになっていると思いますがご容赦ください。精進します）

#!/usr/bin/env python
# -*- coding: utf-8 -*-
import numpy as np
import time
from datetime import datetime
import smbus
import RPi.GPIO as GPIO
import pigpio

GPIO.setwarnings(False) 
GPIO.setmode(GPIO.BCM)
GPIO.setup(17, GPIO.OUT)
GPIO.setup(27, GPIO.OUT)

gpio_pin0 = 12
gpio_pin1 = 13

pi = pigpio.pi()
pi.set_mode(gpio_pin0, pigpio.OUTPUT)


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

if __name__ == '__main__':

    lastErr = 0
    errSum = 0
    angle = 0
    totaltime = 0
    sigmaangle = 0

    count = 0 #転倒検出カウンタ変数
    test = 1 #モーター動作フラグ 1:動作 0:非動作

    #PIDゲイン
    Kp = 1500
    Ki = 2500
    Kd = 60

    K = 0.996 #相補フィルタのパラメータ

    freq = 800 #PWM周波数
    sleep_time = 0.01
    
    #モーター回転数の調整
    adj_b = 0.85
    adj_f = 1

    preTime = time.time()
    
    #加速度センサからの角度演算値のオフセット補正
    #1000回分のオフセット平均値を取得
    for i in range(1000):
        xAc,yAc,zAc = accl()              
        angAccl = (np.arctan2(zAc, xAc) * 180 / 3.141592) 
        sigmaangle += angAccl

    avg_angleAccl = sigmaangle/1000
    print("avg_angleAccl:" + str(avg_angleAccl))

    try:
        while count < 30 : #転倒検出カウンタが30溜まったら終了する
            xAcc,yAcc,zAcc = accl()
            xGyro,yGyro,zGyro = gyro()
            
            now = time.time()
            dt = (now - preTime)
            preTime = now
            totaltime += dt
            
            angleAccl = np.arctan2(zAcc, xAcc) * 180 / 3.141592 #加速度センサによる角度演算値
            angleAccl2 = angleAccl - avg_angleAccl #オフセット補正
            x_deg = xGyro * -0.003815 * dt #角速度から角度への変換
            angle = K * (angle + x_deg) + (1 - K) * angleAccl2 #相補フィルタ
            
            # PID制御
            # Proportional=比例、Integral=積分、Differential=微分
            error = (angle) / 90   # P成分
            errSum += error * dt  # I成分
            dErr = (error - lastErr) / dt  # D成分
            u = Kp * error + Ki * errSum + Kd * dErr

            lastErr = error

            if u < 0:
                duty = int(abs(u)*adj_b)
            else:
                duty = int(abs(u)*adj_f)
                
            if duty > 99:
                duty = 99

            #pdutyはデバッグ用
            if angle < 0:
                pduty = duty * (-1)
            else:
                pduty = duty
                
            print("angle:" + str(angle) +"  u:" + str(u) + "  pduty:" + str(pduty))

            #転倒検出カウンタ 角度30度以上でカウントアップ
            if abs(angle) >= 30:
                count +=1

            #モーター制御
            if test == 1:
                if u < -0.1 :
                    GPIO.output(17, GPIO.HIGH)
                    GPIO.output(27, GPIO.HIGH)
                    pi.hardware_PWM(gpio_pin0, freq, duty*10000)
                    pi.hardware_PWM(gpio_pin1, freq, duty*10000)
                    time.sleep(sleep_time)

                elif u > 0.1 :
                    GPIO.output(17, GPIO.LOW)
                    GPIO.output(27, GPIO.LOW)
                    pi.hardware_PWM(gpio_pin0, freq, duty*10000)
                    pi.hardware_PWM(gpio_pin1, freq, duty*10000)

                else:
                    pi.hardware_PWM(gpio_pin0, freq, 000000)
                    pi.hardware_PWM(gpio_pin1, freq, 000000)
            
            
            time.sleep(0.01)
            
        #転倒検出時にモーターを止めるため↓
        pi.hardware_PWM(gpio_pin0, freq, 000000)
        pi.hardware_PWM(gpio_pin1, freq, 000000)

    except KeyboardInterrupt:
        pi.hardware_PWM(gpio_pin0, freq, 000000)
        pi.hardware_PWM(gpio_pin1, freq, 000000)

##工夫した？ところ
①加速度から演算する角度にオフセットが発生してしまうようで、最初にしばらく(1000回分)加速度センサから角度を計算させて、平均化した値をオフセット値として差し引くようにしました。

② ①のデバッグをやる時に、センサーから取得した値をリアルタイムにグラフ化するということをやってました。で、グラフ化させながらモーターも動かして調整を始めたんですが、どうもモーターの反応が遅い。
色々試しているうちに気付いたのが、グラフを描画する処理によって制御に遅延が発生してしまっていたようです。なので途中でグラフ化はやめました。
ちなみにグラフ化するやつは以下のように動きます。

pic.twitter.com/aA5RmnJQvi June 3, 2020

最初は上で貼ったコードにもグラフ化コードを入れてたんですが、なぜかうまく動かなくなってしまったので消しました。このページの最後に参考までに貼っておきます。

③失敗するとモーターがガンガン回ってすぐに電池がなくなるので、転倒したらモーターが停止するようにしました。大した工夫ではないですね。

以上。
本当はカルマンフィルタを使ってみたかったですが、アルゴリズムが理解できなかったので、とりあえず簡単そうな相補フィルタを使いました。
PIDのパラメータ調整は試行錯誤でしたが、自分的にはI成分を強めるというのがポイントでした。
こちらの作例が参考になりました。
M5StickCはMadgwickフィルタで倒立振子の夢を見るか？

##その他参考にしたサイト
半日で作る倒立振子
倒立振子再チャレンジ（PID制御をExcelでシミュレーション）
ジャイロのドリフト補正と比較（カルマン、相補フィルター）

##おまけ
リアルタイムグラフ化のコード。
中途半端な感じで申し訳ないですが、以下のような感じで使ってください。

#whileループの前に以下を挿入
#4本分のグラフ用配列確保
y1 = np.zeros(1000)
y2 = np.zeros(1000)
y3 = np.zeros(1000)
y4 = np.zeros(1000)
t = np.zeros(1000)
plt.ion()

#whileループの中に以下を挿入(plt.cla()まで)
 t = np.append(t,totaltime)
 t = np.delete(t,0)
 y1 = np.append(y1,float(y1)) #(y1)にはグラフ化したい変数名を入れる
 y1 = np.delete(y1,0)
 y2 = np.append(y2,float(y2))
 y2 = np.delete(y2,0)
 y3 = np.append(y3,float(y3))
 y3 = np.delete(y3,0)
 y4 = np.append(y4,float(y4))
 y4 = np.delete(y4,0)
    
 # グラフ表示設定
 line, = plt.plot(t, y1, 'r-',label="y1")
 line, = plt.plot(t, y2, 'b-',label="y2")
 line, = plt.plot(t, y3, 'g-',label="y3")
 line, = plt.plot(t, y4, 'm-',label="y4")
            
 line.set_ydata(y1)
 line.set_ydata(y2)
 line.set_ydata(y3)
 line.set_ydata(y4)
            
 #plt.title("Real-time inclination angle")
 plt.xlabel("Time [s]")
 plt.ylabel("angle [deg]")
 plt.legend();plt.grid()
 plt.xlim([1,100]); plt.ylim([-100,100])
 plt.pause(0.01)
 plt.cla()