BÀI 56 – PURE PURSUIT CHUYÊN SÂU

Look-Ahead động + bám quỹ đạo đa điểm trên STM32F103

Ở Bài 55, chúng ta đã biết ý tưởng Pure Pursuit:

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

và:

[
\omega=V\kappa
]

Trong Bài 56, chúng ta biến nó thành một Path Tracker thực sự cho robot 2 bánh.

Mục tiêu cuối bài:

Path:
P0 → P1 → P2 → P3 → P4 → ... → Pn
                    ↑
                  Robot
                    │
                    ▼
              Look-ahead
                    │
                    ▼
              Pure Pursuit
                    │
                    ▼
                  V, ω
                    │
                    ▼
              VL         VR
               ↓         ↓
             PID       PID
               ↓         ↓
             Motor     Motor
               ↓         ↓
           Encoder   Encoder
                ↘   ↙
                X,Y,θ
                  │
                  └──── feedback

---

1. Tại sao Bài 56 cần thiết?

Ở Bài 55, ta có thể đưa cho robot một điểm:

target_x
target_y

Nhưng robot thật cần một danh sách quỹ đạo:

P0
 ↓
P1
 ↓
P2
 ↓
P3
 ↓
P4
 ↓
P5

Ví dụ:

PathPoint_t path[] =
{
    {   0,   0 },
    { 100,  10 },
    { 200,  30 },
    { 300,  80 },
    { 400, 150 },
    { 500, 250 },
    { 600, 380 },
    { 700, 500 }
};

Robot phải tự biết:

“Tôi đang ở đâu trên path và bây giờ nên nhìn về điểm nào phía trước?”

Đó chính là nhiệm vụ của PathTracker.

---

2. Kiến trúc của Path Tracker

Ta chia thành 5 bước:

① Pose hiện tại
       ↓
② Tìm điểm gần nhất
       ↓
③ Tìm điểm Look-Ahead
       ↓
④ Tính α
       ↓
⑤ Tính V,ω

Sau đó:

V,ω
 ↓
Inverse Kinematics
 ↓
VL,VR

---

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

Tạo file:

PathTracker.h
PathTracker.c

Đầu tiên:

#ifndef PATH_TRACKER_H
#define PATH_TRACKER_H

#include <stdint.h>

typedef struct
{
    float x;
    float y;

} PathPoint_t;


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

} Pose_t;


typedef struct
{
    float speed;

    float angular_speed;

    float curvature;

    float alpha;

    float lookahead;

    uint16_t nearest_index;

    uint16_t target_index;

    uint8_t finished;

} PathTrackerOutput_t;

#endif

---

4. Tại sao cần nearest_index?

Giả sử path:

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

Robot đang gần:

P2

thì chúng ta không cần tìm kiếm toàn bộ path từ P0 mỗi lần.

Ta chỉ cần tìm quanh vùng:

P2 → P3 → P4 → ...

Điều này rất quan trọng khi path có:

100
500
1000

điểm.

---

5. Tìm điểm gần robot nhất

Công thức khoảng cách:

[
d^2=(x-x_i)^2+(y-y_i)^2
]

Không cần sqrtf().

Ta chỉ cần so sánh:

d2

với nhau.

Đây là một kỹ thuật tối ưu rất hay trên STM32.

---

6. Hàm tính khoảng cách bình phương

static float DistanceSquared(
    float x1,
    float y1,
    float x2,
    float y2)
{
    float dx;
    float dy;

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

    return dx * dx + dy * dy;
}

---

7. Tìm nearest point

static uint16_t FindNearestPoint(
    const PathPoint_t *path,
    uint16_t count,
    const Pose_t *pose,
    uint16_t start_index)
{
    uint16_t i;
    uint16_t best_index;

    float best_dist;
    float dist;

    best_index = start_index;

    best_dist =
        DistanceSquared(
            pose->x,
            pose->y,
            path[start_index].x,
            path[start_index].y);

    for (i = start_index + 1;
         i < count;
         i++)
    {
        dist =
            DistanceSquared(
                pose->x,
                pose->y,
                path[i].x,
                path[i].y);

        if (dist < best_dist)
        {
            best_dist = dist;
            best_index = i;
        }
    }

    return best_index;
}

Đây là phiên bản đơn giản.

Sau này ta có thể tối ưu chỉ tìm trong một cửa sổ:

nearest - N
đến
nearest + M

---

8. Look-Ahead động là gì?

Ở Bài 55:

Ld = constant

Ví dụ:

Ld = 100 mm

Nhưng robot chạy:

50 mm/s

và:

500 mm/s

thì cùng một Ld không phải lúc nào cũng tốt.

Vì vậy ta cho:

[
L_d=L_{min}+K_v|V|
]

---

9. Ví dụ

Cho:

Lmin = 80 mm
Kv = 0.5 s

Nếu:

V = 100 mm/s

thì:

[
L_d=80+0.5(100)
]

[
L_d=130mm
]

Nếu:

V = 300 mm/s

thì:

[
L_d=230mm
]

Tốc độ càng cao:

Look-ahead càng xa

---

10. Hàm tính Look-Ahead

static float CalculateLookAhead(
    float speed,
    float min_lookahead,
    float gain)
{
    float ld;

    ld =
        min_lookahead +
        gain * fabsf(speed);

    return ld;
}

Có thể giới hạn:

if (ld > MAX_LOOKAHEAD)
    ld = MAX_LOOKAHEAD;

---

11. Tìm Look-Ahead Point

Đây là phần quan trọng nhất.

Giả sử:

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

Robot cần tìm điểm:

distance(robot,target) >= Ld

---

12. Phiên bản đơn giản

static uint16_t FindLookAheadPoint(
    const PathPoint_t *path,
    uint16_t count,
    const Pose_t *pose,
    uint16_t nearest_index,
    float lookahead)
{
    uint16_t i;

    float dist2;
    float lookahead2;

    lookahead2 =
        lookahead * lookahead;

    for (i = nearest_index;
         i < count;
         i++)
    {
        dist2 =
            DistanceSquared(
                pose->x,
                pose->y,
                path[i].x,
                path[i].y);

        if (dist2 >= lookahead2)
        {
            return i;
        }
    }

    return count - 1;
}

Đây là cách dễ hiểu nhất.

---

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

Giả sử:

P3 ───────── P4

Robot cần:

Ld = 100 mm

nhưng:

distance tới P3 = 80 mm
distance tới P4 = 150 mm

Ta chọn luôn P4.

Không chính xác lắm.

Tốt hơn là tìm một điểm nằm giữa P3 và P4.

---

14. Nội suy Look-Ahead

Ta muốn:

P3 ●────────────● P4
       ↑
     Target

Nếu khoảng cách cần đi thêm là:

100 - 80 = 20 mm

thì nội suy giữa P3 và P4.

---

15. Hàm nội suy

static PathPoint_t InterpolatePoint(
    PathPoint_t p1,
    PathPoint_t p2,
    float t)
{
    PathPoint_t result;

    result.x =
        p1.x +
        t * (p2.x - p1.x);

    result.y =
        p1.y +
        t * (p2.y - p1.y);

    return result;
}

t nằm trong:

0 → 1

---

16. Tính Look-Ahead chính xác hơn

Ta có thể làm:

static PathPoint_t FindLookAheadPoint(
    const PathPoint_t *path,
    uint16_t count,
    const Pose_t *pose,
    uint16_t nearest_index,
    float lookahead)
{
    uint16_t i;

    float d1;
    float d2;
    float ratio;

    PathPoint_t result;

    result = path[count - 1];

    for (i = nearest_index;
         i < count - 1;
         i++)
    {
        d1 =
            sqrtf(
                DistanceSquared(
                    pose->x,
                    pose->y,
                    path[i].x,
                    path[i].y));

        d2 =
            sqrtf(
                DistanceSquared(
                    pose->x,
                    pose->y,
                    path[i + 1].x,
                    path[i + 1].y));

        if (d1 <= lookahead &&
            d2 >= lookahead)
        {
            ratio =
                (lookahead - d1) /
                (d2 - d1);

            result =
                InterpolatePoint(
                    path[i],
                    path[i + 1],
                    ratio);

            return result;
        }
    }

    return result;
}

Đây là phiên bản dễ học.

---

17. Nhưng có một cải tiến

d1 và d2 là khoảng cách từ robot tới hai điểm.

Điều này chưa thực sự là khoảng cách dọc theo path.

Phiên bản chuyên nghiệp hơn sẽ tính:

arc length

tức là:

P0──P1──P2──P3──P4
    100  100  100

Tổng khoảng cách dọc đường:

P1 → P4 = 300 mm

---

18. Path Length

Ta có thể lưu:

typedef struct
{
    float x;
    float y;
    float length;

} PathPoint_t;

Trong đó:

length[i]

là khoảng cách tích lũy từ đầu path tới điểm i.

---

19. Tạo cumulative length

void Path_CalculateLengths(
    PathPoint_t *path,
    uint16_t count)
{
    uint16_t i;

    path[0].length = 0.0f;

    for (i = 1;
         i < count;
         i++)
    {
        float dx;
        float dy;

        dx =
            path[i].x -
            path[i - 1].x;

        dy =
            path[i].y -
            path[i - 1].y;

        path[i].length =
            path[i - 1].length +
            sqrtf(dx * dx + dy * dy);
    }
}

---

20. Tại sao cumulative length rất hữu ích?

Giả sử:

nearest path position = 750 mm

và:

Ld = 200 mm

ta chỉ cần tìm:

target position = 950 mm

trên path.

Thay vì tìm theo khoảng cách hình học một cách mơ hồ.

---

21. Đây là Path Tracker tốt hơn

Robot
  │
  ▼
Nearest path position
  │
  + Ld
  │
  ▼
Target path position
  │
  ▼
Look-ahead point

---

22. Tính góc α

Sau khi có:

target.x
target.y

ta tính:

dx =
    target.x -
    pose->x;

dy =
    target.y -
    pose->y;

Sau đó:

target_angle =
    atan2f(dy, dx);

và:

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

---

23. Normalize góc

Đây là hàm cực kỳ quan trọng.

Ví dụ:

θ = +179°
target = -179°

Sai số thực tế chỉ:

+2°

chứ không phải:

-358°

Code:

static float NormalizeAngleRad(float angle)
{
    while (angle > M_PI)
        angle -= 2.0f * M_PI;

    while (angle < -M_PI)
        angle += 2.0f * M_PI;

    return angle;
}

---

24. Pure Pursuit

Bây giờ:

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

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

Sau đó:

[
\omega=V\kappa
]

omega =
    speed *
    curvature;

---

25. Thêm giới hạn tốc độ quay

Robot không thể quay vô hạn.

if (omega > MAX_OMEGA)
    omega = MAX_OMEGA;

if (omega < -MAX_OMEGA)
    omega = -MAX_OMEGA;

---

26. Quan trọng hơn: giảm tốc khi cua gắt

Nếu:

|κ| lớn

robot đang cua gắt.

Không nên chạy:

V = MAX_SPEED

Ta có thể:

[
V =
\frac{V_{max}}
{1+K_\kappa|\kappa|}
]

Ví dụ:

speed =
    MAX_SPEED /
    (1.0f +
     CURVATURE_SPEED_GAIN *
     fabsf(curvature));

---

27. Tại sao phải giảm tốc khi cua?

Nếu:

đường thẳng

robot có thể:

500 mm/s

Nhưng nếu:

cua 90°

mà vẫn:

500 mm/s

thì:

Robot
  \
   \
    \
     →→→

rất dễ:

văng khỏi path

---

28. Tạo Speed Profile

Ta có:

Curvature
    ↓
Speed Limit
    ↓
V

Ví dụ:

float CalculatePathSpeed(
    float curvature)
{
    float speed;

    speed =
        MAX_SPEED /
        (1.0f +
         CURVATURE_GAIN *
         fabsf(curvature));

    if (speed < MIN_SPEED)
        speed = MIN_SPEED;

    return speed;
}

---

29. Đây là một cải tiến rất quan trọng

Pure Pursuit ban đầu:

V = constant

Bây giờ:

V = f(curvature)

Nghĩa là:

đường thẳng → nhanh
cua nhẹ     → vừa
cua gắt     → chậm

Robot sẽ chạy tự nhiên hơn rất nhiều.

---

30. PathTracker_Update()

Bây giờ chúng ta ghép tất cả lại.

void PathTracker_Update(
    const PathPoint_t *path,
    uint16_t count,
    const Pose_t *pose,
    uint16_t *nearest_index,
    PathTrackerOutput_t *out)
{
    PathPoint_t target;

    float dx;
    float dy;

    float target_angle;

    uint16_t nearest;
    uint16_t target_index;

    nearest =
        FindNearestPoint(
            path,
            count,
            pose,
            *nearest_index);

    *nearest_index =
        nearest;

    out->nearest_index =
        nearest;

    out->lookahead =
        CalculateLookAhead(
            MAX_SPEED,
            MIN_LOOKAHEAD,
            LOOKAHEAD_GAIN);

    target_index =
        FindLookAheadPoint(
            path,
            count,
            pose,
            nearest,
            out->lookahead);

    out->target_index =
        target_index;

    target =
        path[target_index];

    dx =
        target.x -
        pose->x;

    dy =
        target.y -
        pose->y;

    target_angle =
        atan2f(dy, dx);

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

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

    out->speed =
        CalculatePathSpeed(
            out->curvature);

    out->angular_speed =
        out->speed *
        out->curvature;

    out->finished = 0;
}

Đây là khung controller cốt lõi.

---

31. Nhưng có một lỗi trong phiên bản trên

Ta đang dùng:

MAX_SPEED

để tính Look-Ahead trước khi biết tốc độ thực tế.

Tốt hơn là:

Pose
 ↓
Nearest
 ↓
Curvature sơ bộ
 ↓
Speed
 ↓
Look-Ahead
 ↓
Pure Pursuit

hoặc đơn giản hơn:

LookAhead
=
Lmin + Kv × current_speed

Trong hệ thống thực tế, ta sẽ dùng tốc độ hiện tại hoặc tốc độ mong muốn của chu kỳ trước.

---

32. Cấu hình

Trong PathTracker.h:

#define MIN_LOOKAHEAD          80.0f
#define MAX_LOOKAHEAD         300.0f

#define LOOKAHEAD_GAIN          0.30f

#define MAX_SPEED             300.0f
#define MIN_SPEED              60.0f

#define MAX_OMEGA               2.0f

#define CURVATURE_GAIN          5.0f

Các giá trị này chỉ là giá trị khởi đầu để thử nghiệm, không phải thông số tối ưu cho mọi robot.

---

33. Nên dùng đơn vị gì?

Tôi khuyên bạn dùng:

distance → mm
speed    → mm/s
omega    → rad/s
theta    → rad

Ví dụ:

X = 500 mm
Y = 300 mm
V = 200 mm/s
θ = 1.57 rad
ω = 0.5 rad/s

Không nên trộn:

mm
cm
m

trong cùng một controller.

---

34. Từ V,ω sang 2 bánh

Bài 54 đã học:

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

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

Ví dụ:

W = 150 mm
V = 200 mm/s
ω = 0.5 rad/s

thì:

[
V_L=200-\frac{0.5\times150}{2}
]

[
V_L=162.5
]

và:

[
V_R=237.5
]

Robot quay sang trái/phải tùy convention dấu của bạn.

---

35. Code

void Robot_SetVelocity(
    float V,
    float omega)
{
    float VL;
    float VR;

    VL =
        V -
        omega * WHEEL_BASE / 2.0f;

    VR =
        V +
        omega * WHEEL_BASE / 2.0f;

    Motor_SetSpeedLeft(VL);
    Motor_SetSpeedRight(VR);
}

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

Path Tracker

và:

Motor PID

---

36. Hoàn chỉnh toàn bộ vòng điều khiển

Ví dụ chạy mỗi:

20 ms

void Robot_Control_20ms(void)
{
    Pose_t pose;

    PathTrackerOutput_t output;

    pose.x =
        odometry.x;

    pose.y =
        odometry.y;

    pose.theta =
        odometry.theta;

    PathTracker_Update(
        path,
        PATH_COUNT,
        &pose,
        &nearest_index,
        &output);

    Robot_SetVelocity(
        output.speed,
        output.angular_speed);
}

Sau đó:

Motor PID

sẽ điều khiển tốc độ từng bánh.

---

37. Một vòng lặp thực tế

20 ms
 │
 ├── Read Encoder
 │
 ├── Calculate Wheel Speed
 │
 ├── Speed PID
 │
 ├── Odometry
 │       │
 │       └── X,Y,θ
 │
 └── Path Tracker
         │
         ├── nearest point
         ├── look-ahead
         ├── α
         ├── curvature
         ├── speed
         └── V,ω
                 │
                 ▼
             VL,VR

---

38. Khi nào path kết thúc?

Ta cần điều kiện:

Robot gần điểm cuối

Ví dụ:

[
d < D_{goal}
]

Code:

float dx =
    path[count - 1].x -
    pose->x;

float dy =
    path[count - 1].y -
    pose->y;

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

if (distance < GOAL_TOLERANCE)
{
    output->finished = 1;
}

Ví dụ:

#define GOAL_TOLERANCE 30.0f

---

39. Khi finished = 1

Không nên tiếp tục:

Robot_SetVelocity(
    speed,
    omega);

mà:

Robot_SetVelocity(
    0,
    0);

hoặc chuyển sang trạng thái khác.

---

40. State Machine

Đây là lúc chúng ta kết hợp kiến thức State Machine trước đây.

typedef enum
{
    ROBOT_IDLE,
    ROBOT_TRACKING,
    ROBOT_FINISHED,
    ROBOT_ERROR

} RobotState_t;

Luồng:

IDLE
 ↓ START
TRACKING
 ↓ goal reached
FINISHED

Nếu lỗi:

TRACKING
   ↓
 ERROR

---

41. Path có thể là gì?

Không nhất thiết phải nhập thủ công.

Có thể tạo:

Đường thẳng

P0 → P1

Đường tròn

Circle

Hình số 8

∞

Hình vuông

┌───────┐
│       │
│       │
└───────┘

Chữ S

╭────
     ╲
      ╲
       ────╮

Waypoint từ PC

PC
 ↓ UART
STM32
 ↓
Path[]

Đây sẽ là hướng rất thú vị cho project robot của bạn.

---

42. Ví dụ Path hình số 8

Ta có thể tạo bằng phương trình:

[
x=A\sin(t)
]

[
y=B\sin(t)\cos(t)
]

Sau đó lấy nhiều điểm:

for (i = 0; i < N; i++)
{
    float t =
        2.0f * M_PI * i / N;

    path[i].x =
        A * sinf(t);

    path[i].y =
        B * sinf(t) * cosf(t);
}

Robot sẽ có một quỹ đạo dạng:

      ╭──╮
     ╱    ╲
     ╲    ╱
      ╲──╱
      ╱──╲
     ╱    ╲
     ╲    ╱
      ╰──╯

Đây là bài test rất tốt cho Path Tracker.

---

43. Tại sao hình số 8 khó?

Vì tại vùng giao nhau:

       ╲
        ╲
─────────X────────
        ╱
       ╱

có thể có nhiều điểm path gần robot.

Nếu thuật toán chỉ tìm:

nearest point toàn bộ path

nó có thể nhảy từ:

P100

ngược về:

P20

Đây là lý do chúng ta phải duy trì:

nearest_index

và chỉ cho phép index tiến về phía trước.

---

44. Đây là một nguyên tắc cực kỳ quan trọng

nearest_index_new
    >=
nearest_index_old

trong path không quay lại.

Ví dụ:

if (nearest < *nearest_index)
{
    nearest =
        *nearest_index;
}

Nhờ vậy robot không bị “nhảy ngược” trên quỹ đạo.

---

45. Phiên bản tìm nearest tốt hơn

static uint16_t FindNearestPointForward(
    const PathPoint_t *path,
    uint16_t count,
    const Pose_t *pose,
    uint16_t current_index)
{
    uint16_t i;
    uint16_t best;

    float best_dist;
    float dist;

    best = current_index;

    best_dist =
        DistanceSquared(
            pose->x,
            pose->y,
            path[current_index].x,
            path[current_index].y);

    for (i = current_index + 1;
         i < count;
         i++)
    {
        dist =
            DistanceSquared(
                pose->x,
                pose->y,
                path[i].x,
                path[i].y);

        if (dist < best_dist)
        {
            best_dist = dist;
            best = i;
        }
        else
        {
            /*
             * Có thể break nếu path
             * được lấy mẫu đủ đều.
             */
        }
    }

    return best;
}

---

46. Nhưng cần cẩn thận

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

distance tới điểm kế tiếp > distance hiện tại

thì có thể break.

Với đường cong phức tạp:

────╮
    ╰────╮
         ╰────

có thể còn điểm phía trước gần robot hơn.

Vì vậy trong phiên bản đầu tiên:

Không tối ưu quá sớm.

Hãy làm cho thuật toán đúng trước.

---

47. Debug cực kỳ quan trọng

Bạn nên gửi qua UART:

X=
Y=
THETA=
NEAREST=
TARGET=
LD=
ALPHA=
KAPPA=
V=
OMEGA=
VL=
VR=

Ví dụ:

X=152.3
Y=47.8
THETA=0.31
NEAREST=12
TARGET=16
LD=126.4
ALPHA=-0.18
KAPPA=-0.0028
V=295.9
OMEGA=-0.83
VL=358.1
VR=233.7

Nhìn những số này bạn có thể biết robot sai ở tầng nào.

---

48. Nếu X,Y sai

Kiểm tra:

Encoder
Odometry

---

49. Nếu X,Y đúng nhưng robot đi sai hướng

Kiểm tra:

θ

---

50. Nếu θ đúng nhưng robot không bám path

Kiểm tra:

nearest_index
target_index
lookahead
alpha

---

51. Nếu V,ω đúng nhưng motor chạy sai

Kiểm tra:

Inverse Kinematics

và:

Motor PID

---

52. Đây là tư duy debug rất quan trọng

Đừng nhìn robot rồi đoán:

“Chắc PID sai.”

Hãy kiểm tra từng tầng:

Encoder
  ↓
Wheel speed
  ↓
PID
  ↓
Odometry
  ↓
X,Y,θ
  ↓
Path Tracker
  ↓
V,ω
  ↓
VL,VR
  ↓
Motor

---

53. Bài thực hành chính

Tôi khuyên bạn chưa cần làm ngay đường cong phức tạp.

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

Test 1

Path:
(0,0) → (1000,0)

Robot bắt đầu:

(0,100,0)

---

Test 2

Path:
(0,0) → (1000,1000)

---

Test 3

Circle:

C=(500,500)
R=300

---

Test 4

8 điểm:

{
    {0,0},
    {100,20},
    {200,60},
    {300,130},
    {400,220},
    {500,350},
    {600,500}
}

---

Test 5

Hình số 8.

Đây mới là bài test Path Tracker thực sự.

---

54. Một điểm rất quan trọng về STM32F103C8T6

Bạn đang học STM32F103C8T6, nên đừng cố nhồi ngay:

Path 5000 điểm

vào RAM.

Nếu mỗi point có:

float x;
float y;
float length;

thì:

12 byte / point

1000 điểm:

≈ 12 KB

Trong khi SRAM của STM32F103C8T6 loại phổ biến chỉ khoảng:

20 KB

và firmware còn cần RAM cho:

PID
Encoder
Stack
UART
variables
buffers

Do đó:

100–300 waypoint là mức hợp lý để bắt đầu.

---

55. Một giải pháp tốt hơn

Không nhất thiết phải lưu toàn bộ path trong RAM.

Có thể:

FLASH
 ↓
Path[]
 ↓
RAM chỉ giữ một cửa sổ

hoặc:

PC
 ↓ UART
STM32
 ↓
stream waypoint

Đây là hướng chúng ta có thể học sau.

---

56. Bài 56 đã đưa robot đến đâu?

Từ:

PWM

đến:

Encoder

đến:

PID

đến:

Odometry

đến:

V,ω

đến:

Pure Pursuit

bây giờ robot có thể hiểu:

“Tôi đang ở đâu và tôi phải cong theo hướng nào để bám quỹ đạo.”

Đây là một bước rất lớn.

---

57. Công thức quan trọng nhất của Bài 56

Look-ahead động

[
\boxed{
L_d=L_{min}+K_v|V|
}
]

Góc

[
\boxed{
\alpha=
normalize(
atan2(y_t-y,x_t-x)-\theta)
}
]

Curvature

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

Angular velocity

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

Inverse kinematics

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

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

---

58. Toàn bộ hệ thống bây giờ

             PATH
               │
               ▼
       Find Nearest Point
               │
               ▼
        Look-Ahead Point
               │
               ▼
               α
               │
               ▼
        Pure Pursuit
               │
               ▼
               κ
               │
               ▼
          Speed Profile
               │
          ┌────┴────┐
          ↓         ↓
          V         ω
          └────┬────┘
               ↓
      Inverse Kinematics
          ↙         ↘
        VL           VR
         ↓            ↓
      PID Left     PID Right
         ↓            ↓
      Motor L       Motor R
         ↓            ↓
      Encoder L    Encoder R
          ↘          ↙
            ODOMETRY
                ↓
             X,Y,θ
                │
                └───────────────┐
                                │
                                ▼
                           Path Tracker

Bài tiếp theo rất đáng học

BÀI 57 – TRAJECTORY GENERATION: TỰ SINH QUỸ ĐẠO CHO ROBOT 2 BÁNH

Ở Bài 57, chúng ta sẽ đi thêm một tầng: thay vì phải tự nhập hàng trăm điểm P0, P1, P2..., STM32 sẽ tự sinh quỹ đạo:

Đường thẳng
     ↓
Arc
     ↓
Circle
     ↓
Bezier
     ↓
S-curve
     ↓
Trajectory X(t), Y(t), θ(t)

và quan trọng hơn, chúng ta sẽ bắt đầu đưa thời gian t vào quỹ đạo, tức là chuyển từ Path Tracking sang Trajectory Tracking thực sự.