MODULE 2 — Motion Planning (Lecture 6–10)

📚 Học sâu: Lectures/ Lec06–10. File này = ôn nhanh.

C-space → BFS → A*(cost/gió) → RRT → min-snap → MPC re-plan "vẽ được" ─────────────────────────────► "bay được"

Mục tiêu module.
Robot đã ổn định (Module 1). Module này trả lời: đi đường nào từ A đến B mà không đụng; nếu được thì đường tốt nhất; và đường đó có bay được dưới ràng buộc động lực học hay không.

Luồng module:

Lec 6   Lưới / đồ thị: BFS, DFS (+ ý tưởng C-space)
Lec 7   Đường tối ưu rời rạc: Dijkstra, A*
Lec 8   Không gian liên tục: RRT (và họ hàng)
Lec 9   Differential flatness + min-snap (đặc thù quadrotor)
Lec 10  Kinodynamic planning & MPC

Lecture 6 — Discrete Planning: BFS và DFS

0. Mục tiêu học

Giải thích C-space; chạy được BFS; hiểu vì sao lưới hóa thất bại ở chiều cao (curse of dimensionality).

1. Câu hỏi trung tâm

Nếu thế giới rời rạc hóa được thành đồ thị (ô lưới, waypoint), tìm đường đi
từ start đến goal thế nào cho có hệ thống?

2. Trực giác

Configuration space (C-space) — ý tưởng nền của cả module: thay vì nghĩ
“robot hình dạng phức tạp di chuyển giữa vật cản”, ta nghĩ “một điểm di
chuyển trong không gian cấu hình”, nơi vật cản đã được “phồng” lên đúng bằng
kích thước robot. Drone bán kính \(r\) tránh cột bán kính \(R\) ⇔ điểm tránh
đĩa bán kính \(R + r\). Từ đây mọi thuật toán chỉ làm việc với điểm.

BFS = loang như vết dầu: từ start lan đều từng lớp, lớp nào chạm goal thì
xong. DFS = chuột chui hang: lao sâu một hướng đến cùng, cụt thì lùi.

3. Cốt lõi kỹ thuật

Đồ thị \(G = (V, E)\): đỉnh = cấu hình rời rạc, cạnh = bước đi hợp lệ (không đụng).
BFS dùng queue (FIFO): duyệt theo lớp → tìm đường ít cạnh nhất (tối ưu
khi mọi cạnh giá bằng nhau). Độ phức tạp \(O(|V| + |E|)\).
DFS dùng stack (LIFO): tốn ít bộ nhớ hơn theo chiều sâu, không đảm bảo
đường ngắn. Trong planning gần như chỉ dùng để kiểm tra liên thông.
Cả hai cần tập visited — quên nó là lặp vô hạn.
Lời nguyền chiều (curse of dimensionality): lưới \(k\) ô/chiều, \(n\) chiều
→ \(k^n\) đỉnh. Quadrotor 12D với 100 ô/chiều là \(10^{24}\) — bất khả thi.
Đây là lý do tồn tại của Lec 8 (sampling).

4. ASCII — BFS loang trên lưới (S=start, G=goal, #=vật cản)

  bước 0          bước 3              hoàn tất (số = thứ tự lớp)
  S . . . #       S 1 2 3 #           S 1 2 3 #
  . . . . .       1 2 3 . .           1 2 3 4 5
  . # # . .  →    2 # # . .     →     2 # # 5 6
  . . . . .       3 . . . .           3 4 5 6 7
  . . . G .       . . . G .           4 5 6 7 G ← đường: lần ngược 8→0

5. Demo — BFS ~25 dòng

from collections import deque

grid = ["S...#",
        ".....",
        ".##..",
        ".....",
        "...G."]
R, C = len(grid), len(grid[0])
start = goal = None
for r in range(R):
    for c in range(C):
        if grid[r][c] == "S": start = (r, c)
        if grid[r][c] == "G": goal  = (r, c)

parent, q = {start: None}, deque([start])
while q:
    cur = q.popleft()
    if cur == goal: break
    for dr, dc in [(1,0),(-1,0),(0,1),(0,-1)]:
        nxt = (cur[0]+dr, cur[1]+dc)
        if (0 <= nxt[0] < R and 0 <= nxt[1] < C
                and grid[nxt[0]][nxt[1]] != "#" and nxt not in parent):
            parent[nxt] = cur; q.append(nxt)

path, node = [], goal
while node: path.append(node); node = parent[node]
print("Đường đi:", path[::-1])

6. Gắn với RL–UAV

Lưới + BFS chính là môi trường gridworld trong sách RL — cùng một cấu trúc đồ thị.
Hierarchical RL: tầng cao chọn waypoint trên đồ thị thưa (bài này), tầng thấp là policy liên tục.
Bài học C-space (“phồng vật cản”) dùng trực tiếp khi viết reward phạt va chạm: phạt theo khoảng cách tới C-obstacle, không phải tới bề mặt hình học thô của vật.

7. Lỗi thường gặp

Phạt va chạm theo kích thước vật thật nhưng mô phỏng robot như một điểm — không nhất quán với C-space.
Dùng DFS như “thuật toán tìm đường chính” trong không gian có chi phí cạnh đồng nhất — quên rằng nó không tối ưu độ dài.

8. Checklist

C-space là gì, vì sao nó cho phép xem robot như một điểm?
BFS tối ưu theo nghĩa nào, dưới điều kiện gì?
Vì sao lưới hóa khó sống ở không gian 12 chiều?

Lecture 7 — Optimal Discrete Planning: Dijkstra và A*

0. Mục tiêu học

Phân biệt Dijkstra và A*; định nghĩa heuristic admissible/consistent; liên hệ \(h\) với value function.

1. Câu hỏi trung tâm

Cạnh có giá khác nhau (xa hơn, nguy hiểm hơn, tốn pin hơn) — tìm đường rẻ nhất
thế nào, và tăng tốc bằng gợi ý (heuristic) ra sao?

2. Trực giác

Dijkstra = BFS bản nâng cấp: luôn mở rộng đỉnh đang có tổng giá rẻ nhất
tính từ start. Như nước lan trên địa hình: chỗ trũng (rẻ) ngập trước.
A* = Dijkstra có la bàn: ngoài giá đã đi \(g(n)\), cộng thêm ước lượng
còn lại đến đích \(h(n)\), ưu tiên mở \(f = g + h\) nhỏ nhất. Dijkstra loang
tròn đều mọi hướng; A loang hình giọt nước dồn về phía goal* — ít đỉnh hơn hẳn.

3. Cốt lõi kỹ thuật

Dijkstra: priority queue theo \(g\); mỗi đỉnh chốt (pop) một lần với giá tối ưu.
Yêu cầu giá cạnh không âm. \(O((|V|+|E|)\log|V|)\).
A: pop theo \(f = g + h\). Bảo đảm tối ưu nếu \(h\) admissible — không bao
giờ ước lượng quá giá thật (ví dụ: khoảng cách Euclid luôn ≤ đường thật giữa
vật cản). \(h\) consistent* (thỏa bất đẳng thức tam giác) thì không phải mở lại đỉnh.
Hai thái cực: \(h = 0\) → A thoái hóa thành Dijkstra (chậm, chắc);
\(h\) quá lớn (inadmissible) → greedy, nhanh nhưng có thể lệch tối ưu.
Weighted A (\(f = g + w\,h\), \(w > 1\)) là nút vặn giữa hai thái cực.

  Dijkstra mở rộng:            A* mở rộng:
        · · · · ·                    
     · · · · · · ·                  · · ·
    · · · S · · · ·      S · · · · · · · ► G
     · · · · · · ·                  · · ·
       · · · · ·  ► G               
   (tròn đều, tốn)             (dồn về goal, tiết kiệm)

4. Demo — A* trên lưới có giá địa hình

import heapq

cost = [[1,1,1,9,1],      # 9 = vùng "đắt" (gió mạnh/cấm bay mềm)
        [1,9,1,9,1],
        [1,9,1,1,1],
        [1,1,1,9,1],
        [9,9,1,1,1]]
R, C = 5, 5
start, goal = (0,0), (4,4)
h = lambda n: abs(n[0]-goal[0]) + abs(n[1]-goal[1])   # Manhattan: admissible

g = {start: 0}; parent = {start: None}
pq = [(h(start), start)]
while pq:
    f, cur = heapq.heappop(pq)
    if cur == goal: break
    for dr, dc in [(1,0),(-1,0),(0,1),(0,-1)]:
        nb = (cur[0]+dr, cur[1]+dc)
        if 0 <= nb[0] < R and 0 <= nb[1] < C:
            ng = g[cur] + cost[nb[0]][nb[1]]
            if ng < g.get(nb, 1e18):
                g[nb], parent[nb] = ng, cur
                heapq.heappush(pq, (ng + h(nb), nb))

path, n = [], goal
while n: path.append(n); n = parent[n]
print("Giá tối ưu:", g[goal]); print("Đường:", path[::-1])

Thử đổi vài ô 9 → 1 và xem đường “đổi ý” — cảm giác này chính là cost shaping.

5. Gắn với RL–UAV

\(g(n)\) = cost-to-come, \(h(n)\) = ước lượng cost-to-go → \(h\) là value function xấp xỉ. A* = tìm kiếm được dẫn đường bởi value — cùng dòng máu với heuristic trong MCTS/AlphaZero.
Hướng nghiên cứu tự nhiên: học \(h\) bằng mạng từ kinh nghiệm (learned heuristic) rồi cắm vào A* — kết hợp bảo đảm của search với tốc độ của học.
Cost map (gió, vùng cấm bay mềm, năng lượng) là bản rời rạc của reward / cost map bạn vẫn shaping trong RL–UAV.

6. Lỗi thường gặp

Dùng heuristic không admissible rồi vẫn khẳng định “A* luôn tối ưu”.
Nhầm “đường ngắn theo số ô” với “đường rẻ theo cost map” — đó là BFS vs Dijkstra/A*.

7. Checklist

Điều kiện để Dijkstra đúng? Vì sao cần giá cạnh không âm?
Admissible khác consistent chỗ nào — hệ quả lên thuật toán?
Giải thích “A* = Dijkstra + value function” bằng ngôn ngữ RL của bạn.

Lecture 8 — Randomized Motion Planning: RRT

0. Mục tiêu học

Thuật lại được vòng lặp RRT; phân biệt probabilistic completeness và optimality; biết RRT* / smoothing dùng khi nào.

1. Câu hỏi trung tâm

Không gian liên tục, nhiều chiều, không lưới hóa nổi — làm sao vẫn tìm được
đường? Trả lời: lấy mẫu ngẫu nhiên.

2. Trực giác

RRT (Rapidly-exploring Random Tree) mọc cây từ start: mỗi vòng ném một điểm
ngẫu nhiên vào không gian, tìm nút cây gần nó nhất, rồi mọc một đoạn ngắn từ nút
đó về phía điểm ngẫu nhiên (nếu không đụng vật cản). Điểm ngẫu nhiên là “mồi
nhử” kéo cây tỏa ra vùng chưa khám phá — vùng trống càng lớn càng dễ “câu” được
mẫu, nên cây tự động ưu tiên loang vào nơi rộng rãi. Thỉnh thoảng ném mồi ngay
tại goal (goal bias ~5–10%) để cây có xu hướng ngoặt về đích.

3. Cốt lõi kỹ thuật

Pseudo-code (thuộc lòng được):

T.init(x_start)
for i = 1..N:
    x_rand ← sample ngẫu nhiên (thỉnh thoảng = x_goal)
    x_near ← nút trong T gần x_rand nhất
    x_new  ← x_near + η · hướng(x_near → x_rand)      # bước dài η
    if đoạn [x_near, x_new] không đụng vật cản:
        T.add(x_new, cha = x_near)
        if ‖x_new − x_goal‖ < ε: return đường (lần ngược cha)

Tính chất cần nhớ:
- Probabilistically complete: có đường thì xác suất tìm ra → 1 khi \(N \to \infty\).
- Đường tìm được không tối ưu — gấp khúc, vòng vèo. Hai cách vá:
(a) shortcut smoothing: lặp lại việc chọn 2 điểm trên đường, nối thẳng nếu
không đụng; (b) RRT*: khi thêm nút, xét lại “cha rẻ hơn” trong lân cận và
rewire — tiệm cận tối ưu khi mẫu → ∞, trả giá bằng tính toán.
- Họ hàng: RRT-Connect (hai cây từ start và goal mọc vào nhau — nhanh trong thực
tế), PRM (xây roadmap dùng lại cho nhiều truy vấn).
- Chi phí thật nằm ở collision check (~90% thời gian chạy).

4. ASCII — cây RRT mọc quanh vật cản

   S──┬──●          ####
      │  └──●       ####         ● = nút cây
      ●      \      ####
      │       ●──●   
      └──●         \ 
          \         ●──●──● G
           ●  (cây vòng qua mép vật cản rồi chạm goal)

5. Demo — RRT 2D gọn (~45 dòng)

import numpy as np
rng = np.random.default_rng(0)

obstacles = [(5, 5, 1.6), (7, 2, 1.2), (3, 8, 1.4)]   # (cx, cy, bán kính)
def free(p):
    return all(np.hypot(p[0]-cx, p[1]-cy) > r for cx, cy, r in obstacles)
def seg_free(a, b, k=20):
    return all(free(a + t*(b-a)) for t in np.linspace(0, 1, k))

start, goal = np.array([0.5, 0.5]), np.array([9.5, 9.5])
nodes, parent, eta = [start], {0: None}, 0.6
for i in range(4000):
    x_rand = goal if rng.random() < 0.08 else rng.uniform(0, 10, 2)
    j = int(np.argmin([np.linalg.norm(n - x_rand) for n in nodes]))
    d = x_rand - nodes[j]
    x_new = nodes[j] + eta * d / (np.linalg.norm(d) + 1e-9)
    if free(x_new) and seg_free(nodes[j], x_new):
        nodes.append(x_new); parent[len(nodes)-1] = j
        if np.linalg.norm(x_new - goal) < 0.5:
            k, path = len(nodes)-1, []
            while k is not None: path.append(nodes[k]); k = parent[k]
            print(f"Tìm thấy sau {i} vòng, đường {len(path)} nút,",
                  f"dài ~{sum(np.linalg.norm(path[t]-path[t+1]) for t in range(len(path)-1)):.1f}")
            break

Lab 2 của khóa đúng bài này: đo vị trí cột PVC, chạy RRT, nạp waypoint cho
Crazyflie bay theo.

6. Gắn với RL–UAV

RRT là máy sinh demonstration rẻ: chạy nhiều map ngẫu nhiên → dataset (map, path) cho imitation learning hoặc warm-start policy.
Triết lý khác nhau: RRT tìm một đường cho một map (giải lại mỗi lần); RL học một policy cho cả phân phối map (amortized). Paper tốt thường đặt planner làm oracle và policy làm phiên bản realtime.
Exploration bằng sampling của RRT họ hàng với exploration trong RL; các hướng “RRT trong latent space học được” vẫn là đề tài mở.

7. Lỗi thường gặp

Kết luận “RRT cho đường tối ưu” — mặc định không; cần smoothing hoặc RRT*.
Goal bias quá lớn → cây kém khám phá, dễ kẹt trong môi trường hẹp / nhiều vật cản.
Bỏ qua thời gian collision check khi tối ưu code — thường đó mới là nút thắt.

8. Checklist

Vì sao cây RRT có xu hướng tỏa vào vùng trống (Voronoi bias)?
Probabilistic completeness khác optimality thế nào? RRT* thêm bước gì?
Trong demo, đổi eta và goal bias ảnh hưởng ra sao? (chạy thử)

Lecture 9 — Differential Flatness

0. Mục tiêu học

Phát biểu flatness của quadrotor; giải thích vì sao plan trong \((x,y,z,\psi)\); nắm pipeline RRT → min-snap → tracking.

1. Câu hỏi trung tâm

Quadrotor 12 state — plan quỹ đạo trong không gian nào cho khả thi mà không
phải giải bài toán 12 chiều?

2. Trực giác

Phát hiện đẹp nhất về quadrotor: nó là hệ differentially flat với flat
output \((x, y, z, \psi)\) (vị trí + yaw). Nghĩa là: chỉ cần vẽ quỹ đạo vị trí
đủ mượt \( (x(t), y(t), z(t), \psi(t)) \), thì toàn bộ phần còn lại — góc
nghiêng, tốc độ góc, tổng lực đẩy — suy ra được bằng công thức đại số từ quỹ
đạo và các đạo hàm của nó. Không cần tích phân, không cần mò.

Ví dụ cảm nhận được: drone bay vòng tròn đều — nó phải nghiêng vào tâm một góc
xác định (như xe máy ôm cua). Góc đó không phải lựa chọn tự do: quỹ đạo đã quy
định nó. Flatness tổng quát hóa: quỹ đạo vị trí quy định mọi thứ.

Vì sao đúng (mức trực giác): gia tốc \(\ddot{\mathbf r}\) do quỹ đạo cho trước;
tổng lực = \(m(\ddot{\mathbf r} + g\mathbf e_3)\); mà lực đẩy luôn dọc trục thân
→ hướng trục thân phải trùng hướng vector này → attitude được suy ra. Đạo
hàm tiếp (jerk, snap) cho tốc độ góc và mô-men.

3. Cốt lõi kỹ thuật

Planning giảm từ 12D xuống 4D flat space: plan spline trong \((x,y,z,\psi)\),
map ngược ra state + input đầy đủ.
Cần quỹ đạo khả vi đến đạo hàm bậc 4 của vị trí (snap) vì input mô-men dính
đến bậc 4 → dùng polynomial splines bậc 7 nối qua các waypoint.
Minimum-snap trajectory (Mellinger & Kumar): tối thiểu \(\int \|\mathrm{snap}\|^2 dt\)
qua các waypoint — bài QP giải nhanh; là chuẩn công nghiệp cho drone racing/cinematography.
Pipeline kinh điển: RRT tìm waypoint thô → min-snap làm mượt → LQR/PID bám quỹ đạo.

  RRT waypoints:      ●───●─────●───●        (gấp khúc, bay xấu)
  Min-snap:           ●⌒⌒⌒●⌒⌒⌒⌒⌒●⌒⌒⌒●        (mượt đến bậc snap, bay đẹp)
                              │
                     flatness map (đại số)
                              ▼
       θ(t), ω(t), F(t), M(t)  ← lệnh điều khiển đầy đủ, feasible

4. Demo — suy attitude từ quỹ đạo (flatness map planar)

import numpy as np
# Quỹ đạo phẳng: y(t) = sin(t), z(t) = 0.5 sin(2t); suy góc nghiêng cần có
m, g = 0.03, 9.81
for t in np.linspace(0, 3, 7):
    ay = -np.sin(t)                # ÿ
    az = -2.0*np.sin(2*t)          # z̈
    # thân drone phải hướng theo vector lực cần thiết m(a + g e3)
    theta = np.arctan2(-ay, az + g)          # góc nghiêng suy ra
    F = m*np.hypot(ay, az + g)               # tổng lực đẩy suy ra
    print(f"t={t:4.1f}  θ={np.degrees(theta):6.2f}°   F={F*1000:6.2f} mN")

Không giải ODE nào cả — attitude và thrust “rơi ra” từ đạo hàm quỹ đạo. Đó là flatness.

5. Gắn với RL–UAV

Action space thông minh: thay vì cho policy ra lệnh motor thô, có thể cho ra hệ số / điểm điều khiển spline trong flat space — mọi action dễ khả thi động lực hơn, thường an toàn và sample-efficient hơn.
Các bài agile flight / drone racing dùng flatness rất dày; baseline quỹ đạo tối ưu thời gian thường xây trên đó.
Giới hạn mô hình flat (kéo khí động lớn, hỏng cánh, không mô hình hóa đủ) chính là chỗ residual RL có đất diễn.

6. Lỗi thường gặp

Plan polyline gấp khúc rồi bảo “đã tôn trọng dynamics” — chưa, nếu chưa qua lớp làm mượt / flatness map.
Quên ràng buộc đạo hàm bậc cao (snap) rồi lấy input mô-men không khả thi.

7. Checklist

Flat output của quadrotor là gì? Flatness phát biểu điều gì?
Vì sao quỹ đạo vị trí cần mượt tới đạo hàm bậc 4 (snap)?
Vẽ lại pipeline RRT → min-snap → tracking controller.

Lecture 10 — Planning với ràng buộc dynamics (Kinodynamic Planning)

0. Mục tiêu học

Phân biệt planning hình học và kinodynamic; nêu ba chiến lược chính; giải thích MPC như re-plan vòng kín.

1. Câu hỏi trung tâm

Đường hình học không đụng vật cản nhưng robot không bay theo nổi (cua gắt
quá, cần gia tốc vượt giới hạn motor) — plan thế nào để đường sinh ra là đường
bay được?

2. Trực giác

Xe tải không đi được đường thiết kế cho xe đạp. Ba chiến lược:

Decouple (tách): plan hình học trước, làm mượt + timing sau
(chính là RRT → min-snap ở Lec 9). Nhanh, đủ tốt khi bay hiền.
Kinodynamic RRT: mọc cây trong không gian state \((p, v, ...)\); mỗi
cạnh không phải đoạn thẳng mà là kết quả mô phỏng forward \(\dot x = f(x,u)\)
với một \(u\) nào đó trong khoảng cho phép → mọi cạnh tự động khả thi.
Trả giá: chậm hơn nhiều (mỗi cạnh = một lần tích phân + collision check).
Trajectory optimization: viết thành bài tối ưu có ràng buộc —
\(\min \sum c(x_k, u_k)\) s.t. \(x_{k+1} = f(x_k, u_k)\), giới hạn input,
tránh vật cản — giải bằng nonlinear programming. Nghiệm đẹp, nhưng cần
khởi tạo tốt (thường lấy từ RRT) vì phi lồi, kẹt local minima.

MPC (Model Predictive Control) — cầu nối planning ↔ control, cần biết dù
khóa chỉ chạm nhẹ: mỗi bước điều khiển, giải trajectory optimization trên chân
trời ngắn (0.5–2s), thực thi chỉ bước đầu tiên, rồi giải lại từ trạng thái
đo mới. Tức là “replan liên tục ở tần số control” — chuẩn hiện nay cho drone
tránh vật cản tốc độ cao.

  Decoupled:       [RRT hình học] → [smooth+time] → [tracking ctrl]
  Kinodynamic:     [RRT trong state-space, cạnh = forward sim f(x,u)]
  Trajectory opt:  min Σc  s.t. dynamics + limits + obstacle   (NLP)
  MPC:             trajectory opt chân trời ngắn, lặp mỗi 10-50ms

3. Demo — thấy tận mắt “đường hình học ≠ đường bay được”

import numpy as np
# Đường hình học: rẽ vuông góc tại t=1 với tốc độ không đổi 3 m/s
# Drone giới hạn gia tốc a_max = 5 m/s^2 — kiểm tra khả thi
v, dt = 3.0, 0.001
t_turn = 0.2                       # cho phép 0.2s để đổi hướng 90°
# đổi hướng 90° ở tốc độ v trong t_turn cần gia tốc ngang:
a_need = v * (np.pi/2) / t_turn    # ≈ v·Δψ/Δt
print(f"Cần a ≈ {a_need:.1f} m/s² > a_max = 5 → KHÔNG bay được đường này")
print(f"Muốn khả thi: hoặc chậm lại còn {5*t_turn/(np.pi/2):.2f} m/s,"
      f" hoặc cua rộng bán kính ≥ {v*v/5:.2f} m")

Bài học: mọi “đường” đều ngầm chứa thời gian; khả thi hay không nằm ở
đạo hàm (vận tốc/gia tốc), không nằm ở hình vẽ.

4. Gắn với RL–UAV

Policy RL điều khiển trực tiếp có thể xem như cách amortize planning vào mạng: không giải planner online mỗi lần. So sánh chuẩn trong paper: RL vs MPC (chất lượng, độ trễ tính toán, độ bền khi model sai).
Tính khả thi kinodynamic buộc action bound trong env khớp giới hạn vật lý thật (thrust-to-weight, tốc độ góc tối đa). Env cho action bất khả thi sẽ tạo policy khó chuyển sang máy thật.
Hướng hybrid đáng theo dõi: MPC / shield tầng dưới, RL chọn reference hoặc tham số tầng trên.

5. Lỗi thường gặp

Đánh giá planner chỉ bằng độ dài đường mà quên gia tốc / giới hạn motor.
So RL với MPC nhưng cho MPC chân trời / thời gian tính không thực tế (quá ngắn hoặc quá dài so với onboard).

6. Checklist

Ba chiến lược kinodynamic — trade-off chính của từng cách?
MPC khác trajectory optimization “chạy một phát” ở điểm nào? Vì sao thường robust hơn với nhiễu?
Trong env RL của bạn, action bound lấy từ đâu — đã khớp giới hạn thật chưa?

Tổng kết Module 2

BFS/A xây tư duy đồ thị và heuristic (= value).
RRT cho phép làm việc trong không gian liên tục nhiều chiều.
Flatness biến quadrotor thành hệ “plan được dễ” hơn vẻ ngoài 12 chiều.
Kinodynamic / MPC nhắc: đường phải bay được, không chỉ vẽ được*.

Bài tiếp: 03_Estimation_SLAM.md — bỏ giả định biết state; chỉ còn cảm biến nhiễu.

Ebook Robotics · RL–UAV

MODULE 2 — Motion Planning (Lecture 6–10)

Bản ôn nhanh theo module

Chế độ Slide · phím ← → · F toàn màn · Esc về đọc · S mở slide

📚 Học sâu: Lectures/ Lec06–10. File này = ôn nhanh.

C-space → BFS → A*(cost/gió) → RRT → min-snap → MPC re-plan "vẽ được" ─────────────────────────────► "bay được"

Luồng module:

Lec 6   Lưới / đồ thị: BFS, DFS (+ ý tưởng C-space)
Lec 7   Đường tối ưu rời rạc: Dijkstra, A*
Lec 8   Không gian liên tục: RRT (và họ hàng)
Lec 9   Differential flatness + min-snap (đặc thù quadrotor)
Lec 10  Kinodynamic planning & MPC

Lecture 6 — Discrete Planning: BFS và DFS

0. Mục tiêu học

Giải thích C-space; chạy được BFS; hiểu vì sao lưới hóa thất bại ở chiều cao (curse of dimensionality).

1. Câu hỏi trung tâm

Nếu thế giới rời rạc hóa được thành đồ thị (ô lưới, waypoint), tìm đường đi
từ start đến goal thế nào cho có hệ thống?

2. Trực giác

BFS = loang như vết dầu: từ start lan đều từng lớp, lớp nào chạm goal thì
xong. DFS = chuột chui hang: lao sâu một hướng đến cùng, cụt thì lùi.

3. Cốt lõi kỹ thuật

Đồ thị \(G = (V, E)\): đỉnh = cấu hình rời rạc, cạnh = bước đi hợp lệ (không đụng).
BFS dùng queue (FIFO): duyệt theo lớp → tìm đường ít cạnh nhất (tối ưu
khi mọi cạnh giá bằng nhau). Độ phức tạp \(O(|V| + |E|)\).
DFS dùng stack (LIFO): tốn ít bộ nhớ hơn theo chiều sâu, không đảm bảo
đường ngắn. Trong planning gần như chỉ dùng để kiểm tra liên thông.
Cả hai cần tập visited — quên nó là lặp vô hạn.
Lời nguyền chiều (curse of dimensionality): lưới \(k\) ô/chiều, \(n\) chiều
→ \(k^n\) đỉnh. Quadrotor 12D với 100 ô/chiều là \(10^{24}\) — bất khả thi.
Đây là lý do tồn tại của Lec 8 (sampling).

4. ASCII — BFS loang trên lưới (S=start, G=goal, #=vật cản)

  bước 0          bước 3              hoàn tất (số = thứ tự lớp)
  S . . . #       S 1 2 3 #           S 1 2 3 #
  . . . . .       1 2 3 . .           1 2 3 4 5
  . # # . .  →    2 # # . .     →     2 # # 5 6
  . . . . .       3 . . . .           3 4 5 6 7
  . . . G .       . . . G .           4 5 6 7 G ← đường: lần ngược 8→0

5. Demo — BFS ~25 dòng

from collections import deque

grid = ["S...#",
        ".....",
        ".##..",
        ".....",
        "...G."]
R, C = len(grid), len(grid[0])
start = goal = None
for r in range(R):
    for c in range(C):
        if grid[r][c] == "S": start = (r, c)
        if grid[r][c] == "G": goal  = (r, c)

parent, q = {start: None}, deque([start])
while q:
    cur = q.popleft()
    if cur == goal: break
    for dr, dc in [(1,0),(-1,0),(0,1),(0,-1)]:
        nxt = (cur[0]+dr, cur[1]+dc)
        if (0 <= nxt[0] < R and 0 <= nxt[1] < C
                and grid[nxt[0]][nxt[1]] != "#" and nxt not in parent):
            parent[nxt] = cur; q.append(nxt)

path, node = [], goal
while node: path.append(node); node = parent[node]
print("Đường đi:", path[::-1])

6. Gắn với RL–UAV

Lưới + BFS chính là môi trường gridworld trong sách RL — cùng một cấu trúc đồ thị.
Hierarchical RL: tầng cao chọn waypoint trên đồ thị thưa (bài này), tầng thấp là policy liên tục.
Bài học C-space (“phồng vật cản”) dùng trực tiếp khi viết reward phạt va chạm: phạt theo khoảng cách tới C-obstacle, không phải tới bề mặt hình học thô của vật.

7. Lỗi thường gặp

Phạt va chạm theo kích thước vật thật nhưng mô phỏng robot như một điểm — không nhất quán với C-space.
Dùng DFS như “thuật toán tìm đường chính” trong không gian có chi phí cạnh đồng nhất — quên rằng nó không tối ưu độ dài.

8. Checklist

C-space là gì, vì sao nó cho phép xem robot như một điểm?
BFS tối ưu theo nghĩa nào, dưới điều kiện gì?
Vì sao lưới hóa khó sống ở không gian 12 chiều?

Lecture 7 — Optimal Discrete Planning: Dijkstra và A*

0. Mục tiêu học

Phân biệt Dijkstra và A*; định nghĩa heuristic admissible/consistent; liên hệ \(h\) với value function.

1. Câu hỏi trung tâm

Cạnh có giá khác nhau (xa hơn, nguy hiểm hơn, tốn pin hơn) — tìm đường rẻ nhất
thế nào, và tăng tốc bằng gợi ý (heuristic) ra sao?

2. Trực giác

Dijkstra = BFS bản nâng cấp: luôn mở rộng đỉnh đang có tổng giá rẻ nhất
tính từ start. Như nước lan trên địa hình: chỗ trũng (rẻ) ngập trước.
A* = Dijkstra có la bàn: ngoài giá đã đi \(g(n)\), cộng thêm ước lượng
còn lại đến đích \(h(n)\), ưu tiên mở \(f = g + h\) nhỏ nhất. Dijkstra loang
tròn đều mọi hướng; A loang hình giọt nước dồn về phía goal* — ít đỉnh hơn hẳn.

3. Cốt lõi kỹ thuật

Dijkstra: priority queue theo \(g\); mỗi đỉnh chốt (pop) một lần với giá tối ưu.
Yêu cầu giá cạnh không âm. \(O((|V|+|E|)\log|V|)\).
A: pop theo \(f = g + h\). Bảo đảm tối ưu nếu \(h\) admissible — không bao
giờ ước lượng quá giá thật (ví dụ: khoảng cách Euclid luôn ≤ đường thật giữa
vật cản). \(h\) consistent* (thỏa bất đẳng thức tam giác) thì không phải mở lại đỉnh.
Hai thái cực: \(h = 0\) → A thoái hóa thành Dijkstra (chậm, chắc);
\(h\) quá lớn (inadmissible) → greedy, nhanh nhưng có thể lệch tối ưu.
Weighted A (\(f = g + w\,h\), \(w > 1\)) là nút vặn giữa hai thái cực.

  Dijkstra mở rộng:            A* mở rộng:
        · · · · ·                    
     · · · · · · ·                  · · ·
    · · · S · · · ·      S · · · · · · · ► G
     · · · · · · ·                  · · ·
       · · · · ·  ► G               
   (tròn đều, tốn)             (dồn về goal, tiết kiệm)

4. Demo — A* trên lưới có giá địa hình

import heapq

cost = [[1,1,1,9,1],      # 9 = vùng "đắt" (gió mạnh/cấm bay mềm)
        [1,9,1,9,1],
        [1,9,1,1,1],
        [1,1,1,9,1],
        [9,9,1,1,1]]
R, C = 5, 5
start, goal = (0,0), (4,4)
h = lambda n: abs(n[0]-goal[0]) + abs(n[1]-goal[1])   # Manhattan: admissible

g = {start: 0}; parent = {start: None}
pq = [(h(start), start)]
while pq:
    f, cur = heapq.heappop(pq)
    if cur == goal: break
    for dr, dc in [(1,0),(-1,0),(0,1),(0,-1)]:
        nb = (cur[0]+dr, cur[1]+dc)
        if 0 <= nb[0] < R and 0 <= nb[1] < C:
            ng = g[cur] + cost[nb[0]][nb[1]]
            if ng < g.get(nb, 1e18):
                g[nb], parent[nb] = ng, cur
                heapq.heappush(pq, (ng + h(nb), nb))

path, n = [], goal
while n: path.append(n); n = parent[n]
print("Giá tối ưu:", g[goal]); print("Đường:", path[::-1])

Thử đổi vài ô 9 → 1 và xem đường “đổi ý” — cảm giác này chính là cost shaping.

5. Gắn với RL–UAV

\(g(n)\) = cost-to-come, \(h(n)\) = ước lượng cost-to-go → \(h\) là value function xấp xỉ. A* = tìm kiếm được dẫn đường bởi value — cùng dòng máu với heuristic trong MCTS/AlphaZero.
Hướng nghiên cứu tự nhiên: học \(h\) bằng mạng từ kinh nghiệm (learned heuristic) rồi cắm vào A* — kết hợp bảo đảm của search với tốc độ của học.
Cost map (gió, vùng cấm bay mềm, năng lượng) là bản rời rạc của reward / cost map bạn vẫn shaping trong RL–UAV.

6. Lỗi thường gặp

Dùng heuristic không admissible rồi vẫn khẳng định “A* luôn tối ưu”.
Nhầm “đường ngắn theo số ô” với “đường rẻ theo cost map” — đó là BFS vs Dijkstra/A*.

7. Checklist

Điều kiện để Dijkstra đúng? Vì sao cần giá cạnh không âm?
Admissible khác consistent chỗ nào — hệ quả lên thuật toán?
Giải thích “A* = Dijkstra + value function” bằng ngôn ngữ RL của bạn.

Lecture 8 — Randomized Motion Planning: RRT

0. Mục tiêu học

Thuật lại được vòng lặp RRT; phân biệt probabilistic completeness và optimality; biết RRT* / smoothing dùng khi nào.

1. Câu hỏi trung tâm

Không gian liên tục, nhiều chiều, không lưới hóa nổi — làm sao vẫn tìm được
đường? Trả lời: lấy mẫu ngẫu nhiên.

2. Trực giác

3. Cốt lõi kỹ thuật

Pseudo-code (thuộc lòng được):

T.init(x_start)
for i = 1..N:
    x_rand ← sample ngẫu nhiên (thỉnh thoảng = x_goal)
    x_near ← nút trong T gần x_rand nhất
    x_new  ← x_near + η · hướng(x_near → x_rand)      # bước dài η
    if đoạn [x_near, x_new] không đụng vật cản:
        T.add(x_new, cha = x_near)
        if ‖x_new − x_goal‖ < ε: return đường (lần ngược cha)

4. ASCII — cây RRT mọc quanh vật cản

   S──┬──●          ####
      │  └──●       ####         ● = nút cây
      ●      \      ####
      │       ●──●   
      └──●         \ 
          \         ●──●──● G
           ●  (cây vòng qua mép vật cản rồi chạm goal)

5. Demo — RRT 2D gọn (~45 dòng)

import numpy as np
rng = np.random.default_rng(0)

obstacles = [(5, 5, 1.6), (7, 2, 1.2), (3, 8, 1.4)]   # (cx, cy, bán kính)
def free(p):
    return all(np.hypot(p[0]-cx, p[1]-cy) > r for cx, cy, r in obstacles)
def seg_free(a, b, k=20):
    return all(free(a + t*(b-a)) for t in np.linspace(0, 1, k))

start, goal = np.array([0.5, 0.5]), np.array([9.5, 9.5])
nodes, parent, eta = [start], {0: None}, 0.6
for i in range(4000):
    x_rand = goal if rng.random() < 0.08 else rng.uniform(0, 10, 2)
    j = int(np.argmin([np.linalg.norm(n - x_rand) for n in nodes]))
    d = x_rand - nodes[j]
    x_new = nodes[j] + eta * d / (np.linalg.norm(d) + 1e-9)
    if free(x_new) and seg_free(nodes[j], x_new):
        nodes.append(x_new); parent[len(nodes)-1] = j
        if np.linalg.norm(x_new - goal) < 0.5:
            k, path = len(nodes)-1, []
            while k is not None: path.append(nodes[k]); k = parent[k]
            print(f"Tìm thấy sau {i} vòng, đường {len(path)} nút,",
                  f"dài ~{sum(np.linalg.norm(path[t]-path[t+1]) for t in range(len(path)-1)):.1f}")
            break

Lab 2 của khóa đúng bài này: đo vị trí cột PVC, chạy RRT, nạp waypoint cho
Crazyflie bay theo.

6. Gắn với RL–UAV

RRT là máy sinh demonstration rẻ: chạy nhiều map ngẫu nhiên → dataset (map, path) cho imitation learning hoặc warm-start policy.
Triết lý khác nhau: RRT tìm một đường cho một map (giải lại mỗi lần); RL học một policy cho cả phân phối map (amortized). Paper tốt thường đặt planner làm oracle và policy làm phiên bản realtime.
Exploration bằng sampling của RRT họ hàng với exploration trong RL; các hướng “RRT trong latent space học được” vẫn là đề tài mở.

7. Lỗi thường gặp

Kết luận “RRT cho đường tối ưu” — mặc định không; cần smoothing hoặc RRT*.
Goal bias quá lớn → cây kém khám phá, dễ kẹt trong môi trường hẹp / nhiều vật cản.
Bỏ qua thời gian collision check khi tối ưu code — thường đó mới là nút thắt.

8. Checklist

Vì sao cây RRT có xu hướng tỏa vào vùng trống (Voronoi bias)?
Probabilistic completeness khác optimality thế nào? RRT* thêm bước gì?
Trong demo, đổi eta và goal bias ảnh hưởng ra sao? (chạy thử)

Lecture 9 — Differential Flatness

0. Mục tiêu học

Phát biểu flatness của quadrotor; giải thích vì sao plan trong \((x,y,z,\psi)\); nắm pipeline RRT → min-snap → tracking.

1. Câu hỏi trung tâm

Quadrotor 12 state — plan quỹ đạo trong không gian nào cho khả thi mà không
phải giải bài toán 12 chiều?

2. Trực giác

3. Cốt lõi kỹ thuật

Planning giảm từ 12D xuống 4D flat space: plan spline trong \((x,y,z,\psi)\),
map ngược ra state + input đầy đủ.
Cần quỹ đạo khả vi đến đạo hàm bậc 4 của vị trí (snap) vì input mô-men dính
đến bậc 4 → dùng polynomial splines bậc 7 nối qua các waypoint.
Minimum-snap trajectory (Mellinger & Kumar): tối thiểu \(\int \|\mathrm{snap}\|^2 dt\)
qua các waypoint — bài QP giải nhanh; là chuẩn công nghiệp cho drone racing/cinematography.
Pipeline kinh điển: RRT tìm waypoint thô → min-snap làm mượt → LQR/PID bám quỹ đạo.

  RRT waypoints:      ●───●─────●───●        (gấp khúc, bay xấu)
  Min-snap:           ●⌒⌒⌒●⌒⌒⌒⌒⌒●⌒⌒⌒●        (mượt đến bậc snap, bay đẹp)
                              │
                     flatness map (đại số)
                              ▼
       θ(t), ω(t), F(t), M(t)  ← lệnh điều khiển đầy đủ, feasible

4. Demo — suy attitude từ quỹ đạo (flatness map planar)

import numpy as np
# Quỹ đạo phẳng: y(t) = sin(t), z(t) = 0.5 sin(2t); suy góc nghiêng cần có
m, g = 0.03, 9.81
for t in np.linspace(0, 3, 7):
    ay = -np.sin(t)                # ÿ
    az = -2.0*np.sin(2*t)          # z̈
    # thân drone phải hướng theo vector lực cần thiết m(a + g e3)
    theta = np.arctan2(-ay, az + g)          # góc nghiêng suy ra
    F = m*np.hypot(ay, az + g)               # tổng lực đẩy suy ra
    print(f"t={t:4.1f}  θ={np.degrees(theta):6.2f}°   F={F*1000:6.2f} mN")

Không giải ODE nào cả — attitude và thrust “rơi ra” từ đạo hàm quỹ đạo. Đó là flatness.

5. Gắn với RL–UAV

Action space thông minh: thay vì cho policy ra lệnh motor thô, có thể cho ra hệ số / điểm điều khiển spline trong flat space — mọi action dễ khả thi động lực hơn, thường an toàn và sample-efficient hơn.
Các bài agile flight / drone racing dùng flatness rất dày; baseline quỹ đạo tối ưu thời gian thường xây trên đó.
Giới hạn mô hình flat (kéo khí động lớn, hỏng cánh, không mô hình hóa đủ) chính là chỗ residual RL có đất diễn.

6. Lỗi thường gặp

Plan polyline gấp khúc rồi bảo “đã tôn trọng dynamics” — chưa, nếu chưa qua lớp làm mượt / flatness map.
Quên ràng buộc đạo hàm bậc cao (snap) rồi lấy input mô-men không khả thi.

7. Checklist

Flat output của quadrotor là gì? Flatness phát biểu điều gì?
Vì sao quỹ đạo vị trí cần mượt tới đạo hàm bậc 4 (snap)?
Vẽ lại pipeline RRT → min-snap → tracking controller.

Lecture 10 — Planning với ràng buộc dynamics (Kinodynamic Planning)

0. Mục tiêu học

Phân biệt planning hình học và kinodynamic; nêu ba chiến lược chính; giải thích MPC như re-plan vòng kín.

1. Câu hỏi trung tâm

2. Trực giác

Xe tải không đi được đường thiết kế cho xe đạp. Ba chiến lược:

Decouple (tách): plan hình học trước, làm mượt + timing sau
(chính là RRT → min-snap ở Lec 9). Nhanh, đủ tốt khi bay hiền.
Kinodynamic RRT: mọc cây trong không gian state \((p, v, ...)\); mỗi
cạnh không phải đoạn thẳng mà là kết quả mô phỏng forward \(\dot x = f(x,u)\)
với một \(u\) nào đó trong khoảng cho phép → mọi cạnh tự động khả thi.
Trả giá: chậm hơn nhiều (mỗi cạnh = một lần tích phân + collision check).
Trajectory optimization: viết thành bài tối ưu có ràng buộc —
\(\min \sum c(x_k, u_k)\) s.t. \(x_{k+1} = f(x_k, u_k)\), giới hạn input,
tránh vật cản — giải bằng nonlinear programming. Nghiệm đẹp, nhưng cần
khởi tạo tốt (thường lấy từ RRT) vì phi lồi, kẹt local minima.

  Decoupled:       [RRT hình học] → [smooth+time] → [tracking ctrl]
  Kinodynamic:     [RRT trong state-space, cạnh = forward sim f(x,u)]
  Trajectory opt:  min Σc  s.t. dynamics + limits + obstacle   (NLP)
  MPC:             trajectory opt chân trời ngắn, lặp mỗi 10-50ms

3. Demo — thấy tận mắt “đường hình học ≠ đường bay được”

import numpy as np
# Đường hình học: rẽ vuông góc tại t=1 với tốc độ không đổi 3 m/s
# Drone giới hạn gia tốc a_max = 5 m/s^2 — kiểm tra khả thi
v, dt = 3.0, 0.001
t_turn = 0.2                       # cho phép 0.2s để đổi hướng 90°
# đổi hướng 90° ở tốc độ v trong t_turn cần gia tốc ngang:
a_need = v * (np.pi/2) / t_turn    # ≈ v·Δψ/Δt
print(f"Cần a ≈ {a_need:.1f} m/s² > a_max = 5 → KHÔNG bay được đường này")
print(f"Muốn khả thi: hoặc chậm lại còn {5*t_turn/(np.pi/2):.2f} m/s,"
      f" hoặc cua rộng bán kính ≥ {v*v/5:.2f} m")

Bài học: mọi “đường” đều ngầm chứa thời gian; khả thi hay không nằm ở
đạo hàm (vận tốc/gia tốc), không nằm ở hình vẽ.

4. Gắn với RL–UAV

Policy RL điều khiển trực tiếp có thể xem như cách amortize planning vào mạng: không giải planner online mỗi lần. So sánh chuẩn trong paper: RL vs MPC (chất lượng, độ trễ tính toán, độ bền khi model sai).
Tính khả thi kinodynamic buộc action bound trong env khớp giới hạn vật lý thật (thrust-to-weight, tốc độ góc tối đa). Env cho action bất khả thi sẽ tạo policy khó chuyển sang máy thật.
Hướng hybrid đáng theo dõi: MPC / shield tầng dưới, RL chọn reference hoặc tham số tầng trên.

5. Lỗi thường gặp

Đánh giá planner chỉ bằng độ dài đường mà quên gia tốc / giới hạn motor.
So RL với MPC nhưng cho MPC chân trời / thời gian tính không thực tế (quá ngắn hoặc quá dài so với onboard).

6. Checklist

Ba chiến lược kinodynamic — trade-off chính của từng cách?
MPC khác trajectory optimization “chạy một phát” ở điểm nào? Vì sao thường robust hơn với nhiễu?
Trong env RL của bạn, action bound lấy từ đâu — đã khớp giới hạn thật chưa?

Tổng kết Module 2

BFS/A xây tư duy đồ thị và heuristic (= value).
RRT cho phép làm việc trong không gian liên tục nhiều chiều.
Flatness biến quadrotor thành hệ “plan được dễ” hơn vẻ ngoài 12 chiều.
Kinodynamic / MPC nhắc: đường phải bay được, không chỉ vẽ được*.

Bài tiếp: 03_Estimation_SLAM.md — bỏ giả định biết state; chỉ còn cảm biến nhiễu.