PID Control cho Robot: Từ lý thuyết đến thực hành

PID Controller là gì và tại sao quan trọng?

PID (Proportional-Integral-Derivative) là thuật toán điều khiển phổ biến nhất trong engineering. Từ điều khiển nhiệt độ lò nướng đến giữ thăng bằng cho drone, từ cruise control trên ô tô đến điều khiển vị trí robot arm -- PID có mặt khắp nơi.

Lý do đơn giản: PID đơn giản, hiệu quả, và đủ tốt cho ~90% bài toán điều khiển trong robotics. Trước khi nhảy vào MPC (Model Predictive Control) hay Reinforcement Learning, bạn cần nắm vững PID.

Bài viết này sẽ đưa bạn từ lý thuyết cơ bản đến implement PID controller bằng Python, tuning bằng phương pháp Ziegler-Nichols, xử lý anti-windup, và ứng dụng thực tế điều khiển DC motor.

Lý thuyết PID -- 3 thành phần

Control loop cơ bản

Setpoint (r) → [+] → e(t) → [PID Controller] → u(t) → [Plant/Motor] → y(t)
               [-]                                                      |
                ↑                                                       |
                └───────────────── Feedback ────────────────────────────┘

Setpoint r(t): Giá trị mong muốn (ví dụ: tốc độ 100 RPM)
Error e(t) = r(t) - y(t): Sai số giữa setpoint và output thực tế
Control signal u(t): Tín hiệu điều khiển gửi đến actuator
Plant output y(t): Output đo được từ sensor

P -- Proportional (tỷ lệ)

Thành phần P tạo output tỷ lệ thuận với error hiện tại:

u_P(t) = Kp * e(t)

Kp lớn: Phản ứng nhanh, nhưng dễ oscillation (dao động)
Kp nhỏ: Phản ứng chậm, nhưng ổn định hơn
Vấn đề: Luôn có steady-state error (sai số ở trạng thái ổn định). P-only controller không bao giờ đạt chính xác setpoint.

I -- Integral (tích phân)

Thành phần I tích lũy error theo thời gian:

u_I(t) = Ki * integral(e(t) dt)

Vai trò chính: Loại bỏ steady-state error. Khi error nhỏ nhưng kéo dài, I tích lũy và tạo thêm output.
Ki lớn: Loại bỏ error nhanh, nhưng gây overshoot
Ki nhỏ: Loại bỏ error chậm, nhưng ít overshoot
Vấn đề: Integral windup -- khi actuator saturated (ví dụ motor đã chạy max), I vẫn tiếp tục tích lũy → overshoot lớn khi constraint được giải phóng.

D -- Derivative (đạo hàm)

Thành phần D phản ứng theo tốc độ thay đổi của error:

u_D(t) = Kd * d(e(t))/dt

Vai trò chính: Dự đoán error tương lai, giảm overshoot. Khi error đang giảm nhanh, D giảm output để tránh vượt quá setpoint.
Kd lớn: Giảm overshoot tốt, nhưng nhạy với noise
Kd nhỏ: Ít hiệu quả nhưng ổn định hơn
Vấn đề: Derivative kick -- khi setpoint thay đổi đột ngột, derivative of error spike lên rất lớn.

Công thức tổng hợp

u(t) = Kp * e(t) + Ki * integral(e(t) dt) + Kd * d(e(t))/dt

Implement PID bằng Python

PID class cơ bản

import time
import numpy as np

class PIDController:
    def __init__(self, kp: float, ki: float, kd: float,
                 output_min: float = -1.0, output_max: float = 1.0,
                 anti_windup: bool = True):
        self.kp = kp
        self.ki = ki
        self.kd = kd
        self.output_min = output_min
        self.output_max = output_max
        self.anti_windup = anti_windup

        # Internal state
        self._integral = 0.0
        self._prev_error = 0.0
        self._prev_time = None
        self._prev_measurement = 0.0  # For derivative-on-measurement

    def reset(self):
        """Reset internal state."""
        self._integral = 0.0
        self._prev_error = 0.0
        self._prev_time = None
        self._prev_measurement = 0.0

    def compute(self, setpoint: float, measurement: float,
                dt: float = None) -> float:
        """
        Compute PID output.

        Args:
            setpoint: Desired value
            measurement: Current measured value
            dt: Time step (seconds). Auto-calculated if None.

        Returns:
            Control signal (clamped to output_min/output_max)
        """
        current_time = time.monotonic()
        if dt is None:
            if self._prev_time is None:
                dt = 0.01  # Default 10ms
            else:
                dt = current_time - self._prev_time
        self._prev_time = current_time

        # Error
        error = setpoint - measurement

        # Proportional
        p_term = self.kp * error

        # Integral
        self._integral += error * dt
        i_term = self.ki * self._integral

        # Derivative (on measurement, not error -- avoid derivative kick)
        d_measurement = (measurement - self._prev_measurement) / dt if dt > 0 else 0.0
        d_term = -self.kd * d_measurement  # Negative because we use measurement

        # Total output
        output = p_term + i_term + d_term

        # Anti-windup: clamp integral khi output saturated
        if self.anti_windup:
            if output > self.output_max:
                self._integral -= (output - self.output_max) / self.ki if self.ki != 0 else 0
                output = self.output_max
            elif output < self.output_min:
                self._integral -= (output - self.output_min) / self.ki if self.ki != 0 else 0
                output = self.output_min
        else:
            output = np.clip(output, self.output_min, self.output_max)

        # Save state
        self._prev_error = error
        self._prev_measurement = measurement

        return output

Giải thích anti-windup

Anti-windup bằng phương pháp back-calculation: khi output bị clamp (saturated), ta trừ ngược phần vượt quá khỏi integral term. Điều này ngăn integral "tích tụ" khi actuator không thể tăng thêm.

# Anti-windup back-calculation
if output > self.output_max:
    excess = output - self.output_max
    self._integral -= excess / self.ki  # "Trả lại" excess cho integral
    output = self.output_max

Giải thích derivative-on-measurement

Thay vì tính derivative of error, ta tính derivative of measurement:

# Derivative kick (BAD -- error spike khi setpoint thay đổi)
d_term = self.kd * (error - self._prev_error) / dt

# Derivative on measurement (GOOD -- smooth, không spike)
d_term = -self.kd * (measurement - self._prev_measurement) / dt

Khi setpoint thay đổi đột ngột (step change), error nhảy ngay lập tức → derivative spike rất lớn (derivative kick). Dùng derivative on measurement tránh được vấn đề này vì measurement thay đổi liên tục, smooth.

Phương pháp tuning PID

1. Ziegler-Nichols (Oscillation Method)

Phương pháp kinh điển nhất, phát minh năm 1942 nhưng vẫn được dùng rộng rãi:

Bước 1: Đặt Ki = 0, Kd = 0 (chỉ dùng P controller)

Bước 2: Tăng Kp từ nhỏ đến khi hệ thống dao động đều (sustained oscillation). Ghi lại:

Ku (Ultimate gain): Giá trị Kp tại điểm oscillation
Tu (Ultimate period): Chu kỳ dao động

Bước 3: Tính PID gains theo bảng:

Controller	Kp	Ki	Kd
P only	0.5 * Ku	-	-
PI	0.45 * Ku	1.2 * Kp / Tu	-
PID	0.6 * Ku	2 * Kp / Tu	Kp * Tu / 8

def ziegler_nichols_pid(ku: float, tu: float) -> tuple:
    """
    Calculate PID gains using Ziegler-Nichols method.

    Args:
        ku: Ultimate gain (Kp at sustained oscillation)
        tu: Ultimate period (oscillation period in seconds)

    Returns:
        (kp, ki, kd) tuple
    """
    kp = 0.6 * ku
    ki = 2.0 * kp / tu
    kd = kp * tu / 8.0
    return kp, ki, kd

# Ví dụ: Ku = 2.0, Tu = 0.5s
kp, ki, kd = ziegler_nichols_pid(ku=2.0, tu=0.5)
print(f"Kp={kp:.2f}, Ki={ki:.2f}, Kd={kd:.2f}")
# Output: Kp=1.20, Ki=4.80, Kd=0.075

2. Manual Tuning -- Quy tắc thực hành

Nếu Ziegler-Nichols cho kết quả aggressive (overshoot nhiều), tuning manual theo thứ tự:

Bắt đầu với P only: Tăng Kp đến khi response "vừa đủ nhanh" nhưng có steady-state error.
Thêm I: Tăng Ki từ từ để loại bỏ steady-state error. Dừng khi overshoot < 10%.
Thêm D: Tăng Kd để giảm overshoot. Cẩn thận với noise -- nếu sensor noisy, giữ Kd nhỏ.

# Hiệu ứng của từng tham số
# Tăng Kp: Nhanh hơn, overshoot nhiều hơn, steady-state error nhỏ hơn
# Tăng Ki: Loại bỏ steady-state error, overshoot nhiều hơn, chậm settle
# Tăng Kd: Giảm overshoot, response nhanh hơn, nhạy noise

Simulation và Visualization với Matplotlib

import numpy as np
import matplotlib.pyplot as plt

class DCMotorSimulation:
    """Mô phỏng DC motor đơn giản."""
    def __init__(self, inertia=0.01, damping=0.1, kt=0.01,
                 resistance=1.0, inductance=0.5):
        self.J = inertia      # Moment of inertia (kg*m^2)
        self.b = damping       # Damping ratio (N*m*s)
        self.Kt = kt           # Torque constant (N*m/A)
        self.R = resistance    # Resistance (Ohm)
        self.L = inductance    # Inductance (H)

        self.omega = 0.0       # Angular velocity (rad/s)
        self.current = 0.0     # Motor current (A)

    def step(self, voltage: float, dt: float) -> float:
        """Simulate one timestep, return angular velocity."""
        # Electrical: L * di/dt = V - R*i - Kt*omega
        di_dt = (voltage - self.R * self.current - self.Kt * self.omega) / self.L
        self.current += di_dt * dt

        # Mechanical: J * domega/dt = Kt*i - b*omega
        domega_dt = (self.Kt * self.current - self.b * self.omega) / self.J
        self.omega += domega_dt * dt

        return self.omega

def simulate_pid(kp, ki, kd, setpoint=100.0, duration=5.0, dt=0.001):
    """Run PID simulation on DC motor."""
    motor = DCMotorSimulation()
    pid = PIDController(kp, ki, kd, output_min=-12.0, output_max=12.0)

    times = np.arange(0, duration, dt)
    velocities = []
    setpoints = []
    controls = []

    for t in times:
        # Step change tại t=0 và t=2.5s
        sp = setpoint if t < 2.5 else setpoint * 0.5
        setpoints.append(sp)

        velocity = motor.omega
        velocities.append(velocity)

        control = pid.compute(sp, velocity, dt=dt)
        controls.append(control)

        motor.step(control, dt)

    return times, velocities, setpoints, controls

# So sánh 3 bộ tham số
fig, axes = plt.subplots(3, 1, figsize=(12, 10), sharex=True)

configs = [
    {"kp": 0.5, "ki": 0.0, "kd": 0.0, "label": "P only (Kp=0.5)"},
    {"kp": 0.5, "ki": 2.0, "kd": 0.0, "label": "PI (Kp=0.5, Ki=2.0)"},
    {"kp": 0.8, "ki": 3.0, "kd": 0.01, "label": "PID (Kp=0.8, Ki=3.0, Kd=0.01)"},
]

for cfg in configs:
    t, vel, sp, ctrl = simulate_pid(cfg["kp"], cfg["ki"], cfg["kd"])
    axes[0].plot(t, vel, label=cfg["label"])
    axes[1].plot(t, [s - v for s, v in zip(sp, vel)], label=cfg["label"])
    axes[2].plot(t, ctrl, label=cfg["label"])

axes[0].plot(t, sp, "k--", label="Setpoint", alpha=0.5)
axes[0].set_ylabel("Velocity (rad/s)")
axes[0].set_title("PID Response Comparison")
axes[0].legend()

axes[1].set_ylabel("Error")
axes[1].set_title("Error Over Time")
axes[1].legend()

axes[2].set_ylabel("Control Signal (V)")
axes[2].set_xlabel("Time (s)")
axes[2].set_title("Control Effort")
axes[2].legend()

plt.tight_layout()
plt.savefig("pid_comparison.png", dpi=150)
plt.show()

Ứng dụng thực tế: DC Motor + Encoder

Hardware setup

Một setup điển hình cho PID motor control:

DC Motor: 12V, 200 RPM (ví dụ JGA25-370)
Encoder: 11 PPR (Pulses Per Revolution), quadrature → 44 counts/rev
Motor driver: L298N H-bridge
Controller: Arduino Uno hoặc ESP32

Arduino code (đọc encoder + PID)

// Arduino PID Motor Control
#include <PID_v1.h>

// Encoder pins
#define ENCODER_A 2  // Interrupt pin
#define ENCODER_B 3
#define MOTOR_PWM 9
#define MOTOR_DIR 8

volatile long encoderCount = 0;
double setpoint = 100.0;  // Target RPM
double input = 0.0;       // Measured RPM
double output = 0.0;      // PWM output

// PID gains (tuned via Ziegler-Nichols)
double Kp = 2.0, Ki = 5.0, Kd = 0.1;
PID myPID(&input, &output, &setpoint, Kp, Ki, Kd, DIRECT);

unsigned long prevTime = 0;
long prevCount = 0;

void setup() {
    Serial.begin(115200);
    pinMode(ENCODER_A, INPUT_PULLUP);
    pinMode(ENCODER_B, INPUT_PULLUP);
    pinMode(MOTOR_PWM, OUTPUT);
    pinMode(MOTOR_DIR, OUTPUT);

    attachInterrupt(digitalPinToInterrupt(ENCODER_A), readEncoder, RISING);

    myPID.SetMode(AUTOMATIC);
    myPID.SetOutputLimits(-255, 255);
    myPID.SetSampleTime(10);  // 10ms = 100Hz
}

void loop() {
    unsigned long now = millis();
    if (now - prevTime >= 10) {
        // Tính RPM từ encoder counts
        long counts = encoderCount;
        double rpm = (double)(counts - prevCount) / 44.0 * 60000.0 / (now - prevTime);
        input = rpm;
        prevCount = counts;
        prevTime = now;

        // Compute PID
        myPID.Compute();

        // Apply output
        if (output >= 0) {
            digitalWrite(MOTOR_DIR, HIGH);
            analogWrite(MOTOR_PWM, (int)output);
        } else {
            digitalWrite(MOTOR_DIR, LOW);
            analogWrite(MOTOR_PWM, (int)(-output));
        }

        // Debug
        Serial.print(setpoint);
        Serial.print(",");
        Serial.println(input);
    }
}

void readEncoder() {
    if (digitalRead(ENCODER_B) == HIGH) encoderCount++;
    else encoderCount--;
}

ROS 2 Integration

Trong ROS 2, PID controller thường được implement trong ros2_control framework:

# ROS 2 PID node (simplified)
import rclpy
from rclpy.node import Node
from std_msgs.msg import Float64

class PIDNode(Node):
    def __init__(self):
        super().__init__('pid_controller')

        # Parameters
        self.declare_parameter('kp', 1.0)
        self.declare_parameter('ki', 0.5)
        self.declare_parameter('kd', 0.01)
        self.declare_parameter('setpoint', 0.0)

        kp = self.get_parameter('kp').value
        ki = self.get_parameter('ki').value
        kd = self.get_parameter('kd').value

        self.pid = PIDController(kp, ki, kd)
        self.setpoint = self.get_parameter('setpoint').value

        # Subscribers & Publishers
        self.sub = self.create_subscription(
            Float64, '/motor/velocity', self.feedback_callback, 10)
        self.pub = self.create_publisher(
            Float64, '/motor/command', 10)

        # 100Hz control loop
        self.timer = self.create_timer(0.01, self.control_loop)
        self.measurement = 0.0

    def feedback_callback(self, msg):
        self.measurement = msg.data

    def control_loop(self):
        output = self.pid.compute(self.setpoint, self.measurement, dt=0.01)
        cmd = Float64()
        cmd.data = output
        self.pub.publish(cmd)

def main():
    rclpy.init()
    node = PIDNode()
    rclpy.spin(node)

Với ros2_control, PID controller đã có sẵn trong package ros2_controllers dưới dạng forward_command_controller kết hợp với pid_controller. Xem thêm bài ros2_control hardware interface để biết cách kết nối với hardware thực tế.

Tips thực tế

1. Luôn filter derivative term

Sensor noise sẽ bị khuếch đại bởi derivative. Thêm low-pass filter:

# Exponential moving average cho derivative
alpha = 0.1  # Filter coefficient (0-1, nhỏ = smooth hơn)
filtered_derivative = alpha * raw_derivative + (1 - alpha) * prev_filtered

2. Derivative-on-measurement, không phải error

Như đã giải thích ở trên, luôn tính derivative từ measurement thay vì error để tránh derivative kick.

3. Integral clamping

Ngoài anti-windup, nên giới hạn giá trị tuyệt đối của integral term:

max_integral = 100.0
self._integral = np.clip(self._integral, -max_integral, max_integral)

4. Sample time nhất quán

PID hoạt động tốt nhất khi dt nhất quán. Trên microcontroller, dùng timer interrupt thay vì delay().

5. Feed-forward cho performance tốt hơn

Kết hợp PID với feed-forward term khi biết model của system:

# Feed-forward + PID
ff_term = setpoint * feed_forward_gain  # Open-loop estimate
pid_term = pid.compute(setpoint, measurement)
output = ff_term + pid_term  # Tổng hợp

Đi tiếp từ PID

PID là bước đầu tiên. Khi system phức tạp hơn, bạn sẽ cần:

Cascade PID: Nhiều PID lồng nhau (position → velocity → current loop)
Gain scheduling: Thay đổi PID gains theo operating point
Model Predictive Control (MPC): Optimal control dựa trên model -- xem bài MPC cho Humanoid
Reinforcement Learning: Learn controller từ interaction -- xem series AI cho Robot