BÀI 55 – TRAJECTORY TRACKING TRÊN STM32F103

Robot 2 bánh bám đường thẳng, đường tròn và đường cong bằng X, Y, θ

Đây là bài rất quan trọng vì từ Bài 53–54 chúng ta mới chỉ nói:

“Robot hãy đi đến điểm A.”

Còn từ Bài 55, chúng ta bắt đầu nói:

“Robot hãy đi theo đúng quỹ đạo này.”

Ví dụ:

Bài 53:

Robot ───────────────► ● TARGET


Bài 55:

Robot ───╮
         │
         ╰──────╮
                │
                ╰──────►

Robot phải liên tục so sánh:

Vị trí thực tế (X,Y,θ)
        ↓
Vị trí mong muốn trên quỹ đạo
        ↓
Sai số
        ↓
V, ω
        ↓
2 bánh
        ↓
PID

---

1. Path và Trajectory khác nhau thế nào?

Đây là khái niệm đầu tiên cần phân biệt.

Path – đường đi

Path chỉ quan tâm:

Robot phải đi qua đâu?

Ví dụ:

A ───────────── B

Không quan tâm robot đi mất bao lâu.

---

Trajectory – quỹ đạo theo thời gian

Trajectory quan tâm cả:

X(t)
Y(t)
θ(t)

Ví dụ:

t = 0s     → X=0,   Y=0
t = 1s     → X=100, Y=0
t = 2s     → X=200, Y=20
t = 3s     → X=300, Y=80

Nghĩa là:

Không chỉ phải đi đúng đường mà còn phải đi đúng tiến độ.

---

2. Trong bài này chúng ta tập trung vào Path Tracking

Đầu tiên chưa cần làm trajectory quá phức tạp.

Ta cho robot một đường mong muốn:

P_d = (X_d, Y_d)

và robot thực tế:

P = (X, Y)

Robot liên tục tính:

error = desired - actual

---

3. Hệ tọa độ

Ta tiếp tục sử dụng:

                 +Y
                  ↑
                  │
                  │
                  │
        -X ◄──────┼──────► +X
                  │
                  │
                  ↓
                 -Y

Robot có:

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

} Pose_t;

---

4. Quỹ đạo mong muốn

Ta cũng tạo:

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

} TrajectoryPoint_t;

Ví dụ:

TrajectoryPoint_t target;

target.x = 1000.0f;
target.y = 500.0f;
target.theta = 0.0f;

---

5. Sai số vị trí

Ta có:

float ex;
float ey;

với:

ex = target.x - pose.x;
ey = target.y - pose.y;

Đây là sai số trong hệ tọa độ toàn cục.

---

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

Giả sử robot đang hướng:

θ = 90°

Robot nhìn về:

↑

nhưng:

ex = +100
ey = 0

Mục tiêu nằm bên phải theo hệ tọa độ thế giới:

TARGET ●

       ↑ robot
       │
       │
       ● ROBOT

Đối với robot, mục tiêu nằm bên phải.

Vì vậy tốt hơn là chuyển sai số từ:

WORLD FRAME

sang:

ROBOT FRAME

---

7. Robot frame

Ta định nghĩa:

                 Forward
                   ↑
                   │
                   │
             Robot ●────→ Right

Robot luôn coi:

X_robot = phía trước
Y_robot = bên trái

Đây là convention rất tiện cho điều khiển.

---

8. Chuyển sai số sang Robot Frame

Công thức:

[
e_x^r =
\cos\theta , e_x+
\sin\theta , e_y
]

[
e_y^r =
-\sin\theta , e_x+
\cos\theta , e_y
]

Trong đó:

ex, ey

là sai số trong World Frame.

---

9. Code

void WorldToRobotError(
    float ex,
    float ey,
    float theta,
    float *ex_r,
    float *ey_r)
{
    float c;
    float s;

    c = cosf(theta);
    s = sinf(theta);

    *ex_r =
        c * ex +
        s * ey;

    *ey_r =
       -s * ex +
        c * ey;
}

Đây là phép rotation transform rất quan trọng trong robotics.

---

10. Tại sao làm như vậy?

Ví dụ robot đang:

θ = 0°

thì:

Robot X → World X
Robot Y → World Y

Không cần biến đổi phức tạp.

Nhưng khi:

θ = 90°

hệ tọa độ robot đã quay.

Do đó controller cần biết:

Mục tiêu nằm trước robot bao nhiêu?
Mục tiêu nằm bên trái/phải bao nhiêu?

chứ không chỉ biết:

X thế giới lệch bao nhiêu?
Y thế giới lệch bao nhiêu?

---

11. Đường thẳng – bài toán đầu tiên

Giả sử muốn robot chạy trên:

Y = 0

genui{“graphable_function_v2_learning_block_parameterized”:{“expressions”:[{“latex”:”y=0″,”restrictions”:[“-1000\le x\le 1000″]}],”locale_override”:”vi-VN”}}

Robot có thể đang:

        Robot
          ●
         /
        /
───────/──────────── đường mong muốn

Robot cần sửa sai lệch ngang.

---

12. Cross-track error

Đây là một khái niệm cực kỳ quan trọng.

Cross-track error là:

Khoảng cách robot lệch khỏi đường mong muốn theo phương vuông góc với đường.

Ví dụ:

Robot
  ●
  │
  │ e_y
  │
  ▼
────────────────────
      Path

Nếu:

e_y = 100 mm

robot đang lệch khỏi đường:

100 mm

---

13. Với đường thẳng ngang

Nếu path:

Y = 0

thì:

cross_track_error =
    pose.y;

hoặc tùy convention dấu:

cross_track_error =
    target_y - pose.y;

Điều quan trọng là giữ dấu nhất quán.

---

14. Controller đơn giản

Ta có:

cross-track error
       ↓
      PID/P
       ↓
      ω

Ví dụ:

omega =
    -K_Y *
    cross_track_error;

Trong khi:

V = 200;

---

15. Ví dụ

Robot lệch:

ey = +100 mm

và:

K_Y = 0.01

thì:

ω = -1 rad/s

Robot quay về phía đường.

---

16. Nhưng chỉ dùng Y là chưa đủ

Nếu robot đang:

Y = 0

nhưng hướng:

θ = 30°

thì nó vẫn không bám đường tốt.

Ta cần thêm:

Heading Error

---

17. Heading error

Đường mong muốn có hướng:

θ_d

robot hiện tại:

θ

sai số:

heading_error =
    NormalizeAngle(
        theta_d - theta);

---

18. Hai loại sai số

Đây là phần rất quan trọng:

          Path
─────────────────────────→
        ↑
        │ cross-track
        │ error
        ● Robot
         ↗
        θ

Ta có:

① Cross-track error

e_y

Robot lệch khỏi đường bao nhiêu.

② Heading error

e_theta

Robot đang hướng lệch khỏi hướng đường bao nhiêu.

---

19. Controller đường thẳng

Một controller rất đơn giản:

[
\omega =
K_y e_y+
K_\theta e_\theta
]

Trong C:

omega =
    KY * cross_track_error +
    KTHETA * heading_error;

Sau đó:

Robot_SetVelocity(
    V,
    omega);

---

20. Đây chính là nền tảng của Path Tracking

             PATH
              │
       ┌──────┴──────┐
       ↓             ↓
 Cross-track      Heading
   error           error
       │             │
       └──────┬──────┘
              ↓
          Controller
              ↓
             ω
              ↓
       Robot_SetVelocity
              ↓
           V, ω

---

21. Code controller đường thẳng

float LineTracking(
    Pose_t *pose,
    float path_y,
    float path_theta,
    float forward_speed)
{
    float cross_error;
    float heading_error;
    float omega;

    cross_error =
        path_y - pose->y;

    heading_error =
        NormalizeAngleRad(
            path_theta -
            pose->theta);

    omega =
        KY * cross_error +
        KTHETA * heading_error;

    omega =
        ClampFloat(
            omega,
            -MAX_ANGULAR_SPEED,
            MAX_ANGULAR_SPEED);

    Robot_SetVelocity(
        forward_speed,
        omega);

    return omega;
}

---

22. Ví dụ sử dụng

Muốn robot chạy theo:

Y = 0

và hướng:

θ = 0

ta gọi:

LineTracking(
    &pose,
    0.0f,
    0.0f,
    200.0f);

Robot sẽ cố duy trì:

Y ≈ 0
θ ≈ 0

---

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

Controller trên giả sử đường thẳng luôn:

Y = constant

Còn nếu đường thẳng có góc:

30°

thì phải tính cross-track error theo hình học.

Ta sẽ làm điều đó ngay sau đây.

---

24. Đường thẳng tổng quát

Ta biểu diễn đường thẳng bằng hai điểm:

P1 = (x1,y1)
P2 = (x2,y2)

Ví dụ:

P1 ●────────────● P2

Vector hướng:

[
d=(x_2-x_1,;y_2-y_1)
]

---

25. Vector đơn vị của đường

Tính:

[
L=\sqrt{d_x^2+d_y^2}
]

sau đó:

[
t_x=\frac{d_x}{L}
]

[
t_y=\frac{d_y}{L}
]

Đây là vector đơn vị theo hướng đường.

---

26. Vector pháp tuyến

Vector vuông góc:

[
n_x=-t_y
]

[
n_y=t_x
]

Hình dung:

             n
             ↑
             │
             │
P1 ●─────────┼─────────● P2
             │
             │

---

27. Cross-track error

Robot:

P = (x,y)

Ta lấy:

dx = x - x1
dy = y - y1

Sau đó:

[
e_y = d_x n_x+d_y n_y
]

Trong C:

cross_error =
    dx * nx +
    dy * ny;

---

28. Heading của đường

Ta tính:

path_theta =
    atan2f(
        y2 - y1,
        x2 - x1);

Sau đó:

heading_error =
    NormalizeAngleRad(
        path_theta -
        robot_theta);

---

29. Controller hoàn chỉnh cho đoạn thẳng

void TrackLine(
    float x1,
    float y1,
    float x2,
    float y2,
    Pose_t *pose,
    float speed)
{
    float dx;
    float dy;
    float length;

    float tx;
    float ty;

    float nx;
    float ny;

    float rx;
    float ry;

    float cross_error;
    float path_theta;
    float heading_error;

    float omega;

    dx = x2 - x1;
    dy = y2 - y1;

    length =
        sqrtf(
            dx * dx +
            dy * dy);

    if (length < 0.001f)
        return;

    tx = dx / length;
    ty = dy / length;

    nx = -ty;
    ny = tx;

    rx = pose->x - x1;
    ry = pose->y - y1;

    cross_error =
        rx * nx +
        ry * ny;

    path_theta =
        atan2f(dy, dx);

    heading_error =
        NormalizeAngleRad(
            path_theta -
            pose->theta);

    omega =
        KY * cross_error +
        KTHETA * heading_error;

    omega =
        ClampFloat(
            omega,
            -MAX_ANGULAR_SPEED,
            MAX_ANGULAR_SPEED);

    Robot_SetVelocity(
        speed,
        omega);
}

Đây là một controller đường thẳng rất tốt để học.

---

30. Bây giờ đến đường tròn

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

Center = (Cx,Cy)
Radius = R

Quỹ đạo:

Robot phải duy trì:

[
(x-C_x)^2+(y-C_y)^2\approx R^2
]

---

31. Tính khoảng cách robot tới tâm

dx = pose.x - cx;
dy = pose.y - cy;

distance =
    sqrtf(
        dx * dx +
        dy * dy);

---

32. Sai số bán kính

[
e_r=distance-R
]

Nếu:

distance > R

robot đang ở ngoài đường tròn.

Nếu:

distance < R

robot đang ở bên trong.

---

33. Hướng tiếp tuyến

Đường tròn có một hướng tiếp tuyến tại mỗi điểm.

Nếu đi ngược chiều kim đồng hồ:

[
\theta_t =
atan2(y-C_y,x-C_x)+\frac{\pi}{2}
]

Nếu đi cùng chiều kim đồng hồ:

[
\theta_t =
atan2(y-C_y,x-C_x)-\frac{\pi}{2}
]

---

34. Controller đường tròn

Ta có hai sai số:

Radial error
+
Heading error

Sau đó:

[
\omega=
\omega_{feedforward}
+
K_r e_r
+
K_\theta e_\theta
]

Trong đó:

[
\omega_{feedforward}=\frac{V}{R}
]

Đây là một ý tưởng rất quan trọng:

Không chỉ sửa sai số; ta còn biết trước robot phải quay với tốc độ nào để chạy đúng đường tròn.

---

35. Code

void TrackCircle(
    float cx,
    float cy,
    float radius,
    float speed,
    Pose_t *pose)
{
    float dx;
    float dy;

    float distance;
    float radial_error;

    float radial_angle;
    float tangent_angle;

    float heading_error;

    float omega_ff;
    float omega;

    dx =
        pose->x - cx;

    dy =
        pose->y - cy;

    distance =
        sqrtf(
            dx * dx +
            dy * dy);

    radial_error =
        distance - radius;

    radial_angle =
        atan2f(dy, dx);

    tangent_angle =
        radial_angle +
        M_PI / 2.0f;

    tangent_angle =
        NormalizeAngleRad(
            tangent_angle);

    heading_error =
        NormalizeAngleRad(
            tangent_angle -
            pose->theta);

    omega_ff =
        speed / radius;

    omega =
        omega_ff +
        KR * radial_error +
        KTHETA * heading_error;

    omega =
        ClampFloat(
            omega,
            -MAX_ANGULAR_SPEED,
            MAX_ANGULAR_SPEED);

    Robot_SetVelocity(
        speed,
        omega);
}

---

36. omega_ff là gì?

ff nghĩa là:

Feed Forward

Nếu:

V = 200 mm/s
R = 500 mm

thì:

[
\omega=\frac{200}{500}=0.4
]

Robot biết trước rằng muốn chạy vòng tròn bán kính 500 mm thì phải quay khoảng:

0.4 rad/s

Không cần chờ sai số xuất hiện mới sửa.

---

37. Feedback + Feedforward

Đây là cấu trúc tốt:

          Feedforward
               │
               ▼
             ω_ff
               │
               ├──────────┐
               │          │
               ▼          ▼
                         Feedback
                           │
                           ▼
                      correction
                           │
               ┌───────────┘
               ▼
          ω = ωff + correction

Đây là một bước tiến lớn so với chỉ dùng P/PID.

---

38. Đường cong bất kỳ

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

Line
Circle

mà có thể tạo một danh sách điểm:

typedef struct
{
    float x;
    float y;

} PathPoint_t;

Ví dụ:

PathPoint_t path[] =
{
    {0,   0},
    {200, 0},
    {400, 50},
    {600, 150},
    {800, 300},
    {1000,500}
};

Robot phải đi qua chuỗi điểm này.

---

39. Nhưng không nên đơn giản là GoTo() từng điểm

Nếu làm:

GoTo(P1)
GoTo(P2)
GoTo(P3)
GoTo(P4)

robot có thể:

→ P1
  quay
→ P2
  quay
→ P3

quỹ đạo sẽ bị giật.

Ta muốn:

●
 ╲
  ╲
   ╲
    ╲
     ●
      ╲
       ╲
        ●

một đường cong mượt hơn.

---

40. Đây là lúc xuất hiện khái niệm Look-Ahead

Thay vì nhắm vào:

điểm gần nhất

robot nhìn về phía trước một khoảng:

L_d

gọi là:

Look-ahead distance

---

41. Hình dung

                 Look Ahead
                     ●
                    /
                   /
                  /
Robot ●──────────●

Robot không cố bám đúng điểm ngay dưới nó.

Nó hướng tới một điểm phía trước quỹ đạo.

Đây là ý tưởng nền tảng của:

Pure Pursuit

---

42. Pure Pursuit

Nguyên lý:

PATH
─────────────────────────────
             ● target point
            /
           /
          /
       ● ROBOT

Tìm một điểm trên đường cách robot:

L_d

sau đó tính góc robot phải hướng tới điểm đó.

---

43. Góc tới điểm look-ahead

Sau khi tìm được:

target = (xt,yt)

ta tính:

dx = xt - pose.x;
dy = yt - pose.y;

target_angle =
    atan2f(dy, dx);

Sau đó:

alpha =
    NormalizeAngleRad(
        target_angle -
        pose.theta);

---

44. Công thức Pure Pursuit

Với robot differential-drive:

[
\kappa =
\frac{2\sin\alpha}{L_d}
]

Trong đó:

κ = curvature
α = góc tới điểm look-ahead
Ld = look-ahead distance

Sau đó:

[
\omega=V\kappa
]

Hay:

[
\boxed{
\omega=
V\frac{2\sin\alpha}{L_d}
}
]

Đây là một trong những công thức nổi tiếng nhất trong mobile robotics.

---

45. Code Pure Pursuit cơ bản

float PurePursuit(
    Pose_t *pose,
    float target_x,
    float target_y,
    float lookahead,
    float speed)
{
    float dx;
    float dy;

    float target_angle;
    float alpha;

    float curvature;
    float omega;

    dx =
        target_x -
        pose->x;

    dy =
        target_y -
        pose->y;

    target_angle =
        atan2f(dy, dx);

    alpha =
        NormalizeAngleRad(
            target_angle -
            pose->theta);

    curvature =
        2.0f *
        sinf(alpha) /
        lookahead;

    omega =
        speed *
        curvature;

    Robot_SetVelocity(
        speed,
        omega);

    return omega;
}

---

46. Đây là bước rất quan trọng

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

Path
 ↓
Look-ahead point
 ↓
α
 ↓
Curvature κ
 ↓
ω
 ↓
Inverse Kinematics
 ↓
VL, VR

---

47. Tại sao Pure Pursuit hay?

Nó không cần tính PID trực tiếp trên:

X
Y

mà dùng hình học để tạo ra:

curvature

Sau đó robot tự điều chỉnh hướng.

Đặc biệt phù hợp với robot:

2 bánh

và:

xe tự hành

---

48. Nhưng Look-ahead phải chọn hợp lý

Nếu:

Ld quá nhỏ

robot sẽ:

giật

và phản ứng quá mạnh.

Nếu:

Ld quá lớn

robot sẽ:

cắt góc

và bám đường kém chính xác.

---

49. Đây là trade-off

Ld nhỏ
  ↓
chính xác hơn
nhưng dễ rung


Ld lớn
  ↓
mượt hơn
nhưng cắt góc nhiều hơn

Một cách nâng cao là cho:

Ld = f(V)

Tốc độ càng cao:

Ld ↑

Tốc độ thấp:

Ld ↓

---

50. Ví dụ

lookahead =
    100.0f;

ở:

V = 100 mm/s

có thể tốt.

Nhưng khi:

V = 500 mm/s

có thể cần:

Ld = 250 mm

để robot ổn định hơn.

---

51. Một controller hoàn chỉnh có thể có dạng

void PathTracking_Update(
    Pose_t *pose,
    PathPoint_t *path,
    uint16_t count)
{
    PathPoint_t target;

    target =
        FindLookAheadPoint(
            pose,
            path,
            count);

    PurePursuit(
        pose,
        target.x,
        target.y,
        LOOKAHEAD_MM,
        TARGET_SPEED);
}

Phần khó ở đây là:

FindLookAheadPoint()

---

52. Tìm Look-Ahead Point

Ý tưởng:

1. Tìm điểm gần robot nhất
2. Đi về phía trước path
3. Chọn điểm cách robot khoảng Ld

Ví dụ:

PATH:

P0──P1──P2──P3──P4──P5──P6
        ↑
      robot

Nếu:

Ld = 100 mm

thì chọn một điểm phía trước:

P4

hoặc nội suy giữa:

P3 ↔ P4

để có chính xác hơn.

---

53. Nội suy

Giả sử:

P1 = (100,100)
P2 = (200,150)

ta có thể tìm một điểm giữa:

P(t)

với:

[
P(t)=P_1+t(P_2-P_1)
]

Trong C:

x =
    x1 +
    t * (x2 - x1);

y =
    y1 +
    t * (y2 - y1);

Đây là linear interpolation.

---

54. Một lưu ý quan trọng về STM32F103

Pure Pursuit có:

sinf()
cosf()
atan2f()
sqrtf()

Các phép toán floating-point này có thể khá nặng đối với:

STM32F103C8T6
72 MHz
Cortex-M3

Nhưng:

Với chu kỳ điều khiển khoảng 10–20 ms, một lượng vừa phải phép toán float vẫn hoàn toàn khả thi cho project học tập/robot nhỏ.

Chỉ cần tránh gọi hàng nghìn lần mỗi giây một cách không cần thiết.

---

55. Chu kỳ điều khiển nên như thế nào?

Một kiến trúc hợp lý:

1 ms
 ↓
Timer tick

10 ms
 ↓
Speed PID

20 ms
 ↓
Odometry
 ↓
Path Tracking

Ví dụ:

if (control_10ms)
{
    Speed_PID_Update();
}

if (control_20ms)
{
    Odometry_Update();
    PathTracking_Update();
}

Không nhất thiết mọi thứ phải chạy ở cùng một tần số.

---

56. Kiến trúc firmware lúc này

Project của bạn bắt đầu có thể tổ chức như sau:

STM32_ROBOT/
│
├── motor.c
├── motor.h
│
├── encoder.c
├── encoder.h
│
├── pid.c
├── pid.h
│
├── odometry.c
├── odometry.h
│
├── motion.c
├── motion.h
│
├── trajectory.c
├── trajectory.h
│
├── path.c
├── path.h
│
└── main.c

---

57. Vai trò từng module

motor.c

PWM
DIR

encoder.c

COUNT
RPM
wheel speed

pid.c

speed control

odometry.c

X,Y,θ

motion.c

V,ω

trajectory.c

desired X,Y,θ

path.c

path points
look-ahead

---

58. Chuỗi xử lý cuối cùng

Đây là sơ đồ bạn nên ghi nhớ:

                  PATH
                   │
                   ▼
          Look-Ahead Point
                   │
                   ▼
                  α
                   │
                   ▼
             Curvature κ
                   │
                   ▼
               V, ω
                   │
                   ▼
        INVERSE KINEMATICS
             ↙         ↘
          VL             VR
           ↓              ↓
       Speed PID      Speed PID
           ↓              ↓
          PWM            PWM
           ↓              ↓
        Motor L         Motor R
           ↓              ↓
       Encoder L      Encoder R
             ↘         ↙
              ODOMETRY
                 ↓
               X,Y,θ
                 │
                 └────────── feedback

Đây chính là một closed-loop trajectory tracking system.

---

59. Bài tập 1 – bám đường thẳng

Cho:

P1 = (0,0)
P2 = (2000,0)

Robot bắt đầu:

X = 0
Y = 100
θ = 0

Mục tiêu:

Y → 0

Bạn hãy quan sát:

Y
↓
100
80
60
40
20
10
5
0

Nếu Y giảm dần về 0 mà không dao động mạnh → controller đang hoạt động tốt.

---

60. Bài tập 2 – bám đường nghiêng

Cho:

P1 = (0,0)
P2 = (1000,1000)

Đường:

      /
     /
    /
   /
  /
 /

Robot phải tự hướng:

θ ≈ 45°

và bám đường.

---

61. Bài tập 3 – đường tròn

Cho:

Center = (500,500)
Radius = 300 mm

Robot phải chạy:

        ●
     ╱     ╲
   ●         ●
   │         │
   ●         ●
     ╲     ╱
        ●

Theo dõi:

radial_error

Nếu controller tốt:

radial_error → gần 0

---

62. Bài tập 4 – Pure Pursuit

Tạo:

PathPoint_t path[] =
{
    {0,0},
    {100,20},
    {200,50},
    {300,100},
    {400,180},
    {500,300},
    {600,450}
};

Cho robot:

V = 100 mm/s

và:

LOOKAHEAD = 100 mm

Quan sát robot bám đường.

---

63. Các thông số bạn cần tuning

Có 4 nhóm chính:

Đường thẳng

KY
KTHETA

Đường tròn

KR
KTHETA

Pure Pursuit

LOOKAHEAD

Motor

PID_LEFT
PID_RIGHT

---

64. Thứ tự tuning đúng

Không nên tuning tất cả cùng lúc.

Hãy làm:

1. Motor PWM
       ↓
2. Encoder
       ↓
3. Speed PID
       ↓
4. Odometry
       ↓
5. V,ω
       ↓
6. Inverse Kinematics
       ↓
7. Line Tracking
       ↓
8. Circle Tracking
       ↓
9. Pure Pursuit

Nếu Speed PID chưa ổn mà đã tuning Pure Pursuit thì rất khó biết lỗi nằm ở đâu.

---

65. Một lỗi rất phổ biến

Nếu robot bám đường bị:

↗
↘
↗
↘

liên tục, nguyên nhân có thể là:

K quá lớn

Giảm:

KY

hoặc tăng:

LOOKAHEAD

nếu đang dùng Pure Pursuit.

---

66. Nếu robot phản ứng quá chậm

Ví dụ:

──────────────
        ↗
       /
      ●

robot mất rất lâu mới quay về đường.

Có thể:

K quá nhỏ

hoặc:

LOOKAHEAD quá lớn

---

67. Nếu robot chạy vòng cung cắt đường

Ví dụ:

Path
──────────────────

Robot
  ╲
   ╲
    ╲
     ╲

thường:

LOOKAHEAD quá lớn

Giảm LOOKAHEAD.

---

68. Một điều rất quan trọng: θ phải chuẩn

Trajectory tracking phụ thuộc rất mạnh vào:

X
Y
θ

Nếu:

θ sai 10°

thì controller sẽ tưởng robot đang hướng khác thực tế.

Vì vậy Bài 52–53 về odometry không phải phần phụ.

Nó chính là feedback sensor của trajectory controller.

---

69. Đến đây robot của chúng ta đã có 5 lớp kiến thức

                 ROBOTICS
                    │
        ┌───────────┴───────────┐
        │                       │
     SENSING                 CONTROL
        │                       │
    Encoder                   PID
        │                       │
    Odometry              V, ω control
        │                       │
      X,Y,θ              Inverse Kinematics
        │                       │
        └──────────┬────────────┘
                   │
                   ▼
            PATH TRACKING
                   │
          Line / Circle / Curve

Đây đã là nền tảng thực sự của mobile robot.

---

70. Tóm tắt Bài 55

Bạn cần nhớ 6 khái niệm:

① Pose

X, Y, θ

② Cross-track error

Robot lệch khỏi đường bao nhiêu?

③ Heading error

Robot hướng lệch bao nhiêu?

④ Curvature

Robot cần cong đường mạnh đến mức nào?

⑤ Look-ahead

Robot nhìn về phía trước bao xa?

⑥ Pure Pursuit

[
\boxed{
\kappa=
\frac{2\sin\alpha}{L_d}
}
]

và:

[
\boxed{
\omega=V\kappa
}
]

Sau đó Bài 54 xử lý:

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

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

---

71. Chuỗi kiến thức từ Bài 50 đến Bài 55

Bạn có thể hình dung toàn bộ quá trình như sau:

BÀI 50
2 Motor + 2 Encoder + PID
        ↓
BÀI 51
Distance Control
        ↓
BÀI 52
Angle Control
        ↓
BÀI 53
Odometry X,Y,θ + GoTo
        ↓
BÀI 54
V,ω → VL,VR
        ↓
BÀI 55
PATH / TRAJECTORY TRACKING
        ↓
   Line
   Circle
   Curve
        ↓
   Pure Pursuit

Và đây là một cột mốc rất đáng chú ý:

Từ Bài 55, robot không còn đơn thuần là một hệ thống “điều khiển motor”. Nó đã trở thành một hệ thống điều khiển chuyển động có feedback theo vị trí.

---

🚀 Bài 56

Bước tiếp theo rất hợp lý là:

BÀI 56 – PURE PURSUIT CHUYÊN SÂU: LOOK-AHEAD ĐỘNG + BÁM QUỸ ĐẠO ĐA ĐIỂM TRÊN STM32F103

Chúng ta sẽ xây dựng hoàn chỉnh:

Path:
P0 → P1 → P2 → P3 → ... → Pn
                  │
                  ▼
          Find nearest point
                  │
                  ▼
        Find look-ahead point
                  │
                  ▼
              α
                  │
                  ▼
        Pure Pursuit Controller
                  │
                  ▼
              V, ω
                  │
                  ▼
          Inverse Kinematics
                  │
              ┌───┴───┐
              ▼       ▼
             VL      VR
              ↓       ↓
             PID     PID
              ↓       ↓
            Motor   Motor
              └───┬───┘
                  ▼
               Encoder
                  ↓
              X, Y, θ
                  └──────── feedback

Đặc biệt ở Bài 56, chúng ta sẽ viết hẳn một PathTracker.c/.h có thể chạy trên STM32F103, thay vì chỉ dừng ở công thức lý thuyết.