5.3 PID Regulyatorlar — Amaliyot
Laboratoriya ishi: Python da PID controller simulyatsiyasi
1. Asosiy PID Implementatsiyasi
import numpy as np
import matplotlib.pyplot as plt
class PIDController:
def __init__(self, Kp, Ki, Kd, dt,
integral_limit=None, derivative_filter=None):
self.Kp = Kp
self.Ki = Ki
self.Kd = Kd
self.dt = dt
self.integral = 0
self.prev_error = 0
self.prev_derivative = 0
self.integral_limit = integral_limit
self.derivative_filter = derivative_filter # alpha, 0-1
def compute(self, setpoint, measured):
error = setpoint - measured
# Proportional
P = self.Kp * error
# Integral with anti-windup
self.integral += error * self.dt
if self.integral_limit:
self.integral = np.clip(self.integral,
-self.integral_limit,
self.integral_limit)
I = self.Ki * self.integral
# Derivative with optional filter
derivative = (error - self.prev_error) / self.dt
if self.derivative_filter:
derivative = (self.derivative_filter * derivative +
(1 - self.derivative_filter) * self.prev_derivative)
D = self.Kd * derivative
self.prev_error = error
self.prev_derivative = derivative
return P + I + D, {'P': P, 'I': I, 'D': D, 'error': error}
def reset(self):
self.integral = 0
self.prev_error = 0
self.prev_derivative = 0
# Test
pid = PIDController(Kp=1, Ki=0.5, Kd=0.1, dt=0.01)
output, components = pid.compute(setpoint=100, measured=90)
print(f"Output: {output:.2f}")
print(f"Components: P={components['P']:.2f}, I={components['I']:.2f}, D={components['D']:.2f}")
2. Birinchi Tartibli Tizim Simulyatsiyasi
import numpy as np
import matplotlib.pyplot as plt
def first_order_system(u, y, tau, K, dt):
"""
First-order system: tau * dy/dt + y = K * u
"""
dydt = (K * u - y) / tau
return y + dydt * dt
def simulate_closed_loop(pid, setpoint, tau, K, duration, dt=0.01):
"""PID bilan yopiq sikl simulyatsiyasi"""
t = np.arange(0, duration, dt)
y = np.zeros_like(t)
u = np.zeros_like(t)
for i in range(1, len(t)):
# PID
control, _ = pid.compute(setpoint, y[i-1])
u[i] = np.clip(control, 0, 100) # Saturatsiya
# Plant
y[i] = first_order_system(u[i], y[i-1], tau, K, dt)
return t, y, u
# Simulyatsiya
pid = PIDController(Kp=2, Ki=1, Kd=0.5, dt=0.01, integral_limit=50)
t, y, u = simulate_closed_loop(pid, setpoint=50, tau=2, K=1, duration=20)
fig, axes = plt.subplots(2, 1, figsize=(12, 8))
axes[0].plot(t, y, 'b-', linewidth=2, label='Output')
axes[0].axhline(y=50, color='r', linestyle='--', label='Setpoint')
axes[0].set_ylabel('Output')
axes[0].set_title('PID Closed-Loop Response')
axes[0].legend()
axes[0].grid(True)
axes[1].plot(t, u, 'g-', linewidth=2)
axes[1].set_xlabel('Vaqt (s)')
axes[1].set_ylabel('Control Signal')
axes[1].grid(True)
plt.tight_layout()
plt.savefig('pid_response.png', dpi=150)
plt.show()
3. PID Tuning Solishtirish
import numpy as np
import matplotlib.pyplot as plt
def simulate_with_gains(Kp, Ki, Kd, label):
pid = PIDController(Kp=Kp, Ki=Ki, Kd=Kd, dt=0.01)
t, y, u = simulate_closed_loop(pid, setpoint=50, tau=2, K=1, duration=15)
return t, y, label
# Turli tuning
configs = [
(1, 0, 0, 'P only'),
(2, 0.5, 0, 'PI'),
(2, 0, 0.5, 'PD'),
(2, 0.5, 0.5, 'PID'),
(4, 1, 1, 'Aggressive PID'),
]
plt.figure(figsize=(12, 6))
for Kp, Ki, Kd, label in configs:
t, y, label = simulate_with_gains(Kp, Ki, Kd, label)
plt.plot(t, y, linewidth=2, label=label)
plt.axhline(y=50, color='k', linestyle='--', label='Setpoint')
plt.xlabel('Vaqt (s)')
plt.ylabel('Output')
plt.title('PID Tuning Comparison')
plt.legend()
plt.grid(True)
plt.savefig('pid_tuning_comparison.png', dpi=150)
plt.show()
4. Quadcopter Altitude Controller
import numpy as np
import matplotlib.pyplot as plt
class QuadcopterAltitude:
def __init__(self, mass=2.0, g=9.81):
self.mass = mass
self.g = g
self.z = 0 # Balandlik
self.vz = 0 # Vertikal tezlik
def update(self, thrust, dt):
"""Dinamika yangilash"""
# F = ma, a = (T - mg) / m
az = (thrust - self.mass * self.g) / self.mass
# Integratsiya
self.vz += az * dt
self.z += self.vz * dt
# Yer chegarasi
if self.z < 0:
self.z = 0
self.vz = 0
return self.z, self.vz
def altitude_control_simulation():
# Quadcopter
quad = QuadcopterAltitude(mass=2.0)
# PID Controller
pid = PIDController(Kp=15, Ki=5, Kd=8, dt=0.01, integral_limit=10)
# Simulyatsiya
dt = 0.01
duration = 20
t = np.arange(0, duration, dt)
z_history = []
vz_history = []
thrust_history = []
setpoint_history = []
for i, time in enumerate(t):
# O'zgaruvchan setpoint
if time < 5:
setpoint = 10
elif time < 10:
setpoint = 20
elif time < 15:
setpoint = 5
else:
setpoint = 15
setpoint_history.append(setpoint)
# PID
control, _ = pid.compute(setpoint, quad.z)
# Thrust = hover thrust + PID output
thrust = quad.mass * quad.g + control
thrust = np.clip(thrust, 0, 50) # Motor limit
thrust_history.append(thrust)
# Update
z, vz = quad.update(thrust, dt)
z_history.append(z)
vz_history.append(vz)
# Plot
fig, axes = plt.subplots(3, 1, figsize=(12, 10))
axes[0].plot(t, z_history, 'b-', linewidth=2, label='Altitude')
axes[0].plot(t, setpoint_history, 'r--', linewidth=2, label='Setpoint')
axes[0].set_ylabel('Balandlik (m)')
axes[0].set_title('Quadcopter Altitude Control')
axes[0].legend()
axes[0].grid(True)
axes[1].plot(t, vz_history, 'g-', linewidth=2)
axes[1].set_ylabel('Vertikal tezlik (m/s)')
axes[1].grid(True)
axes[2].plot(t, thrust_history, 'm-', linewidth=2)
axes[2].axhline(y=quad.mass*9.81, color='k', linestyle='--', label='Hover thrust')
axes[2].set_xlabel('Vaqt (s)')
axes[2].set_ylabel('Thrust (N)')
axes[2].legend()
axes[2].grid(True)
plt.tight_layout()
plt.savefig('quadcopter_altitude.png', dpi=150)
plt.show()
altitude_control_simulation()
5. Motor Tezlik Kontroli
import numpy as np
import matplotlib.pyplot as plt
class DCMotor:
"""Soddalashtirilgan DC motor modeli"""
def __init__(self, J=0.01, b=0.1, K=0.01, R=1, L=0.5):
self.J = J # Inertia
self.b = b # Damping
self.K = K # Motor constant
self.R = R # Resistance
self.L = L # Inductance
self.omega = 0 # Angular velocity
self.i = 0 # Current
def update(self, V, dt):
"""Motor dinamikasi"""
# di/dt = (V - R*i - K*omega) / L
didt = (V - self.R * self.i - self.K * self.omega) / self.L
self.i += didt * dt
# domega/dt = (K*i - b*omega) / J
domega_dt = (self.K * self.i - self.b * self.omega) / self.J
self.omega += domega_dt * dt
return self.omega, self.i
def motor_speed_control():
motor = DCMotor()
pid = PIDController(Kp=5, Ki=10, Kd=0.1, dt=0.001, integral_limit=100)
dt = 0.001
duration = 2
t = np.arange(0, duration, dt)
omega_history = []
voltage_history = []
setpoint = 100 # rad/s
for time in t:
# PID
control, _ = pid.compute(setpoint, motor.omega)
voltage = np.clip(control, 0, 24) # 24V max
voltage_history.append(voltage)
# Update
omega, i = motor.update(voltage, dt)
omega_history.append(omega)
fig, axes = plt.subplots(2, 1, figsize=(12, 8))
axes[0].plot(t, omega_history, 'b-', linewidth=2)
axes[0].axhline(y=setpoint, color='r', linestyle='--', label='Setpoint')
axes[0].set_ylabel('Tezlik (rad/s)')
axes[0].set_title('DC Motor Speed Control')
axes[0].legend()
axes[0].grid(True)
axes[1].plot(t, voltage_history, 'g-', linewidth=2)
axes[1].set_xlabel('Vaqt (s)')
axes[1].set_ylabel('Voltage (V)')
axes[1].grid(True)
plt.tight_layout()
plt.savefig('motor_speed_control.png', dpi=150)
plt.show()
motor_speed_control()
6. Auto-Tuning (Ziegler-Nichols)
import numpy as np
import matplotlib.pyplot as plt
def find_ultimate_gain(plant_func, dt=0.01, duration=50):
"""
Relay feedback auto-tuning
"""
d = 1.0 # Relay amplitude
t = np.arange(0, duration, dt)
y = np.zeros_like(t)
u = np.zeros_like(t)
y[0] = 0.5 # Initial offset
for i in range(1, len(t)):
# Relay control
if y[i-1] > 0:
u[i] = -d
else:
u[i] = d
# Plant (first order with delay)
y[i] = plant_func(u[i], y[i-1], dt)
# Oscillation davri va amplitudasini topish
# (soddalashtirilgan - zero crossing)
crossings = []
for i in range(1, len(y)):
if y[i-1] < 0 and y[i] >= 0:
crossings.append(t[i])
if len(crossings) >= 4:
Tu = np.mean(np.diff(crossings[-4:]))
# Amplituda
idx_start = int(crossings[-2] / dt)
idx_end = int(crossings[-1] / dt)
a = (np.max(y[idx_start:idx_end]) - np.min(y[idx_start:idx_end])) / 2
Ku = 4 * d / (np.pi * a)
return Ku, Tu, t, y, u
return None, None, t, y, u
def plant_model(u, y, dt, tau=1, K=1, delay_samples=10):
"""First order + delay"""
dydt = (K * u - y) / tau
return y + dydt * dt
# Auto-tuning
Ku, Tu, t, y, u = find_ultimate_gain(
lambda u, y, dt: plant_model(u, y, dt),
duration=30
)
if Ku and Tu:
print(f"Ultimate Gain Ku = {Ku:.3f}")
print(f"Ultimate Period Tu = {Tu:.3f} s")
# Ziegler-Nichols PID
Kp = 0.6 * Ku
Ti = Tu / 2
Td = Tu / 8
Ki = Kp / Ti
Kd = Kp * Td
print(f"\nZiegler-Nichols PID:")
print(f"Kp = {Kp:.3f}")
print(f"Ki = {Ki:.3f}")
print(f"Kd = {Kd:.3f}")
# Plot relay response
plt.figure(figsize=(12, 6))
plt.subplot(2, 1, 1)
plt.plot(t, y, 'b-')
plt.ylabel('Output')
plt.title('Relay Feedback Test')
plt.grid(True)
plt.subplot(2, 1, 2)
plt.plot(t, u, 'r-')
plt.xlabel('Vaqt (s)')
plt.ylabel('Relay Output')
plt.grid(True)
plt.tight_layout()
plt.savefig('auto_tuning.png', dpi=150)
plt.show()
7. Topshiriq: Kaskadli PID
import numpy as np
import matplotlib.pyplot as plt
def cascaded_pid_control():
"""
Kaskadli PID: Pozitsiya -> Tezlik -> Motor
TODO:
1. Tashqi PID (pozitsiya) yarating
2. Ichki PID (tezlik) yarating
3. Simulyatsiya qiling
4. Oddiy PID bilan solishtiring
"""
# Sizning kodingiz...
pass
# cascaded_pid_control()
Yechim
def cascaded_pid_control():
dt = 0.01
duration = 20
t = np.arange(0, duration, dt)
# Tashqi PID (pozitsiya)
outer_pid = PIDController(Kp=2, Ki=0.5, Kd=0.5, dt=dt)
# Ichki PID (tezlik)
inner_pid = PIDController(Kp=5, Ki=2, Kd=0.1, dt=dt)
# Tizim holati
position = 0
velocity = 0
mass = 1
damping = 0.5
pos_history = []
vel_history = []
setpoint = 10
for time in t:
# Tashqi PID: pozitsiya -> velocity setpoint
vel_setpoint, _ = outer_pid.compute(setpoint, position)
vel_setpoint = np.clip(vel_setpoint, -5, 5) # Tezlik limiti
# Ichki PID: velocity -> force
force, _ = inner_pid.compute(vel_setpoint, velocity)
force = np.clip(force, -20, 20) # Kuch limiti
# Dinamika: F = ma + bv
acceleration = (force - damping * velocity) / mass
velocity += acceleration * dt
position += velocity * dt
pos_history.append(position)
vel_history.append(velocity)
# Plot
fig, axes = plt.subplots(2, 1, figsize=(12, 8))
axes[0].plot(t, pos_history, 'b-', linewidth=2)
axes[0].axhline(y=setpoint, color='r', linestyle='--')
axes[0].set_ylabel('Pozitsiya')
axes[0].set_title('Cascaded PID Control')
axes[0].grid(True)
axes[1].plot(t, vel_history, 'g-', linewidth=2)
axes[1].set_xlabel('Vaqt (s)')
axes[1].set_ylabel('Tezlik')
axes[1].grid(True)
plt.tight_layout()
plt.savefig('cascaded_pid.png', dpi=150)
plt.show()
cascaded_pid_control()
✅ Laboratoriya Tekshirish Ro'yxati
- Asosiy PID class ishlaydi
- First-order system simulyatsiyasi
- Tuning comparison
- Quadcopter altitude control
- Motor speed control
- Auto-tuning relay test
- Cascaded PID topshiriq