トランジスタ技術 2020年9月号

Arduino×Pythonで動かしながら学ぶモータ制御入門

Y.Minami

初期設定

ライブラリの読み込みとグラフの設定

from control.matlab import *
import matplotlib.pyplot as plt
import numpy as np
import serial

plt.rcParams['font.family'] ='sans-serif' #使用するフォント
plt.rcParams['xtick.direction'] = 'in' #x軸の目盛線が内向き('in')か外向き('out')か双方向か('inout')
plt.rcParams['ytick.direction'] = 'in' #y軸の目盛線が内向き('in')か外向き('out')か双方向か('inout')
plt.rcParams['xtick.major.width'] = 1.0 #x軸主目盛り線の線幅
plt.rcParams['ytick.major.width'] = 1.0 #y軸主目盛り線の線幅
plt.rcParams['font.size'] = 10 #フォントの大きさ
plt.rcParams['axes.linewidth'] = 1.0 # 軸の線幅edge linewidth。囲みの太さ
plt.rcParams['mathtext.default'] = 'regular'
plt.rcParams['axes.xmargin'] = '0' #'.05'
plt.rcParams['axes.ymargin'] = '0.05'
plt.rcParams['savefig.facecolor'] = 'None'
plt.rcParams['savefig.edgecolor'] = 'None'

グラフを整える関数

def plot_set(fig_ax, *args):
    fig_ax.set_xlabel(args[0])
    fig_ax.set_ylabel(args[1])
    fig_ax.grid(ls=':')
    if len(args)==3:
        fig_ax.legend(loc=args[2])

• 実験環境を構築してください
• Arduinoスケッチ（リスト１）を書き込んでください
• 準備ができたら以下のコードを実行してください．

入門１：モータのON/OFF制御

リスト２

y = []
u = []
t = []

# --- 初期設定 --------------------------
ang_ref = 30 # 角度の目標値 [deg]
Tend = 3   # 実行時間 [s]
uref = 2   # 指令電圧 [V]
ang = 0
# --- 初期設定（ここまで） ----------------

with serial.Serial('/dev/cu.usbmodem14201', 115200, timeout=1) as ser:
        
    while ser.isOpen() == True:

        # --- 制御入力の計算 --------------------------
        uref = 0
        if ang_ref - ang > 0:
            uref = 2
        if ang_ref - ang < 0:
            uref = -2
        # --- 制御入力の計算（ここまで） ----------------
        
        # Arduinoへデータを送信
        msg = str(uref) + 'e'
        ser.write(msg.encode()) 
        ser.flush()
    
        # Arduinoからデータを受信
        buf = ser.readline().decode('utf8').strip()
        line = buf.split()
        if len(line) > 2:
            ang = float(line[1])
            t = np.append(t, float(line[0]))
            y = np.append(y, float(line[1]))
            u = np.append(u, float(line[2]))
        
            if float(line[0]) >= Tend:
                break

    ser.close()

fig, ax = plt.subplots(1,2, figsize=(6, 2.5))
ax[0].plot(t, y, c='k')
ax[0].axhline(ang_ref, color="k", linewidth=0.5) 

plot_set(ax[0], 'time [s]', 'angle [deg]')
ax[1].plot(t, u, c='k') # duty比 [%]
plot_set(ax[1], 'time [s]', 'input [%]')
fig.tight_layout()

#fig.savefig("onoffcont.pdf", transparent=True, bbox_inches="tight", pad_inches=0.0)

入門２：モータ角度のP制御

リスト３

y = []
u = []
t = []

# --- 初期設定 --------------------------
ang_ref = 30 # 角度の目標値 [deg]
Tend = 3   # 実行時間 [s]
uref = 2   # 指令電圧 [V]
ang = 0
kP = 0.06   # Pゲイン（入力の最大値の見積もり：(30-0)*0.12 = 3.6V）
# --- 初期設定（ここまで） ----------------

with serial.Serial('/dev/cu.usbmodem14201', 115200, timeout=1) as ser:
        
    while ser.isOpen() == True:

        # --- 制御入力の計算 --------------------------
        uref = kP * (ang_ref - ang)
        # --- 制御入力の計算ここまで -------------------
        
        # Arduinoへデータを送信
        msg = str(uref) + 'e'
        ser.write(msg.encode()) 
        ser.flush()
    
        # Arduinoからデータを受信
        buf = ser.readline().decode('utf8').strip()
        line = buf.split()
        if len(line) > 2:
            ang = float(line[1])
            t = np.append(t, float(line[0]))
            y = np.append(y, float(line[1]))
            u = np.append(u, float(line[2]))
        
            if float(line[0]) >= Tend:
                break

    ser.close()

fig, ax = plt.subplots(1,2, figsize=(6, 2.5))
ax[0].plot(t, y, c='k')
ax[0].axhline(ang_ref, color="k", linewidth=0.5)

plot_set(ax[0], 'time [s]', 'angle [deg]')
ax[1].plot(t, u, c='k') # duty比 [%]
plot_set(ax[1], 'time [s]', 'input [%]')
fig.tight_layout()

#fig.savefig("pcont.pdf", transparent=True, bbox_inches="tight", pad_inches=0.0)

PID制御

擬似微分器の離散化

リスト4

Tlp = 0.02
Plp = tf([1, 0], [Tlp, 1])
Plp_d = c2d(Plp, 0.01)
print(Plp_d)

50 z - 50
----------
z - 0.6065

dt = 0.01

ステップ応答法によるゲインチューニング

リスト6（一定電圧を印加する）

y = []
u = []
t = []

# --- 初期設定 --------------------------
Tend = 1   # 実行時間 [s]
uref = 4   # 指令電圧 [V]
# --- 初期設定（ここまで） ----------------

with serial.Serial('/dev/cu.usbmodem14201', 115200, timeout=1) as ser:
        
    while ser.isOpen() == True:

        # Arduinoへデータを送信
        msg = str(uref) + 'e'
        ser.write(msg.encode()) 
        ser.flush()
    
        # Arduinoからデータを受信
        buf = ser.readline().decode('utf8').strip()
        line = buf.split()
        if len(line) > 2:
            ang = float(line[1])
            t = np.append(t, float(line[0]))
            y = np.append(y, float(line[1]))
            u = np.append(u, float(line[2]))
        
            if float(line[0]) >= Tend:
                break

    ser.close()

fig, ax = plt.subplots(figsize=(3, 2.3))
ax.plot(t, y, c='k')
plot_set(ax, 'time [s]', 'angle [deg]')
ax.set_ylim(-150, 680)
#fig.savefig("step_angle.pdf", transparent=True, bbox_inches="tight", pad_inches=0.0)

(-150, 680)

リスト7（ステップ応答法）

yslice = y[20:50]
tslice = t[20:50]

fig, ax = plt.subplots(figsize=(3, 2.3))
ax.plot(t, y)
ax.plot(t, np.poly1d(np.polyfit(tslice, yslice, 1))(t))
plot_set(ax, 'time [s]', 'angle [deg]')
#fig.savefig("fitting.pdf", transparent=True, bbox_inches="tight", pad_inches=0.0)

p = np.polyfit(tslice, yslice, 1)
R = p[0]/uref
L = -p[1]/p[0]
print(R,L)

kP = 1.2/R/L
kI = kP/2/L
kD = 0.5*L*kP
print(kP, kI, kD)

186.22914349276974 0.08998150858121287
0.07161109757382672 0.39792118793604175 0.0032218372954246808

リスト8（PI-D制御）

y = []
u = []
t = []

# --- 初期設定 --------------------------
ang_ref = 30 # 角度の目標値 [deg]
Tend = 3   # 実行時間 [s]
uref = 2   # 指令電圧 [V]
Ts = 0.01  # サンプリング周期 [s]
ang = 0
ang_old = 0
dang_old = 0
error_itg = 0

# ステップ応答法
kP = 1.2/R/L
kI = kP/2/L
kD = 0.5*L*kP
# --- 初期設定（ここまで） ----------------


with serial.Serial('/dev/cu.usbmodem14201', 115200, timeout=1) as ser:
        
    while ser.isOpen() == True:

      
        # --- 制御入力の計算 --------------------------
        
        # 偏差の計算
        error = ang_ref - ang
        # 微分値の計算（擬似微分）：リスト５ 
        dang = 50.0 * (ang - ang_old) + 0.6065 * dang_old;  # [deg/s]
        dang_old = dang
        ang_old = ang
        # 積分値の計算
        error_itg += Ts * error
        # 制御入力（PI-D制御）
        uref = kP * error + kI * error_itg - kD * dang
        # --- 制御入力の計算ここまで -------------------
        
        # Arduinoへデータを送信
        msg = str(uref) + 'e'
        ser.write(msg.encode()) 
        ser.flush()
    
        # Arduinoからデータを受信
        buf = ser.readline().decode('utf8').strip()
        line = buf.split()
        if len(line) > 2:
            ang = float(line[1])
            t = np.append(t, float(line[0]))
            y = np.append(y, float(line[1]))
            u = np.append(u, float(line[2]))
        
            if float(line[0]) >= Tend:
                break

    ser.close()

fig, ax = plt.subplots(1,2, figsize=(6, 2.5))
ax[0].plot(t, y, c='k')
ax[0].axhline(ang_ref, color="k", linewidth=0.5) 

plot_set(ax[0], 'time [s]', 'angle [deg]')
ax[1].plot(t, u, c='k') # duty比 [%]
plot_set(ax[1], 'time [s]', 'input [%]')
fig.tight_layout()

#fig.savefig("pidcont_step.pdf", transparent=True, bbox_inches="tight", pad_inches=0.0)

入門４：モデルに基づく制御系設計

モデリング

リスト6 をもう一度実行

y = []
u = []
t = []

# --- 初期設定 --------------------------
Tend = 1   # 実行時間 [s]
uref = 4   # 指令電圧 [V]
# --- 初期設定（ここまで） ----------------

with serial.Serial('/dev/cu.usbmodem14201', 115200, timeout=1) as ser:
        
    while ser.isOpen() == True:

        # Arduinoへデータを送信
        msg = str(uref) + 'e'
        ser.write(msg.encode()) 
        ser.flush()
    
        # Arduinoからデータを受信
        buf = ser.readline().decode('utf8').strip()
        line = buf.split()
        if len(line) > 2:
            ang = float(line[1])
            t = np.append(t, float(line[0]))
            y = np.append(y, float(line[1]))
            u = np.append(u, float(line[2]))
        
            if float(line[0]) >= Tend:
                break

    ser.close()

リスト9（中心差分近似）

N = len(y)-1
dy = (-3*y[0] + 4*y[1] - 3*y[2])/(2*0.01)
for i in range(1, N, 1):
    dy = np.append(dy, (y[i+1]-y[i-1])/(2*0.01))
dy = np.append(dy, (y[N-2]-4*y[N-1]+3*y[N])/(2*0.01) )

#Pmd = tf([ 0, b ], [ 1, a ])
#dysim, _ = step(Pmd, np.arange(0,1,0.01) )

fig, ax = plt.subplots(figsize=(3, 2.3))
ax.plot(t, dy, label='experiment', c='k')
# ax.plot(t, dysim*uref, label='simulation')
#plot_set(ax, 'time [s]', 'velocity [deg/s]')
#fig.savefig("model_velocity.pdf", transparent=True, bbox_inches="tight", pad_inches=0.0)

[<matplotlib.lines.Line2D at 0x7fd8ca50e190>]

リスト10（数式処理）

import sympy as sp
sp.init_printing()
s = sp.Symbol('s')
a = sp.Symbol('a', real=True)
b = sp.Symbol('b', real=True)
t = sp.Symbol('t', positive=True)
G = sp.apart(b/((s+a)*s**2), s)
G

$\displaystyle \frac{b}{a s^{2}} + \frac{b}{a^{2} \left(a + s\right)} - \frac{b}{a^{2} s}$

t = sp.Symbol('t', positive=True)
sp.inverse_laplace_transform(G, s, t)

$\displaystyle \frac{b t}{a} - \frac{b}{a^{2}} + \frac{b e^{- a t}}{a^{2}}$

リスト11（モデリング）

リスト6,7 の実行結果を利用します

Km = p[0]/uref
Tm = -p[1]/p[0]
# a = 1/Tm
# b = Km/Tm

a = -p[0]/p[1]
b = -p[0]**2/p[1]/uref
print(a,b)

Pm = tf([0, b], [1, a, 0])
print(Pm)

11.113394471459092 2069.637933717109

    2070
-------------
s^2 + 11.11 s

リスト12（I-PD制御）

y = []
u = []
t = []

# --- 初期設定 --------------------------
ang_ref = 30 # 角度の目標値 [deg]
Tend = 3   # 実行時間 [s]
uref = 2   # 指令電圧 [V]
Ts = 0.01  # サンプリング周期 [s]
ang = 0
ang_old = 0
dang_old = 0
error_itg = 0

# モデルマッチング 
a = 12
b = 2086
omega_n = 10
alpha1 = 2
alpha2 = 2
kP = alpha1*omega_n**2/b
kI = omega_n**3/b
kD = (alpha2*omega_n-a)/b
# --- 初期設定（ここまで） ----------------


with serial.Serial('/dev/cu.usbmodem14201', 115200, timeout=1) as ser:
        
    while ser.isOpen() == True:

        # --- 制御入力の計算 --------------------------
        
        # 偏差の計算
        error = ang_ref - ang
        # 微分値の計算（不完全微分） 
        dang = 50.0 * (ang - ang_old) + 0.6065 * dang_old;  # [deg/s]
        dang_old = dang
        ang_old = ang
        # 積分値の計算
        error_itg += Ts * error
        # 制御入力（I-PD制御）
        uref = - kP * ang + kI * error_itg - kD * dang
        # --- 制御入力の計算ここまで -------------------
        
        # Arduinoへデータを送信
        msg = str(uref) + 'e'
        ser.write(msg.encode()) 
        ser.flush()
    
        # Arduinoからデータを受信
        buf = ser.readline().decode('utf8').strip()
        line = buf.split()
        if len(line) > 2:
            ang = float(line[1])
            t = np.append(t, float(line[0]))
            y = np.append(y, float(line[1]))
            u = np.append(u, float(line[2]))
        
            if float(line[0]) >= Tend:
                break

    ser.close()

print(kP, kI, kD)

0.09587727708533078 0.4793863854266539 0.003835091083413231

M = tf(omega_n**3, [1, alpha2*omega_n, alpha1*omega_n**2, omega_n**3])
ym, tm = step(M, np.arange(0, Tend, Ts))

fig, ax = plt.subplots(1,2, figsize=(6, 2.5))
ax[0].plot(t, y, c='k')
ax[0].plot(tm, ym*ang_ref, c='k', ls='--')
ax[0].axhline(ang_ref, color="k", linewidth=0.5) 

plot_set(ax[0], 'time [s]', 'angle [deg]')
ax[1].plot(t, u, c='k') # duty比 [%]
plot_set(ax[1], 'time [s]', 'input [%]')
fig.tight_layout()

#fig.savefig("ipdcont_modelmatch.pdf", transparent=True, bbox_inches="tight", pad_inches=0.0)

入門５：状態空間モデルに基づく制御系設計

リスト14

a = 12
b = 2086

A = [ [0, 1], [0, -a]]
B = [ [0], [b] ]
C = [1, 0]
D = [0]
Pmss = ss(A, B, C, D)
print(Pmss)

A = [[  0.   1.]
 [  0. -12.]]

B = [[   0.]
 [2086.]]

C = [[1. 0.]]

D = [[0.]]

リスト15（拡大系の構築と極配置による設計）

Abar = np.r_[ np.c_[Pmss.A, np.zeros((2,1))], -np.c_[ Pmss.C, 0 ] ]
Bbar = np.c_[ Pmss.B.T, 0 ].T
Cbar = np.c_[ Pmss.C, 0 ]

Pole = [-7+3j, -7-3j, -10]
F = -acker(Abar, Bbar, Pole)
F

#Q = [ [1, 0, 0], [0, 0.1, 0], [0, 0, 1]]
#R = 50
#F, X, E = lqr(Abar, Bbar, Q, R)
#F = -F
print(F)

[[-0.0949185  -0.00575264  0.2780441 ]]

リスト16（積分型サーボ）

y = []
u = []
t = []

# --- 初期設定 --------------------------
ang_ref = 30 # 角度の目標値 [deg]
Tend = 3   # 実行時間 [s]
uref = 2   # 指令電圧 [V]
Ts = 0.01  # サンプリング周期 [s]
ang = 0
ang_old = 0
dang_old = 0
error_itg = 0
# --- 初期設定（ここまで） ----------------


with serial.Serial('/dev/cu.usbmodem14201', 115200, timeout=1) as ser:
        
    while ser.isOpen() == True:

        # --- 制御入力の計算 --------------------------
        
        # 微分値の計算（不完全微分） 
        dang = 50.0 * (ang - ang_old) + 0.6065 * dang_old;  # [deg/s]
        dang_old = dang
        ang_old = ang
  
        # 積分値の計算
        error_itg += Ts * (ang_ref - ang)
        
        # 制御入力（積分型サーボ系）
        uref =  F[0,0]*ang  + F[0,2] * error_itg + F[0,1] * dang
        # --- 制御入力の計算ここまで -------------------
        
        # Arduinoへデータを送信
        msg = str(uref) + 'e'
        ser.write(msg.encode()) 
        ser.flush()
    
        # Arduinoからデータを受信
        buf = ser.readline().decode('utf8').strip()
        line = buf.split()
        if len(line) > 2:
            ang = float(line[1])
            t = np.append(t, float(line[0]))
            y = np.append(y, float(line[1]))
            u = np.append(u, float(line[2]))
        
            if float(line[0]) >= Tend:
                break

    ser.close()

print(F)

[[-0.0949185  -0.00575264  0.2780441 ]]

fig, ax = plt.subplots(1,2, figsize=(6, 2.5))
ax[0].plot(t, y, c='k')
ax[0].axhline(ang_ref, color="k", linewidth=0.5) 
plot_set(ax[0], 'time [s]', 'angle [deg]')
ax[1].plot(t, u, c='k') # duty比 [%]
plot_set(ax[1], 'time [s]', 'input [%]')
fig.tight_layout()
#
#fig.savefig("exp_servo.pdf", transparent=True, bbox_inches="tight", pad_inches=0.0)