BÀI 57 – TRAJECTORY GENERATION

TỰ SINH QUỸ ĐẠO CHO ROBOT 2 BÁNH BẰNG STM32F103

Ở Bài 56, robot nhận một Path[] gồm nhiều waypoint rồi dùng Pure Pursuit để bám đường.

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

Nếu muốn robot đi từ A → B thật mượt, chúng ta có thực sự muốn tự tạo 100, 200 hay 500 waypoint không?

Không.

Ở Bài 57, chúng ta học cách để STM32 tự sinh quỹ đạo theo công thức toán học.

---

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

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

Path

Path chỉ nói:

Robot phải đi qua đâu?

Ví dụ:

A ●──────────────● B

Ta có:

X(t)
Y(t)

nhưng chưa quan tâm robot đi nhanh hay chậm.

---

Trajectory

Trajectory nói:

Robot phải đi qua đâu và vào thời điểm nào?

Ví dụ:

t = 0s       → A
t = 1s       → P1
t = 2s       → P2
t = 3s       → P3
t = 4s       → B

Do đó trajectory có:

[
x(t),y(t),\theta(t)
]

và thậm chí:

[
v(t),\omega(t)
]

---

2. Hãy hình dung như thế này

Path:

A ●────────────────────● B

Robot có thể đi:

10 giây
20 giây
100 giây

miễn là đi đúng đường.

Trajectory:

A ●──●──●──●──●──●──● B
   1s 2s 3s 4s 5s 6s

Bây giờ ta quy định luôn thời gian.

---

3. Tại sao Trajectory Generation quan trọng?

Trong robot thực tế, ta thường muốn:

không giật
không tăng tốc đột ngột
không giảm tốc đột ngột
không đổi hướng quá nhanh

Ví dụ không tốt:

0 mm/s
   │
   │
   └────────────── 500 mm/s

Robot sẽ:

GIẬT

Tốt hơn:

0
   ╱
  ╱
 ╱
╱────────────

Tức là:

[
V(t)
]

thay đổi từ từ.

---

4. Ba tầng chúng ta cần phân biệt

Từ Bài 53–57, hệ thống robot đã tiến hóa:

Odometry
   ↓
Path
   ↓
Path Tracking
   ↓
Trajectory Generation
   ↓
Trajectory Tracking

Cụ thể:

Odometry

Encoder
 ↓
X,Y,θ

Path Tracking

X,Y,θ
 ↓
bám đường

Trajectory Generation

A → B
 ↓
tính quỹ đạo

Trajectory Tracking

X,Y,θ thực tế
       ↓
X,Y,θ mong muốn
       ↓
Controller

---

5. Quỹ đạo đơn giản nhất: đường thẳng

Giả sử robot đi từ:

A = (0,0)

đến:

B = (1000,500)

Ta có:

[
\Delta x=1000
]

[
\Delta y=500
]

Độ dài:

[
L=\sqrt{1000^2+500^2}
]

---

6. Góc của đường thẳng

[
\theta_d=
atan2(\Delta y,\Delta x)
]

Trong ví dụ:

[
\theta_d=atan2(500,1000)
]

≈ 26.565°.

---

7. Nội suy tuyến tính

Đặt:

[
s\in[0,1]
]

thì:

[
x(s)=x_0+s(x_f-x_0)
]

[
y(s)=y_0+s(y_f-y_0)
]

Ví dụ:

s=0
→ A

s=0.5
→ giữa

s=1
→ B

---

8. Code

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

} TrajectoryPoint_t;

Hàm:

TrajectoryPoint_t Trajectory_Line(
    float x0,
    float y0,
    float xf,
    float yf,
    float s)
{
    TrajectoryPoint_t p;

    p.x =
        x0 +
        s * (xf - x0);

    p.y =
        y0 +
        s * (yf - y0);

    p.theta =
        atan2f(
            yf - y0,
            xf - x0);

    return p;
}

---

9. Nhưng s chưa phải thời gian

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

Nếu:

s = 0.5

chúng ta chỉ biết robot ở giữa quãng đường.

Chưa biết:

t = ?

Do đó ta cần thêm time parameterization.

---

10. Cách đơn giản nhất

Cho tổng thời gian:

T = 5.0f;

thì:

[
s=\frac{t}{T}
]

với:

0 ≤ t ≤ T

Code:

float s;

s = t / T;

if (s > 1.0f)
    s = 1.0f;

---

11. Trajectory lúc này

Ta có:

[
x(t)=x_0+\frac{t}{T}(x_f-x_0)
]

[
y(t)=y_0+\frac{t}{T}(y_f-y_0)
]

Robot sẽ chạy đều.

Nhưng vẫn có vấn đề:

Robot bắt đầu ngay lập tức ở tốc độ cao.

---

12. Trapezoidal Velocity Profile

Đây là một trong những profile quan trọng nhất trong điều khiển chuyển động.

Thay vì:

V
│──────────────
│
└────────────── t

ta tạo:

V
│       ┌──────────┐
│      /            \
│     /              \
│____/                \____
└────────────────────────── t
     accel    cruise decel

Ba giai đoạn:

1. ACCEL
2. CRUISE
3. DECEL

---

13. Ví dụ

Robot cần đi:

L = 1000 mm

với:

Vmax = 300 mm/s
Amax = 100 mm/s²

Thời gian tăng tốc:

[
t_a=\frac{V_{max}}{A_{max}}
]

[
t_a=3s
]

Quãng đường tăng tốc:

[
d_a=\frac12 A t_a^2
]

[
d_a=450mm
]

Hai bên tăng tốc + giảm tốc:

900 mm

vẫn còn:

100 mm

để chạy cruise.

---

14. Nếu quãng đường quá ngắn?

Ví dụ:

L = 500 mm

nhưng:

d_accel + d_decel > L

thì robot không thể đạt Vmax.

Khi đó profile trở thành:

      /\
     /  \
    /    \
___/      \___

Đây gọi là:

Triangular Velocity Profile

---

15. So sánh

Trapezoidal

      ┌───────┐
     /         \
____/           \____

Triangular

       /\
      /  \
_____/    \_____

Đây là nền tảng của rất nhiều hệ thống motion control.

---

16. Tạo cấu trúc Motion Profile

typedef struct
{
    float distance;

    float vmax;

    float accel;

    float decel;

    float t_accel;

    float t_cruise;

    float t_decel;

    float total_time;

    float v_peak;

} MotionProfile_t;

---

17. Khởi tạo profile

void MotionProfile_Init(
    MotionProfile_t *p,
    float distance,
    float vmax,
    float accel,
    float decel)
{
    float t1;
    float d1;

    float t3;
    float d3;

    p->distance = distance;
    p->vmax = vmax;
    p->accel = accel;
    p->decel = decel;

    t1 =
        vmax / accel;

    d1 =
        0.5f *
        accel *
        t1 *
        t1;

    t3 =
        vmax / decel;

    d3 =
        0.5f *
        decel *
        t3 *
        t3;

    if ((d1 + d3) <= distance)
    {
        /*
         * Trapezoidal
         */

        p->v_peak = vmax;

        p->t_accel = t1;
        p->t_decel = t3;

        p->t_cruise =
            (distance - d1 - d3)
            / vmax;
    }
    else
    {
        /*
         * Triangular
         */

        p->v_peak =
            sqrtf(
                2.0f *
                distance *
                accel *
                decel /
                (accel + decel));

        p->t_accel =
            p->v_peak / accel;

        p->t_decel =
            p->v_peak / decel;

        p->t_cruise = 0.0f;
    }

    p->total_time =
        p->t_accel +
        p->t_cruise +
        p->t_decel;
}

---

18. Hàm lấy vận tốc tại thời điểm t

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

float MotionProfile_GetVelocity(
    MotionProfile_t *p,
    float t)
{
    float t2;

    if (t <= 0.0f)
        return 0.0f;

    /*
     * ACCEL
     */

    if (t < p->t_accel)
    {
        return
            p->accel * t;
    }

    /*
     * CRUISE
     */

    t2 =
        t -
        p->t_accel;

    if (t2 < p->t_cruise)
    {
        return p->v_peak;
    }

    /*
     * DECEL
     */

    t2 =
        t -
        p->t_accel -
        p->t_cruise;

    if (t2 < p->t_decel)
    {
        return
            p->v_peak -
            p->decel * t2;
    }

    return 0.0f;
}

---

19. Lấy quãng đường đã đi

Ta cần:

[
s(t)
]

ACCEL

[
s=\frac12at^2
]

CRUISE

[
s=d_a+V_{max}t_c
]

DECEL

[
s=d_a+d_c+
V_{peak}t_d-\frac12at_d^2
]

---

20. Code

float MotionProfile_GetPosition(
    MotionProfile_t *p,
    float t)
{
    float d_accel;
    float t2;
    float d_cruise;

    d_accel =
        0.5f *
        p->accel *
        p->t_accel *
        p->t_accel;

    if (t <= 0.0f)
        return 0.0f;

    /*
     * ACCEL
     */

    if (t < p->t_accel)
    {
        return
            0.5f *
            p->accel *
            t *
            t;
    }

    /*
     * CRUISE
     */

    t2 =
        t -
        p->t_accel;

    d_cruise =
        p->v_peak *
        p->t_cruise;

    if (t2 < p->t_cruise)
    {
        return
            d_accel +
            p->v_peak *
            t2;
    }

    /*
     * DECEL
     */

    t2 =
        t -
        p->t_accel -
        p->t_cruise;

    return
        d_accel +
        d_cruise +
        p->v_peak *
        t2 -
        0.5f *
        p->decel *
        t2 *
        t2;
}

---

21. Bây giờ chúng ta có gì?

Chỉ cần:

t

ta lấy được:

s(t)
V(t)

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

---

22. Kết hợp với đường thẳng

Ta có:

A(x0,y0)
       \
        \
         \
          B(xf,yf)

Tính:

dx = xf - x0;
dy = yf - y0;

và:

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

Sau đó:

s = MotionProfile_GetPosition(&profile, t);

---

23. Chuẩn hóa s

[
u=\frac{s}{L}
]

float u;

u = s / L;

if (u > 1.0f)
    u = 1.0f;

Sau đó:

x =
    x0 +
    u * dx;

y =
    y0 +
    u * dy;

---

24. Hàm hoàn chỉnh

TrajectoryPoint_t Trajectory_Line_Profile(
    float x0,
    float y0,
    float xf,
    float yf,
    MotionProfile_t *profile,
    float t)
{
    TrajectoryPoint_t p;

    float dx;
    float dy;

    float length;
    float s;
    float u;

    dx = xf - x0;
    dy = yf - y0;

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

    s =
        MotionProfile_GetPosition(
            profile,
            t);

    u =
        s / length;

    if (u > 1.0f)
        u = 1.0f;

    p.x =
        x0 +
        u * dx;

    p.y =
        y0 +
        u * dy;

    p.theta =
        atan2f(dy, dx);

    return p;
}

---

25. Đây chính là Trajectory Generation

Ví dụ:

t=0
x=0
y=0
V=0

t=1
x=50
y=25
V=100

t=2
x=200
y=100
V=200

t=3
x=400
y=200
V=300

t=4
x=650
y=325
V=250

t=5
x=900
y=450
V=100

t=6
x=1000
y=500
V=0

Robot không còn:

START → FULL SPEED

mà:

0
 ↓
tăng tốc
 ↓
Vmax
 ↓
giảm tốc
 ↓
0

---

26. Nhưng robot 2 bánh cần thêm ω

Đường thẳng:

[
\omega=0
]

nên:

[
V_L=V_R=V
]

Rất đơn giản.

Nhưng nếu là đường cong:

      ______
    /
   /
  /
 /

thì:

ω ≠ 0

---

27. Quỹ đạo đường tròn

Đây là quỹ đạo cực kỳ quan trọng.

Cho:

Center = (cx, cy)
Radius = R

Ta có:

[
x(t)=c_x+R\cos\phi
]

[
y(t)=c_y+R\sin\phi
]

---

28. Nếu robot chạy với vận tốc V

Ta có:

[
\omega=\frac{V}{R}
]

và:

[
\phi(t)=\phi_0+\omega t
]

Do đó:

[
x(t)=c_x+R\cos(\phi_0+\omega t)
]

[
y(t)=c_y+R\sin(\phi_0+\omega t)
]

---

29. Góc robot

Nếu robot chạy theo chiều ngược kim đồng hồ:

[
\theta(t)=\phi(t)+\frac{\pi}{2}
]

Nếu chạy theo chiều kim đồng hồ:

[
\theta(t)=\phi(t)-\frac{\pi}{2}
]

---

30. Code Circle

TrajectoryPoint_t Trajectory_Circle(
    float cx,
    float cy,
    float radius,
    float phi0,
    float omega,
    float t)
{
    TrajectoryPoint_t p;

    float phi;

    phi =
        phi0 +
        omega * t;

    p.x =
        cx +
        radius * cosf(phi);

    p.y =
        cy +
        radius * sinf(phi);

    if (omega >= 0.0f)
    {
        p.theta =
            phi +
            M_PI / 2.0f;
    }
    else
    {
        p.theta =
            phi -
            M_PI / 2.0f;
    }

    return p;
}

---

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

Trong Bài 56:

Path Tracker

robot có thể chạy:

circle

bằng Pure Pursuit.

Nhưng trong Bài 57:

Trajectory Generator

chúng ta biết trước:

x(t)
y(t)
θ(t)
V(t)
ω(t)

Do đó controller có nhiều thông tin hơn.

---

32. So sánh

Pure Pursuit

Robot
 ↓
nhìn phía trước
 ↓
tính curvature
 ↓
V,ω

Trajectory Tracking

Time t
 ↓
Trajectory Generator
 ↓
x_d,y_d,θ_d
V_d,ω_d
 ↓
Controller
 ↓
V,ω

Đây là bước chuyển từ:

“Bám đường”

sang:

“Bám chuyển động mong muốn theo thời gian.”

---

33. Controller của Trajectory Tracking

Ta có sai số:

[
e_x=x_d-x
]

[
e_y=y_d-y
]

[
e_\theta=\theta_d-\theta
]

Nhưng không nên điều khiển trực tiếp ex, ey trong hệ tọa độ toàn cục.

Ta chuyển sang hệ tọa độ robot.

---

34. Sai số trong hệ robot

        y_robot
           ↑
           |
           |
           ●────→ x_robot
         robot

Ta tính:

[
e_x^r

\cos\theta e_x+
\sin\theta e_y
]

[
e_y^r

-\sin\theta e_x+
\cos\theta e_y
]

---

35. Sai số góc

[
e_\theta=
normalize(\theta_d-\theta)
]

Ví dụ:

θd = 179°
θ  = -179°

phải ra:

eθ = 2°

chứ không phải:

358°

---

36. Controller đơn giản

Một controller cơ bản:

[
V=
V_d+K_xe_x
]

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

Đây là một controller rất thú vị.

---

37. Code

void Trajectory_Controller(
    Pose_t *actual,
    TrajectoryPoint_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);

    *V =
        Vd +
        KX * ex;

    *omega =
        omega_d +
        KY * ey +
        KTHETA * etheta;
}

---

38. Nhưng không được quên giới hạn

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

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

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

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

---

39. Sau đó lại quay về Inverse Kinematics

V
ω
 ↓
VL = V - ωW/2
VR = V + ωW/2

rồi:

VL → PID motor trái
VR → PID motor phải

---

40. Toàn bộ hệ thống Bài 57

              TIME t
                 │
                 ▼
      ┌────────────────────┐
      │ Trajectory         │
      │ Generator          │
      └─────────┬──────────┘
                │
       ┌────────┼────────┐
       ↓        ↓        ↓
      Xd       Yd       θd
       │        │        │
       └────────┼────────┘
                ↓
             Vd,ωd
                │
                ▼
       Trajectory Controller
                ▲
                │
             X,Y,θ
                │
             Odometry
                ▲
                │
             Encoders
                │
                ▼
       ┌─────────────────┐
       │ Inverse         │
       │ Kinematics      │
       └──────┬────┬─────┘
              ↓    ↓
             VL    VR
              ↓    ↓
             PID  PID
              ↓    ↓
           Motor  Motor

---

41. Một ví dụ hoàn chỉnh

Robot bắt đầu:

X = 0
Y = 0
θ = 0

Mục tiêu:

X = 1000 mm
Y = 500 mm

Tạo profile:

MotionProfile_t profile;

MotionProfile_Init(
    &profile,
    1118.0f,
    300.0f,
    100.0f,
    100.0f);

Vì:

[
L=
\sqrt{1000^2+500^2}
\approx1118mm
]

---

42. Trong Timer 1 ms

Bài 36 của chúng ta đã tạo:

millis()

Vậy ta có thể:

float t;

t =
    (millis() -
     trajectory_start_time)
    / 1000.0f;

Sau đó:

desired =
    Trajectory_Line_Profile(
        0,
        0,
        1000,
        500,
        &profile,
        t);

---

43. Đây là sự kết nối rất đẹp với các bài trước

Bạn đã học:

Bài 36

Timer
 ↓
millis 1 ms

Bài 45

Encoder

Bài 46

Motor speed PID

Bài 48

Position control

Bài 53

Odometry

Bài 54

Inverse Kinematics

Bài 55

Trajectory Tracking

Bài 56

Pure Pursuit

Bài 57

Trajectory Generation

Tất cả đang bắt đầu ghép thành một hệ thống robot hoàn chỉnh.

---

44. Một lưu ý rất quan trọng với STM32F103

STM32F103C8T6 không có FPU phần cứng.

Các hàm:

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

đều sử dụng tính toán floating-point bằng phần mềm.

Trong bài học chúng ta vẫn dùng float vì dễ hiểu và dễ kiểm tra.

Khi robot chạy thực tế với vòng điều khiển nhanh, chúng ta có thể tối ưu sau bằng:

lookup table
CORDIC
fixed-point
giảm số lần tính sin/cos

Đừng tối ưu quá sớm.

---

45. Bài thực hành số 1

Hãy tạo robot đi:

(0,0)
   ↓
(1000,0)

với:

Vmax = 300 mm/s
A = 100 mm/s²
D = 100 mm/s²

Quan sát UART:

TIME
Xd
Yd
Vd
θd

Bạn sẽ thấy:

TIME    Xd       Yd       Vd
0.0     0        0        0
0.5     ...      0        50
1.0     ...      0        100
2.0     ...      0        200
3.0     ...      0        300
...

---

46. Bài thực hành số 2 – Circle

Cho:

R = 300 mm
V = 150 mm/s

thì:

[
\omega=\frac{150}{300}
]

[
\omega=0.5rad/s
]

Chu kỳ:

[
T=\frac{2\pi}{0.5}
]

≈ 12.57 s.

Robot sẽ hoàn thành một vòng trong khoảng:

12.57 s

---

47. Bài thực hành số 3 – kiểm tra Tracking Error

UART:

Xd
Yd
θd

X
Y
θ

ex
ey
etheta

Ví dụ:

Xd=500
Yd=200
θd=0.8

X=480
Y=190
θ=0.75

ex=20
ey=10
etheta=0.05

Đây mới là cách debug robot chuyên nghiệp.

---

48. Điều quan trọng nhất của Bài 57

Bạn cần nhớ:

PATH

là:

Robot đi đâu?

Còn:

TRAJECTORY

là:

Robot đi đâu + đi lúc nào + với vận tốc nào?

Và:

TRAJECTORY TRACKING

là:

Robot thực tế phải bám theo trạng thái mong muốn theo thời gian.

---

49. Sơ đồ tư duy cuối bài

                 ROBOT 2 BÁNH
                       │
        ┌──────────────┴──────────────┐
        │                             │
     PATH                         TRAJECTORY
        │                             │
   x(s), y(s)                  x(t),y(t),θ(t)
        │                             │
 Pure Pursuit                 V(t),ω(t)
        │                             │
        └──────────────┬──────────────┘
                       │
                  CONTROLLER
                       │
                     V,ω
                       │
              INVERSE KINEMATICS
                 ↙           ↘
               VL             VR
                ↓              ↓
             PID L          PID R
                ↓              ↓
             MOTOR L         MOTOR R
                 ↘           ↙
                   ENCODER
                      │
                   ODOMETRY
                      │
                    X,Y,θ
                      │
                      └──── feedback

Bài 58 – bước tiếp theo

Sau Bài 57, bài rất hợp lý tiếp theo là:

BÀI 58 – TRAJECTORY TRACKING NÂNG CAO: CONTROLLER X,Y,θ + FEEDFORWARD + FEEDBACK

Ở đó chúng ta sẽ không chỉ dùng:

[
V=V_d+K_xe_x
]

mà tách thành:

[
\boxed{
u=u_{feedforward}+u_{feedback}
}
]

tức là robot biết trước mình cần chạy thế nào từ trajectory, đồng thời dùng encoder + odometry để sửa sai số thực tế. Đây là bước rất quan trọng để tiến tới các controller robot thực tế như kinematic control, dynamic control và sau này là MPC.