BÀI 60 – ĐO TỐC ĐỘ ENCODER CHÍNH XÁC

LOW-PASS FILTER + DERIVATIVE FILTER + PID ANTI-WINDUP TRÊN STM32F103

Đây là một bài rất quan trọng.

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

Trajectory
   ↓
Position Controller
   ↓
V, ω
   ↓
Inverse Kinematics
   ↓
VLd, VRd
   ↓
Speed PID
   ↓
PWM
   ↓
Motor
   ↓
Encoder

Nhưng để Speed PID hoạt động tốt, chúng ta phải trả lời một câu hỏi:

Encoder thực sự đang cho biết motor chạy bao nhiêu mm/s?

Nếu đo tốc độ không tốt thì:

Encoder nhiễu
      ↓
Speed nhiễu
      ↓
PID nhiễu
      ↓
PWM rung
      ↓
Motor giật

Bài 60 sẽ giải quyết toàn bộ chuỗi này.

---

1. Mục tiêu của Bài 60

Sau bài này bạn phải hiểu được 5 vấn đề:

① Đếm encoder

Encoder
   ↓
Δticks

② Từ tick → tốc độ

Δticks / Δt
      ↓
mm/s

③ Lọc tốc độ

Raw speed
   ↓
Low-pass filter
   ↓
Filtered speed

④ Lọc đạo hàm PID

Error
 ↓
Derivative
 ↓
Filter

⑤ Chống Integral Windup

PID
 ↓
Saturation
 ↓
Anti-windup

---

2. Encoder thực chất đo cái gì?

Encoder không trực tiếp đo:

mm/s

Nó chỉ tạo ra các xung.

Ví dụ:

A B A B A B A B ...

hoặc ở Encoder Mode:

TIMx->CNT

thay đổi:

1000
1001
1002
1003
...

Ta phải biến sự thay đổi đó thành tốc độ.

---

3. Công thức cơ bản

Gọi:

ΔN = số tick trong khoảng thời gian Δt

thì tốc độ tick:

[
f_{tick}=\frac{\Delta N}{\Delta t}
]

Nếu encoder có:

[
N_{rev}
]

tick/vòng và bánh xe có chu vi:

[
C=\pi D
]

thì:

[
V=
\frac{\Delta N}{N_{rev}}
\frac{C}{\Delta t}
]

---

4. Ví dụ

Bánh xe:

D = 65 mm

Encoder:

Nrev = 600 tick/rev

Trong:

Δt = 10 ms = 0.01 s

đếm được:

ΔN = 30 tick

Chu vi:

[
C=\pi\times65
]

≈ 204.2 mm.

Số vòng:

[
N=
\frac{30}{600}

0.05
]

Quãng đường:

[
S=0.05\times204.2
]

≈ 10.21 mm.

Tốc độ:

[
V=\frac{10.21}{0.01}
]

≈

[
\boxed{1021mm/s}
]

---

5. Đừng nhầm CPR với PPR

Đây là lỗi rất phổ biến.

Nhà sản xuất có thể ghi:

PPR = 300

nhưng khi dùng quadrature:

x1
x2
x4

thì số count có thể khác.

Ví dụ:

PPR = 300

với x4:

[
CPR=1200
]

Do đó trong code phải biết chính xác:

Timer Encoder Mode đang đếm bao nhiêu count cho một vòng encoder?

---

6. STM32F103 Encoder Mode

Ví dụ dùng:

TIM2 CH1
TIM2 CH2

Timer có:

TIM2->CNT

Giả sử lần trước:

previous_count = 10000;

lần này:

current_count = 10035;

thì:

delta =
    current_count -
    previous_count;

[
\Delta N=35
]

---

7. Vấn đề Counter Overflow

Đây là điểm rất quan trọng.

Timer 16-bit:

0
1
2
...
65534
65535
0
1
2
...

Ví dụ:

previous = 65530
current  = 5

Nếu dùng:

int delta = current - previous;

có thể gặp vấn đề tùy kiểu dữ liệu.

Cách đơn giản:

int16_t delta =
    (int16_t)(current_count -
              previous_count);

Với timer 16-bit, cách này tận dụng phép toán số nguyên có dấu để xử lý wrap-around trong phạm vi phù hợp.

---

8. Tại sao không đo mỗi 1 ms?

Giả sử:

Encoder = 600 CPR

motor chạy rất chậm.

Trong 1 ms có thể:

Δtick = 0

Kết quả:

speed = 0

ms tiếp theo:

Δtick = 1

Kết quả lại nhảy rất lớn.

Ta sẽ thấy:

0
0
0
1020
0
0
1020
...

Speed PID sẽ rất khó hoạt động.

---

9. Vì vậy phải chọn cửa sổ đo

Ví dụ:

10 ms
20 ms
50 ms
100 ms

Cửa sổ càng dài:

+ độ phân giải tốc độ tốt hơn
- phản ứng chậm hơn

Cửa sổ càng ngắn:

+ phản ứng nhanh
- nhiễu nhiều

Đây là một trade-off quan trọng.

---

10. Với robot 2 bánh nhỏ

Một điểm bắt đầu hợp lý có thể là:

Encoder measurement: 10–20 ms
Speed PID:            10–20 ms
Odometry:             10–20 ms
Trajectory controller:20 ms

Sau đó đo thực tế rồi điều chỉnh.

Không có một con số “đúng” cho mọi motor.

---

11. Tạo cấu trúc Encoder Speed

typedef struct
{
    int32_t previous_count;

    float speed_raw;
    float speed_filtered;

    float wheel_diameter;
    float counts_per_rev;

} EncoderSpeed_t;

---

12. Hàm tính tốc độ

float Encoder_GetSpeed(
    EncoderSpeed_t *enc,
    int32_t current_count,
    float dt)
{
    int32_t delta;

    float distance;

    delta =
        current_count -
        enc->previous_count;

    enc->previous_count =
        current_count;

    distance =
        ((float)delta /
         enc->counts_per_rev)
        *
        (float)M_PI *
        enc->wheel_diameter;

    enc->speed_raw =
        distance / dt;

    return enc->speed_raw;
}

Ví dụ:

float speed =
    Encoder_GetSpeed(
        &left_encoder,
        TIM2->CNT,
        0.01f);

---

13. Nhưng tốc độ Raw rất nhiễu

Ví dụ:

100
102
97
104
99
103
96

Trong khi tốc độ thật có thể gần:

100

Nếu đưa trực tiếp vào PID:

Raw speed
    ↓
PID
    ↓
PWM

PWM sẽ rung.

---

14. Low-Pass Filter

Ta dùng bộ lọc thông thấp bậc nhất.

Công thức:

[
y_k=y_{k-1}+\alpha(x_k-y_{k-1})
]

Trong đó:

x = raw
y = filtered

---

15. Ý nghĩa

Nếu:

raw = 120
filtered = 100

với:

α = 0.2

thì:

[
y=100+0.2(120-100)
]

[
y=104
]

Không nhảy ngay lên 120.

---

16. Code Low-Pass Filter

typedef struct
{
    float alpha;
    float output;

} LowPassFilter_t;

Hàm:

float LPF_Update(
    LowPassFilter_t *filter,
    float input)
{
    filter->output +=
        filter->alpha *
        (input -
         filter->output);

    return filter->output;
}

---

17. Khởi tạo

LowPassFilter_t left_speed_filter;

left_speed_filter.alpha =
    0.2f;

left_speed_filter.output =
    0.0f;

Sau đó:

speed_filtered =
    LPF_Update(
        &left_speed_filter,
        speed_raw);

---

18. alpha càng nhỏ thì sao?

Ví dụ:

α = 0.8

Raw → Filter

phản ứng nhanh.

Nhưng lọc yếu.

---

Nếu:

α = 0.05

lọc mạnh hơn.

Nhưng:

phản ứng chậm

---

19. Quan hệ với cutoff frequency

Với bộ lọc bậc nhất rời rạc, có thể chọn:

[
\alpha=
\frac{dt}{RC+dt}
]

với:

[
RC=\frac{1}{2\pi f_c}
]

Do đó:

[
\boxed{
\alpha=
\frac{dt}
{\frac{1}{2\pi f_c}+dt}
}
]

Đây là cách tốt hơn việc chọn alpha hoàn toàn bằng cảm tính.

---

20. Ví dụ

Giả sử:

dt = 0.01 s
fc = 5 Hz

Ta có:

[
RC=
\frac{1}{2\pi5}
]

≈ 0.0318 s.

Do đó:

[
\alpha=
\frac{0.01}{0.0318+0.01}
]

≈

[
\boxed{0.239}
]

Có thể bắt đầu với:

alpha = 0.24f;

rồi kiểm tra thực tế.

---

21. Một điều quan trọng

Đừng nghĩ:

“Lọc càng mạnh càng tốt.”

Không.

Nếu lọc quá mạnh:

Encoder
 ↓
LPF quá mạnh
 ↓
speed phản ứng chậm
 ↓
PID thấy thông tin cũ
 ↓
robot phản ứng chậm

Do đó:

Lọc đủ để giảm nhiễu, nhưng không được làm mất động học cần thiết của hệ thống.

---

22. Bây giờ đến D-term

PID:

[
u=K_pe+K_i\int e,dt+K_d\frac{de}{dt}
]

Trong đó:

[
\frac{de}{dt}
]

rất nhạy với nhiễu.

Ví dụ error:

10
11
10
12
9
11

Derivative có thể:

+100
-100
+200
-300
+200

nếu dt nhỏ.

---

23. Vì sao D-term đặc biệt nhạy?

Đạo hàm là:

[
D=\frac{x_k-x_{k-1}}{dt}
]

Nếu:

dt = 0.01 s

thì một thay đổi nhỏ:

Δx = 1

đã cho:

[
D=100
]

Do đó nhiễu nhỏ có thể trở thành tín hiệu rất lớn.

---

24. Không nên lọc Error rồi mới đạo hàm một cách máy móc

Một cách tốt hơn trong Speed PID là:

lọc tín hiệu đạo hàm hoặc dùng derivative-on-measurement.

Đặc biệt trong vòng tốc độ:

speed measurement

thường đã là tín hiệu có ý nghĩa vật lý trực tiếp.

---

25. Derivative on Measurement

Thay vì:

[
D=K_d\frac{d(error)}{dt}
]

ta có thể dùng:

[
D=-K_d\frac{d(measurement)}{dt}
]

Tại sao dấu âm?

Vì:

[
e=SP-PV
]

nên:

[
\frac{de}{dt}

-\frac{dPV}{dt}
]

Ưu điểm lớn:

Khi setpoint thay đổi đột ngột, derivative không tạo “kick” lớn chỉ vì bước nhảy của setpoint.

---

26. Derivative Filter

Ta có derivative raw:

[
D_{raw}

-\frac{PV_k-PV_{k-1}}{dt}
]

Sau đó lọc:

[
D=
D_{prev}+
\alpha_D(D_{raw}-D_{prev})
]

---

27. Code

float PID_DerivativeFiltered(
    PID_t *pid,
    float measurement,
    float dt)
{
    float derivative_raw;

    derivative_raw =
        -(measurement -
          pid->previous_measurement)
        / dt;

    pid->previous_measurement =
        measurement;

    pid->derivative_filtered +=
        pid->derivative_alpha *
        (derivative_raw -
         pid->derivative_filtered);

    return
        pid->derivative_filtered;
}

Cần bổ sung PID structure:

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

    float integral;

    float previous_measurement;

    float derivative_filtered;

    float derivative_alpha;

    float output_min;
    float output_max;

} PID_t;

---

28. PID hoàn chỉnh hơn

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

    error =
        setpoint -
        measurement;

    /*
     * Derivative
     */

    derivative =
        -(measurement -
          pid->previous_measurement)
        / dt;

    pid->previous_measurement =
        measurement;

    /*
     * Filter derivative
     */

    pid->derivative_filtered +=
        pid->derivative_alpha *
        (derivative -
         pid->derivative_filtered);

    /*
     * Integral
     */

    pid->integral +=
        error * dt;

    /*
     * Calculate output
     */

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

    /*
     * Saturation
     */

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

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

    return output;
}

Nhưng vẫn còn một vấn đề:

Integral Windup.

---

29. Integral Windup là gì?

Giả sử motor bị kẹt.

PID thấy:

Target = 500
Actual = 0

nên:

error = 500

Integral liên tục:

500
1000
1500
2000
2500
...

Trong khi PWM đã:

PWM = MAX

Motor không thể nhanh hơn nữa.

---

30. Khi motor được giải phóng

Integral vẫn cực lớn:

I = 2500

→ PWM vẫn giữ rất cao.

Kết quả:

MOTOR VỌT

Đây là:

Integral Windup

---

31. Anti-Windup kiểu Conditional Integration

Ý tưởng:

Nếu output đã bão hòa và error vẫn đẩy output theo hướng bão hòa → không tích phân thêm.

Ví dụ:

if ((output >= max_output && error > 0) ||
    (output <= min_output && error < 0))
{
    /*
     * Không tích phân
     */
}
else
{
    pid->integral +=
        error * dt;
}

---

32. Code PID Anti-Windup

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

    error =
        setpoint -
        measurement;

    /*
     * Derivative on measurement
     */

    derivative =
        -(measurement -
          pid->previous_measurement)
        / dt;

    pid->previous_measurement =
        measurement;

    /*
     * Filter derivative
     */

    pid->derivative_filtered +=
        pid->derivative_alpha *
        (derivative -
         pid->derivative_filtered);

    /*
     * Candidate integral
     */

    integral_candidate =
        pid->integral +
        error * dt;

    /*
     * Candidate output
     */

    output =
        pid->Kp * error +
        pid->Ki *
        integral_candidate +
        pid->Kd *
        pid->derivative_filtered;

    /*
     * Anti-windup
     */

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

        if (error < 0.0f)
        {
            pid->integral =
                integral_candidate;
        }
    }
    else if (output < pid->output_min)
    {
        output =
            pid->output_min;

        if (error > 0.0f)
        {
            pid->integral =
                integral_candidate;
        }
    }
    else
    {
        pid->integral =
            integral_candidate;
    }

    return output;
}

---

33. Ý tưởng của đoạn code trên

Ví dụ output đã:

MAX

và error:

+

thì:

error đang muốn output cao hơn

→ không cho integral tiếp tục tăng.

Nhưng nếu:

output = MAX
error = -

thì error đang muốn kéo output xuống.

→ cho phép integral giảm.

Đây là logic rất quan trọng.

---

34. Một phương pháp Anti-Windup mạnh hơn

Có một phương pháp rất hay:

Back Calculation

Ta thêm:

[
I_{aw}

K_{aw}(u_{sat}-u)
]

Trong đó:

u     = output trước saturation
usat  = output sau saturation

Sau đó:

[
I=
I+
(K_i e+
K_{aw}(u_{sat}-u))dt
]

Phương pháp này cho phép integral “quay trở lại” khi actuator bão hòa.

---

35. Khi nào dùng Conditional Integration?

Ưu điểm:

đơn giản
dễ hiểu
dễ debug

Rất phù hợp cho:

STM32F103
robot nhỏ

Tôi khuyên bạn bắt đầu với nó.

---

36. Khi nào dùng Back Calculation?

Khi hệ thống đã ổn định và bạn muốn:

PID chuyên nghiệp hơn

thì dùng:

Back Calculation

đặc biệt khi:

actuator saturation thường xuyên

---

37. Một vấn đề nữa: PWM âm

Robot 2 bánh có thể:

VL = +200

hoặc:

VL = -200

Do đó PID output phải cho phép:

-MAX_PWM
...
0
...
+MAX_PWM

Ví dụ:

pid.output_min = -1000.0f;
pid.output_max =  1000.0f;

---

38. Sau đó tách chiều motor

if (pwm_left >= 0)
{
    Motor_Left_Forward();
    Motor_Left_PWM(
        (uint16_t)pwm_left);
}
else
{
    Motor_Left_Backward();
    Motor_Left_PWM(
        (uint16_t)(-pwm_left));
}

---

39. Tốc độ encoder cũng có dấu

Ví dụ:

Forward:
+200 mm/s

Reverse:

-200 mm/s

Do đó:

speed_raw =
    distance / dt;

distance phải giữ dấu dựa trên chiều encoder.

Không được:

abs(delta)

nếu bạn cần phân biệt tiến/lùi.

---

40. Low-pass Filter cũng giữ dấu

Ví dụ:

Raw:
+200

Filtered:
+180

Khi đảo chiều:

Raw:
-200

filter sẽ từ từ đi:

+180
+100
+20
-50
-120
-160
-190

Đó là hành vi bình thường.

---

41. Một vấn đề thực tế: Encoder tốc độ thấp

Giả sử:

10 ms

nhưng chỉ có:

0 tick

thì:

speed = 0

Mặc dù bánh xe thực sự đang quay.

Có hai phương pháp đo tốc độ:

Frequency method

Đo:

thời gian giữa hai xung

Periodic counting

Đếm:

số xung trong một khoảng thời gian

---

42. Periodic counting

Đây là cách chúng ta đang dùng:

10 ms
 ↓
đếm tick
 ↓
speed

Ưu điểm:

đơn giản
ổn định

Nhược điểm:

tốc độ rất thấp → độ phân giải kém

---

43. Frequency / Period Measurement

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

tick
   │
   │
   │
   ●────────●
       T

thì:

[
f=\frac1T
]

Sau đó:

[
V=
\frac{f}{CPR}C
]

Phương pháp này có độ phân giải tốt hơn ở tốc độ thấp.

Nhưng phức tạp hơn.

Đó sẽ là một chủ đề nâng cao sau này.

---

44. Một bộ Speed Estimator tốt

Có thể thiết kế:

             Encoder
                │
          ┌─────┴─────┐
          │           │
       Tick count   Period
          │           │
          └─────┬─────┘
                ↓
         Speed Estimator
                ↓
          Raw Speed
                ↓
            LPF
                ↓
       Filtered Speed
                ↓
             PID

---

45. Tại sao không dùng LPF quá mạnh rồi bỏ D-filter?

Bởi vì hai bộ lọc phục vụ hai mục đích khác nhau.

Speed LPF

Lọc:

measurement

Derivative filter

Lọc:

d(measurement)/dt

Bạn có thể cần cả hai.

Nhưng phải tránh:

LPF quá mạnh
+
D filter quá mạnh

→ hệ thống phản ứng quá chậm.

---

46. Một cấu hình ban đầu

Ví dụ:

#define SPEED_DT        0.01f
#define SPEED_LPF_ALPHA 0.20f
#define D_FILTER_ALPHA  0.20f

PID:

PID_t pid_left =
{
    .Kp = 2.0f,
    .Ki = 5.0f,
    .Kd = 0.01f,

    .integral = 0.0f,

    .previous_measurement = 0.0f,

    .derivative_filtered = 0.0f,

    .derivative_alpha = 0.2f,

    .output_min = -1000.0f,
    .output_max = 1000.0f
};

Lưu ý: Đây chỉ là giá trị minh họa. Không nên lấy nguyên các gain này để áp dụng cho motor của bạn.

---

47. Quy trình tuning đúng

Đây là phần tôi muốn bạn đặc biệt chú ý.

Bước 1 – Ki = 0

Kp > 0
Ki = 0
Kd = 0

Tăng Kp từ thấp lên.

---

Bước 2

Quan sát:

Target = 200
Actual

Nếu:

Actual → 200

nhưng còn sai số:

Actual = 180

thì cần Integral.

---

Bước 3

Tăng Ki từ từ.

Mục tiêu:

Target = 200
Actual ≈ 200

---

Bước 4

Chỉ thêm Kd nếu thực sự cần.

Rất nhiều hệ thống tốc độ motor có thể hoạt động tốt với:

PI

mà không cần D.

---

48. Một điều rất đáng nhớ

Đối với wheel speed PID, hãy thử:

[
\boxed{PI}
]

trước.

Tức là:

P + I

Không phải lúc nào cũng cần:

PID

Bởi vì encoder speed đã có nhiễu và D-term có thể làm hệ thống khó tuning hơn.

---

49. Kiến trúc thực tế nên là

Encoder
   ↓
Speed Estimator
   ↓
Low Pass Filter
   ↓
PI / PID
   ↓
Anti-Windup
   ↓
PWM

---

50. Toàn bộ Bài 60

                  ENCODER
                     │
                     ▼
                CNT / TICKS
                     │
                     ▼
              Δtick / Δtime
                     │
                     ▼
               RAW SPEED
                     │
                     ▼
              LOW PASS FILTER
                     │
                     ▼
            FILTERED SPEED
                     │
                     ├─────────────┐
                     │             │
                     │        Derivative
                     │             │
                     │             ▼
                     │        D FILTER
                     │             │
                     ▼             │
                   ERROR           │
                     │             │
                     └──────┬──────┘
                            ▼
                           PID
                            │
                       Anti-Windup
                            │
                            ▼
                          PWM
                            │
                            ▼
                          MOTOR
                            │
                            └──→ Encoder

---

51. Bài thực hành STM32F103

Bạn hãy tạo chương trình test chưa cần robot chạy trajectory.

Chỉ cần:

Encoder
+
Motor
+
PWM
+
UART

Cho motor chạy một tốc độ cố định:

Target = 200 mm/s

UART in mỗi 100 ms:

Target
Raw
Filtered
Error
Integral
Derivative
PWM

Ví dụ:

Target:     200
Raw:        178
Filtered:   181
Error:       19
Integral:    12
Derivative: -4
PWM:        435

Sau vài giây mong muốn:

Target:     200
Raw:        199
Filtered:   200
Error:        0
PWM:        420

Đây mới là lúc bạn có một wheel speed controller thực sự.

---

52. Bài kiểm tra rất quan trọng

Thử 4 trường hợp:

Test 1

Target = 100

Test 2

Target = 200

Test 3

Target = 300

Test 4

Target:
0 → 200 → 0 → -200 → 0

Nếu cả bốn trường hợp đều ổn:

không rung mạnh
không overshoot lớn
không tích phân vô hạn

thì Speed PID của bạn đã khá tốt.

---

53. Và đây là mảnh ghép còn thiếu

Sau Bài 60, hệ thống của chúng ta đã có:

           TRAJECTORY
                ↓
      X,Y,θ Controller
        FF + Feedback
                ↓
              V,ω
                ↓
       Inverse Kinematics
                ↓
             VL,VR
                ↓
        ┌───────┴───────┐
        ↓               ↓
    Speed PID L      Speed PID R
        ↓               ↓
      PWM L           PWM R
        ↓               ↓
     Motor L         Motor R
        ↓               ↓
    Encoder L       Encoder R
        └───────┬───────┘
                ↓
             ODOMETRY
                ↓
              X,Y,θ

Đây là nền tảng điều khiển chuyển động hoàn chỉnh cho robot 2 bánh.

---

54. Bước tiếp theo – Bài 61

Sau khi đã có Speed PID + lọc + Anti-Windup, bài rất đáng học tiếp sẽ là:

BÀI 61 – KALMAN FILTER CHO ENCODER + IMU: ƯỚC LƯỢNG TỐC ĐỘ, GÓC θ VÀ SENSOR FUSION

Lúc đó chúng ta sẽ gặp một vấn đề mới:

Encoder
   ↓
Odometry
   ↓
θ bị drift

Trong khi IMU cho:

Gyroscope → tốc độ quay
Accelerometer → gia tốc
Magnetometer → hướng

Ta sẽ học cách kết hợp:

Encoder + Gyroscope + Accelerometer
             ↓
        Sensor Fusion
             ↓
          X,Y,θ

và từ đó robot sẽ tiến từ odometry thuần encoder sang ước lượng trạng thái robot (state estimation) — một bước rất quan trọng trước khi học LQR, EKF và SLAM.