Chương B · không gian liên tục, không lưới hóa nổi — lấy mẫu để tìm đường

Khung 6 câu hỏi

🖼️ Hình nhanh: Ném mồi ngẫu nhiên → kéo cành cây về phía vùng trống (Voronoi bias); thỉnh thoảng ném đúng goal.

Hình minh họa (seminar-ready)

Hình gốc hoặc minh họa cho ebook — không cắt từ slide Princeton. Dùng Present: bật Slide · F toàn màn.

   S──┬──●          ####
      │  └──●       ####         ● = nút cây
      ●      \      ####
      │       ●──●
      └──●         \
          \         ●──●──● G
           ●
   ★ probabilistically complete; đường thường gấp khúc ≠ tối ưu

0. Mục tiêu học

1. Thuật lại vòng lặp RRT (sample / nearest / steer / check / add).
2. Phân biệt probabilistic completeness vs optimality; biết RRT* / smoothing.
3. Hiểu Voronoi bias và vai trò goal bias / \(\eta\).
4. Chạy demo RRT 2D tránh đĩa; đọc số vòng, độ dài path.

1. Trực giác — nói bằng lời thường

Phố đặc nhiều ngõ hẹp, góc yaw–vị trí không lưới hóa nổi: cách làm là mọc cây từ start thay vì quét hết ô.

Mỗi vòng: ném một điểm ngẫu nhiên vào không gian (mồi nhử), tìm nút cây gần nhất, mọc một đoạn ngắn \(\eta\) về phía mồi nếu không đụng. Vùng trống càng lớn càng dễ “câu” được mẫu → cây tự ưu tiên loang nơi rộng (Voronoi bias). Thỉnh thoảng ném mồi ngay goal (bias ~5–10%) để ngoặt về đích.

Path tìm được thường gấp khúc như đi theo lối tắt tạm — chưa đẹp bay; Lec 9 (min-snap) làm mượt trước khi giao tracker.

🇻🇳 Chuyện thật (sân B): Trong phố đặc Hà Nội / HCM, C-space đầy hẻm và góc chết — lưới mịn nổ bộ nhớ trước khi tìm ra đường.
RRT mọc cây ngẫu nhiên qua khe hẹp: vài nghìn mẫu có thể nối start–goal nơi BFS trên lưới quá thô hoặc quá nặng.
Cái giá: path gấp như đi tắt qua ngõ — góc 90° cứng, quadrotor không bay nổi nếu đưa thẳng xuống động lực.
Chốt: RRT = khả năng tìm đường trong không gian liên tục; trên chuyến bay, path thô phải qua làm mượt (Lec 9) trước khi bám. Chi tiết sân: ../00C.

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

2.1 Pseudo-code thuộc lòng

T.init(x_start)
for i = 1..N:
    x_rand ← sample (thỉnh thoảng = x_goal)      # goal bias
    x_near ← nút trong T gần x_rand nhất
    x_new  ← x_near + η · hướng(x_near → x_rand) # steer
    if đoạn [x_near, x_new] free:                # collision check
        T.add(x_new, cha = x_near)
        if ‖x_new − x_goal‖ < ε: return path (lần ngược cha)

2.2 Tính chất cần nhớ

2.3 Sơ đồ pipeline thực dụng UAV

  [RRT waypoints thô] ──► [shortcut / RRT*] ──► [min-snap Lec9] ──► [LQR/PID track]
         │
         ▼
  dataset (map, path) ──► imitation / warm-start RL

2.4 Tham số ảnh hưởng

  η lớn  → mọc nhanh, dễ đụng / bỏ lỡ khe hẹp
  η nhỏ  → chậm, tỉ mỉ hơn
  goal bias lớn → về đích sớm nhưng kém khám phá (kẹt ngõ hẹp)
  goal bias nhỏ → khám phá rộng, chậm chạm goal

3. Demo — RRT 2D tránh đĩa tròn

"""
RRT 2D tránh đĩa tròn. Goal bias 8%.
In ra số vòng và độ dài path khi chạm gần goal.
"""
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, radius)

def free(p):
    """True nếu điểm p không nằm trong vật cản (đã 'phồng' sẵn bằng radius)."""
    return all(np.hypot(p[0] - cx, p[1] - cy) > r for cx, cy, r in obstacles)

def seg_free(a, b, k=20):
    """Kiểm tra đoạn thẳng a→b bằng k mẫu trên đoạn."""
    for t in np.linspace(0, 1, k):
        if not free(a + t * (b - a)):
            return False
    return True

start, goal = np.array([0.5, 0.5]), np.array([9.5, 9.5])
nodes = [start]
parent = {0: None}
eta = 0.6
success = False

for i in range(4000):
    x_rand = goal if rng.random() < 0.08 else rng.uniform(0, 10, size=2)
    dists = [np.linalg.norm(n - x_rand) for n in nodes]
    j = int(np.argmin(dists))
    direction = x_rand - nodes[j]
    length = np.linalg.norm(direction) + 1e-9
    x_new = nodes[j] + eta * direction / length

    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:
            idx, path = len(nodes) - 1, []
            while idx is not None:
                path.append(nodes[idx])
                idx = parent[idx]
            path.reverse()
            plen = sum(
                np.linalg.norm(path[t] - path[t + 1])
                for t in range(len(path) - 1)
            )
            print(f"Thành công sau {i} vòng | {len(path)} nút | dài ≈ {plen:.2f}")
            success = True
            break

if not success:
    print("Chưa chạm goal — tăng N, chỉnh eta/goal bias, hoặc dùng RRT-Connect.")

🧪 Kỳ vọng demo:
- Với seed 0: thường in Thành công sau … vòng | … nút | dài ≈ … (số thay đổi nếu đổi seed).
- Path gấp khúc, không phải đường ngắn Euclid — bình thường.
- Tăng goal bias quá mạnh trong map hẹp hơn có thể chậm hơn hoặc fail.
★ Collision check chiếm phần lớn thời gian khi scale lên 3D.

4. Gắn RL–UAV / path-planning đa mục tiêu của bạn

• RRT = máy sinh demonstration rẻ: nhiều map ngẫu nhiên → dataset (map, path) cho IL / warm-start.
• Triết lý: RRT tìm một đường / một map; RL học một policy / phân phối map (amortized). Paper tốt: planner = oracle, policy = realtime.
• Độ phủ / năng lượng: RRT thuần tối ưu không gắn cost — gắn cost → T-RRT / RRT* với edge cost gió–pin, hoặc tối ưu sau (Lec 9–10).
• Gió đổi theo thời gian: path RRT tĩnh cần replan / MPC; đừng bay open-loop waypoint cũ.
• Nhớ C-space: radius obstacle đã gồm “phồng” an toàn coverage.

5. Ưu / nhược (câu 5–6)

6. 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 → kém khám phá, kẹt 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; quên kinodynamic (đường vẽ ≠ đường bay được — Lec 10).

7. Checklist

1. Vì sao cây RRT có xu hướng tỏa vào vùng trống (Voronoi bias)?
2. Probabilistic completeness khác optimality thế nào? RRT* thêm bước gì?
3. Trong demo, đổi eta và goal bias ảnh hưởng ra sao?
4. Pipeline RRT → smooth → track dùng khi nào cho UAV của bạn?

⚡ Nhớ một câu: RRT mọc cây bằng lấy mẫu — đầy đủ theo xác suất, không tối ưu mặc định; đẹp bay nhờ smooth / RRT* / min-snap.

🔗 Sang bài sau mang theo: Waypoint RRT gấp khúc → Lec 9 differential flatness + min-snap biến polyline thành quỹ đạo bay được.

Trước: Lec07 · Sau: Lec09 — Differential Flatness