BÀI 49 – PID ĐIỀU KHIỂN VỊ TRÍ MOTOR + CASCADE POSITION → SPEED TRÊN STM32F103

Posted on by congnghe9s

BÀI 49 – PID ĐIỀU KHIỂN VỊ TRÍ MOTOR + CASCADE POSITION → SPEED TRÊN STM32F103

Bài 48 chúng ta đã làm điều khiển vị trí bằng Encoder. Nhưng dùng P-position trực tiếp để điều khiển PWM có một nhược điểm lớn: motor có quán tính, nên dễ vượt vị trí đích và dao động.

Bài 49 sẽ giải quyết vấn đề đó bằng kiến trúc cascade control:

TARGET POSITION
       │
       ▼
┌──────────────────┐
│ POSITION CONTROL │
└────────┬─────────┘
         │ Target Speed
         ▼
┌──────────────────┐
│    SPEED PID     │
└────────┬─────────┘
         │ PWM
         ▼
       MOTOR
         │
         ▼
      ENCODER
         │
      ┌──┴──────────┐
      ▼             ▼
  POSITION        SPEED
      │             │
      └─────────────┘

Đây là một kiến trúc rất quan trọng trong điều khiển servo.

1. Tại sao phải có hai vòng điều khiển?

Giả sử:

Target = 2000 counts
Current = 0

Nếu chúng ta làm trực tiếp:

Position Error
      ↓
PWM

thì:

Error lớn → PWM lớn
Error nhỏ → PWM nhỏ

Nhưng PWM không trực tiếp kiểm soát tốc độ.

Motor có quán tính:

PWM
 ↓
Motor tăng tốc
 ↓
PWM giảm
 ↓
Motor vẫn còn đang quay
 ↓
Vượt Target

Do đó ta tách thành hai nhiệm vụ:

Vòng vị trí

Tôi còn cách đích bao xa?

Vòng tốc độ

Motor đang chạy nhanh hay chậm hơn tốc độ mong muốn?

2. Cấu trúc Cascade

Vòng ngoài:

Target Position
       ↓
Position Controller
       ↓
Target RPM

Vòng trong:

Target RPM
       ↓
Speed PID
       ↓
PWM

Encoder cung cấp cả:

Position
Speed

3. Ví dụ rất dễ hiểu

Giả sử:

Target Position = 2000 counts
Current Position = 0

Position controller thấy:

Error = 2000

và quyết định:

Target Speed = 100 RPM

Motor chạy.

Khi:

Current = 1500

thì:

Error = 500

Position controller giảm:

Target Speed = 40 RPM

Khi:

Current = 1900

thì:

Error = 100

Target speed chỉ còn:

10 RPM

Khi:

Current = 1998

thì:

Target Speed ≈ 0

Motor dừng rất nhẹ nhàng.

4. Công thức vòng vị trí

Ở phiên bản đầu tiên, chúng ta dùng P:

[
Speed_{target}=K_{pos}\times PositionError
]

Trong đó:

PositionError =
TargetPosition - CurrentPosition

Ví dụ:

Kpos = 0.05 RPM/count
Error = 1000

thì:

[
TargetSpeed=50RPM
]

5. Giới hạn tốc độ

Không được để:

Error = 100000

và:

TargetSpeed = 5000 RPM

nếu motor chỉ chạy được 150 RPM.

Do đó:

#define MAX_SPEED_RPM 150.0f

Sau đó:

if (target_speed > MAX_SPEED_RPM)
    target_speed = MAX_SPEED_RPM;

if (target_speed < -MAX_SPEED_RPM)
    target_speed = -MAX_SPEED_RPM;

6. Vòng tốc độ

Bây giờ chúng ta có:

Target RPM

và Encoder đo được:

Actual RPM

Sai số:

[
e(t)=RPM_{target}-RPM_{actual}
]

Sau đó đưa vào PID:

[
u(t)=K_p e(t)+K_i\int e(t)dt+K_d\frac{de(t)}{dt}
]

Kết quả:

PID output
      ↓
PWM

7. Đây chính là sự khác nhau giữa Bài 46 và Bài 49

Bài 46

Target RPM
    ↓
PWM

đơn giản.

Bài 47

Target RPM
    ↓
Speed PID
    ↓
PWM

Bài 49

Target Position
       ↓
Position Controller
       ↓
Target RPM
       ↓
Speed PID
       ↓
PWM

Đây là bước nâng cấp rất lớn.

8. Tạo cấu trúc PID

Ta có thể viết một cấu trúc C:

typedef struct
{
    float kp;
    float ki;
    float kd;

    float integral;
    float previous_error;

    float output_min;
    float output_max;

} PID_t;

Ví dụ:

PID_t speed_pid =
{
    .kp = 2.0f,
    .ki = 0.5f,
    .kd = 0.05f,

    .integral = 0.0f,
    .previous_error = 0.0f,

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

9. Hàm PID

Giả sử PID chạy mỗi:

10 ms

thì:

float PID_Update(
    PID_t *pid,
    float target,
    float actual,
    float dt)
{
    float error;
    float derivative;
    float output;

    error = target - actual;

    pid->integral +=
        error * dt;

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

    output =
        pid->kp * error
        + pid->ki * pid->integral
        + pid->kd * derivative;

    pid->previous_error =
        error;

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

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

    return output;
}

Đây là PID cơ bản.

10. Nhưng PID này còn một lỗi

Đó là:

Integral Windup

Ví dụ motor bị kẹt:

Target = 100 RPM
Actual = 0 RPM

PID cứ nghĩ:

Error = 100

và cộng dồn:

Integral
Integral
Integral
Integral
...

đến rất lớn.

Sau khi motor được giải phóng, PID có thể:

PWM = 100%

trong một thời gian dài.

11. Chống Integral Windup

Ta giới hạn Integral:

#define INTEGRAL_MAX 500.0f

Sau:

pid->integral +=
    error * dt;

thêm:

if (pid->integral >
    INTEGRAL_MAX)
{
    pid->integral =
        INTEGRAL_MAX;
}

if (pid->integral <
    -INTEGRAL_MAX)
{
    pid->integral =
        -INTEGRAL_MAX;
}

12. Phiên bản PID tốt hơn

float PID_Update(
    PID_t *pid,
    float target,
    float actual,
    float dt)
{
    float error;
    float derivative;
    float output;

    error =
        target - actual;

    pid->integral +=
        error * dt;

    if (pid->integral >
        INTEGRAL_MAX)
    {
        pid->integral =
            INTEGRAL_MAX;
    }

    if (pid->integral <
        -INTEGRAL_MAX)
    {
        pid->integral =
            -INTEGRAL_MAX;
    }

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

    output =
        pid->kp * error
        + pid->ki *
          pid->integral
        + pid->kd *
          derivative;

    pid->previous_error =
        error;

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

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

    return output;
}

13. Position Controller

Bây giờ viết vòng ngoài:

float Position_To_Speed(
    int32_t target,
    int32_t actual)
{
    float error;
    float speed;

    error =
        target - actual;

    speed =
        POSITION_KP * error;

    if (speed >
        MAX_SPEED_RPM)
    {
        speed =
            MAX_SPEED_RPM;
    }

    if (speed <
        -MAX_SPEED_RPM)
    {
        speed =
            -MAX_SPEED_RPM;
    }

    return speed;
}

14. Ví dụ

#define POSITION_KP 0.05f
#define MAX_SPEED_RPM 150.0f

Nếu:

Target = 2000
Actual = 1000

thì:

Error = 1000

và:

Target Speed = 50 RPM

Nếu:

Target = 2000
Actual = 1900

thì:

Error = 100

→

Target Speed = 5 RPM

15. Khi gần vị trí

Ta thêm:

#define POSITION_TOLERANCE 3

Nếu:

if (abs(error) <=
    POSITION_TOLERANCE)

thì:

target_speed = 0;

16. Nhưng còn một vấn đề

Giả sử:

Target = 2000
Actual = 1998

Position controller:

Target Speed = 0

Speed PID thấy:

Target RPM = 0
Actual RPM = 5

nó sẽ tạo:

PWM âm

để hãm motor.

Đây chính là điều chúng ta muốn.

17. Hệ thống hoàn chỉnh

                   TARGET POSITION
                          │
                          ▼
                 ┌─────────────────┐
                 │ Position P      │
                 │ Controller      │
                 └────────┬────────┘
                          │
                       Target RPM
                          │
                          ▼
                 ┌─────────────────┐
                 │   Speed PID     │
                 └────────┬────────┘
                          │
                         PWM
                          │
                          ▼
                       MOTOR
                          │
                          ▼
                      ENCODER
                     ┌────┴────┐
                     │         │
                     ▼         ▼
                 POSITION     RPM
                     │         │
                     └─────────┘

18. Chu kỳ chạy hai vòng

Đây là phần cực kỳ quan trọng.

Không nên chạy mọi thứ trong:

while(1)

một cách không kiểm soát.

Ta có thể dùng Timer 1 ms từ Bài 36.

Ví dụ:

Timer 1 ms
    │
    ├── 1 ms: Encoder update
    │
    ├── 10 ms: Speed PID
    │
    └── 10 ms: Position Controller

19. Tại sao Speed PID phải nhanh hơn Position Loop?

Bởi vì:

Speed

thay đổi nhanh.

Còn:

Position

thay đổi chậm hơn.

Ví dụ:

Speed PID:
10 ms

Position loop:
20–50 ms

Một cấu hình đơn giản:

Speed PID       = 10 ms
Position loop   = 20 ms

20. State Machine cho Position Control

Đừng chỉ có:

RUN

Nên có:

typedef enum
{
    POSITION_IDLE,
    POSITION_MOVING,
    POSITION_REACHED
} PositionState_t;

21. Trạng thái IDLE

Không có lệnh di chuyển

Motor:

STOP

22. Trạng thái MOVING

Target != Current

Position loop hoạt động.

23. Trạng thái REACHED

Khi:

abs(error) <= tolerance

thì:

Target Speed = 0

và motor được giữ ở vị trí.

24. Hàm Position_Start()

Ví dụ:

void Position_Start(
    int32_t target)
{
    target_position =
        target;

    position_state =
        POSITION_MOVING;
}

Gọi:

Position_Start(2000);

motor sẽ đi đến:

2000 counts

25. Position Update

void Position_Update(void)
{
    int32_t error;
    float target_speed;

    error =
        target_position -
        encoder.position;

    if (error <=
        POSITION_TOLERANCE &&
        error >=
        -POSITION_TOLERANCE)
    {
        target_speed = 0;

        position_state =
            POSITION_REACHED;
    }
    else
    {
        target_speed =
            Position_To_Speed(
                target_position,
                encoder.position);

        position_state =
            POSITION_MOVING;
    }

    speed_target =
        target_speed;
}

26. Speed PID Update

void Speed_Update(void)
{
    float actual_rpm;
    float pwm;

    actual_rpm =
        Encoder_GetRPM();

    pwm =
        PID_Update(
            &speed_pid,
            speed_target,
            actual_rpm,
            0.01f);

    Motor_SetPWM(
        (int16_t)pwm);
}

27. Toàn bộ hệ thống

Ví dụ:

void Control_Task(void)
{
    Encoder_Update();

    if (tick_10ms)
    {
        tick_10ms = 0;

        Speed_Update();
    }

    if (tick_20ms)
    {
        tick_20ms = 0;

        Position_Update();
    }
}

28. Nhưng thứ tự cần lưu ý

Trong thực tế ta muốn:

Encoder
   ↓
Position/RPM
   ↓
Position Controller
   ↓
Speed PID
   ↓
PWM

Vì vậy một chu kỳ điều khiển có thể:

1. Đọc encoder
2. Tính position
3. Tính RPM
4. Position loop
5. Speed PID
6. Cập nhật PWM

29. Ví dụ thực tế

Giả sử:

Encoder CPR = 400
Target Position = 2000
MAX RPM = 100
Kpos = 0.05

Ban đầu:

Position = 0
Error = 2000
Target RPM = 100

Sau một thời gian:

Position = 1000
Error = 1000
Target RPM = 50

Tiếp tục:

Position = 1700
Error = 300
Target RPM = 15

Gần đích:

Position = 1950
Error = 50
Target RPM = 2.5

Cuối cùng:

Position = 1998
Error = 2

Nếu:

Tolerance = 3

thì:

REACHED

30. Một điểm cực kỳ quan trọng: tốc độ âm

Nếu:

Target = 1000
Actual = 1500

thì:

Error = -500

Position Controller:

Target RPM = -25

Speed PID sẽ cố gắng đưa motor quay ngược.

Như vậy hệ thống có khả năng tự sửa vị trí:

1000 ← 1500

31. Điều này rất quan trọng cho robot

Ví dụ robot cần quay bánh đến:

5000 counts

nhưng bánh đã đi:

5100 counts

thì controller không đơn giản là:

STOP

mà có thể:

Error = -100
↓
Target Speed = -5 RPM
↓
Motor quay ngược
↓
Position → 5000

32. Tuning Cascade

Đây là phần quan trọng nhất khi chạy phần cứng.

Bước 1 – Tắt Position Loop

Chỉ test:

Speed PID

Làm cho motor đạt RPM ổn định.

Bước 2

Bật Position P:

Kpos nhỏ

Ví dụ:

0.01

Bước 3

Ra lệnh:

1000 counts

Bước 4

Quan sát:

Target
Actual
RPM
PWM

Bước 5

Tăng:

Kpos

từ từ.

33. Nếu xảy ra hiện tượng này

1000
 ↓
950
 ↓
1030
 ↓
980
 ↓
1020
 ↓
...

→ Position loop quá mạnh hoặc tốc độ tối đa quá cao.

Giảm:

Kpos

hoặc:

MAX_SPEED_RPM

34. Nếu motor tiến tới đích quá chậm

Ví dụ:

1000
 ↓
200
 ↓
400
 ↓
600
 ↓
800
 ↓
...

→ có thể:

Kpos quá nhỏ

hoặc:

MAX_SPEED quá thấp

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

Để học và thử nghiệm, có thể bắt đầu:

#define POSITION_KP       0.02f
#define MAX_SPEED_RPM     80.0f
#define POSITION_TOLERANCE 3

Speed PID:

kp = nhỏ
ki = nhỏ
kd = 0

Sau khi hệ thống chạy ổn định mới thêm:

I

và cuối cùng:

D

Không nên vừa thay Kp, Ki, Kd và Kpos cùng lúc, vì sẽ rất khó biết thông số nào gây ra lỗi.

36. Một kiến trúc code nên hướng tới

Sau Bài 49, code của bạn có thể được chia thành:

main.c
│
├── encoder.c
│   ├── Encoder_Init()
│   ├── Encoder_Update()
│   ├── Encoder_GetPosition()
│   └── Encoder_GetRPM()
│
├── motor.c
│   ├── Motor_Init()
│   ├── Motor_Forward()
│   ├── Motor_Reverse()
│   ├── Motor_Stop()
│   └── Motor_SetPWM()
│
├── pid.c
│   ├── PID_Init()
│   ├── PID_Update()
│   └── PID_Reset()
│
└── position.c
    ├── Position_Start()
    ├── Position_Update()
    └── Position_GetState()

Đây chính là cách tư duy driver + control algorithm thay vì nhét toàn bộ code vào main.c.

37. Kiến thức quan trọng nhất của Bài 49

Bạn hãy nhớ chuỗi này:

ENCODER
   │
   ├──────────────► POSITION
   │
   └──────────────► SPEED
                         │
                         ▼
TARGET POSITION
       │
       ▼
POSITION CONTROLLER
       │
       ▼
TARGET SPEED
       │
       ▼
SPEED PID
       │
       ▼
PWM
       │
       ▼
MOTOR
       │
       └────────────► ENCODER

Đây là cascade position → speed control.

38. Và đây là bước chuyển rất lớn

Từ đầu khóa học:

GPIO
 ↓
Timer
 ↓
PWM
 ↓
Encoder

bây giờ chúng ta đã đến:

Sensor
 ↓
Feedback
 ↓
Controller
 ↓
Actuator
 ↓
Feedback

Đây chính là nền tảng của:

  • Servo motor
  • Robot 2 bánh
  • Robot arm
  • CNC
  • 3D printer
  • Gimbal
  • AGV
  • Máy tự động công nghiệp

Bài 50 sẽ rất thú vị

Sau khi đã có position + speed + encoder, bước hợp lý tiếp theo là:

BÀI 50 – ĐIỀU KHIỂN 2 MOTOR ĐỘC LẬP BẰNG ENCODER + PID TRÊN STM32F103

Chúng ta sẽ chuyển từ:

STM32
  ↓
1 Motor
  ↓
1 Encoder

sang:

                 STM32F103
                /         \
               /           \
          LEFT MOTOR     RIGHT MOTOR
              │               │
          Encoder L       Encoder R
              │               │
              └──────┬────────┘
                     ↓
                 2 PID LOOP

Và từ đó có thể bắt đầu xây dựng robot 2 bánh chạy thẳng chính xác bằng encoder, là nền tảng rất phù hợp với robot tránh vật cản mà bạn đang làm.

Leave a Reply

Your email address will not be published. Required fields are marked *