Chuyển đến nội dung chính
Trang chủ • Dự án • DỰ ÁN ROBOT

#108 Mô phỏng drone Quadrotor trên MATLAB – mô hình động lực học 6 bậc tự do và điều khiển bay theo quỹ đạo

Mô phỏng drone quadrotor 4 động cơ trên MATLAB: mô hình động lực học 6 bậc tự do (Euler–Lagrange, ma trận quay ZYX, lực cản, gió), bộ điều khiển tầng vị trí – tư thế trên SO(3), mixer có bão hòa động cơ, bay theo quỹ đạo dài và vẽ quỹ đạo 3D, bám vị trí, góc Euler.

DỰ ÁN ROBOT ~9 phút đọc Cập nhật 17/11/2026

Nội dung bài đăng

  • Video demo
  • Nội dung dự án
  • Mô hình toán học
  • Bộ điều khiển
  • Kết quả mô phỏng
  • Mã MATLAB (các phần chính, minh họa)

Bài đăng kỹ thuật

Triển khai và phân tích hệ thống

#108 Mô phỏng drone Quadrotor trên MATLAB – mô hình động lực học 6 bậc tự do và điều khiển bay theo quỹ đạo

Mô phỏng drone quadrotor 4 động cơ trên MATLAB: mô hình động lực học 6 bậc tự do (Euler–Lagrange, ma trận quay ZYX, lực cản, gió), bộ điều khiển tầng vị trí – tư thế trên SO(3), mixer có bão hòa động cơ, bay theo quỹ đạo dài và vẽ quỹ đạo 3D, bám vị trí, góc Euler.

Dự án xây dựng mô hình động lực học 6 bậc tự do của drone quadrotor 4 động cơ trên MATLAB và mô phỏng drone bay theo một quỹ đạo dài trong không gian. Mô phỏng trước khi bay thật giúp kiểm tra phương trình, chỉnh bộ điều khiển và giảm rủi ro rơi hỏng drone khi thử nghiệm.

1. Video demo

Xem video trên TikTok TechVis Automation: Mô phỏng drone Quadrotor trên MATLAB – mô hình 6 bậc tự do (112 giây).

Kết quả trong video: drone bay hết quỹ đạo dài (t = 200 s), các cửa sổ quỹ đạo 3D, bám vị trí x/y/z, mô-men thân và góc Euler theo quy ước ZYX.
Hình 1. Kết quả trong video: drone bay hết quỹ đạo dài (t = 200 s), các cửa sổ quỹ đạo 3D, bám vị trí x/y/z, mô-men thân và góc Euler theo quy ước ZYX.
Mã nguồn trong video: hàm quadrotor_controller điều khiển tầng vị trí – tư thế, giới hạn góc nghiêng, xây dựng ma trận tư thế mong muốn và sai lệch trên SO(3).
Hình 2. Mã nguồn trong video: hàm quadrotor_controller điều khiển tầng vị trí – tư thế, giới hạn góc nghiêng, xây dựng ma trận tư thế mong muốn và sai lệch trên SO(3).

2. Nội dung dự án

  • Chuyển động tịnh tiến theo x, y, z và quay theo roll φ, pitch θ, yaw ψ (6 bậc tự do).
  • Ma trận quay theo thứ tự Z–Y–X, quan hệ giữa vận tốc góc thân và đạo hàm góc Euler.
  • Thiết lập phương trình bằng phương pháp Euler–Lagrange: động năng tịnh tiến, động năng quay, thế năng trọng trường.
  • Lực nâng của 4 động cơ, mô-men roll/pitch/yaw, lực cản khi chuyển động, nhiễu gió.
  • Bộ điều khiển tầng vị trí – tư thế, giới hạn góc nghiêng và tốc độ động cơ.
  • Mô phỏng quỹ đạo bay dài, vẽ quỹ đạo 3D động, đồ thị bám vị trí, mô-men và góc Euler.

3. Mô hình toán học

Cấu hình động cơ và ma trận phân phối lực/mô-men.
Hình 3. Cấu hình động cơ và ma trận phân phối lực/mô-men.

Tịnh tiến (hệ quán tính). m·p̈ = R(η)·[0; 0; T] − m·g·e3 − D·ṗ + F_gió, trong đó R(η) = Rz(ψ)·Ry(θ)·Rx(φ).

Quay (hệ thân). I·ω̇ = τ − ω × (I·ω), với ω = W(η)·η̇. Ma trận W suy biến khi θ = ±90° – nhược điểm của góc Euler; vì vậy bộ điều khiển tính sai lệch tư thế trực tiếp trên ma trận quay (SO(3)) thay vì trừ góc Euler.

Euler–Lagrange. Với L = ½m‖ṗ‖² + ½ωᵀIω − m·g·z, phương trình Lagrange cho các tọa độ (x, y, z) và (φ, θ, ψ) cho ra đúng hai hệ trên (phần quay viết theo η có thêm ma trận Coriolis C(η, η̇)). Hai cách viết tương đương; dạng Newton–Euler gọn hơn khi lập trình.

Đại lượngGiá trị trong bản tái hiệnGhi chú
Khối lượng m1,0 kg
Quán tính Ixx, Iyy, Izz0,0123; 0,0123; 0,0224 kg·m²
Cánh tay đòn l0,25 m
kF, kM1,2·10⁻⁵ N/(rad/s)²; 2,0·10⁻⁷ N·m/(rad/s)²treo cân bằng ≈ 4.317 vòng/phút mỗi động cơ
Giới hạn nghiêng30°tránh vùng suy biến, giữ đủ lực nâng

4. Bộ điều khiển

Sơ đồ điều khiển tầng.
Hình 4. Sơ đồ điều khiển tầng.
  • Vòng vị trí: a_cmd = a_ref + Kp(p_ref − p) + Kd(v_ref − v); lực mong muốn F = m(a_cmd + g·e3) + D·v, kẹp thành phần ngang theo góc nghiêng tối đa.
  • Tư thế mong muốn: trục b3 của drone phải trùng hướng F; trục b1 lấy theo góc yaw mong muốn → Rd = [b1 b2 b3].
  • Vòng tư thế: sai lệch eR = ½·vee(RdᵀR − RᵀRd); τ = −K_R·eR − K_ω·ω + ω × Iω.
  • Lực nâng: T = F·(R·e3) – chỉ lấy phần lực dọc trục thân drone.
  • Mixer: giải 4 phương trình tìm ω1²…ω4², kẹp trong giới hạn động cơ.

5. Kết quả mô phỏng

Bộ ảnh dưới đây do TechVis tái hiện bằng Python với cùng cấu trúc mô hình và bộ điều khiển (tham số ví dụ ở mục 3) để giải thích kết quả; đồ thị gốc trong MATLAB ở hình 1.

Quỹ đạo 3D từ bản mô phỏng tái hiện (Python, cùng cấu trúc mô hình và bộ điều khiển).
Hình 5. Quỹ đạo 3D từ bản mô phỏng tái hiện (Python, cùng cấu trúc mô hình và bộ điều khiển).
Vị trí, sai số và góc Euler theo thời gian của bản mô phỏng tái hiện.
Hình 6. Vị trí, sai số và góc Euler theo thời gian của bản mô phỏng tái hiện.

Drone cất cánh lên 1 m, bay hết quỹ đạo zig-zag rồi treo tại điểm cuối. Sai số vị trí trung bình (RMS) sau giai đoạn cất cánh khoảng 9 cm, góc nghiêng lớn nhất khoảng 2.1°. Sai số không về 0 vì có gió ngang mà bộ điều khiển PD không có khâu tích phân – thêm khâu I (hoặc ước lượng nhiễu) sẽ triệt tiêu sai số này. Ở mỗi khúc quay, góc yaw đổi 180° theo hướng bay.

6. Mã MATLAB (các phần chính, minh họa)

Các đoạn dưới đây thể hiện cấu trúc chương trình; hàm tạo quỹ đạo quadrotor_reference và phần vẽ đồ thị/animation có trong bộ tài liệu.

Tham số mô hình (đơn vị SI):

P.m = 1.0;  P.g = 9.81;
P.I = diag([0.0123 0.0123 0.0224]);        % mô-men quán tính (kg·m^2)
P.l = 0.25;                                 % cánh tay đòn (m)
P.kF = 1.2e-5;  P.kM = 2.0e-7;              % hệ số lực nâng và mô-men cản của cánh quạt
P.drag = diag([0.25 0.25 0.30]);            % lực cản ~ vận tốc
P.wMax = 1000;                              % giới hạn tốc độ động cơ (rad/s)
P.maxTilt = deg2rad(30);
P.KpPos = diag([2.0 2.0 4.0]);  P.KdPos = diag([2.6 2.6 3.5]);
P.KR = diag([0.9 0.9 0.35]);    P.Kw = diag([0.16 0.16 0.10]);

Ma trận quay ZYX và ma trận W (ω = W·η̇):

function R = quadrotor_rotation(eta)
    ph = eta(1); th = eta(2); ps = eta(3);
    Rz = [cos(ps) -sin(ps) 0; sin(ps) cos(ps) 0; 0 0 1];
    Ry = [cos(th) 0 sin(th); 0 1 0; -sin(th) 0 cos(th)];
    Rx = [1 0 0; 0 cos(ph) -sin(ph); 0 sin(ph) cos(ph)];
    R = Rz * Ry * Rx;
end

function W = quadrotor_W(eta)
    ph = eta(1); th = eta(2);
    W = [1 0 -sin(th);
         0 cos(ph) sin(ph)*cos(th);
         0 -sin(ph) cos(ph)*cos(th)];          % suy biến khi cos(theta) = 0
end

Mô hình động lực học (vế phải phương trình vi phân), trạng thái X = [p; v; eta; omega]:

function dX = quadrotor_dynamics(X, T, tau, P, wind)
    v = X(4:6);  eta = X(7:9);  w = X(10:12);
    R = quadrotor_rotation(eta);
    forceWorld = R*[0; 0; T] - P.drag*v + wind;
    acc = forceWorld/P.m - [0; 0; P.g];
    wDot = P.I \ (tau - cross(w, P.I*w));      % phương trình Euler cho vật rắn
    etaDot = quadrotor_W(eta) \ w;
    dX = [v; acc; etaDot; wDot];
end

Bộ điều khiển tầng: vị trí → lực mong muốn → tư thế mong muốn → mô-men:

function [T, tau] = quadrotor_controller(X, ref, P)
    p = X(1:3); v = X(4:6); eta = X(7:9); w = X(10:12);
    aCmd = ref.acc + P.KpPos*(ref.pos - p) + P.KdPos*(ref.vel - v);
    F = P.m*(aCmd + [0; 0; P.g]) + P.drag*v;
    F(3) = max(F(3), 0.1*P.m*P.g);
    fMax = tan(P.maxTilt)*F(3);                 % giới hạn góc nghiêng
    if norm(F(1:2)) > fMax
        F(1:2) = F(1:2)*fMax/norm(F(1:2));
    end
    R = quadrotor_rotation(eta);
    b3 = F/norm(F);
    b1h = [cos(ref.yaw); sin(ref.yaw); 0];
    b2 = cross(b3, b1h); b2 = b2/norm(b2);
    b1 = cross(b2, b3);
    Rd = [b1 b2 b3];
    E = 0.5*(Rd'*R - R'*Rd);
    eR = [E(3,2); E(1,3); E(2,1)];              % ánh xạ vee
    T = F' * R(:,3);
    tau = -P.KR*eR - P.Kw*w + cross(w, P.I*w);
end

Mixer cấu hình "+" có bão hòa động cơ, và vòng lặp mô phỏng:

function [omega, T, tau] = quadrotor_mixer(T, tau, P)
    k = P.kF; l = P.l; m = P.kM;
    M = [ k    k    k    k;
          0    l*k  0   -l*k;
         -l*k  0    l*k  0;
         -m    m   -m    m ];
    w2 = M \ [T; tau];
    w2 = min(max(w2, 0), P.wMax^2);
    omega = sqrt(w2);
    u = M*w2;  T = u(1);  tau = u(2:4);
end

dt = 0.002;  tEnd = 200;  N = round(tEnd/dt);
X = zeros(12,1);  wind = [0.15; -0.1; 0];
log = zeros(N, 15);
for i = 1:N
    t = (i-1)*dt;
    ref = quadrotor_reference(t);               % quỹ đạo zig-zag + góc yaw theo hướng bay
    [Tc, tauc] = quadrotor_controller(X, ref, P);
    [omega, T, tau] = quadrotor_mixer(Tc, tauc, P);
    X = X + dt*quadrotor_dynamics(X, T, tau, P, wind);
    log(i,:) = [t, X(1:3)', ref.pos', X(7:9)', omega(1:2)'];
end

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

  • Drone lật hoặc phân kỳ ngay: sai dấu trong mixer hoặc chiều quay động cơ; kiểm tra từng kênh roll/pitch/yaw riêng.
  • NaN khi bay gắt: θ tiến tới ±90° làm W suy biến – giới hạn góc nghiêng, hoặc chuyển sang quaternion.
  • Kết quả phụ thuộc bước thời gian: Euler hiện với dt lớn dễ mất ổn định; dùng dt ≤ 2 ms hoặc ode45/RK4.
  • Bám quỹ đạo kém ở khúc quay: thiếu thành phần gia tốc tham chiếu a_ref (feed-forward) hoặc vòng tư thế quá chậm so với vòng vị trí.

8. Hướng mở rộng

  • Thêm khâu tích phân hoặc bộ quan sát nhiễu để chống gió.
  • Thay PD bằng LQR, sliding mode hoặc MPC; so sánh đáp ứng.
  • Mô hình động cơ (độ trễ, giới hạn gia tốc cánh quạt) và nhiễu cảm biến IMU.
  • Chuyển bộ điều khiển xuống vi điều khiển (STM32) và thử trên drone thật.

9. Bộ tài liệu khi mua dự án (149.000đ)

  • Mã nguồn MATLAB đầy đủ: mô hình 6 bậc tự do, bộ điều khiển, mixer, tạo quỹ đạo, chương trình mô phỏng và vẽ đồ thị/animation 3D như trong video.
  • Tài liệu giải thích phương trình động lực học và cách chỉnh tham số.

Tài liệu tham khảo

Xem liên kết ngoài