Motor Control cho AMR: vòng PID khép kín tốc độ từng bánh

Các bài trước đã học riêng lẻ từng mảnh: Động cơ, Motor Driver, Encoder, và nguyên lý PID ở chuyên mục Lập trình nhúng. Bài này ráp chúng lại thành một vòng điều khiển hoàn chỉnh — cụ thể là vòng điều khiển tốc độ cho một bánh xe AMR, mắt xích chạy hàng chục lần mỗi giây trên MCU của mọi AMR thực tế.

Khái niệm chính

Vòng điều khiển tốc độ một bánh xe gồm 4 thành phần nối tiếp thành một vòng kín:

• Setpoint — tốc độ mong muốn, tính ra từ bước mixing (đã học ở bài Differential Drive/Mecanum Drive)
• Bộ điều khiển PID — so sánh setpoint với tốc độ đo được, tính ra tín hiệu điều chỉnh
• Driver + Motor — nhận tín hiệu PWM, thực sự quay bánh xe
• Encoder — đo tốc độ quay thực tế, gửi phản hồi ngược lại cho PID

Đây chính là mô hình "closed-loop control" kinh điển — khác biệt cốt lõi với việc chỉ xuất PWM tỉ lệ thuận với tốc độ mong muốn (open-loop) mà không có phản hồi.

Nguyên lý hoạt động

Mỗi chu kỳ điều khiển (thường 50-100Hz), firmware thực hiện đúng 4 bước:

1. Đọc encoder → tính tốc độ thực tế đo được (đã học ở bài Encoder)
2. Tính sai số: error = setpoint − tốc_độ_đo_được
3. PID tính tín hiệu điều chỉnh: output = Kp×error + Ki×∫error + Kd×(d error/dt)
4. Chuyển output thành giá trị PWM, xuất cho driver động cơ

Vì sao cần cả 3 thành phần P, I, D thay vì chỉ P đơn giản: thành phần P phản ứng ngay với sai số hiện tại nhưng để lại sai số dư (steady-state error) khi có tải/ma sát cản trở liên tục; thành phần I tích luỹ sai số theo thời gian để triệt tiêu sai số dư đó; thành phần D phản ứng với tốc độ thay đổi của sai số, giúp giảm dao động (overshoot) khi setpoint thay đổi đột ngột. Nguyên lý đầy đủ về 3 thành phần này đã trình bày ở bài PID tại chuyên mục Lập trình nhúng — bài này áp dụng trực tiếp vào bài toán tốc độ bánh xe.

Kiến thức kỹ thuật

Công thức

Bộ điều khiển PID rời rạc (discrete), dạng thực thi trên MCU mỗi chu kỳ Δt:

error(t) = setpoint − đo_được(t)
integral += error(t) × Δt
derivative = (error(t) − error(t−1)) / Δt

output(t) = Kp×error(t) + Ki×integral + Kd×derivative
PWM = giới_hạn(output(t), PWM_min, PWM_max)

Bước giới_hạn (clamp/saturation) bắt buộc phải có — driver chỉ nhận PWM trong khoảng hợp lệ (VD: 0-255 hoặc -100% đến 100%), và thiếu bước này khiến integral có thể tích luỹ vượt mức không kiểm soát (integral windup) khi động cơ bị chặn cứng không quay được dù output tính ra rất lớn.

Thông số

Tham số	Ảnh hưởng khi tăng	Rủi ro nếu quá lớn
Kp	Phản ứng nhanh hơn với sai số	Dao động, overshoot
Ki	Triệt tiêu sai số dư nhanh hơn	Integral windup, dao động chậm kéo dài
Kd	Giảm overshoot, mượt hơn	Khuếch đại nhiễu đo từ encoder

Ứng dụng thực tế

Trong kiến trúc AMR thực tế, mỗi bánh xe chạy một instance PID độc lập với bộ gain (Kp, Ki, Kd) riêng — dù về lý thuyết 2 bánh trái/phải của một Differential Drive có thể dùng chung gain, thực tế lắp ráp (ma sát ổ trục không đều, dây điện dài ngắn khác nhau gây sụt áp khác nhau) khiến việc tinh chỉnh riêng từng bánh cho kết quả ổn định hơn đáng kể. Việc tinh chỉnh gain thường làm thủ công theo phương pháp thử-sai: tăng Kp tới khi bắt đầu dao động nhẹ, thêm Kd để giảm dao động đó, rồi thêm Ki nếu vẫn còn sai số dư khi tải nặng.

Ví dụ thực tế

def pid_step(setpoint, measured, state, Kp=2.0, Ki=0.8, Kd=0.05, dt=0.02, pwm_max=255):
    error = setpoint - measured
    state["integral"] += error * dt
    derivative = (error - state["prev_error"]) / dt

    output = Kp*error + Ki*state["integral"] + Kd*derivative
    pwm = max(-pwm_max, min(pwm_max, output))  # chống integral windup

    state["prev_error"] = error
    return pwm, state

state = {"integral": 0, "prev_error": 0}
setpoint = 0.5  # m/s

# Mo phong 5 chu ky, dong co khoi dong tu 0, dan tang toc do
measured_values = [0, 0.15, 0.32, 0.44, 0.49]
for v in measured_values:
    pwm, state = pid_step(setpoint, v, state)
    print(f"do_duoc={v:.2f} m/s -> PWM={pwm:.1f}")

Output:
do_duoc=0.00 m/s -> PWM=25.0
do_duoc=0.15 m/s -> PWM=22.6
do_duoc=0.32 m/s -> PWM=17.5
do_duoc=0.44 m/s -> PWM=10.8
do_duoc=0.49 m/s -> PWM=4.1

PWM giảm dần khi tốc độ đo được tiến gần setpoint — đúng hành vi mong đợi của vòng điều khiển khép kín, không phải giá trị PWM cố định như cách tính open-loop.

Lỗi thường gặp

• Thiếu bước giới hạn (clamp) PWM, gây integral windup — như đã nêu ở mục Công thức, động cơ bị chặn cứng khiến integral tích luỹ không giới hạn, khi vật cản được gỡ bỏ động cơ giật mạnh bất thường do sai số tích luỹ quá lớn.
• Tăng Kp quá cao để phản ứng nhanh, gây dao động — cần tăng Kd đi kèm để giảm overshoot, không chỉ tăng Kp một mình.
• Chạy vòng PID không đúng chu kỳ cố định — tính Δt không nhất quán (do code khác chạy chen giữa trên cùng MCU) khiến thành phần I và D tính sai, vòng điều khiển không ổn định.
• Dùng cùng bộ gain cho mọi bánh mà không kiểm tra thực tế — như đã nêu ở phần Ứng dụng, sai lệch cơ khí nhỏ giữa các bánh khiến gain tối ưu không hoàn toàn giống nhau, đặc biệt quan trọng với Mecanum Drive (4 bánh) đã học ở bài trước.

Kết luận

Vòng PID khép kín là mắt xích biến "công thức mixing tính ra tốc độ mong muốn" thành "bánh xe thực sự quay đúng tốc độ đó" — không có nó, robot chỉ điều khiển open-loop, trôi tốc độ theo tải và ma sát thực tế. Bài tiếp theo trong chuyên mục đi vào Odometry — khép kín vòng lặp lớn hơn: dùng chính dữ liệu encoder vừa dùng cho PID để tính ra vị trí robot theo thời gian.