ĐẠI HỌC QUỐC GIA TP. HỒ CHÍ MINH
TRƯỜNG ĐẠI HỌC BÁCH KHOA
--------

CAO HỒNG TIẾN

ĐIỀU KHIỂN BÁM QUỸ ĐẠO CHO MƠ HÌNH MÁY
BAY BỐN CÁNH QUẠT - QUADROTOR

CHUYÊN NGÀNH: TỰ ĐỘNG HĨA (CT)
MÃ SỐ: 60.52.60

LUẬN VĂN THẠC SĨ

TP. HỒ CHÍ MINH, 07 – 2014


CƠNG TRÌNH ĐƯỢC HỒN THÀNH TẠI
TRƯỜNG ĐẠI HỌC BÁCH KHOA –ĐHQG -HCM

Cán bộ hướng dẫn khoa học: TS NGUYỄN CHÁNH NGHIỆM

Cán bộ chấm nhận xét 1: PGS. TS. HUỲNH THÁI HỒNG

Cán bộ chấm nhận xét 2: TS. VÕ CƠNG PHƯƠNG

Luận văn thạc sĩ được bảo vệ tại Trường Đại học Bách Khoa, ĐHQG Tp.
HCM ngày 18 tháng 07 năm 2014.
Thành phần Hội đồng đánh giá luận văn thạc sĩ gồm:
1. TS. NGUYỄN ĐỨC THÀNH
2. TS. NGÔ MẠNH DŨNG

3. PGS. TS. HUỲNH THÁI HỒNG
4. TS. VÕ CƠNG PHƯƠNG
5. TS. TRỊNH HỒNG HƠN

CHỦ TỊCH HỘI ĐỒNG

TRƯỞNG KHOA


ĐẠI HỌC QUỐC GIA TP.HCM
TRƯỜNG ĐẠI HỌC BÁCH KHOA

CỘNG HÒA XÃ HỘI CHỦ NGHĨA VIỆT NAM
Độc lập - Tự do - Hạnh phúc

NHIỆM VỤ LUẬN VĂN THẠC SĨ
Họ tên học viên:

CAO HOÀNG TIẾN

MSHV: 11900722

Ngày, tháng, năm sinh: 10/08/1988

Nơi sinh: Cần Thơ

Chuyên ngành: Tự động hóa (CT)

Mã số : 60.52.61


I. TÊN ĐỀ TÀI:
ĐIỀU KHIỂN BÁM QUỸ ĐẠO CHO MƠ HÌNH MÁY BAY BỐN CÁNH
QUẠT - QUADROTOR
II. NHIỆM VỤ VÀ NỘI DUNG:
-

Mô hình hóa quadrotor.
Thiết kế mơ hình quadrotor.
Hiệu chỉnh bộ điều khiển cân bằng quadrotor.
Xác định vị trí quadrotor.
Điều khiển bay.

III. NGÀY GIAO NHIỆM VỤ : 23/07/2013
IV. NGÀY HOÀN THÀNH NHIỆM VỤ: 20/06/2014
V. CÁN BỘ HƯỚNG DẪN : TS NGUYỄN CHÁNH NGHIỆM

Tp. HCM, ngày . . . . tháng .. . . năm 20....
CÁN BỘ HƯỚNG DẪN
(Họ tên và chữ ký)

CHỦ NHIỆM BỘ MÔN ĐÀO TẠO
(Họ tên và chữ ký)

TRƯỞNG KHOA….………
(Họ tên và chữ ký)


1
LỜI CẢM ƠN


Trong quá trình thực hiện đề tài luận văn, tơi đã gặp rất nhiều khó khăn. Với
sự nỗ lực của bản thân cùng với sự động viên, giúp đỡ rất nhiệt tình từ q thầy cơ,
bạn bè và những người thân, tơi đã hồn thành luận văn của mình.
Trước tiên, tơi chân thành cảm ơn tất cả q Thầy Cô thuộc Khoa Điện – Điện
Tử trường Đại học Bách Khoa TP.Hồ Chí Minh đã trang bị cho tơi những kiến thức
quý báu trong suốt quá trình học tập tại trường.
Cảm ơn các bạn học viên lớp Cao học Tự Động Hóa 2011 đã động viên và
nhiệt tình giúp đỡ khi tơi gặp khó khăn.
Đặc biệt, tơi xin chân thành cảm ơn Thầy Nguyễn Chánh Nghiệm đã tận tình góp ý
và giúp đỡ tơi trong suốt q trình thực hiện đề tài.
Tôi cũng cảm ơn các tác giả của các bài báo mà tôi sử dụng để làm tài liệu tham
khảo.
Cuối cùng tôi gửi lời cảm ơn đến tất cả những người thân, bạn bè luôn luôn ở
bên tôi và ủng hộ tôi.

TP.HCM, tháng 7 năm 2014
Học viên TĐH 2011

Cao Hoàng Tiến

Cao Hoàng Tiến

Điều khiển bám quỹ đạo cho máy bay bốn cánh quạt - quadrotor


2
TÓM TẮT
Luận văn này hướng đến nghiên cứu điều khiển mơ hình máy bay lên thẳng
bốn cánh quạt (quadrotor). Hệ thống động quadrotor được mơ hình hóa theo phương
pháp Newton-Euler. Vận tốc góc, góc và vị trí trên ba trục của quadrotor được điều

khiển bằng bộ điều khiển PID. Mô hình thật được thiết kế, hiệu chỉnh các thơng số
PID của bộ điều khiển cân bằng và các giải pháp chống rung cho cảm biến để đạt độ
chính xác cao hơn cũng được trình bày. Vị trí quadrotor được ước lượng bằng phương
pháp dead reckoning. Dựa trên kết quả ước lượng, quadrotor có thề được điều khiển
bay đến vị trí đặt.

Cao Hoàng Tiến

Điều khiển bám quỹ đạo cho máy bay bốn cánh quạt - quadrotor


3
ABSTRACT
This thesis work focused on the study of quadrotor. The Newton-Euler
formalism was used to model the dynamic system. The PID control algorithms were
used to control angular rate, angle and position on three axes of quadrotor. A real
platform of quadrotor was assembled, the PID parameters were tuned to reach the
robustness of control algorithms, vibration damping methods were carried out to
reach better accuracy on sensor signals. The position of quadrotor was estimated by
using dead reckoning algorithm. Based on the result, quadrotor could be controlled
to fly to new desired position.

Cao Hoàng Tiến

Điều khiển bám quỹ đạo cho máy bay bốn cánh quạt - quadrotor


4
MỤC LỤC
LỜI CẢM ƠN ............................................................................................................. 1

TÓM TẮT ................................................................................................................... 2
ABSTRACT ................................................................................................................ 3
MỤC LỤC ................................................................................................................... 4
DANH SÁCH CÁC HÌNH VẼ ................................................................................... 7
DANH SÁCH BẢNG ............................................................................................... 10
CÁC KÍ HIỆU VÀ TỪ VIẾT TẮT........................................................................... 11
Chương 1 ................................................................................................................... 12
GIỚI THIỆU TỔNG QUAN ..................................................................................... 12
1.1. Đặt vấn đề ....................................................................................................... 12
1.2. Giới thiệu đề tài .............................................................................................. 15
1.3. Tóm lượt nội dung luận văn ........................................................................... 15
1.3.1 Chương 1: Giới thiệu tổng quan ............................................................... 15
1.3.2 Chương 2: Cơ sở lý thuyết ........................................................................ 15
1.3.3 Chương 3: Thiết kế hệ thống .................................................................... 16
1.3.4 Chương 4: Kết quả thí nghiệm và kết luận ............................................... 16
Chương 2 ................................................................................................................... 17
CƠ SỞ LÝ THUYẾT ................................................................................................ 17
2.1. Mơ hình hóa quadrotor ................................................................................... 17
2.1.1 Giới thiệu mơ hình quadrotor ................................................................... 17
2.1.1.1 Throttle (U1 [N]) (lực nâng) ...............................................................17
2.1.1.2. Roll (U2[Nm]) (moment trên trục x) .................................................18
2.1.1.3. Pitch (U3[Nm]) (moment trên trục y) ................................................18
2.1.1.4. Yaw (U4[Nm]) (moment trên trục z) .................................................19
Cao Hoàng Tiến

Điều khiển bám quỹ đạo cho máy bay bốn cánh quạt - quadrotor


5
2.1.2. Mơ hình hóa theo Newton-Euler ............................................................. 19

2.2. Hệ thống định vị quán tính INS (Inertial Navigation System) ....................... 26
2.2.1. Giới thiệu chung ...................................................................................... 26
2.2.2. Một số lý thuyết về không gian 3 chiều liên quan đến hệ thống định vị
INS ..................................................................................................................... 28
2.2.2.1 Góc Euler và việc định vị phương hướng trong khơng gian ..............28
2.2.2.2 Quaternions .........................................................................................29
2.2.2.3 Chuyển đổi góc Euler và Quaternions ................................................30
2.2.2.4 Đơn vị đo lường quán tính IMU (Inertial Measurement Unit) ...........31
2.2.2.5 Bộ lọc Kalman ....................................................................................32
2.2.2.6 Bộ lọc Kalman gián đoạn ....................................................................35
Chương 3 ................................................................................................................... 37
THIẾT KẾ HỆ THỐNG............................................................................................ 37
3.1 Thiết kế phần cứng .......................................................................................... 37
3.1.1 Các thành phần chính của quadrotor ........................................................ 37
3.1.1.1 Khung quadrotor .................................................................................38
3.1.1.2 Động cơ ...............................................................................................39
3.1.1.3 Cánh quạt ............................................................................................41
3.1.1.4 Bộ điều tốc (ESC) ...............................................................................43
3.1.1.5 Pin .......................................................................................................46
3.1.1.6 Board điều khiển APM .......................................................................47
3.1.1.7 Bộ điều khiển từ xa .............................................................................49
3.1.1.8 Đơn vị đo lường quán tính xIMU .......................................................52
3.1.2 Kết nối các thiết bị .................................................................................... 53
3.1.3 Giải pháp chống rung ............................................................................... 54
Cao Hoàng Tiến

Điều khiển bám quỹ đạo cho máy bay bốn cánh quạt - quadrotor


6

3.1.4 Hiệu chỉnh PID ......................................................................................... 55
3.2 Chương trình điều khiển .................................................................................. 58
3.2.1 Chương trình giao tiếp giữa xIMU và máy tính ....................................... 58
3.2.2 Giải thuật ước lượng vị trí ........................................................................ 59
3.2.2.1. Ước lượng vị trí trên một trục bằng phương pháp dead reckoning ...59
3.2.2.2. Ước lượng vị trí bằng bộ lọc Kalman ................................................67
3.2.2.3 Ước lượng vị trí trên ba trục ...............................................................68
3.2.3 Giải thuật điều khiển quadrotor ................................................................ 71
3.2.3.1. Bộ điều khiển vận tốc góc ..................................................................73
3.2.3.2. Bộ điều khiển góc ..............................................................................77
3.2.3.3. Bộ điều khiển vị trí ............................................................................80
Chương 4 ................................................................................................................... 83
KẾT QUẢ THỰC NGHIỆM VÀ KẾT LUẬN ........................................................ 83
4.1 Kết quả thí nghiệm .......................................................................................... 83
4.1.1 Thí nghiệm kiểm tra giải thuật ................................................................. 83
4.1.1.1 Kết quả thí nghiệm giải thuật ước lượng vị trí bằng phương pháp dead
.........................................................................................................................83
4.1.1.2 Kết quả giải thuật ước lượng vị trí bằng bộ lọc Kalman ....................84
4.1.2 Thí nghiệm trên mơ hình thực .................................................................. 87
4.2 Kết luận ........................................................................................................... 89
4.3 Hướng phát triển .............................................................................................. 89
TÀI LIỆU THAM KHẢO ......................................................................................... 90

Cao Hoàng Tiến

Điều khiển bám quỹ đạo cho máy bay bốn cánh quạt - quadrotor


7


DANH SÁCH CÁC HÌNH VẼ

Hình 2.1 – Mơ hình quadrotor ............................................................................................. 17
Hình 2.2 – Điều khiển lực nâng ........................................................................................... 18
Hình 2.3 – Điều khiển góc Roll ........................................................................................... 18
Hình 2.4 – Điều khiển góc Pitch .......................................................................................... 18
Hình 2.5 – Điều khiển góc Yaw .......................................................................................... 19
Hình 2.6 – Hệ tọa độ ............................................................................................................ 19
Hình 2.7 – Các góc Euler [26] ............................................................................................. 29
Hình 2.8 – Các góc Tait Bryan [26]..................................................................................... 29
Hình 2.9 – Bộ lọc Kalma gián đoạn..................................................................................... 36
Hình 3.1 (a) Mơ hình CAD của quadrotor. (b) Mơ hình thực tế......................................... 38
Hình 3.2 Kẹp nhựa trong (a) mơ hình CAD và (b) mơ hình thực tế. .................................. 39
Hình 3.3. Động cơ Sunny Sky V2216 12 . ......................................................................... 40
Hình 3.4. Sải cánh X và góc pitch Y của cánh quạt [31]. ................................................... 42
Hình 3.5. Cánh quạt 11x5” [31]. ......................................................................................... 43
Hình 3.6. Bố trí chiều quay của các động cơ. ..................................................................... 43
Hình 3.7. Điện áp cấp cho ba pha của động cơ BLDC [34]. .............................................. 44
Hình 3.8. Tầng cơng suất của ESC [34]. ............................................................................ 44
Hình 3.9. ESC 30A HobbyKing Blue series [31] ............................................................... 45
Hình 3.10. Pin Li-Po Wild Scorpion 2800mAh, 45C [36]. ................................................ 47
Hình 3.11. Các cổng kết nối của bo APM 2.5 [37]............................................................. 48
Hình 3.12. Bộ phát tín hiệu DEVO 10 [39]. ....................................................................... 50
Hình 3.13. Bộ thu tín hiệu DEVO 10 RX1002 [39]. .......................................................... 50
Hình 3.14 – x-IMU (bo mạch và pin) [29] ......................................................................... 52
Cao Hoàng Tiến

Điều khiển bám quỹ đạo cho máy bay bốn cánh quạt - quadrotor



8
Hình 3.15 - x-IMU (mặt trước)

Hình 3.16 - x-IMU (mặt sau) ................................ 53

Hình 3.17 – sơ đồ kết nối thiết bị......................................................................................... 53
Hình 3.18 – giải pháp chống rung cho bo APM 2.5 ............................................................ 54
Hình 3.19 – kết quả chống rung cho bo APM 2.5 ............................................................... 55
Hình 3.20 – Bộ điều khiển cân bằng quadrotor [40] ........................................................... 56
Hình 3.21 – Đáp ứng góc roll (a) và pitch (b) với các thơng số PID ................................... 57
Hình 3.22 – giao diện giao tiếp giữa xIMU và máy tính ..................................................... 58
Hình 3.23 – ví dụ tín hiệu cảm biến gia tốc [24] ................................................................. 60
Hình 3.24 – sai số trong q trình tích phân [24] ................................................................ 60
Hình 3.25 – Giảm sai số tích phân bằng cách sử dụng hàm bậc 1 [24] ............................... 61
Hình 3.26 – lưu đồ giải thuật dead reckoning [24] .............................................................. 63
Hình 3.27 – Kết quả thí nghiệm khi di chuyển xIMU theo phương x một khoảng 0.5m .... 64
Hình 3.28 – Kết quả lọc dữ liệu bằng bộ lọc thông thấp ..................................................... 65
Hình 3.29 – Kết quả thí nghiệm sau khi lọc dữ liệu gia tốc và đặt gia tốc bằng 0 khi xIMU
đang đứng yên ...................................................................................................................... 66
Hình 3.30 - Bộ lọc Kalman gián đoạn [22].......................................................................... 68
Hình 3.31 – Lưu đồ giải thuật ước lượng vị trí trong khơng gian 3 chiều ........................... 70
Hình 3.32 – Bộ điều khiển vị trí quadrotor .......................................................................... 73
Hình 3.33 – Bộ điều khiển PID vận tốc góc trên một trục [40] ........................................... 74
Hình 3.34 – Mơ phỏng bộ điều khiển PID vận tốc góc ....................................................... 74
Hình 3.35– Kết quả mơ phỏng bộ điều khiển vận tốc góc Roll ........................................... 75
Hình 3.36 – Kết quả mơ phỏng bộ điều khiển vận tốc góc Pitch ........................................ 76
Hình 3.37 – Kết quả mô phỏng bộ điều khiển vận tốc góc Yaw ......................................... 76
Hình 3.38 – Bộ điều khiển PID góc trên một trục [40] ....................................................... 77
Hình 3.39 - Mơ phỏng bộ điều khiển PID góc.................................................................... 78
Hình 3.40– Kết quả mơ phỏng bộ điều khiển góc Roll ....................................................... 78


Cao Hoàng Tiến

Điều khiển bám quỹ đạo cho máy bay bốn cánh quạt - quadrotor


9
Hình 4.1 – kết quả ước lượng vị trí bằng phương pháp dead reckoning ............................. 83
Hình 4.2 – kết quả lọc dữ liệu gia tốc bằng bộ lọc Kalman với qc=10 và R=5 ................... 84
Hình 4.3 kết quả bộ lọc Kalman với qc=0.1, R=0.1 ............................................................ 85
Hình 4.4 - kết quả bộ lọc Kalman với qc=0.5, R=1 ............................................................. 85
Hình 4.5 - kết quả bộ lọc Kalman với qc=0.5, R=1 ............................................................. 86
Hình 4.6 – kết quả ước lượng gia tốc, vận tốc, vị trí bằng bộ lọc Kalman .......................... 86
Hình 4.7 – Kết quả lọc Kalman với qc=0.05, R=100 .......................................................... 88
Hình 4.8 – Kết quả tính góc roll, pitch, yaw ........................................................................ 88
Hình 4.9 – Kết quả ước lượng vị trí ..................................................................................... 88

Cao Hoàng Tiến

Điều khiển bám quỹ đạo cho máy bay bốn cánh quạt - quadrotor


10
DANH SÁCH BẢNG
Bảng 3.1. Thông số kỹ thuật của động cơ Sunny Sky v2216 12 [30]. ............................... 40
Bảng 3.2. Thông số về sức nâng của động cơ Sunny Sky v2216 12 [30]. ......................... 41
Bảng 3.3. Thông số kỹ thuật của ESC 30A HobbyKing Blue series [31] .......................... 45
Bảng 3.4. Thông số kỹ thuật của Pin LiPo Wild Scorpion 3500mAh 45C [36]................. 47
Bảng 3.5. Thông số kỹ thuật của bộ phát DEVO 10 [38] ................................................... 51
Bảng 2.6. Thông số kỹ thuật của bộ thu DEVO 10 RX1002 [22] ...................................... 51

Bảng 3.4 – các thơng số mơ hình quadrotor ........................................................................ 72

Cao Hồng Tiến

Điều khiển bám quỹ đạo cho máy bay bốn cánh quạt - quadrotor


11

CÁC KÍ HIỆU VÀ TỪ VIẾT TẮT

Thuật ngữ viết tắt

Diễn giải

ESC

Electronic Speed Control

Li-Po

Lithium polymer

APM

ArduPilot Mega

GPS

Global Positioning System


UAV

Unmanned Aerial Vehicle

IMU

Inertial Mesurement Unit

INS

Inertial Navigation System

Cao Hoàng Tiến

Điều khiển bám quỹ đạo cho máy bay bốn cánh quạt - quadrotor


12
Chương 1
GIỚI THIỆU TỔNG QUAN
1.1. Đặt vấn đề
Robotics là một lĩnh vực đang được các nhà nghiên cứu quan tâm. Robotics
được ứng dụng trong nhiều lĩnh vực như: quân sự, cơng nghiệp, y tế, cứu hộ,…với
mục đích thay thế con người thực hiện các nhiệm vụ nguy hiểm. Robot bay –
Unmanned Aerial Vehicle (UAV) là một mảng đang được quan tâm nhiều của lĩnh
vực robotics [1].
Mơ hình UAV có nhiều dạng cấu trúc như: khinh khí cầu, máy bay cánh bằng,
trực thăng, robot chim, máy bay nhiều cánh quạt (multirotor). Mỗi dạng UAV đều có
ưu và nhược điểm riêng, trong phạm vi nghiên cứu của đề tài hướng đến nghiên cứu

dạng máy bay bốn cánh quạt – quadrotor.
Quadrotor là dạng máy bay lên thẳng bốn cánh quạt rất linh động và có cấu
trúc đơn giản. Vì vậy, quadrotor rất phù hợp với các ứng dụng thu thập không ảnh,
quan sát mơi trường, tìm kiếm cứu hộ, vẽ bản đồ. Ngồi ra quadrotor còn được ứng
dụng cho việc thay thế con người thực hiện các nhiệm vụ nguy hiểm. Trong lĩnh vực
này nếu sử dụng GPS để định vị quadrotor là chưa đủ, vì quadrotor thường hoạt động
trong những mơi trường chật hẹp, những nơi mà con người chưa biết hay những khu
nhà đỗ nát – nơi mà tín hiệu GPS yếu hay khơng có GPS.
Mơ hình quadrotor có nhiều ưu điểm nổi bậc so với dạng máy bay cánh bằng,
quadrotor có thể cất cánh, hạ cánh thẳng đứng, bay cân bằng tại một vị trí [2]. Với
đặc điểm này cho phép quadrotor dễ dàng hoạt động trong những môi trường chật
hẹp, bay trong nhà [3], bay qua cửa sổ [4], di chuyển dọc hành lang [5], bay lên cầu
thang [6] và hơn nữa là có thể gấp các vật thể nhỏ [7].
Trong lĩnh vực định vị và điều khiển mô hình quadrotor, vấn đề quan trọng
nhất là điều khiển sáu bậc tự do của mơ hình, bao gồm: các góc quay và di chuyển
tịnh tiến trên ba trục của quadrotor. Đối với bộ điều khiển góc quay, các nhà nghiên
cứu đã đạt được những thành tựu đáng kể [1],[8] ,[9] ,[10] ,[11] ,[12] : trong đó phải
kể đến các bộ điều khiển PID, hồi quy tuyến tính (LQR),cuốn chiếu (back stepping),
Cao Hoàng Tiến

Điều khiển bám quỹ đạo cho máy bay bốn cánh quạt - quadrotor


13
bộ điều khiển trượt (sliding mode). Tuy nhiên nếu không có bộ điều khiển vị trí,
quadrotor dễ dàng bi trơi (drift) ngay cả khi khơng có ngoại lực tác động vào. Trong
mơi trường khơng có GPS, bộ điều khiển vị trí có thể được thực hiện nhờ các loại
cảm biến định vị. Các loại này có thể được chia làm hai dạng, một là dạng cảm biến
ngoại vi (cảm biến khơng gắn trên mơ hình) – offboard sensors (như hệ thống camera
–motion capture systems), hai là dạng cảm biến tích hợp trên mơ hình – onboard

sensors (như: camera, cảm biến laser, IMU). Bằng cách sử dụng cảm biến offboard,
các nhà nghiên cứu có thể phát triển giải thuật mà khơng cần giải quyết các vấn đề về
định vị như cảm biến offboard. Một trong những hệ thống cảm biến offboard được
các nhà nghiên cứu sử dụng phổ biến là Vicon. Vicon bao gồm nhiều camera hồng
ngoại cố định, được dùng để xác định vị trí quadrotor bằng cách phát hiện các quả
cầu phản quang được gắn trên mơ hình, với độ chính xác vài mili mét và tốc độ khung
hình cao (vài trăm hz) [13]. Hệ thống này thích hợp cho thử nghiệm các giải thuật
điều khiển. Một số nhà nghiên cứu đã sử dụng hệ thống này điều khiển quadrotor
thực hiện các nhiệm vụ như: điều khiển quadrotor nhào lộn, tung hứng một thanh gỗ
[14] hay một quả bóng [15], lắp ráp một mơ hình đơn giản [16].
Những nghiên cứu trên cho thấy quadrotor khơng có khả năng tự định vị mà
phải thông qua một hệ thống quan sát bên ngồi. Khơng gian hoạt động của quadrotor
chỉ nằm trong tầm quan sát của hệ thống camera. Do đó hệ thống này không sử dụng
được trong việc điều khiển quadrotor di chuyển trong những môi trường chật hẹp,
những môi trường chưa biết trước hay những nơi khơng có GPS. Chính vì vậy các
cảm biến định vị cần phải được gắn trên quadrotor (onboard sensors).
Một số tác giả đã sử dụng cảm biến laser 2D kết hợp camera để xác định vị trí
và điều khiển quadrotor, đồng thời xây dựng bản đồ [3], [4], [17], [18]. Mặc dù cảm
biến laser hoạt động với độ tin cậy cao và ổn định, tuy nhiên, cảm biến khá nặng và
tiêu hao nhiều năng lượng. So với laser thì camera là lựa chọn khả thi hơn, tuy nhiên,
cần có thiết bị xữ lý ảnh đủ mạnh để phân tích dữ liệu (như: 1.6 GHz Intel Atom
computer [4], [13], Core-2-Duo onboard computer [13]). Bên cạnh đó, camera cịn
phụ thuộc nhiều vào độ sáng của mơi trường. “Optical flow” là một lựa chọn mà
nhiều nhà nghiên cứu sử dụng để định vị quadrotor [5], [19], [20]. Optical flow chỉ
Cao Hoàng Tiến

Điều khiển bám quỹ đạo cho máy bay bốn cánh quạt - quadrotor


14

có thể đo được vận tốc tương đối của vật thể trong hình ảnh thu thập được, vị trí sau
đó được ước tính từ giá trị vận tốc đo được. Do đó, giá trị vận tốc sẽ có sai số tích lũy
theo thời gian. Một số tác giả khác sử dụng RGB-D camera để điều khiển quadrotor
và vẽ bản đồ 3D [3], [18]. Quadrotor có thể tự định hướng và di chuyển trong môi
trường 3D nhờ sự kết hợp của RGBD-Camera (Kinect) và đơn vị đo lường quán tính
- Inertial Measurement Unit (IMU). Tất cả các tính tốn và cảm biến đều nằm trên
quadrotor. Ứng dụng thuật toán Visual odometry (thuật tốn xác định vị trí và hướng
của robot bằng cách phân tích độ sâu hình ảnh quay được từ camera), quadrotor có
thể hoạt động tốt trong mơi trường phức tạp, nhiều vật cản. Tuy nhiên, quadrotor lại
không hoạt động tốt trong mơi trường có khoảng khơng rộng vì khoảng cách của vật
thể xa hơn so với tầm hoạt động của Kinect.
Từ những nghiên cứu trên, cho thấy IMU giữ một vai trò quan trọng trong việc
định vị quadrotor trong mơi trường khơng có GPS. Do đó luận văn này hướng đến
nghiên cứu các giải thuật định vị quadrotor bằng IMU. Vị trí ước lượng bằng IMU có
độ chính xác khơng cao và sai số tích lũy theo thời gian. Do đó, cần kết hợp với một
thiết bị khác để nâng cao độ chính xác và bù sai số, tốt nhất là sử dụng camera [13].
Tuy nhiên, trong luận văn này khơng có điều kiện trạng bị thiết bị xử lý ảnh (computer
based module) nên chỉ sử dụng IMU và việc bù sai số được thực hiện bằng phần mềm.
Các tác giả [21], [22], [23], [24], [25] mô tả phương pháp xác định vị trí bằng
cách lấy tích phân kép tín hiệu gia tốc của IMU. Tuy nhiên, q trình tích phân sẽ dẫn
đến sai số tích lũy trên giá trị vị trí. Đối với xác định vị trí cần nghiên cứu 3 vấn đề:
một là bộ lọc tín hiệu (như bộ lọc thông thấp [21], [24], bộ lọc Kalman [22]), hai là
ước lượng vị trí, ba là bù sai số vị trí. Đối với từng trường hợp cụ thể mà có cách bù
sai số khác nhau như: trong trường hợp vật di chuyển trên mặt phẳng nẳm ngang, giải
thuật phát hiện chuyển động [24] được thực hiện để bù sai số hay biết trước vận tốc
đầu và vận tốc cuối, từ đó bù sai số cho vị trí [25]; trong trường hợp IMU được gắn
trên bàn chân người, giải thuật phát hiện bước chân được thực hiện dựa trên sự thay
đổi của góc quay và vận tốc bằng khơng giữa hai bước chân [21]. Đối với trường hợp
định vị quadrotor trong đề tài này, quadrotor được điều khiển bay ở một độ cao xác
định nên việc định vị là xác định vị trí 2D. Để bù sai số cho IMU, quadrotor được

Cao Hoàng Tiến

Điều khiển bám quỹ đạo cho máy bay bốn cánh quạt - quadrotor


15
điều khiển bay theo các điểm đặt, tại các điểm đặt ta biết trước vận tốc và gia tốc đầu
bằng khơng.
1.2. Giới thiệu đề tài
Luận văn trình bày việc xây dựng mơ hình quadrotor có khả năng bay theo
quỹ đạo trong mơi trường khơng có GPS. Vị trí quadrotor được xác định bằng cách
sử dụng một đơn vị đo lường qn tính – IMU được gắn trên mơ hình. Nội dung luận
văn bao gồm: xây dựng mơ hình quadrotor với các thiết bị, cảm biến đủ để quadrotor
có khả năng bay theo quỹ đạo; phát triển phần mềm ước lượng vị trí quadrotor bằng
IMU, từ đó điều khiển quadrotor bay theo quỹ đạo.
Mục tiêu chính của đề tài gồm có:
- Mơ hình hóa quadrotor.
- Mơ phỏng bộ điều khiển vị trí cho quadrotor bằng Matlab-Simulink.
- Xây dựng mơ hình, hiệu chỉnh bộ điều khiển cân bằng quadrotor.
- Tìm hiểu giải thuật xác định vị trí quadrotor.
- Thí nghiệm kiểm tra độ chính xác giải thuật.
- Thí nghiệm trên mơ hình thực.
1.3. Tóm lượt nội dung luận văn
1.3.1 Chương 1: Giới thiệu tổng quan
Chương này giới thiệu sơ lược về mô hình quadrotor và những ưu điểm của
mơ hình này. Bên cạnh đó trình bày sơ lược các phương pháp điều khiển và ước lượng
vị trí của quadrotor.
1.3.2 Chương 2: Cơ sở lý thuyết
Chương này trình bày nguyên lý hoạt động, mơ hình hóa động học quadrotor
theo hệ phương trình Newton-Euler và phương pháp điều khiển cân bằng mơ hình

bằng bộ điều khiển PID. Giới thiệu hệ thống định vị quán tính INS và các thành phần
của hệ thống này (đơn vị đo lường quán tính IMU – bao gồm cảm biến gia tốc và cảm
biến con quay hồi chuyển), đồng thời các định nghĩa về góc Euler, quaternion liên
Cao Hồng Tiến

Điều khiển bám quỹ đạo cho máy bay bốn cánh quạt - quadrotor


16
quan đến hệ thống này cũng được trình bày trong chương này. Cuối cùng, trình bày
phương pháp ước lượng vị trí bằng giải thuật dead reckoning và lý thuyết bộ lọc
Kalman để lọc nhiễu dự báo trạng thái ngõ ra. Các nội dung của chương này được
dùng làm cơ sở áp dụng trong chương 3.
1.3.3 Chương 3: Thiết kế hệ thống
Chương này gồm 2 phần chính: thiết kế phần cứng và chương trình điều khiển.
Thiết kế phần cứng: phần này mơ tả cấu tạo phần cơ khí, phần điện, phần điều
khiển của mơ hình quadrotor. Ngồi ra phần này cịn trình bày cách kết nối các thiết
bị của mơ hình, phương án giảm rung động cho cảm biến, và hiệu chỉnh các thông số
PID của bộ điều khiển cân bằng quadrotor.
Chương trình điều khiển: phần này trình bày chương trình kết nối giữa
quadrotor và máy tính thơng qua chuẩn Bluetooth. Bên cạnh đó, kết quả mơ phỏng
các giải thuật điều khiển vận tốc góc, góc và vị trí quadrotor cũng được trình bày
trong phần này.
1.3.4 Chương 4: Kết quả thí nghiệm và kết luận
Phẩn này mơ tả các thí nghiệm đã thực hiện, bao gồm: Thí nghiệm kiểm tra
giải thuật ước lượng vị trí; thí nghiệm trên mơ hình thực và thí nghiệm điều khiển
bay. Phần này cũng rút ra nhận xét giữa các kết quả thí nghiệm và tổng kết lại những
mặt đã đạt được và những hạn chế của đề tài và đưa ra hướng phát triển của đề tài.

Cao Hoàng Tiến


Điều khiển bám quỹ đạo cho máy bay bốn cánh quạt - quadrotor


17
Chương 2
CƠ SỞ LÝ THUYẾT
2.1. Mơ hình hóa quadrotor
2.1.1 Giới thiệu mơ hình quadrotor
Phần này trình bày mơ hình hóa động học Quadrotor theo phương trình
Newton-Euler.
Quadrotor là mơ hình máy bay lên thẳng, có cấu trúc đơn giản, bao gồm 4 cánh
quạt đượcc bố trí như hình 2.1. Mơ hình được điều khiển bằng cách thay đổi lực tác
động của bốn động cơ. Do đặc điểm trên quadrotor có thể di chuyển tới, lui, sang trái,
phải theo cấu hình dấu + (tức mũi của quadrotor là 1 trong 4 động) hay cấu hình chữ
X (tức mũi của quadrotor là 2 động cơ kể nhau). Trong luận văn này chủ yếu nghiên
cứu mơ hình quadrotor hình dấu +.

Hình 2.1 – Mơ hình quadrotor
Hệ thống gồm 6 bậc tự do: tịnh tiến theo x, y ,z [x, y, z]; quay quanh x, y, z
[ϕ, θ, ψ], nhưng chỉ có bốn ngõ vào (lực từ bốn động cơ) nên khó có thể điều khiển
trực tiếp 6 trạng thái này. Do đó, quadrotor được phân tích thành một chuyển động
thẳng đứng dưới tác động của lực nâng và ba chuyển động quay dưới tác động của
moment quanh truc x, y, z như sau:
2.1.1.1 Throttle (U1 [N]) (lực nâng)
Tăng hoặc giảm tốc độ 4 động cơ với cùng một giá trị để thay đổi độ cao
quadrotor.

Cao Hoàng Tiến


Điều khiển bám quỹ đạo cho máy bay bốn cánh quạt - quadrotor


18

Hình 2.2 – Điều khiển lực nâng
2.1.1.2. Roll (U2[Nm]) (moment trên trục x)
Moment được tạo ra bằng cách tăng (hoặc giảm) vận tốc động cơ trái và giảm
(hoặc tăng) vân tốc động cơ phải.

Hình 2.3 – Điều khiển góc Roll
ΔA, ΔB được chọn sao cho tổng lực nâng không đổi.
2.1.1.3. Pitch (U3[Nm]) (moment trên trục y)
Moment được tạo ra bằng cách tăng (hoặc giảm) vận tốc động cơ sau và giảm (hoặc
tăng) vân tốc động cơ trước.
ΔA, ΔB được chọn sao cho tổng lực nâng khơng đổi.

Hình 2.4 – Điều khiển góc Pitch
Cao Hồng Tiến

Điều khiển bám quỹ đạo cho máy bay bốn cánh quạt - quadrotor


19
2.1.1.4. Yaw (U4[Nm]) (moment trên trục z)
Moment được tạo ra bằng cách tăng (hoặc giảm) vận tốc cặp trước-sau và giảm (hoặc
tăng) vân tốc cặp động cơ trái-phải.

Hình 2.5 – Điều khiển góc Yaw
2.1.2. Mơ hình hóa theo Newton-Euler


Hình 2.6 – Hệ tọa độ
Trong đó:
• Hệ tọa độ qn tính E-frame (the earth inertial frame)
• Hệ tọa độ động gắn với vật B-frame (the body-fixed frame)
Phương trình động học mơ hình 6 bậc tự do (6DOF) [1]
  J v

(1)

Trong đó:

- vector vận tốc tổng quát so với E-frame.
v - vector vận tốc tổng quát so với B-frame.

Cao Hoàng Tiến

Điều khiển bám quỹ đạo cho máy bay bốn cánh quạt - quadrotor


20

J  - ma trận chuyển đổi.
[

E

E T

]


[V B

Trong đó:

B T

(2)

E
vector vị trí  [m] và góc

Trong đó:

v

]T

[X Y Z

E

[u v w p q r ]T

]

[rad] so với E-frame
(3)

V B [m/s]- vector vận tốc dài so với B-frame.


 B [rad/s]-vector vận tốc góc so với B-frame.

J

R

03 3

03 3

(4)

T

Trong đó:

R - ma trận quay.
T - ma trận chuyển.

c c  s c  c s s

R   s c c c  s s s
 s
c s
 
1

T  0
0



s c  c s c 

 c s  s s c 

c c


(5)



c  s

s / c c / c 

s t

c t

Trong đó: s=sin, c=cos, t=tan
Phương trình động lực học của mơ hình theo định luật 2 Newton:

mI3 x 3
0
 3 x3
Trong đó:

03 x 3  V B   B  (mV B )  F B 

 
 
I   B   B  ( I B )   B 

(6)

m (kg) – khối lượng Quadrotor.
I (N m s2) – ma trận moment quán tính.
I3x3 – ma trận đơn vị.
V B (m/s2) – vector gia tốc dài so với B-frame.

Cao Hoàng Tiến

Điều khiển bám quỹ đạo cho máy bay bốn cánh quạt - quadrotor


21
 B (rad/s2) – vector gia tốc góc so với B-frame.
F B (N) – vector lực so với B-frame.

 B (N m) – moment so với B-frame.

* Giả thuyết:
- Góc OB được đặt ở trọng tâm quadrotor.
- Các trục của B-frame trùng với các trục moment quán tính.
Vector lực tổng quát được định nghĩa theo phương trình:



  FB  B


  F
T

x

Fy Fz  x  y  z



T

(7)

Như vậy phương trình động lực học (6) được viết lại dưới dạng ma trận, như sau:

M B v  C B (v)v  

(8)

M B (v) - ma trận moment qn tính.

Trong đó:

CB (v) - ma trận Coriolis

mI 33
MB  
033


033
C B (v )  
033

m
0

033  0

I  0
0

0

0
m
0
0

0
0
m
0

0 0
0 0
0 0
Ix 0

0


0

0

Iy

0

0

0

0

0
0

 mS (V B ) 0

 S ( I B )  0
0

0

0
0 
0

0

0

I z 

0
0
0
0

0
0
0
0

0
 m
mv
0

0

0

 Izr

0

0

I yq


(9)

m
0
 mu
Izr

 mv 
mu 

0

 I y q
0
Ix p 

 Ix p 0


(10)

Lực tổng quát Λ trong phương trình (8) chứa 3 thành phần:
Cao Hoàng Tiến

Điều khiển bám quỹ đạo cho máy bay bốn cánh quạt - quadrotor


22
-


Thành phần thứ nhất: vector trọng lực GB ( ) được sinh ra bởi gia tốc trọng
trường g [m/s2].
mgs

 mgc s
 0
 
 
 T
 

B
1 E
 F   R F  R 0
    mgc s
GB ( )   G     G    
0
031  031    mg   

0
 031
 

0

Trong đó:

-


FGB

- lực hấp dẫn so vời B-frame.

FGE

- lực hấp dẫn so với E-frame.





 (11)





Thành phần thứ hai: hiệu ứng con quay (gyroscopic effects) do cánh quạt tạo
ra khi quay. Một cặp cánh quạt quay theo chiều kim đồng hồ, cặp kia quay
ngược chiều kim đồng hồ. Nếu tổng đại số vận tốc các cánh quạt khác khơng
thì sự mất cân bằng tổng thể của quadrotor sẽ xảy ra. Hoặc vận tốc roll, pitch
khác không quadrotor sẽ chịu một moment xoắn theo phương trình (12)



031 




 4


0  
k

OB (v)    J TP  B   (1)  k 
 k 1

   0  






1


 

0
0
0
0

 031

0
0

0
0


0
0
0
0
 q 
      J TP 
 J TP p 
q q
q  q
   
 p p  p p
  0  

0
0
0
0

Trong đó:

(12)

JTP - moment qn tính quanh trục động cơ.
  1  2  3  4

Cao Hoàng Tiến












- vận tốc chung cho cánh quạt.

Điều khiển bám quỹ đạo cho máy bay bốn cánh quạt - quadrotor