PID制御のPython実装：シミュレーションとチューニング

class PIDController:
    def __init__(self, kp, ki, kd, dt, output_limits=None):
        self.kp = kp
        self.ki = ki
        self.kd = kd
        self.dt = dt
        self.output_limits = output_limits
        self.integral = 0.0
        self.prev_error = 0.0

    def update(self, setpoint, measured):
        error = setpoint - measured

        # 比例項
        p_term = self.kp * error

        # 積分項（累積和による近似）
        self.integral += error * self.dt
        i_term = self.ki * self.integral

        # 微分項（後退差分による近似）
        derivative = (error - self.prev_error) / self.dt
        d_term = self.kd * derivative
        self.prev_error = error

        output = p_term + i_term + d_term

        # 出力制限（アンチワインドアップ）
        if self.output_limits is not None:
            lo, hi = self.output_limits
            if output > hi:
                output = hi
                self.integral -= error * self.dt
            elif output < lo:
                output = lo
                self.integral -= error * self.dt

        return output

    def reset(self):
        self.integral = 0.0
        self.prev_error = 0.0

class FirstOrderSystem:
    def __init__(self, gain, time_constant, dt):
        self.gain = gain
        self.time_constant = time_constant
        self.dt = dt
        self.y = 0.0

    def update(self, u):
        self.y += self.dt / self.time_constant * (self.gain * u - self.y)
        return self.y

    def reset(self):
        self.y = 0.0

import numpy as np
import matplotlib.pyplot as plt

dt = 0.01
t_end = 10.0
t = np.arange(0, t_end, dt)
setpoint = np.ones_like(t)  # ステップ入力

plant_params = {'gain': 1.0, 'time_constant': 1.0, 'dt': dt}

configs = [
    ('P control (Kp=2.0)', {'kp': 2.0, 'ki': 0.0, 'kd': 0.0}),
    ('PI control (Kp=2.0, Ki=1.0)', {'kp': 2.0, 'ki': 1.0, 'kd': 0.0}),
    ('PID control (Kp=2.0, Ki=1.0, Kd=0.5)', {'kp': 2.0, 'ki': 1.0, 'kd': 0.5}),
]

fig, axes = plt.subplots(2, 1, figsize=(10, 8), sharex=True)

for label, params in configs:
    pid = PIDController(**params, dt=dt)
    plant = FirstOrderSystem(**plant_params)
    y_hist = []
    u_hist = []

    for sp in setpoint:
        u = pid.update(sp, plant.y)
        y = plant.update(u)
        y_hist.append(y)
        u_hist.append(u)

    axes[0].plot(t, y_hist, label=label)
    axes[1].plot(t, u_hist, label=label)

axes[0].axhline(y=1.0, color='k', linestyle='--', alpha=0.5, label='Setpoint')
axes[0].set_ylabel('Output y(t)')
axes[0].set_title('Step Response Comparison')
axes[0].legend()
axes[0].grid(True, alpha=0.3)

axes[1].set_ylabel('Control input u(t)')
axes[1].set_xlabel('Time [s]')
axes[1].legend()
axes[1].grid(True, alpha=0.3)

plt.tight_layout()
plt.show()

import numpy as np

class FirstOrderSystem:
    def __init__(self, gain, time_constant, dt):
        self.gain = gain
        self.time_constant = time_constant
        self.dt = dt
        self.y = 0.0
    def update(self, u):
        self.y += self.dt / self.time_constant * (self.gain * u - self.y)
        return self.y

class PIDNoAntiWindup:
    """アンチワインドアップなし：積分は常に加算し続ける"""
    def __init__(self, kp, ki, kd, dt, output_limits):
        self.kp, self.ki, self.kd, self.dt = kp, ki, kd, dt
        self.lo, self.hi = output_limits
        self.integral = 0.0
        self.prev_error = None
    def update(self, setpoint, measured):
        error = setpoint - measured
        if self.prev_error is None:
            self.prev_error = error
        self.integral += error * self.dt
        p = self.kp * error
        i = self.ki * self.integral
        d = self.kd * (error - self.prev_error) / self.dt
        self.prev_error = error
        u_unsat = p + i + d
        return min(max(u_unsat, self.lo), self.hi), u_unsat

class PIDClamping:
    """clamping法：飽和中は積分の加算を打ち消す"""
    def __init__(self, kp, ki, kd, dt, output_limits):
        self.kp, self.ki, self.kd, self.dt = kp, ki, kd, dt
        self.lo, self.hi = output_limits
        self.integral = 0.0
        self.prev_error = None
    def update(self, setpoint, measured):
        error = setpoint - measured
        if self.prev_error is None:
            self.prev_error = error
        self.integral += error * self.dt
        p = self.kp * error
        i = self.ki * self.integral
        d = self.kd * (error - self.prev_error) / self.dt
        self.prev_error = error
        u_unsat = p + i + d
        u = u_unsat
        if u > self.hi:
            u = self.hi
            self.integral -= error * self.dt
        elif u < self.lo:
            u = self.lo
            self.integral -= error * self.dt
        return u, u_unsat

class PIDBackCalc:
    """back-calculation法：飽和誤差を1/Ttでフィードバック"""
    def __init__(self, kp, ki, kd, dt, output_limits, Tt):
        self.kp, self.ki, self.kd, self.dt = kp, ki, kd, dt
        self.lo, self.hi = output_limits
        self.Tt = Tt
        self.integral = 0.0
        self.prev_error = None
    def update(self, setpoint, measured):
        error = setpoint - measured
        if self.prev_error is None:
            self.prev_error = error
        p = self.kp * error
        i = self.ki * self.integral
        d = self.kd * (error - self.prev_error) / self.dt
        self.prev_error = error
        u_unsat = p + i + d
        u = min(max(u_unsat, self.lo), self.hi)
        self.integral += (error + (u - u_unsat) / self.Tt) * self.dt
        return u, u_unsat

def run(controller_cls, kwargs, t_end, dt, setpoint_val):
    t = np.arange(0, t_end, dt)
    plant = FirstOrderSystem(gain=1.0, time_constant=1.0, dt=dt)
    ctrl = controller_cls(dt=dt, **kwargs)
    y_hist, int_hist = [], []
    for _ in t:
        u, u_unsat = ctrl.update(setpoint_val, plant.y)
        y = plant.update(u)
        y_hist.append(y)
        int_hist.append(ctrl.integral)
    return t, np.array(y_hist), np.array(int_hist)

dt, t_end, setpoint_val = 0.01, 15.0, 1.0
kwargs_common = dict(kp=1.0, ki=3.0, kd=0.5, output_limits=(-1.2, 1.2))

results = {
    "No anti-windup": run(PIDNoAntiWindup, kwargs_common, t_end, dt, setpoint_val),
    "Clamping": run(PIDClamping, kwargs_common, t_end, dt, setpoint_val),
    "Back-calculation (Tt=0.3)": run(PIDBackCalc, dict(**kwargs_common, Tt=0.3), t_end, dt, setpoint_val),
}

for name, (t, y, integral) in results.items():
    overshoot = (np.max(y) - setpoint_val) / setpoint_val * 100
    tol = 0.02 * setpoint_val
    settle_idx = next(i for i in range(len(y)) if np.all(np.abs(y[i:] - setpoint_val) < tol))
    print(f"{name}: overshoot={overshoot:.2f}%, settle_t={t[settle_idx]:.2f}s, "
          f"peak_integral={np.max(integral):.4f}")

No anti-windup: overshoot=17.71%, settle_t=7.32s, peak_integral=0.7127
Clamping: overshoot=7.67%, settle_t=3.94s, peak_integral=0.4301
Back-calculation (Tt=0.3): overshoot=7.86%, settle_t=3.95s, peak_integral=0.4318

import numpy as np

K_plant, T_plant = 1.0, 1.0
Kp, Ki = 2.0, 1.0

def poles(dt, method):
    """厳密にZOH離散化された一次遅れ系とPI制御器（methodで離散化）から成る
    閉ループの特性方程式の根"""
    a = np.exp(-dt / T_plant)
    b = K_plant * (1 - a)
    if method == "backward":
        c1 = -(1 + a) + b * Kp + b * Ki * dt
        c0 = a - b * Kp
    elif method == "forward":
        c1 = -(1 + a) + b * Kp
        c0 = a - b * Kp + b * Ki * dt
    elif method == "tustin":
        c1 = -(1 + a) + b * Kp + b * Ki * dt / 2
        c0 = a - b * Kp + b * Ki * dt / 2
    return np.roots([1.0, c1, c0])

def is_stable(dt, method):
    return np.all(np.abs(poles(dt, method)) < 1.0)

print("安定限界（両方の根が単位円内に収まる最大のdt）:")
for method in ["backward", "forward", "tustin"]:
    lo, hi = 0.01, 5.0
    if is_stable(hi, method):
        print(f"  {method}: dt={hi}でも安定")
        continue
    for _ in range(60):
        mid = (lo + hi) / 2
        lo, hi = (mid, hi) if is_stable(mid, method) else (lo, mid)
    print(f"  {method}: 安定限界 dt ~ {lo:.3f} s")

# 後退差分の安定限界付近（dt=0.8s）での時間応答
from scipy.integrate import solve_ivp

def simulate(dt, method, t_end=30.0, setpoint=1.0):
    y, integral, prev_error = 0.0, 0.0, 0.0
    t_hist, y_hist = [0.0], [0.0]
    for k in range(int(t_end / dt)):
        error = setpoint - y
        if method == "backward":
            integral += error * dt
            i_term = Ki * integral
        elif method == "forward":
            i_term = Ki * integral
            integral += error * dt
        elif method == "tustin":
            integral += (error + prev_error) / 2 * dt
            i_term = Ki * integral
        u = Kp * error + i_term
        prev_error = error
        sol = solve_ivp(lambda tt, yy: (K_plant * u - yy) / T_plant, [0, dt], [y])
        y = sol.y[0, -1]
        t_hist.append(t_hist[-1] + dt)
        y_hist.append(y)
    return np.array(t_hist), np.array(y_hist)

print("\ndt=0.8sでの時間応答:")
for method in ["backward", "tustin", "forward"]:
    t_hist, y_hist = simulate(0.8, method)
    overshoot = (np.max(y_hist) - 1.0) * 100
    tol = 0.02
    settle_idx = next((i for i in range(len(y_hist)) if np.all(np.abs(y_hist[i:] - 1.0) < tol)), None)
    settle_t = t_hist[settle_idx] if settle_idx is not None else None
    print(f"  {method:9s}: overshoot={overshoot:.2f}%, settle_t={settle_t}")

安定限界（両方の根が単位円内に収まる最大のdt）:
  backward: 安定限界 dt ~ 0.872 s
  forward: 安定限界 dt ~ 3.000 s
  tustin: 安定限界 dt ~ 1.099 s

dt=0.8sでの時間応答:
  backward : overshoot=54.18%, settle_t=19.200000000000006
  tustin   : overshoot=32.16%, settle_t=7.199999999999999
  forward  : overshoot=10.13%, settle_t=4.8

import numpy as np

class FirstOrderSystem:
    def __init__(self, gain, time_constant, dt):
        self.gain = gain
        self.time_constant = time_constant
        self.dt = dt
        self.y = 0.0
    def update(self, u):
        self.y += self.dt / self.time_constant * (self.gain * u - self.y)
        return self.y

class PIDErrorDerivative:
    """通常形式：微分項が偏差 e = r - y に作用する"""
    def __init__(self, kp, ki, kd, dt):
        self.kp, self.ki, self.kd, self.dt = kp, ki, kd, dt
        self.integral = 0.0
        self.prev_error = None
    def update(self, setpoint, measured):
        error = setpoint - measured
        if self.prev_error is None:
            self.prev_error = error
        self.integral += error * self.dt
        p = self.kp * error
        i = self.ki * self.integral
        d = self.kd * (error - self.prev_error) / self.dt
        self.prev_error = error
        return p + i + d

class PIDMeasurementDerivative:
    """微分先行型：微分項が観測値 -y のみに作用する"""
    def __init__(self, kp, ki, kd, dt):
        self.kp, self.ki, self.kd, self.dt = kp, ki, kd, dt
        self.integral = 0.0
        self.prev_measured = None
    def update(self, setpoint, measured):
        error = setpoint - measured
        if self.prev_measured is None:
            self.prev_measured = measured
        self.integral += error * self.dt
        p = self.kp * error
        i = self.ki * self.integral
        d = -self.kd * (measured - self.prev_measured) / self.dt
        self.prev_measured = measured
        return p + i + d

dt = 0.01
t = np.arange(0, 12.0, dt)
setpoint = np.where(t < 5.0, 1.0, 2.0)  # t=5sで目標値がステップ変化

for name, cls in [("Derivative-on-error", PIDErrorDerivative),
                   ("Derivative-on-measurement", PIDMeasurementDerivative)]:
    pid = cls(kp=2.0, ki=1.0, kd=0.5, dt=dt)
    plant = FirstOrderSystem(gain=1.0, time_constant=1.0, dt=dt)
    u_hist = []
    for sp in setpoint:
        u = pid.update(sp, plant.y)
        plant.update(u)
        u_hist.append(u)
    u_hist = np.array(u_hist)
    idx_step = np.argmax(t >= 5.0)
    print(f"{name}: u just before step={u_hist[idx_step-1]:.4f}, "
          f"u just after step={u_hist[idx_step]:.4f}, "
          f"jump={u_hist[idx_step]-u_hist[idx_step-1]:.4f}")

Derivative-on-error: u just before step=0.9852, u just after step=52.9953, jump=52.0101
Derivative-on-measurement: u just before step=0.9852, u just after step=2.9953, jump=2.0101

import numpy as np

class FirstOrderSystem:
    def __init__(self, gain, time_constant, dt):
        self.gain = gain
        self.time_constant = time_constant
        self.dt = dt
        self.y = 0.0
    def update(self, u):
        self.y += self.dt / self.time_constant * (self.gain * u - self.y)
        return self.y

class PIDFilteredDerivative:
    """微分先行型に、生の微分値へ時定数1/Nの一次ローパスフィルタを
    (Noneならフィルタなし)追加した実装"""
    def __init__(self, kp, ki, kd, dt, N=None):
        self.kp, self.ki, self.kd, self.dt = kp, ki, kd, dt
        self.N = N
        self.integral = 0.0
        self.prev_measured = None
        self.d_filt = 0.0
    def update(self, setpoint, measured):
        error = setpoint - measured
        if self.prev_measured is None:
            self.prev_measured = measured
        self.integral += error * self.dt
        p = self.kp * error
        i = self.ki * self.integral
        d_raw = -(measured - self.prev_measured) / self.dt
        self.prev_measured = measured
        if self.N is None:
            d_used = d_raw
        else:
            tau_f = 1.0 / self.N
            alpha = min(self.dt / tau_f, 1.0)
            self.d_filt += alpha * (d_raw - self.d_filt)
            d_used = self.d_filt
        return p + i + self.kd * d_used

dt = 0.01
t = np.arange(0, 15.0, dt)
setpoint = np.ones_like(t)
noise_std = 0.01

for label, N in [("No filter", None), ("N=10", 10.0), ("N=2 (stronger filter)", 2.0)]:
    np.random.seed(42)
    pid = PIDFilteredDerivative(kp=2.0, ki=1.0, kd=0.5, dt=dt, N=N)
    plant = FirstOrderSystem(gain=1.0, time_constant=1.0, dt=dt)
    y_hist, u_hist = [], []
    for sp in setpoint:
        noisy_measurement = plant.y + np.random.normal(0, noise_std)
        u = pid.update(sp, noisy_measurement)
        y = plant.update(u)
        y_hist.append(y)
        u_hist.append(u)
    y_hist, u_hist = np.array(y_hist), np.array(u_hist)
    mask = t > 5.0  # 定常状態区間
    u_std = np.std(u_hist[mask])
    u_ptp = np.ptp(u_hist[mask])
    tracking_rmse = np.sqrt(np.mean((y_hist[mask] - 1.0) ** 2))
    print(f"{label}: std(u)={u_std:.4f}, ptp(u)={u_ptp:.4f}, tracking_RMSE={tracking_rmse:.5f}")

No filter: std(u)=1.0377, ptp(u)=7.2849, tracking_RMSE=0.01069
N=10: std(u)=0.0717, ptp(u)=0.4361, tracking_RMSE=0.00876
N=2 (stronger filter): std(u)=0.0301, ptp(u)=0.1786, tracking_RMSE=0.00792

import numpy as np
import matplotlib.pyplot as plt

dt = 0.01
t_end = 20.0
t = np.arange(0, t_end, dt)
setpoint = np.ones_like(t)

# 外乱：t=10sで大きさ0.5のステップ外乱
disturbance = np.zeros_like(t)
disturbance[t >= 10.0] = 0.5

plant_params = {'gain': 1.0, 'time_constant': 1.0, 'dt': dt}

configs = [
    ('P control (Kp=2.0)', {'kp': 2.0, 'ki': 0.0, 'kd': 0.0}),
    ('PID control (Kp=2.0, Ki=1.0, Kd=0.5)', {'kp': 2.0, 'ki': 1.0, 'kd': 0.5}),
]

fig, axes = plt.subplots(2, 1, figsize=(10, 8), sharex=True)

for label, params in configs:
    pid = PIDController(**params, dt=dt)
    plant = FirstOrderSystem(**plant_params)
    y_hist = []
    u_hist = []

    for i, sp in enumerate(setpoint):
        u = pid.update(sp, plant.y)
        # 外乱をプラントの入力に加算
        y = plant.update(u + disturbance[i])
        y_hist.append(y)
        u_hist.append(u)

    axes[0].plot(t, y_hist, label=label)
    axes[1].plot(t, u_hist, label=label)

axes[0].axhline(y=1.0, color='k', linestyle='--', alpha=0.5, label='Setpoint')
axes[0].axvline(x=10.0, color='r', linestyle=':', alpha=0.5, label='Disturbance onset')
axes[0].set_ylabel('Output y(t)')
axes[0].set_title('Disturbance Rejection Comparison')
axes[0].legend()
axes[0].grid(True, alpha=0.3)

axes[1].set_ylabel('Control input u(t)')
axes[1].set_xlabel('Time [s]')
axes[1].legend()
axes[1].grid(True, alpha=0.3)

plt.tight_layout()
plt.show()