Bài đăng kỹ thuật
Triển khai và phân tích hệ thống
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).


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

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ượng | Giá trị trong bản tái hiện | Ghi chú |
|---|---|---|
| Khối lượng m | 1,0 kg | |
| Quán tính Ixx, Iyy, Izz | 0,0123; 0,0123; 0,0224 kg·m² | |
| Cánh tay đòn l | 0,25 m | |
| kF, kM | 1,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êng | 30° | tránh vùng suy biến, giữ đủ lực nâng |
4. Bộ điều khiển

- 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.


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
endMô 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];
endBộ đ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);
endMixer 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ố.