Luận văn tốt nghiệp Trường Đại học Bách Khoa - Đại học Quốc gia Thành phố hồ Chí Minh. Full source code trên Github: https://github.com/dinhthong/quadrotor_bk

1. TRƯỜNG ĐẠI HỌC BÁCH KHOA TP. HỒ CHÍ MINH
KHOA ĐIỆN – ĐIỆN TỬ
BỘ MÔN ĐIỀU KHIỂN TỰ ĐỘNG
NGUYỄN ĐÌNH THÔNG
LUẬN VĂN TỐT NGHIỆP
THIẾT KẾ BỘ ĐIỀU KHIỂN VẬN TỐC VÀ VỊ TRÍ CHO
QUADCOPTER SỬ DỤNG CẢM BIẾN OPTICAL FLOW
KỸ SƯ NGÀNH KỸ THUẬT ĐIỀU KHIỂN & TỰ ĐỘNG HÓA

2. Note:
Some pages (forewords, thesis overview,...) was removed from the original
version, the main content is kept the same.

3. MỤC LỤC
TRƯỜNG ĐẠI HỌC BÁCH KHOA TP. HỒ CHÍ MINH...........................................................1
MỤC LỤC ...............................................................................................................................................3
DANH MỤC HÌNH VẼ.......................................................................................................................7
DANH MỤC BẢNG ..........................................................................................................................10
DANH MỤC TỪ VIẾT TẮT...........................................................................................................11
TÓM TẮT LUẬN VĂN.......................................................................................................................1
MỞ ĐẦU.................................................................................................................................................2
Chương 1. Giới thiệu .....................................................................................................................4
1.1. Giới thiệu chung về UAV và quadcopter................................................................4
1.2. Tình hình nghiên cứu về UAV.....................................................................................5
1.3. Mục tiêu và nội dung của luận văn ...........................................................................8
1.3.1. Lý do chọn đề tài..................................................................................................8
1.3.2. Tính thiết thực của đề tài luận văn ..............................................................9
1.3.2.1. Bộ điều khiển ....................................................................................................9
1.3.2.2. Điều khiển quadcopter bằng Optical Flow ..........................................9
1.3.3. Mục tiêu và nội dung.......................................................................................10
Chương 2. Hệ thống cảm biến và phần cứng...................................................................11
2.1. Sơ đồ kết nối của hệ thống........................................................................................11
2.2. Phần cứng.........................................................................................................................12
2.2.1. Pin Lipo .................................................................................................................12
2.2.2. Động cơ BLDC.....................................................................................................14
2.2.3. Bộ điều khiển ESC.............................................................................................15

4. 2.2.4. Cánh quạt..............................................................................................................15
2.2.5. Tay cầm DEVO7.................................................................................................16
2.2.6. Đọc tín hiệu PPM...............................................................................................17
2.2.7. Khung quadcopter............................................................................................18
2.3. Hệ thống cảm biến........................................................................................................19
2.3.1. Cảm biến vận tốc góc và gia tốc MPU6050............................................19
2.3.2. Cảm biến từ trường HMC5883...................................................................21
2.3.3. Cảm biến độ cao dùng sóng siêu âm HC-SR04.....................................21
2.3.4. Cảm biến Optical Flow PX4FLOW.............................................................22
2.3.4.1. Giới thiệu và thông số kỹ thuật của PX4FLOW ...............................22
2.3.4.2. Calib cảm biến của px4flow.....................................................................25
2.3.4.3. Giải thuật xử lý giữ liệu của px4flow...................................................25
2.4. Mạch Altium.....................................................................................................................27
2.4.1. Sơ đồ mạch Schematics..................................................................................28
2.4.2. Sơ đồ mạch PCB.................................................................................................29
Chương 3. Cơ sở lý thuyết .......................................................................................................32
3.1. Lập trình vi điều khiển ARM ....................................................................................32
3.1.1. Bo mạch STM32F407 Discovery vi điều khiển 32 bits ARM Cortex
M4 33
3.1.2. Lập trình trên vi điều khiển ARM Cortex M4 .......................................34
3.1.2.1. Thư viện lập trình........................................................................................34
3.1.2.2. Hệ thống ngắt trên vi điều khiển ARM Cortex M4.........................35
3.1.2.3. Giao tiếp I2C ...................................................................................................39
3.2. Đơn vị ước lượng trạng thái.....................................................................................41

5. 3.2.1. Quy ước hệ trục tọa độ...................................................................................42
3.2.2. Quaternion...........................................................................................................43
3.2.2.1. Đại số về quaternion...................................................................................44
3.2.2.2. Phương trình động học dưới dạng quaternion...............................47
3.2.3. Giải thuật Mahony Filter................................................................................48
3.2.4. Bộ lọc thông thấp..............................................................................................51
3.2.4.1. Giải thuật bộ lọc bù......................................................................................52
3.2.4.2. Giải thuật bộ lọc trung bình ( moving averaging filter ).............52
3.3. Xây dựng phương trình toán và động lực học..................................................54
3.3.1. Mô hình động cơ và cánh quạt....................................................................54
3.3.1.1. Hệ thống động cơ, cánh quạt và ESC. ..................................................54
3.3.1.2. Mối liên hệ giữa tốc độ động cơ và xung PWM...............................55
3.3.1.3. Mối liên hệ giữa tốc độ động cơ và lực tạo ra..................................55
3.3.1.4. Mối liên hệ giữa tốc độ động cơ và điện áp cấp cho động cơ....56
3.3.2. Phương tình động lực học của quadcopter...........................................56
3.3.3. Phương trình động lực học vật rắn...........................................................58
3.3.4. Phương trình tuyến tính hóa quanh điểm làm việc ..........................60
Chương 4. Thiết kế bộ điều khiển........................................................................................60
4.1. Lý thuyết bộ điều khiển PID.....................................................................................60
4.1.1. Bộ điều khiển PID tổng quát........................................................................60
4.1.2. Bộ điều khiển cascade PID............................................................................61
4.2. Áp dụng bộ điều khiển cho hệ thống quadcopter...........................................62
4.2.1. Bộ điều khiển góc roll pitch yaw................................................................62
4.2.1.1. Bộ điều khiển góc roll, pitch, yaw.........................................................62

6. 4.2.1.2. Áp dụng thuật toán điều khiển roll, pitch, yaw...............................62
4.2.2. Bộ điều khiển độ cao.......................................................................................65
4.2.2.1. Chế độ tự động cất cánh và xác định thrust hover........................65
4.2.2.2. Thiết kế bộ điều khiển giữ độ cao.........................................................65
4.2.3. Bộ điều khiển vận tốc và vị trí bằng Optical Flow..............................67
4.2.3.1. Chuyển đổi vị trí giữa Reference Frame và Body Frame............67
4.2.3.2. Bộ điều khiển vận tốc.................................................................................70
4.2.3.3. Bộ điều khiển giữ vị trí ..............................................................................72
Chương 5. Kết quả của hệ thống và hướng phát triển đề tài ...................................73
5.1. Kết quả và chất lượng của hệ thống......................................................................73
5.1.1. Hệ thống quadcopter được xây dựng......................................................73
5.1.2. Kết quả cân bằng roll pitch yaw.................................................................75
5.1.3. Kết quả giữ độ cao............................................................................................77
5.1.4. Kết quả giữ vận tốc và vị trí dùng Optical Flow..................................77
5.2. Hướng phát triển của đề tài......................................................................................79
5.2.1. Sử dụng RTOS.....................................................................................................80
5.2.2. Áp dụng bộ lọc Extended Kalman Filter và GPS..................................80
5.2.3. Tích hợp máy tính nhúng..............................................................................80
TÀI LIỆU THAM KHẢO.................................................................................................................82

7. DANH MỤC HÌNH VẼ
Hı̀nh 1.1.1 Máy bay phản lực chiến đấu Sukhoi Su-57.....................................................4
Hı̀nh 1.1.2 Quadcopter sử dụng mạch bay DJI Naza..........................................................5
Hı̀nh 1.2.1 Mạch điều khiển Pixhawk 4, đều có thể sử dụng firmware của
Ardupilot hoặc PX4 ..........................................................................................................................6
Hı̀nh 1.2.2 Ứng dụng Drone để phục vụ cho nông nghiệp của khoa Kỹ thuật
hàng không Đại học bách khoa TP HCM.................................................................................7
Hı̀nh 1.2.3 Drone quay phim và chụp hình DJI Phantom ................................................7
Hı̀nh 1.2.4 Luận văn hệ thống định vị trong nhà với độ chính xác 2cm của sinh
viên Bách Khoa HCM .......................................................................................................................8
Hı̀nh 1.3.1 Hệ thống quadcopter có tích hợp GPS ..............................................................9
Hı̀nh 2.1.1 Thành phần phần cứng đầy đủ của hệ thống..............................................11
Hı̀nh 2.1.2 Sơ đồ kết nối và giao tiếp các thành phần chính của quadcopter......12
Hı̀nh 2.2.1 Pin Phantom LiPo....................................................................................................13
Hı̀nh 2.2.2 Đồ thị điện áp và thời gian thể hiện đặc tuyến xả của pin LiPo 6C ...14
Hı̀nh 2.2.3 Động cơ BLDC ...........................................................................................................14
Hı̀nh 2.2.4 Bộ điều khiển ESC cho động cơ BLDC............................................................15
Hı̀nh 2.2.5 Cánh quạt cho quadcopter..................................................................................16
Hı̀nh 2.2.6 Bộ điều khiển DEVO 7 ...........................................................................................16
Hı̀nh 2.2.7 Bộ chuyển đổi cho RX DEVO7 PWM to PPM ...............................................17
Hı̀nh 2.2.8 Sơ đồ tín hiệu xung PPM ......................................................................................18
Hı̀nh 2.2.9 Khung quadcopter S500 Carbon Fiber Arms..............................................19
Hı̀nh 2.3.1 Cảm biến MPU6050................................................................................................20
Hı̀nh 2.3.2 Cảm biến từ trường HMC5883..........................................................................21
Hı̀nh 2.3.3 Giãn đồ đọc khoảng cách từ cảm biến sóng siêu âm HC-SR04............22
Hı̀nh 2.3.4 Cảm biến px4flow....................................................................................................23
Hı̀nh 2.3.5 Damping foam cho px4flow................................................................................23
Hı̀nh 2.3.6 Đồ thị kết quả calib px4flow trên trục y........................................................25

8. Hı̀nh 2.3.7 Hình mình hoạ sự thay đổi góc roll, pitch ảnh hưởng lên flow nhận
được.....................................................................................................................................................26
Hı̀nh 2.3.8 Sự ảnh hưởng của độ cao tới độ dịch chuyển thực tế.............................27
Hı̀nh 2.4.1 Mạch Schematics .....................................................................................................28
Hı̀nh 2.4.2 PCB lớp bottom........................................................................................................29
Hı̀nh 2.4.3 PCB lớp top.................................................................................................................30
Hı̀nh 2.4.4 Cả hai lớp ....................................................................................................................31
Hı̀nh 2.4.5 Lớp bottom mạch PCB đã gia công thực tế..................................................32
Hı̀nh 3.1.1 Mạch phát triển STM32F4 Discovery.............................................................33
Hı̀nh 3.1.2 Mô hình của hệ thống firmware dùng thư viện CMSIS trên nền vi
điều khiển ARM-Cortex M4 .......................................................................................................34
Hı̀nh 3.1.3 Các thư viện hỗ trợ lập trình chip STM32 và mức trừu tượng tương
ứng........................................................................................................................................................35
Hı̀nh 3.1.4 Sơ đồ khối NVIC minh họa ngắt GPIO trên vi điều khiểm ARM Cortex
M4.........................................................................................................................................................36
Hı̀nh 3.1.5 Một phần của bảng vector ngắt chip STM32F407VG..............................37
Hı̀nh 3.1.6 Chế độ chọn nhóm ngắt NVIC ............................................................................38
Hı̀nh 3.1.7 Mô hình kết nối phần cứng giao tiếp I2C......................................................39
Hı̀nh 3.1.8 Giãn đồ đọc dữ liệu px4flow dùng I2C...........................................................40
Hı̀nh 3.2.1 Quy ước hệ tọa độ BF và EF................................................................................42
Hı̀nh 3.2.2 Quy ước chiều và góc xoay..................................................................................43
Hı̀nh 3.2.3 Minh họa phép xoay dùng đại số quaternion .............................................46
Hı̀nh 3.2.4 Lưu đồ giải thuật Mahony Filter.......................................................................49
Hı̀nh 3.2.5 Đồ thị đáp ứng miền tần số của bộ lọc thông thấp...................................51
Hı̀nh 3.3.1 Hệ thống điều khiển động cơ BLDC ................................................................54
Hı̀nh 3.3.2 Quy ước số thứ tự và chiều quay của động cơ ...........................................58
Hı̀nh 4.1.1 Sơ đồ tổng quát bộ điều khiển cascade PID.................................................61
Hı̀nh 4.2.1 Sơ đồ bộ điều khiển góc roll, pitch, yaw........................................................62
Hı̀nh 4.2.2 Sơ đồ bộ điều khiển PID độ cao ........................................................................66

9. Hı̀nh 4.2.3 Hệ tọa độ phương x-y quadcopter...................................................................68
Hı̀nh 4.2.4 Chuyển đổi vận tốc từ BF sang RF ...................................................................69
Hı̀nh 4.2.5 Chuyển đổi tọa độ vị trí từ RF sang BF ..........................................................70
Hı̀nh 4.2.6 Sơ đồ bộ điều khiển vận tốc................................................................................70
Hı̀nh 4.2.7 Sơ đồ bộ điều khiển vị trí.....................................................................................72
Hı̀nh 5.1.1 Hệ thống quadcopter được thiết kế................................................................73
Hı̀nh 5.1.2 Mặt dưới của quadcopter ....................................................................................74
Hı̀nh 5.1.3 Hệ thống quadcopter đang hoạt động trên không ...................................75
Hı̀nh 5.1.4 Kết quả điều khiển góc roll, pitch.....................................................................76
Hı̀nh 5.1.5 Kết quả điều khiển tốc độ góc............................................................................76
Hı̀nh 5.1.6 Kết quả giữ độ cao dùng cảm biến siêu âm..................................................77
Hı̀nh 5.1.7 Kết quả giữ vị trí với px4flow............................................................................79
Hı̀nh 5.1.8 Kết quả điều khiển giữ vị trí với px4flow theo thời gian.......................79
Hı̀nh 5.2.1 Mô hình tích hợp máy tính nhúng Odroid trên bộ điều khiển bay
Pixhawk..............................................................................................................................................81

10. DANH MỤC BẢNG
Bảng 2.1 Các dữ liệu của px4flow i2c integral frame.....................................................25
Bảng 5.1 Bảng thông số bộ điều khiển roll, pitch, yaw .................................................76
Bảng 5.2 Bảng thông số bộ điều khiển độ cao...................................................................77
Bảng 5.3 Bảng thông số bộ điều khiển vận tốc và vị trí................................................78

11. DANH MỤC TỪ VIẾT TẮT
ARM: Acorn RISC Machine, dòng vi điều khiển của ARM Holdings
BF: Body Frame, hệ tọa độ gắn với vật
BLDC: Brushless DC Electric Motor, động cơ một chiều không chổi than
CMSIS: Cortex Microcontroller Software Interface Standard
DCM: Direction Cosine Matrix
EF: Earth Frame, hệ tọa độ gắn với mặt đất
ESC: Electronics Speed Controller, bộ điều khiển tốc độ điện tử
I2C: Inter-Integrated Circuit
INS: Indoor Navigation System, hệ thống định vị trong nhà
NVIC: Nested Vector Interrupt Controller, bộ quản lý ngắt lồng nhau
PID: Proportional Integral Derivative, khâu tỷ lệ - tích phân – vi phân
PPM: Pulse Position Modulation, điều chế xung theo vị trí
PWM: Pulse Width Modulation, điều chế xung theo độ rộng
RF: Reference Frame, hệ tọa độ tham chiếu
RTOS: Real Time Operating System, hệ điều hành thời gian thực
UAV: Unmanned Aerial Vehicle, phương tiện bay không người lái

12. KÝ HIỆU TOÁN HỌC VÀ CÁC ĐẠI LƯỢNG VẬT LÝ
𝜙, 𝜃, 𝜓 theo thứ tự là roll, pitch, yaw.
𝑠 = s𝛼 = sin(𝛼), 𝑐 = c𝛼 = cos(𝛼), 𝑡 = t𝛼 = tan(𝛼)
×: tích có hướng
torque: moment xoắn
thrust: lực gây nên chuyển động theo các phương tịnh tiến
𝑅 : ma trận chuyển từ hệ tọa độ A sang hệ tọa độ B
g: gia tốc trọng trường tại nơi hoạt động
m: khối lượng của vật

13. 1
TÓM TẮT LUẬN VĂN
Giới thiệu một số chủ đề quan trọng trong việc thiết kế hệ thống lập trình bằng
vi điều khiển STM32. Luận văn tập trung vào việc nghiên cứu thiết kế và điều
khiển UAV quadcopter, xây dựng mô hình toán để hiểu rõ nguyên lý hoạt động
và các thành phần có ảnh hưởng đến việc điều khiển quadcopter, giới thiệu bộ
điều khiển PID và cascade PID, sử dụng thuật toán điều khiển PID, trong đó
phần trọng tâm là sử dụng cảm biến PX4FLOW để điều khiển vận tốc vị trí hai
phương ngang. Cuối cùng trình bày kết quả chất lượng của hệ thống
quadcopter đã thiết kế bằng các số liệu thực tế, hướng phát triển của đề tài.
Luận văn được chia thành 8 chương, ở mỗi đầu chương đều có tóm tắt cụ thể
về nội dung tương ứng.
Chương 1. Giới thiệu:
Chương 2. Hệ thống cảm biến và phần cứng
Chương 3. Cơ sở lý thuyết
Chương 4. Thiết kế bộ điều khiển
Chương 5. Kết quả của hệ thống và hướng phát triển đề tài

14. 2
MỞ ĐẦU
Lời đầu tiên em xin cảm ơn tất cả các quý thầy cô đã dạy và hướng dẫn
em trong suốt 4 năm học đã qua. Quá trình học tập tại trường Đại học Bách
Khoa Tp Hồ Chính Minh đã cung cấp cho em rất nhiều kiến thức bổ ích quý giá
để tạo thành nền tảng vững chắc cho em trong cuộc sống và sự nghiệp sau này.
Trường Đại Học Bách Khoa là một môi trường tốt để học tập và phát
triển, nơi mà ta có thể gặp những con người sáng tạo, tài giỏi và ai cũng có phần
nào đó để mình cần học hỏi để hoàn thiện bản thân hơn, điều đó làm cho em
cảm thấy may mắn khi được học tập và rèn luyện tại ngôi trường này.
Luận văn này là cơ hội cho em tổng hợp lại tất cả những kiến thức của
mình đã học được và khả năng nghiên cứu giải quyết vấn đề em đã đạt được
trong quá trình học để giải quyết vấn đề. Em xin chân thành cảm ơn thầy
Huỳnh Thái Hoàng đã tận tình chỉ bảo và hướng dẫn em trong quá trình làm Đề
cương và nay là Luận văn. Thầy đã rất nhiệt tình giúp đỡ và chỉ bảo em trong
việc định hướng con đường đi đúng đắn và cách giải quyết các vấn đề khó khăn
gặp phải, cách làm việc có kế hoạch thích hợp. Và đồng thời em cũng xin cảm
ơn thầy Ngô Đình Trí đã hỗ trợ về thiết bị và kiến thức cho em. Bên cạnh đó em
cũng xin cảm ơn sự giúp đỡ của các người bạn em trong ngôi trường Bách Khoa
trong cả 4 năm học sát cánh cùng nhau và trưởng thành hơn trong môi trường
học tập tại Đại học Bách Khoa.
Em cũng xin dành lời cảm ơn này cho gia đình, bạn bè và thầy cô luôn ở
bên và hỗ trợ em trong cuộc sống và quá trình học tập tại trường.
Tp. Hồ Chí Minh, tháng 12 năm 2018
Nguyễn Đình Thông

15. 3

16. 4
Chương 1. Giới thiệu
Giới thiệu chung về UAV quadcopter và tình hình phát triển của đề tài hiện nay,
cũng như trình bày về ý nghĩa, nội dung và mục tiêu của luận văn thực hiện.
1.1. Giới thiệu chung về UAV và quadcopter
Hı̀nh 1.1.1 Máy bay phản lực chiến đấu Sukhoi Su-57
Việc nghiên cứu để thiết kế và điều khiển chính xác các hệ thống máy bay luôn
là một lĩnh vực được chú trọng trong ngành nghiên cứu robot và hàng không,
bởi các hệ thống máy bay nói chung được áp dụng rộng rãi trong các lĩnh vực
dân dụng, quân sự hay khoa học vũ trụ. Đặc biệt đối với thiết bị bay không
người lái (UAV), bởi các tính năng ưu việt của nó như: có khả năng hoạt động tự
động hoặc được điều khiển từ xa, khả năng hoạt động ở những nơi mà con
người khó tiếp cận trong một số lĩnh vực như quan sát núi lửa, quan trắc môi
trường, gieo trồng, phun thuốc trừ sâu nông nghiệp…
Quadcopter được sử dụng rất rộng rãi và tương lai sẽ được ứng dụng rất nhiều
trong các ứng dụng khác như: cứu hộ, vận chuyển hàng hóa, quay camera hành
trình, khảo sát địa hình,…

17. 5
Hı̀nh 1.1.2 Quadcopter sử dụng mạch bay DJI Naza
Quadcopter thuộc họ multi-copter và được coi là dạng multi-copter phổ biến
nhất, nó gồm có bốn cánh chong chóng, ưu điểm của UAV multi-copter so với
các hệ thống bay khác như máy bay trực thăng hay máy bay cánh bằng đó là dễ
điều khiển, tính ổn định cao, khả năng giữ vị trí đứng yên trong không gian tốt,
và khả năng cất cánh và đáp theo phương thẳng đứng (Vertical Take Off and
Landing).
1.2. Tình hình nghiên cứu về UAV
Dự án mã nguồn mở lớn hàng đầu về robot UAV hiện nay là: Ardupilot, PX4,
inav, paparazzi,… được xây dựng bởi những chuyên gia hàng đầu trong lĩnh vực
robot, linux, phần mềm, mô phỏng.

18. 6
Hı̀nh 1.2.1 Mạch điều khiển Pixhawk 4, đều có thể sử dụng firmware của
Ardupilot hoặc PX4
Điểm đặc biệt của các dự án mã nguồn mở đó là chúng ta có thể truy cập vào
mã nguồn của nó, từ đó có thể tham khảo và học hỏi những kiến thức để xây
dựng lên hệ thống riêng của mình.
Hiện nay, người ta tập trung vào xây dựng các ứng dụng của quadcopter vào
thực tế giúp ích cho cuộc sống. Ví dụ như các sinh viên, thầy cô trong trường
Đại học Bách Khoa TP. Hồ Chí Minh đã có một số thành công trong lĩnh vực chế
tạo và ứng dụng các thiết bị bay. Ví dụ như các giải thuật và hệ thống điều khiển
cân bằng quadcopter/hexacopter, thiết kế cơ khí và công suất để cho nó có thể
chịu tải trọng lớn và thời gian hoạt động tốt. Một số ví dụ như:

19. 7
Hı̀nh 1.2.2 Ứng dụng Drone để phục vụ cho nông nghiệp của khoa Kỹ thuật
hàng không Đại học bách khoa TP HCM
Hı̀nh 1.2.3 Drone quay phim và chụp hình DJI Phantom

20. 8
Hı̀nh 1.2.4 Luận văn hệ thống định vị trong nhà với độ chính xác 2cm của sinh
viên Bách Khoa HCM
1.3. Mục tiêu và nội dung của luận văn
1.3.1. Lý do chọn đề tài
Đề tài nghiên cứu và chế tạo mô hình máy bay quadcopter là một đề tài đòi hỏi
kiến thức tổng hợp của rất nhiều lĩnh vực như là: thiết kế cơ khí, động lực học,
khí động học, mạch điều khiển và lập trình vi điều khiển, xử lý cảm biến và áp
dụng các giải thuật điều khiển, giao tiếp máy tính và truyền nhận tín hiệu, xử lý
nhiễu… Nên đây là một đề tài có nhiều thử thách.
Khối lượng kiến thức khá nhiều do đó việc hiểu hoàn toàn hệ thống và điều
khiển tất cả các thành phần là rất khó, nên trong luận văn này sẽ tận dụng một
vài kết quả nghiên cứu để có thể định tính về vài yếu tố ảnh hưởng tới hệ
thống, và cũng sử dụng một số giả thiết cho việc đơn giản hóa bài toán điều
khiển.
Bộ điều khiển vận tốc và vị trí bằng Optical Flow là một đề tài hiện nay ở Việt
Nam chưa có nhiều người nghiên cứu, đa phần là chỉ sử dụng bộ điều khiển có
sẵn tích hợp Optical Flow với phần mềm của các hãng. Do vậy nội dung đề tài
có tính mới mẻ khi tích hợp giữa việc tự thiết kế bộ điều khiển trên STM32, và

21. 9
một cảm biến mới cho những ưu điểm nhất định cho việc điều khiển 3 góc và 3
trục của quadcopter.
1.3.2. Tính thiết thực của đề tài luận văn
1.3.2.1. Bộ điều khiển
Tuy hiện nay có rất nhiều bộ điều khiển sẵn có, có đầy đủ nhiều chức năng như
Pixhawk hay là PX4, việc tự thiết kế bộ điều khiển sẽ giúp hiểu hơn về giải
thuật điều khiển, cách áp dụng các giải thuật điều khiển, bộ lọc, đơn vị tính toán
cảm biến trên vi xử lý, phân tích thiết kế chương trình để hệ thống hoạt động
đúng, cách giải quyết các vấn đề nảy sinh trong quá trình làm.
Điều khiển động học và động lực học quadcopter là bước đầu tiên để có thể
phát triển một hệ thống ứng dụng vào các nhiệm vụ cụ thể mà có ý nghĩa thực
tiễn tận dụng lợi thế của loại robot bay trên không, như đã giới thiệu một vài
thành tựu ở (1.2)
1.3.2.2. Điều khiển quadcopter bằng Optical Flow
Hı̀nh 1.3.1 Hệ thống quadcopter có tích hợp GPS

22. 10
Hiện nay thiết bị định vị phổ biến nhất là GPS và hệ thống định vị trong nhà
(INS), tuy nhiên hệ thống GPS và INS cho kết quả giá trị vị trí và vận tốc ở một
khoảng sai số khá lớn (≥ 3𝑚) và ở các khu vực bị che chắn bởi cây cối, tòa nhà
trong nhà, có thể một lúc chỉ nhận được 4,5 vệ tinh hoặc không nhận được thì
giá trị hệ thống GPS không thể sử dụng.
Khắc phục những hạn chế của hệ thống GPS và INS, Optical Flow được dùng để
đo chính xác vận tốc của quadcopter.
Các hệ thống được đặt cố định để định vị quadcopter, như hệ thống POXYZ, hệ
thống xác định vị trí bằng VICON cần phải được lắp đặt, cài đặt mới có thể sử
dụng, và chỉ hoạt động trong môi trường không gian nhất định, đây chính là
nhược điểm mà hệ thống giữ vị trí bằng Optical Flow sẽ khắc phục.
1.3.3. Mục tiêu và nội dung
Áp dụng các giải thuật xử lý dữ liệu cảm biến, xây dựng phương trình động học
và động lực học cho quadcopter, từ đó thiết kế bộ điều khiển cho quadcopter
trên nền vi điều khiển ARM trên board STM32F407VG. Xây dựng hệ thống
quadcopter hoàn chỉnh, giữa phần cứng, phần điện điện tử, giao tiếp cảm biến
và phần mềm, tiếp theo xây dựng các phương trình toán cho quadcopter, xử lý
dữ liệu cảm biến bằng các thuật toán phù hợp trên vi điều khiển, từ đó thiết kế
các bộ điều khiển thích hợp cho hệ. Bay quadcopter thực tế và thu thập dữ liệu,
đánh giá chất lượng hệ thống từ đó áp dụng các kiến thức để hiệu chỉnh đạt
được kết quả mong muốn.
Mục tiêu kết quả: hiểu được bản chất chuyển động của quadcopter, điều khiển
hệ quadcopter cân bằng ổn định, giữ được độ cao và vị trí 2 phương ngang x,y.

23. 11
Chương 2. Hệ thống cảm biến và phần cứng
Chương này sơ lược thành phần phần cứng và giới thiệu các cảm biến được sử
dụng, sơ đồ kết nối và giao tiếp được sử dụng tương ứng. Ngoài ra còn trình
bày về đặc tính cần chú ý của cảm biến.
2.1. Sơ đồ kết nối của hệ thống
Hı̀nh 2.1.1 Thành phần phần cứng đầy đủ của hệ thống

24. 12
Hı̀nh 2.1.2 Sơ đồ kết nối và giao tiếp các thành phần chính của quadcopter
Dưới đây sẽ trình bày cụ thể về hệ thống phần cứng và cảm biến được mô tả
bằng hai sơ đồ ở trên được dùng cho mô hình quadcopter trong luận văn.
2.2. Phần cứng
2.2.1. Pin Lipo
Được sử dụng phổ biến cho các robot điều khiển từ xa. Đặc điểm nổi trội của
pin Lipo để nó được ứng dùng nhiều trong hệ thống là:
+ Pin có thể sạc.
+ Năng lượng dự trữ lớn so với các loại pin khác có khối lượng gần bằng nhau.
+ Dòng xả lớn.

25. 13
Hı̀nh 2.2.1 Pin Phantom LiPo
Pin Phamtom là loại pin Lipo 4 cells, dung lượng 4480mAh, điện áp cung cấp từ
[14.5𝑉 → 16.8𝑉]

26. 14
Hı̀nh 2.2.2 Đồ thị điện áp và thời gian thể hiện đặc tuyến xả của pin LiPo 6C
Ta thấy rằng khi chúng ta sử dụng năng lượng của pin Lipo thì điện áp của nó
giảm dần theo đồ thị Hình 3.3. Khi sử dụng pin Lipo cần chú ý về cách sử dụng
nếu không có thể gây ra hiện tượng pin phồng, chai pin, tuổi thọ pin giảm
nhanh, cháy nổ.
2.2.2. Động cơ BLDC
Hı̀nh 2.2.3 Động cơ BLDC
Động cơ được sử dụng là phantom, thông số kỹ thuật.

27. 15
Động cơ BLDC có nhiều ưu điểm so với các loại động cơ khác như: có sức kéo
tải trọng lớn, hình dáng nhỏ và nhẹ, quán tính thấp.
Thông số kỹ thuật động cơ BLDC A2312.
+ 800KV = 800 rpm/V
+ Điện áp hoạt động: 14.8V ( pin Lipo 4 cells ).
+ Thrust tối đa: 850 gram.
2.2.3. Bộ điều khiển ESC
Hı̀nh 2.2.4 Bộ điều khiển ESC cho động cơ BLDC
Là mạch công suất điều khiển động cơ BLDC, nhờ có ESC mà ta có thể khiển
động cơ BLDC như điều khiển động cơ servo chỉ với bằng xung PWM.
2.2.4. Cánh quạt
Để tạo ra lực đẩy nhờ khí động lực học thì hình dạng cánh quạt và chiều quay
phải phù hợp.

28. 16
Hı̀nh 2.2.5 Cánh quạt cho quadcopter
2.2.5. Tay cầm DEVO7
Hı̀nh 2.2.6 Bộ điều khiển DEVO 7
DEVO7 TX-RX là bộ truyền nhận sóng từ xa để có thể điều khiển quadcopter,
Devo 7 có khoảng cách xa ( tùy thuộc vào điều kiện hoạt động ) và kết nối giao
tiếp được đảm bảo tin cậy.

29. 17
2.2.6. Đọc tín hiệu PPM
Tín hiệu của điều khiển TX, RX là xung có chu kỳ T=20ms, và Ton (đơn vị
microseconds) trong mỗi chu kỳ là tín hiệu của kênh đó, PPM encoder là bộ
chuyển đổi tín hiệu PWM của nhiều kênh được gộp lại và điều chế thành xung
PPM.
Hı̀nh 2.2.7 Bộ chuyển đổi cho RX DEVO7 PWM to PPM
Bộ chuyển đổi sang PPM sẽ giúp tiết kiệm chân GPIO cho vi điều khiển sử dụng
vào các mục đích khác.

30. 18
Hı̀nh 2.2.8 Sơ đồ tín hiệu xung PPM
2.2.7. Khung quadcopter
Khung cơ khí quadcopter là phần thiết kế quan trọng để đảm bảo sự chắc chắn,
tính đối xứng để đảm bảo moment quán tính phân bố đều nhất có thể, khối
lượng không quá nặng, để có thể điều khiển quadcopter một cách dễ dàng.

31. 19
Hı̀nh 2.2.9 Khung quadcopter S500 Carbon Fiber Arms
2.3. Hệ thống cảm biến
2.3.1. Cảm biến vận tốc góc và gia tốc MPU6050.
MPU6050 tích hợp hai cảm biến vận tốc góc (gyroscope) và gia tốc kế
(accelerometer).
Thông số kỹ thuật
+ Đo vận tốc góc (gyroscope) full-scale range of ±250, ±500, ±1000, and
±2000°/sec (dps).
+ Gia tốc kế (accelerometer) full-scale range of ±2g, ±4g, ±8g, and ±16g.
+ Dữ liệu được lưu trong thanh ghi ADC 16 bits.
+ Giao tiếp cảm biến I2C.
Đặc tính cảm biến vận tốc góc gyroscope

32. 20
Hı̀nh 2.3.1 Cảm biến MPU6050
Vấn đề thường gặp phải nhất của gyro là drift (độ trôi) tích lũy chậm theo
thời gian. Nguyên nhân bởi vì do các tác động cơ khí, rung động tác động
lên gyro, sau một thời gian sử dụng thì giá trị trôi này tích lũy lên đáng kể,
làm giá trị đo góc không còn chính xác.
Dù vậy, điểm mạnh của gyro là ít bị nhiễu hơn gia tốc kế, nghĩa là giá trị
tức thời của nó đáng tin cậy.
Đặc tính cảm biến gia tốc kế
Gia tốc kế luôn có offset trên mỗi trục làm cho giá trị đo được thường lệch
đi so với thực tế một chút. Ngoài ra, giá trị đo từ cảm biến thường nhạy
với rung động cơ khí dù rất nhỏ khiến cho giá trị tức thời của nó không
đáng tin cậy, do đó chúng ta chỉ sử dụng giá trị trung bình của acc thì nó
mới có hữu hiệu, điểm mạnh của gia tốc kế là nếu để lâu dài thì không bị
trôi như gyroscope, do vậy gia tốc kế có vai trò hiệu chỉnh lại độ trôi của
gyro trong thuật toán fuse dữ liệu cho bộ ước lượng trạng thái.

33. 21
2.3.2. Cảm biến từ trường HMC5883
Hı̀nh 2.3.2 Cảm biến từ trường HMC5883
+ Độ phân giải 5 milli-gauss
+ Giao tiếp I2C
Cảm biến từ trường đo từ trường của Trái Đất từ đó xác định được hướng, từ
trường của Trái Đất ở những vùng khác nhau có đặc tính khác nhau, do vậy khi
mua các loại cảm biến từ trường ta cần phải calib chúng trước khi sử dụng.
Cảm biến từ trường khi được gắn ở gần các thiết bị điện tử rất dễ bị nhiễu, đặc
biệt là gần các động cơ, bởi vì từ trường sinh ra bởi các động cơ thay đổi liên
tục trong quá trình hoạt động, do vậy cảm biến từ trường cần được đặt xa các
thiết bị điện càng tốt.
2.3.3. Cảm biến độ cao dùng sóng siêu âm HC-SR04
+ Tầm hoạt động đo khoảng cách: 2cm – 400cm
+ Tần số cập nhật, cỡ 18Hz
Độ chính xác của cảm biến siêu âm cao nên ta chỉ cần dùng bộ lọc bù là có thể
sử dụng dữ liệu để sử dụng.

34. 22
Hı̀nh 2.3.3 Giãn đồ đọc khoảng cách từ cảm biến sóng siêu âm HC-SR04
Tuy nhiên cảm biến HC-SR04, ultrasonic sensor có đặc tính là ảnh hưởng lớn
bởi nhiễu cơ khí, nên giá trị chỉ đúng ở khoảng cách gần khi gắn lên khung
quadcopter (từ 2cm – 100 cm).
2.3.4. Cảm biến Optical Flow PX4FLOW
2.3.4.1. Giới thiệu và thông số kỹ thuật của PX4FLOW
Sản phẩm nghiên cứu của ETH Zurich, ưu điểm của nó là có thể hoạt động
trong môi trường ánh sáng thấp.
PX4FLOW là một dự án mã nguồn mở, do đó ta có thể chỉnh sửa code ( thay đổi
thuật toán, truy cập trực tiếp cảm biến ), build và nạp lại tuy theo nhu cầu của
mình
Thông số kỹ thuật
168 MHz Cortex M4F CPU (128 + 64 KB RAM)
Cảm biến hình ảnh 752x480 MT9V034.
L3GD20 3D Gyroscope, full scale lên tới 2000°/s với tần số cập nhật 780Hz,
mặc định được chọn ở chế độ 500°/s.

35. 23
Giao tiếp hỗ trợ:
+ I2C.
+ UART, MAVLink package.
+ USB.
Tích hợp cảm biến sonar MB1043. Độ phân giải 1mm và tầm hoạt động từ
300mm đến 4000mm
Hı̀nh 2.3.4 Cảm biến px4flow
PX4FLOW được cố định bên dưới quadcopter được gắn damping foam để hạn
chế ảnh hưởng của rung động cơ khí do quadcopter khi hoạt động gây ra.
Hı̀nh 2.3.5 Damping foam cho px4flow
Các dữ liệu trả về của px4flow khi đọc integral frame tại địa chỉ 0x16

36. 24
Dữ liệu Diễn giải
int16_t pixel_flow_x_integral accumulated flow in radians*10000
around x axis since last I2C readout
[rad*10000]
int16_t pixel_flow_y_integral accumulated flow in radians*10000
around y axis since last I2C readout
[rad*10000]
int16_t gyro_x_rate_integral accumulated gyro x rates in
radians*10000 since last I2C readout
[rad*10000]
int16_t gyro_y_rate_integral accumulated gyro y rates in
radians*10000 since last I2C readout
[rad*10000]
int16_t gyro_z_rate_integral accumulated gyro z rates in
radians*10000 since last I2C readout
[rad*10000]
uint32_t integration_timespan accumulation timespan in
microseconds since last I2C readout
[microseconds]
uint32_t sonar_timestamp time since last sonar update
[microseconds]
int16_t ground_distance Ground distance in meters*1000
[meters*1000]
uint8_t quality averaged quality of accumulated flow
values [0:bad quality;255: max

37. 25
quality]
Bảng 2.1 Các dữ liệu của px4flow i2c integral frame
2.3.4.2. Calib cảm biến của px4flow
Trên thực tế dù nhà sản xuất đã tối ưu thiết kế và thông số để có kết quả tốt
nhất, nhưng có thể trong quá trình sản xuất không hoàn hảo nên ta cần phải
calib để px4flow cho kết quả tốt nhất.
Khi ta nghiêng quadcopter, đồng thời nghiêng luôn PX4FLOW smart camera
nhưng vẫn giữ nguyên vị trí của nó, thì qua công thức thì vận tốc của nó trong
hệ tọa độ body phải bằng 0.
Hı̀nh 2.3.6 Đồ thị kết quả calib px4flow trên trục y
Kết quả calib thành công khi flow và gyro rate có cùng giá trị.
2.3.4.3. Giải thuật xử lý giữ liệu của px4flow
Tính flow thực tế bằng các bù gyro

38. 26
Hı̀nh 2.3.7 Hình mình hoạ sự thay đổi góc roll, pitch ảnh hưởng lên flow nhận
được
flow_x, flow_y được tính nhờ so sánh sự các biệt giữa các frame ảnh liên tiếp
nhau với tốc độ cực cao bằng thuật toán SAD, khi hoạt động, px4flow được gắn
trên quadrotor có thể bị rung lắc, làm kết quả hình ảnh so sánh khác so với khi
px4flow cố định, px4flow được tích hợp sẵn cảm biến gyroscope để phát hiện
chuyển động 3 trục, và bù lại sự nhiễu động này.
𝑝𝑖𝑥𝑒𝑙 = 𝑓𝑙𝑜𝑤 − 𝑔𝑦𝑟𝑜
𝑝𝑖𝑥𝑒𝑙 là số điểm ảnh thực tế di chuyển sau khi bù ảnh hưởng của gyroscope.
Bù độ cao

39. 27
Hı̀nh 2.3.8 Sự ảnh hưởng của độ cao tới độ dịch chuyển thực tế
Hình bên trái, khi ở khoảng cách gần (1 mét so với mặt đất), cùng một lượng
dịch chuyển là 10cm, nhưng nó sẽ nhận được lượng flow nhiều hơn so với khi ở
xa (2 mét so với mặt đất).
Khi camera px4flow dịch chuyển được 1 lượng pixcel nhất định, thì nếu càng ở
xa mặt phẳng đang nhìn, cùng với lượng khác biệt thì độ dịch chuyển thực tế
càng lớn, do đó ta bù thêm độ cao.
𝑑𝑖𝑠𝑡𝑎𝑛𝑐𝑒 = 𝑝𝑖𝑥𝑒𝑙 ∗ 𝑎𝑙𝑡𝑖𝑡𝑢𝑑𝑒
Để tính vận tốc đã dịch chuyển, ta tính thêm chu kỳ giữa 2 lần lấy mẫu px4flow
đó là timespan.
𝑣𝑒𝑙𝑜𝑐𝑖𝑡𝑦 =
𝑑𝑖𝑠𝑡𝑎𝑛𝑐𝑒
𝑡𝑖𝑚𝑒𝑠𝑝𝑎𝑛
2.4. Mạch Altium
Mạch PCB được thiết kế nhằm cho các kết nối phần cứng chắc chắn và các kết
nối được đảm bảo một cách tốt nhất khi quadcopter hoạt động. Và đồng thời có
thể thay thế bo mạch điều khiển và phần cứng khi hư hỏng dễ dàng.

40. 28
2.4.1. Sơ đồ mạch Schematics
Dựa trên kết nối đã được trình bày ở mục 3.1
Hı̀nh 2.4.1 Mạch Schematics

41. 29
2.4.2. Sơ đồ mạch PCB
Hı̀nh 2.4.2 PCB lớp bottom

42. 30
Hı̀nh 2.4.3 PCB lớp top

43. 31
Hı̀nh 2.4.4 Cả hai lớp

44. 32
Hı̀nh 2.4.5 Lớp bottom mạch PCB đã gia công thực tế
Chương 3. Cơ sở lý thuyết
3.1. Lập trình vi điều khiển ARM
Chương 2 giới thiệu về mạch phát triển STM32F407 và tập trung vào những
phần kiến thức quan trọng cần hiểu, để có thể thiết kế được một hệ thống trên
nền vi điều khiển chip ARM Cortex-M4, từ đó xây dựng hệ thống firmware sử
dụng trong luận văn này.

45. 33
3.1.1. Bo mạch STM32F407 Discovery vi điều khiển 32 bits ARM Cortex
M4
STM32F407VG là mạch phát triển của STMicroeletronics với mục đích hướng
đến người bắt đầu tìm hiểu về dòng vi điều khiển cao cấp 32 bits ARM-Cortex
M4, mạch phát triển được tích hợp đầy đủ các ngoại vi đi kèm tài liệu thuận
tiện cho việc học, nghiên cứu, prototype và áp dụng vào thực tế nhanh chóng và
thuận tiện.
Hı̀nh 3.1.1 Mạch phát triển STM32F4 Discovery
Thông số kỹ thuật
+ Điện áp, nguồn clock: 1.8 V – 3.6V, 4 Mhz – 26Mhz thạch anh ngoài.
+ Bộ nhớ: 1 Mbyte bộ nhớ flash, 192 Kbyte SRAM + 4 Kbyte SRAM.
+ Đơn vị bao vệ bộ nhớ (Memory Protection Unit)
+ Bộ tạo giá trị ngẫu nhiên (Random Number Generator)
+ Real Time Clock
+ Chế độ standby tiết kiệm năng lượng

46. 34
+ ADC: 12 bit, 10 bit, 8 bit hoặc 6 bit, DAC: 12 bit
+ Trình xử lý ngắt NVIC, cấu hình 16 bộ DMA, 8 kênh trên mỗi DMA
+ Timer: 12 timers 16 bits, 2 timers 32 bit với nhiều chế độ hoạt động như
Input Capture, Output Compare, PWM
+ Hỗ trợ các chuẩn debug JTAG và SWD (Serial Wire Debug)
+ Cortex-M4F Embedded Trace Macrocell
Hỗ trợ các giao thức truyền thông:
+ Giao tiếp nối tiếp: I2C, USART, SPI, I2S, CAN (2.0B), SDIO, USB 2.0, Ethernet
+ Giao tiếp song song FSMC: Flash, SRAM, PC Card…
3.1.2. Lập trình trên vi điều khiển ARM Cortex M4
3.1.2.1. Thư viện lập trình
CMSIS là thư viện chuẩn do ARM viết ra được dùng chung cho các loại dòng
chip được phát triển dựa trên dòng ARM cụ thể (ví dụ như ARM-Cortex M3,
ARM-Cortex M4), đa phần các hãng sản xuất chip vi điều khiển đều dòng ARM
đều viết thư viện dựa trên CMSIS này.
Hı̀nh 3.1.2 Mô hình của hệ thống firmware dùng thư viện CMSIS trên nền vi
điều khiển ARM-Cortex M4

47. 35
Các dòng chip được thiết kế bởi STMicroeletronics như STM32F407VG có thư
viện hỗ trợ chính là Standard Peripheral Library (SPL) và Cube HAL, đều
được viết dựa trên thư viện CMSIS của ARM. Trong luận văn này sử dụng SPL
vì lớp trừu tường (abstraction) của nó thấp, đơn giản cho việc sử dụng và
debug phần mềm.
Hı̀nh 3.1.3 Các thư viện hỗ trợ lập trình chip STM32 và mức trừu tượng tương
ứng
3.1.2.2. Hệ thống ngắt trên vi điều khiển ARM Cortex M4
i. Khối quản lý ngắt NVIC controller
Hệ thống ngắt trên vi điều khiển ARM Cortex bao gồm các loại ngắt: ngắt hệ
thống (exception), ngắt GPIO và ngắt phần mềm. Ví dụ:
Ngắt hệ thống: Khi có sự truy cập vào vùng nhớ không được cho phép, thì
chương trình sẽ tạo ra ngắt MemManage_Handler.
Ngắt GPIO: 1 chân GPIO PA3 có sự chuyển tiếp từ mức thấp lên cao tạo ra ngắt.
Ngắt phần mềm: Khi timer 1 bị tràn tạo ra ngắt.

48. 36
Hı̀nh 3.1.4 Sơ đồ khối NVIC minh họa ngắt GPIO trên vi điều khiểm ARM Cortex
M4
Các ngắt đều được nối qua khối NVIC, NVIC có nhiệm vụ quản lý tất cả các ngắt
của hệ thống, quyết định ngắt nào sẽ được thực thi trước khi có nhiều ngắt xảy
ra cùng lúc, dựa trên cài đặt mức độ ưu tiên của người dùng.

49. 37
Hı̀nh 3.1.5 Một phần của bảng vector ngắt chip STM32F407VG
Mỗi loại ngắt đều có một địa chỉ trên bản đồ bộ nhớ trong Flash (Flash memory
map). Là nơi để cho khối NVIC controller biết được loại ngắt và địa chỉ chương
trình tương ứng của ngắt đó.
Sau khi NVIC đã quyết định được loại ngắt cần được thực hiện: NVIC sẽ lưu lại
trạng thái của hệ thống (lưu lại thanh ghi của vi điều khiển) tại thời điểm chuẩn
bị chuyển ngắt, sau đó buộc vi điều khiển thực thi ngắt bằng cách tra bảng
vector ngắt, khi đã thực thi xong tất cả cả ngắt thì phục hồi trạng thái và tiếp
tục thực thi chương trình chính.
ii. Mức độ ưu tiên của ngắt
NVIC trên chip ARM được dùng cho STM32F4 hỗ trợ cài đặt 16 mức độ ưu tiên
của các ngắt.

50. 38
Hı̀nh 3.1.6 Chế độ chọn nhóm ngắt NVIC
Số n quyết định có bao nhiêu mức cấu hình ngắt cho Preemption Priority và
Sub-priority.
Quy tắc hoạt động của mức độ ưu tiên ngắt như sau:
1. Khi cả hai ngắt có cùng mức độ ưu tiên
Ngắt nào xảy ra trước sẽ được thực thi trước
2. Hai ngắt khác Preemption Priority
Preemption Priority quyết định ngắt A có được phép thực thi khi ngắt B đang
hoạt động hay không.
Nếu ngắt A có Preemption Priority lớn hơn B, thì nó sẽ được NVIC ưu tiên hơn
so với ngắt B trong mọi trường hợp, nghĩa là ngắt B đang hoạt động, nhưng
ngắt A yêu cầu thực thi thì NVIC sẽ cho phép dừng ngắt B, thực thi ngắt A trước
và sau đóvề thực thi tiếp ngắt B.
3. Hai ngắt khác Sub-priority

51. 39
Khi có một ngắt A đang được thực thi (với Preemption Priority lớn hơn B, C),
cùng lúc đó hai ngắt B và C có cờ báo cần thực thi, thì sub-priority sẽ quyết định
2 ngắt đang chờ này cái nào sẽ được thực thi trước
3.1.2.3. Giao tiếp I2C
Giao tiếp I2C được sử dụng phổ biến trong việc giao tiếp với các cảm biến.
Thông thường vi điều khiển sẽ là master device, và cảm biến là slave device.
Hı̀nh 3.1.7 Mô hình kết nối phần cứng giao tiếp I2C
i. Đặc điểm, ưu điểm và nhược điểm chính của giao tiếp I2C
+ Điện áp hoạt động 5V, ở 2 đường dây SCL và SDA có điện trở kéo lên mức cao.
+ Chỉ sử dụng 2 dây SCL và SDA để giao tiếp với nhiều slave một lúc, nếu có 2
thiết bị trùng địa chỉ thì người ta sẽ giải quyết bằng cách thiết kế thêm 1 chân ở
slave để đổi địa chỉ của nó tùy thuộc vào trạng thái bit này.
+ Hạn chế của việc dùng chung 1 bộ I2C để giao tiếp với nhiều cảm biến đó là
master chỉ có thể giao tiếp với các slave một cách tuần tự.
+ Tốc độ của giao tiếp I2C khá cao, từ 100kbit/s đến 400kbit/s tùy thuộc chế
độ hoạt động.
ii. Cách sử dụng I2C để giao tiếp với cảm biến
Để có thể giao tiếp I2C một cách chuẩn nhất thì ta nên đọc tài liệu của hãng sản
xuất cảm biến cần giao tiếp, để biết được các khoảng thời gian giữa các xung
chân SCL, SDA.

52. 40
Hı̀nh 3.1.8 Giãn đồ đọc dữ liệu px4flow dùng I2C
Cách gửi lệnh đọc từ cảm biến có địa chỉ 0x42 thanh ghi 0x16 dùng giao tiếp
I2C như sau, với | là dấu hiệu ngăn cách giữa các lệnh:
1. Truyền lệnh giao tiếp cảm biến có địa chỉ 0x42 tại thanh ghi có địa chỉ 0x16
START condition | 0x42<<1 | 0x16 | ACK | STOP condition
2. Truyền lệnh đọc giữ liệu được gửi từ cảm biến
START condition | (0x42<<1+0x01 )| data | ACK | STOP condition
Trong đó, data là giữ liệu được gửi từ cảm biến về vi điều khiển, ta phải liên tục
kiểm tra thanh ghi dữ liệu I2C và tiếp tục gửi các bit ACK sau khi đã nhận dữ
liệu. Và sau khi đã nhận đủ thì tạo xung STOP để dừng giao tiếp.
Code C để thực hiện lệnh đọc dữ liệu qua giao tiếp I2C như sau.

53. 41
u8 IICreadBytes(u8 dev, u8 reg, u8 length, u8 *data)
{
u8 count = 0;
IIC_Start();
IIC_Send_Byte(dev);
IIC_Wait_Ack();
IIC_Send_Byte(reg);
IIC_Wait_Ack();
IIC_Stop();
delay_us(5);
IIC_Start();
IIC_Send_Byte(dev + 1);
IIC_Wait_Ack();
delay_us(6);
for(count = 0; count < length; count++)
{
if(count != length - 1) {
data[count] = IIC_Read_Byte(1);
delay_us(3);
}
else data[count] = IIC_Read_Byte(0);
}
IIC_Stop();//²úÉúÒ»¸öÍ£Ö¹Ìõ¼þ
return count;
}
3.2. Đơn vị ước lượng trạng thái
Đơn vị ước lượng trạng thái là phần quan trọng cho việc điều khiển một hệ
thống UAV, để xây dựng đơn vị ước lượng trạng thái chúng ta cần có kiến thức

54. 42
về toán đặc biệt là về phép xoay trong không gian 3 chiều, giải thuật bộ lọc xử
lý tín hiệu.
Nội dung trong chương này là giới thiệu các kiến thức toán học cho phép xoay
trong không gian ba chiều, trọng tâm là lý thuyết đại số quaternion. Trình bày ý
tưởng và giải thuật bộ lọc Mahony cho việc ước lượng trạng thái của hệ thống,
và giới thiệu về hai loại bộ lọc thông thấp phổ biến là bộ lọc bù và bộ lọc trung
bình.
3.2.1. Quy ước hệ trục tọa độ
Hı̀nh 3.2.1 Quy ước hệ tọa độ BF và EF
Chọn hệ tọa độ như hình, hệ tọa độ quán tính (Earth Frame – EF) cố định
𝑋 𝑌 𝑍 , hệ tọa độ body luôn luôn gắn với quadcopter 𝑋 𝑌 𝑍 .
Gốc tọa độ BF trùng với trọng tâm của quadcopter.

55. 43
Hı̀nh 3.2.2 Quy ước chiều và góc xoay
Chiều z hướng xuống, x hướng trùng trục xoay góc 𝜃, y hướng trùng trục xoay
góc 𝜙. Chiều dương các góc xoay được quy ước theo quy tắc nắm tay phải.
3.2.2. Quaternion
Các thuật toán ước lượng trang thái trong không gian ba chiều thường được
xây dựng trên hai cách biểu diễn DCM hoặc là quaternion, bởi vì đây là hai cách
biểu diễn chính xác cho mô tả định hướng và các phép xoay trong không gian
ba chiều.
Ưu điểm của quaternion so với Direction Cosine Matrix.
Phép xoay trong không gian ba chiều được biểu diễn bởi ma trận trực giao 3x3,
quaternion chỉ dùng 4x1. Do đó khối lượng tính toán thực hiện là ít hơn, do đó
ít tốn thời gian và tài nguyên vi xử lý hơn.
Sau mỗi bước cập nhật trạng thái, dùng DCM chúng ta cần phải thực hiện phép
toán để đảm bảo tính trực giao của 3 vectors biểu diễn tương ứng, sẽ gây ra các

56. 44
vấn đề như, tốn tài nguyên và thời gian của vi điều khiển, tích lũy sai số tính
toán. Với quaternion chúng ta không cần phải lo đảm bảo tính trực giao như
DCM nữa, và với mỗi lần cập nhật quaternion trạng thái, ta chỉ cần chuẩn hóa
nó về quaternion đơn vị.
3.2.2.1. Đại số về quaternion
i. Định nghĩa và các phép toán cơ bản
Một quaternion là một dạng số phức đại lượng có hai phần chính là phần thực
𝑞 , và phần vector 𝑞
⃗.
𝑞 = [𝑞
⃗, 𝑞 ] = 𝑞 𝑖 + 𝑞 𝑗 + 𝑞 𝑘 + 𝑞
𝑖, 𝑗, 𝑗 được gọi là các thành phần ảo quaternion.
Tính chất giữa các thành phần ảo.
𝑖 = 𝑘 = 𝑘 = 𝑖𝑗𝑘 = −1
Từ đó suy ra các phương trình sau
𝑖𝑗 = 𝑘 = −𝑗𝑖
𝑗𝑘 = 𝑖 = −𝑘𝑗
𝑘𝑖 = 𝑗 = −𝑖𝑘
Độ lớn của quaternion
‖𝑞‖ = 𝑞 + 𝑞 + 𝑞 + 𝑞
Quaternion liên hợp
𝑐𝑜𝑛𝑗(𝑞) = 𝑞∗
= [𝑞
⃗, 𝑞 ]∗
= −𝑞 𝑖 − 𝑞 𝑗 − 𝑞 𝑘 + 𝑞
Quaternion chuẩn hóa.
𝑛𝑜𝑟𝑚(𝑞) =
𝑞
‖𝑞‖
Quaternion nghịch đảo.

57. 45
𝑖𝑛𝑣(𝑞) = 𝑞 =
𝑞∗
‖𝑞‖
Cho 2 quaternion q và p có dạng.
𝑞 = [𝑞
⃗, 𝑞 ] = 𝑞 𝑖 + 𝑞 𝑗 + 𝑞 𝑘 + 𝑞
𝑝 = [𝑝
⃗, 𝑝 ] = 𝑝 𝑖 + 𝑝 𝑗 + 𝑝 𝑘 + 𝑝
Phép cộng hai quaternion
𝑝 + 𝑞 = [𝑞
⃗, 𝑞 ] + [𝑝
⃗, 𝑝 ] = [𝑝
⃗ + 𝑞
⃗, 𝑞 + 𝑝 ]
= (𝑞 + 𝑝 )𝑖 + (𝑞 + 𝑝 )𝑗 + (𝑞 + 𝑝 )𝑘 + (𝑞 + 𝑝 )
Phép nhân quaternion
𝑞⨂𝑝 =
𝑞 −𝑞 −𝑞 −𝑞
𝑞 𝑞 −𝑞 𝑞
𝑞 𝑞 𝑞 −𝑞
𝑞 −𝑞 𝑞 𝑞
𝑝
𝑝
𝑝
𝑝
= 𝐿(𝑞)𝑝
=
𝑝 −𝑝 −𝑝 −𝑝
𝑝 𝑝 𝑝 −𝑝
𝑝 −𝑝 𝑝 𝑝
𝑝 𝑝 −𝑝 𝑝
𝑞
𝑞
𝑞
𝑞
= 𝑅(𝑝)𝑞
ii. Phép toán xoay dựa trên quaternion
Theo định lý xoay của Euler (Euler's rotation theorem), mọi sự thay đổi định
hướng của vật rắn trong không gian 3 chiều từ trạng thái này sang trạng thái
khác đều có thể biểu diễn bằng một phép xoay 1 góc 𝜑 quanh 1 vector đơn vị 𝑢
⃗.

58. 46
Hı̀nh 3.2.3 Minh họa phép xoay dùng đại số quaternion
Phép xoay này được biểu diễn bởi 1 quaternion xoay như sau.
𝑞 = [𝑞
⃗, 𝑞 ] = 𝑢
⃗𝑠𝑖𝑛
𝜑
2
+ 𝑐𝑜𝑠
𝜑
2
Một vector trong không gian 3 chiều có tọa độ [𝑝 ; 𝑝 ; 𝑝 ] được biểu diễn là
quaternion thuần 𝑝 = 𝑝 𝑖 + 𝑝 𝑗 + 𝑝 𝑘 + 0
Trạng thái mới của 𝑝 sau khi áp dụng phép xoay 1 góc 𝜑 quanh 1 vector đơn vị
𝑢
⃗, là 𝑝′, có công thức tính là:
𝑝 = 𝑞𝑝𝑞
Công thức này có thể được chứng minh nhờ kết quả công thức xoay của
Rodrigues.
iii. Ma trận xoay dựa trên quaternion
Cũng tương tự như ma trận xoay từ DCM, từ công thức phép toán xoay ở trên ta
xây dựng ma trận xoay dựa trên quaternion.
𝑝 = 𝑞𝑝𝑞 = 𝑅𝑝

59. 47
𝑅 =
1 − 2(𝑞 + 𝑞 ) 2(𝑞 𝑞 − 𝑞 𝑞 ) 2(𝑞 𝑞 + 𝑞 𝑞 )
2(𝑞 𝑞 + 𝑞 𝑞 ) 1 − 2(𝑞 + 𝑞 ) 2(𝑞 𝑞 − 𝑞 𝑞 )
2(𝑞 𝑞 − 𝑞 𝑞 ) 2(𝑞 𝑞 + 𝑞 𝑞 ) 1 − 2(𝑞 + 𝑞 )
Ma trận này thể hiện sự liên hệ giữa biểu diễn quaternion và DCM, dó vậy ta có
thể sử dụng một số kết quả của DCM cho việc tính toán và xây dựng phương
trình dùng quaternion.
iv. Chuyển đổi giữa quaternion và các góc Euler
Chuyển đổi từ quaternion sang góc Euler
Từ quaternion biểu diễn định hướng, ta có công thức sau để chuyển thành các
góc trong không gian.
𝜓
𝜃
𝜙
=
𝑎𝑡𝑎𝑛2(2(𝑞 𝑞 + 𝑞 𝑞 ), 2𝑞 + 2𝑞 − 1)
asin(−2𝑞 𝑞 + 2𝑞 𝑞 )
𝑎𝑡𝑎𝑛2(2(𝑞 𝑞 + 𝑞 𝑞 ), 2𝑞 + 2𝑞 − 1)
Chuyển đổi từ các góc Euler sang quaternion
Từ định hướng trong không gian được biểu diễn bởi 3 góc Euler, công thức
chuyển sang quaternion tương ứng là:
𝑞
𝑞
𝑞
𝑞
=
⎣
⎢
⎢
⎢
⎢
⎢
⎢
⎡𝑐
𝜙
2
𝑐
𝜃
2
𝑐
𝜓
2
+ 𝑠
𝜙
2
𝑠
𝜃
2
𝑠
𝜓
2
𝑠
𝜙
2
𝑐
𝜃
2
𝑐
𝜓
2
− 𝑐
𝜙
2
𝑠
𝜃
2
𝑠
𝜓
2
𝑐
𝜙
2
𝑠
𝜃
2
𝑐
𝜓
2
+ 𝑠
𝜙
2
𝑐
𝜃
2
𝑠
𝜓
2
𝑐
𝜙
2
𝑐
𝜃
2
𝑠
𝜓
2
− 𝑠
𝜙
2
𝑠
𝜃
2
𝑐
𝜓
2 ⎦
⎥
⎥
⎥
⎥
⎥
⎥
⎤
3.2.2.2. Phương trình động học dưới dạng quaternion
𝑞̇ =
1
2
𝑞⨂𝑝(𝜔 )
Với 𝜔 là vận tốc góc trong BF, và các đại lượng:

60. 48
𝑞 = 𝑞 𝑖 + 𝑞 𝑗 + 𝑞 𝑘 + 𝑞
𝑝(𝜔 ) = 𝜔 𝑖 + 𝜔 𝑖 + 𝜔 𝑖 + 0
𝑞̇ =
𝑞̇
𝑞̇
𝑞̇
𝑞̇
Suy ra
𝑞̇ =
1
2
𝑞 −𝑞 𝑞
𝑞 𝑞 −𝑞
−𝑞 𝑞 𝑞
−𝑞 −𝑞 −𝑞
𝜔𝑥
𝜔𝑦
𝜔𝑧
hoặc viết ở dạng khác
𝑞̇ =
1
2
⎣
⎢
⎢
⎢
⎡
0 𝜔 −𝜔 𝜔
−𝜔 0 𝜔 𝜔
𝜔 −𝜔 0 𝜔
−𝜔 −𝜔 −𝜔 0 ⎦
⎥
⎥
⎥
⎤
𝑞
𝑞
𝑞
𝑞
(3. 1)
3.2.3. Giải thuật Mahony Filter
Từ lý thuyết về quaternion ở mục (3.2.2), và các nghiên cứu của Mahony, bộ
lọc Mahony Filter được dựa trên ý tưởng sai số đo từ cảm biến gyroscope sẽ
được tính toán qua bộ điều khiển PI, với đầu vào được tính toán dựa vào gia tốc
kế và từ trường kế, trạng thái quaternion bước trước đó.

61. 49
Hı̀nh 3.2.4 Lưu đồ giải thuật Mahony Filter
Giải thuật Mahony Filter như sau:
i. Tính thành phần sai số hiệu chỉnh tốc độ góc từ gia tốc
Chuẩn hóa gia tốc kế từ giá trị cảm biến.
𝑎 =
𝑎
𝑎
𝑎
=
𝑎
𝑎 + 𝑎 +𝑎
Tính vector gia tốc từ quaternion bước trước đó.
𝑣 =
𝑣
𝑣
𝑣
=
2(𝑞 𝑞 − 𝑞 𝑞 )
2(𝑞 𝑞 + 𝑞 𝑞 )
𝑞 𝑞 − 𝑞 𝑞 − 𝑞 𝑞 + 𝑞 𝑞
Sai số hiệu chỉnh tốc độ góc từ gia tốc kế là tích có hướng của vector gia tốc kế
và vector gia tốc của trạng thái hiện tại.
𝑒𝑟𝑟𝑜𝑟 = 𝑎 × 𝑣
ii. Tính thành phần sai số hiệu chỉnh từ trường kế
Chuẩn hóa từ trường kế từ giá trị cảm biến.

62. 50
𝑚 =
𝑚
𝑚
𝑚
=
𝑚
𝑚 + 𝑚 +𝑚
Tính vector 𝑤, vector từ trường ước lượng.
𝑤 = 𝑤(𝑞, 𝑏(ℎ(𝑞, 𝑚 )))
Sai số hiệu chỉnh tốc độ góc từ từ trường kế là tích có hướng của vector từ
trường từ cảm biến và vector từ trường được ướng lượng .
𝑒𝑟𝑟𝑜𝑟 = 𝑚 × 𝑤
iii. Tính sai số hiệu chỉnh cho vận tốc góc (gyroscope)
Từ sai số vận tốc góc tính được từ gia tốc và từ trường. Tổng sai số:
𝑒𝑟𝑟𝑜𝑟 =
𝑒
𝑒
𝑒
= 𝑒𝑟𝑟𝑜𝑟 + 𝑒𝑟𝑟𝑜𝑟
Sai số này qua bộ điều khiển PI như trên sơ đồ Hình 3.2.2, ngõ ra của bộ điều
khiển PI là lượng để hiệu chỉnh lại tín hiệu gyroscope.
Công thức cập nhật gyro ở miền rời rạc:
𝜔 =
𝑔
𝑔
𝑔
=
𝑔
𝑔
𝑔
+ 𝐾
𝑒
𝑒
𝑒
+ 𝐾
𝑒
𝑒
𝑒
iv. Tính lượng quaternion thay đổi
Tính vi phân quaternion ước lượng được dùng công thức phương trình động
học dưới dạng quaternion từ phương trình (3.1).
𝑞̇ =
𝑞̇
𝑞̇
𝑞̇
𝑞̇
=
1
2
⎣
⎢
⎢
⎢
⎡
0 𝑔 −𝑔 𝑔
−𝑔 0 𝑔 𝑔
𝑔 −𝑔 0 𝑔
−𝑔 −𝑔 −𝑔 0 ⎦
⎥
⎥
⎥
⎤
𝑞
𝑞
𝑞
𝑞
v. Cập nhật quaternion trạng thái mới
𝑞 = 𝑞̇𝑑𝜏

63. 51
Ở miền rời rạc.
𝑞(𝑘) = 𝑞(𝑘 − 1) + 𝑞̇(𝑘)𝑇
𝑞
𝑞
𝑞
𝑞
=
𝑞
𝑞
𝑞
𝑞
+
𝑞̇
𝑞̇
𝑞̇
𝑞̇
𝑇
vi. Chuẩn hóa lại quaternion cho bước kế tiếp và tính góc
Chuẩn hóa lại quaternion
𝑞 =
𝑞
‖𝑞‖
=
𝑞
𝑞 + 𝑞 + 𝑞 + 𝑞
Chuyển đổi từ quaternion sang góc 𝜙, 𝜃, 𝜓 dùng công thức chuyển đổi từ
quaternion sang góc Euler ở mục (4.2.1.4)
𝜓
𝜃
𝜙
=
𝑎𝑡𝑎𝑛2(2(𝑞 𝑞 + 𝑞 𝑞 ), 2𝑞 + 2𝑞 − 1)
asin(−2𝑞 𝑞 + 2𝑞 𝑞 )
𝑎𝑡𝑎𝑛2(2(𝑞 𝑞 + 𝑞 𝑞 ), 2𝑞 + 2𝑞 − 1)
3.2.4. Bộ lọc thông thấp
Bộ lọc thông thấp sẽ cho các giá trị có tần số trong dải bé hơn hoặc bằng tần số
cắt đi qua, và lọc đi những tín hiệu có tần số lớn hơn tần số cắt.
Hı̀nh 3.2.5 Đồ thị đáp ứng miền tần số của bộ lọc thông thấp

64. 52
Sau đây trình bày 2 dạng bộ lọc thông thấp ở miền rời rạc
3.2.4.1. Giải thuật bộ lọc bù
Giải thuật bộ lọc bù với quy ước các biến như sau.
dt: thời gian lấy mẫu của cảm biến.
sample: tín hiệu cảm biến được đọc gần nhất.
pre_value: tín hiệu cảm biến trước đó.
cut_off: tần số cắt của bộ lọc thông thấp.
float LowPassFilter(float sample, float pre_value, float cut_off, float dt)
{
float RC, alpha, y;
RC = 1.0f/(cut_off*2*3.1416f);
alpha = dt/(RC+dt);
y = pre_value + alpha * ( sample - pre_value );
return y;
}
3.2.4.2. Giải thuật bộ lọc trung bình ( moving averaging filter )
Ý tưởng của bộ lọc trung bình là lưu trữ n giá trị đọc từ cảm biến gần nhất, sau
đó lấy giá trị trung bình của chúng.
Pseudocode như sau.
Trong đó
n = BUFFER_SIZE = 10
value_filtered là tín hiệu sau khi qua bộ lọc trung bình.

65. 53
#define BUFFER_SIZE 10
value_buffer[BUFFER_SIZE+1], value_filtered;
count = 0;
while(1) {
value = read_sensor();
value_buffer[BUFFER_SIZE]= value_buffer[BUFFER_SIZE] - value_buffer[count];
value_buffer[count] = value;
value_buffer[BUFFER_SIZE] = value_buffer[BUFFER_SIZE] + value_buffer[count];
value_filtered = value_buffer[BUFFER_SIZE]/BUFFER_SIZE;
count = (count+1)% BUFFER_SIZE;
}

66. 54
3.3. Xây dựng phương trình toán và động lực học.
Chương này sẽ phân tích các nguyên nhân gây ra lực cho hệ thống, để có trực
quan về cách quadcopter hoạt động, và là cơ sở để xây dựng các bộ điều khiển
sẽ thiết kế ở Chương 4
3.3.1. Mô hình động cơ và cánh quạt.
Phần này sẽ chỉ trình bày lý thuyết về động cơ và cánh quạt tạo nên thrust,
torque, các yếu tố ảnh hưởng đến các đại lượng này, và định tính về sự ảnh
hưởng của hệ thống nguyên nhân tạo nên lực. Việc đo các đại lượng có thể
được xác định bằng cách bố trí các thí nghiệm thích hợp, từ đó xác định các
thông số của hệ động cơ, cánh quạt, xung điều khiển, do đó ta có thể xác định
hàm truyền của hệ thống, hoặc có các hàm nội suy xấp xỉ trong miền làm việc,
giúp có thể điều khiển các nguyên nhân gây chuyển động cho hệ thống tốt và
chính xác hơn.
Hı̀nh 3.3.1 Hệ thống điều khiển động cơ BLDC
3.3.1.1. Hệ thống động cơ, cánh quạt và ESC.
Bộ điều khiển ESC đơn giản hóa việc điều khiển động cơ loại BLDC, ta chỉ cần
cấp xung PWM cho ESC. Thông thường, các ESC nhận các xung điều khiển có
tần số 50Hz với các độ rộng xung từ 1ms đến 2ms để điều khiển động cơ lần
lượt ứng với mức tốc độ từ thấp nhất tới cao nhất.

67. 55
3.3.1.2. Mối liên hệ giữa tốc độ động cơ và xung PWM.
Vì ta điều khiển tốc độ động cơ gián tiếp qua xung PWM, việc phân tích và có
mối liên hệ này gián tiếp giúp chúng ta sẽ có mối liên hệ giữa xung PWM và lực.
Để có thể xây dựng mối liên hệ này qua hàm truyền là khá khó khăn, do đó,
bằng các phương pháp như loop-up table, nội suy hàm. Ta có thể có được mối
liên hệ giữa xung PWM và tốc độ động cơ.
3.3.1.3. Mối liên hệ giữa tốc độ động cơ và lực tạo ra.
Từ lý thuyết cánh bay, lực đẩy tạo ra bởi cánh quạt (thrust) liên hệ với tốc độ
góc theo phương trình:
𝑇 = 𝐶 𝜋𝑅 𝜌(ω𝑅) (3. 2)
Phương trình torque gây ra
𝑄 = 𝐶 𝜋𝑅 𝜌(ω𝑅) (3. 3)
Trong đó:
𝐶 , CT: hằng số không thứ nguyên
R: bán kính đĩa tròn quét lên bởi cánh khi quay (m)
ω: tốc độ quay của cánh (rad/s)
𝜌: khối lượng riêng của không khí
Thông thường Với các giả sử môi trường mà quadcopter hoạt động không thay
đổi nhiều về độ cao, khối lượng riêng của không khí được coi xấp xỉ bằng hằng
số. Ta có thể xác định được 2 hằng số không thứ nguyên 𝐶 , 𝐶 bằng cách bố trí
thí nghiệm xác định lực của động cơ gây ra từ việc điều khiển ω bằng xung
PWM.
Hàm truyền động cơ có dạng.
𝐺(𝑠) =
𝐾
1 + τ𝑠
Trong đó

68. 56
K cho biết tỷ lệ giữa lực đẩy sinh ra do cánh quạt và độ rộng xung tín hiệu cấp
cho ESC
Hằng số thời gian τ gọi là độ trễ cơ khí, cho biết tốc độ đáp ứng của cụm ESC và
động cơ, là khoảng thời gian để đạt được tín hiệu đầu ra mong muốn từ khi có
tín hiệu điều khiển.
3.3.1.4. Mối liên hệ giữa tốc độ động cơ và điện áp cấp cho động cơ.
Thông số Kv cho biết mối liên hệ giữa tốc độ động cơ và điện áp cấp cho động
cơ, bằng cách gắn thêm mạch đo điện áp hiện tại của nguồn cung cấp cho hệ
thống, ta có thể bù lại phần sụt áp pin Lipo để luôn đạt được tốc độ động cơ
mong muốn, do đó duy trì được lực điều khiển mong muốn.
Trong giới hạn của luận văn này, mối liên hệ giữa xung PWM và lực gây ra được
giả sử có mối liên hệ tuyến tính, nhờ các kết quả thí nghiệm thực nghiệm để đo
tốc độ động cơ theo xung PWM từ các nghiên cứu khác đã thực hiện.
3.3.2. Phương tình động lực học của quadcopter.
Trong phần này tập trung phân tích nguyên nhân gây ra sự chuyển động của
quadcopter. Xây dựng phương trình toán học để làm rõ bản chất của hệ thống,
các để điều khiển hệ thống.
Khi động cơ – cánh quạt quay sẽ sinh ra đồng thời torque và thrust, liên hệ với
tốc độ quay của cánh quạt bằng phương trình (3.2) và (3.3)
Nhờ việc bố trí các cánh quạt liên tiếp có chiều quay khác nhau, do đó trong
trạng thái khi cả 4 động cơ quay cùng một tốc độ mà các torque này triệt tiêu
lẫn nhau không làm quadcopter bị xoay góc yaw.
Ma trận xoay chuyển từ BF sang EF. Khi xoay theo thứ tự 𝜓 → 𝜃 → 𝜙
𝑅 =
𝑐 𝑐 −𝑐 𝑠 𝑠
𝑠 𝑠 𝑐 + 𝑐 𝑠 −𝑠 𝑠 𝑠 + 𝑐 𝑐 −𝑠 𝑐
−𝑐 𝑠 𝑐 + 𝑠 𝑠 𝑐 𝑠 𝑠 + 𝑠 𝑐 𝑐 𝑐
Tổng lực do các động cơ – cánh quạt gây ra trong EF.

69. 57
𝐹 = 𝑅
0
0
−(𝑇 + 𝑇 + 𝑇 + 𝑇 )
Đặt 𝑇 = 𝑇 + 𝑇 + 𝑇 + 𝑇
Kết hợp với trọng lực luôn luôn tác dụng lên vật, ta có tổng lực tác dụng lên vật
trong EF là.
𝐹 =
𝐹
𝐹
𝐹
= 𝐹 + 𝐹 _ =
0
0
𝑚𝑔
+ 𝑅
0
0
−𝑇
Suy ra
𝐹
𝐹
𝐹
=
−𝑠 𝑇
𝑠 𝑐 𝑇
𝑚𝑔 − 𝑐 𝑐 𝑇
Theo định luật 2 Newton ta có:
𝐹 = 𝑚𝑃̈ = 𝑚
𝑥̈
𝑦̈
𝑧̈
Ta suy ra phương trình gia tốc các phương dịch chuyển x, y, z trong EF.
𝑥̈ =
−𝑠 𝑇
𝑚
𝑦̈ =
𝑠 𝑐 𝑇
𝑚
𝑧̈ =
𝑚𝑔 − 𝑐 𝑐 𝑇
𝑚

70. 58
3.3.3. Phương trình động lực học vật rắn.
Hı̀nh 3.3.2 Quy ước số thứ tự và chiều quay của động cơ
Nguyên nhân gây nên chuyển động của góc roll được tạo nên từ sự khác biệt
của thrust của động cơ 1,4 và 2,3. Và của góc pitch được tạo nên từ sự khác biệt
của thrust của động cơ 1,3 và 2,4. Sự chuyển động của góc yaw tạo nên từ sự
khác biệt của torque 1,2 và 3,4. Từ đó ta suy ra được ma trận moment lực tác
động lên vật rắn, bằng định nghĩa các ma trận như sau.
𝐵 =
⎣
⎢
⎢
⎢
⎢
⎡√2
2
𝑙 0 0
0
√2
2
𝑙 0
0 0 𝑐 ⎦
⎥
⎥
⎥
⎥
⎤
Ma trận dấu của quadcopter là 𝐶, ma trận này phụ thuộc vào chiều của các động
cơ, và cách bố trí các động cơ và hệ tọa độ đã quy ước.
𝐶 =
−1 1 1
1 −1 1
1 1 −1
−1
−1
−1
Vector thrust của động cơ.
𝑇 = [𝑇 𝑇 𝑇 𝑇 ]
Tổng lực gây nên chuyển động xoay cho quadcopter là:

71. 59
𝑀 = 𝜏 =
𝜏
𝜏
𝜏
= 𝐵𝐶𝑇
𝜏 là ma trận torque trong BF
Phương trình động lực học của vật rắn.
𝜏 = 𝐼𝜔̇ + 𝜔 × (𝐼𝜔 ) (3. 4)
Với giả sử vật rắn có phân bố đối xứng qua các trục, ma trận moment quán tính
𝐼 =
𝐼 0 0
0 𝐼 0
0 0 𝐼
(3. 5)
Từ phương trình (3.4) và (3.5) ta có phương trình:
𝜏
𝜏
𝜏
=
𝐼 𝑝̇ − 𝐼 − 𝐼 𝑞𝑟
𝐼 𝑞̇ − (𝐼 − 𝐼 )𝑟𝑝
𝐼 𝑟̇ − (𝐼 − 𝐼 )𝑝𝑞
𝜔 là ma trận tốc độ góc trong BF.
𝜔 =
𝑝
𝑞
𝑟
Từ các phương trình trên, ta có kết quả cuối cùng
𝑝̇ =
𝐼 − 𝐼
𝐼
𝑞𝑟 +
𝜏𝜙
𝐼𝑋𝑋
̇
𝑞̇ =
(𝐼 − 𝐼 )
𝐼
𝑞𝑟 +
𝜏𝜃
𝐼𝑌𝑌
𝑞̇ =
(𝐼 − 𝐼 )
𝐼
𝑞𝑟 +
𝜏𝜓
𝐼𝑍𝑍
Lưu ý là kết quả này bỏ qua gyroscope effect trong BF.
Mối liên hệ giữa vận tốc góc giữa BF và EF được biểu diễn ma trận chuyển hệ
trục tọa độ khi xoay một góc.

72. 60
𝜙̇
𝜃̇
𝜓̇
=
0 sin 𝜙 sec 𝜃 cos 𝜙 sec 𝜃
0 cos 𝜃 −sin 𝜃
1 sin 𝜙 tan 𝜃 cos 𝜙 tan 𝜃
𝑝
𝑞
𝑟
3.3.4. Phương trình tuyến tính hóa quanh điểm làm việc
𝜙̈ =
𝜏
𝐼
𝜃̈ =
𝜏
𝐼
𝜓̈ =
𝜏
𝐼
Đây là các kết quả quan trọng để thiết kế các bộ điều khiển cho hệ thống.
Chương 4. Thiết kế bộ điều khiển
Bằng cách phân tích phương trình động lực học quadcopter ở Chương 5, ta đã
hiểu được các nguyên nhân gây nên chuyển động của vật, trong chương này sẽ
trình bày về bộ điều khiển PID và áp dụng các thuật toán điều khiển, đồng thời
nêu lên những vấn đề cần chú ý để có thể thực hiện trên vi điều khiển.
4.1. Lý thuyết bộ điều khiển PID
4.1.1. Bộ điều khiển PID tổng quát
Bộ điều khiển PID tổng quát ở miền thời gian liên tục.
𝑢(𝑡) = 𝑘 𝑒(𝑡) + 𝑘 𝑒(𝜏)𝑑𝜏 + 𝑘
𝑑𝑒(𝑡)
𝑑𝑡
Bộ điều khiển ở miền thời gian rời rạc.
Với khởi tạo 𝑒(0) = 0
𝑢(𝑘) = 𝑘 𝑒(𝑘) + 𝑘 𝑒(𝑘)𝑇 + 𝑘
𝑒(𝑘) − 𝑒(𝑘 − 1)
𝑇

73. 61
Trong đó T là chu kỳ thực thi bộ điều khiển PID, thông thường được chọn là
chu kỳ lấy mẫu của cảm biến đọc tín hiệu cần điều khiển.
Thành phần I được dùng để tạo xác lập cho hệ thống điều khiển bằng cách
luôn luôn theo dõi tín hiệu đặt và tín hiệu hồi tiếp về.
Thành phần D được dùng để hạn chế sự đáp ứng quá nhanh của hệ thống gây
ra vọt lố.
Ví dụ nếu setpoint không thay đổi thì thành phần D chính là bù của vận tốc của
giá trị điều khiển được hổi tiếp.
4.1.2. Bộ điều khiển cascade PID
Lý thuyết: bộ điều khiển cascade PID chia nhiệm vụ điều khiển ra nhiều thành
phần, làm giảm độ phức tạp điều khiển, và dễ dàng điều chỉnh thông số PID hơn
so với bộ điều khiển PID thông thường trong trường hợp hệ thống phức tạp,
thông thường ta sẽ điều chỉnh thông số PID từ trong ra ngoài.
Hı̀nh 4.1.1 Sơ đồ tổng quát bộ điều khiển cascade PID
Bộ điều khiển PID_2 inner loop sẽ có tốc độ thực hiện nhanh hơn với bộ PID_1
outer loop để đảm bảo tính ổn định của hệ thống. Ta sẽ chỉnh PID của bộ điều
khiển PID_2 inner loop trước, khi tín hiệu và setpoint đã bám nhau trong một
tiêu chuẩn đánh giá nào đó thì chuyển sang bộ PID_1 outer loop.
Trong thực tế thì bộ cascade PID 2 tầng thường được sử dụng phổ biến nhất, và
được áp dụng theo cách sau: inner loop PID điều khiển tốc độ của tín hiệu cần

74. 62
điều khiển, outer loop PID chuyển sai số của tín hiệu cần điều khiển sang tốc độ
đặt bằng hệ số P thích hợp.
4.2. Áp dụng bộ điều khiển cho hệ thống quadcopter
4.2.1. Bộ điều khiển góc roll pitch yaw
4.2.1.1. Bộ điều khiển góc roll, pitch, yaw
Hı̀nh 4.2.1 Sơ đồ bộ điều khiển góc roll, pitch, yaw
Sai số góc đặt và góc từ đơn vị tính toán trạng thái
𝑒 = 𝛼 − 𝛼 =
𝜙
𝜃
𝜓
−
𝜙
𝜃
𝜓
Áp dụng bộ điều khiển vòng ngoài, Với 𝐾 là hệ số chuyển đổi từ sai số góc sang
vận tốc góc đặt, ta tính sai số vận tốc góc.
𝑒 = Ω − Ω = 𝐾 𝑒 – Ω
Áp dụng bộ điều khiển PID.
𝑢 =
𝑢
𝑢
𝑢
= 𝑘 𝑒 + 𝑘 𝑒 (𝜏)𝑑𝜏 + 𝑘
𝑑𝑒
𝑑𝑡
4.2.1.2. Áp dụng thuật toán điều khiển roll, pitch, yaw
Tín hiểu ngõ ra PID 𝑢 ở mục trên chính là giá trị xung PWM qua hệ thống
PWM-ESC-Động cơ được xuất trực tiếp vào các động cơ. Phần này sẽ trình bày
những vấn đề trọng tâm để có thể áp dụng vào vi điều khiển.

75. 63
Chuyển giá trị tín hiệu RC sang góc đặt
Giao tiếp TX-RX DEVO 7 có giá trị trả về trong khoảng [1100 → 1900] ,ở trạng
thái cần gạt ở giữa là 1500, định nghĩa góc đặt max khi gạt cần tối đa là
MAX_ANGLE_SETPOINT.
Công thức chuyển đổi từ giá trị RC sang góc đặt.
𝛼 = 𝑎𝑛𝑔𝑙𝑒_𝑠𝑒𝑡𝑝𝑜𝑖𝑛𝑡 =
(𝑟𝑒𝑎𝑑𝑅𝐶 − 1500)
400
𝑀𝐴𝑋_𝐴𝑁𝐺𝐿𝐸_𝑆𝐸𝑇𝑃𝑂𝐼𝑁𝑇
Xử lý góc yaw điều khiển
Kết quả của tính toán từ bộ ước lượng trạng thái ở mục (3.2) sẽ cho góc theo
từ trường Trái Đất làm mốc và khi ta chỉ về hướng Bắc từ trường , và tính chất
góc đường tròn nên đặc tính góc yaw có thể xoay một góc bất kì từ [−180 →
180] độ.
Góc chuyển từ 180 độ sang -180 sẽ chuyển đột ngột nên ta cần chú trọng vấn
đề này khi tính sai số góc điều khiển.
Ta sẽ chọn điểm khi chuyển sang chế độ giữ góc yaw là góc tham chiếu và bằng
0, công thức tính góc hiện tại từ góc ra của bộ ước lượng thái dựa theo code
sau.

76. 64
if (yaw_offset >=0.0f){
if (rpy[2] < (yaw_offset-180) && rpy[2] > -180){
yaw_control = 360 - ( - rpy[2] + yaw_offset );
}
else {
yaw_control = rpy[2] - yaw_offset;
}
}
// offset < 0.0f
else {
if ( rpy[2] < 180 && rpy[2] > (180 + yaw_offset)){
yaw_control = - yaw_offset + rpy[2] - 360 ;
}
else {
yaw_control = rpy[2] - yaw_offset;
}
}
Công thức xuất tín hiệu PWM ra các động cơ
Với 𝑢 = 𝑢 ; 𝑢 ; 𝑢 là ngõ ra của bộ điều khiển PID roll, pitch, yaw. Và
𝑢 là ngõ ra PWM để tạo nên thrust tổng cho quadcopter, có thể là ngõ ra
PID độ cao. Ta có công thức tính ngõ ra PWM động cơ là.
𝑚𝑜𝑡𝑜𝑟[1] = 𝑢 − 𝑢 + 𝑢 + 𝑢
𝑚𝑜𝑡𝑜𝑟[2] = 𝑢 + 𝑢 − 𝑢 + 𝑢
𝑚𝑜𝑡𝑜𝑟[3] = 𝑢 + 𝑢 + 𝑢 − 𝑢
𝑚𝑜𝑡𝑜𝑟[4] = 𝑢 − 𝑢 − 𝑢 − 𝑢

77. 65
Nhờ việc bù đối xứng giữa các động cơ, ta thấy rằng bộ điều khiển ổn định góc
roll, pitch, yaw không ảnh hưởng đến tổng đại số thrust tạo nên bởi 4 động cơ
trong BF.
4.2.2. Bộ điều khiển độ cao
4.2.2.1. Chế độ tự động cất cánh và xác định thrust hover.
Có 2 phương pháp thường được sử dụng là so sánh độ cao so với độ cao mốc,
hoặc là kiểm tra sự thay đổi của vận tốc từ cảm biến đo độ cao.
Ta giả sử quadcopter không nghiêng trong chế độ này. Do đó tổng thrust được
coi là bằng tổng đại số, luôn hướng lên trên .
Ý tưởng là kiểm tra và liên tục tăng xung PWM khi điều kiện đã cất cánh chưa
được thỏa mãn. Thắng được trọng lực, do đó khi nó bay lên thì T_total > mg.
Do đó ta cần trừ đi một lượng xung PWM để xác định T_hover -> PWM_hover.
4.2.2.2. Thiết kế bộ điều khiển giữ độ cao
Ta sẽ chọn chiều dương của độ cao là hướng lên trên ngược lại với quy ước
trong phương trình động lực học, để phù hợp với tín hiệu đo của các cảm biến.
Ta có.
𝑧̈ =
𝑐 𝑐 𝑇
𝑚
− 𝑔
𝑇 là tổng thrust của 4 động cơ luôn gắn liền với BF, là tín hiệu điều khiển
được nhờ mối liên hệ xung PWM -> ESC -> động cơ -> Thrust.
𝑧̈ =
𝑐 𝑐 𝑢
𝑚
− 𝑔
Ta tách tín hiệu điều khiển 𝑢 thành 2 thành phần.
𝑢 = 𝑢 + 𝑢
Trong đó 𝑢 sẽ triệt tiêu phần trọng lực luôn luôn tác dụng lên quadcopter.

78. 66
𝑢 có thể được xác định qua mối liên hệ giữa: điện áp hiện tại của pin Lipo,
khối lượng của vật rắn, mối liên hệ giữa thrust, PWM.
Việc xác định này có thể chỉ đúng tương đối do các sai số không mong muốn gây
ra, có thể cần thực nghiệm để điều chỉnh lại. Do đó trong việc áp dụng vào thực tế
và pseudocode có thành phần I để cho hệ thống tiến tới trạng thái xác lập hover.
Sai số độ cao là:
𝑒 = 𝑧 − 𝑧
Hı̀nh 4.2.2 Sơ đồ bộ điều khiển PID độ cao
Ta sẽ thiết kế bộ điều khiển PD cho tín hiệu 𝑢𝑓, vì 𝑢𝑓 là ngõ ra của bộ 𝑃𝐷(𝑧 ,
𝑧), ta có:
𝑢 = 𝑘 𝑒 + 𝑘 𝑒̇
Kết hợp phương trình, ta có.
𝑧̈ =
𝑐𝜙𝑐𝜃
𝑚
(𝑘 𝑒 + 𝑘 𝑒̇ )
𝑧̈ =
𝑐𝜙𝑐𝜃
𝑚
(𝑘 (𝑧 − 𝑧) + 𝑘 (𝑧̇ − 𝑧̇))
𝑧̈ + 𝑘
𝑐𝜙𝑐𝜃
𝑚
𝑧̇ + 𝑘
𝑐𝜙𝑐𝜃
𝑚
𝑧 = 𝑘
𝑐𝜙𝑐𝜃
𝑚
𝑧 + 𝑘
𝑐𝜙𝑐𝜃
𝑚
𝑧̇
Hàm truyền ở miền Laplace là:
𝐺(𝑠) =
𝑍(𝑠)
𝑍 (𝑠)
=
𝑘
𝑐𝜙𝑐𝜃
𝑠 + 𝑘
𝑐𝜙𝑐𝜃
𝑠 + 𝑘
𝑐𝜙𝑐𝜃
𝑠 + 𝑘
𝑐𝜙𝑐𝜃

79. 67
Đây là hệ bậc 2 giữa độ cao hiện tại ( được đo từ cảm biến ) và độ cao đặt, hệ số
PD 𝑘 và 𝑘 được chọn để đạt được các tiêu chuẩn đáp ứng mong muốn.
Biến đổi Laplace của hàm quá độ.
𝐻(𝑠) =
𝐺(𝑠)
𝑠
Giá trị của hàm quá độ tại trạng thái xác lập
lim
→
ℎ(𝑡) = lim
→
𝑠𝐻(𝑠) = lim
→
𝑠
1
𝑠
𝐺(𝑠) = 1
4.2.3. Bộ điều khiển vận tốc và vị trí bằng Optical Flow
4.2.3.1. Chuyển đổi vị trí giữa Reference Frame và Body Frame
Cảm biến PX4FLOW qua giải thuật tính vận tốc đã trình bày ở mục (2.3.4.3), thì
giá trị trả về luôn là vận tốc trong BF gắn với quadcopter, do vậy khi tính toán
để bù ta cần phải xét sự ảnh hưởng của góc yaw. Khi ta chọn vị trí giữ tại 𝜓 ,
nhưng trong quá trình hoạt động góc yaw của quadcopter là bất kì, gọi là 𝜓 .
Giải thuật chuyển như sau.

80. 68
Hı̀nh 4.2.3 Hệ tọa độ phương x-y quadcopter
Chọn hệ trục tọa độ như trên là RF (hệ tọa độ tham chiếu làm mốc), chiều
dương góc yaw là chiều ngược kim đồng hồ.
Tại thời điểm chuyển sang trạng thái giữ vị trí, gán mốc thời gian ban đầu t=0 ,
ta chọn góc 𝜓 = 0 và 𝜓 = 0 , do đó 𝜓 = 0. Đồng thời reset giá trị tọa độ trong
RF và BF. Hệ tọa độ gốc RF là 𝑋 − 𝑌 được mô tả như hình trên.
Chuyển giá trị vận tốc từ BF sang RF
Tại thời điểm bất kỳ, góc yaw của quadcopter thay đổi, ta tính góc 𝜓 tại đó.

81. 69
Hı̀nh 4.2.4 Chuyển đổi vận tốc từ BF sang RF
Áp dụng công thức Euler cho đại lượng dài là vận tốc, ta có vận tốc của
quadcopter trong RF là:
𝑣 = 𝑣 𝑒
−𝜓 vì xoay vector vận tốc theo chiều ngược chiều kim đồng hồ.
Tính vị trí hiện tại trong RF
𝑝 = 𝑣 (𝜏)𝑑𝜏
Tính giá trị vị trí trong BF

82. 70
Hı̀nh 4.2.5 Chuyển đổi tọa độ vị trí từ RF sang BF
Góc tọa độ của 𝑋 − 𝑌 trùng với gốc tọa độ đã chọn 𝑋 − 𝑌 của RF.
Áp dụng công thức Euler cho đại lượng dài vị trí, ta có vị trí của quadcopter
trong BF là
𝑝 = 𝑝 𝑒
Đây là giá trị vị trí cho lượng bù góc roll pitch tương ứng được phân tích ở phần
tiếp theo.
4.2.3.2. Bộ điều khiển vận tốc
Hı̀nh 4.2.6 Sơ đồ bộ điều khiển vận tốc
𝑒 = 𝑣 − 𝑣

83. 71
Áp dụng bộ điều khiển PID.
𝑢 = 𝑘 𝑒 + 𝑘 𝑒 (𝜏)𝑑𝜏 + 𝑘
𝑑𝑒
𝑑𝑡
Từ hai phương trình kết quả sau từ mục (3.3.2):
𝑥̈ =
−𝑠 𝑇
𝑚
𝑦̈ =
𝑠 𝑐 𝑇
𝑚
Ta sẽ điều khiển gia tốc của quadcopter từ ai phương trình trên, do đó 𝑢 ngõ
ra bộ điều khiển PID vận tốc sẽ là:
𝑢 =
𝑢
𝑢 =
−𝑠 𝑇
𝑚
𝑠 𝑐 𝑇
𝑚
=
𝑇
𝑚
−𝑠
𝑠 𝑐
𝑇 đã được tính từ bộ điều khiển độ cao được thiết kể ở , do đó điều khiển
vận tốc trục x, y được thực hiện bằng cách điều khiển góc 𝜙, 𝜃, nhờ bộ điều
khiển ổn định góc đã thiết kế ở mục (4.2.1).
Ta có:
𝜃 = 𝑎𝑟𝑐𝑠𝑖𝑛
𝑢 𝑚
𝑇
𝜙 = 𝑎𝑟𝑐𝑠𝑖𝑛
𝑢 𝑚
𝑇 1 − sin (𝜃 )
= 𝑎𝑟𝑐𝑠𝑖𝑛
⎝
⎛
𝑢 𝑚
𝑇 1 −
⎠
⎞
Do đặc tính điều khiển của hệ thống ta không thể đưa các giá trị đặt thay đổi
quá nhanh làm hệ thống đáp ứng không kịp, nên như hình sơ đồ bộ điều khiển
cần các bộ lọc thích hợp để hệ thống có chất lượng đáp ứng tốt.

84. 72
4.2.3.3. Bộ điều khiển giữ vị trí
Từ lý thuyết bộ điều khiển cascade PID ở mục (4.1.2), và bộ điều khiển vận tốc
phương x, y đã được xây dựng ở trên, ta sẽ xây dựng bộ điều khiển vị trí.
Hı̀nh 4.2.7 Sơ đồ bộ điều khiển vị trí
Áp dụng công thức tích phân để nhận được vị trí
𝑝 = 𝑣𝑑𝑡
Công thức tính tọa độ phương ngang miền rời rạc
𝑝 (𝑘)
𝑝 (𝑘)
=
𝑝 (𝑘 − 1)
𝑝 (𝑘 − 1)
+
𝑣 (𝑘)
𝑣 (𝑘)
𝑇
Sai số vị trí.
𝑝 = 𝑝 − 𝑝
Vòng ngoài của bộ điều khiển chỉ có thành phần khâu tỷ lệ P đơn giản, hệ số
chuyển đổi 𝐾 sẽ chuyển sai lệch vị trí sang vận tốc bù tương ứng, trong luận
văn này 𝐾 được chọn sao cho khi có sự thay đổi vị trí 100𝑐𝑚 thì vận tốc đặt
để bù lại là 0.55 𝑚/𝑠, áp dụng công thức chuyển đổi.
𝑣 =
𝑣
𝑣 = 𝐾 𝑝

85. 73
Chương 5. Kết quả của hệ thống và hướng phát triển đề tài
5.1. Kết quả và chất lượng của hệ thống
Thể hiện chất lượng của hệ thống được thiết kế và điều khiển, các đồ thị được
vẽ bằng Matlab.
5.1.1. Hệ thống quadcopter được xây dựng
Hı̀nh 5.1.1 Hệ thống quadcopter được thiết kế

86. 74
Hı̀nh 5.1.2 Mặt dưới của quadcopter

87. 75
Hı̀nh 5.1.3 Hệ thống quadcopter đang hoạt động trên không
5.1.2. Kết quả cân bằng roll pitch yaw
Bảng các giá trị thông số được chọn:
Thông số Giá trị Chú thích
𝐾 0.55 Chuyển đổi từ sai số
góc sang vận tốc góc
đặt của roll, pitch
𝑘 0.62 Hệ số P trong PID vận
tốc góc
𝑘 0.4 Hệ số I trong PID vận

88. 76
tốc góc
𝑘 0.0514 Hệ số D trong PID vận
tốc góc
Bảng 5.1 Bảng thông số bộ điều khiển roll, pitch, yaw
Hı̀nh 5.1.4 Kết quả điều khiển góc roll, pitch
Hı̀nh 5.1.5 Kết quả điều khiển tốc độ góc

89. 77
5.1.3. Kết quả giữ độ cao
Bảng các giá trị thông số được chọn:
Thông số Giá trị Chú thích
𝑘 0.38 Hệ số P trong PID độ
cao
𝑘 0.12 Hệ số P trong PID độ
cao
𝑘 0.62 Hệ số P trong PID độ
cao
Bảng 5.2 Bảng thông số bộ điều khiển độ cao
Hı̀nh 5.1.6 Kết quả giữ độ cao dùng cảm biến siêu âm
5.1.4. Kết quả giữ vận tốc và vị trí dùng Optical Flow
Bảng các giá trị thông số được chọn:
Thông số Giá trị Chú thích
𝑘 0.20588 Hệ số P trong PID vận

90. 78
tốc
𝑘 0.05 Hệ số I trong PID vận
tốc.
𝑘 0.02735 Hệ số D trong PID vận
tốc.
𝐾 0.55 Hệ số chuyển từ sai số
vị trí sang vận tốc đặt
PX4FLOW_VEL_SIZE 8 Kích thước bộ lọc
trung bình vận tốc từ
px4flow
velocity_sp_lpf_hz 12 Tần số bộ lọc thông
thấp cho vận tốc đặt
pid_vel_filter_hz 17 Tần số bộ lọc thông
thấp cho ngõ ra PID
vận tốc
d_filter_hz 15 Tần số bộ lọc thông
thấp cho thành D trong
PID vận tốc
POS_CONTROLLER_T 10 Tỷ số giữa tần số cập
nhật của bộ điều khiển
vận tốc chia bộ điều
khiển vị trí
Bảng 5.3 Bảng thông số bộ điều khiển vận tốc và vị trí

91. 79
Hı̀nh 5.1.7 Kết quả giữ vị trí với px4flow
Hı̀nh 5.1.8 Kết quả điều khiển giữ vị trí với px4flow theo thời gian
5.2. Hướng phát triển của đề tài.
Phần này trình bày những phương pháp để cải thiện chất lượng hệ thống
quadcopter của luận văn này, nhằm để xây dựng một hệ thống hoàn thiện và

92. 80
bền vững hơn, mà do giới hạn về thời gian, khối lượng luận văn và kiến thức
hiện có, mà trong luận văn chưa thể thực hiện được.
5.2.1. Sử dụng RTOS
Việc sử dụng phương pháp state-machine, kiểm tra các biến trạng thái và dựa
vào đó chọn phần code thực thi, chỉ sử dụng ngắt timer có sẵn của vi điều khiển
để xây dựng chương trình như trong luận văn sẽ gặp vấn đề khi chúng ta muốn
mở rộng hệ thống phần mềm, do đó sẽ gặp nhiều khó khăn và khó quản lý.
RTOS sẽ đảm nhận việc quản lý các tài nguyên của hệ thống, để người dùng có
thể chạy song song nhiều vòng lặp, và việc thiết kế chương trình cũng đơn giản
hơn nhiều do cơ chế quản lý các biến mutex, semaphore,..., và các nhiệm vụ
chạy được coi như lại độc lập với nhau.
Các hệ điều hành RTOS phổ biến cho dòng vi điều khiển thông dụng ARM
Cortex-M3, ARM Cortex-M3 là: FreeRTOS, NuttX, ChibiOS,…
5.2.2. Áp dụng bộ lọc Extended Kalman Filter và GPS
Thuật toán Extended Kalman Filter tận dụng tối đa tất cả thông tin cảm biến mà
hệ thống có được, do vậy các kết quả tính toán thông tin của quadcopter sẽ
được cải thiện từ EKF.
Kết hợp giữa GPS để xác định vị trí, bởi nếu chỉ dùng Optical Flow để xác định
vị trí tuy nó có độ chính xác cao tại một thời điểm hiện tại, nhưng hoạt động
trong một thời gian dài sẽ bị sai số do làm tròn, và sai số cảm biến tích lũy dần,
do đó đây là dead reckoning, GPS sẽ điều chỉnh lại sai số này.
5.2.3. Tích hợp máy tính nhúng
Khi bài toán điều khiển động học và động lực học đã được đảm bảo. Việc tích
hợp máy tính nhúng kèm trên các thiết bị bay sẽ giúp chúng ta có thể ứng dụng
vào các nhiệm vụ thực tiễn trong cuộc sống.

93. 81
Hı̀nh 5.2.1 Mô hình tích hợp máy tính nhúng Odroid trên bộ điều khiển bay
Pixhawk
Ví dụ các ứng dụng của máy tính nhúng tích hợp trên drone như:
+ Xử lý ảnh, phát hiện đối tượng.
+ Tận dụng các tài nguyên sẵn có mà máy tính nhúng hỗ trợ như hệ thống ROS
trên Linux, điều khiển qua mạng, …
+ Gắn dụng cụ lên quadcopter như vòi nước để ứng dụng trong công tắc chữa
cháy.

94. 82
TÀI LIỆU THAM KHẢO
[1] Chang Liu , Member, IEEE and Stephen D. Prior, “Design and Implementation
of a Mini Quadrotor Control System in GPS Denied Environments”
[2] Luis E. Romero, David F. Pozo, Jorge A. Rosales, “Quadcopter stabilization
by using PID controllers”
[3] Đặng Anh Tùng, “Xây Dựng Bộ Ước Lượng Góc Quay Ba Trục”
[4] Mark Euston, Paul Coote, Robert Mahony, Jonghyuk Kim and Tarek Hamel
“A Complementary Filter for Attitude Estimation of a Fixed-Wing UAV”
[5] Huỳnh Thái Hoàng, “bài giảng Cở sở tự động”
[6] Yan-Bin Jia, “Quaternions (Com S 477/577 Notes)”
[7] Sebastian O.H. Madgwick, “An efficient orientation filter for inertial and
inertial/magnetic sensor arrays”
[8] Các trang web: ardupilot.org, px4.io, diydrones.com, github.com, pudn.com,
brokking.net, sci-hub.tw