Chương A · máy đổi state theo luật nào khi nhận \(u\)? — xương sống mọi controller & env RL

Khung 6 câu hỏi

🖼️ Hình nhanh: Tăng ga không “nhảy” vị trí — phải qua gia tốc → vận tốc → vị trí (trễ hai tầng).

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.

   u (lực) ──► a = F/m ──► v = ∫a ──► p = ∫v
                 ▲
                 │  muốn dịch ngang?
                 │  quadrotor: phải nghiêng θ trước
                 ★ underactuated: không có lực ngang trực tiếp

0. Mục tiêu học

1. Viết dạng state-space \(\dot{x}=f(x,u)\) và double integrator.
2. Nhớ 3 phương trình quadrotor phẳng; giải thích underactuation.
3. Tuyến tính hóa quanh hover → \(Ax+Bu\); nêu khi xấp xỉ gãy.
4. Chạy demo open-loop: tắt máy / hover / lệch 1% — thấy vì sao cần feedback (Lec 3).

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

Đẩy xe hàng trên đường đồi chè: xe không dịch tức thì theo tay bạn — có quán tính, có ma sát. Drone cũng vậy: tăng ga không làm nó đứng ở chỗ cao hơn ngay, mà tạo gia tốc, gia tốc tích thành vận tốc, vận tốc tích thành vị trí.

Chuỗi “tích phân hai lần” là lý do điều khiển / RL trên vị trí khó hơn game state tức thì: tác động của bạn đến vị trí bị trễ hai tầng. Muốn dịch ngang giữa hai hàng chè? Quadrotor không có “động cơ đẩy ngang” — phải nghiêng rồi mới bay ngang; đó là underactuation, không phải bug phần mềm.

🇻🇳 Chuyện thật (sân A): Buổi sớm trên sườn chè Thái Nguyên / Mộc Châu, bạn muốn máy trượt ngang dọc luống để phủ ảnh.
Tăng lực tổng không giúp nó “trượt sang trái” — phải nghiêng \(\theta\) trước, lực đẩy nghiêng mới sinh gia tốc ngang; trong lúc nghiêng, độ cao cũng muốn tụt nếu không bù ga.
Gió sườn buổi chiều làm chuỗi tích phân hai lần càng “nhão”: lệnh \(u\) hôm qua không còn đúng hôm nay.
Chốt: underactuation + \(\ddot{p}=f(u,\theta)\) = trên chuyến bay, mọi path “chỉ waypoint” phải qua động lực nghiêng–trượt, không nhảy vị trí tức thì. Chi tiết sân: ../00C.

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

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

2.1 Dạng chuẩn

\[ \dot{x} = f(x,u), \qquad x \in \mathbb{R}^n,\ u \in \mathbb{R}^m \]

Mẹo: hệ bậc hai (Newton \(F=ma\)) → đặt \(x = [\text{vị trí}; \text{vận tốc}]\).

Double integrator (xương sống vòng position):

\[ \dot{p}=v, \qquad \dot{v}=u/m \]

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

2.2 Quadrotor phẳng — mô hình đáng nhớ nhất module

                 F1              F2
                  ▲               ▲
                  │               │
              ┌───┴───────────────┴───┐
              │        thân drone      │   ← nghiêng góc θ
              └───────────┬───────────┘
                          │
                          ▼  mg

     F = F1 + F2          (đẩy dọc thân)
     M = (F2 − F1)·d      (mô-men)

\[ \ddot{y}=-\frac{F}{m}\sin\theta,\quad \ddot{z}=\frac{F}{m}\cos\theta-g,\quad \ddot{\theta}=\frac{M}{I} \]

Đọc chậm ba dòng — “tính cách” của drone:

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

2.3 Linearization quanh hover (Sơ đồ cầu sang Lec 3–5)

Quanh \(\theta\approx 0\), \(F\approx mg\): \(\sin\theta\approx\theta\), \(\cos\theta\approx 1\)

\[ \ddot{y}\approx -g\,\theta, \qquad \ddot{z}\approx \frac{\Delta F}{m}, \qquad \ddot{\theta}=\frac{M}{I} \]

  f(x,u) phi tuyến ──► ≈ ──► Ax + Bu (hover)
         │                        │
         │ θ lớn, flip            ▼
         ≠ hết hiệu lực      Lec 3–5: P / PD / LQR

3. Demo — vì sao open-loop chết

"""
Mục tiêu demo:
  So sánh 3 chính sách open-loop trên quadrotor phẳng:
  (1) tắt máy  (2) hover đúng lực  (3) lệch 1% giữa hai cánh.
Kết luận mang sang Lec 3: cần feedback.
"""
import numpy as np

# --- Tham số cỡ Crazyflie (SI) ---
m, I, g, d = 0.03, 1.4e-5, 9.81, 0.046
dt = 0.001  # bước Euler; nhỏ để bớt lỗi số

def f(x, u):
    """Động lực học liên tục.
    x = [y, z, theta, vy, vz, omega]
    u = [F1, F2]  lực từng phía (đơn giản hóa 2 rotor)
    """
    y, z, th, vy, vz, om = x
    F = u[0] + u[1]              # tổng lực đẩy dọc thân
    M = (u[1] - u[0]) * d        # mô-men do lệch lực
    return np.array([
        vy,
        vz,
        om,
        -F / m * np.sin(th),
         F / m * np.cos(th) - g,
         M / I,
    ])

def rollout(u_fn, T=2.0):
    """Mô phỏng T giây, trả về state cuối."""
    x = np.zeros(6)              # gốc, đứng yên, theta=0
    for _ in range(int(T / dt)):
        u = u_fn(x)
        x = x + dt * f(x, u)     # Euler: x_{k+1} = x_k + dt * f
    return x

u_hover = np.array([m * g / 2, m * g / 2])

x_off = rollout(lambda x: np.zeros(2))
x_ok  = rollout(lambda x: u_hover)
x_bad = rollout(lambda x: u_hover * np.array([1.01, 0.99]))

print("Tắt máy   [y,z,th] =", np.round(x_off[:3], 3), "  → rơi theo g")
print("Hover OK  [y,z,th] =", np.round(x_ok[:3], 3),  "  → gần 0 nếu model khớp")
print("Lệch 1%   [y,z,th] =", np.round(x_bad[:3], 3), "  → xoay + trôi: open-loop mong manh")

🧪 Kỳ vọng demo:
- Tắt máy: \(z\) âm mạnh (~ rơi \(\frac12 g T^2\)).
- Hover OK: \([y,z,\theta]\approx 0\).
- Lệch 1%: \(\theta\) và \(y\) lệch rõ — cùng “cố hover” nhưng không đo sai số thì không sửa. → Lec 3 đóng vòng.

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

• Planar 6-state là env huấn luyện tối thiểu đáng tự viết trước simulator lớn.
• Underactuation giải thích vì sao action “attitude + thrust” phổ biến trong RL agile — tôn trọng cấu trúc \(f\).
• Domain randomization = random hóa \(m, I, d\), hệ số motor trong \(f\).
• Với năng lượng: đường phủ nhiều cua ngang → nhiều nghiêng → tốn \(F\) hơn polyline ngắn trên giấy.
• Với gió: gió vào \(\dot{v}\) như nhiễu; open-loop path không đủ — cần feedback (Lec 3–5) hoặc replan.

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

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

• ⚠️ Nhầm input \(u\) với setpoint gửi xuống firmware — hai tầng khác nhau.
• ⚠️ Dùng mô hình tuyến tính quanh hover giải thích manoeuvre \(\theta\) lớn — ngoài miền xấp xỉ.
• ⚠️ Trong sim RL: cho action vượt giới hạn thrust thật rồi ngạc nhiên khi không chuyển máy thật.

7. Checklist

1. Viết được 3 PT quadrotor phẳng từ trí nhớ?
2. Underactuated nghĩa là gì — vì sao phải nghiêng mới dịch ngang?
3. Hover linearization cho ra hệ nào? Khi nào gãy?
4. Demo lệch 1% minh họa điều gì cho feedback?

⚡ Nhớ một câu: Dynamics = vật lý của robot; quadrotor underactuated — muốn ngang phải nghiêng, và open-loop lệch 1% là đủ chết.

🔗 Sang bài sau mang theo: \(f\) + underactuation + chứng cứ “hover open-loop mong manh” → Lec 3 chữa bằng feedback \(u = k_p e\).

Trước: Lec01 · Sau: Lec03 — Feedback Control