Mô phỏng Quadcopter Drone

Phạm Minh Thắng, Mai Thanh Tùng

VNU - UET, VietNam
21021638@vnu.edu.vn, 21021645@vnu.edu.vn

I. GIỚI THIỆU C. Cấu trúc của bài báo

Tiếp theo đây sẽ là mục II mô tả cách tiếp cận mô
Drone là một loại phương tiện bay không người lái có
hình Quadcopter Drone, các mô hình toán học dùng cho
thể được áp dụng vào nhiều ứng dụng khác nhau. Trên
mô phỏng. Mục III trình bày chi tiết các thành phần của
thực tế hiện nay đã có nhiều mẫu Drone được nghiên cứu,
mô hình. Mục IV là đánh giá kết quả mô phỏng dựa trên
thử nghiệm và sản xuất hàng loạt. Tuy nhiên để có thể
những số liệu đầu ra.
sản xuất một mẫu Drone đòi hỏi chi phí lớn. Vì thế việc
ứng dụng mô phỏng vào việc thiết kế và áp dụng những II. CÁC BÀI TOÁN LIÊN QUAN
công nghệ mới vào các thiết bị bay là một giải pháp tiết
kiệm chi phí, giảm thiểu rủi ro và giúp các nhà phát triển
dễ dàng tối ưu sản phẩm cho phù hợp với mục đích. Trong
báo cáo này sẽ trình bày mô hình hệ thống Quadcopter
Drone với chức năng giao hàng theo lộ trình đặt trước
trong phần mềm Matlab - Simulink.

A. Tổng quan Drone

Những ý tưởng phát triển Drone đã xuất hiện từ những
năm 1900 ở Ohio, Charles Kettering đã đề xuất ý tưởng
về một thiết bị bay trang bị một cơ cấu hẹn giờ, khi thiết Fig. 1: Mô hình khung thân Quadcopter (Modelling and
bị đã bay được một khoảng thời gian nhất định, cánh sẽ control of quadcopter - Teppo Luukkonen)
tự tách ra và thiết bị bay sẽ lao xuống mục tiêu. Sau này [1]
quân đội Anh đã phát triển UAV điều khiển bằng sóng
Quadcopter là một thiết bị bay không người lái sử dụng
vô tuyến, tiếp sau đó Mỹ và Liên Xô cũ cũng phát triển
hệ thống gồm có khung thân và bốn động cơ với một cặp
những mẫu UAV phục vụ cho mục đích quân sự và cũng
động cơ quay thuận chiều kim đồng hồ và một cặp động
là nền móng cho sự phát triển của Drone hiện đại. [4]
cơ quay người chiều kim đồng hồ được đặt ở vị trí đối
Hiện nay ngoài những ứng dụng trong quân sự, Drone
xứng nhau qua các trục [4]. Quadcopter di chuyển bằng
còn được sử dụng nhiều vào trong đời sống hàng ngày,
cách thay đổi tốc độ quay của các động cơ từ đó có thể di
tiêu biểu là những mẫu flycam của DJI với khả năng ghi
chuyển linh hoạt trong gian với các động tác cơ bản như
hình trên không, những mẫu Drone của Amazon dùng cho
bay thẳng đứng, xoay tại chỗ, tiến, lùi...
mục đích vận tải hàng hóa... Với ưu điểm linh hoạt, đơn
Bài toán đặt ra là tìm cách điều khiển tốc độ quay của
giản, khả năng tùy biến cho các nhiệm vụ đa dạng, Drone
các động cơ để Quadcopter có thể di chuyển theo quỹ đạo
đang ngày trở thành một phương án mới trong cách tiếp
mong muốn thông qua các bước sau
cận cách giải quyết các vấn đề hàng ngày của con người.
A. Xây dựng mô hình toán học
B. Sự cần thiết của mô phỏng Sử dụng các mô hình toán học có sẵn
Với sự phát triển nhanh chóng hiện nay, việc cho ra đời • Phương trình Newton - Euler [1] Vận tốc góc của

các mẫu Drone phù hợp với nhu cầu của người dùng một
cách nhanh chóng là yêu cầu bắt buộc. Để đáp ứng yêu
cầu trên, mỗi nhà sản xuất đều cần phải nghiên cứu và
cho ra đời những sản phẩm mới một cách nhanh chóng và
tiết kiệm nhưng vẫn phải đảm bảo độ tin cậy, chính xác
và đảm bảo an toàn. Một trong những giải pháp cho vấn
đề này là mô phỏng những thiết kế mới trong điều kiện
giả lập, là một giải pháp hiệu quả, tiết kiệm thời gian, chi
phí, an toàn để đạt được những yêu cầu từ người dùng.
rotor ωi , tạo ra lực fi theo hướng trục của rotor. Vận
tốc góc và gia tốc của rotor cũng tạo ra mô-men xoắn
τMi xung quanh trục rotor. Trong đó hằng số nâng
là k, hằng số cản là b và mô men quán tính của cánh
quạt là IM . [1]
fi = kωi2 , τMi = bωi2 + IM ω̇i (1)
Lực kết hợp của các rotor tạo ra lực đẩy τ theo
hướng trục z của thân máy bay. Mô-men xoắn τB
 bao gồm các mô-men xoắn τϕ , τθ và τψ theo hướng
của các góc khung thân máy bay tương ứng.
 
∑4 ∑4 0
T = fi = k ωi2 , T B =  0  (2)
i=1 i=1 T
   
τϕ lk(−ω22 + ω42 )
τB =  τθ  = lk(−ω
∑4 1
2
+ ω32 ) (3)
τψ i=1 τ M i

Trong đó l là khoảng cách giữa rotor và trọng tâm

của quadcopter. Do đó, chuyển động lăn (roll) được
thực hiện bằng cách giảm tốc độ rotor thứ 2 và tăng
tốc độ rotor thứ 4. Tương tự, chuyển động nghiêng
(pitch) được thực hiện bằng cách giảm tốc độ rotor
thứ 1 và tăng tốc độ rotor thứ 3. Chuyển động quay
(yaw) được thực hiện bằng cách tăng tốc độ góc của
hai rotor đối diện và giảm tốc độ của hai rotor còn Fig. 2: Sơ đồ khối (OSTOJIĆ, Gordana, et al. Design,
lại. control and application of quadcopter)
Với Sx = Sin(x), Cx = Cos(x), TB là lực đẩy của [2]
động cơ, trọng lực g, khối lượng Drone m ta có phương
trình trạng thái:
mξ¨ = G + GTB (4)
     
ẍ 0 C ψ Sθ C ϕ + Sψ Sϕ
ÿ  = −g 0 + T  Sψ Sθ Cϕ − Cψ Sθ  (5)
m
z̈ 1 Cθ Cϕ
B. Thuật toán điều khiển Fig. 3: Hệ thống Quadcopter giao hàng
Thuật toán điều khiển PID (Proportional-Integral- [3]
Derivative) đã được xem xét và triển khai trong mô hình
này để có thể điều khiển chiếc Quadcopter [2]. Thuật toán
điều khiển PID là một thuật toán điều khiển tuyến tính B. Sơ đồ mô hình khối Maneuver Controller
được sử dụng rộng rãi trong ngành robot và ngành tự
động hóa. Thuật toán điều khiển PID thường được sử
dụng vì:
• Kiến trúc thuật toán cơ bản
• Hiệu năng cao
• Có thể dễ dàng điều chỉnh
Bộ điều khiển PID hoạt động bằng cách liên tục tính
toán giá trị sai số giữa giá trị đo được với giá trị mong Fig. 4: Khối Maneuver Controller
muốn. Bộ điều khiển sau đó điều chỉnh các giá trị điều [3]
khiển động cơ để giá trị sai số là tối thiểu
Sơ đồ khối thuật toán điều khiển PID :
1) Khối Position Control: 5 Bao gồm các thành phần:
III. HỆ THỐNG MÔ PHỎNG QUADCOPTER GIAO • Body Frame: là hệ quy chiếu đặt trên thân Drone có
HÀNG vai trò xác định vị trí tương đối của vật thể hoặc quỹ
A. Sơ đồ mô hình tổng thể của bài mô phỏng: đạo so với Drone
Trong đó có: • World Frame: là hệ quy chiếu không gian có vai trò
• Khối Reference để thiết lập sẵn quỹ đạo di chuyển xác định vị trí tương đối của Drone trong không gian
của mô phỏng. ba chiều
• Maneuver Controller để điều khiên di chuyển của • Khối Calculate RBI: là ma trận chuyển từ hệ quy
Quadcopter trong không gian mô phỏng. chiếu thân sang hệ quy chiếu không gian
• Khối Quadcopter là mô hình Drone được mô phỏng Vị trí của Drone sẽ được tính toán thông qua hai hệ quy
• Khối Environment tiếp nhận thông tin môi trường từ chiếu thân và hệ quy chiếu không gian để xác định chính
https://openweathermap.org/api và chuyển thông số xác tọa độ thân của Drone. Dựa vào quỹ đạo bay được
vào cho Quadcopter. thiết lập từ trước, khối Position Control sẽ tính toán độ

2
sai lệch giữa vị trí hiện tại và vị trí chuẩn, từ đó đưa ra 3) Khối Motor Mixer: tạo ra các tín hiệu điều khiển
tín hiệu điều khiển cho khối Altitude and YPR Control đến từng motors.

Fig. 5: Khối Position Control

[3]

2) Khối Altitude and YPR control: Khối nhận đầu vào

là các tín hiệu độ lệch góc đã được chuẩn hóa từ khối
trước: (Pitch Cmd,Roll Cmd,Yaw Cmd).
Sau đó khối sẽ tính toán sai số giữa các đầu vào và thông
tin từ Quadcopter (m.Chassis.pitch,roll,yaw) ,các sai số
này sẽ được tinh chỉnh thông qua các bộ PID controllers
và xuất ra tín hiệu điều khiển các trục xoay của motors. Fig. 7: Khối Motor Mixer
[3]

C. Sơ đồ mô hình tổng thể của khối Quadcopter

Fig. 6: Khối Altitude and YPR control

[3]
Ngoài ra khối còn nhận vào thông tin vị trí được đặt sẵn
(Ref.pos) và vị trí của vỏ thân Quadcopter (m.Chassis.pz)
và so sánh sai số giữa hai đầu vào này. Sai số này được đưa
vào bộ PID controllers để xuất ra tín hiệu điều khiển lực
phóng.Tín hiệu lực phóng này sẽ được chuyển thành tín
hiệu điều khiển mô men xoắn của các motor từ đó điều
khiển được độ cao của Quadcopter (Altitude Control).
Tùy vào trạng thái tải (m.Load.status) mà các motor sẽ
có khoảng nghỉ để có thể trả hàng được vận chuyển một
cách ổn định.
Fig. 8: Mô hình tổng thể của khối Quadcopter
[3]
Trong đó:
• Mô hình khung thân Quadcopter với:
- 4 cổng P1-P4 để kết nối với các cánh quạt
propeller.
- Cổng G để kết nối với gói hàng được vận chuyển
(Khối Load).
- Cổng Ref để tiếp nhận ảnh hưởng từ môi trường
được cung cấp từ Khối Wind and Drag

3
 • Mô hình Ground là khu vực để chạy mô phỏng
• Các khối Camera và 6 DOF (Degree of Freedom) để
quan sát mô phỏng.
• Khối mô hình Electrical là khối quản lý các chức năng
vận hành motor của Quadcopter.
Sau đây ta sẽ đi vào chi tiết từng khối trong mô hình
tổng thể này:
Fig. 9: Khối Wind and Drag
[3]
1) Khối Wind and Drag: Thay đổi trọng tâm của
Quadcopter khi bị ảnh hưởng bởi môi trường ở 2 trạng
thái khác nhau là : có tải và không tải. Khối nhận 3 đầu
vào chính:
- Hiệu số của tốc độ tại khung thân (Cha) với ảnh
hưởng môi trường (Env): [vxRel, vyRel, vzRel].
- Độ nghiêng của các trục: [nRoll, nPitch, nYaw].
- Trạng thái tải: Load.status.
Từ ba đầu vào này , ta tính toán được ngoại lực tác động
vào khung thân Quadcopter theo từng trường hợp trạng
thái tải ở Khối External Force.Ngoại lục này sẽ làm thay
đổi trọng tâm của khung thông qua khối Transform Center
of Pressure và đưa tín hiệu ra ngoài.
Fig. 10: Khối Load
[3]
2) Khối Load: Có vai trò điều khiển cơ chế chở hàng
của Quadcopter. Khối gồm 3 bộ phận chính đó là:
4
- Khối chức năng điều khiển logic thả gói hàng dựa
trên vị trị và tốc độ của Drone(Disengage Logic)
- Gói hàng được vận chuyển (Package)
- Các mối tiếp xúc giữa chân Quadcopter và Pack-
age.(Weld Joint)
Khi đạt ngưỡng vị trí và tốc độ,khối Disengage Logic gửi
tín hiệu thả gói hàng tới chân các mối tiếp xúc và chân
Quadcopter sẽ mở ra và nhả gói hàng xuống (G) (vị trí
mục tiêu gửi hàng đã định sẵn trên bản đồ). Đồng thời
khối Disengage Logic cũng gửi tín hiệu (m) để báo hiệu
bật/tắt động cơ khi thả hàng
Fig. 11: Khối Electrical
[3]
3) Khối Electrical: Khối Electrical có nhiệm vụ điều
khiển hoạt động của bốn động cơ dựa trên tín hiệu điều
khiển từ khối Maneuver Controller. Trong khối này có các
thành phần: khối control, motor, battery, cảm biến nhiệt
độ, cảm biến đo dòng điện.
Fig. 12: Khối Control
[3]
Tín hiệu từ khối Maneuver Controller và tín hiệu phản
hồi từ dộng cơ sẽ được truyền vào trong khối Control để
điều khiển động cơ. Tín hiệu phản hồi sẽ được xử lý thông
1
qua hàm truyền f spd.s+1 với hệ số f spd có giá trị là 0.001.
Tín hiệu phản hồi sau khi được xử lý sẽ được kết hợp với
tín hiệu cmd rồi được truyền vào trong khối PID, từ đó
trả về tín hiệu điều khiển mô men xoắn phù hợp cho dộng
cơ. Tín hiệu mô phỏng sẽ được hiển thị thông qua scope.
 - Khối Camera phục vụ cho việc quan sát mô phỏng.
- Khối World Frame, Ground là môi trường mô phỏng.
- Khối Gravity tạo trọng lực lên môi trường mô phỏng.
- Khối 6 DOF cho phép camera có thể di chuyển theo
6 chiều tự do .

Fig. 15: Các khối bổ trợ

[3]
Fig. 13: Khối Body
[3] IV. MÔ PHỎNG VÀ ĐÁNH GIÁ
Các tham số mô phỏng và điều kiện mô phỏng được
giới thiệu trong bảng Table I. Thực hiện mô phỏng Drone
4) Khối Body: Khung thân của Drone được cấu thành
giao hàng với gói hàng có kích thước và khối lượng tương
từ bốn arm, càng đáp, plate, máy tính. Chúng được kết
đồng với các gói hàng y tế, nhu yếu phẩm cơ bản cần
nối với nhau thông qua block rigid transform. Các yếu tố
cung cấp cho nạn nhân một cách nhanh chóng. Trong các
tác động bên ngoài như trọng lực, tương tác của Drone
bài mô phỏng có bao gồm các trường hợp thay đổi tốc độ
với mặt đất được đưa truyền vào thông qua cổng Ref và
của Quadcopter, bay trong môi trường có gió và không
G
có gió. Các đánh giá được đưa ra dựa trên kết quả tương
đối giữa lộ trình bay đặt sẵn và quỹ đạo bay, thời gian
bay với mỗi điều kiện khác nhau.
A. Mô phỏng Quadcopter giao hàng tại mặt đất
Trong trường hợp mô phỏng Quadcopter giao hàng tại
mặt đất, Quadcopter sẽ bay qua các mốc theo lộ trình
đặt sẵn. Trong trường hợp này điểm mốc sẽ được đặt
trên mặt đất, khi đến điểm mốc cuối cùng sẽ tự động thả
hàng, sau đó bay về theo lộ trình cũ. Cài đặt lộ trình của
Quadcopter như sau:
   
x −2.00 −2.00 0.00 2.00 5.00
y  = −2.00 −2.00 0.00 0.00 0.00 (6)
z 0.14 6.00 6.00 6.00 0.14
B. Mô phỏng Quadcopter giao hàng trên không
Quadcopter bay theo lộ trình đã đặt trước, thả hàng
Fig. 14: Khối Propeller tại vị trí mốc cuối cùng ở độ cao 6m so với mặt đất. Lộ
[3] trình của Quadcopter như sau:
 
−3.00 0.56 4.54 7.76 9.00 7.34 3.70 −0.01 2.26 5.00
−5.00 −4.47 −4.57 −2.39 1.52 5.30 6.59 6.87 9.57 8.00
5) Các khối Propeller: Khối Propeller mô phỏng một 0.15 6.00 6.00 6.00 6.00 6.00 6.00 6.00 6.00 6.00
(7)
động cơ gắn cánh quạt và mô phỏng lực đẩy cũng như
lực cản khi động cơ hoạt động. Đầu vào của khối là cổng C. Đánh giá kết quả mô phỏng
P nhận tín hiệu điều khiển từ khối Electrical. Tín hiệu Kết quả mô phỏng theo lộ trình (6), và (7) được thể
được xử lý thông qua các khối Thrust and Drag, External hiện trong các đồ thị Figure 19, Figure 20, Figure 21,
Force Thrust and Drag và truyền qua cổng m.w, m.thrust, Figure 23, Figure 24, Figure 25. Quadcopter có khả năng
m.drag lần lượt cho biết tốc độ vòng quay, lực đẩy, lực bay theo đường đi đặt trước, sai số ở mức < 10%. Tuy
cản mà cánh quạt tạo ra. nhiên dựa vào kết quả góc của các trục theo thời gian ở cả
6) Các khối bổ trợ: bốn trường hợp Figure 20, Figure 21, Figure 24, Figure 25

5
đều có sự sai lệch, và lớn nhất lên tới 77% trong khoảng
xấp xỉ 3 giây. Khi tăng tốc độ của Quadcopter lên 100%,
thời gian bay cũng được cải thiện 30% trong cả hai trường
hợp (6), và (7).

Fig. 16: Tốc độ Quadcopter trong trường hợp (6)

Fig. 17: Tốc độ Quadcopter trong trường hợp (7)

Từ kết quả mô phỏng cho thấy hệ thống Quadcopter

sử dụng bộ điều khiển PID có khả năng hoạt động trong
môi trường được cài đặt. Hệ thống đã bám sát lộ trình
được đăt trước, thả hàng tại điểm đích như yêu cầu, tuy
nhiên vẫn còn một số hạn chế như độ lệch góc Yaw, Pitch,
Roll lớn so với tham chiếu, có thể ảnh hướng đến sự vận
hành của Quadcopter trong thực tế với các hoàn cảnh mô
phỏng chưa thể hiện được
References
[1] T. Luukkonen. Modelling and control of quadcopter. Indepen-
dent research project in applied mathematics, Espoo, 22(22),
2011.
[2] G. Ostojić, S. Stankovski, B. Tejić, N. Đukić, and S. Tegeltija.
Design, control and application of quadcopter. International
Journal of Industrial Engineering and Management, 6(1):43,
2015.
[3] M. Steve. Quadcopter drone model in simscape. https://github.
com/mathworks/Quadcopter-Drone-Model-Simscape. Accessed:
2023-10-30.
[4] B. Vergouw, H. Nagel, G. Bondt, and B. Custers. Drone
technology: Types, payloads, applications, frequency spectrum
issues and future developments. The Future of Drone Use:
Opportunities and Threats from Ethical and Legal Perspectives,
pages 21–45, 2016.

6
 TABLE I: Tham số mô phỏng
Chiều dài trục x 12.5m
Chiều dài trục y 8.5m
Khối lương Drone 1.2726kg
Kích thước gói hàng 2744cm3
Khối lượng riêng của gói hàng 0.36443g/cm3
Bán kính cánh quạt 0.254m
Hệ số lực đẩy 0.1072
Hệ số lực cản 0.01
Mật độ không khí 1.245kg/m3
Nhiệt độ không khí 25◦ C
Tốc độ gió 0 m/s
Momen xoắn cực đại của động cơ 0.8N ∗ m
Công suất cực đại của động cơ 160W
Hiệu suất 0.88
Tốc độ hiệu quả 5000rpm
Mô men xoắn hiệu quả 0.05N ∗ m
Độ đầm của rotor 10− 7

Fig. 18: Quỹ đạo bay của Quadcopter theo lộ trình (6)

7
Fig. 19: Vị trí Quadcopter theo từng trục với lộ trình (6)

8
Fig. 20: YPR của Quadcopter theo lộ trình (6)

9
Fig. 21: YPR của Quadcopter trong môi trường có gió theo lộ trình (6)

10
Fig. 22: Quỹ đạo bay của Quadcopter theo lộ trình (7)

11
Fig. 23: Vị trí Quadcopter theo từng trục với lộ trình (7)

12
Fig. 24: YPR của Quadcopter theo lộ trình (7)

13
Fig. 25: YPR của Quadcopter trong môi trường có gió theo lộ trình (7)

14