MODULE 1 — Dynamics & Feedback Control (Lecture 1–5)

📚 Học sâu: từng bài có hình + demo ở Lectures/ (Lec01–05).
File này = bản gom để ôn nhanh / tra cứu.

Mục tiêu module.
Hiểu robot chuyển động theo luật nào (\(\dot{x} = f(x,u)\)), và cách thiết kế input \(u\) để hệ ổn định, bám mục tiêu — trước khi đưa học máy / RL vào vòng điều khiển.

  f(x,u) ──► open-loop chết ──► feedback ──► PD ổn định ──► LQR (cầu RL)
  Lec2         Lec2 demo          Lec3         Lec4            Lec5

Luồng module:

Lec 1  Robot tự chủ là gì? (bản đồ stack)
Lec 2  Dynamics — viết f(x,u)
Lec 3  Feedback control — vì sao cần vòng kín
Lec 4  Ổn định & PD
Lec 5  LQR — gain tối ưu từ mô hình (+ cầu nối sang RL)

Lecture 1 — Robotics là gì

0. Mục tiêu học

Phát biểu được bài toán robot tự chủ; vẽ stack Sense–Think–Act; nêu ba mức “cắm” policy RL trên UAV.

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

Máy thế nào thì gọi là robot, và bài toán robot tự chủ gồm những mảnh nào?

2. Trực giác

Robot = máy tự đóng vòng kín với thế giới vật lý: cảm nhận → quyết định →
hành động → thế giới thay đổi → cảm nhận lại. Khác chương trình offline ở chỗ:
sai lầm có giá vật lý (rơi máy), thông tin không đầy đủ (cảm biến nhiễu), và
thời gian thực (không được “suy nghĩ” 5 giây mỗi bước khi đang rơi).

Ba ứng dụng khóa hay nhắc: UAV, xe tự lái, tay máy. Cả ba chung một khung:

        nhiễu ngoài (gió, ma sát, va chạm)
                     │
                     ▼
   u(t) ──►┌────────────────┐──► x(t) thật
           │  THẾ GIỚI /    │
           │  DYNAMICS f    │
           └────────────────┘
                     │ cảm biến (IMU, camera, GPS...)
                     ▼
                z(t) = h(x) + nhiễu
                     │
           ┌────────────────┐
           │ ESTIMATOR      │──► x̂(t) (ước lượng state)
           └────────────────┘
                     │
           ┌────────────────┐
           │ PLANNER/POLICY │──► quỹ đạo / hành vi mong muốn
           └────────────────┘
                     │
           ┌────────────────┐
           │ CONTROLLER     │──► u(t)  (đóng vòng)
           └────────────────┘

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

State \(x\): tập biến đủ để dự đoán tương lai nếu biết input. Với quadrotor
đầy đủ: vị trí \((x,y,z)\), vận tốc, quaternion/Euler góc, tốc độ góc → 12–13 chiều.
Tách tầng (separation): thực tế kỹ thuật, ta tách estimator / planner /
controller và thiết kế riêng từng khối. Không tối ưu toàn cục nhưng kiểm soát được.
Tần số vòng lặp: attitude control ~500Hz–1kHz, position control ~50–100Hz,
planning ~1–10Hz, vision ~15–30Hz. Nhiều tầng chạy tốc độ khác nhau đồng thời.

4. Gắn với RL–UAV

Khi nói “policy điều khiển drone”, phải nói rõ policy thay khối nào:
- thay planner (output waypoint) → an toàn hơn, dễ triển khai, RL chạy tần số thấp;
- thay position controller (output attitude setpoint) → phổ biến nhất trong các bài RL–UAV;
- thay attitude controller (output lực motor) → khó nhất, đòi hỏi mô phỏng rất trung thực.
Việc chọn “chỗ cắm” của policy thường quyết định phần lớn độ khó sim-to-real.

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

Gộp tất cả thành “end-to-end” mà không nêu tầng action — khiến baseline và so sánh trở nên không công bằng.
Quên rằng các vòng (attitude / position / planning) chạy tần số khác nhau; policy 10 Hz không thay được attitude 500 Hz một cách ngây thơ.

6. Checklist

Vẽ lại được sơ đồ vòng kín từ trí nhớ?
State của quadrotor gồm những thành phần nào, khoảng bao nhiêu chiều?
Ba mức cắm RL policy vào stack UAV — mức nào rủi ro sim-to-real nhất?

Lecture 2 — Dynamics (Động lực học)

0. Mục tiêu học

Viết được dạng state-space; giải thích underactuation của quadrotor; tuyến tính hóa quanh hover và nêu khi nào xấp xỉ gãy.

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

Cho lệnh \(u\), trạng thái robot biến đổi theo luật nào? Viết luật đó thành
\(\dot x = f(x, u)\) như thế nào?

2. Trực giác

Dynamics là “vật lý của chính robot”. Bạn đẩy xe hàng: xe không dịch tức thì
theo tay bạn — 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 lập tức, 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” này là lý do điều khiển khó:
tác động của bạn đến vị trí bị trễ hai tầng.

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

3.1 Dạng chuẩn state-space

Mọi hệ cơ học viết được về:
\[ \dot x = f(x, u), \qquad x \in \mathbb{R}^n, u \in \mathbb{R}^m \]
Mẹo chuẩn: hệ bậc hai (Newton \(F = ma\)) → đặt \(x = [\text{vị trí}; \text{vận tốc}]\).

Ví dụ 1 — double integrator (hệ đơn giản nhất, nhưng là xương sống):
một khối lượng \(m\) trên trục, đẩy bằng lực \(u\):
\[ \dot p = v, \qquad \dot v = u/m \]
Đây gần đúng là drone chuyển động một trục khi đã giữ được attitude. Rất nhiều
bài toán UAV cấp position quy về double integrator + nhiễu.

Ví dụ 2 — con lắc (pendulum):
\[ \ddot\theta = -\frac{g}{l}\sin\theta + \frac{u}{ml^2} \]
Phi tuyến vì \(\sin\theta\). Là “ruồi giấm” của control: đủ đơn giản để giải,
đủ phi tuyến để thấy mọi vấn đề.

3.2 Quadrotor phẳng (planar) — mô hình đáng nhớ nhất module

Drone nhìn nghiêng, chuyển động trong mặt phẳng đứng \((y, z)\), góc nghiêng \(\theta\),
hai cánh quạt tạo lực \(F_1, F_2\):

                 F1              F2
                  ▲               ▲
                  │               │
              ┌───┴───────────────┴───┐
              │        thân drone      │   ← nghiêng góc θ so với ngang
              └───────────┬───────────┘
                          │
                          ▼  mg (trọng lực)

     Tổng lực đẩy  F = F1 + F2   (dọc trục thân, vuông góc cánh)
     Mô-men xoay   M = (F2 - F1)·d   (d = nửa sải tay)

Phương trình (khối lượng \(m\), quán tính \(I\)):
\[ \ddot y = -\frac{F}{m}\sin\theta, \qquad \ddot z = \frac{F}{m}\cos\theta - g, \qquad \ddot\theta = \frac{M}{I} \]

Đọc chậm ba dòng này, chúng chứa toàn bộ “tính cách” của drone:
- Muốn dịch ngang (\(\ddot y \neq 0\))? Bắt buộc phải nghiêng (\(\sin\theta\)).
Drone không có “lực ngang” trực tiếp — đây là hệ underactuated:
6 bậc tự do (3D) nhưng chỉ 4 input.
- Nghiêng thì thành phần đẩy dọc \( \cos\theta \) giảm → muốn giữ độ cao khi
nghiêng phải tăng tổng lực đẩy. Các khớp nối chéo (coupling) kiểu này là
thứ khiến điều khiển bằng tay khó.
- Chuỗi tác động: \(M \to \theta \to \ddot y \to y\): muốn dịch 1 mét phải đi
qua bốn lần tích phân. Trễ sâu → dễ dao động nếu điều khiển vụng.

3.3 Linearization (tuyến tính hóa) — cây cầu sang control tuyến tính

Quanh điểm cân bằng hover (\(\theta \approx 0\), \(F \approx mg\)), lấy xấp xỉ
\(\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} \]
Hệ thành tuyến tính: \(\dot x = Ax + Bu\). Toàn bộ Lec 3–5 làm việc trên dạng này.
Tổng quát: \(A = \partial f/\partial x\), \(B = \partial f/\partial u\) tại điểm cân bằng.

4. Demo — mô phỏng quadrotor phẳng rơi tự do vs hover

import numpy as np

m, I, g, d = 0.03, 1.4e-5, 9.81, 0.046   # số liệu cỡ Crazyflie
dt = 0.001

def f(x, u):
    """x = [y, z, th, vy, vz, om]; u = [F1, F2]"""
    y, z, th, vy, vz, om = x
    F, M = u[0] + u[1], (u[1] - u[0]) * d
    return np.array([vy, vz, om,
                     -F/m*np.sin(th),
                      F/m*np.cos(th) - g,
                      M/I])

def rollout(u_func, T=2.0):
    x = np.zeros(6)
    for k in range(int(T/dt)):
        x = x + dt * f(x, u_func(x))       # Euler integration
    return x

print("Tắt máy   :", rollout(lambda x: np.array([0, 0]))[:3])     # rơi ~19.6m
print("Hover đúng:", rollout(lambda x: np.array([m*g/2, m*g/2]))[:3])
print("Lệch 1%   :", rollout(lambda x: np.array([m*g/2*1.01, m*g/2*0.99]))[:3])

Chạy thử sẽ thấy: hover open-loop đúng lý thuyết thì đứng yên, nhưng chỉ lệch
lực 1% giữa hai cánh là drone xoay và văng đi — minh chứng vì sao cần feedback.

5. Gắn với RL–UAV

Mô hình planar 6-state này là môi trường huấn luyện tối thiểu đáng tự viết trước khi dùng simulator lớn: bạn kiểm soát được từng giả định.
Underactuation giải thích vì sao action space “attitude + thrust” phổ biến trong RL agile flight: nó tôn trọng cấu trúc dynamics.
Domain randomization thực chất là random hóa \(m, I, d\), hệ số motor, … trong \(f\).

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

Nhầm input \(u\) với setpoint gửi xuống firmware: đó là hai tầng khác nhau.
Dùng mô hình tuyến tính quanh hover để giải thích manoeuvre cực mạnh (\(\theta\) lớn) — ngoài miền hợp lệ của 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 được sang máy thật.

7. Checklist

Viết được dynamics quadrotor phẳng từ trí nhớ (3 phương trình)?
Vì sao drone phải nghiêng mới dịch ngang được? Underactuated nghĩa là gì?
Tuyến tính hóa quanh hover cho ra hệ nào? Khi nào xấp xỉ này gãy?
(gợi ý: bay aggressive, \(\theta\) lớn)

Lecture 3 — Feedback Control

0. Mục tiêu học

Phân biệt open-loop và feedback; giải thích vì sao P thuần dao động trên hệ có quán tính; nêu steady-state error.

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

Vì sao điều khiển vòng hở (open-loop) thất bại, và feedback sửa điều đó thế nào?

2. Trực giác

Open-loop = “tính trước rồi nhắm mắt chạy”. Như rót nước không nhìn cốc: kế
hoạch đúng đến mấy, chỉ cần model lệch nhẹ hoặc ai đó xê cốc là tràn.
Feedback = “vừa rót vừa nhìn”: đo sai số giữa muốn và có, rồi hành động theo
sai số. Cái hay: bạn không cần model hoàn hảo — sai số tự nói cho bạn biết phải làm gì.

Demo Lec 2 đã cho thấy: hover open-loop chết vì lệch 1%. Feedback là thuốc chữa.

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

3.1 Luật P (proportional)

Sai số \(e = x_{des} - x\). Luật đơn giản nhất:
\[ u = k_p\, e \]
Lệch nhiều kéo mạnh, lệch ít kéo nhẹ.

3.2 Vấn đề của P thuần trên hệ quán tính

Với double integrator \(\ddot p = u\), đặt \(u = k_p(p_{des} - p)\):
\[ \ddot p + k_p\, p = k_p\, p_{des} \]
Đây là phương trình dao động điều hòa không ma sát — nghiệm là sin/cos:
robot lắc quanh đích mãi mãi, không bao giờ dừng. Kéo về mà không phanh thì
quá đà; quá đà thì kéo ngược; lại quá đà…

 vị trí
   │      ╭─╮     ╭─╮     ╭─╮
 p*┤─────╱───╲───╱───╲───╱───╲──────  ← P thuần: dao động không tắt
   │    ╱     ╲_╱     ╲_╱     ╲
   │   ╱
   └──┴────────────────────────────► t

3.3 Sơ đồ khối chuẩn (đọc được sơ đồ này là đọc được sách control)

  x_des ──►(+)──── e ────►┌────────────┐── u ──►┌───────────┐──┬──► x
            ▲ –           │ CONTROLLER │        │  PLANT f  │  │
            │             └────────────┘        └───────────┘  │
            └───────────────────── đo lường (sensor) ◄─────────┘

“Plant” = hệ bị điều khiển (drone). Vòng tròn (+/–) = tính sai số.

4. Demo — open-loop vs P-control dưới nhiễu gió

import numpy as np
dt, T = 0.01, 8.0
def sim(controller, wind=0.3):
    p, v, log = 0.0, 0.0, []
    for k in range(int(T/dt)):
        u = controller(p, v)
        a = u + wind                # gió = nhiễu cộng vào gia tốc
        v += a*dt; p += v*dt; log.append(p)
    return np.array(log)

target = 1.0
openloop = sim(lambda p, v: 0.0)                     # không phản hồi
pctrl    = sim(lambda p, v: 4.0*(target - p))        # P thuần
print("open-loop cuối:", openloop[-1])   # trôi vô hạn theo gió
print("P-control cuối:", pctrl[-1])      # dao động quanh ~1.075 (lệch tĩnh + lắc)

Quan sát: P thuần (i) dao động, (ii) dừng lệch khỏi đích dưới gió hằng
(steady-state error) — hai khuyết tật mà Lec 4 (D) và khâu I (tích phân) xử lý.

5. Gắn với RL–UAV

Policy RL học được về bản chất cũng là một luật feedback \(u = \pi(x)\) — chỉ khác là phi tuyến và học từ dữ liệu. Hiểu P/PD giúp đọc được policy đã học “đang làm gì” (nhiều policy hội tụ về thứ rất giống PD quanh hover).
Reward shaping kiểu \(-\|e\|^2\) chính là nói với agent: “hãy trở thành một feedback controller tốt”.

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

Kết luận “P đủ dùng” chỉ vì nhìn một mô phỏng không có gió / không có sai model.
Phạt reward kiểu \(-\|e\|^2\) nhưng không phạt đạo hàm / độ giật — tái tạo đúng bệnh dao động của P thuần.

7. Checklist

Vì sao open-loop dễ thất bại khi mô hình lệch, còn feedback thường sống sót hơn?
Vì sao P thuần dao động trên hệ có quán tính? (viết được phương trình bậc hai)
Steady-state error dưới nhiễu hằng — vì sao P không khử được?

Lecture 4 — Stability và PD Control

0. Mục tiêu học

Phát biểu ổn định Lyapunov / tiệm cận; dùng eigenvalue để kiểm tra; chỉnh PD qua \(\zeta\); biết vai trò khâu I.

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

“Ổn định” định nghĩa chính xác là gì, kiểm tra bằng gì, và thêm khâu D giúp gì?

2. Trực giác

Thả viên bi:
- đáy chén → đẩy nhẹ, bi lăn về đáy: ổn định tiệm cận
- mặt bàn phẳng → đẩy, bi ở chỗ mới: ổn định biên (marginal)
- đỉnh mũ úp ngược → đẩy nhẹ, bi lăn tuột: bất ổn định

Khâu D (derivative) = thêm ma sát nhân tạo: không nhìn “đang lệch bao nhiêu”
mà nhìn “đang lao nhanh cỡ nào” để phanh sớm. Bi trong chén có mật ong thì về
đáy êm không lắc. PD = kéo về (P) + phanh (D).

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

3.1 Định nghĩa

Điểm cân bằng \(x^*\) (nơi \(f(x^*, u^*) = 0\)):
- Ổn định (Lyapunov): xuất phát đủ gần thì mãi ở gần.
- Ổn định tiệm cận: gần + hội tụ về \(x^*\) khi \(t \to \infty\).

3.2 Tiêu chuẩn eigenvalue — công cụ số 1

Hệ tuyến tính \(\dot x = Ax\): ổn định tiệm cận khi và chỉ khi mọi eigenvalue
của \(A\) có phần thực âm (nằm bên trái mặt phẳng phức).
- Phần thực = tốc độ tắt (càng âm càng nhanh).
- Phần ảo = tần số dao động (càng lớn càng lắc).
Hệ phi tuyến: linearize rồi xét eigenvalue của \(A\) — kết luận đúng cục bộ.

3.3 PD trên double integrator

\[ u = k_p(p_{des} - p) - k_d\, v \quad\Rightarrow\quad \ddot p + k_d\dot p + k_p p = k_p p_{des} \]
Chính là lò xo–giảm chấn: \(k_p\) = độ cứng lò xo, \(k_d\) = độ nhớt giảm chấn.
Đặt \(\omega_n = \sqrt{k_p}\) (tần số tự nhiên), \(\zeta = k_d / (2\sqrt{k_p})\) (hệ số tắt):

  ζ < 1  (underdamped)   :  vọt lố rồi lắc tắt dần      ╱╲╱‾‾
  ζ = 1  (critical)      :  về nhanh nhất không vọt lố  ╱‾‾‾
  ζ > 1  (overdamped)    :  về từ từ, chậm              ╱⎯⎯⎯ (bò lên)

Tuning thực dụng: chọn \(k_p\) theo tốc độ đáp ứng muốn có, rồi
\(k_d \approx 2\sqrt{k_p}\) (critical damping), tinh chỉnh quanh đó.

3.4 Vì sao có cả PID

Khâu I cộng dồn sai số theo thời gian → khử lệch tĩnh (gió hằng, pin yếu,
drone nặng hơn danh định). Giá phải trả: dễ vọt lố, windup. Firmware
(PX4/Betaflight/Crazyflie) dùng PID lồng nhau: vòng attitude trong, position ngoài.

4. Demo — quét \(k_d\) để thấy 3 chế độ damping

import numpy as np
dt, T, kp = 0.01, 6.0, 9.0        # ω_n = 3 rad/s
def pd(kd):
    p, v, log = 0.0, 0.0, []
    for _ in range(int(T/dt)):
        u = kp*(1.0 - p) - kd*v
        v += u*dt; p += v*dt; log.append(p)
    return np.array(log)

for kd, name in [(1.5, "under"), (6.0, "critical"), (14.0, "over")]:
    tr = pd(kd)
    print(f"{name:9s} kd={kd:4.1f}  overshoot={tr.max()-1:+.3f}  p(T)={tr[-1]:.3f}")

5. Gắn với RL–UAV

Eigenvalue là ngôn ngữ để nói “policy này có làm hệ kín ổn định không” — nhiều hướng stability-certified RL xoay quanh câu hỏi này (Lyapunov + học).
Trong reward, phạt \(\|v\|\) hay tốc độ đổi action chính là cách “cài \(k_d\)” vào policy: thiếu nó, policy thường giật và dao động — cùng bệnh với P thuần.
Nhiều hệ thực tế giữ PID/LQR attitude bên trong, policy chỉ xuất setpoint: ổn định được “bảo hành” bởi vòng trong, RL lo tầng nhiệm vụ.

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

Tăng \(k_p\) mãi để “bám chặt hơn” mà quên rằng nhiễu đo sẽ bị khuếch đại và motor dễ bão hòa.
Nhầm ổn định của mô hình tuyến tính hóa với ổn định toàn cục của hệ phi tuyến.

7. Checklist

Ba kiểu hành vi ổn định (tiệm cận / biên / bất ổn), ví dụ viên bi tương ứng?
Phát biểu tiêu chuẩn eigenvalue; phần thực và phần ảo cho biết gì?
\(\zeta\) là gì? Muốn không vọt lố mà về nhanh, chọn \(\zeta\) khoảng bao nhiêu?
Khâu I chữa gì và gây rủi ro gì (windup)?

Lecture 5 — Linear Quadratic Regulator (LQR)

0. Mục tiêu học

Phát biểu bài toán LQR; đọc được vai trò \(Q,R\); liên hệ \(P\) với value function và \(-Kx\) với policy tối ưu; nêu giới hạn của LQR.

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

Thay vì mò \(k_p, k_d\) bằng tay cho hệ nhiều chiều, có cách nào tính gain tối ưu
một phát từ mô hình không?

2. Trực giác

PD tuning tay được với 1–2 biến. Nhưng quadrotor 12 state, 4 input — chọn tay
hàng chục gain đan chéo nhau là ác mộng. LQR đổi cách đặt vấn đề: bạn không chọn
gain, bạn chọn mức độ quan tâm:
- \(Q\): lệch state nào là “đắt”? (lệch độ cao đắt hơn lệch yaw?)
- \(R\): tốn năng lượng/action mạnh “đắt” cỡ nào?

rồi toán học trả về ma trận gain \(K\) tối ưu. Giống thuê chuyên gia tuning:
bạn chỉ nêu khẩu vị, họ tính chi tiết.

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

3.1 Bài toán

Hệ tuyến tính \(\dot x = Ax + Bu\) (từ linearization quanh hover). Tối thiểu:
\[ J = \int_0^\infty \big( x^\top Q x + u^\top R u \big)\, dt, \qquad Q \succeq 0,\ R \succ 0 \]
- \(x^\top Q x\): phạt lệch trạng thái (bình phương → lệch lớn phạt rất nặng)
- \(u^\top R u\): phạt dùng lực (tiết kiệm pin, tránh bão hòa motor)

3.2 Nghiệm

Giải phương trình Riccati đại số tìm \(P\):
\[ A^\top P + PA - PBR^{-1}B^\top P + Q = 0 \]
rồi:
\[ K = R^{-1}B^\top P, \qquad \boxed{u = -Kx} \]
Ba điều đáng nhớ:
1. Nghiệm là state feedback tuyến tính — vẫn họ hàng với PD, nhưng gain
tối ưu và tự động phối hợp mọi kênh chéo nhau.
2. \(x^\top P x\) chính là cost-to-go tối ưu — người RL đọc ngay ra:
\(P\) là value function! LQR = trường hợp value iteration hội tụ có công thức đóng.
3. Hệ kín \(\dot x = (A - BK)x\) được bảo đảm ổn định (nếu \((A,B)\) controllable).

3.3 Trade-off Q vs R (trực giác tuning)

   Q lớn, R nhỏ  →  gain lớn  →  bám sát, phản ứng dữ, tốn năng lượng, dễ chạm
                                  giới hạn motor, khuếch đại nhiễu đo
   Q nhỏ, R lớn  →  gain nhỏ  →  êm, tiết kiệm, nhưng lười và bám kém

Thực dụng: bắt đầu \(Q = \mathrm{diag}(1/x_{i,max}^2)\), \(R = \mathrm{diag}(1/u_{j,max}^2)\)
(quy tắc Bryson) rồi chỉnh.

3.4 Giới hạn của LQR

Chỉ tối ưu quanh điểm linearize — bay xa hover (lộn vòng) là hết hiệu lực.
Không xử lý ràng buộc cứng (giới hạn thrust) — MPC sinh ra để vá chỗ này.
Cần model; model sai nhiều thì “tối ưu trên giấy”.

Lab 1 của khóa: nạp gain LQR vào firmware Crazyflie để hover — sinh viên thấy
ngay reality gap: gain tối ưu trong mô hình vẫn phải chỉnh khi bay thật.

4. Demo — LQR cho quadrotor phẳng (dùng scipy)

import numpy as np
from scipy.linalg import solve_continuous_are

m, I, g, d = 0.03, 1.4e-5, 9.81, 0.046
# state x = [y, z, th, vy, vz, om], input u = [ΔF, M] quanh hover
A = np.zeros((6, 6)); A[0, 3] = A[1, 4] = A[2, 5] = 1.0; A[3, 2] = -g
B = np.zeros((6, 2)); B[4, 0] = 1/m; B[5, 1] = 1/I

Q = np.diag([10, 10, 1, 1, 1, 0.1])
R = np.diag([1.0, 0.1])
P = solve_continuous_are(A, B, Q, R)
K = np.linalg.solve(R, B.T @ P)
print("Gain K =\n", np.round(K, 3))
print("Eig hệ kín:", np.round(np.linalg.eigvals(A - B @ K), 2))  # đều Re<0

# thử: xuất phát lệch 1m ngang, 0.5m thấp
dt, x = 0.002, np.array([1.0, -0.5, 0, 0, 0, 0.0])
for k in range(int(4/dt)):
    u = -K @ x
    xdot = A @ x + B @ u
    x = x + dt * xdot
print("Sau 4s:", np.round(x, 4))   # → về gần 0 hết

Điểm đáng ngắm trong output: cột gain cho thấy kênh \(M\) (mô-men) tự động
“nhìn” cả \(y\) và \(v_y\) — LQR tự phát hiện “muốn sửa lệch ngang phải nghiêng”,
điều bạn phải tự nghĩ ra nếu tuning PD tay.

5. Gắn với RL–UAV

Đây là bài quan trọng nhất của module với người làm RL:
- LQR = trường hợp RL giải được bằng giấy bút khi dynamics tuyến tính và cost bậc hai: \(P\) ↔ value function, \(-Kx\) ↔ policy tối ưu, phương trình Riccati ↔ Bellman.
Nếu thuật toán RL không tái tạo gần đúng \(K\) trên bài LQR chuẩn, hãy nghi ngờ implementation trước.
- LQR là baseline bắt buộc trong các bài RL–UAV về hover/tracking — reviewer phía robotics sẽ hỏi.
- \(Q, R\) là ngôn ngữ reward shaping có nguyên tắc: phóng theo giới hạn vật lý (Bryson), tránh hệ số “bốc thuốc”.
- Residual RL: \(u = -Kx + \pi_\theta(x)\) — LQR gánh ổn định, policy học phần phi tuyến còn thiếu. Đáng thử cho hướng NCS.

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

Coi LQR là “đã giải xong điều khiển UAV” — quên miền tuyến tính hóa và ràng buộc thrust.
Chọn \(Q,R\) tùy hứng không gắn đơn vị / giới hạn vật lý (bỏ qua quy tắc Bryson).
Không đặt LQR làm baseline khi viết paper RL hover/tracking.

7. Checklist

Viết được hàm cost LQR và nêu vai trò \(Q, R\)?
\(u = -Kx\): vì sao nói LQR là “PD nhiều chiều được tối ưu hóa”?
Chỉ ra tương ứng LQR ↔ RL (value, policy, Bellman)?
Ba giới hạn của LQR và hướng vá tương ứng (gain scheduling, MPC, robust / residual RL)?

Tổng kết Module 1

Dynamics cho bạn \(f\). Feedback cho bạn cách sống sót khi \(f\) không hoàn hảo.
PD tạo trực giác chỉnh tham số. LQR cho nghiệm tối ưu tuyến tính — và là “trường hợp có công thức đóng” gần nhất với value/policy mà bạn dùng trong RL.

Bài tiếp: 02_Motion_Planning.md — robot đã đứng vững, giờ học cách tìm đường.

Ebook Robotics · RL–UAV

MODULE 1 — Dynamics & Feedback Control (Lecture 1–5)

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: từng bài có hình + demo ở Lectures/ (Lec01–05).
File này = bản gom để ôn nhanh / tra cứu.

  f(x,u) ──► open-loop chết ──► feedback ──► PD ổn định ──► LQR (cầu RL)
  Lec2         Lec2 demo          Lec3         Lec4            Lec5

Luồng module:

Lec 1  Robot tự chủ là gì? (bản đồ stack)
Lec 2  Dynamics — viết f(x,u)
Lec 3  Feedback control — vì sao cần vòng kín
Lec 4  Ổn định & PD
Lec 5  LQR — gain tối ưu từ mô hình (+ cầu nối sang RL)

Lecture 1 — Robotics là gì

0. Mục tiêu học

Phát biểu được bài toán robot tự chủ; vẽ stack Sense–Think–Act; nêu ba mức “cắm” policy RL trên UAV.

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

Máy thế nào thì gọi là robot, và bài toán robot tự chủ gồm những mảnh nào?

2. Trực giác

Ba ứng dụng khóa hay nhắc: UAV, xe tự lái, tay máy. Cả ba chung một khung:

        nhiễu ngoài (gió, ma sát, va chạm)
                     │
                     ▼
   u(t) ──►┌────────────────┐──► x(t) thật
           │  THẾ GIỚI /    │
           │  DYNAMICS f    │
           └────────────────┘
                     │ cảm biến (IMU, camera, GPS...)
                     ▼
                z(t) = h(x) + nhiễu
                     │
           ┌────────────────┐
           │ ESTIMATOR      │──► x̂(t) (ước lượng state)
           └────────────────┘
                     │
           ┌────────────────┐
           │ PLANNER/POLICY │──► quỹ đạo / hành vi mong muốn
           └────────────────┘
                     │
           ┌────────────────┐
           │ CONTROLLER     │──► u(t)  (đóng vòng)
           └────────────────┘

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

State \(x\): tập biến đủ để dự đoán tương lai nếu biết input. Với quadrotor
đầy đủ: vị trí \((x,y,z)\), vận tốc, quaternion/Euler góc, tốc độ góc → 12–13 chiều.
Tách tầng (separation): thực tế kỹ thuật, ta tách estimator / planner /
controller và thiết kế riêng từng khối. Không tối ưu toàn cục nhưng kiểm soát được.
Tần số vòng lặp: attitude control ~500Hz–1kHz, position control ~50–100Hz,
planning ~1–10Hz, vision ~15–30Hz. Nhiều tầng chạy tốc độ khác nhau đồng thời.

4. Gắn với RL–UAV

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

Gộp tất cả thành “end-to-end” mà không nêu tầng action — khiến baseline và so sánh trở nên không công bằng.
Quên rằng các vòng (attitude / position / planning) chạy tần số khác nhau; policy 10 Hz không thay được attitude 500 Hz một cách ngây thơ.

6. Checklist

Vẽ lại được sơ đồ vòng kín từ trí nhớ?
State của quadrotor gồm những thành phần nào, khoảng bao nhiêu chiều?
Ba mức cắm RL policy vào stack UAV — mức nào rủi ro sim-to-real nhất?

Lecture 2 — Dynamics (Động lực học)

0. Mục tiêu học

Viết được dạng state-space; giải thích underactuation của quadrotor; tuyến tính hóa quanh hover và nêu khi nào xấp xỉ gãy.

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

Cho lệnh \(u\), trạng thái robot biến đổi theo luật nào? Viết luật đó thành
\(\dot x = f(x, u)\) như thế nào?

2. Trực giác

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

3.1 Dạng chuẩn state-space

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

3.2 Quadrotor phẳng (planar) — mô hình đáng nhớ nhất module

Drone nhìn nghiêng, chuyển động trong mặt phẳng đứng \((y, z)\), góc nghiêng \(\theta\),
hai cánh quạt tạo lực \(F_1, F_2\):

                 F1              F2
                  ▲               ▲
                  │               │
              ┌───┴───────────────┴───┐
              │        thân drone      │   ← nghiêng góc θ so với ngang
              └───────────┬───────────┘
                          │
                          ▼  mg (trọng lực)

     Tổng lực đẩy  F = F1 + F2   (dọc trục thân, vuông góc cánh)
     Mô-men xoay   M = (F2 - F1)·d   (d = nửa sải tay)

Phương trình (khối lượng \(m\), quán tính \(I\)):
\[ \ddot y = -\frac{F}{m}\sin\theta, \qquad \ddot z = \frac{F}{m}\cos\theta - g, \qquad \ddot\theta = \frac{M}{I} \]

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

3.3 Linearization (tuyến tính hóa) — cây cầu sang control tuyến tính

4. Demo — mô phỏng quadrotor phẳng rơi tự do vs hover

import numpy as np

m, I, g, d = 0.03, 1.4e-5, 9.81, 0.046   # số liệu cỡ Crazyflie
dt = 0.001

def f(x, u):
    """x = [y, z, th, vy, vz, om]; u = [F1, F2]"""
    y, z, th, vy, vz, om = x
    F, M = u[0] + u[1], (u[1] - u[0]) * d
    return np.array([vy, vz, om,
                     -F/m*np.sin(th),
                      F/m*np.cos(th) - g,
                      M/I])

def rollout(u_func, T=2.0):
    x = np.zeros(6)
    for k in range(int(T/dt)):
        x = x + dt * f(x, u_func(x))       # Euler integration
    return x

print("Tắt máy   :", rollout(lambda x: np.array([0, 0]))[:3])     # rơi ~19.6m
print("Hover đúng:", rollout(lambda x: np.array([m*g/2, m*g/2]))[:3])
print("Lệch 1%   :", rollout(lambda x: np.array([m*g/2*1.01, m*g/2*0.99]))[:3])

5. Gắn với RL–UAV

Mô hình planar 6-state này là môi trường huấn luyện tối thiểu đáng tự viết trước khi dùng simulator lớn: bạn kiểm soát được từng giả định.
Underactuation giải thích vì sao action space “attitude + thrust” phổ biến trong RL agile flight: nó tôn trọng cấu trúc dynamics.
Domain randomization thực chất là random hóa \(m, I, d\), hệ số motor, … trong \(f\).

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

Nhầm input \(u\) với setpoint gửi xuống firmware: đó là hai tầng khác nhau.
Dùng mô hình tuyến tính quanh hover để giải thích manoeuvre cực mạnh (\(\theta\) lớn) — ngoài miền hợp lệ của 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 được sang máy thật.

7. Checklist

Viết được dynamics quadrotor phẳng từ trí nhớ (3 phương trình)?
Vì sao drone phải nghiêng mới dịch ngang được? Underactuated nghĩa là gì?
Tuyến tính hóa quanh hover cho ra hệ nào? Khi nào xấp xỉ này gãy?
(gợi ý: bay aggressive, \(\theta\) lớn)

Lecture 3 — Feedback Control

0. Mục tiêu học

Phân biệt open-loop và feedback; giải thích vì sao P thuần dao động trên hệ có quán tính; nêu steady-state error.

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

Vì sao điều khiển vòng hở (open-loop) thất bại, và feedback sửa điều đó thế nào?

2. Trực giác

Demo Lec 2 đã cho thấy: hover open-loop chết vì lệch 1%. Feedback là thuốc chữa.

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

3.1 Luật P (proportional)

Sai số \(e = x_{des} - x\). Luật đơn giản nhất:
\[ u = k_p\, e \]
Lệch nhiều kéo mạnh, lệch ít kéo nhẹ.

3.2 Vấn đề của P thuần trên hệ quán tính

 vị trí
   │      ╭─╮     ╭─╮     ╭─╮
 p*┤─────╱───╲───╱───╲───╱───╲──────  ← P thuần: dao động không tắt
   │    ╱     ╲_╱     ╲_╱     ╲
   │   ╱
   └──┴────────────────────────────► t

3.3 Sơ đồ khối chuẩn (đọc được sơ đồ này là đọc được sách control)

  x_des ──►(+)──── e ────►┌────────────┐── u ──►┌───────────┐──┬──► x
            ▲ –           │ CONTROLLER │        │  PLANT f  │  │
            │             └────────────┘        └───────────┘  │
            └───────────────────── đo lường (sensor) ◄─────────┘

“Plant” = hệ bị điều khiển (drone). Vòng tròn (+/–) = tính sai số.

4. Demo — open-loop vs P-control dưới nhiễu gió

import numpy as np
dt, T = 0.01, 8.0
def sim(controller, wind=0.3):
    p, v, log = 0.0, 0.0, []
    for k in range(int(T/dt)):
        u = controller(p, v)
        a = u + wind                # gió = nhiễu cộng vào gia tốc
        v += a*dt; p += v*dt; log.append(p)
    return np.array(log)

target = 1.0
openloop = sim(lambda p, v: 0.0)                     # không phản hồi
pctrl    = sim(lambda p, v: 4.0*(target - p))        # P thuần
print("open-loop cuối:", openloop[-1])   # trôi vô hạn theo gió
print("P-control cuối:", pctrl[-1])      # dao động quanh ~1.075 (lệch tĩnh + lắc)

Quan sát: P thuần (i) dao động, (ii) dừng lệch khỏi đích dưới gió hằng
(steady-state error) — hai khuyết tật mà Lec 4 (D) và khâu I (tích phân) xử lý.

5. Gắn với RL–UAV

Policy RL học được về bản chất cũng là một luật feedback \(u = \pi(x)\) — chỉ khác là phi tuyến và học từ dữ liệu. Hiểu P/PD giúp đọc được policy đã học “đang làm gì” (nhiều policy hội tụ về thứ rất giống PD quanh hover).
Reward shaping kiểu \(-\|e\|^2\) chính là nói với agent: “hãy trở thành một feedback controller tốt”.

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

Kết luận “P đủ dùng” chỉ vì nhìn một mô phỏng không có gió / không có sai model.
Phạt reward kiểu \(-\|e\|^2\) nhưng không phạt đạo hàm / độ giật — tái tạo đúng bệnh dao động của P thuần.

7. Checklist

Vì sao open-loop dễ thất bại khi mô hình lệch, còn feedback thường sống sót hơn?
Vì sao P thuần dao động trên hệ có quán tính? (viết được phương trình bậc hai)
Steady-state error dưới nhiễu hằng — vì sao P không khử được?

Lecture 4 — Stability và PD Control

0. Mục tiêu học

Phát biểu ổn định Lyapunov / tiệm cận; dùng eigenvalue để kiểm tra; chỉnh PD qua \(\zeta\); biết vai trò khâu I.

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

“Ổn định” định nghĩa chính xác là gì, kiểm tra bằng gì, và thêm khâu D giúp gì?

2. Trực giác

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

3.1 Định nghĩa

3.2 Tiêu chuẩn eigenvalue — công cụ số 1

3.3 PD trên double integrator

  ζ < 1  (underdamped)   :  vọt lố rồi lắc tắt dần      ╱╲╱‾‾
  ζ = 1  (critical)      :  về nhanh nhất không vọt lố  ╱‾‾‾
  ζ > 1  (overdamped)    :  về từ từ, chậm              ╱⎯⎯⎯ (bò lên)

Tuning thực dụng: chọn \(k_p\) theo tốc độ đáp ứng muốn có, rồi
\(k_d \approx 2\sqrt{k_p}\) (critical damping), tinh chỉnh quanh đó.

3.4 Vì sao có cả PID

4. Demo — quét \(k_d\) để thấy 3 chế độ damping

import numpy as np
dt, T, kp = 0.01, 6.0, 9.0        # ω_n = 3 rad/s
def pd(kd):
    p, v, log = 0.0, 0.0, []
    for _ in range(int(T/dt)):
        u = kp*(1.0 - p) - kd*v
        v += u*dt; p += v*dt; log.append(p)
    return np.array(log)

for kd, name in [(1.5, "under"), (6.0, "critical"), (14.0, "over")]:
    tr = pd(kd)
    print(f"{name:9s} kd={kd:4.1f}  overshoot={tr.max()-1:+.3f}  p(T)={tr[-1]:.3f}")

5. Gắn với RL–UAV

Eigenvalue là ngôn ngữ để nói “policy này có làm hệ kín ổn định không” — nhiều hướng stability-certified RL xoay quanh câu hỏi này (Lyapunov + học).
Trong reward, phạt \(\|v\|\) hay tốc độ đổi action chính là cách “cài \(k_d\)” vào policy: thiếu nó, policy thường giật và dao động — cùng bệnh với P thuần.
Nhiều hệ thực tế giữ PID/LQR attitude bên trong, policy chỉ xuất setpoint: ổn định được “bảo hành” bởi vòng trong, RL lo tầng nhiệm vụ.

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

Tăng \(k_p\) mãi để “bám chặt hơn” mà quên rằng nhiễu đo sẽ bị khuếch đại và motor dễ bão hòa.
Nhầm ổn định của mô hình tuyến tính hóa với ổn định toàn cục của hệ phi tuyến.

7. Checklist

Ba kiểu hành vi ổn định (tiệm cận / biên / bất ổn), ví dụ viên bi tương ứng?
Phát biểu tiêu chuẩn eigenvalue; phần thực và phần ảo cho biết gì?
\(\zeta\) là gì? Muốn không vọt lố mà về nhanh, chọn \(\zeta\) khoảng bao nhiêu?
Khâu I chữa gì và gây rủi ro gì (windup)?

Lecture 5 — Linear Quadratic Regulator (LQR)

0. Mục tiêu học

Phát biểu bài toán LQR; đọc được vai trò \(Q,R\); liên hệ \(P\) với value function và \(-Kx\) với policy tối ưu; nêu giới hạn của LQR.

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

Thay vì mò \(k_p, k_d\) bằng tay cho hệ nhiều chiều, có cách nào tính gain tối ưu
một phát từ mô hình không?

2. Trực giác

rồi toán học trả về ma trận gain \(K\) tối ưu. Giống thuê chuyên gia tuning:
bạn chỉ nêu khẩu vị, họ tính chi tiết.

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

3.1 Bài toán

3.2 Nghiệm

3.3 Trade-off Q vs R (trực giác tuning)

   Q lớn, R nhỏ  →  gain lớn  →  bám sát, phản ứng dữ, tốn năng lượng, dễ chạm
                                  giới hạn motor, khuếch đại nhiễu đo
   Q nhỏ, R lớn  →  gain nhỏ  →  êm, tiết kiệm, nhưng lười và bám kém

Thực dụng: bắt đầu \(Q = \mathrm{diag}(1/x_{i,max}^2)\), \(R = \mathrm{diag}(1/u_{j,max}^2)\)
(quy tắc Bryson) rồi chỉnh.

3.4 Giới hạn của LQR

Chỉ tối ưu quanh điểm linearize — bay xa hover (lộn vòng) là hết hiệu lực.
Không xử lý ràng buộc cứng (giới hạn thrust) — MPC sinh ra để vá chỗ này.
Cần model; model sai nhiều thì “tối ưu trên giấy”.

Lab 1 của khóa: nạp gain LQR vào firmware Crazyflie để hover — sinh viên thấy
ngay reality gap: gain tối ưu trong mô hình vẫn phải chỉnh khi bay thật.

4. Demo — LQR cho quadrotor phẳng (dùng scipy)

import numpy as np
from scipy.linalg import solve_continuous_are

m, I, g, d = 0.03, 1.4e-5, 9.81, 0.046
# state x = [y, z, th, vy, vz, om], input u = [ΔF, M] quanh hover
A = np.zeros((6, 6)); A[0, 3] = A[1, 4] = A[2, 5] = 1.0; A[3, 2] = -g
B = np.zeros((6, 2)); B[4, 0] = 1/m; B[5, 1] = 1/I

Q = np.diag([10, 10, 1, 1, 1, 0.1])
R = np.diag([1.0, 0.1])
P = solve_continuous_are(A, B, Q, R)
K = np.linalg.solve(R, B.T @ P)
print("Gain K =\n", np.round(K, 3))
print("Eig hệ kín:", np.round(np.linalg.eigvals(A - B @ K), 2))  # đều Re<0

# thử: xuất phát lệch 1m ngang, 0.5m thấp
dt, x = 0.002, np.array([1.0, -0.5, 0, 0, 0, 0.0])
for k in range(int(4/dt)):
    u = -K @ x
    xdot = A @ x + B @ u
    x = x + dt * xdot
print("Sau 4s:", np.round(x, 4))   # → về gần 0 hết

5. Gắn với RL–UAV

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

Coi LQR là “đã giải xong điều khiển UAV” — quên miền tuyến tính hóa và ràng buộc thrust.
Chọn \(Q,R\) tùy hứng không gắn đơn vị / giới hạn vật lý (bỏ qua quy tắc Bryson).
Không đặt LQR làm baseline khi viết paper RL hover/tracking.

7. Checklist

Viết được hàm cost LQR và nêu vai trò \(Q, R\)?
\(u = -Kx\): vì sao nói LQR là “PD nhiều chiều được tối ưu hóa”?
Chỉ ra tương ứng LQR ↔ RL (value, policy, Bellman)?
Ba giới hạn của LQR và hướng vá tương ứng (gain scheduling, MPC, robust / residual RL)?

Tổng kết Module 1

Dynamics cho bạn \(f\). Feedback cho bạn cách sống sót khi \(f\) không hoàn hảo.
PD tạo trực giác chỉnh tham số. LQR cho nghiệm tối ưu tuyến tính — và là “trường hợp có công thức đóng” gần nhất với value/policy mà bạn dùng trong RL.

Bài tiếp: 02_Motion_Planning.md — robot đã đứng vững, giờ học cách tìm đường.