バネマスダンパ系のPIDシミュレーション

はじめに

前回記事のプログラムを改良してPIDシミュレーションしてみた negizoku.hatenablog.jp

この方程式をちょっと改造してPID制御を組み込む

\displaystyle \frac{d}{dt} \begin{pmatrix} x \\v \\ \end{pmatrix} = \begin{pmatrix} 0 & 1\\ -c/m & -k/m \\ \end{pmatrix} \begin{pmatrix} x \\v \\ \end{pmatrix} + \begin{pmatrix} 0\\ -f/m\\ \end{pmatrix}

時間微分すると変位xになる変数\etaを定義する

\displaystyle \frac{d \eta}{dt} = x

運動方程式の中に\etaを組み込むと、次のような常微分方程式となる

\displaystyle \frac{d}{dt} \begin{pmatrix} \eta\\ x \\ v\\ \end{pmatrix} = \begin{pmatrix} 0 & 1 & 0\\ 0 & 0 & 1\\ 0 & -c/m & -k/m\\ \end{pmatrix} \begin{pmatrix} \eta\\ x \\ v\\ \end{pmatrix} + \begin{pmatrix} 0\\ 0 \\ f(\eta,x,v,t)/m\\ \end{pmatrix}

ここで外力f(\eta,x,v,t)は

\displaystyle f(\eta,x,v,t) = \begin{pmatrix} K_i\\ K_p \\ K_d\\ \end{pmatrix} \cdot \left\{ \begin{pmatrix} \int \left(x- x_{target}\right) dt\\x- x_{target} \\ \frac{d}{dt}\left(x- x_{target} \right) \\ \end{pmatrix} \right\} \\= \begin{pmatrix} K_i\\ K_p \\ K_d\\ \end{pmatrix} \cdot \left\{ \begin{pmatrix} \eta\\ x \\ v\\ \end{pmatrix} - \begin{pmatrix} \int x_{target} dt\\ x_{target} \\ \frac{d}{dt} x_{target}\\ \end{pmatrix} \right\}

このときx_{target}=\rm{const.}なら

\displaystyle f(\eta,x,v,t) = \begin{pmatrix} K_i\\ K_p \\ K_d\\ \end{pmatrix} \cdot \left\{ \begin{pmatrix} \eta\\ x \\ v\\ \end{pmatrix} - \begin{pmatrix} t x_{target}\\ x_{target} \\ 0\\ \end{pmatrix} \right\}

実装

from scipy.integrate import odeint
import matplotlib.pyplot as plt
import numpy as np


class Simulation:
    def __init__(self, m, c, k, x0, v0):
        self.setSystem(m, c, k)
        self.intiVal(x0,v0)

    def setSystem(self, m, c, k):
        self.m=m
        self.c=c
        self.k=k
    
    def intiVal(self, x0, v0):
        self.u0 = [0, x0, v0]

    def exForce(self, t):
        return 0

    def equationOfMotion(self, u, t):
        self.u=u
        f = self.exForce(t)
        A = np.array([
            [0,1,0],
            [0,0,1],
            [0,-k/m, -c/m]])
        b = np.array([0,0,f/m])
        return np.dot(A, u) + b

    def do(self, times):
        u0 = np.copy(self.u0)
        orbit=odeint(self.equationOfMotion, u0, times)
        return (orbit[:,1], orbit[:,2])

class PID(Simulation):
    def __init__(self, m, c, k, x0, v0):
        super().__init__(m, c, k, x0, v0)
    
    def setCoefficients(self, kp, ki, kd):
        self.coefficients = np.array([ki, kp, kd])
    
    def setTarget(self, target):
        self.target = target

    def exForce(self, t):
        errors =  np.array([t*self.target, self.target, 0])-self.u
        return np.dot(self.coefficients, errors)



if __name__ =="__main__":
    m=1
    c=2
    k=0
    x0 = 0
    v0 = 0
    sim = PID(m,c,k,x0,v0)
    sim.setTarget(10)
    sim.setCoefficients(20, 20, 10)
    
    dt = 0.001
    stop = 10
    times=np.arange(0., stop+dt, dt)
    x,v = sim.do(times)

    plt.plot(times,x)
    plt.xlim(0,stop)
    plt.xlabel("t[s]")
    plt.ylabel("x[m]")
    plt.grid()
    plt.show()

結果

f:id:negizoku:20211120205853p:plain

目標値を時間関数にする

'''python from scipy.integrate import odeint from scipy import integrate from scipy.misc import derivative import matplotlib.pyplot as plt import numpy as np from math import sin, pi

class Simulation: def init(self, m, c, k, x0, v0): self.setSystem(m, c, k) self.intiVal(x0,v0)

def setSystem(self, m, c, k):
    self.m=m
    self.c=c
    self.k=k

def intiVal(self, x0, v0):
    self.u0 = [0, x0, v0]

def exForce(self, t):
    return 0

def equationOfMotion(self, u, t):
    self.u=u
    f = self.exForce(t)
    A = np.array([
        [0,1,0],
        [0,0,1],
        [0,-k/m, -c/m]])
    b = np.array([0,0,f/m])
    return np.dot(A, u) + b

def do(self, times):
    u0 = np.copy(self.u0)
    orbit=odeint(self.equationOfMotion, u0, times)
    return (orbit[:,1], orbit[:,2])

class PID(Simulation): def init(self, m, c, k, x0, v0): super().init(m, c, k, x0, v0)

def setCoefficients(self, kp, ki, kd):
    self.coefficients = np.array([ki, kp, kd])

def target(self,t):
    return sin(t/5 *2*pi)

def exForce(self,t):
    dt=0.00001
    u_target=np.array([
        integrate.quad(self.target, 0, t)[0],
        self.target(t),
        (self.target(t+dt)-self.target(t+dt))/2/dt
    ])

    errors =  u_target-self.u
    return np.dot(self.coefficients, errors)

if name =="main": m=1 c=2 k=0 x0 = 0 v0 = 0 sim = PID(m,c,k,x0,v0) sim.setCoefficients(100, 0, 10)

dt = 0.001
stop = 20
times=np.arange(0., stop+dt, dt)
x,v = sim.do(times)

plt.plot(times,x)
plt.xlim(0,stop)
plt.xlabel("t[s]")
plt.ylabel("x[m]")
plt.grid()
plt.show()

# 出力結果
[f:id:negizoku:20211120215456p:plain]

##目標の書き換え

def target(self,t):
    return float(int(t)%2)

[f:id:negizoku:20211120221213p:plain]