BÀI 59 – CASCADE CONTROL CHO ROBOT 2 BÁNH

Vòng điều khiển vị trí → vận tốc → PWM + Encoder trên STM32F103

Ở Bài 58, chúng ta đã có:

[
Trajectory
\rightarrow
X,Y,\theta
\rightarrow
V,\omega
]

Nhưng V, ω vẫn chưa trực tiếp điều khiển motor.

Bài 59 sẽ ghép toàn bộ các tầng điều khiển lại thành một hệ thống hoàn chỉnh:

        TRAJECTORY
             │
             ▼
      Position Controller
             │
          V , ω
             │
             ▼
      Inverse Kinematics
          ↙       ↘
       VLd         VRd
        │           │
        ▼           ▼
    Speed PID    Speed PID
        │           │
        ▼           ▼
      PWM L       PWM R
        │           │
        ▼           ▼
     Motor L     Motor R
        │           │
        └────┬──────┘
             ▼
          Encoder
             │
             ▼
         Odometry
             │
             ▼
          X,Y,θ
             │
             └──────── feedback

Đây là một trong những bài quan trọng nhất từ Bài 45 đến nay.

---

1. Tại sao phải dùng Cascade Control?

Giả sử chúng ta muốn robot chạy:

V = 300 mm/s
ω = 0

Nếu điều khiển trực tiếp PWM:

V = 300
 ↓
PWM = 150

thì sẽ không ổn định.

Bởi vì cùng PWM = 150 nhưng:

Pin đầy       → tốc độ A
Pin yếu       → tốc độ B
Sàn khác      → tốc độ C
Tải khác      → tốc độ D

Do đó ta cần:

PWM không phải mục tiêu. Tốc độ motor mới là mục tiêu.

---

2. Vòng trong: Speed PID

Ví dụ:

Tốc độ mong muốn:
VLd = 200 mm/s

Encoder đo:
VL = 170 mm/s

Sai số:

[
e=200-170=30
]

PID sẽ tăng PWM.

Nếu:

VL = 210

thì:

[
e=200-210=-10
]

PID giảm PWM.

---

3. Vòng ngoài: Position / Trajectory

Robot lại có một mục tiêu lớn hơn:

Xd
Yd
θd

và robot thực tế:

X
Y
θ

Controller tính:

V
ω

Sau đó mới chuyển thành:

VLd
VRd

---

4. Vì vậy chúng ta có hai loại PID khác nhau

PID tốc độ bánh

VLd → PID → PWM_L
VRd → PID → PWM_R

Controller quỹ đạo

Xd,Yd,θd
     ↓
X,Y,θ
     ↓
   V,ω

Không nên nhầm hai tầng này.

---

5. Cấu trúc dữ liệu

Trước tiên tạo:

typedef struct
{
    float x;
    float y;
    float theta;

} Pose_t;

Tốc độ bánh:

typedef struct
{
    float left;
    float right;

} WheelSpeed_t;

Lệnh vận tốc robot:

typedef struct
{
    float linear;
    float angular;

} RobotVelocity_t;

---

6. PID structure

typedef struct
{
    float Kp;
    float Ki;
    float Kd;

    float integral;
    float previous_error;

    float output_min;
    float output_max;

} PID_t;

---

7. Hàm PID

float PID_Update(
    PID_t *pid,
    float setpoint,
    float measurement,
    float dt)
{
    float error;
    float derivative;
    float output;

    error =
        setpoint -
        measurement;

    /*
     * Integral
     */

    pid->integral +=
        error * dt;

    /*
     * Anti-windup
     */

    if (pid->integral > pid->output_max)
        pid->integral = pid->output_max;

    if (pid->integral < pid->output_min)
        pid->integral = pid->output_min;

    /*
     * Derivative
     */

    derivative =
        (error -
         pid->previous_error)
        / dt;

    output =
        pid->Kp * error +
        pid->Ki * pid->integral +
        pid->Kd * derivative;

    pid->previous_error =
        error;

    /*
     * Output limit
     */

    if (output > pid->output_max)
        output = pid->output_max;

    if (output < pid->output_min)
        output = pid->output_min;

    return output;
}

---

8. Nhưng có một vấn đề

Nếu dùng:

derivative =
    (error - previous_error) / dt;

mà encoder bị nhiễu:

200
198
203
199
201

thì D-term có thể nhảy mạnh.

Ở bài sau chúng ta sẽ xử lý sâu vấn đề này bằng:

• derivative filtering
• low-pass filter
• velocity estimation
• anti-windup tốt hơn

Hiện tại chúng ta giữ controller đơn giản để hiểu kiến trúc.

---

9. Inverse Kinematics

Robot 2 bánh có khoảng cách hai bánh:

float WHEEL_BASE;

Công thức:

[
V_L=V-\frac{\omega W}{2}
] [
V_R=V+\frac{\omega W}{2}
]

Code:

WheelSpeed_t InverseKinematics(
    float V,
    float omega,
    float wheel_base)
{
    WheelSpeed_t w;

    w.left =
        V -
        omega * wheel_base / 2.0f;

    w.right =
        V +
        omega * wheel_base / 2.0f;

    return w;
}

---

10. Ví dụ

V = 300 mm/s
ω = 0.4 rad/s
W = 150 mm

Ta có:

[
V_L=300-\frac{0.4\times150}{2}
] [
V_L=270
]

và:

[
V_R=330
]

Robot sẽ:

Left  = 270
Right = 330

→ quay theo hướng tương ứng.

---

11. Đây là Feedforward

Trajectory đã cho:

Vd
ωd

nên chúng ta có:

VL_feedforward
VR_feedforward

thông qua inverse kinematics.

---

12. Nhưng còn Feedback?

Ở tầng position controller:

Xd,Yd,θd
       ↓
X,Y,θ
       ↓
error
       ↓
Vfb,ωfb

Sau đó:

[
V=V_d+V_{fb}
] [
\omega=\omega_d+\omega_{fb}
]

---

13. Controller vị trí hoàn chỉnh

void PositionController(
    const Pose_t *actual,
    const Pose_t *desired,
    float Vd,
    float omega_d,
    float *V,
    float *omega)
{
    float dx;
    float dy;

    float ex;
    float ey;
    float etheta;

    dx =
        desired->x -
        actual->x;

    dy =
        desired->y -
        actual->y;

    ex =
        cosf(actual->theta) * dx +
        sinf(actual->theta) * dy;

    ey =
        -sinf(actual->theta) * dx +
        cosf(actual->theta) * dy;

    etheta =
        NormalizeAngleRad(
            desired->theta -
            actual->theta);

    /*
     * Feedback
     */

    float Vfb =
        KX * ex;

    float Wfb =
        KY * ey +
        KTHETA * etheta;

    /*
     * Feedforward + Feedback
     */

    *V =
        Vd + Vfb;

    *omega =
        omega_d + Wfb;
}

---

14. Sau đó giới hạn

if (*V > MAX_LINEAR_SPEED)
    *V = MAX_LINEAR_SPEED;

if (*V < -MAX_LINEAR_SPEED)
    *V = -MAX_LINEAR_SPEED;

if (*omega > MAX_ANGULAR_SPEED)
    *omega = MAX_ANGULAR_SPEED;

if (*omega < -MAX_ANGULAR_SPEED)
    *omega = -MAX_ANGULAR_SPEED;

---

15. Sau đó Inverse Kinematics

WheelSpeed_t target;

target =
    InverseKinematics(
        V,
        omega,
        WHEEL_BASE);

Ta nhận được:

target.left
target.right

---

16. Encoder đo tốc độ thực tế

Giả sử:

target.left  = 250 mm/s
target.right = 300 mm/s

Encoder đo:

actual.left  = 230 mm/s
actual.right = 310 mm/s

PID trái:

error = 250 - 230
      = +20

→ tăng PWM trái.

PID phải:

error = 300 - 310
      = -10

→ giảm PWM phải.

---

17. Hai PID độc lập

float pwm_left =
    PID_Update(
        &pid_left,
        target.left,
        actual.left,
        dt);

float pwm_right =
    PID_Update(
        &pid_right,
        target.right,
        actual.right,
        dt);

---

18. Đây chính là Cascade

Ta có:

                  Xd Yd θd
                     │
                     ▼
             POSITION CONTROLLER
                     │
                  V, ω
                     │
                     ▼
             INVERSE KINEMATICS
                  ↙       ↘
               VLd        VRd
                │           │
                ▼           ▼
             PID LEFT    PID RIGHT
                │           │
                ▼           ▼
             PWM LEFT    PWM RIGHT

---

19. Tại sao PID tốc độ phải nhanh hơn controller vị trí?

Giả sử:

Position Controller
20 Hz

nhưng:

Speed PID
100 Hz

thì:

Position
  ↓
50 ms
  ↓
Speed target
  ↓
10 ms
  ↓
Motor correction

Motor được sửa nhanh hơn nhiều so với vị trí.

Nếu ngược lại:

Position: 100 Hz
Speed:    20 Hz

thì controller ngoài thay đổi lệnh quá nhanh nhưng vòng trong phản ứng chậm.

Điều đó có thể làm hệ thống rung hoặc khó tuning.

---

20. Kiến trúc thời gian trên STM32F103

Ta có thể dùng Timer tạo tick 1 ms:

1 ms
 │
 ├── encoder update
 │
 ├── speed calculation
 │
 ├── speed PID mỗi 10 ms
 │
 ├── odometry mỗi 10 ms
 │
 └── trajectory controller mỗi 20 ms

Ví dụ:

if ((millis() - last_speed) >= 10)
{
    last_speed += 10;

    SpeedPID_Update();
}

và:

if ((millis() - last_trajectory) >= 20)
{
    last_trajectory += 20;

    TrajectoryController_Update();
}

---

21. Tại sao không dùng HAL_Delay()?

Không nên:

HAL_Delay(20);

trong hệ thống điều khiển robot.

Bởi vì:

HAL_Delay
   ↓
CPU chờ
   ↓
controller không chạy

Trong khi robot cần:

Encoder
PID
Odometry
Trajectory
Communication

chạy liên tục.

Bài 36 đã cho chúng ta nền tảng millis() 1 ms để tránh kiểu blocking này.

---

22. Odometry

Từ encoder:

ΔL
ΔR

ta tính:

[
\Delta S=
\frac{\Delta L+\Delta R}{2}
] [
\Delta\theta=
\frac{\Delta R-\Delta L}{W}
]

Sau đó cập nhật:

[
x=x+\Delta S\cos(\theta+\Delta\theta/2)
] [
y=y+\Delta S\sin(\theta+\Delta\theta/2)
] [
\theta=\theta+\Delta\theta
]

Đây là cầu nối giữa:

Encoder

và:

X,Y,θ

---

23. Một chu kỳ hoàn chỉnh

Giả sử dt = 10 ms.

Bước 1

Encoder:

Left ticks
Right ticks

Bước 2

Tính:

VL
VR

Bước 3

Cập nhật:

X
Y
θ

Bước 4

Trajectory Generator:

Xd
Yd
θd
Vd
ωd

Bước 5

Position Controller:

V
ω

Bước 6

Inverse Kinematics:

VLd
VRd

Bước 7

Wheel PID:

PWM_L
PWM_R

Bước 8

Motor chạy.

Bước 9

Encoder lại đo.

LOOP

---

24. Sơ đồ cực kỳ quan trọng

                  ┌───────────────┐
                  │  TRAJECTORY   │
                  │ Generator     │
                  └───────┬───────┘
                          │
                   Xd,Yd,θd,Vd,ωd
                          │
                          ▼
              ┌─────────────────────┐
              │ Position Controller │
              │ FF + FB             │
              └──────────┬──────────┘
                         │
                       V,ω
                         │
                         ▼
                ┌─────────────────┐
                │ Inverse         │
                │ Kinematics      │
                └───────┬─────────┘
                        │
                    VLd    VRd
                     │      │
                     ▼      ▼
                  ┌───┐   ┌───┐
                  │PID│   │PID│
                  └─┬─┘   └─┬─┘
                    │       │
                   PWM     PWM
                    │       │
                    ▼       ▼
                  MOTOR   MOTOR
                    │       │
                    └───┬───┘
                        │
                     ENCODER
                        │
                        ▼
                     ODOMETRY
                        │
                        ▼
                      X,Y,θ
                        │
                        └─────── feedback

---

25. Một vấn đề lớn: Saturation

Giả sử:

MAX_PWM = 1000

nhưng PID tính:

PWM = 1500

Ta phải:

if (pwm > 1000)
    pwm = 1000;

Nhưng nếu chỉ làm như vậy, integral có thể tiếp tục tăng.

Đó là:

Integral Windup

Ví dụ:

PWM giới hạn
     ↑
     │
PID integral vẫn tăng
     │
     │
robot hết tải
     ↓
integral quá lớn
     ↓
robot vọt mạnh

Đây là vấn đề rất quan trọng của PID thực tế.

---

26. Anti-Windup đơn giản

Một cách:

if ((output >= pid->output_max &&
     error > 0) ||
    (output <= pid->output_min &&
     error < 0))
{
    /*
     * Không tiếp tục tích phân
     */
}
else
{
    pid->integral +=
        error * dt;
}

Đây tốt hơn cách tích phân vô điều kiện.

---

27. Tại sao bài này rất quan trọng?

Bởi vì từ đây robot của chúng ta không còn chỉ là:

Arduino
→ PWM
→ motor

mà đã trở thành một hệ thống điều khiển nhiều tầng:

POSITION
   ↓
TRAJECTORY
   ↓
VELOCITY
   ↓
WHEEL SPEED
   ↓
PWM
   ↓
MOTOR
   ↓
ENCODER
   ↓
ODOMETRY
   ↓
POSITION

Đây chính là tư duy của motion control.

---

28. Thực hành trên STM32F103

Với robot 2 bánh, tôi khuyên bạn xây theo thứ tự sau.

Bước 1 – chỉ Motor PWM

PWM → Motor

Bước 2 – Encoder

Motor → Encoder

Bước 3 – Wheel Speed

Encoder → mm/s

Bước 4 – Speed PID

Speed target
     ↓
PID
     ↓
PWM

Bước 5 – Odometry

Encoder
 ↓
X,Y,θ

Bước 6 – Inverse Kinematics

V,ω
 ↓
VL,VR

Bước 7 – Position Controller

Xd,Yd,θd
 ↓
V,ω

Bước 8 – Trajectory Generator

t
 ↓
Xd,Yd,θd,Vd,ωd

Đến đây mới ghép tất cả.

---

29. Bài thực hành quan trọng nhất

Cho robot:

Wheel base = 150 mm

đi từ:

(0,0,0)

đến:

(1000,0,0)

Trajectory:

Vmax = 300 mm/s
Acceleration = 100 mm/s²
Deceleration = 100 mm/s²

Controller:

KX
KY
KTHETA

Wheel PID:

Kp
Ki
Kd

UART in:

t

Xd
Yd
θd

X
Y
θ

ex
ey
eθ

Vd
ωd

V
ω

VLd
VRd

VL
VR

PWM_L
PWM_R

Nếu làm được bảng này, bạn sẽ nhìn thấy toàn bộ hệ thống điều khiển robot hoạt động như thế nào.

---

30. Một điều rất quan trọng về tuning

Đừng tuning tất cả cùng lúc.

Hãy làm theo thứ tự:

Tầng 1

Motor + Encoder

tuning:

Speed PID

cho đến khi:

Target 200
Actual ≈ 200

---

Tầng 2

Odometry

kiểm tra:

X,Y,θ

có đúng không.

---

Tầng 3

Inverse Kinematics

kiểm tra:

V,ω
→ VL,VR

---

Tầng 4

Position Controller

tuning:

KX
KY
KTHETA

---

Tầng 5

Trajectory

thêm:

Vd
ωd

---

31. Quy tắc tuning rất đáng nhớ

Vòng trong trước, vòng ngoài sau.

Motor Speed PID
       ↓
Wheel control
       ↓
Position control
       ↓
Trajectory tracking

Không nên làm ngược.

---

32. Bài 59 đã đưa chúng ta đến đâu?

Từ đầu chuỗi bài:

B45 Encoder Mode
      ↓
B46 Motor + Encoder + PWM
      ↓
B48 Position Control
      ↓
B53 Odometry
      ↓
B54 Inverse Kinematics
      ↓
B55 Trajectory Tracking
      ↓
B56 Pure Pursuit
      ↓
B57 Trajectory Generation
      ↓
B58 Feedforward + Feedback
      ↓
⭐ B59 Cascade Control

Robot bây giờ đã có kiến trúc điều khiển tương đối hoàn chỉnh.

---

33. Nhưng vẫn còn một vấn đề rất lớn

Trong Bài 59, chúng ta đang giả định:

Encoder → tốc độ chính xác

Nhưng thực tế:

Encoder
   ↓
ticks
   ↓
Δticks
   ↓
speed

Nếu tốc độ thấp:

100 ms
→ chỉ vài tick

thì phép đo tốc độ sẽ rất nhiễu.

Ngoài ra D-term của PID cũng rất nhạy với nhiễu.

Vì vậy bài tiếp theo nên đi sâu vào:

BÀI 60 – ĐO TỐC ĐỘ ENCODER CHÍNH XÁC + LOW-PASS FILTER + DERIVATIVE FILTER + PID ANTI-WINDUP

Chúng ta sẽ xây một Wheel Speed Controller thực tế cho STM32F103, xử lý luôn các vấn đề:

Encoder tick
     ↓
Velocity Estimation
     ↓
Low-pass Filter
     ↓
PID
     ↓
Anti-Windup
     ↓
PWM

Đây là bước rất đáng học trước khi chuyển sang các controller nâng cao hơn như LQR và sau đó MPC.